Notes_09_02
1 of 17
Lagrangian Dynamics for Simple Pendulum
Y
gravity
a
m, JG
G
T
B
A
q  
q=

X
Q=T
x  a  sin 
y  a  cos 
xG = a cos
yG = a sin




K  12 m x 2  y 2  12 J G  2  12 m a 2  2 sin 2   a 2  2 cos 2   12 J G  2 
1
2
J
P = m g y = m g a sin
L  KP 
1
2
J
G

 ma 2  2  m g a sin 
d  L  L
 
Q
dt  q  q
d  L  L
T
 
dt    
L
 J G  ma 2 

d  L 
2
   J G  ma 
dt   

J
G


 ma 2   m g a cos   T


L
 m g a cos 

G

 ma 2  2
Notes_09_02
2 of 17
Lagrangian Dynamics for Spring-Mass
x
x
k
m
q=x
q  x
K  12 mx 2
Q = FEXT
P  12 kx 2
d  L  L
 
Q
dt  q  q
L
 mx
x
mx  kx  FEXT
L=K-P=
1
2
mx 2  12 kx 2
d  L  L
 FEXT
 
dt  x  x
d  L 
   mx
dt  x 
L
  kx
x
FEXT
Notes_09_02
3 of 17
Lagrangian Dynamics for Cylindrical Coordinate Manipulator
Y
Y
Y
r
r3
T
F
a


X

X
Both links
X
Link 2
Link 3
Main body link 2 - Shaft and end-effector link 3
Mass centers at a and r3 from waist rotation axis, a=constant, r3 = variable
Masses m2 and m3 - centroidal mass moments of inertia J2 and J3
 CCW from positive x axis – a and r3 radial from rotation axis
T is rotary actuator torque of ground on body 2 about waist measured CCW positive
F is radial actuator force of body 2 on body 3 measured positive outward
Gravity g acts along negative y axis
q2 = 
q3 = r
q 2  
q 3  r3
Q3 = F
x 2  a  sin 
y 2  a  cos 
x2 = a cos
y2 = a sin
x 3  r3 cos   r3  sin 
y  r sin   r  cos 
x3 = r3 cos
y3 = r3 sin

Q2 = T
3


3
3

K  12 m 2 x 22  y 22  12 m 3 x 32  y 32  12 J 2  2  12 J 3  2
K  12 m 2 a 2  2  12 m 3 r32  2  12 m 3 r32  12 (J 2  J 3 ) 2
P = m2 y2 g + m3 y3 g
P = m2 g a sin + m3 g r3 sin
L=K-P
L  12 m 2 a 2  2  12 m 3 r32  2  12 m 3 r32  12 (J 2  J 3 ) 2  g(m 2 a  m 3 r3 ) sin 
Notes_09_02
d  L  L


 Qi
dt  q i  q i
d  L  L
T
 
dt    
d  L  L


F
dt  r3  r3
L
 m 2 a 2  m 3 r32  J 2  J 3 


d  L 
2
2
    m 2 a  m 3 r3  J 2  J 3   2m 3 r3 r3 
dt   
L
 gm 2 a  m 3 r3  cos 







T  m 2 a 2  m 3 r32  J 2  J 3   2m 3 r3 r3   gm 2 a  m 3 r3  cos 
L
 m 3 r3
r3
d  L 

  m 3r3
dt  r3 
L
 m 3 r3  2  m 3 g sin 
r3
F  m 3r3  m 3 r3  2  m 3 g sin 
m 2 a 2  m 3 r32  J 2  J 3

0

0    
T  2m 3 r3 r3   gm 2 a  m 3 r3  cos 


   
2

m 3  r3  

F  m 3 r3   m 3 g sin 

