You are on page 1of 23

TECNOLÓGI

CONACI
ONALDEMÉXI
CO
I
NSTI
TUTOTECNOL CODEL
ÓGI ALAGUNA

DI
VI ÓNDEESTUDI
SI OSDEPOSGRADO
EINVESTI
GACI
ÓN

M.
C.ENI
NGENI ERÍ
AELÉCTRI
CA
MECATRÓNICAYCONTROL
ROBÓTICA

TAREA10.
1CONSIGNASDEVELOCI
DADES
ARTICULARESDELROBOTDENSOVP-6242G

PRESENTA:
I
NG.MARI
OIVÁNNAVABUSTAMANTE
M1913001

CATEDRÁTI
CO:DOCTORJ
.ALFONSOPÁMANESGARCÍ
A

TORREÓN,COAHUI
LA MAYODE2019
Índice

1. Problema a resolver 2

1.1. Etapa 1 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4

1.2. Etapa 2 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 5

1.3. Etapa 3 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 5

1.4. Etapa 4 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 6

2. Resultados 6

3. Conclusiones 9

4. Referencias bibliográficas 9
Robótica 2

1. Problema a resolver

Resolver el modelo inverso de velocidad del robot Denso VP-6242G, el cual debe llevar a cabo
la tarea de aplicación de pegamento sobre una placa rectangular de 30 cm por 18 cm. Además,
desarrollar un código de simulación en MatLab para obtener graficamente las 6 consignas de
velocidades articulares (θ1 , θ2 , θ3 , θ4 , θ5 y θ6 ) correspondientes al movimiento del robot.

Resolución

Este trabajo tiene como enfoque principal la obtención de las gráficas de las 6 consignas
de velocidades del robot, para la tarea correspondiente a realizar. Se tomó como base el trabajo
realizado en la tarea número 7 del curso de robótica, complementando el código de MatLab para
que este pudiera graficar las consignas de velocidades articulares del robot. Con la finalidad de
facilitar el procedimiento, se usó el paquete SYMORO para calcular la matriz Jacobiana, ya que
llevar a cabo su obtención mediante técnicas clásicas de cálculo resulta ser una tarea compleja
cuando se aplica en robots con un número alto de grados de libertad.

Las dimensiones y los lı́mites articulares del manipulador se muestran en la figura 1.

(a) Robot DENSO


VP-6242G.
(b) Dimensiones de robot DENSO VP-6242G.

Figura 1: Esquema de robot DENSO VP-6242G.

La tabla con los parámetros de la convención modificada de Denavit-Hartenberg se muestra


Tarea 10.1 Consignas de velocidades articulares del robot DENSO VP-6242G 3

a continuación:

J σ α d θ r
1 0 0 0 θ1 0
2 0 90◦ 0 θ2 0
3 0 0 d3 θ3 0
4 0 90◦ d4 θ4 r4
5 0 −90◦ 0 θ5 0
6 0 90◦ 0 θ6 0

Cuadro 1: Parámetros geométricos del manipulador

Las dimensiones de la herramienta a utilizar son las siguientes:

Figura 2: Herramienta a utilizar.

Los datos a tomar en cuenta para la resolución del problema son los siguientes:

Segmento AB

0 ≤ t0 ≤ 10seg
T xoh = XA + V t0
T yoh = YA (constante)
T zoh = XA (constante)
Robótica 4

Segmento BC

0 ≤ t00 ≤ 6seg
T xoh = XB (constante)
T yoh = YB − V t00
T zoh = 0(constante)

Segmento CD

0 ≤ t00 ≤ 10seg
T xoh = XC − V t000
T yoh = YC (constante)
T zoh = 0

Segmento DA

0 ≤ tIV ≤ 6seg
T xoh = XD (constante)
T yoh = YD + V tIV
T zoh = 0

1.1. Etapa 1

En esta etapa, el robot debe desplazarse sobre la arista superior de la placa recorriendo una
distancia de 30 cm de izquierda a derecha como lo muestra la figura 3, aplicando pegamento
sobre ella.
Tarea 10.1 Consignas de velocidades articulares del robot DENSO VP-6242G 5

Figura 3: Etapa 1.

1.2. Etapa 2

En esta etapa, el robot debe desplazarse sobre la arista del lado derecho de la placa reco-
rriendo una distancia de 18 cm como lo muestra la figura 4, aplicando pegamento sobre ella.

Figura 4: Etapa 2.

1.3. Etapa 3

En esta etapa, el robot debe desplazarse sobre la arista inferior de la placa recorriendo una
distancia de 30 cm, de izquierda a derecha como lo muestra la figura 5, aplicando pegamento
sobre ella.
Robótica 6

