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

TAREA14ÍNDI
CESDEMANIPULABI LI
DAD
DELROBOTDENSOVP-6242G

PRESENTA:
I
NG.MARI
OIVÁNNAVABUSTAMANTE
M1913001

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

TORREÓN,COAHUI
LA MAYODE2019
Índice

1. Introducción 2

2. Problema a resolver 2

3. Resultados 4

4. Conclusiones 7

5. Referencias bibliográficas 7
 Robótica 2

1. Introducción

La manipulabilidad de un robot se puede definir como la capacidad de cambio de posición

y orientación del efector final del sistema robótico para una determinada configuración. Cuanto
mas cercano sea el número del ı́ndice de manipulabilidad a la unidad, el robot tendra un mejor
desarrollo para determinadas tareas.

Se define la manipulabilidad del robot como:

ω = σ1 σ2

o en forma general

p
ω= det(JJ T )

Esta variable ω es definida por T. Yoshikawa como ı́ndice de manipulabilidad del manipu-
lador.

2. Problema a resolver

Desarrollar un código de simulación en MatLab para obtener graficamente los ı́ndices de

manipulabilidad correspondientes al movimiento del robot Denso VP-6242G al describir la ruta
para aplicar pegamento sobre una placa rectangular. Dicha tarea ya fue resuelta con anterioridad
en el curso de robótica (tarea 7 resolución de modelo directo e inverso de posición y graficación
de lı́mites articulares, tarea 10.1 resolución de modelo directo e inverso de velocidad y graficación
de consignas de velocidades articulares, tarea 13 resolución y graficación de ı́ndices articulares.),

Resolución

Este trabajo tiene como enfoque principal la obtención y graficación de los ı́ndices de mani-
pulabilidad del robot Denso VP-6242G, para la tarea correspondiente a realizar. Se tomó como
anecedente el trabajo realizado en las tarea número 7, número 10.1 y número 13 del curso de
robótica, complementando el código de MatLab para que este pudiera graficar los ı́ndices de
desempeño.
 Tarea 14. Índices de manipulabilidad del robot DENSO VP-6242G 3

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

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

 Robótica 4

3. 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 y obtenidos en las tareas número
7, número 10.1 y número 13 del curso de robótica, el código de simulación desarrollado permite
observar la trayectoria que describe el robot para poder lograr la tarea programada, además de
graficar los lı́mites articulares, las consignas de velocidades articulares y los ı́ndices articulares
del robot para analizar visualmente el desempeño que tiene. Dicho código fue complementado,
para que pudiera graficar también los ı́ndices de manipulabilidad correspondientes.

En la figura 3, 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 2: Simulación de movimiento del robot realizando la tarea asignada.

En la figura 4, 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.
 Tarea 14. Índices de manipulabilidad del robot DENSO VP-6242G 5

Figura 3: Comportamiento de las articulaciones del robot.

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

Figura 4: Consignas de velocidades articulares del robot.

 Robótica 6

En la figura 6, se pueden observar los ı́ndices articulares del robot obtenidos.

Figura 5: Índices articulares del robot.

Finalmente, en la figura 7 se observan los ı́ndices de manipulabilidad del robot al desarrollar

la tarea de aplicación de pegamento sobre la placa rectangular.
 Tarea 14. Índices de manipulabilidad del robot DENSO VP-6242G 7

Figura 6: Índices de manipulabilidad del robot.

4. Conclusiones

Si se quiere estudiar todo lo relacionado a la cuestión cinemática de un robot manipulador, es

de principal importancia analizar la manipulabilidad de dicho robot. El ı́ndice de manipulabilidad
es uno de los parámetros mas importantes de funcionabilidad de un robot. Gracias a el pueden
ser definidos otros tipos de ı́ndices de comportamiento cinemático, con lo cual se puede optimizar
un robot en cuestión de dimensiones y parámetros.

T. Yoshikawa fue quien estableció las bases de la teorı́a de la manipulabilidad en robots. En

este trabajo, se tomo como base dicha teorı́a para analizar los ı́ndices de manipulabilidad del
robot Denso VP-6242G ante la tarea de aplicación de pegamento sobre una placa rectangular.

5. Referencias bibliográficas

J. Alfonso Pámanes Garcı́a. Modelado cinemático de manipuladores redundantes.

%%%%%%%DESARROLLADO POR ING. MARIO IVAN NAVA BUSTAMANTE %%%%%%%%%%%%%%%%%
%%%%%%%APLICACION DE PEGAMENTO SOBRE PLACA CON ROBOT DENSO VP6242G%%%%%%%
%%%%%%% GRAFICACION DE INDICES DE MANIPULABILIDAD %%%%%%%

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

%Parámetros geométricos:
%Robot:
d3 = 210;
d4 = 75;
r4 = 210;
d6 = 70;
rh = 250; %Porque el marco 6 está unido al 4 y al 5
rp = 160; %Para trazar el vástago del muñón a la herramienta

