Cinemática directa

θi αi di ai
1 θ1 0° 0 L1
2 θ2 0° 0 L2
3 θ3 0° 0 L3

[ ]
cθi −s θi . cαi sθi. sαi ai. cθi
sθi cθi. cαi −cθi . sαi ai . sθi
Ai=
0 s αi c αi di
0 0 0 1

[ ] [ ] [ ]
cθ 1 −s θ 1 0 L 1.cθ 1 cθ 2 −s θ 2 0 L 2. cθ 2 cθ 3 −s θ 3 0 L3. cθ 3
sθ 1 cθ 1 0 L 1. sθ 1 sθ 2 cθ 2 0 L 2. sθ 2 sθ 3 cθ 3 0 L3. sθ 3
A1= A2= A3=
0 0 1 0 0 0 1 0 0 0 1 0
0 0 0 1 0 0 0 1 0 0 0 1
0
T3 =0A3= A1. A2. A3
0
A2= A1. A2

[ ][ ]
cθ 1 −s θ 1 0 L 1.cθ 1 cθ 2 −s θ 2 0 L2. cθ 2
sθ 1 cθ 1 0 L 1. sθ 1 . sθ 2 cθ 2 0 L 2. sθ 2
=
0 0 1 0 0 0 1 0
0 0 0 1 0 0 0 1

[ ]
c (θ 1+ θ 2) −s (θ 1+θ 2) 0 L 2 c ( θ 1+θ 2 ) + L1. cθ 1
s(θ 1+θ 2) c (θ 1+θ 2) 0 L 2 s ( θ 1+θ 2 )+ L1. sθ 1
=
0 0 1 0
0 0 0 1
0
T3 =0A3=0A2. A3= A1. A2. A3

[ ][ ]
c (θ 1+ θ 2) −s (θ 1+θ 2) 0 L 2 c ( θ 1+θ 2 ) + L1. cθ 1 cθ 3 −s θ 3 0 L 3.cθ 3
s(θ 1+θ 2) c (θ 1+θ 2) 0 L 2 s ( θ 1+θ 2 )+ L1. sθ 1 sθ 3 cθ 3 0 L 3. sθ 3
=
0 0 1 0 0 0 1 0
0 0 0 1 0 0 0 1

[ ]
c (θ 1+ θ 2+ θ 3) −s (θ 1+θ 2+θ 3) 0 L 3 c (θ 1+θ 2+θ 3 ) + L 2 c ( θ 1+θ 2 ) + L1. cθ 1
s(θ 1+θ 2+θ 3) c (θ 1+θ 2+θ 3) 0 L3 s ( θ 1+ θ 2+θ 3 )+ L 2 s (θ 1+θ 2 ) + L 1. sθ 1
=
0 0 1 0
0 0 0 1
θ 1=θ 2=θ 3=0

[ ] [ ] [ ]
1 0 0 L3+ L 2+ L 1
1 0 0 L3+ L 2+ L1
0 1 0 0
0
T3= R3 = 0 1 0
0 0
t2= 0
0 0 1 0
0 0 1 0
0 0 0 1

Cinemática inversa

Cinematica diferencial (jacobiano )

Para el robot RRR

J6x3= [ Z o x ( P−P o) Z 1 x (P−P 1) Z 2 x ( P−P2)

Zo Z1 Z2 ] Zox(P-Po) Z1x(P-P1) Z2x(P-P2) Zo Z1 Z2

[ ]
c (θ 1+ θ 2+ θ 3) −s (θ 1+θ 2+θ 3) 0 L 3 c (θ 1+θ 2+θ 3 ) + L 2 c ( θ 1+θ 2 ) + L1. cθ 1
0 s(θ 1+θ 2+θ 3) c (θ 1+θ 2+θ 3) 0 L3 s ( θ 1+ θ 2+θ 3 )+ L 2 s (θ 1+θ 2 ) + L 1. sθ 1
A3=
0 0 1 0
0 0 0 1