Figura 5: Etapa 3.

1.4. Etapa 4

En esta etapa, el robot debe desplazarse sobre la arista izquierda de la placa recorriendo
una distancia de 18 cm, de arriba hacia abajo como lo muestra la figura 6, aplicando pegamento
sobre ella.

Figura 6: Etapa 4.

2. Resultados

Para resolver el problema de este trabajo, se realizó un programa de simulación en Matlab


el cual se encuentra anexado al final de este documento. Se propone que el emplazamiento del
robot se encuentre en el piso. En base a los datos presentados en el apartado anterior, el código de
simulación desarrollado permite observar la trayectoria que describe el robot para poder lograr
la tarea programada. Las ecuaciones del modelo directo e inverso de posición y velocidad, fueron
obtenidas mediante el paquete computacional SYMORO.
Tarea 10.1 Consignas de velocidades articulares del robot DENSO VP-6242G 7

En la figura 7, se puede observar la simulación de movimiento y la ruta que describe el robot


DENSO ante la tarea de aplicación de pegamento sobre una placa rectangular.

Figura 7: Simulación de movimiento del robot realizando la tarea asignada.

En la figura 8, se muestran las gráficas con el comportamiento de las articulaciones del


robot. Se puede observar que dichas articulaciones no superan sus lı́mites, lo cual indica un buen
desempeño del robot ante la tarea programada.
Robótica 8

Figura 8: Comportamiento de las articulaciones del robot.

En la figura 9, se pueden observar las consignas de las velocidades articulares del robot al
desarrollar la tarea programada.

Figura 9: Consignas de velocidades articulares del robot.


Tarea 10.1 Consignas de velocidades articulares del robot DENSO VP-6242G 9

3. Conclusiones

Esta tarea habia sido realizada con anterioridad en el curso de robótica. Sin embargo, la
graficación de los lı́mites articulares ası́ como sus consignas de velocidad se habia resuelto de
manera errónea. Los principales errores consistieron en el cálculo de las ecuaciones del modelo
inverso de posición, además del modelo inverso de velocidad. Esto provocaba discontinuidades en
las gráficas de los lı́mites articulares, además de gráficas de velocidad completamente distintas
a lo que indicaba el movimiento del robot en la simulación. Al corregir dichos errores de cálculo
y con ello mejorar el código de simulación de Matlab, se pudo obtener las gráficas de limites
articulares y velocidades articulares de manera correcta.

La obtención de la matriz Jacobiana es indispensable para el modelado de velocidad de un


robot, ya que nos proporciona información importante sobre el robot y su relación con la tarea a
realizar. Para el modelado de velocidad, la obtención de la matriz Jacobiana es el pilar central en
la resolución de este tipo de problemas. En esta ocasión se hizo uso del paquete computacional
SYMORO, el cual permite realizar los cálculos necesarios de forma rápida y confiable. Además,
por medio de un código de simulación de Matlab se pudieron obtener las gráficas de las consignas
de velocidades articulares, dando solución al objetivo de este trabajo. Dichas gráficas nos otorgan
información de la velocidad de las articulaciones del robot respecto al tiempo.

4. Referencias bibliográficas

J. Alfonso Pámanes Garcı́a. Robótica: Introducción al modelado de manipuladores.


%%%%%%%DESARROLLADO POR ING. MARIO IVAN NAVA BUSTAMANTE %%%%%%%%%%%%%%%%%
%%%%%%%APLICACION DE PEGAMENTO SOBRE PLACA CON ROBOT DENSO VP6242G%%%%%%%
%%%%%%% GRAFICACION DE LIMITES ARTICULARES Y VELOCIDADES ARTICULARES %%%%%%%

np = 100;
np1 = np+1;
e = 1; %solución del MIP

%Parámetros geométricos:
d3 = 210;
d4 = 75;
r4 = 210;
d6 = 70;
rh = 250;
rp = 160;

%Herramienta:
a = 3;
b = 3;
c = 60;

%Placa:
l = 180;
m = 300;
ak = 150;
bk = 300;
ck = -300;

%Inicialización de arreglos de variables:

pnx = zeros(1,np1);
pny = zeros(1,np1);
pnz = zeros(1,np1);
t1g = zeros(1,np);
t2g = zeros(1,np);
t3g = zeros(1,np);
t4g = zeros(1,np);
t5g = zeros(1,np);
t6g = zeros(1,np);
time = zeros(1,np);
vx = zeros(1,np);
vy = zeros(1,np);
vz = zeros(1,np);

%Especificación del emplazamiento del robot:

aa = 0;
bb = 0;
cc = 280;

%Emplazamiento en el piso:

Te0 = [1 0 0 aa
0 1 0 bb
0 0 1 cc
0 0 0 1 ];

T0e = [1 0 0 -aa
0 1 0 -bb
0 0 1 -cc
0 0 0 1 ];

%Matriz que especifica el estado de la placa:

Tek = [0 0 1 ak
0 -1 0 bk
1 0 0 ck
0 0 0 1];
%Especificación de la base del robot en el marco 0:

sp1 = [ 0 0 0 0]';
sp2 = [ 0 0 0 1]';
b1 = [ -80 80 -280 1]';
b2 = [ 80 80 -280 1]';
b3 = [ 80 -80 -280 1]';
b4 = [ -80 -80 -280 1]';

%Especificación de los ejes del marco 0 en el marco 0:

ex0 = [5 0 0 1]';
ey0 = [0 5 0 1]';
ez0 = [0 0 5 1]';

% Especificación de los vértices de la pinza en el marco h:


%' en los vectores implica transponerlos para obtener los
%vectores columna necesarios.

p1 = [ a b -c 1]';
p2 = [ a b 0 1]';
p3 = [ a -b 0 1]';
p4 = [ a -b -c 1]';
p5 = [-a b -c 1]';
p6 = [-a b 0 1]';
p7 = [-a -b 0 1]';
p8 = [-a -b -c 1]';

%Especificación de los vértices de la placa en el marco k:

Ak = [62 -70 -m 1]';


Bk = [62 -70 0 1]';
Ck = [62 110 0 1]';
Dk = [62 110 -m 1]';

%Matriz de transformación de 6 a h:

T6h = [1 0 0 0
0 1 0 0
0 0 1 rh
0 0 0 1 ];

%Matriz que especifica la geometría de la herramienta:

Th6 = [1 0 0 0
0 1 0 0
0 0 1 -rh
0 0 0 1 ];

%Parámetros de movimiento del robot:

T1 = 2;
T2 = 4;
x06 = 285;
y06 = 0;
z06 = -100;

ra1=25;
ra2=50;
ra3=75;
ra4=100;
npm=25;

for i=0:np
% t=T1*i/np;
%Trayectoria de la placa
if i<ra1
t = T1*i/npm;
x06 = 345;%525;
y06 = -70;%-70;
z06 = -300;%-300;
delx06 = 0;
dely06 = 0;
delz06 = 300;
T = T1;
ti1 = t;
elseif (ra1<=i&&i<ra2)
i2 = i-ra1;
t = T2*i2/npm;
x06 = 345;%525;
y06 = -70;
z06 = 0;
delx06 = 0;%-180;
dely06 = 180;%50;
delz06 = 0;
T = T2;
ti2 = t+ti1;
elseif(ra2<=i&&i<ra3)
i3 = i-ra2;
t = T1*i3/npm;
x06 = 345;
y06 = 110;%-20;
z06 = 0;
delx06 = 0;
dely06 = 0;
delz06 = -300;
T = T1;
ti3 = t+ti2;
elseif(ra3<=i&&i<ra4)
i4 = i-ra3;
t = T2*i4/npm;
x06 = 345;
y06 = 110;%-20;
z06 = -300;
delx06 = 0;%180;
dely06 = -180;%-50;
delz06 = 0;
T = T2;
ti4 = t+ti3;
end

%Especificación de las variables articulares:

funt = (t/T)-(1/(2*pi))*(sin(2*pi*t/T));
px = x06+delx06*funt;
py = y06+dely06*funt;
pz = z06+delz06*funt;

Tkh = [0 0 -1 px
0 1 0 py
1 0 0 pz
0 0 0 1];

%Matriz SNAP:

snap = T0e*Tek*Tkh*Th6;
sx = snap(1,1);
sy = snap(2,1);
sz = snap(3,1);
nx = snap(1,2);
ny = snap(2,2);
nz = snap(3,2);
ax = snap(1,3);
ay = snap(2,3);
az = snap(3,3);
Px = snap(1,4);
Py = snap(2,4);
Pz = snap(3,4);

%Definición del ángulo de cada articulación del robot:


%MIP

%Theta 1:
t1 = atan2(Py,Px);

%Theta 2:
Z1=Px*cos(t1) + Py*sin(t1);
B1=2*(-(d4*Pz) - r4*Z1);
B2=2*(Pz*r4 - d4*Z1);
B3=d3^2 - d4^2 - Pz^2-r4^2 - Z1^2;
SQ=(B1*B3 + B2*sqrt(B1^2+B2^2-B3^2)*e)/(B1^2 + B2^2);
CQ=(B2*B3 - B1*sqrt(B1^2 +B2^2 -B3^2)*e)/(B1^2 + B2^2);
Q=atan2( SQ , CQ );
t2=atan2(-((-Pz-r4*cos(Q)+d4*sin(Q))/d3),(Z1-d4*cos(Q)-r4*sin(Q))/d3);