4 of 17
Notes_09_02
5 of 17
Lagrangian Dynamics for Two Link Anthropomorphic Manipulator (Double
Pendulum)
Y
C
3
d2
m 3 , J3
a2
d3
a3
T3
m 2 , J2
2
T2
B
A
X
Two solid rigid bars with revolute joints A and B
Lengths d2 and d3 - mass centers at a2 and a3 from proximal ends
Masses m2 and m3 - centroidal mass moments of inertia J2 and J3
2 CCW from positive x axis
T2 is torque of ground on bar 2 about pin A, CCW positive
3 CCW from centerline of bar 2
T3 is torque of bar 2 on bar 3 about pin B, CCW positive
Gravity g acts along negative y axis
q 2   2
q 3   3
q 2  2
q 3  3
Q 2  T2
Q 3  T3
x 2  a 2  2 sin  2
y 2  a 2  2 cos  2
x 2  a 2 cos  2
y 2  a 2 sin  2
x 3  d 2 cos  2  a 3 cos 2   3 

y 3  d 2 sin  2  a 3 sin  2  3 





K  12 m 2 x 22  y 22  12 m 3 x 32  y 32  12 J 2  22  12 J 3  2   3

K  12 m 2 a 22  22  12 m 3 d 22  22  12 m 3 a 32  2   3

 12 J 2  22  12 J 3  2   3
P  m 2 y 2 g  m3 y 3g

2


x 3  d 2  2 sin  2  a 3  2   3 sin  2   3 
y 3  d 2  2 cos  2  a 3  2   3 cos 2   3 

2


2


 m 3 d 2 a 3  2  2   3 cos  3
Notes_09_02
6 of 17
P  m 2 a 2  m 3 d 2 g sin  2  m 3 a 3 g sin  2  3 
d  L  L


 Qi
dt  q i  q i
L  KP
d  L  L


 T2
dt   2   2




L
 m 2 a 22  m 3 d 22  J 2  2  m 3 a 32  J 3  2   3  m 3 d 2 a 3 2 2   3 cos  3

 2


d  L

dt   2


  m 2 a 22  m 3 d 22  J 2  2  m 3 a 32  J 3  2   3


 m 3 d 2 a 3 2 2   3 cos  3  m 3 d 2 a 3 2 2  3   32 sin  3









L
 m 2 a 2  m 3 d 2  cos  2 g  m 3 a 3 cos 2   3 g
 2


T2  m 2 a 22  m 3 d 22  J 2  2  m 3 a 32  J 3   2   3
 m 3 d 2 a 3 2 2   3 cos  3  m 3 d 2 a 3 2 2  3   32 sin  3



 m 2 a 2  m 3 d 2  cos  2 g  m 3 a 3 cos 2   3 g



T2  m 2 a 22  m 3 d 22  J 2  m 3 a 32  J 3  2m 3 d 2 a 3 cos  3  2
 m a 2  J  m d a cos  

3
3
3
2
3
3
2
3
3
 2m 3 d 2 a 3 sin  3  2  3
 m d a sin   2
3
3
 
3
3
 m 2 a 2  m 3 d 2  cos  2 g  m 3 a 3 cos 2   3 g
d  L  L


 T3
dt   3  3
L
 m 3 a 32  J 3

 3

 
2
d  L 

  m 3 a 32  J 3
dt   3 


  3  m 3 d 2 a 3  2 cos  3
 
2

  3  m 3 d 2 a 3 2 cos  3  m 3 d 2 a 3  2  3 sin 3
Notes_09_02


L
 m 3 d 2 a 3  2  2   3 sin  3  m 3 a 3 cos 2   3 g
3

 

 3  m 3 d 2 a 3 2 cos 3  m 3 d 2 a 3  2  3 sin 3
 m 3 d 2 a 3  2  2   3 sin 3  m 3 a 3 cos 2  3 g
T3  m 3 a 32  J 3
2


T3  m 3 a 32  J 3  m 3 d 2 a 3 cos  3   2
 m a 2  J 
3
3
3
3
 m 3 d 2 a 3 sin  3  22
 m 3 a 3 cos 2   3 g
