Notes_08_11
Two-Dimensional Inverse Dynamics
Kinematically driven
Must use centroidal coordinate frames !
r 
qi    i 
 i 
r 
qi    i 
 i 
F 
Q    T 
on i
on i

on i
 mi
Mi    0
 0

0
mi
0
0
0 
J Gi 
Single body
Mi qi    Qon i ALL 
Q 
on i ALL
Q 
on i APPLIED
Q 
on i CONSTRAINT
1 of 9
Notes_08_11
System of multiple bodies
q 2 
q  q3
q 
 4
q2 
q  q3
q 
 4
 M 2 
M  03x 3 
0 3x 3 
 M 2 
0 
 3x 3
0 3x 3 
03x 3  03x 3 
M 3  03x 3 
03x 3  M 4 
0 3x 3  0 3x 3  q 2   Q on 2 ALL 
M 3  0 3x 3  q 3    Q on 3 ALL 
0 3x 3  M 4  q 4   Q on 4 ALL 



 Qon 2 
ALL
QALL   Qon 3 ALL
 Q 
 on 4 ALL



Mq  QALL
QALL  QAPPLIED  QCONSTRAINT


CONSTRAINT   KINEMATIC 
 DRIVER 
QCONSTRAINT
QKINEMATIC
QDRIVER
2 of 9
Notes_08_11
3 of 9
Virtual work
Mq QALL  0
q T Mq QALL   0
QALL  QAPPLIED  QCONSTRAINT
q T QCONSTRAINT  0
 q   0
for kinematic consistency
q T Mq QAPPLIED  0
q
 q   0
subject to
q
Virtual work for one revolute
q T QCONSTRAINT  ?
r 
q     
r 
q i    i 
j
 i 
F 
0
j

j

 Fon j REV
P
on i REV
P


Fon i PREV



P
P T
Fon i REV 

 Bi s i '
Q 
P

on i REV

q i T Q on i PREV  q j T Q on j PREV
ri 
 
 i 
T
Q 
P
on j REV


Fon j PREV



P
P T
Fon j REV 

 B j s j '
 

? 0
T



Fon i PREV
Fon j PREV

 rj  




   
? 0
P
P
P T
P T











B
s
'
F
B
s
'
F
j



   j j
on i REV 
on j REV 
 i i


 


ri T Fon i PREV   i Bi s i ' P T Fon i PREV  rj T Fon j PREV  j B j s j ' P T Fon j PREV
r   r   B s '
ri P  ri    i Bi s i ' P
r   F 
P T
i
P
on i REV

P
 rj 
P
j
 F 
T
P
on j REV
j
? 0
j
j
OK
j
P
ri P  rj P
? 0
Notes_08_11
4 of 9
Lagrange multiplier theorem
bT x  0
general problem
bT x  T Ax  0
using Lagrange multipliers  for any arbitrary x
q T Mq QAPPLIED  0
virtual work
b  Mq  QAPPLIED
x  q 
Mq  QAPPLIEDT q   T  q q   0
Mq Q
APPLIED
Ax  0
subject to
subject to
 q   0
q
A  q 
for arbitrary size but kinematically consistent q 
   q   0
 q
T
T
Mq   q T   QAPPLIED
 
  
each row in  is multiplied times corresponding column in q
T
q
 
each row in  corresponds to matching row in  q and 
Lagrange multipliers
Mq  q T   QAPPLIED
Mq  QAPPLIED  q T 
Mq  QALL  QAPPLIED
 QCONSTRAINTS
QCONSTRAINTS  q T 



 Qon 2 
ALL
QALL   Qon 3 ALL
 Q 
 on 4 ALL



QCONSTRAINTS



 Qon 2 
CONSTRAINTS

  Qon 3 CONSTRAINTS
 Q 
 on 4 CONSTRAINTS



T
Notes_08_11
5 of 9
Equations of motion (EOM)
Mq  q T   QAPPLIED
 q  
q
q
q
M
nq x 1
nq x 1
nq x nq
QAPPLIED



nq x 1
nc x 1
nc x 1
nc x 1
 