%Theta 3:
t3=Q-t2;

%Theta 4:
X=-(ay*cos(t1)) + ax*sin(t1);
Y=-(ax*cos(t1)*cos(t2+t3))-ay*cos(t2+t3)*sin(t1)-az*sin(t2+t3);
t4 = atan2(-X,Y );

%Theta 5:
Y5=-(cos(t4)*(ax*cos(t1)*cos(t2+t3)+ay*cos(t2+t3)*sin(t1)+az*sin(t2+t3))) -(-
(ay*cos(t1))+ax*sin(t1))*sin(t4);
Y15=az*cos(t2+t3)-ax*cos(t1)*sin(t2+t3)-ay*sin(t1)*sin(t2+t3);
t5 = atan2(-Y5,-Y15);

%Theta 6:
Y6=-(cos(t4)*(-
(sy*cos(t1))+sx*sin(t1)))+(sx*cos(t1)*cos(t2+t3)+sy*cos(t2+t3)*sin(t1)+sz*sin(t2+t3))*sin(t4
);
Y16=-(cos(t4)*(-
(ny*cos(t1))+nx*sin(t1)))+(nx*cos(t1)*cos(t2+t3)+ny*cos(t2+t3)*sin(t1)+nz*sin(t2+t3))*sin(t4
);
t6 = atan2(-Y6 , -Y16 );

if i<ra1
t1g(i+1)= rad2deg(t1);
t2g(i+1)= rad2deg(t2);
t3g(i+1)= rad2deg(t3);
t4g(i+1)= rad2deg(t4);
t5g(i+1)= rad2deg(t5);
t6g(i+1)= rad2deg(t6);
time(i+1)= ti1;
elseif (ra1<=i&&i<ra2)
t1g(i+1)= rad2deg(t1);
t2g(i+1)= rad2deg(t2);
t3g(i+1)= rad2deg(t3);
t4g(i+1)= rad2deg(t4);
t5g(i+1)= rad2deg(t5);
t6g(i+1)= rad2deg(t6);
time(i+1)= ti2;
elseif(ra2<=i&&i<ra3)
t1g(i+1)= rad2deg(t1);
t2g(i+1)= rad2deg(t2);
t3g(i+1)= rad2deg(t3);
t4g(i+1)= rad2deg(t4);
t5g(i+1)= rad2deg(t5);
t6g(i+1)= rad2deg(t6);
time(i+1)= ti3;
elseif(ra3<=i&&i<ra4)
t1g(i+1)= rad2deg(t1);
t2g(i+1)= rad2deg(t2);
t3g(i+1)= rad2deg(t3);
t4g(i+1)= rad2deg(t4);
t5g(i+1)= rad2deg(t5);
t6g(i+1)= rad2deg(t6);
time(i+1)= ti4;
end

%%%%%%%%%%%%%%%%%%%%%%% Nomenclatura usada

s1 = sin(t1);
c1 = cos(t1);
s2 = sin(t2);
c2 = cos(t2);
s3 = sin(t3);
c3 = cos(t3);
s4 = sin(t4);
c4 = cos(t4);
s5 = sin(t5);
c5 = cos(t5);
s6 = sin(t6);
c6 = cos(t6);
s23 = sin(t2+t3);
c23 = cos(t2+t3);
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%Matriz para transformar los vértices de la pinza del marco h al marco 0:

t11 = c1*c23*c4*c5*c6 + c5*c6*s1*s4-c1*c6*s23*s5 + c4*s1*s6 -c1*c23*s4*s6;


t21 = c23*c4*c5*c6*s1-c1*c5*c6*s4-c6*s1*s23*s5 -c1*c4*s6-c23*s1*s4*s6;
t31 = c4*c5*c6*s23+c23*c6*s5 -s23*s4*s6;
t12 = c4*c6*s1-c1*c23*c6*s4-c1*c23*c4*c5*s6-c5*s1*s4*s6+c1*s2*s5*s6;
t22 = -(c1*c4*c6)-c23*c6*s1*s4-c23*c4*c5*s1*s6+c1*c5*s4*s6+s1*s23*s5*s6;
t32 = -(c6*s23*s4)-c4*c5*s23*s6-c23*s5*s6;
t13 = c1*c5*s23+c1*c23*c4*s5+s1*s4*s5;
t23 = c5*s1*s23+c23*c4*s1*s5-c1*s4*s5;
t33 = -(c23*c5)+c4*s23*s5;
t14 = d3*c1*c2+d4*c1*c23+r4*c1*s23;
t24 = d3*c2*s1+d4*c23*s1+r4*s1*s23;
t34 = -(r4*c23)+d3*s2+d4*s23;