+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
J B  m 3 a 32  J 3
J A  J B  m 2 a 22  m 3 d 22  J 2  2m 3 d 2 a 3 cos  3
C  J B  m 3 d 2 a 3 cos 3
D  m 3 d 2 a 3 sin 3
G 2  m 2 a 2  m 3 d 2 g cos 2
G 3  m 3 a 3 g cos ( 2  3 )
T2  J A  2  C 3  2D 2  3  D 32  G 2  G 3
T3  C 2  J B  3  D 22  G 3
2
T2  J A C 
 2 
 
 D 3  2D 2  3 
 G 2  G 3 
 
   



2

T3  C J B  
 3 
 
 D 2
  G3 
1
2

 2 
 J A C  T2  
 D 3  2D 2  3 
 G 2  G 3  


  






 T3   D 2
C
J




B


2
 3

  G 3 

7 of 17
Notes_09_02
8 of 17
Validated with harmonic drivers using Haug's inverse dynamics and then calculate torques per
above
-15
8
Planar two link manipulator
x 10
T2
T3
Validate torque equations [N.m]
6
4
2
0
-2
-4
-6
-8
0
0.2
0.4
0.6
0.8
1
1.2
Time [sec]
1.4
1.6
1.8
2
Notes_09_02
9 of 17
Lagrangian Dynamics for Three Link Anthropomorphic Manipulator
D
4
m 4 , J4
d4
a4
Y
T4
3
C
d2
m 3 , J3
T3
a2
m 2 , J2
T2
B
2
d3
a3
A
X
Three solid rigid bars with revolute joints A, B and C
Lengths d2 d3 d4 - mass centers at a2 a3 a4 from proximal ends
Masses m2 m3 m4 - centroidal mass moments of inertia J2 J3 J4
2 CCW from positive x axis
T2 is torque of ground on bar 2 about pin A, CCW positive
3 CCW from centerline of bar 2
T3 is torque of bar 2 on bar 3 about pin B, CCW positive
4 CCW from centerline of bar 3
T4 is torque of bar 3 on bar 4 about pin C, CCW positive
Gravity g acts along negative y axis
q 2  2
q 3  3
q 4  4
q 2   2
q 3   3
q  
x 2  a 2 cos  2
y 2  a 2 sin  2
4
4
Q 2  T2
Q 3  T3
Q 4  T4
x 2  a 2  2 sin  2
y 2  a 2  2 cos  2
x 3  d 2 cos  2  a 3 cos 2   3 
y 3  d 2 sin  2  a 3 sin  2  3 


x 3  d 2  2 sin  2  a 3  2   3 sin  2   3 
y 3  d 2  2 cos  2  a 3  2   3 cos 2   3 


Notes_09_02
10 of 17
x 4  d 2 cos  2  d 3 cos 2  3   a 4 cos 2  3   4 
x 4  d 2  2 sin  2  d 3  2   3 sin  2  3   a 4  2   3   4 sin  2   3   4 




y 4  d 2 sin  2  d 3 sin  2  3   a 4 sin  2  3   4 
y 4  d 2  2 cos  2  d 3  2   3 cos 2  3   a 4  2   3   4 cos 2  3   4 










  
K  12 m 2 x 22  y 22  12 m 3 x 32  y 32  12 m 4 x 24  y 24

 12 J 2  22  12 J 3  2   3

2

 12 J 4  2   3

K  12 m 2 a 22  22  12 m 3 d 22  22  12 m 3 a 32  2   3




2
4


 m 3 d 2 a 3  2  2   3 cos  3
2




 12 m 4 d 22  22  12 m 4 d 32  2   3  12 m 4 a 24  2   3   4
 m 4 d 2 d 3  2  2   3 cos  3  m 4 d 2 a 4  2  2   3   4 cos 3   4 
 m d a         cos 
4
3
4


2

3

2
 12 J 2  22  12 J 3  2   3
2

2
3
4


2
4
 12 J 4  2   3   4

