PRIMERA FILA:

[ s6*(c4*s1 - s4*(c1*c2*c3p2 - c1*s2*s3p2)) - c6*(s4*(c1*c2*s3p2 + c1*c3p2*s2) - c5*(s1*s4 +

c4*(c1*c2*c3p2 - c1*s2*s3p2))),

s6*(s4*(c1*c2*s3p2 + c1*c3p2*s2) - c5*(s1*s4 + c4*(c1*c2*c3p2 - c1*s2*s3p2))) + c6*(c4*s1 -

s4*(c1*c2*c3p2 - c1*s2*s3p2)),

c5*(c1*c2*s3p2 + c1*c3p2*s2) + s4*(s1*s4 + c4*(c1*c2*c3p2 - c1*s2*s3p2)),

(l5 + l6)*(c5*(c1*c2*s3p2 + c1*c3p2*s2) + s4*(s1*s4 + c4*(c1*c2*c3p2 - c1*s2*s3p2))) + (l3 +

l4)*(c1*c2*s3p2 + c1*c3p2*s2) + c1*c2*l2]

SEGUNDA FILA:

[ - c6*(s4*(c2*s1*s3p2 + c3p2*s1*s2) + c5*(c1*s4 - c4*(c2*c3p2*s1 - s1*s2*s3p2))) - s6*(c1*c4 +

s4*(c2*c3p2*s1 - s1*s2*s3p2)),

s6*(s4*(c2*s1*s3p2 + c3p2*s1*s2) + c5*(c1*s4 - c4*(c2*c3p2*s1 - s1*s2*s3p2))) - c6*(c1*c4 +

s4*(c2*c3p2*s1 - s1*s2*s3p2)),

c5*(c2*s1*s3p2 + c3p2*s1*s2) - s4*(c1*s4 - c4*(c2*c3p2*s1 - s1*s2*s3p2)),

(l5 + l6)*(c5*(c2*s1*s3p2 + c3p2*s1*s2) - s4*(c1*s4 - c4*(c2*c3p2*s1 - s1*s2*s3p2))) + (l3 +

l4)*(c2*s1*s3p2 + c3p2*s1*s2) + c2*l2*s1]

TERCERA FILA:

c6*(s4*(c2*c3p2 - s2*s3p2) + c4*c5*(c2*s3p2 + c3p2*s2)) - s4*s6*(c2*s3p2 + c3p2*s2),

- s6*(s4*(c2*c3p2 - s2*s3p2) + c4*c5*(c2*s3p2 + c3p2*s2)) - c6*s4*(c2*s3p2 + c3p2*s2),

c4*s4*(c2*s3p2 + c3p2*s2) - c5*(c2*c3p2 - s2*s3p2),

l1 + l2*s2 - (l3 + l4)*(c2*c3p2 - s2*s3p2) - (l5 + l6)*(c5*(c2*c3p2 - s2*s3p2) - c4*s4*(c2*s3p2 +

c3p2*s2))]

CUARTA FILA:

[ 0, 0, 0, 1]

% Programa en Matlab para calcular la matriz T, utilizando el Toolbox

symbolic.
c1=sym('c1','real');
s1=sym('s1','real');
l1=sym('l1','real');
A1=[c1 0 s1 0;s1 0 -c1 0;0 1 0 l1;0 0 0 1];

c2=sym('c2','real');
s2=sym('s2','real');
l2=sym('l2','real');
A2=[c2 -s2 0 (l2*c2);s2 c2 0 (l2*s2);0 0 1 0;0 0 0 1];

c3=sym('c3','real');
c3p2=sym('c3p2','real'); % c3p2 = Cos(th3+pi/2)
s3=sym('s3','real');
s3p2=sym('s3p2','real'); % s3p2 = Sin(th3+pi/2)
A3=[c3p2 0 s3p2 0;s3p2 0 -c3p2 0;0 1 0 0;0 0 0 1];

c4=sym('c4','real');
s4=sym('s4','real');
l3=sym('l3','real');
l4=sym('l4','real');
A4=[c4 0 -s4 0;s4 0 c4 0;0 -1 0 (l3+l4);0 0 0 1];

c5=sym('c5','real');
s5=sym('s4','real');
A5=[c5 0 s5 0;s5 0 -c5 0;0 1 0 0;0 0 0 1];

c6=sym('c6','real');
s6=sym('s6','real');
l5=sym('l5','real');
l6=sym('l6','real');

A6=[c6 -s6 0 0;s6 c6 0 0;0 0 1 (l5+l6);0 0 0 1];

T=A1*A2*A3*A4*A5*A6

Matriz Jacobiana

PRIMERA FILA