T06 = [t11 t12 t13 t14


t21 t22 t23 t24
t31 t32 t33 t34
0 0 0 1];

T0h = T06*T6h;

%Cálculo de las posiciones de los orígenes O2, O3, O4, O5 y Oh


%con respecto al marco 0 unido a la base del robot:
%Obtenidas de T02, T03, T04, T05 y T0h (T06):

x2 = 0;
y2 = 0;
z2 = 0;

x3 = d3*c1*c2;
y3 = d3*s1*c2;
z3 = d3*s2;

xc = d4*c1*c23+d3*c1*c2;
yc = d4*s1*c23+d3*s1*c2;
zc = d4*s23+d3*s2;

x4 = d3*c1*c2+d4*c1*c23+r4*c1*s23;
y4 = d3*s1*c2+d4*s1*c23+r4*s1*s23;
z4 = -r4*c23+d3*s2+d4*s23;

x5 = x4;
y5 = y4;
z5 = z4;

x6 = d3*c1*c2+d4*c1*c23+r4*c1*s23;
y6 = d3*c2*s1+d4*c23*s1+r4*s1*s23;
z6 = -(r4*c23)+d3*s2+d4*s23;

xOh = (c1*s23*c5+c1*c23*c4*s5+s1*s4*s5)*rh+d3*c1*c2+d4*c1*c23+r4*c1*s23;
yOh = (s1*s23*c5+s1*c23*c4*s5-c1*s4*s5)*rh+d3*s1*c2+d4*s1*c23+r4*s1*s23;
zOh = (-c23*c5+s23*c4*s5)*rh+d3*s2+d4*s23-r4*c23;

xOp = x4+(c1*s23*c5+c1*c23*c4*s5+s1*s4*s5)*rp;
yOp = y4+(s1*s23*c5+s1*c23*c4*s5-c1*s4*s5)*rp;
zOp = z4+(-c23*c5+s23*c4*s5)*rp;

%Transformación de las coordenadas de los extremos de los eslabones


%del marco 0 al marco e:

rpO2 = Te0*[x2 y2 z2 1]';


rpO3 = Te0*[x3 y3 z3 1]';
rpOc = Te0*[xc yc zc 1]';
rpO4 = Te0*[x4 y4 z4 1]';
rpO5 = Te0*[x5 y5 z5 1]';
rpO6 = Te0*[x6 y6 z6 1]';
rpOh = Te0*[xOh yOh zOh 1]';
rpOp = Te0*[xOp yOp zOp 1]';

%Se guardan las coordenadas de Oh en arreglos pnx, pny y pnz


%para preparar la graficación de la ruta:

pnx(i+1) = rpOh(1);
pny(i+1) = rpOh(2);
pnz(i+1) = rpOh(3);

%%%%%%%%%%%%%MDV%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%derivadas del mdp=mdv


V06x=(c1*(d3*s2-d4*s23+c23*r4)+s1*(-c2*d3-d4*c23-r4*s23));
V06y=(s1*(d3*c2+d4*c23+r4*s23)+c1*(d3*c2+d4*c23+r4*s23));
V06z=(s23*r4+c2*d3+c23*d4);

%%%%%%vector so
SO=[V06x;V06y;V06z;0;0;0];

%%%%%%%Jacobiana
J11 = -(d3*c2*s1) - d4*c23*s1 - r4*s1*s23;
J21 = d3*c1*c2 + d4*c1*c23 + r4*c1*s23;
J31 = 0;
J41 = 0;
J51 = 0;
J61 = 1;

J12 = r4*c1*c23 - d3*c1*s2 - d4*c1*s23;


J22 = r4*c23*s1 - d3*s1*s2 - d4*s1*s23;
J32 = d3*c2 + d4*c23 + r4*s23;
J42 = s1;
J52 = -c1;
J62 = 0;

J13 = r4*c1*c23 - d4*c1*s23;


J23 = r4*c23*s1 - d4*s1*s23;
J33 = d4*c23 + r4*s23;
J43 = s1;
J53 = -c1;
J63 = 0;

J14 = 0;
J24 = 0;
J34 = 0;
J44 = c1*s23;
J54 = s1*s23;
J64 = -c23;