2
P  m 2 y 2 g  m3 y 3g  m 4 y 4 g
P  m 2 a 2  m 3 d 2  m 4 d 2 g sin  2  m 3 a 3  m 4 d 3 g sin  2  3   m 4 a 4 g sin  2  3   4 
d  L  L


 Qi
dt  q i  q i
L  KP
d  L  L


 T2
dt   2   2

 

L
 m 2 a 22  m 3 d 22  m 4 d 22  J 2  2  m 3 a 32  m 4 d 32  J 3  2   3  m 4 a 24  J 4  2   3   4

 2
 m 3 d 2 a 3  m 4 d 2 d 3  2 2   3 cos  3  m 4 d 2 a 4 2 2   3   4 cos 3   4 
 m d a 2  2   cos 


4
d  L

dt   2
3
4


2
3
4






4

  m 2 a 22  m 3 d 22  m 4 d 22  J 2  2  m 3 a 32  m 4 d 32  J 3  2   3  m 4 a 24  J 4  2   3   4


 m 3 d 2 a 3  m 4 d 2 d 3  2 2   3 cos  3  m 3 d 2 a 3  m 4 d 2 d 3  3 2 2   3 sin  3
 m d a 2     cos     m d a    2     sin    

4

2
4
 m 4 d 3a 4

2

2
2
3
4





3
4
4
2

4

3
4

 


2

3
 2 3   4 cos  4  m 4 d 3 a 4  4 2 2  2 3   4 sin  4
4


3
4

Notes_09_02
11 of 17
L
 m 2 a 2  m 3 d 2  m 4 d 2 g cos  2  m 3 a 3  m 4 d 3 g cos 2   3   m 4 a 4 g cos 2   3   4 
 2




3

 


T2  m 2 a 22  m 3 d 22  m 4 d 22  J 2  2  m 3 a 32  m 4 d 32  J 3  2   3  m 4 a 24  J 4  2   3   4
 m 3 d 2 a 3  m 4 d 2 d 3  2 2   3 cos  3  m 3 d 2 a 3  m 4 d 2 d 3  3 2 2   3 sin  3
 m d a 2     cos     m d a    2     sin    
4
2
4
 m 4 d 3a 4

2

2
2
3
4


4
4
2

4

3
4

2


3
4

3

4
 2 3   4 cos  4  m 4 d 3 a 4  4 2 2  2 3   4 sin  4
 m 2 a 2  m 3 d 2  m 4 d 2 g cos  2  m 3 a 3  m 4 d 3 g cos 2   3   m 4 a 4 g cos 2   3   4 
 m a 2  m 3 d 22  m 3 a 32  m 4 d 22  m 4 d 32  m 4 a 24  J 2  J 3  J 4

 2
T2   2 2

  2m 3 d 2 a 3  m 4 d 2 d 3  cos  3  2m 4 d 2 a 4 cos 3   4   2m 4 d 3 a 4 cos  4 
 m 3 a 32  m 4 d 32  m 4 a 24  J 4  J 3
 
 3
 






m
d
a

m
d
d
cos


m
d
a
cos




2
m
d
a
cos

3 2 3
4 2 3
3
4 2 4
3
4
4 3 4
4 

 m 4 a 24  J 4  m 4 d 2 a 4 cos 3   4   m 4 d 3 a 4 cos  4  4
 m d a  m d d  sin   m d a sin     2


3
2
3
4
2
3
3
4
2
4
3
 m 4 d 2 a 4 sin  3   4   m 4 d 3 a 4 sin  4  24
4
3
 2m 3 d 2 a 3  m 4 d 2 d 3  sin  3  m 4 d 2 a 4 sin  3   4  2  3
 2m d a sin      m d a sin   
4
2
4
3
4
4
3
4
4
2
4
 2m 4 d 2 a 4 sin  3   4   m 4 d 3 a 4 sin  4  3  4
 m 2 a 2  m 3 d 2  m 4 d 2 g cos  2  m 3 a 3  m 4 d 3 g cos 2   3   m 4 a 4 g cos 2   3   4 