[] [] [ ]
0 0 L3 c ( θ1+θ 2+θ 3 )+ L 2 c ( θ 1+θ 2 ) + L1. cθ 1
Zo= 0 P o= 0 P= L3 s ( θ 1+θ 2+ θ 3 ) + L2 s ( θ1+θ 2 ) + L 1. sθ 1
1 0 0

Zox(P-Po)

[][ ][ ]
0 L3 c ( θ1+θ 2+θ 3 )+ L 2 c ( θ 1+θ 2 ) + L1. cθ 1 −L3 s ( θ 1+ θ 2+ θ 3 )−L 2 s ( θ 1+θ 2 )−L 1. sθ 1
0 x L3 s ( θ 1+θ 2+ θ 3 ) + L2 s ( θ1+θ 2 ) + L 1. sθ 1 = L3 c ( θ 1+θ 2+ θ 3 ) + L2 c (θ 1+θ 2 ) + L 1. cθ 1
1 0 0

0
A1=A1

[ ]
cθ 1 −s θ 1 0 L 1.cθ 1
sθ 1 cθ 1 0 L 1. sθ 1
=
0 0 1 0
0 0 0 1

[] [ ]
0 L1. cθ 1
Z1= 0 P1= L1. sθ 1
1 0

Z1x(P-P1)

[ ][ ][ ]
0 L3 c ( θ1+θ 2+θ 3 )+ L 2 c ( θ 1+θ 2 ) −L3 s ( θ 1+θ 2+ θ 3 )−L 2 s ( θ 1+θ 2 )
0 x L3 s ( θ1+θ 2+θ 3 )+ L 2 s ( θ 1+θ 2 ) = L 3 c ( θ 1+θ 2+θ 3 ) + L2 c ( θ1+θ 2 )
1 0 0
0
A2= A1. A2
 [ ]
c (θ 1+ θ 2) −s (θ 1+θ 2) 0 L 2 c ( θ 1+θ 2 ) + L1. cθ 1
s(θ 1+θ 2) c (θ 1+θ 2) 0 L 2 s ( θ 1+θ 2 )+ L1. sθ 1
=
0 0 1 0
0 0 0 1

[] [ ]
0 L2 c (θ 1+θ 2 ) + L 1. cθ1
Z2= 0 P2= L 2 s ( θ 1+θ 2 ) + L1. sθ 1
1 0

Z2x(P-P2)

[ ][ ][ ]
0 L3 c ( θ 1+θ 2+θ 3 ) −L3 s ( θ 1+ θ 2+ θ 3 )
0 x L 3 s ( θ 1+θ 2+θ 3 ) = L 3 c ( θ 1+θ 2+θ 3 )
1 0 0

[ ]
−L3 s ( θ 1+ θ 2+ θ 3 )−L 2 s ( θ 1+θ 2 )−L 1. sθ 1 −L 3 s (θ 1+θ 2+θ 3 ) −L2 s (θ 1+θ 2 ) −L3 s ( θ 1+ θ 2+θ 3 )
L3 c ( θ 1+θ 2+ θ 3 ) + L2 c (θ 1+θ 2 ) + L 1. cθ 1 L3 c ( θ1+θ 2+θ 3 ) + L 2 c ( θ 1+θ 2 ) L 3 c ( θ 1+θ 2+θ 3 )
J6x3=
0 0 0
0 0 0
0 0 0
1 1 1

Dinámica directa

Energía cinética para el primer estabón:

θi αi di ai
1 θ1 0° 0 Lc1

0
A1=A1

[ ]
cθ 1 −s θ 1 0 L c 1 . cθ 1
sθ 1 cθ 1 0 L c 1 . sθ 1
= x=L c1.cθ1 y=Lc1.sθ1 z=0
0 0 1 0
0 0 0 1