J15 = 0;
J25 = 0;
J35 = 0;
J45 = c4*s1-c1*c23*s4;
J55 = -(c1*c4)-c23*s1*s4;
J65 = -(s23*s4);

J16 = 0;
J26 = 0;
J36 = 0;
J46 = c1*c5*s23+c1*c23*c4*s5+s1*s4*s5;
J56 = c5*s1*s23+ c23*c4*s1*s5 - c1*s4*s5;
J66 = -(c23*c5) + c4*s23*s5;

J = [J11 J12 J13 J14 J15 J16


J21 J22 J23 J24 J25 J26
J31 J32 J33 J34 J35 J36
J41 J42 J43 J44 J45 J46
J51 J52 J53 J54 J55 J56
J61 J62 J63 J64 J65 J66];

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%Modelo inverso de velocidad (multiplicando jacobiana inversa por so)

JI=inv(J);

qd = (JI*SO);

%Vector de velocidad de 06 con respecto a 0:

v061 = qd(1);
v062 = qd(2);
v063 = qd(3);
v064 = qd(4);
v065 = qd(5);
v066 = qd(6);

if i<ra1
v1(i+1)= v061;
v2(i+1)= v062;
v3(i+1)= v063;
v4(i+1)= v064;
v5(i+1)= v065;
v6(i+1)= v066;

elseif (ra1<=i&&i<ra2)
v1(i+1)= v061;
v2(i+1)= v062;
v3(i+1)= v063;
v4(i+1)= v064;
v5(i+1)= v065;
v6(i+1)= v066;

elseif(ra2<=i&&i<ra3)
v1(i+1)= v061;
v2(i+1)= v062;
v3(i+1)= v063;
v4(i+1)= v064;
v5(i+1)= v065;
v6(i+1)= v066;

elseif(ra3<=i&&i<ra4)
v1(i+1)= v061;
v2(i+1)= v062;
v3(i+1)= v063;
v4(i+1)= v064;
v5(i+1)= v065;
v6(i+1)= v066;
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%Transformación para dibujar el codo del robot:

T03 = [c1*c23 -c1*c23 s1 d3*c1*c2


s1*c23 -s1*s23 -c1 d3*s1*c2
s23 c23 0 d3*s2
0 0 0 1 ];

T3c = [1 0 0 d4
0 1 0 0
0 0 1 0
0 0 0 1];

T0c = [c1*c23 -c1*c23 s1 d4*c1*c23+d3*c1*c2


s1*c23 -s1*s23 -c1 d4*s1*c23+d3*s1*c2
s23 c23 0 d4*s23+d3*s2
0 0 0 1 ];

%Especificación de arreglos que se usarán en los plots:

%Primer eslabón:
es1x = [ aa rpO2(1)];
es1y = [ bb rpO2(2)];
es1z = [ cc rpO2(3)];

%Segundo eslabón:
es2x = [rpO2(1) rpO3(1)];
es2y = [rpO2(2) rpO3(2)];
es2z = [rpO2(3) rpO3(3)];

%Codo:
escx = [rpO3(1) rpOc(1)];
escy = [rpO3(2) rpOc(2)];
escz = [rpO3(3) rpOc(3)];

%Tercer eslabón:
es3x = [rpOc(1) rpO5(1)];
es3y = [rpOc(2) rpO5(2)];
es3z = [rpOc(3) rpO5(3)];

%Cuarto eslabón:
es4x = [rpO4(1) rpO6(1)];
es4y = [rpO4(2) rpO6(2)];
es4z = [rpO4(3) rpO6(3)];

%Eslabón herramienta:
espx = [rpO5(1) rpOp(1)];
espy = [rpO5(2) rpOp(2)];
espz = [rpO5(3) rpOp(3)];

%Herramienta:
hspx= [rpOp(1) rpOh(1)];
hspy= [rpOp(2) rpOh(2)];
hspz= [rpOp(3) rpOh(3)];
%Transformación de los puntos de la base del robot y de los extremos de
%los ejes del marco 0 al marco e:

s1n = Te0*sp1;
s2n = Te0*sp2;
b1n = Te0*b1;
b2n = Te0*b2;
b3n = Te0*b3;
b4n = Te0*b4;

s1x = [s1n(1) s2n(1)];


s1y = [s1n(2) s2n(2)];
s1z = [s1n(3) s2n(3)];

b1x = [b1n(1) b2n(1)];


b1y = [b1n(2) b2n(2)];
b1z = [b1n(3) b2n(3)];

b2x = [b2n(1) b3n(1)];


b2y = [b2n(2) b3n(2)];
b2z = [b2n(3) b3n(3)];

