 1 0 0 x cos( γ) -sin( γ)


0 0 0

0 0 0

0

0 sin( β) 0 

1 1 1 0 0 cos( β) 0 0
     
0 cos( α) -sin( α)
Tx( x) := 
1 0 0
Ty( y) := 
1 0 y
Tz( z) := 
1 0 0
Rx( α) := 
0
Ry( β) := 
0
Rz( γ) := 
0 0 0 0 1 0 sin( γ) cos( γ) 0 0
0 0 1 0 0 0 1 0 0 0 1 z  0 sin( α) cos( α) 0  -sin( β) 0 cos( β) 0   0 0 1 0
0  0  0  0 0   0   0 
 0 0 1  0 0 1  0 0 1  0 1  0 0 1  0 0 1

 cos( q1) -sin( q1) 0 0


 1 0 0 0 
A0_1 := Tz( l1)  Rz( q1)  
sin( q1) cos( q1) 0 0
 0 0 1 l1  A5_6 := Tz( l4)  
0 1 0 0
 0  0 0 1 l4 
 0 0 1
0 
 0 0 1
 π  1  0 
 cos 2 + q2 -sin 2  π + q2 0
 
-1 0 
A1_2 := Rx   Rz q2 +  
π π 0 0
2  2  π  π  
 sin 2 + q2 cos 2 + q2 0 0
 
 0 0 0 1

 cos( q3) -sin( q3) 0 l2 

 
A2_3 := Tx( l2)  Rz( q3)   sin( q3) cos( q3) 0 0 
 0 0 1 0
 0 
 1 
 cos q40- π 0 -sin q4 -  π 0 l3 
1
  2
 
 2 
 
 π   -
π  -
π 
A3_4 := Tx( l3)  Rz q4 -   sin q4  cos  q4  0 0
 2   2  2 
 0 0 1 0
 
 0 0 0 1
cos( q5) -sin( q5) 0 0 
 
 -π   0 0 1 0
A4_5 := Rx   Rz( q5)  
 2  -sin( q5) -cos( q5) 0 0 
 0 
 0 0 1
  2 