[ ][
dx dx dx

]
dθ1 dθ 2 dθ 3
−L c 1. sθ 1 0 0
dy dy dy
Jv1 = = L c 1.cθ 1 0 0
dθ1 dθ 2 dθ 3
0 0 0
dz dz dz
dθ1 dθ 2 dθ 3
 [ ][ ] [ ]
˙
−L c 1. sθ 1 0 0 θ˙1 −L c 1 . sθ 1. θ1
V1= Jv1.θ̇ 1 = L c 1.cθ 1 0 0 0 = L c 1. cθ 1. θ˙1
0 0 0 0 0

[ ]
−L c 1 . sθ 1. θ˙1
v v =[ −L c 1 . sθ 1. θ˙1 L c 1 . cθ 1. θ˙1 0 ] L c 1. cθ 1. θ˙1 =Lc12 θ̇ 12
T
1 1

[ ][ ]
1 0 0 0
w 1TI1w1=[ 0 0 θ˙1 ] 0 1 0 0 = I1θ̇ 12
˙
0 0 I 1 θ1

1 1 T
K1= m1v1Tv1+ w 1 I1w1
2 2
1 1
K1= m1 Lc12 θ̇ 12+ I1θ̇ 12
2 2
Energía cinética para el segundo estabón:

θi αi di ai
1 θ1 0° 0 L1
2 θ2 0° 0 Lc2

[ ] [ ]
cθ 1 −s θ 1 0 L 1.cθ 1 cθ 2 −s θ 2 0 L c 2 . cθ 2
sθ 1 cθ 1 0 L 1. sθ 1 sθ 2 cθ 2 0 L c 2. sθ 2
A1= A2=
0 0 1 0 0 0 1 0
0 0 0 1 0 0 0 1
0
A2= A1. A2

[ ][ ]
cθ 1 −s θ 1 0 L 1.cθ 1 cθ 2 −s θ 2 0 L c 2 . cθ 2
sθ 1 cθ 1 0 L 1. sθ 1 sθ 2 cθ 2 0 L c 2 . sθ 2
=
0 0 1 0 0 0 1 0
0 0 0 1 0 0 0 1

[ ]
c (θ 1+ θ 2) −s (θ 1+θ 2) 0 Lc 2. c ( θ 1+θ 2 )+ L 1. cθ 1
s(θ 1+θ 2) c (θ 1+θ 2) 0 Lc 2. s (θ 1+θ 2 ) + L 1. sθ 1
=
0 0 1 0
0 0 0 1

x=Lc2c(θ1+θ2)+L1.cθ1 y= Lc2s(θ1+θ2)+L1.sθ1 z=0

 [ ][
dx dx dx

]
dθ1 dθ 2 dθ 3
−L c 2 s (θ 1+θ 2)−L1 s θ 1 −L c 2 s (θ 1+ θ 2) 0
dy dy dy
Jv2 = = L c 2 c (θ 1+θ 2)+ L 1 c θ 1 L c 2 c (θ 1+θ 2) 0
dθ1 dθ 2 dθ 3
0 0 0
dz dz dz
dθ1 dθ 2 dθ 3

V2= Jv2.θ̇ 1,2

[ ][ ]
−L c 2 s (θ 1+θ 2)−L1 s θ 1 −L c 2 s (θ 1+ θ 2) 0 θ˙1
= L c 2 c (θ 1+θ 2)+ L 1 c θ 1 L c 2 c (θ 1+θ 2) 0 θ˙2
0 0 0 0

[ ]
[−L c 2 s ( θ 1+θ 2 )−L 1 s θ 1 ] θ1−L
˙ c 2 s (θ 1+θ 2) θ˙2
= [ L c 2 c ( θ 1+θ 2 ) + L1 c θ1 ] θ˙1+ L c 2 c (θ 1+ θ 2) θ˙2
0

V2TV2 =