q
nc x nq
nq = number of generalized coordinates
nk = number of kinematic constraints
nd = number of driver constraints
nc = total number of constraints (nc = nk + nd)
Inverse dynamics – kinematically driven
  must have full rank
q  q 1
solve kinematics
compute constraint forces


   KINEMATIC 
 DRIVER 
q
nc = nq
  q T  QAPPLIED  Mq
1


   KINEMATIC 
 DRIVER 
Inverse dynamics – simultaneous EOM matrix
Mq  q T   QAPPLIED
 M 
nq x nq


 q
 nc x nq
 
    q  Q

 
T
q
nq x nc
 
T
0 

1








nc x 1
   Q
q
q
APPLIED
nq x 1
nq x 1
 





0     
nc x nc  nc x 1 


q  M
 
   q
 q  
and



APPLIED

 M 
EOM   
  q
  
T
q
  0
 nc nq  x  nc nq 


Notes_08_11
6 of 9
Statics
q  0
Mq  q T   QAPPLIED
  q T  QAPPLIED
1
Lagrange multipliers for specific constraints
 
F 
  r i
T 

F 
  r j
T 

on i CONSTRAINT
on i CONSTRAINT
on j CONSTRAINT
on j CONSTRAINT
T
CONSTRAINT
CONSTRAINT
B s '   
P T
i
T
r i CONSTRAINT
i
 
T
CONSTRAINT
P T
T
r j CONSTRAINT
j

 

T
CONSTRAINT
CONSTRAINT
CONSTRAINT
B s '   
j
 
  i
  j
T
CONSTRAINT
CONSTRAINT
Revolute
REV  rj P  ri P  0 2x1 
F 
 REV
T 
0
F 
 REV
T 
0
on i REV
on i REV
on j REV
on j REV
check body i
 
 I 2 
check body j
 
 I 2 
P
r i REV
P
r j REV
 
P
 i REV
 
P
 j REV
 B i s i ' P
 
 B j s j ' P
OK
OK
Notes_08_11
7 of 9
Double revolute
 REV _ REV  dij dij C2  0
T
F 
on i REV _ REV
on i CONSTRAINT
on j REV _ REV
d  r   r 
for
a i   ri Q  ri P
P
ij
P
j
i
 2dij REV _ REV
T 
F 
for
0
 2d ij REV _ REV
T 
on j CONSTRAINT
0
Parallel vectors
a i  parallel to a j 
 PARALLEL  a i  R  a j   0
T
T
F 
 02x1
T 
 norma i norma j PARALLEL
F 
 02x1
T 
 norma i norma j PARALLEL
on i PARALLEL
on i PARALLEL
on j PARALLEL
on j PARALLEL
and
a  r   r 
and
a i   ri Q  ri P
Q
j
j
P
j
Pin-in-slot
a i  parallel to d ij
 PIN _ SLOT  a i  R  dij  0
T
F 
on i PIN _ SLOT
T 
on j PIN _ SLOT
for
d  r   r 
P
ij
 R a i  PIN _ SLOT
on i CONSTRAINT
F 
T
 norma i normdij PIN _ SLOT
 R a i  PIN _ SLOT
j
P
i
Notes_08_11
T 
on j PIN _ SLOT
8 of 9
0
Relative angle (including driver)
 ANGLE   j  i  C  f ( t )  0
F 
 0 2x1 
T 
  ANGLE
F 
 0 2x1 
T 
  ANGLE
on i ANGLE
on i ANGLE
on j ANGLE
on j ANGLE
Gear pair
 GEAR   j  i  C  0
F 
on i GEAR
on i CONSTRAINT
on j GEAR
internal/external pair   0
 02 x1
T 
F 
external pair   0
  GEAR
 02 x1
T 
on j CONSTRAINT
  GEAR
Gear pair on rotating link k
 GEAR _ ON _ K   j   k   i   k   C  0
F 
on i GEAR
T 
 02 x1
on i CONSTRAINT
F 
on j GEAR
  GEAR
 02 x1
external pair   0
internal/external pair   0
Notes_08_11
T 
on j CONSTRAINT
F 
on k GEAR
T 
  GEAR
 02x1
on j CONSTRAINT
 1  GEAR
9 of 9