Notes_11_08
1 of 7
Three-Dimensional Dynamics
Must use centroidal coordinate frames
 
 r    r 2 KINEMATIC
  r 2 DRIVER
 
'


   ' 2 KINEMATIC

   ' 2 DRIVER


 r3 KINEMATIC
 r3 DRIVER


 
 
for


 ' 3 KINEMATIC
 ' 3 DRIVER
r2 
r  r3 
  
 
  2 ' 
'  3 ' 
  


for
     r'  


     r'  


*   2 * G 
*   * G 
r
pi
'
' i
m i
M i    0
 0
' i
i
0
mi
0
'
r
1
2
T
pi
0
0 
m i 
M 2  0 3x 3 0 3x 3 
M   0 3x 3 M 3  0 3x 3 
 0 3x 3 0 3x 3  
J G 2 '
J'   0 3x 3
 0 3x 3
0 3x 3 
J G 3 ' 0 3x 3 
0 3x 3
 
0 3x 3
~ ' 0
0 3x 3 

2
3x 3
~ '   0 3x 3 ~ 3 ' 0 3x 3 
 0 3x 3 0 3x 3  
F 
on i ALL
F 
on i APPLIED
F 
on i CONSTRAINT
i
 2 ' 
 

 '   3 ' 
  


Notes_11_08


2 of 7


 Fon 2 APPLIED 
FAPPLIED   Fon 3 APPLIED 





 Ton 2 ' APPLIED
T' APPLIED   Ton 3 ' APPLIED





Equations of motion (EOM)
Mr   r T   FAPPLIED
J' '    ' T   T 'APPLIED  ~ ' J''
     r'  


r
 q  
or
'
r
M
3 nb x 1
3 nb x 3 nb
 '
J'
T' APPLIED
3 nb x 1
3 nb x 3 nb
3 nb x 1
FAPPLIED
3 nb x 1
q
 

r
nc x 3 nb
~ ' J' '
nc x 1
3 nb x 1
 
'
nc x 3 nb

nc x 1
nb = number of moving bodies
nk = number of kinematic constraints
nd = number of driver constraints
nc = total number of constraints (nc = nk + nd)
Inverse dynamics – kinematically driven
solve kinematics
 r 

   r    '
 ' 


      must have full rank
r
'
compute constraint forces
   
1
nc = 6nb
    r 
    T' F ~ ' J'M'rJ' '


T 1
APPLIED
'
APPLIED


   KINEMATIC 
 DRIVER 


   KINEMATIC 
 DRIVER 
Notes_11_08
Statics
r  0
    r 
' 0
and
 ' 0

and
    TF'



APPLIED
Inverse dynamics – simultaneous EOM matrix
 M 
 3nb x 3nb


0 3nb x 3nb


  
 nc x 3rnb
T 1
APPLIED
'
 r T   r 
FAPPLIED


 3nb x 1 
3 nb x 1



 
T 


~
 '  T' APPLIED' J ' '
  '  

 

3 nb x 1
3 nb x nc
 3nb x 1 


 












0 nc x nc
  nc x 1  
nc x 1

0 3nb x 3nb
3 nb x nc
 
J'
3 nb x 3 nb
 
'
nc x 3 nb
 M 
EOM   0 3nb x 3nb
  
r

 r T 
0 3nb x 3nb
  
J '
T
'
 
'
6 nb nc  x 6 nb nc 
 r   M 

 
 '   0 3nb x 3nb

     
r

 
0 3nb x 3nb
J'
 
'
0 nc x nc 
 r T 

' 
0 nc x nc 
 
T
1
FAPPLIED




~
T' APPLIED' J ' ' 





3 of 7
Notes_11_08
4 of 7
Lagrange multipliers for specific constraints
 
F 
  r i
on i CONSTRAINT
T '
on i
CONSTRAINT
F 
on j CONSTRAINT
T '
on j
CONSTRAINT

CONSTRAINT
 
T
 ~
si ' P A i   r i
 
  r j