=
[ [−L c 2 s ( θ 1+ θ 2 )−L1 s θ 1 ] θ˙1−L c 2 s(θ 1+θ 2)θ˙2 [ L c 2 c ( θ 1+θ 2 )+ L1 c θ 1 ] θ˙1+ Lc 2 c(θ1+θ 2)θ˙2 0]
.

[ ]
[−L c 2 s ( θ 1+θ 2 )−L 1 s θ 1 ] θ1−L
˙ c 2 s (θ 1+θ 2) θ˙2
[ L c 2 c ( θ 1+θ 2 ) + L1 c θ1 ] θ˙1+ L c 2 c (θ 1+ θ 2) θ˙2
0

= L12θ̇ 12+Lc2(θ̇ 1+θ̇ 2)2+2L1Lc2(θ̇ 12+θ̇ 1θ̇ 2)cosθ2

[ ]
1 0 0
w 2TI2w2=[ 0 0 ( θ̇ 1+ θ̇ 2) ] 0 1 0 ¿= I2(θ̇ 1+θ̇ 2)2
0 0 I2

1 1 T
K2= m2v2Tv2+ w 2 I2w2
2 2
1 1
K2= m2 [L12θ̇ 12+Lc2(θ̇ 1+θ̇ 2)2+2L1Lc2(θ̇ 12+θ̇ 1θ̇ 2)cosθ2] + I2(θ̇ 1+θ̇ 2)2
2 2
Energía cinética para el tercer estabón:

θi αi di ai
1 θ1 0° 0 L1
2 θ2 0° 0 L2
3 θ3 0° 0 Lc3
 [ ] [ ] [ ]
cθ 1 −s θ 1 0 L 1.cθ 1 cθ 2 −s θ 2 0 L 2. cθ 2 cθ 3 −s θ 3 0 Lc 3. cθ 3
sθ 1 cθ 1 0 L 1. sθ 1 sθ 2 cθ 2 0 L 2. sθ 2 sθ 3 cθ 3 0 Lc 3. sθ 3
A1= A2= A3=
0 0 1 0 0 0 1 0 0 0 1 0
0 0 0 1 0 0 0 1 0 0 0 1

0
A2= A1. A2

[ ][ ]
cθ 1 −s θ 1 0 L 1.cθ 1 cθ 2 −s θ 2 0 L2. cθ 2
sθ 1 cθ 1 0 L 1. sθ 1 sθ 2 cθ 2 0 L 2. sθ 2
= .
0 0 1 0 0 0 1 0
0 0 0 1 0 0 0 1

[ ]
c (θ 1+ θ 2) −s (θ 1+θ 2) 0 L 2 c ( θ 1+θ 2 ) + L1. cθ 1
s(θ 1+θ 2) c (θ 1+θ 2) 0 L 2 s ( θ 1+θ 2 )+ L1. sθ 1
=
0 0 1 0
0 0 0 1
0
A3=0A2. A3

[ ][ ]
c (θ 1+ θ 2) −s (θ 1+θ 2) 0 L 2 c ( θ 1+θ 2 ) + L1. cθ 1 cθ 3 −s θ 3 0 Lc 3. cθ 3
s(θ 1+θ 2) c (θ 1+θ 2) 0 L 2 s ( θ 1+θ 2 )+ L1. sθ 1 sθ 3 cθ 3 0 Lc 3. sθ 3
=
0 0 1 0 0 0 1 0
0 0 0 1 0 0 0 1

[ ]
c (θ 1+ θ 2+ θ 3) −s (θ 1+θ 2+θ 3) 0 L c 3 . c ( θ 1+ θ 2+ θ 3 )+ L2 . c ( θ 1+θ 2 ) + L1. cθ 1
s(θ 1+θ 2+θ 3) c (θ 1+θ 2+θ 3) 0 Lc 3 . s ( θ 1+θ 2+θ 3 ) + L 2. s ( θ1+θ 2 ) + L 1. sθ 1
=
0 0 1 0
0 0 0 1