[ (l5 + l6)*(sin(th5)*(cos(th1)*sin(th4) - cos(th4)*(cos(th2)*cos(pi/2 +

th3)*sin(th1) - sin(th1)*sin(th2)*sin(pi/2 + th3))) -
cos(th5)*(cos(th2)*sin(th1)*sin(pi/2 + th3) + cos(pi/2 +
th3)*sin(th1)*sin(th2))) - (l3 + l4)*(cos(th2)*sin(th1)*sin(pi/2 + th3) +
cos(pi/2 + th3)*sin(th1)*sin(th2)) - l2*cos(th2)*sin(th1),

- (l3 + l4)*(cos(th1)*sin(th2)*sin(pi/2 + th3) - cos(th1)*cos(th2)*cos(pi/2 +

th3)) - (l5 + l6)*(cos(th5)*(cos(th1)*sin(th2)*sin(pi/2 + th3) -
cos(th1)*cos(th2)*cos(pi/2 + th3)) +
cos(th4)*sin(th5)*(cos(th1)*cos(th2)*sin(pi/2 + th3) + cos(th1)*cos(pi/2 +
th3)*sin(th2))) - l2*cos(th1)*sin(th2),
 - (l3 + l4)*(cos(th1)*sin(th2)*sin(pi/2 + th3) - cos(th1)*cos(th2)*cos(pi/2 +
th3)) - (l5 + l6)*(cos(th5)*(cos(th1)*sin(th2)*sin(pi/2 + th3) -
cos(th1)*cos(th2)*cos(pi/2 + th3)) +
cos(th4)*sin(th5)*(cos(th1)*cos(th2)*sin(pi/2 + th3) + cos(th1)*cos(pi/2 +
th3)*sin(th2))),

sin(th5)*(cos(th4)*sin(th1) + sin(th4)*(cos(th1)*sin(th2)*sin(pi/2 + th3) -

cos(th1)*cos(th2)*cos(pi/2 + th3)))*(l5 + l6),

(l5 + l6)*(cos(th5)*(sin(th1)*sin(th4) - cos(th4)*(cos(th1)*sin(th2)*sin(pi/2

+ th3) - cos(th1)*cos(th2)*cos(pi/2 + th3))) -
sin(th5)*(cos(th1)*cos(th2)*sin(pi/2 + th3) + cos(th1)*cos(pi/2 +
th3)*sin(th2))),

0]

SEGUNDA FILA

[ (l3 + l4)*(cos(th1)*cos(th2)*sin(pi/2 + th3) + cos(th1)*cos(pi/2 +

th3)*sin(th2)) + (l5 + l6)*(sin(th5)*(sin(th1)*sin(th4) -
cos(th4)*(cos(th1)*sin(th2)*sin(pi/2 + th3) - cos(th1)*cos(th2)*cos(pi/2 +
th3))) + cos(th5)*(cos(th1)*cos(th2)*sin(pi/2 + th3) + cos(th1)*cos(pi/2 +
th3)*sin(th2))) + l2*cos(th1)*cos(th2),

(l3 + l4)*(cos(th2)*cos(pi/2 + th3)*sin(th1) - sin(th1)*sin(th2)*sin(pi/2 +

th3)) + (l5 + l6)*(cos(th5)*(cos(th2)*cos(pi/2 + th3)*sin(th1) -
sin(th1)*sin(th2)*sin(pi/2 + th3)) -
cos(th4)*sin(th5)*(cos(th2)*sin(th1)*sin(pi/2 + th3) + cos(pi/2 +
th3)*sin(th1)*sin(th2))) - l2*sin(th1)*sin(th2),

(l3 + l4)*(cos(th2)*cos(pi/2 + th3)*sin(th1) - sin(th1)*sin(th2)*sin(pi/2 +

th3)) + (l5 + l6)*(cos(th5)*(cos(th2)*cos(pi/2 + th3)*sin(th1) -
sin(th1)*sin(th2)*sin(pi/2 + th3)) -
cos(th4)*sin(th5)*(cos(th2)*sin(th1)*sin(pi/2 + th3) + cos(pi/2 +
th3)*sin(th1)*sin(th2))),

-sin(th5)*(l5 + l6)*(cos(th1)*cos(th4) + sin(th4)*(cos(th2)*cos(pi/2 +

th3)*sin(th1) - sin(th1)*sin(th2)*sin(pi/2 + th3))),

-(l5 + l6)*(cos(th5)*(cos(th1)*sin(th4) - cos(th4)*(cos(th2)*cos(pi/2 +

th3)*sin(th1) - sin(th1)*sin(th2)*sin(pi/2 + th3))) +
sin(th5)*(cos(th2)*sin(th1)*sin(pi/2 + th3) + cos(pi/2 +
th3)*sin(th1)*sin(th2))),

0]

TERCERA FILA