b3x = [b3n(1) b4n(1)];


b3y = [b3n(2) b4n(2)];
b3z = [b3n(3) b4n(3)];

b4x = [b4n(1) b1n(1)];


b4y = [b4n(2) b1n(2)];
b4z = [b4n(3) b1n(3)];

%Transformaciones de los vértices de la placa del marco al marco e:

T0k = Te0*Tek;
plA = T0k*Ak;
plB = T0k*Bk;
plC = T0k*Ck;
plD = T0k*Dk;

%Definición de arreglos para dibujar las aristas de la placa con


%respecto al marco e:

plABx = [plA(1) plB(1)];


plABy = [plA(2) plB(2)];
plABz = [plA(3) plB(3)];

plBCx = [plB(1) plC(1)];


plBCy = [plB(2) plC(2)];
plBCz = [plB(3) plC(3)];

plCDx = [plC(1) plD(1)];


plCDy = [plC(2) plD(2)];
plCDz = [plC(3) plD(3)];

plDAx = [plD(1) plA(1)];


plDAy = [plD(2) plA(2)];
plDAz = [plD(3) plA(3)];

%Transformaciones de los vértices de la pinza del marco 6 al marco e:

Teh = Te0*T0h;
pnz1 = Teh*p1;
pnz2 = Teh*p2;
pnz3 = Teh*p3;
pnz4 = Teh*p4;
pnz5 = Teh*p5;
pnz6 = Teh*p6;
pnz7 = Teh*p7;
pnz8 = Teh*p8;

%Definición de arreglos para dibujar las aristas de la pinza con


%respecto al marco e:

ar12x = [pnz1(1) pnz2(1)];


ar12y = [pnz1(2) pnz2(2)];
ar12z = [pnz1(3) pnz2(3)];

ar23x = [pnz2(1) pnz3(1)];


ar23y = [pnz2(2) pnz3(2)];
ar23z = [pnz2(3) pnz3(3)];

ar14x = [pnz1(1) pnz4(1)];


ar14y = [pnz1(2) pnz4(2)];
ar14z = [pnz1(3) pnz4(3)];

ar34x = [pnz3(1) pnz4(1)];


ar34y = [pnz3(2) pnz4(2)];
ar34z = [pnz3(3) pnz4(3)];

ar15x = [pnz1(1) pnz5(1)];


ar15y = [pnz1(2) pnz5(2)];
ar15z = [pnz1(3) pnz5(3)];

ar48x = [pnz4(1) pnz8(1)];


ar48y = [pnz4(2) pnz8(2)];
ar48z = [pnz4(3) pnz8(3)];

ar56x = [pnz5(1) pnz6(1)];


ar56y = [pnz5(2) pnz6(2)];
ar56z = [pnz5(3) pnz6(3)];

ar67x = [pnz6(1) pnz7(1)];


ar67y = [pnz6(2) pnz7(2)];
ar67z = [pnz6(3) pnz7(3)];

ar58x = [pnz5(1) pnz8(1)];


ar58y = [pnz5(2) pnz8(2)];
ar58z = [pnz5(3) pnz8(3)];

ar78x = [pnz7(1) pnz8(1)];


ar78y = [pnz7(2) pnz8(2)];
ar78z = [pnz7(3) pnz8(3)];

%%%%%Gráficas:
figure(1)
clf %Se limpia el espacio de graficación
hold on
grid on
grid minor

%Dibujo de la trayectoria de la pinza:

if i>1
for j=2:i
plot3([pnx(j-1),pnx(j)],[pny(j-1),pny(j)],[pnz(j-1),pnz(j)],'b','LineWidth',1);
hold on

if j==i

plot3([pnx(j),pnx(j+1)],[pny(j),pny(j+1)],[pnz(j),pnz(j+1)], 'b','LineWidth',1);
end
end
end

%Dibujo de la base del robot:


plot3(b1x,b1y,b1z,'k', b2x,b2y,b2z,'k', b3x,b3y,b3z,'k',
b4x,b4y,b4z,'k','LineWidth',1);
plot3(s1x,s1y,s1z,'k','MarkerSize',5,'LineWidth',1);

%Dibujo de la placa:
plot3(plABx ,plABy ,plABz ,'k', plBCx ,plBCy ,plBCz,'k', plCDx ,plCDy ,plCDz
,'k','LineWidth',1);
plot3(plDAx ,plDAy ,plDAz ,'k','LineWidth',1);

%Dibujo de la herramienta:
plot3(espx,espy,espz, 'k-o','LineWidth',1);
plot3(hspx,hspy,hspz, 'r','LineWidth',1.5);