x= LC3c(θ1+θ2+ θ3)+L2c(θ1+θ2)+L1.cθ1 y= LC3s(θ1+θ2+ θ3)+L2s(θ1+θ2)+L1.sθ1 z=0

[ ][
dx dx dx

]
dθ1 dθ 2 dθ 3
−L c 2 s (θ 1+θ 2)−L1 s θ 1 −L c 2 s (θ 1+ θ 2) 0
dy dy dy
Jv3 = = L c 2 c (θ 1+θ 2)+ L 1 c θ 1 L c 2 c (θ 1+θ 2) 0
dθ1 dθ 2 dθ 3
0 0 0
dz dz dz
dθ1 dθ 2 dθ 3

V3= Jv3.θ̇ 1,2,3 θ̇ 3

V3TV3 =

(L12+L22+Lc32+2L1L2cθ2+2L1Lc3c(θ2+ θ3)+2L2Lc3c θ3)θ̇ 12+(L22+Lc32+2L2Lc3cθ3)θ̇ 22+Lc32θ̇ 32+2(Lc32+L2Lc3cθ3)θ̇ 2θ̇ 3

+2(L22+Lc32+L1L2cθ2+ L1Lc3c(θ2+ θ3)+2 L2Lc3c(θ2+ θ3))θ̇ 1θ̇ 2+2(Lc32+L1Lc3c(θ2+ θ3)+ L2Lc3cθ3)θ̇ 1θ̇ 3
 [ ]
1 0 0
w I w3=[ 0 0 ( θ̇ 1+ θ̇ 2+ θ̇ 3) ] 0 1 0 ¿= I2(θ̇ 1+θ̇ 2+θ̇ 3)2
T
3 3

0 0 I3

1 1 T
K3= m3v3Tv3+ w 3 I3w3
2 2

1
K3= m3[(L12+L22+Lc32+2L1L2cθ2+2L1Lc3c(θ2+ θ3)+2L2Lc3c θ3)θ̇ 12+(L22+Lc32+2L2Lc3cθ3)θ̇ 22+Lc32θ̇ 32+2(Lc32+L2Lc3cθ3)θ̇ 2θ̇ 3
2
1
+2(L22+Lc32+L1L2cθ2+ L1Lc3c(θ2+ θ3)+2 L2Lc3c(θ2+ θ3))θ̇ 1θ̇ 2+2(Lc32+L1Lc3c(θ2+ θ3)+ L2Lc3cθ3)θ̇ 1θ̇ 3]+ I3(θ̇ 1+θ̇ 2+θ̇ 3)2
2
L(θ,θ̇ )=K(θ,θ̇ ¿-U(θ) donde : U(θ)=0

L= K1+ K2+ K3

1 1 1 1 1
= m1 Lc12 θ̇ 12+ I1θ̇ 12+ m2 [L12θ̇ 12+Lc2(θ̇ 1+θ̇ 2)2+2L1Lc2(θ̇ 12+θ̇ 1θ̇ 2)cosθ2] +
I2(θ̇ 1+θ̇ 2)2 +
2 2 2 2 2
m3[(L1 +L2 +Lc3 +2L1L2cθ2+2L1Lc3c(θ2+ θ3)+2L2Lc3c θ3)θ̇ 1 +(L2 +Lc3 +2L2Lc3cθ3)θ̇ 2 +Lc3 θ̇ 3 +2(Lc3 +L2Lc3cθ3)θ̇ 2θ̇ 3
2 2 2 2 2 2 2 2 2 2

1
+2(L22+Lc32+L1L2cθ2+ L1Lc3c(θ2+ θ3)+2 L2Lc3c(θ2+ θ3))θ̇ 1θ̇ 2+2(Lc32+L1Lc3c(θ2+ θ3)+ L2Lc3cθ3)θ̇ 1θ̇ 3]+ I3(θ̇ 1+θ̇ 2+θ̇ 3)2
2
d dL
¿) - = τ
dt dθ
d dL
¿) - = τ1
dt dθ 1
d dL
¿) - = τ2
dt dθ 2
d dL
¿) - = τ3
dt dθ 3
Donde : d12=d21 , d13=d31 y d23=d32