[0,

(l3 + l4)*(cos(th2)*sin(pi/2 + th3) + cos(pi/2 + th3)*sin(th2)) + l2*cos(th2)

+ (cos(th5)*(cos(th2)*sin(pi/2 + th3) + cos(pi/2 + th3)*sin(th2)) +
cos(th4)*sin(th5)*(cos(th2)*cos(pi/2 + th3) - sin(th2)*sin(pi/2 + th3)))*(l5 +
l6),
(l3 + l4)*(cos(th2)*sin(pi/2 + th3) + cos(pi/2 + th3)*sin(th2)) +
(cos(th5)*(cos(th2)*sin(pi/2 + th3) + cos(pi/2 + th3)*sin(th2)) +
cos(th4)*sin(th5)*(cos(th2)*cos(pi/2 + th3) - sin(th2)*sin(pi/2 + th3)))*(l5 +
l6),

-sin(th4)*sin(th5)*(l5 + l6)*(cos(th2)*sin(pi/2 + th3) + cos(pi/2 +

th3)*sin(th2)),

(sin(th5)*(cos(th2)*cos(pi/2 + th3) - sin(th2)*sin(pi/2 + th3)) +

cos(th4)*cos(th5)*(cos(th2)*sin(pi/2 + th3) + cos(pi/2 + th3)*sin(th2)))*(l5 +
l6),

0]

% Programa en Matlab para calcular el Jacobiano de la matriz T.

th1=sym('th1','real');
%s1=sym('s1','real');
l1=sym('l1','real');
A1=[cos(th1) 0 sin(th1) 0;sin(th1) 0 -cos(th1) 0;0 1 0 l1;0 0 0 1];

th2=sym('th2','real');
%s2=sym('s2','real');
l2=sym('l2','real');
A2=[cos(th2) -sin(th2) 0 (l2*cos(th2));sin(th2) cos(th2) 0 (l2*sin(th2));0 0 1
0;0 0 0 1];

th3=sym('th3','real');
%s3=sym('s3','real');
sym(pi/2);
A3=[(cos(th3+pi/2)) 0 (sin(th3+pi/2)) 0;(sin(th3+pi/2)) 0 (-cos(th3+pi/2)) 0;0
1 0 0;0 0 0 1];

th4=sym('th4','real');
%s4=sym('s4','real');
l3=sym('l3','real');
l4=sym('l4','real');
A4=[cos(th4) 0 -sin(th4) 0;sin(th4) 0 cos(th4) 0;0 -1 0 (l3+l4);0 0 0 1];

th5=sym('th5','real');
%s5=sym('s4','real');
A5=[cos(th5) 0 sin(th5) 0;sin(th5) 0 -cos(th5) 0;0 1 0 0;0 0 0 1];

th6=sym('th6','real');
%s6=sym('s6','real');
l5=sym('l5','real');
l6=sym('l6','real');
A6=[cos(th6) -sin(th6) 0 0;sin(th6) cos(th6) 0 0;0 0 1 (l5+l6);0 0 0 1];

T=A1*A2*A3*A4*A5*A6;

x=T(1,4);
y=T(2,4);
z=T(3,4);
f=[x; y; z];
Cord_Art=[th1, th2, th3, th4, th5, th6];

J=jacobian(f,Cord_Art)

%====================================================================
%Calculo de matriz Jacobiana de velocidad angular
R1=A1(1:3,1:3);
R2aux=A1*A2;
R2=R2aux(1:3,1:3);
R3aux=R2aux*A3;
R3=R3aux(1:3,1:3);
R4aux=R3aux*A4;
R4=R4aux(1:3,1:3);
R5aux=R4aux*A5;
R5=R5aux(1:3,1:3);
R6aux=R5aux*A6;
R6=R6aux(1:3,1:3);
Jw=[R1(:,3), R2(:,3), R3(:,3), R4(:,3), R5(:,3), R6(:,3)]
%====================================================================

PRIMERA FILA

[ s1,
s1,
c1*c2*s3p2 + c1*c3p2*s2,
c4*s1 - s4*(c1*c2*c3p2 - c1*s2*s3p2),
c5*(c1*c2*s3p2 + c1*c3p2*s2) + s4*(s1*s4 + c4*(c1*c2*c3p2 - c1*s2*s3p2)),
c5*(c1*c2*s3p2 + c1*c3p2*s2) + s4*(s1*s4 + c4*(c1*c2*c3p2 - c1*s2*s3p2))]

SEGUNDA FILA

[ -c1,
-c1,
c2*s1*s3p2 + c3p2*s1*s2,
- c1*c4 - s4*(c2*c3p2*s1 - s1*s2*s3p2),
c5*(c2*s1*s3p2 + c3p2*s1*s2) - s4*(c1*s4 - c4*(c2*c3p2*s1 - s1*s2*s3p2)),
c5*(c2*s1*s3p2 + c3p2*s1*s2) - s4*(c1*s4 - c4*(c2*c3p2*s1 - s1*s2*s3p2))]

TERCERA FILA

[ 0,
0,
s2*s3p2 - c2*c3p2,
-s4*(c2*s3p2 + c3p2*s2),
c4*s4*(c2*s3p2 + c3p2*s2) - c5*(c2*c3p2 - s2*s3p2),
c4*s4*(c2*s3p2 + c3p2*s2) - c5*(c2*c3p2 - s2*s3p2)]