%Pinza:
a = 30;
b = 5;
c = 60;

%Placa:
l = 180;
m = 300;
ak = 150;%350;%400; %150;
bk = 320;%75;%100; %200;
ck = -350;%-280-140;%-100; %400;

%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);
Vel1=zeros(1,np);
Vel2=zeros(1,np);
Vel3=zeros(1,np);
Vel4=zeros(1,np);
Vel5=zeros(1,np);
Vel6=zeros(1,np);

%wx = zeros(1,np);
%wy = zeros(1,np);
%wz = zeros(1,np);
th1g = zeros(1,np);
th2g = zeros(1,np);
th3g = zeros(1,np);
th4g = zeros(1,np);
th5g = zeros(1,np);
th6g = zeros(1,np);

kk1= zeros(1,np);
kk2= zeros(1,np);
kk3= zeros(1,np);
kk4= zeros(1,np);
kk5= zeros(1,np);
kk6= zeros(1,np);
kkk= zeros(1,5);
 kappa= zeros(1,np);
Wa= zeros(1,np);
Ww= zeros(1,np);

%limites articulares Denso:

th1min=-160;
th1max=160;
th1mean=((th1max+th1min)/2);
delth1mx= abs(th1max-th1mean);

th2min=-30;
th2max=210;
th2mean=((th2max+th2min)/2);
delth2mx= abs(th2max-th2mean);

th3min=-70;
th3max=71;
th3mean=((th3max+th3min)/2);
delth3mx= abs(th3max-th3mean);

th4min=-160;
th4max=160;
th4mean=((th4max+th4min)/2);
delth4mx= abs(th4max-th4mean);

th5min=-120;
th5max=120;
th5mean=((th5max+th5min)/2);
delth5mx= abs(th5max-th5mean);

th6min=-360;
th6max=360;
th6mean=((th6max+th6min)/2);
delth6mx= abs(th6max-th6mean);

%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 ];

% %Emplazamiento en muro:

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

%T0e = [0 0 1 -aa
% 0 1 0 -bb
% -1 0 0 -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 100 -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;
%delx06 = 0;
%dely06 = 0;
%delz06 = 100;
ra1=25;
ra2=50;
ra3=75;
ra4=100;
npm=25;

for i=0:np
% t=T1*i/np;

if i<ra1
t = T1*i/npm;
x06 = 500;%525;
y06 = 30;%-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 = 500;%525;
y06 = 30;
z06 = 0;
delx06 = 0;%-180;
dely06 = 140;%50;
delz06 = 0;
T = T2;
ti2 = t+ti1;
elseif(ra2<=i&&i<ra3)
i3 = i-ra2;
t = T1*i3/npm;
x06 = 500;
y06 = 170;%-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 = 500;
y06 = 170;%-20;
z06 = -300;
delx06 = 0;%180;
dely06 = -140;%-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:

%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

if i < np
th1g(i+1)=rad2deg(t1);
th2g(i+1)=rad2deg(t2);
th3g(i+1)=rad2deg(t3);
th4g(i+1)=rad2deg(t4);
th5g(i+1)=rad2deg(t5);
th6g(i+1)=rad2deg(t6);

kk1(i+1)=abs((th1g(i+1)- th1mean)/delth1mx);
kk2(i+1)=abs((th2g(i+1)- th2mean)/delth2mx);
kk3(i+1)=abs((th3g(i+1)- th3mean)/delth3mx);
kk4(i+1)=abs((th4g(i+1)- th4mean)/delth4mx);
kk5(i+1)=abs((th5g(i+1)- th5mean)/delth5mx);
kk6(i+1)=abs((th6g(i+1)- th6mean)/delth6mx);

kkk(1)=kk1(i+1);
kkk(2)=kk2(i+1);
kkk(3)=kk3(i+1);
kkk(4)=kk4(i+1);
kkk(5)=kk5(i+1);
kkk(6)=kk6(i+1);

kmean= mean(kkk);
kstd= std(kkk,1);
kappa_i=kmean + kstd;

kappa(i+1)=kappa_i;
end

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

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 Jacobiana Básica:

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];

JI = inv(J);

%Manipulabilidad
Jta = [J11 J12 J13
J21 J22 J23
J31 J32 J33 ];

JtaT =[J11 J21 J31

J12 J22 J32
J13 J23 J33 ];

Jrw = [J44 J45 J46

J54 J55 J56
J64 J65 J66];
JrwT =[J44 J54 J64
J45 J55 J65
J46 J56 J66];

if i<np
W1(i+1)=sqrt(det(Jta*JtaT));
W2(i+1)=sqrt(det(Jrw*JrwT));
W22=W2/1.537e7;

end
%Velocidad:

funtd = (1/T)*(1-cos((2*pi*t)/T)); %Derivada de función cicloidal

%Velocidades operacionales con respecto a K:

kvx06 = delx06*funtd;
kvy06 = dely06*funtd;
kvz06 = delz06*funtd;
wx=0;
wy=0;
wz=0;

%velocidad de 06 con respecto a 0:

T0k = T0e*Tek;

R0k =[T0k(1,1) T0k(1,2) T0k(1,3)

T0k(2,1) T0k(2,2) T0k(2,3)
T0k(3,1) T0k(3,2) T0k(3,3)];

v06k = [kvx06;kvy06;kvz06];
v060 = R0k*v06k;
s0 = [v060(1); v060(2); v060(3);wx;wy;wz];
qd = JI*s0;

%Vector de velocidad de 06 con respecto a 0:

v06x = qd(1,1);
v06y = qd(2,1);
v06z = qd(3,1);
Vel = JI*s0;
Velaux=JI*0;
if i<ra1
vx(i+1)= v06x;
vy(i+1)= v06y;
vz(i+1)= v06z;
Vel1(i+1)=(Vel(1,1));
Vel2(i+1)=(Vel(2,1));
Vel3(i+1)=(Vel(3,1));
Vel4(i+1)=(Vel(4,1));
Vel5(i+1)=(Vel(5,1));
Vel6(i+1)=(Vel(6,1));

elseif (ra1<=i&&i<ra2)
vx(i+1)= v06x;
vy(i+1)= v06y;
vz(i+1)= v06z;
Vel1(i+1)=(Vel(1,1));
Vel2(i+1)=(Vel(2,1));
Vel3(i+1)=(Vel(3,1));
Vel4(i+1)=(Vel(4,1));
Vel5(i+1)=(Vel(5,1));
Vel6(i+1)=(Vel(6,1));

elseif(ra2<=i&&i<ra3)
vx(i+1)= v06x;
 vy(i+1)= v06y;
vz(i+1)= v06z;
Vel1(i+1)=(Vel(1,1));
Vel2(i+1)=(Vel(2,1));
Vel3(i+1)=(Vel(3,1));
Vel4(i+1)=(Vel(4,1));
Vel5(i+1)=(Vel(5,1));
Vel6(i+1)=(Vel(6,1));
elseif(ra3<=i&&i<ra4)
vx(i+1)= v06x;
vy(i+1)= v06y;
vz(i+1)= v06z;
Vel1(i+1)=(Vel(1,1));
Vel2(i+1)=(Vel(2,1));
Vel3(i+1)=(Vel(3,1));
Vel4(i+1)=(Vel(4,1));
Vel5(i+1)=(Vel(5,1));
Vel6(i+1)=(Vel(6,1));

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 ];

%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 = d3*s2+d4*s23-r4*c23;

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

x6 = x4+(c1*s23*c5+c1*c23*c4*s5+s1*s4*s5)*d6;
y6 = y4+(s1*s23*c5+s1*c23*c4*s5-c1*s4*s5)*d6;
z6 = z4+(-c23*c5+s23*c4*s5)*d6;

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);

%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.
% view([-1,-1,1]) %Para emplazamiento en muro

xlabel('x');
ylabel('y');
zlabel('z');

pause(0.0001)

end

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

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

%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')

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

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

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

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

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

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

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% indices articulares

figure(4)
title('Indices Articulares')

plot(time,kk1,'b','LineWidth',1)
legend('Articulacion 1')
hold on
axis([0,11,0,2])

figure(4)
plot(time,kk2,'r','LineWidth',1)
legend('Articulacion 2')
hold on
axis([0,11,0,2])

figure(4)
plot(time,kk3,'m','LineWidth',1)
legend('Articulacion 3')
hold on
axis([0,11,0,2])

figure(4)
plot(time,kk4,'k','LineWidth',1)
legend('Articulacion 4')
hold on
axis([0,11,0,2])

figure(4)
plot(time,kk5,'g','LineWidth',1)
legend('Articulacion 5')
hold on
axis([0,11,0,2])% plot(time(np),Ww,'g')

figure(4)
plot(time,kk6,'c','LineWidth',1)
legend('Articulacion 1','Articulacion 2','Articulacion 3','Articulacion 4','Articulacion
5','Articulacion 6')
hold on
axis([0,11,0,2])% plot(time(np),Ww,'g')
grid minor

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% indices de manipulabilidad

figure(5)
title('Indices de manipulabilidad')

plot(time,W1,'b','LineWidth',1)
legend('Indice de manipulabilidad (posicion)','Indice de manipulabilidad (orientacion)')
hold on
grid minor
axis([0,10,0,1])

figure(5)
plot(time,W22,'r','LineWidth',1)
% legend('Indice de manipulabilidad (orientacion)')
hold on
grid minor
axis([0,10,0,1])