1
K= ¿d11θ̇ 12+d22θ̇ 22 + d33θ̇ 32+2d21θ̇ 1θ̇ 2+2d31θ̇ 1θ̇ 3+ 2d32θ̇ 2θ̇ 3¿
2
d11=m1LC12+m2L12+m2Lc22+2m2L1Lc2cθ2+ m3L12+ m3L22+ m3Lc32+2m3L1L2cθ2+2m3L1Lc3c(θ2+ θ3)+ 2m3L2Lc3cθ3+ I1+ I2+ I3

d22= m2LC22+ m3L22+ m3LC32+2m3L2Lc3cθ3+ I2+ I3

d33= m3LC32+ I3

d21= m2LC22+m2L1Lc2cθ2+ m3L22+ m3LC32+ m3L1L2cθ2+m3L1Lc3c(θ2+ θ3)+2m3L2Lc3cθ3+ I2+ I3

d31= m3LC32+ m3L1Lc3c(θ2+ θ3)+ m3L2Lc3cθ3+ I3

d32= m3LC32+ m3L2Lc3cθ3+ I3

 dd 11 1 dd 11 1 dd 11
C111= - = =0
dθ 1 2 dθ 1 2 dθ 1
dd 21 1 dd 11
C112= - = m2L1Lc2sθ2+ m3L1L2sθ2+ m3L1Lc3s(θ2+θ3)
dθ 1 2 dθ 2
dd 31 1 dd 11
C113= - = m3L2Lc3sθ3+ m3L1Lc3s(θ2+θ3)
dθ 1 2 dθ 3
dd 12 1 dd 12 1 dd 12
C121= - = =0
dθ 1 2 dθ 1 2 dθ 1
dd 22 1 dd 12 1 1 1
C122= - = m2L1Lc2sθ2+ m3L1L2sθ2+ m3L1Lc3s(θ2+θ3)
dθ 1 2 dθ 2 2 2 2
dd 32 1 dd 12 1
C123= - = m3L1Lc3s(θ2+θ3)+ m3L2Lc3sθ3
dθ 1 2 dθ 3 2
dd 13 1 dd 13 1 dd 13
C131= - = =0
dθ 1 2 dθ 1 2 dθ 1
dd 23 1 dd 13 1
C132= - = m3L1Lc3s(θ2+θ3)
dθ 1 2 dθ 2 2
dd 33 1 dd 13 1
C133= - = m3L1Lc3s(θ2+θ3)+ m3L2Lc3sθ3
dθ 1 2 dθ 3 2
dd 11 1 dd 21
C211= - = -2m2L1Lc2sθ2-2m3L1L2sθ2-2m3L1Lc3s(θ2+θ3)
dθ 2 2 dθ 1
dd 21 1 dd 21 1 dd 21 1 1 1
C212= - = = - m2L1Lc2sθ2- m3L1L2sθ2- m3L1Lc3s(θ2+θ3)
dθ 2 2 dθ 2 2 dθ 2 2 2 2
dd 31 1 dd 21
C213= - = -m3L1Lc3s(θ2+θ3)+ m3L2Lc3sθ3
dθ 2 2 dθ 3
dd 12 1 dd 22
C221= - = -m2L1Lc2sθ2- m3L1L2sθ2- m3L1Lc3s(θ2+θ3)
dθ 2 2 dθ 1
dd 22 1 dd 22 1 dd 22
C222= - = =0
dθ 2 2 dθ 2 2 dθ 2
dd 32 1 dd 22
C223= - = m3L2Lc3sθ3
dθ 2 2 dθ 3
dd 13 1 dd 23
C231= - =- m3L1Lc3s(θ2+θ3)
dθ 2 2 dθ 1
dd 23 1 dd 23 1 dd 23
C232= - = =0
dθ 2 2 dθ 2 2 dθ 2
dd 33 1 dd 23 1
C233= - = m3L1Lc3s(θ2+θ3)
dθ 2 2 dθ 3 2
 dd 11 1 dd 31