cos( q1)  cos( q2)  cos( q3)  cos( q4)  cos( q5) - sin( q1)  sin( q5) - cos( q1)  cos( q2)  cos( q5)  sin( q3)  sin( q4) - cos( q1)  cos( q3)  cos( q5)  sin( q2)  sin( q4) - cos( q1)  cos( q4)  cos( q5)  sin( q2)  sin( q3) cos( q1)  cos( q2)  sin( q3)  sin( q4)  sin( q5) - cos( q1)  cos( q2)  cos( q3)  cos( q4)  sin( q5) - cos( q5)  sin( q1) + cos( q1)  cos( q3)  sin( q2)  sin( q4)  sin( q5) + cos( q1)  cos( q4)  sin( q2)  sin( q3)  sin( q5) -sin( q2 + q3 + q4)  cos( q1)  2  sin q1  - 1  ( l4 sin( q2 + q3 + q4) + l3 sin( q2 +
A5_6 := Tz( l4)   2 
 cos( q1 + q2 + q3 + q4) cos( q1 - q2 - q3 - q4)
CDIR := A0_1  A1_2  A2_3  A3_4  A4_5  A5_6 simplify  
cos( q1)  sin( q5) + cos( q2)  cos( q3)  cos( q4)  cos( q5)  sin( q1) - cos( q2)  cos( q5)  sin( q1)  sin( q3)  sin( q4) - cos( q3)  cos( q5)  sin( q1)  sin( q2)  sin( q4) - cos( q4)  cos( q5)  sin( q1)  sin( q2)  sin( q3) cos( q1)  cos( q5) - cos( q2)  cos( q3)  cos( q4)  sin( q1)  sin( q5) + cos( q2)  sin( q1)  sin( q3)  sin( q4)  sin( q5) + cos( q3)  sin( q1)  sin( q2)  sin( q4)  sin( q5) + cos( q4)  sin( q1)  sin( q2)  sin( q3)  sin( q5) - -sin( q1)  ( l4 sin( q2 + q3 + q4) + l3 sin( q2 + q3) +
2 2

 cos( q2 + q3 + q4 + q5) cos( q2 + q3 + q4 - q5)
sin( q2 + q3 + q4)  cos( q5) - cos( q2 + q3 + q4) l1 + l4 cos( q2 + q3 + q4) + l3 cos( q2 + q3) + l2
 2 2

 0 0 0 1

cos( γ)  cos( β) -sin( γ)  cos( β)


sin( β) px 

sin( γ)  cos( α) + cos( γ)  sin( β)  sin( α) cos( γ)  cos( α) - sin( γ)  sin( β)  sin( α) -cos( β)  sin( α)
T := Tx( px)  Ty( py)  Tz( pz)  Rx( α)  Ry( β)  Rz( γ) simplify  
py 
 sin( γ)  sin( α) - cos( γ)  cos( α)  sin( β) cos( γ)  sin( α) + sin( γ)  cos( α)  sin( β) cos( β)  cos( α) pz 
 
 0 0 0 1 

 cos( q5) -sin( q5) 0 

 cos( q1) -sin( q1) 0   cos( q3) -sin( q3) 0 
R 4_5 := submatrix( A4_5 , 0 , 2 , 0 , 2 )   1
R 0_1 := submatrix( A0_1 , 0 , 2 , 0 , 2 )  sin( q1) cos( q1) 0 
 R 2_3 := submatrix( A2_3 , 0 , 2 , 0 , 2 )   sin( q3) cos( q3) 0 
0 0
     
 0 0 1  0 0 1  -sin( q5) -cos( q5) 0 

  π
-sin q4 -  π 0 
1
 cos π + q2 -sin 1  π + q2 0   cos q4 -  1 0 0 
       
2  2 

 2  2   R 5_6 := submatrix( A5_6 , 0 , 2 , 0 , 2 )   0 1 0 
R 3_4 := submatrix( A3_4 , 0 , 2 , 0 , 2 )    π  π   
R 1_2 := submatrix( A1_2 , 0 , 2 , 0 , 2 )   -1  sin q4 -  cos q4 -  0 
 
0 0
2  2  0 0 1 
 π  π    
 sin + q2 cos + q2 0   0 0 1
 2  2  

 cos( q1)  cos( q2)  cos( q3)  cos( q4)  cos( q5) - sin( q1)  sin( q5) - cos( q1)  cos( q2)  cos( q5)  sin( q3)  sin( q4) - cos( q1)  cos( q3)  cos( q5)  sin( q2)  sin( q4) - cos( q1)  cos( q4)  cos( q5)  sin( q2)  sin( q3) cos( q1)  cos( q2)  sin( q3)  sin( q4)  sin( q5) - cos( q1)  cos( q2)  cos( q3)  cos( q4)  sin( q5) - cos( q5)  sin( q1) + cos( q1)  cos( q3)  sin( q2)  sin( q4)  sin( q5) + cos( q1)  cos( q4)  sin( q2)  sin( q3)  sin( q5) -sin( q2 + q3 + q4)  cos( q1) 
 
 cos( q1)  sin( q5) + cos( q2)  cos( q3)  cos( q4)  cos( q5)  sin( q1) - cos( q2)  cos( q5)  sin( q1)  sin( q3)  sin( q4) - cos( q3)  cos( q5)  sin( q1)  sin( q2)  sin( q4) - cos( q4)  cos( q5)  sin( q1)  sin( q2)  sin( q3) cos( q1)  cos( q5) - cos( q2)  cos( q3)  cos( q4)  sin( q1)  sin( q5) + cos( q2)  sin( q1)  sin( q3)  sin( q4)  sin( q5) + cos( q3)  sin( q1)  sin( q2)  sin( q4)  sin( q5) + cos( q4)  sin( q1)  sin( q2)  sin( q3)  sin( q5) cos( q1 + q2 + q3 + q4 ) cos( q1 - q2 - q3 - q4) 
-
R 0_6 := R 0_1  R1_2  R2_3  R3_4  R 4_5  R5_6 simplify   2 2 
 cos( q2 + q3 + q4 + q5) cos( q2 + q3 + q4 - q5) 
 sin( q2 + q3 + q4)  cos( q5) - cos( q2 + q3 + q4) 
 2 2 
 0  l3 
P3_4 := submatrix( A3_4 , 0 , 2 , 3 , 3)   0 
P0_1 := submatrix( A0_1 , 0 , 2 , 3 , 3)   0   
  0 
 l1

0  0 
P4_5 := submatrix( A4_5 , 0 , 2 , 3 , 3)   0 
P1_2 := submatrix( A1_2 , 0 , 2 , 3 , 3)   0   
  0 
0 

 l2  0 
P5_6 := submatrix( A5_6 , 0 , 2 , 3 , 3)   0 
P2_3 := submatrix( A2_3 , 0 , 2 , 3 , 3)  0 
  
   l4 
0 

0  0
ω0_0 :=  0  v0_0 := 0 

   
0  0
0  0 
ω0_0 + q'1  0    0 
-1
ω1_1 := R 0_1
   
 1   q'1 

0 
 ( v0_0 + ω0_0  P0_1 )  0 

-1
v1_1 := R0_1
 
0 

 q'1 sin
π
+ q2 
 
 2  
 π 2 2
+ q2 + sin q2 
π
 cos 2 +
0    2  
ω2_2 := R 1_2 ω1_1 + q'2 0 

-1
 
q'1 cos + q2
π
 
1  2  
 2 2
 cos π + q2 + sin
π
+  
  2
q2
 2  
 
 q'2 
 0 
 ( v1_1 + ω1_1  P1_2 ) simplify   0 
-1
v2_2 := R1_2
 
0 

 q'1 cos + q2  sin( q3)

π
q'1 sin
π
+ q2  cos( q3) 
 
 2  +
 
2

  π 2  2
2 
( ) ( )
2 2
 π  2 2
 cos
π
+ q2 + sin + q2   cos( q3) + sin( q3) 
π 2
  cos 2 + q2 + sin 2 + q2   cos( q3) + sin( q3)
0        2  2   
ω3_3 := R 2_3 ω2_2 + q'3 0 

-1
 
q'1 cos + q2  cos( q3)  
π π
  
q'1 sin + q2  sin( q3)

1 2  -
 2 
 
 2  2
( ) ( )
2 2
  cos + q2 + sin + q2   cos( q3) 2 + sin( q3) 2
π π
 cos π   π  2
+ q2 + sin + q2   cos( q3) + sin( q3)
2 
   2 

2


   2  2   
 
 q'2 + q'3 

 l2 q'2 sin( q3) 

 ( v2_2 + ω2_2  P2_3 ) simplify  l2 q'2 cos( q3) 

-1
v3_3 := R2_3
 
 l2 q'1 sin( q2) 

  q'1 cos + q2  sin( q3)

π
q'1 sin + q2  cos( q3)
π   q'1 cos + q2  cos( q3)
π
q'1 sin + q2  sin( q3)
π 
    
 cos q4 -
π

2  +
2   π
 sin q4 -     2  -
 2  

  2   2  2   2   2  2 
( ) ( ) ( ) ( )
2 2 2 2
π  π  2 2
 cos + q2 + sin + q2   cos( q3) 2 + sin( q3) 2
π π π  π  2 2
 cos + q2 + sin + q2   cos( q3) 2 + sin( q3) 2  
π π
   cos 2 + q2 + sin 2 + q2   cos( q3) + sin( q3)    cos 2 + q2 + sin 2 + q2   cos( q3) + sin( q3)
        2  2    +        2  2   
 2 2 2 2 
cos q4 -  + sin q4 -  cos q4 -  + sin q4 - 
π π π π
 
0   2  2  2  2

ω3_3 + q'4 0 

-1
ω4_4 := R 3_4    π   π     π   π   
     q'1 cos + q2  cos( q3) q'1 sin + q2  sin( q3)   q'1 cos + q2  sin( q3) q'1 sin + q2  cos( q3)  
1 π 2  2   sin q4 -   
π 2  2 
 cos q4 -  - + 
 
2   2  2  2   2  2
( ) 2  ( ) ( 2 
) ( )
2 2 2 2
  π   π   2 2
  π   π   2
  π   π   2 2
  π   π  2
cos + q2 + sin + q2   cos( q3) + sin( q3) 
  cos + q2 + sin + q2  cos( q3) + sin( q3) cos + q2 + sin + q2  cos( q3) + sin( q3)   cos + q2 + sin + q2  cos( q3) + sin( q3)
  2  2    2  2    -   2  2    2  2   
 2 2 2 2 
  π 
cos q4 -  + sin q4 - 
π  π 
cos q4 -  + sin q4 - 
π 
  2  2  2  2 
 
 q'2 + q'3 + q'4 

-l2 q'2 cos( q3 + q4) - l3 q'2 cos( q4) - l3 q'3 cos( q4) 
 ( v3_3 + ω3_3  P3_4 ) simplify   l2 q'2 sin( q3 + q4) + l3 q'2 sin( q4) + l3 q'3 sin( q4) 
-1
v4_4 := R3_4
 
 q'1 ( l3 sin( q2 + q3) + l2 sin( q2) ) 

0
ω4_4 + q'5 0  

-1
ω5_5 := R 4_5
 
1

 l2 q'2 cos( q5)  sin( q3)  sin( q4) - l3 q'3 cos( q4)  cos( q5) - l2 q'1 sin( q2)  sin( q5) - l2 q'2 cos( q3)  cos( q4)  cos( q5) - l3 q'1 cos( q2)  sin( q3)  sin( q5) - l3 q'1 cos( q3)  sin( q2)  sin( q5) - l3 q'2 cos( q4)  cos( q5) 
v5_5 := R4_5
-1
 ( v4_4 + ω4_4  P4_5 ) simplify   l2 q'2 cos( q3 + q4)  sin( q5) - l3 q'1 sin( q2 + q3)  cos( q5) - l2 q'1 cos( q5)  sin( q2) + l3 q'2 cos( q4)  sin( q5) + l3 q'3 cos( q4)  sin( q5) 
 
 l2 q'2 sin( q3 + q4) + l3 q'2 sin( q4) + l3 q'3 sin( q4) 

0
ω5_5 + 0  0  

-1
ω6_6 := R 5_6
 
1

 l2 q'2 cos( q5)  sin( q3)  sin( q4) - l4 q'3 cos( q5) - l4 q'4 cos( q5) - l3 q'2 cos( q4)  cos( q5) - l3 q'3 cos( q4)  cos( q5) - l2 q'1 sin( q2)  sin( q5) - l2 q'2 cos( q3)  cos( q4)  cos( q5) - l3 q'1 cos( q2)  sin( q3)  sin( q5) - l3 q'1 cos( q3)  sin( q2)  sin( q5) - l4 q'2 cos( q5) - l4 q'1 cos( q2)  cos( q3)  sin( q4)  sin( q5) - l4 q'1 cos( q2)  cos( q4)  sin( q3)  sin( q5) - l4 q'1 cos( q3)  cos( q4)  sin( q2)  sin( q5) + l4 q'1 sin( q2)  sin( q3)  sin( q4)  sin( q5) 
 ( v5_5 + ω5_5  P5_6 ) simplify  l4 q'2 sin( q5) + l4 q'3 sin( q5) + l4 q'4 sin( q5) - l2 q'1 cos( q5)  sin( q2) + l3 q'2 cos( q4)  sin( q5) + l3 q'3 cos( q4)  sin( q5) - l3 q'1 cos( q2)  cos( q5)  sin( q3) - l3 q'1 cos( q3)  cos( q5)  sin( q2) + l2 q'2 cos( q3)  cos( q4)  sin( q5) - l2 q'2 sin( q3)  sin( q4)  sin( q5) - l4 q'1 cos( q2)  cos( q3)  cos( q5)  sin( q4) - l4 q'1 cos( q2)  cos( q4)  cos( q5)  sin( q3) - l4 q'1 cos( q3)  cos( q4)  cos( q5)  sin( q2) + l4 q'1 cos( q5)  sin( q2)  sin( q3)  sin( q4) 

-1
v6_6 := R5_6
 
 l2 q'2 sin( q3 + q4) + l3 q'2 sin( q4) + l3 q'3 sin( q4) 

 l4 q'1 cos( q1 - q2 - q3 - q4) - l4 q'2 cos( q1 + q2 + q3 + q4) - l4 q'3 cos( q1 + q2 + q3 + q4) - l4 q'4 cos( q1 + q2 + q3 + q4) - l2 q'1 cos( q1 + q2) - l2 q'2 cos( q1 + q2) - l4 q'1 cos( q1 + q2 + q3 + q4) - l4 q'2 cos( q1 - q2 - q3 - q4) - l4 q'3 cos( q1 - q2 - q3 - q4) - l4 q'4 cos( q1 - q2 - q3 - q4) + l3 q'1 cos( q1 - q2 - q3) - l3 q'2 cos( q1 - q2 - q3) - l3 q'3 cos( q1 - q2 - q3) + l2 q'1 cos( q1 - q2) - l2 q'2 cos( q1 - q2) - l3 q'1 cos( q1 + q2 + q3) - l3 q'2 cos( q1 + q2 + q3) - l3

2 2 2 2 2 2 2 2 2 2 2 2 2 2 2 2 2

v0_6 := R0_6  v6_6 simplify   l4 q'1 sin( q1 - q2 - q3 - q4) l4 q'2 sin( q1 + q2 + q3 + q4) l4 q'3 sin( q1 + q2 + q3 + q4) l4 q'4 sin( q1 + q2 + q3 + q4) l2 q'1 sin( q1 + q2) l2 q'2 sin( q1 + q2) l4 q'1 sin( q1 + q2 + q3 + q4) l4 q'2 sin( q1 - q2 - q3 - q4) l4 q'3 sin( q1 - q2 - q3 - q4) l4 q'4 sin( q1 - q2 - q3 - q4) l3 q'1 sin( q1 - q2 - q3) l3 q'2 sin( q1 - q2 - q3) l3 q'3 sin( q1 - q2 - q3) l2 q'1 sin( q1 - q2) l2 q'2 sin( q1 - q2) l3 q'1 sin( q1 + q2 + q3) l3 q'2 sin( q1 + q2 + q3) l3 q'3
- - - - - - - - - + - - + - - - -
 2 2 2 2 2 2 2 2 2 2 2 2 2 2 2 2 2

 -l3 q'2 sin( q2 + q3) - l3 q'3 sin( q2 + q3) - l2 q'2 sin( q2) - l4 q'2 sin( q2 + q3 + q4) - l4 q'3 sin( q2 + q3 + q4) - l4 q'4 sin( q2 + q3 + q4)

 q'2 sin( q1) - q'5 sin( q1 + q2 + q3 + q4) + q'3 sin( q1) + q'4 sin( q1) + q'5 sin( q1 - q2 - q3 - q4) 
 
2 2
 
ω0_6 := R 0_6  ω6_6 simplify   q'5 cos( q1 + q2 + q3 + q4) q'5 cos( q1 - q2 - q3 - q4) 
- q'2 cos( q1) - q'3 cos( q1) - q'4 cos( q1) -
 2 2 
 
 q'1 + q'5 cos( q2 + q3 + q4) 
 d v d d d d 
 0_6 0 v0_6 v0_6 v0_6 v0_6
 dq'1 dq'2 0 dq'3 0 dq'4 0 dq'5 0
 l4 ( cos( q1 - q2 - q3 - q4) + cos( q1 + q2 + q3 + q4) )
d d d d d   sin( q1)  ( l4 sin( q2 + q3 + q4) + l3 sin( q2 + q3) + l2 sin( q2) ) -cos( q1)  ( l4 cos( q2 + q3 + q4) + l3 cos( q2 + q3) + l2 cos( q2) ) -cos( q1)  ( l4 cos( q2 + q3 + q4) + l3 cos( q2 + q3) ) - 0 
2
 dq'1 v0_6 1 dq'2
v0_6
1 dq'3
v0_6
1 dq'4
v0_6
1 dq'5
v0_6 
1  
   2  l4 ( sin( q1 - q2 - q3 - q4) + sin( q1 + q2 + q3 + q4) )

2  sin  - 1   ( l4 sin( q2 + q3 + q4) + l3 sin( q2 + q3) + l2 sin( q2) ) -sin( q1)  ( l4 cos( q2 + q3 + q4) + l3 cos( q2 + q3) + l2 cos( q2) ) -sin( q1)  ( l4 cos( q2 + q3 + q4) + l3 cos( q2 + q3) ) -
q1
d v d
v0_6
d
v0_6
d
v0_6
d
v0_6
   0 
 dq'1 0_6 2 dq'2 2 dq'3 2 dq'4 2 dq'5 2   2  2 
J0_6 :=   simplify   0 -l4 sin( q2 + q3 + q4) - l3 sin( q2 + q3) - l2 sin( q2) -l4 sin( q2 + q3 + q4) - l3 sin( q2 + q3) -l4 sin( q2 + q3 + q4) 0 
 d ω0_6 d
ω0_6
d
ω
d
ω
d
ω0_6   0 sin( q1) sin( q1) sin( q1) -sin( q2 + q3 + q4)  cos( q1)

 dq'1 0 dq'2 0 dq'3 0_6 0 dq'4 0_6 0 dq'5 0  
d   cos( q1 + q2 + q3 + q4) cos( q1 - q2 - q3 - q4) 
d d d d 0 -cos( q1) -cos( q1) -cos( q1) -
 ω0_6 ω0_6 ω0_6 ω0_6 ω0_6   2 2 
 d q'1 1 dq'2 1 dq'3 1 dq'4 1 dq'5 1
  
d   1 0 0 0 cos( q2 + q3 + q4) 
d d d d
 dq'1 ω0_6 2 ω0_6 ω0_6 ω
2 dq'4 0_6 2
ω0_6 
 dq'2 2 dq'3 dq'5 2

 d v6_6 d
v6_6
d
v6_6
d
v6_6
d
v6_6

 dq'1 0 dq'2 0 dq'3 0 dq'4 0 dq'5 0

d d d d d 
 -sin( q5)  ( l4 sin( q2 + q3 + q4) + l3 sin( q2 + q3) + l2 sin( q2) ) -cos( q5)  ( l4 + l2 cos( q3 + q4) + l3 cos( q4) ) -cos( q5)  ( l4 + l3 cos( q4) ) -l4 cos( q5) 0
 dq'1 v6_6 1 v6_6 v6_6 v6_6 v6_6 

dq'2 1 dq'3 1 dq'4 1 dq'5 1
  
   q5  - 1 
2
d v  
d d d d 2 sin   ( l4 sin( q2 + q3 + q4) + l3 sin( q2 + q3) + l2 sin( q2) ) sin( q5)  ( l4 + l2 cos( q3 + q4) + l3 cos( q4) ) sin( q5)  ( l4 + l3 cos( q4) ) l4 sin( q5) 0
 dq'1 6_6 2
v6_6 v6_6 v6_6 v6_6
   2   
 simplify   
dq'2 2 dq'3 2 dq'4 2 dq'5 2
J6_6 :=  0 l2 sin( q3 + q4) + l3 sin( q4) l3 sin( q4) 0 0
 d ω6_6 d d d d   
 dq'1 0 dq'2
ω6_6
0 dq'3
ω6_6 ω
0 dq'4 6_6 0 dq'5
ω6_6
0  sin( q2 + q3 + q4)  cos( q5) -sin( q5) -sin( q5) -sin( q5) 0
 -sin( q2 + q3 + q4)  sin( q5) -cos( q5) -cos( q5) -cos( q5) 0
d d d d d
  
 ω6_6 ω6_6 ω ω ω6_6   cos( q2 + q3 + q4) 0 0 0 1
1 dq'3 6_6 1 dq'4 6_6 1
 dq'1 1 dq'2 dq'5 1

d d d d d 
 dq'1 ω6_6 2 ω6_6 ω ω
2 dq'3 6_6 2 dq'4 6_6 2
ω6_6 
 dq'2 dq'5 2

  2  
 sin( q1)  ( l4 sin( q2 + q3 + q4) + l3 sin( q2 + q3) + l2 sin( q2) )  2 sin q1  - 1  ( l4 sin( q2 + q3 + q4) + l3 sin( q2 + q3) + l2 sin( q2) ) 0 0 0 1 
  2  
-cos( q1)  ( l4 cos( q2 + q3 + q4) + l3 cos( q2 + q3) + l2 cos( q2) ) -sin( q1)  ( l4 cos( q2 + q3 + q4) + l3 cos( q2 + q3) + l2 cos( q2) ) -l4 sin( q2 + q3 + q4) - l3 sin( q2 + q3) - l2 sin( q2) sin( q1) -cos( q1) 0 
 
T
JT := J0_6 simplify   -cos( q1)  ( l4 cos( q2 + q3 + q4) + l3 cos( q2 + q3) ) -sin( q1)  ( l4 cos( q2 + q3 + q4) + l3 cos( q2 + q3) ) -l4 sin( q2 + q3 + q4) - l3 sin( q2 + q3) sin( q1) -cos( q1) 0

 - l4 ( cos( q1 - q2 - q3 - q4) + cos( q1 + q2 + q3 + q4) ) -
l4 ( sin( q1 - q2 - q3 - q4) + sin( q1 + q2 + q3 + q4) )
-l4 sin( q2 + q3 + q4) sin( q1) -cos( q1) 0

 2 2 
 cos( q1 + q2 + q3 + q4) cos( q1 - q2 - q3 - q4)

 0 0 0 -sin( q2 + q3 + q4)  cos( q1) - cos( q2 + q3 + q4) 
 2 2 

 2 2 2
( ) 2
cos( q1)  ( l4 sin( q2 + q3 + q4) + l3 sin( q2 + q3) + l2 sin( q2) ) - cos( q1) - 1  ( l4 sin( q2 + q3 + q4) + l3 sin( q2 + q3) + l2 sin( q2) ) + 1 0 0 0 cos( q2 + q3 + q4) 
 0
2 2 2 2 2 2
l2 + 2  cos( q3)  l2 l3 + 2 cos( q3 + q4)  l2 l4 + l3 + 2 cos( q4)  l3 l4 + l4 + 1 l3 + 2  cos( q4)  l3 l4 + l2 cos( q3)  l3 + l4 + l2 cos( q3 + q4)  l4 + 1 l4 + l2 l4 cos( q3 + q4) + l3 l4 cos( q4) + 1

0
 
Jnuevo := JT J0_6 simplify   2 2 2 2 2 
0 l3 + 2 cos( q4)  l3 l4 + l2 cos( q3)  l3 + l4 + l2 cos( q3 + q4)  l4 + 1 l3 + 2  cos( q4)  l3 l4 + l4 + 1 l4 + l3 cos( q4)  l4 + 1 0
 
 0
2
l4 + l2 l4 cos( q3 + q4) + l3 l4 cos( q4) + 1
2
l4 + l3 cos( q4)  l4 + 1
2
l4 + 1 0 
 
 cos( q2 + q3 + q4) 0 0 0 1 

2  l2 1
2 2 2 2 2 2
2 2 l3  cos( 2  q2 + 2  q3) cos( 2  q2 + 2  q3 + 2  q4) l3 l4 l2  cos( 2  q2) l4  cos( 2  q2 + 2  q3 + 2  q4)
detJ := Jnuevo simplify  l2  l3  sin( q3)   - - + + - - + l2 l4 cos( q3 + q4) + l2 l3 cos( q3) + l3 l4 cos( q4) - l3 l4 cos( 2  q2 + 2  q3 + q4) - l2 l3 cos( 2  q2 + q3) - l2 l4 cos( 2 q2 + q3 + q4) + 
 2 2 2 2 2 2 2 2
 
q3) + l2 sin( q2) ) 


+ l2 sin( q2) ) 


l2 cos( q2)



l3 q'3 cos( q1 + q2 + q3) 

2

q'3 sin( q1 + q2 + q3) 
2 