%Dibujo de los eslabones:


plot3(es1x,es1y,es1z, 'k-O',es2x,es2y,es2z, 'k-o',escx,escy,escz,'k-
o','LineWidth',1);
plot3(es3x,es3y,es3z, 'k-o',es4x,es4y,es4z,'k-O','LineWidth',1);
axis([-200,650,-325,525,-250,600])

% view(2)
view([1,-1,1]) %Para emplazamiento en el piso.
%
xlabel('x');
ylabel('y');
zlabel('z');

pause(0.0001)

end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %Límites Articulares

figure (2)
title('Límites Articulares')
clf

%Grafica Theta 1: -160 +160


subplot(2,3,1)
plot(time,t1g,'b','LineWidth',1)
hold on
plot([0 time(np)],[160 160],'r','LineWidth',1)
plot([0 time(np)],[-160 -160],'r','LineWidth',1)
axis([0,11.52,-170,170])
xlabel('Tiempo')
ylabel('Articulación 1')
grid minor

%Grafica Theta 2: -30 +210


subplot(2,3,2)
plot(time,t2g,'b','LineWidth',1)
hold on
plot([0 time(np)],[210 210],'r','LineWidth',1)
plot([0 time(np)],[-30 -30],'r','LineWidth',1)
axis([0,11.52,-40,220])
title('Límites Articulares')
xlabel('Tiempo')
ylabel('Articulación 2')
grid minor

%Grafica Theta 3: -70 +71


subplot(2,3,3)
plot(time,t3g,'b','LineWidth',1)
hold on
plot([0 time(np)],[71 71],'r','LineWidth',1)
plot([0 time(np)],[-70 -70],'r','LineWidth',1)
axis([0,11.52,-80,80])
xlabel('Tiempo')
ylabel('Articulación 3')
grid minor

%Grafica Theta 4: -160 +160


subplot(2,3,4)
plot(time,t4g,'b','LineWidth',1)
hold on
plot([0 time(np)],[160 160],'r','LineWidth',1)
plot([0 time(np)],[-160 -160],'r','LineWidth',1)
axis([0,11.52,-170,170])
xlabel('Tiempo')
ylabel('Articulación 4')
grid minor

%Grafica Theta 5: -120 +120


subplot(2,3,5)
plot(time,t5g,'b','LineWidth',1)
hold on
plot([0 time(np)],[120 120],'r','LineWidth',1)
plot([0 time(np)],[-120 -120],'r','LineWidth',1)

axis([0,11.52,-130,130])
xlabel('Tiempo')
ylabel('Articulación 5')
grid minor

%Grafica Theta 6: -360 +360


subplot(2,3,6)
plot(time,t6g,'b','LineWidth',1)
hold on
plot([0 time(np)],[360 360],'r','LineWidth',1)
plot([0 time(np)],[-360 -360],'r','LineWidth',1)
axis([0,11.52,-370,370])
xlabel('Tiempo')
ylabel('Articulación 6')
grid minor

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %Velocidades Articulares


figure (3)
title('Velocidades Articulares')
clf

%Grafica Theta 1: -160 +160


subplot(2,3,1)
plot(time,v1,'r','LineWidth',1)
hold on
axis([0,11.52,-0.2,2])
xlabel('Tiempo')
ylabel('Velocidad Articulación 1')
grid minor

%Grafica Theta 2: -160 +160


subplot(2,3,2)
plot(time,v2,'r','LineWidth',1)
hold on
axis([0,11.52,-3,1])
xlabel('Tiempo')
ylabel('Velocidad Articulación 2')
grid minor

%Grafica Theta 3: -160 +160


subplot(2,3,3)
plot(time,v3,'r','LineWidth',1)
hold on
axis([0,11.52,0.8,4.9])
xlabel('Tiempo')
ylabel('Velocidad Articulación 3')
grid minor

%Grafica Theta 4: -160 +160


subplot(2,3,4)
plot(time,v4,'r','LineWidth',1)
hold on
axis([0,11.52,-1,1])
xlabel('Tiempo')
ylabel('Velocidad Articulación 4')
grid minor

%Grafica Theta 5: -160 +160


subplot(2,3,5)
plot(time,v5,'r','LineWidth',1)
hold on
axis([0,11.52,-4.6,1.36])
xlabel('Tiempo')
ylabel('Velocidad Articulación 5')
grid minor

%Grafica Theta 6: -360 +360


subplot(2,3,6)
plot(time,v6,'r','LineWidth',1)
hold on
axis([0,11.52,-0.8,3.5])
xlabel('Tiempo')
ylabel('Velocidad Articulación 6')
grid minor