T2  J A  2  A  3  B 4  2D  E  2  3  2E  F 2  4  2E  F 3  4
 D  E  2  E  F 2  G  G  G
3
4
2
3
4
d  L  L


 T3
dt   3  3




L
 m 3 a 32  m 4 d 32  J 3   2   3  m 4 a 24  J 4   2   3   4

 3
 m 3 d 2 a 3  m 4 d 2 d 3  2 cos  3  m 4 d 2 a 4  2 cos 3   4   m 4 d 3 a 4 2 2  2 3   4 cos  4


Notes_09_02
12 of 17
d  L 

  m 3 a 32  m 4 d 32  J 3  2   3  m 4 a 24  J 4  2   3   4


dt   3 
 m 3 d 2 a 3  m 4 d 2 d 3  2 cos  3  m 3 d 2 a 3  m 4 d 2 d 3  2  3 sin  3
 m d a  cos     m d a     sin    


4
2
4

2
3
4
 

4
2
4

2

3

4


3
4

 m 4 d 3 a 4 2 2  2 3   4 cos  4  m 4 d 3 a 4 2 2  2 3   4  4 sin  4






L
 m 3 d 2 a 3  2  2   3 sin  3  m 4 d 2 d 3  2  2   3 sin  3  m 4 d 2 a 4  2  2   3   4 sin  3   4 
3
 m 3 a 3  m 4 d 3 g cos 2   3   m 4 a 4 g cos 2   3   4 


 


T3  m 3 a 32  m 4 d 32  J 3  2   3  m 4 a 24  J 4  2   3   4
 m 3 d 2 a 3  m 4 d 2 d 3  2 cos  3  m 3 d 2 a 3  m 4 d 2 d 3  2  3 sin  3
 m d a  cos     m d a     sin    
4
2
4

2
3
4

4
2
4
2

3

4

3
4

 m 4 d 3 a 4 2 2  2 3   4 cos  4  m 4 d 3 a 4 2 2  2 3   4  4 sin  4
 m 3 d 2 a 3  2  2   3 sin  3  m 4 d 2 d 3  2  2   3 sin  3  m 4 d 2 a 4  2  2   3   4 sin  3   4 




 m 3 a 3  m 4 d 3 g cos 2   3   m 4 a 4 g cos 2   3   4 


 m 3 a 32  m 4 d 32  m 4 a 24  J 3  J 4

 2
T3  






m
d
a

m
d
d
cos


m
d
a
cos




2
m
d
a
cos

3 2 3
4 2 3
3
4 2 4
3
4
4 3 4
4

 m 3 a 32  m 4 d 32  m 4 a 24  J 3  J 4 
 3
 


2
m
d
a
cos

4 3 4
4


2
 m 4 a 4  J 4  m 4 d 3 a 4 cos  4  4
 2m d a sin   


4
3
4
4
2
4
 2m 4 d 3 a 4 sin  4  3  4
 m 3 d 2 a 3  m 4 d 2 d 3 sin  3  22  m 4 d 2 a 4 sin  3   4  22
 m d a sin   2
4
3
4
4
4
 m 3 a 3  m 4 d 3 g cos 2   3   m 4 a 4 g cos 2   3   4 
T3  A 2  J B  3  C 4  2F 2  4  2F 3  4  D  E  22  F 24  G 3  G 4
d  L  L


 T4
dt   4   4




L
 m 4 a 24  J 4  2   3   4  m 4 d 2 a 4  2 cos 3   4   m 4 d 3 a 4  2   3 cos  4

 4

Notes_09_02
d  L

dt   4

  m 4 a 24  J 4   2   3   4


 m 4 d 2 a 4  2 cos 3   4   m 4 d 2 a 4  2
 m d a    cos   m d a 

4
3
4