CONSTRAINT
T
T
CONSTRAINT
CONSTRAINT
T

 


 
T
CONSTRAINT
T
r j CONSTRAINT
  ' j
T
CONSTRAINT
local joint definition frame at P
s i ' A  s i ' P C i P s i ' ' A
F ' '  C   A  F 
P T
on i
T
i
i
on i
T ' '  C   T '
P T
on i
i
on i
Spherical
SPH  rj P  ri P  03x1
F 
on i SPH
T '
on i
SPH
F 
on j SPH
T '
on j
SPH
 SPH
 0 3x1 
 SPH
 0 3x1 
Double spherical
SPH _ SPH  dij dij C2  0
T
F 
on i CONSTRAINT
 2d ij   SPH _ SPH
CONSTRAINT
CONSTRAINT
     
 ~
sj ' P A j

  'i
T
for
d   r   r 
P
ij
j
P
i
CONSTRAINT
Notes_11_08
T '
on i
CONSTRAINT
F 
on j CONSTRAINT
T '
on j
5 of 7
 0 3 x1 
 2d ij   SPH _ SPH
CONSTRAINT
 0 3 x1 
Dot-1
 DOT _ 1  a i  a j   0
T
F 
on i DOT _ 1
T '
on i
DOT _ 1
on j DOT _ 1
on j
and
a   r   r 
and
a i   ri Q  ri P
Q
j
j
P
j
 01x 3 
F 
T '
a i   ri Q  ri P
for
 
T
 ~
a i ' A i  A j a j '  DOT _ 1
 01x 3 
DOT _ 1
    A a ' 
~
aj ' Aj
T
i
i
DOT _ 1
Dot-2
 DOT _ 2  a i  dij  0
T
F 
on i DOT _ 2
T '
on i
on j DOT _ 2
T '
on j
d   r   r 
P
ij
 a i   DOT _ 2
DOT _ 2
F 
for
T
 ~
a i ' A i  d ij  DOT _ 2
 a i   DOT _ 2
DOT _ 2
0
j
P
i
Notes_11_08
6 of 7
Forward dynamics - dynamically driven
nc < 6nb
      does not have full row rank
'
r
given
r
compute
p
 r 
r
'
 

0 3nb x 3nb
 r T 
'
 r   M 

 
 '   0 3nb x 3nb

     
r

 
J'
 
'
FAPPLIED

' 
0 nc x nc 
 
T
1
 r 
 p 
y    
 r 

 '


M
J'
FAPPLIED




~
T' APPLIED' J ' ' 





must use forward time integration of r
 r 
 p
y   
 r 

'

T'APPLIED
 ' to get r p r ' at next time step
p   G  '
1
2
T
G 2  0 3x 4 0 3x 4 
G   0 3x 4 G 3  0 3x 4 
 0 3x 4 0 3x 4  
Notes_11_08
7 of 7
EOM using Euler parameters
 M 

0 4 nb x 3nb
  
r

 0 nb x 3nb
0 3nb x 4 nb
4G  J ' G 
T
 
 
p E
FAPPLIED
0 3nb x nb   r  

T
T
T 


 J ' G
 p
 p E   p  2G  T' APPLIED8 G




0 nc x nb     



E
0 nb x nb  E  
 
    
T
p
p
0 nc x nc
p E
0 nb x nc
G 2  0 3 x 4 0 3 x 4 
G    0 3x 4 G 3  0 3x 4 
 0 3 x 4 0 3 x 4  
p 2 
p  p 3 
  


 
 r T
p 2 T

 2  01x 4
0
 1x 4
01x 4
p 2 T
01x 4
01x 4 

01x 4 
 
E
p 2 T p 2 


T
 2p 3  p 3 





Euler parameter
EUL  pi T pi  1  0
 
 
 
 01x 3 
F 
 03x1 
r i EUL
 p i  G i   G i p i   01x 3 
T
 ' i EUL
 2p i 
T
p i EUL
on i EUL
T '
on i
EUL
F 
on j EUL
T '
on j
EUL
 0 3 x1 
 03x1 
 0 3 x1 
T
T