Professional Documents
Culture Documents
3 Grados de libertad
RRR
Examen Parcial
Curso : Robotica Movl
Alumno : Castro Martinez, Alexander
CINEMATICA DIRECTA
Matriz de transformacion de coordenadas de "j" Para rotaciones respecto al eje Y
respecto a "i" donde:
cos( j) 0 sin j ( ) Lxi
j = Es la rotacion del sistema "j" con
Ti_j( j , Lxi , Lz i ) :=
respecto al sistema "i". 0 1 0 0
(Lx,Ly) = Es la traslac ion del sistema "j" c on sin( j) 0 cos( j ) Lzi
respecto al sistema "i".
0 0 0 1
Matrices de rotacion
cos( 1 ) ( )
0 sin 1 0 cos( 2 ) 0 sin 2( ) L1
T0_1 := Ti_j( 1 , 0 , 0 )
0
T1_2 := Ti_j( 2 , L1 , 0 )
0 1 0 0 1 0 0
sin( 1 ) 0 cos( 1 ) 0 sin( 2 ) 0 cos 2( ) 0
0 0 0 1 0 0 0 1
cos( 3 ) ( )
0 sin 3 L2 1
0 0 L3
T3_4 := Ti_j( 0 , L3 , 0 )
0 1 0 0
T2_3 := Ti_j( 3 , L2 , 0 )
0 1 0 0
0 0 1 0
sin( 3 ) ( )
0 cos 3 0
0 0 0 1
0 0 0 1
cos( 1 + 2 + 3 ) (
0 sin 1 + 2 + 3 ) (
L3 cos 1 +
T0_4 := T0_1 T1_2 T2_3 T3_4 simplify
0 1 0
sin( 1 + 2 + 3 ) (
0 cos 1 + 2 + 3 ) (
L3 sin 1 +
0 0 0
(
x0 := P0_3 L2 cos 1 + 2 + L1 cos 1
0
) ( )
y0 := P0_3 0
1
(
z0 := P0_3 L2 sin 1 + 2 L1 sin 1
2
) ( )
Posicion y Orientacion
( )
x = L2 cos 1 + 2 + L1 cos 1 ( )
z = L2 sin( 1 + 2 ) L1 sin( 1 )
No consideraremos la longitud del efector final para la
cos( ) = cos( 1 + 2 + 3 ) cinematica inversa
sin( ) = sin( 1 + 2 + 3 )
( 1) (L2) ( 1) (L2)
2 2 2 2 2 2 2 2
x +z L x +z L
( 2)
cos
2 L L
D :=
2 L L
1 2 1 2
( 2) :=
2
sin 1D Dependiendo del signo elegido tenemos
( )
cos 2 = D dos valores. Los cuales representas
nuestras 2 soluciones.
( 2) :=
2
sin 1D
Elbow up (codo arriba).
Elbow down (codo abajo).
L sin( )
1 y 1
x tan L + L cos
2 2
:= tan
1
1 2 ( 2)
1 D2 1 D2
Elbow UP := arctan Elbow DOWN := arctan
2
D 2
D
Luego
d x d
x0_4
d
x0_4
d 0_4 d2 d3
1
d d d
JP0_4 := y0_4 y0_4 y0_4
d1 d2 d3
d d d
d z0_4 d2
z0_4
d3
z0_4
1
cos( 1 ) 0 sin( 1 )
cos( 1) 0 sin( 1)
R 1_0 simplify 0
R 0_1 0 1 0
1 0
sin( ) 0 cos( ) sin( ) 0 cos( )
1 1 1 1
1
( )
R 1_2 := R i_j 2 R 2_1 := R 1_2
1
( )
R 2_3 := R i_j 3 R 3_2 := R 2_3
1
R 3_4 := R i_j( 0 ) R 4_3 := R 3_4
1 0 0 1 0 0
R 3_4 0 1 0 R 4_3 simplify 0 1 0
0 0 1 0 0 1
0 0 0
w0_0 := 0 w1_1 := R 1_0 w0_0 + '1 1 w1_1 '1
0 0 0
0 0
v0_0 := 0 P0_1 := 0
0 0
0
(
v1_1 := R1_0 v0_0 + w0_0 P0_1 ) v1_1 0
0
0 0
w2_2 := R 2_1 w1_1 + '2 1 w2_2 '1 + '2
0
0
L1 L1 '1 sin( 2)
P1_2 := 0 v2_2 := R2_1 ( v1_1 + w1_1 P1_2 ) v2_2 simplify 0
0 L ' cos( )
1 1 2
0
0
w3_3 := R 3_2 w2_2 + '3 1
w3_3 '1 + '2 + '3
0
0
L2 L1 '1 sin( 2 + 3 )
P2_3 := 0
v3_3 := R3_2 ( v2_2 + w2_2 P2_3 ) v3_3 simplify
0
L ' cos( + )
1 1 2 3
0
0
w4_4 := R 4_3 w3_3 + 0 1
w3_3 '1 + '2 + '3
0
0
L3 L1 '1
P3_4 := 0
v4_4 := R4_3 ( v3_3 + w3_3 P3_4 ) v4_4 simplify
0
L ' L ' L
3 1 3 2 3
Ejemplo, cuando el manipulador se encuentra en el eje X:
simplify 0
v4_4 substitute , 1 = 0 , 2 = 0 , 3 = 0 0
( 1 2 3 ) 1 ( 2 3 ) 2 3 3
collect , '1 , '2
L L L ' + L L ' L '
L3 sin( 1 + 2 + ) (
3 L2 sin 1 + 2 )
simplify
v20_4 := v0_4 0
substitute , '1 = 0 , '2 = 1 , '3 = 0
L cos( + + 3 ) L2 cos( 1 + 2 )
3 1 2
L3 sin( 1 + 2 + 3 )
simplify
v30_4 := v0_4 0
substitute , '1 = 0 , '2 = 0 , '3 = 1
L cos( + + )
3 1 2 3
(
JV0_4 := augment v10_4 , v20_4 , v30_4 )
0
w0_4 := R 0_1 R1_2 R2_3 R3_4 w4_4 w0_4 '1 + '2 + '3
0
MODELO DINAMICO
1 1
( ) ( ) ( )
T T
Ki_i vi_i , wi_i , Pi_Ci , ICi_i , mi := mi vi_i + wi_i Pi_Ci vi_i + wi_i Pi_Ci + wi_i ICi_i wi_i
2 2
Ixxi 0 0
1
( )
Pi_Ci Pk_i := Pk_i
2
k := i + 1 ICi_i ( Ixxi , Iyyi, Izzi) := 0 Iyyi 0
0 0 Izzi
Links y posicion del centro de masa.
L1 Ixx1 0 0
P1_C1 := Pi_Ci ( P1_2 )
2
IC1_1 := ICi_i ( Ixx1 , Iyy1 , Izz1) 0 Iyy1 0
0 0
0 Izz1
0
L2 Ixx2 0 0
P2_C2 := Pi_Ci ( P2_3 )
2
IC2_2 := ICi_i ( Ixx2 , Iyy2 , Izz2) 0 Iyy2 0
0
0 0 Izz2
0
L3 Ixx3 0 0
P3_C3 := Pi_Ci ( P3_4 )
2
IC3_3 := ICi_i ( Ixx3 , Iyy3 , Izz3) 0 Iyy3 0
0 0
0 Izz3
0
Energia Cinetica (
Ki_i vi_i , wi_i , Pi_Ci , ICi_i , mi )
L1
0 0
P1_C1
2
v1_1 0 w1_1 '1 0
0 0
0
'1 m1 L1 + 4 Iyy1
2 2
( )
T1 := Ki_i v1_1 , w1_1 , P1_C1 , IC1_1 , m1 simplify
8
L1 '1 sin( 2) L2
0
P2_C2
2
v2_2 simplify 0 w2_2 '1 + '2
L ' cos( ) 0 0
1 1 2
0
( )
2 2 2
m2 L1 '1 m2 cos 2 L1 L2 '1 m2 cos
( )
T2 := Ki_i v2_2 , w2_2 , P2_C2 , IC2_2 , m2 simplify
2
+
2
+
2 2
m3 L1 '1
( )
T3 := Ki_i v3_3 , w3_3 , P3_C3 , IC3_3 , m3 simplify
2
( ) 2
+ m3 cos 2 L1 L2 '1 + m3 cos (
Efectos de la gravedad
L1 cos( 1 )
L1
2
2 0 L1 sin 1 ( )
P1_C1 := 0 P0_C1 := T0_1 P1_C1 z0_C1 := P0_C1
0 L1 sin( 1) 2 2
2
1
1
L2 cos( 1 + 2 )
L2 + L1 cos( 1 )
2
2 0
P2_C2 := 0 P0_C2 := T0_1 T1_2 P2_C2 simplify
0 L2 sin( 1 + 2 ) L sin
2 1 ( 1)
1
1
L3 cos( 1 + 2 + 3 )
L3 + L2 cos( 1
2
2 0
P3_C3 := 0 P0_C3 := T0_1 T1_2 T2_3 P3_C3 simplify
0 L3 sin( 1 + 2 + 3) L sin
2 2 ( 1
1
1
L1 g m1 sin 1( )
P1 := m1 g z0_C1
2
L2 sin( 1 + 2 )
P2 := m2 g z0_C2 g m2 + L1 sin( 1 )
2
L3 sin( 1 + 2 + 3 )
P3 := m3 g z0_C3 g m3 + L2 sin( 1 + 2 ) + L1 sin( 1 )
2
1 '1 ''1
q := q' := q'' :=
2 '2 ''2
L := T1 + T2 + T3 P1 P2 P3
'1 m1 L1 + 4 Iyy1
2 2 2 2 2 2 2 2
Iyy2 '1 Iyy2 '2 Iyy3 '1 Iyy3 '2 Iyy3 '3 L1 m2
L + + + + +
+
2 2 2 2 2 8 2
Para 1
simplify 2
L1 m1
2
L2 m2
d 2 2 2
I + Iyy2 + Iyy3 + + L1 m2 + L1 m3 + + L2 m3 +
collect , '1 , '2 , '3 yy1
L
d'1 4 4
d d
M 1 := L
dt d'1
2
L1 m1
2
L2 m2
2
L3 m3
2 2 2
M 1 := Iyy1 + Iyy2 + Iyy3 + + L1 m2 + L1 m3 + + L2 m3 + + L1 L3 m3
4 4 4
simplify 2
L1 m1
d 2 2
1 := M 1 I + Iyy2 + Iyy3 + + L1 m2 + L1 m
collect , ''1 , ''2 , ''3 , '1 , '2 , '3 yy1
L
d1 4
Para 2
simplify 2
L2 m2
2
L3 m3 L1 L3 m3 cos( 2 + 3 )
d 2
L Iyy2 + Iyy3 + + L2 m3 + + +
d'2 collect , '1 , '2 , '3 4 4 2
d d
M 2 := L
dt d'2
2
L2 m2
2
L3 m3 L1 L3 m3 cos( 2 + 3 ) L1 L2 m2 cos( 2 )
2
M 2 := Iyy2 + Iyy3 + + L2 m3 + + + +L
4 4 2 2
simplify 2
L2 m2
2
L3 m3 L1 L3 m3 cos
d 2
2 := M 2 L Iyy2 + Iyy3 + + L2 m3 + +
d2 collect , ''1 , ''2 , ''3 4 4 2
Para 3
simplify 2
L3 m3 L1 L3 m3 cos( 2 + 3 ) L2 L3 m3 cos( 3 )
d
L Iyy3 + + + '1 +
d'3 collect , '1 , '2 , '3 4 2 2
d d
M 3 := L
dt d'3
2
L3 m3 L1 L3 m3 cos( 2 + 3 ) L2 L3 m3 cos( 3 ) L1 L3 m3 sin( 2 + 3
M 3 := Iyy3 + + + ''1 +
4 2 2 2
simplify 2
L3 m3 L1 L3 m3 cos( 2 + 3 ) L2 L3 m3 cos(
d
3 := M 3 L Iyy3 + + +
d3 collect , ''1 , ''2 , ''3 4 2 2
) ( )
2 + 3 + L2 cos 1 + 2 + L1 cos 1 ( ) 0 0
P4_4 := P3_3 :=
0 0
0
0 0
2 + 3 ) L2 sin( 1 + 2 ) L1 sin( 1 )
1 1
1
L2 cos( 1 + 2 ) + L1 cos( 1 )
0
0_3 := T0_1 T1_2 T2_3 P3_3 simplify L sin + L sin
2 ( 1 2) 1 ( 1)
1
) (
1 + 2 + 3 L2 sin 1 + 2 ) (
L3 sin 1 + 2 + 3 )
0 0
1 + 2 + 3 ) L2 cos( 1 + 2 ) L3 cos( 1 + 2 + 3 )
+ 2 ) (
L3 sin 1 + 2 + 3 )
0
+ 2 ) L3 cos( 1 + 2 + 3 )
( ) ( )
+ L2 '1 sin 3 + L2 '2 sin 3
0
L2 '1 cos( 3 ) L2 '2 cos( 3 )
( ) ( )
sin 2 + 3 + L2 '1 sin 3 + L2 '2 sin 3 ( )
0
3 '3 L1 '1 cos( 2 + 3 ) L2 '1 cos( 3 ) L2 '2 cos( 3 )
) ( )) ( (
1 + 2 + 3 L2 sin 1 + 2 '2 + L3 sin 1 + 2 + 3 '3 ))
1 + 2 + 3 ) L2 cos( 1 + 2 ) ) '2 + ( L3 cos( 1 + 2 + 3 ) ) '3
( )
sin 1
cos( 1 )
(
L2 sin 1 + 2 ) (
L3 sin 1 + 2 + 3 )
0
L2 cos( 1 + 2 ) L3 cos( 1 + 2 + 3 )
( )
2 2 2 2 2 2
cos 2 L1 L2 '1 '2 m2 L2 '1 m2 L2 '1 '2 m2 L2 '2 Iyy2 '1 Iyy2 '2
+ + + + + Iyy2 '1 '2 +
2 8 4 8 2 2
L3
P3_C3
2
'3
0
0
( ) ( ) ( )
2
m3 cos 2 + 3 L1 L3 '1 m3 cos 2 + 3 L1 L3 '1 '2 m3 cos 2 + 3 L1
)
2 L1 L2 '1 '2 +
2
+
2
+
2
(
L2 sin 1 + 2 )
z0_C2 := P0_C2
2 2
( )
L1 sin 1
)
+ 2 + L1 cos 1 ( )
(
L3 sin 1 + 2 + 3 )
z0_C3 := P0_C3
2 2
( )
L2 sin 1 + 2 L1 sin 1 ( )
+ 2 ) L1 sin( 1 )
2 2 2 2 2 2 2 2 2 2 2 2 2 2
'1 L1 m3 '1 L2 m2 '1 L2 m2 '2 L2 m3 '1 L2 m3 '2 L3 m3 '1 L3
+ + + + + + +
2 8 8 2 2 8
2
L3 m3
+ + L1 L3 m3 cos( 2 + 3 ) + L1 L2 m2 cos( 2 ) + 2 L1 L2 m3 cos( 2 ) + L2 L3 m3 cos( 3 ) '1
4
( ) ( ) ( ) ( )
3 cos 2 + 3 + L1 L2 m2 cos 2 + 2 L1 L2 m3 cos 2 + L2 L3 m3 cos 3 ''1 + L1 L3 m3 sin 2 (
2 2
L2 m2 L3 m3
( ) ( ) ( )
2
m3 + + L2 m3 + + L1 L3 m3 cos 2 + 3 + L1 L2 m2 cos 2 + 2 L1 L2 m3 cos 2
4 4
L1 L2 m2 cos 2 ( ) 2
L2 m2
+ L1 L2 m3 cos( 2 ) + L2 L3 m3 cos( 3 ) '1 +
2
Iyy2 + Iyy3 + 4 + L2 m3 +
2
L1 L3 m3 sin( 2 + 3 ) ( '2 + '3 ) L1 L2 m2 sin( 2 )
L1 L2 m3 cos( 2 ) + L2 L3 m3 cos( 3 ) ''1 +
2 2
( ) ( )
2
cos 2 + 3 L1 L2 m2 cos 2 L2 m2
+ + L1 L2 m3 cos( 2 ) + L2 L3 m3 cos( 3 ) ''1 + Iyy2 + Iyy3 + 4
2
m3 L3
2
( )
L2 m3 cos 3 L3 m L 2
3 3
+ + Iyy3 '2 + 4 + Iyy3 '3
4 2
)( ) ( ) m L 2 L m cos( ) L L2 m3 sin(
3 '2 + '3 L2 L3 m3 sin 3 '3 3 3 2 3 3 3
'1 + + + Iyy3 ''2 +
2 4 2 2
( )+ ( ) +L ( )
2 2
m2 cos 1 + 2 L1 L2 m2 '1 sin 2 L2 L3 m3 '3 sin 3
( )
2
2 2 1 L2 m3 '1 sin 2 2
L2 L3 m3
m3 (
L1 L3 m3 cos 2 + 3 ) ( )
L1 L2 m2 cos 2
4
+
2
+
2
( )
+ L1 L2 m3 cos 2 + L2 L3 m3 cos 3( )
( ) ( )
2
L3 m3 L1 L3 m3 cos 2 + 3 L2 L3 m3 cos 3
+ + +
4 2 2
2
2
( )
2 2 2 2 2
1 L3 '1 '3 m3 L2 '1 m3 L2 '2 m3 cos 3 L2 L3 '1
+
2
2
+ m3 L2 '1 '2 +
2
+
2
( )
+ m3 cos 3 L2
L3 sin( 1 + 2 + 3 )
2 2 2 2
m3 '2 L3 m3 '3
+ + g m3 + L2 sin( 1 + 2 ) + L1 sin( 1 ) + Iyy2 '1 '2 +
8 8 2
2
L2 m2
2
L3 m3 L1 L3 m3 cos( 2 + 3 ) L1 L2 m2 cos( 2 )
2
+
1 yy2
I + Iyy3 + + L 2 m3 + + + + L1 L2
4 4 2 2
)( ) ( ) ( ) ( )
2 + 3 '2 + '3 L1 L2 m2 sin 2 '2 2 L1 L2 m3 sin 2 '2 L2 L3 m3 sin 3 '3 '1 + Iyy2
2
L2 m2
2
L3 m3 L1 L3 m3 cos( 2 + 3 ) L1
+ L2 L3 m3 cos( 3 ) ''1 +
2
+
yy2 yy3
I I + + L 2 m3 + + +
4 4 2
2
L3 m3 m L 2 L m cos( ) L
3 3 2 3 3 3
+ L2 L3 m3 cos( 3 ) '2 + 4 + + Iyy3 '3
4 2
2
L2 m2
2
L3 m3
'2
L1 L2 m3 sin( 2 ) '2 L2 L3 m3 sin( 3 ) '3 '1 +
2
Iyy2 + Iyy3 + 4 + L2 m3 + +
4
m L 2 L m cos L
( 3) 3
2
L3 m3 3 3 2 3 L1 L3 m
+ L2 L3 m3 cos( 3 ) ''2 +
2
+ L2 m3 + 4 + + Iyy3 ''3 +
4 2
)
3 L3 '3 m L 2
3 3
'2 + + Iyy3 ''3
4
) ( ) ( ) ( )
2 2
+ 3 L3 g m3 cos 1 + 2 + 3 L2 L3 m3 '1 sin 3 L2 L3 m3 '2 sin 3
+ + + L2 L3 m3 '1
2 2 2
(
2 2
L2 m2 L3 m3 L1 L3 m3 cos
( ) ( )
2
'1 '3 sin 3 L2 L3 m3 '2 '3 sin 3 Iyy2 + Iyy3 + + L2 m3 + +
4 4 2
2
L2 m2
Iyy2 + Iyy3 + + L2
4
2
m3 L3 L2
+
4
( ) ( ) ( )
2 2
m3 cos 3 L2 L3 '1 '3 m3 cos 3 L2 L3 '2 m3 cos 3 L2 L3 '2 '3 m3 L3
L3 '1 '2 + + + +
2 2 2 8
L2 sin( 1 + 2)
2
L2 m2 '1 '2
+ Iyy3 '1 '2 + Iyy3 '1 '3 + Iyy3 '2 '3 + g m2 + L1 sin( 1 ) + + L2
2 4
2
L3 m3 L1 L3 m3 cos( 2 + 3 ) L2 L3 m3 cos( 3 )
2 m3 cos( 2 ) + L2 L3 m3 cos( 3 ) '2 + yy3
I + + +
4 2 2
2
L2 m2
2
L3 m3 (
L1 L3 m3 cos 2 + 3 ) ( )
L1 L2 m2 cos 2
(
2
+ Iyy3 + + L2 m3 + + + + L1 L2 m3 cos
4 4 2 2
( )
1 L2 m2 cos 2
2
L3 m3 L1 L3 m3 cos( 2 +
+ L1 L2 m3 cos( 2 ) + L2 L3 m3 cos( 3 ) ''2 + yy3
I + +
2 4 2
m L 2 L m cos L
3 3 2 3 ( 3) 3
L2 L3 m3 cos( 3 ) ''2 + ( L2 L3 m3 sin( 3 ) '3 ) '2 + 4 + + Iyy3 ''3 +
2
( ) ( ) ( )
2
m3 '1 sin 2 + 3 L3 g m3 cos 1 + 2 + 3 L2 g m2 cos 1 + 2 L1
2
(
L2 g m3 cos 1 + 2 ) 2
2
+
( )
'2 sin 3
)+ ( ) +L
2
2 + 3 L1 L2 m2 cos 2 L3 m3 L1 L3 m3 cos
2
( )
1 L2 m3 cos 2 + L2 L3 m3 cos 3 ( ) Iyy3 +
4
+
2
2 2
L3 m3 m3 L3 L2 m3
( )
2
2 m3 + 4
+ L2 L3 m3 cos 3
4
+
( )
2
2 m3 cos 3 L3 m3 L3
+ Iyy3
2 4
2 2 2 2 2 2 2 2 2
'1 m3 L3 '1 '2 m3 L3 '1 '3 m3 L3 '2 m3 L3 '2 '3 m3 L3 '3 Iyy3 '1
+ + + + + + +
4 4 8 4 8 2
2
2
L3 m3 '1 '2
2
L3 m3 '1 '3
2
L3 m3 '2 '3 ( )
L1 g m1 sin 1 L1 L3 m3 '1
2
2 m3 '1 '2 + 4
+
4
+
4
+
2
+
2
)
'3
L1 L3 m3 sin( 2 + 3 ) ( '2 + '3 ) L1 L2 m2 sin( 2) '2
) ( )
2 + L2 L3 m3 cos 3 ''2 +
2
2
L1 L2 m3
3 ) ( )
L2 L3 m3 cos 3
+ ''3 + ( L1 L3 m3 sin( 2 + 3) L1 L2 m2 sin( 2 ) 2 L1 L2 m3 sin( 2 ) )
2
( )
L2 m3 sin 3 L3 '3
'3
2
( ) ( )
2 2
1 L2 m2 '1 sin 2 L2 L3 m3 '3 sin 3
2
2
( )
+ L1 L2 m3 '1 sin 2
2
( )
L2 L3 m3 '1 '3 sin 3 L2
(
cos 2 + 3 ) +
( )
L2 L3 m3 cos 3
( L1 L3 m3 sin( 2 + 3) L1 L2 m2 sin( 2 ) 2 L1 L2 m3
2
''1
( )
cos 3 L3 '' +
2
+ Iyy3 2
''3
2
+ Iyy3
2 2
Iyy3 '2 Iyy3 '3
+ Iyy3 '1 '2 + Iyy3 '1 '3 + + Iyy3 '2 '3 +
2 2
(
cos 2 + 3 ) 2
( )
L1 L2 m2 '1 cos 2
2
( )
L2 L3 m3 '1 cos 3 L2 L3 m3
( )
2
+ + L1 L2 m3 '1 cos 2 + +
2 2
2
L3 m3 L1 L3 m3 cos( 2 + 3 ) L2 L3 m3 cos(
3 sin( 2 ) '2 L2 L3 m3 sin( 3 ) '3 '2 + Iyy3 + 4 + +
2 2
L1 L3 m3 sin( 2 + 3 ) L1 L2 m2 sin(
( ( ) ( ))
'2 + L1 L3 m3 sin 2 + 3 L2 L3 m3 sin 3 '3 '1 +
2
2
( )
L3 m3 '2 '3 sin 3
L1 L3 m3 sin( 2 + 3 ) L1
( )) ( ( ) ( ))
3 sin 2 '2 + L1 L3 m3 sin 2 + 3 L2 L3 m3 sin 3 '3 '1 +
2
2
(
L1 L3 m3 '1 sin 2 + 3 )
2
(
L2 g m3 cos 1 + 2 )
L
2
'2 cos 3 ( ) (
L1 L3 m3 '1 '2 cos 2 + 3 ) (
L1 L3 m3 '1 '3 cos 2 + 3 ) (
L1 L2 m2 '1 '2 cos
+ + +
2 2 2 2
(2) L1 L3 m3 sin
( ) ( ( ) ( ))
2
L1 L2 m3 sin 2 '2 + L1 L3 m3 sin 2 + 3 L2 L3 m3 sin 3 '3 '2 +
2
( )
L2 m2 sin 2 L1
( ) ( ( ) ( ))
2
L1 L2 m3 sin 2 '2 + L1 L3 m3 sin 2 + 3 L2 L3 m3 sin 3 '3 '2 +
2
(
L3 g m3 cos 1 + 2 + 3 ) (
L2 g m2 cos 1 + 2 ) L1 L2 m2 '1 sin 2
2
( )
) (
2
+ + L1 L2 m3 '1 sin 2
2 2 2
( ) ( ) ( ) (
2 2 2
L1 L3 m3 '1 sin 2 + 3 L3 g m3 cos 1 + 2 + 3 L2 L3 m3 '1 sin 3 L2 L3 m3 '2 sin
+ +
2 2 2 2
(2) ( )
L2 L3 m3 '1 '3 cos 3 L2 L3 m3 '2
( ) ( )
+ L1 L2 m3 '1 '2 cos 2 + L2 L3 m3 '1 '2 cos 3 +
2
+
2
(
sin 2 + 3 ) ( )
L2 L3 m3 sin 3
'3 2 L2 g m2 cos( 1 + 2) + L2 g m3 cos( 1 + 2 ) + L1 g m1 cos
1 1
2 2 2
(
1 L3 m3 sin 2 + 3 ) ( ) ' 2 1 L
L2 L3 m3 sin 3 1
3 2 2 g m2 cos( 1 + 2) + L2 g m3 cos( 1 + 2 ) + 2
2 2
2
( )
L2 L3 m3 '3 sin 3
)
2 2
( ) ( )
L2 L3 m3 '1 '3 sin 3 L2 L3 m3 '2 '3 sin 3
3 )
( )
+ L2 L3 m3 '1 '2 sin 3
( )
2 '3 cos 3
2
)
1
( ) ( ) ( ) (
cos 1 + L1 g m2 cos 1 + L1 g m3 cos 1 + L3 g m3 cos 1 + 2 + 3
2
)
1 1
( ) ( ) ( ) (
L1 g m1 cos 1 + L1 g m2 cos 1 + L1 g m3 cos 1 + L3 g m3 cos 1 + 2 + 3
2 2