2
3
13 of 17


4

4
3
4
4




  4 sin  3   4 

2   3 sin  4
3




L
 m 4 d 2 a 4  2  2   3   4 sin  3   4   m 4 d 3 a 4  2   3  2   3   4 sin  4
 4
 m 4 a 4 g cos 2   3   4 



T4  m 4 a 24  J 4  2   3   4
 m 4 d 2 a 4  2 cos 3   4   m 4 d 2 a 4  2
 m d a    cos   m d a 




  4 sin  3   4 

4 3 4
2
3
4
4 3 4 4
2   3 sin  4
 m 4 d 2 a 4  2  2   3   4 sin  3   4   m 4 d 3 a 4  2   3  2   3   4 sin  4




 m 4 a 4 g cos 2   3   4 

3




T4  m 4 a 24  J 4  m 4 d 2 a 4 cos 3   4   m 4 d 3 a 4 cos  4  2
 m a 2  J  m d a cos  

 m a
4
4
4
4
2
4
4
 J 4  4

3
4
4

3
 2m 4 d 3 a 4 sin  4  2  3
 m 4 d 3 a 4 sin  4  m 4 d 2 a 4 sin  3   4  22
 m d a sin   2
4
3
4
4
3
 m 4 a 4 g cos 2   3   4 
T4  B 2  C 3  J C  4  2F 2  3  E  F 22  F 32  G 4
+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
J C  m 4 a 24  J 4
J B  J C  m 3 a 32  m 4 d 32  J 3  2m 4 d 3 a 4 cos  4
J A  J B  m 2 a 22  m 3 d 22  m 4 d 22  J 2  2m 3 d 2 a 3  m 4 d 2 d 3  cos  3  2m 4 d 2 a 4 cos 3   4 
A  J B  m 3 d 2 a 3  m 4 d 2 d 3 cos 3  m 4 d 2 a 4 cos3   4 
B  J C  m 4 d 3 a 4 cos  4  m 4 d 2 a 4 cos3   4 
C  J C  m 4 d 3 a 4 cos  4
D  m 3 d 2 a 3  m 4 d 2 d 3 sin  3
E  m 4 d 2 a 4 sin 3   4 
Notes_09_02
14 of 17
F  m 4 d 3 a 4 sin  4
G 4  m 4 a 4 g cos 2  3   4 
G 3  m 3 a 3  m 4 d 3 g cos 2  3 
G 2  m 2 a 2  m 3 d 2  m 4 d 2 g cos  2
T2  J A  2  A  3  B 4  2D  E  2  3  2E  F 2  4  2E  F 3  4
 D  E  2  E  F 2  G  G  G
3
4
2
3
4
T3  A 2  J B  3  C 4  2F 2  4  2F 3  4  D  E  22  F 24  G 3  G 4
T4  B 2  C 3  J C  4  2F 2  3  E  F 22  F 32  G 4
B   2   2D  E   2E  F  2E  F  2  3 
 


J B C   3   
0
 2F
 2F   2  4 
  3  4 
C J C   4  
2F
0
0
 D  E   E  F  22  G 2  G 3  G 4 
 0
  

 D  E 
0
 F   32    G 3  G 4 

 E  F
  24  
F
0
G4

T2  J A
  
T3    A
T   B
 4 
 2  J A
  
 3    A
   B
 4 
A
A
JB
C
T2   2D  E   2E  F  2E  F  2  3 

  

  

T 
0
 2F
 2F   2  4 

1  3 
B    
  


T
2
F
0
0
 3 4 
 4  


C 

 22  G 2  G 3  G 4 




0

D

E

E

F



J C 
  


 D  E 
0
 F   32    G 3  G 4 



 E  F
  24  
F
0
G4


