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  q 3 
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 
j

j

 Fon j REV
P
on i REV
Q 
0
P
 
P
on i REV
P

Fon i REV


P T

Bi s i  '  Fon i
T
 
P

Fon i REV


P T

Bi s i  '  Fon i
Q 
P
 
q i T Q on i PREV  q j T Q on j PREV
ri 
 
 i 



P
REV 

on j REV
 
    F 

Fon j


P

 Bj s j '
P
REV
T
on j



P
REV 

? 0
 
    F 

T

Fon j
  rj  

   
P
P

REV 
  j 
 Bj s j '
 
P
REV
T
on j


? 0
P
REV 

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
? 0
j
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 2 x1 
F 
 REV
T 
0
F 
 REV
T 
0
on i REV
on i REV
on j REV
on j REV
Note: Haug uses ri   r j 
P
at ri 
P
at rj 
P
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
  
 Bj sj ' P
OK
OK
P
Notes_08_11
Double revolute
 REV _ REV  d ij  d ij  L2  0
L  cons tan t length
T
for
and
and
and
d   r   r 
d   r   r 
d   r   r 
d   r  r
P
ij
P
j
i
P
ij
j
ij
j
ij
j
P
i
P
P
i
P
F 
on i REV _ REV
 2dijREV _ REV
T 
on i CONSTRAINT
F 
on j REV _ REV
T 
P
i
P
0
 2dij REV _ REV
on j CONSTRAINT
at ri 
at rj 
P
0
Parallel vectors (planar parallel-1)
a i  parallel
to a j 
 PARALLEL  a i  R  a j   0
T
for
T
a i   ri Q  ri P
and
a   r   r 
Q
j
j
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
Pin-in-slot (planar parallel-2)
P
j
7 of 9
Notes_08_11
to d ij 
a i  parallel
 PIN _ SLOT  a i  R  d ij   0
T
for
and
T
d   r   r 
P
ij
j
P
i
d  d  d 
ij
ij
F 
on i PIN_ SLOT
ij
and
from above
 R a i  PIN_ SLOT
T 
on i CONSTRAINT
 R a i PIN _ SLOT
T 
0
on j PIN _ SLOT
at rj 
P
 norma i normdij PIN_ SLOT
F 
on j PIN _ SLOT
a i   ri Q  ri P
at rj
P
Relative angle 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
C  cons tan t
Gear pair (chain/sprockets, belt/pulleys)
 GEAR   j  Ki  C  0
K  cons tan t, C  cons tan t
external gears K  i /  j , int ernal gears K  i /  j
F 
on i GEAR
T 
 02 x1
on i CONSTRAINT
F 
on j GEAR
 K  GEAR
 02 x1
8 of 9
Notes_08_11
T 
on j CONSTRAINT
  GEAR
Gear pair on rotating link k
 GEAR _ ON _ K   j  k   K i  k   C  0
F 
on i GEAR
 02 x1
T 
 K  GEAR
on i CONSTRAINT
F 
on j GEAR
T 
 02 x1
on j CONSTRAINT
F 
on k GEAR
T 
  GEAR
 02x1
on k CONSTRAINT
 1  K GEAR
K  cons tan t, C  cons tan t from above
9 of 9