C311= - = -2m3L2Lc3sθ3 -2m3L1Lc3s(θ2+θ3)
dθ 3 2 dθ 1
dd 21 1 dd 31 1
C312= - = - m3L1Lc3s(θ2+θ3)-2m3L2Lc3sθ3
dθ 3 2 dθ 2 2
dd 31 1 dd 31 1 dd 31 1 1
C313= - = = - m3L1Lc3s(θ2+θ3)- m3L2Lc3sθ3
dθ 3 2 dθ 3 2 dθ 3 2 2
dd 12 1 dd 32
C321= - = -m3L1Lc3s(θ2+θ3)-2m3L2Lc3sθ3
dθ 3 2 dθ 1
dd 22 1 dd 32
C322= - =-2m3L2Lc3sθ3
dθ 3 2 dθ 2
dd 32 1 dd 32 1 dd 32 −1
C323= - = = m3L2Lc3sθ3
dθ 3 2 dθ 3 2 dθ 3 2
dd 13 1 dd 33
C331= - = - m3L1Lc3s(θ2+θ3)- m3L2Lc3sθ3
dθ 3 2 dθ 1
dd 23 1 dd 33
C332= - =- m3L2Lc3sθ3
dθ 3 2 dθ 2
dd 33 1 dd 33 1 dd 33
C333= - = =0
dθ 2 2 dθ 3 2 dθ 3
dU dU dU
Φ1= - , Φ2= - y Φ3= -
dθ 1 dθ 2 dθ 3

Donde la ecuación de movimiento es :

τ1 = d11θ̈ 1 + d12θ̈ 2 + d13θ̈ 3 + C111θ̇ 12+ C221θ̇ 22+ C331θ̇ 32+(C121+ C211)θ̇ 1θ̇ 2+(C131+ C311)θ̇ 1θ̇ 3+

(C231+ C321)θ̇ 2θ̇ 3 + Φ1

τ2 = d21θ̈ 1 + d22θ̈ 2 + d23θ̈ 3 + C112θ̇ 12+ C222θ̇ 22+ C332θ̇ 32+(C122+ C212)θ̇ 1θ̇ 2+(C132+ C312)θ̇ 1θ̇ 3+

(C232+ C322)θ̇ 2θ̇ 3 + Φ2

τ3 = d31θ̈ 1 + d32θ̈ 2 + d33θ̈ 3 + C113θ̇ 12+ C223θ̇ 22+ C333θ̇ 32+(C123+ C213)θ̇ 1θ̇ 2+(C133+ C313)θ̇ 1θ̇ 3+

(C233+ C323)θ̇ 2θ̇ 3 + Φ3

τ1 = (m1LC12+m2L12+m2Lc22+2m2L1Lc2cθ2+ m3L12+ m3L22+ m3Lc32+2m3L1L2cθ2+2m3L1Lc3c(θ2+ θ3)+ 2m3L2Lc3cθ3+ I1+ I2+ I3)

θ̈ 1 + (m2LC22+m2L1Lc2cθ2+ m3L22+ m3LC32+m3L1L2cθ2+m3L1Lc3c(θ2+ θ3)+2m3L2Lc3cθ3+ I2+ I3)θ̈ 2 + d13θ̈ 3 + C111θ̇ 12+ C221θ̇ 22+
C331θ̇ 32+(C121+ C211)θ̇ 1θ̇ 2+(C131+ C311)θ̇ 1θ̇ 3+

(C231+ C321)θ̇ 2θ̇ 3 + Φ1