Notes_09_02
15 of 17
Validated with harmonic drivers using Haug's inverse dynamics and then calculate torques per
above
-14
1.5
Planar three link manipulator
x 10
T2
T3
T4
Validate torque equations [N.m]
1
0.5
0
-0.5
-1
-1.5
0
0.2
0.4
0.6
0.8
1
1.2
Time [sec]
1.4
1.6
1.8
2
Notes_09_02
16 of 17
Linear State Space Model for Two Link Manipulator
 y 1   2 
 y    
   
{y }   2    3 

 y 3   2 
 y 4   3 
 y1   2 
 y   
   
{y}   2     3 
 y 3   2 
 y 4   3 
{y }  f ({y})
{y }  [A LINEAR ]{y}
linearize about nominal values of y
1
2

 2 
 J A C  T2  
 D 3  2D 2  3 
 G 2  G 3  

   






2
 3 
 C J B   T3  


 D 2
  G 3 
J A  J B  m 2 a 22  m 3 d 22  J 2  2m 3 d 2 a 3 cos  3
J B  m 3 a 32  J 3
C  J B  m 3 d 2 a 3 cos 3
D  m 3 d 2 a 3 sin 3
G 2  m 2 a 2  m 3 d 2 g cos 2
G 3  m 3 a 3 g cos ( 2  3 )

J B  C  G 
1
 2 

  
 
2 

 3 
 J A J B  C   C J A  H 
2
G  T2  D 3  2D 2  3  G 2  G 3 
 

2

H  T3  D 2  G 3
T2  m 3 d 2 a 3 sin  3  32  2m 3 d 2 a 3 sin  3  2  3  m 2 a 2  m 3 d 2 g cos 2  m 3 a 3 g cos ( 2   3 )


T3  m 3 d 2 a 3 sin  3  22  m 3 a 3 g cos ( 2   3 )

 2   2CC /  3   J B J A /  3  J B  C  G  

    
 C J  H  3
2 2
 3  
JAJB  C
A  



1
 
2
 JAJB  C


0

 C / 
3

 C /  3  G  
  3
J A /  3  H  
J B  C  G /  2
1

2 
J A J B  C  C J A  H /  2
G /  3
H /  3
G /  2
H / 
2
2 


G /  3  3 


0   2 
 3 
Notes_09_02
17 of 17
G /  2  m 2 a 2  m 3 d 2 g sin  2  m 3 a 3 g sin(  2   3 )
G /  3  m 3 d 2 a 3 cos  3  32  2m 3 d 2 a 3 cos  3  2  3  m 3 a 3 g sin(  2   3 )
G /   2D
2

3
G /  3  2D  2   3

H /  2  m 3 a 3 g sin(  2   3 )
H /  3  m 3 d 2 a 3 cos  3  22  m 3 a 3 g sin(  2   3 )
H /   2D
2
2
H /  3  0
J A /  3  2D
C /  3  D
 2 
D
   
 3  J A J B  C 2



2
  2J B J B  C 

2
 J A J B  2J B C  C
 G 
  3
 2 2 J A J B  J A C  C  H 
J A J B  2J B C  C 2

J B  C  G /  2
1

2 
J A J B  C  C J A  H /  2
P23 
D
 
2
P33  J A J B  C 2 
 2  
   
 3  
    
1
 2  
 3   J A J B  C 2

 
  2J B J B  C

2
 J A J B  2J B C  C
2
G /  3
H /  3

2 



G /  3  3 


0   2 
 3 
G /  2
H /  2
 G 
 
 22J A J B  J A C  C  H
J B  C  G /  2
 C J  
A  H /  2



A LINEAR   
J B  C  G /  2
1


2 
 J A J B  C  C J A  H /  2
J A J B  2J B C  C 2
2
0 0 1 0 
0 0 0 1 


G /  3 G /  2
H /  H / 
3
G /  3  0 P23

0  0 P33
2
0 0 1 0 
0 0 0 1 


G / 3 G /  2
H /  H / 
3
2
G /  3  0 P23

0  0 P33
 2 


 3 
0 0   2 

0 0   3 




0 0  

0 0 
