Dynamics of Linked Hierarchies

Constrained dynamics
The Featherstone equations
Constrained dynamics
Apply force to one component, other components repositioned, from near
to far, to satisfy distance constraints

F
 Constrained Body Dynamics

Chapter 4 in:
Mirtich
Impulse-based Dynamic Simulation of Rigid Body
Systems
Ph.D. dissertation, Berkeley, 1996

R.Parent, CSE788 OSU

 Preliminaries
• Links numbered 0 to n
• Fixed base: link 0; Outermost like: link n
• Joints numbered 1 to n
• Link i has inboard joint, Joint i
• Each joint has 1 DoF
• Vector of joint positions: q=(q1,q2,…qn)T
…
2 n

R.Parent, CSE788 OSU

 The Problem
• Given:
– the positions q and velocities q of the n joints of
a serial linkage,
– the external forces acting on the linkage,
– and the forces and torques being applied by the
joint actuators

• Find: The resulting accelerations of the joints: q

R.Parent, CSE788 OSU

 First Determine equations that
give absolute motion of all links

Given: the joint positions q, velocities

and accelerations
Compute: for each link the linear and
angular velocity and acceleration relative
to an inertial frame

R.Parent, CSE788 OSU

 Notation – global variables

vi Linear velocity of link i

ai Linear acceleration of link i

i Angular velocity of link i

i Angular acceleration of link i

R.Parent, CSE788 OSU

 Joint variables
qi joint position

qi joint velocity

ui Unit vector in direction of the axis of joint i

ri vector from origin of Fi-1 to origin of Fi

di vector from axis of joint i to origin of Fi

R.Parent, CSE788 OSU

 Basic terms
• Fi – body frame of link i
• Origin at center of mass
• Axes aligned with principle axes of inertia
Frames at center of mass

Fi

ri di
Fi-1
ui

Need to determine: q

[q, q ]

Axis of articulation State vector

R.Parent, CSE788 OSU
 From base outward

• Velocities and accelerations of link i are

completely determined by:
1. the velocities and accelerations of link i-1
2. and the motion of joint i

R.Parent, CSE788 OSU

 First – determine velocities and
accelerations
From velocity and acceleration of previous
link, determine total (global) velocity and
acceleration of current link
Computed from base outward
To be computed
vi 1, , ai 1 , i 1 ,  i 1 vi , , ai , i ,  i
Motion of link i
Motion of link i-1

vrel , , arel , rel ,  rel

Motion of link i from local joint
R.Parent, CSE788 OSU
 Compute outward
Angular velocity of link i =
angular velocity of link i-1 plus
angular velocity induced by rotation at joint i
i  i 1  rel
Linear velocity =
linear velocity of link i-1 plus
linear velocity induced by rotation at link -1 plus
linear velocity from translation at joint i
vi  vi 1  i 1  ri  vrel
R.Parent, CSE788 OSU
 Compute outward
Angular acceleration propagation  i   i 1   rel

Linear acceleration propagation

ai  ai 1   i 1  ri  i 1  ri  vrel
Rewritten, using ri  vi  vi 1 and vi  vi 1  i 1  ri  vrel
(relative velocity) (from previous slide)
ri  i 1  ri  vrel

ai  ai 1   i 1  ri  i 1  (i 1  ri )  i 1  vrel  vrel

R.Parent, CSE788 OSU
 Compute outward
Angular acceleration propagation

 i   i 1   rel

Linear acceleration propagation

ai  ai 1   i 1  ri  i 1  (i 1  ri )  i 1  vrel  vrel

 rel
Need
vrel qi
In terms of joint axis motion qi
ui
R.Parent, CSE788 OSU
 Define wrel and vrel and
their time derivatives
Joint velocity vector Joint acceleration vector
Axis times parametric velocity Axis times parametric acceleration

i  qi ui i  qi ui (unkown)

prismatic revolute

rel  0 rel  i
vrel  i vrel  i  di
R.Parent, CSE788 OSU
 Velocity propagation formulae
(revolute)

linear
vi  vi 1  i 1  ri  vrel
vrel  i  di
vi  vi 1  i 1  ri  i  di

i  i 1  rel
angular

rel  i
R.Parent, CSE788 OSU
i  i 1  i
Time derivatives of vrel and wrel
(revolute)

Joint acceleration vector

Change in joint velocity vector

 rel  i  i 1 i

vrel  2i 1  (i  di )  i  di  i  (i  di )

From joint acceleration vector
From change in joint velocity vector

From change in change in vector from joint to CoM

R.Parent, CSE788 OSU
 Derivation of vrel
(revolute)
i  qi ui
i  qui  qui  i  qu ui  i 1  ui
i  i  i 1 i
d d
(vrel )  (i  d i )  i  d i  i  di
dt dt
di  i  di  (i 1  i )  di
vrel  2i 1  (i  di )  i  di  i  (i  di )

R.Parent, CSE788 OSU

 Acceleration propagation formulae
(revolute)

linear ai  ai 1   i 1  ri  i 1  ri  vrel Previously

derived

vrel  2i 1  (i  di )  i  di  i  (i  di )

ri  i 1  ri  vrel
ai  ai 1   i 1  ri  i  d i  i 1  (i 1  ri ) 
2i 1  (i  d i )  i  (i  di )

angular
 i   i 1   rel
 rel  i  i 1 i
R.Parent, CSE788 OSU
 i   i 1  i  i 1 i
 Spatial formulation of
acceleration propagation
(revolute)
vi  vi 1  i 1  ri  i  di
i  i 1  i
 i   i 1  i  i 1 i
ai  ai 1   i 1  ri  i  d i  i 1  (i 1  ri ) 
2i 1  (i  d i )  i  (i  di )

But remember i  qi ui is an unknown

R.Parent, CSE788 OSU

 First step in forward dynamics
• Use known dynamic state: q, q
• Compute absolute linear and angular
velocities: v, 
• Remember: Acceleration propagation
equations involve unknown joint
accelerations

But first – need to introduce notation to facilitate equation writing

Spatial Algebra
R.Parent, CSE788 OSU
 Spatial Algebra

Spatial velocity Spatial acceleration

     
v  a 
v a

R.Parent, CSE788 OSU

 Spatial Transform Matrix
r – offset vector R– rotation

     R 0
vG  G X FvF G X F  ~ 
    r R R 
aG  G X Fa F (cross product operator)

R.Parent, CSE788 OSU

 Spatial Algebra
Spatial force Spatial transpose
 f 
f  
 
 a 

x    b 
T
a T

b 

Spatial joint axis Spatial inner product

  ui  
si  
  x y
 i i
u d

(used in later)
R.Parent, CSE788 OSU
 ComputeSerialLinkVelocities
(revolute)

0 , v0 ,  0 , a0  0
For i = 1 to N do

Rrotation matrix from frame i-1 to i

rradius vector from frame i-1 to frame i (in frame i coordinates)
i  Ri 1
vi  Rvi 1  i  r
i  i  qiui
Specific to revolute joints
vi  vi  qi (ui  di )
end
R.Parent, CSE788 OSU
 Spatial formulation of
acceleration propagation
Previously: (revolute)

 i   i 1  i  i 1 i
ai  ai 1   i 1  ri  i  d i  i 1  (i 1  ri ) 
2i 1  (i  d i )  i  (i  di )
Want to put in form:
    
ai i X i 1ai 1  qi s  ci
  R 0   u 
G X F  ~
Where:  si   i 
  r R R  ui  d i 
R.Parent, CSE788 OSU
 Spatial Coriolis force
(revolute)

 i   i 1  i  i 1 i
ai  ai 1   i 1  ri  i  d i  i 1  (i 1  ri ) 
2i 1  (i  d i )  i  (i  di )
    
ai i X i 1ai 1  qi si  ci   u 
si   i 
ui  d i 
These are the terms involving i  qi ui

  i 1 i 
ci   

 i 1  (i 1  ri )  2 i 1  ( i  d i )   i  ( i  d i 
)

R.Parent, CSE788 OSU

 Featherstone algorithm
 Spatial acceleration of link i
ai
I
fi Spatial force exerted on link i through its inboard joint

O
fi Spatial force exerted on link i through its outboard joint

All expressed in frame i

Forces expressed as acting on center of mass of link i

R.Parent, CSE788 OSU

 Serial linkage articulated motion
I  A   A
f i  I i ai  Z i
A Spatial articulated inertia of link I; articulated means
Ii entire subchain is being considered

A Spatial articulated zero acceleration force of link I

Zi (independent of joint accelerations); force exerted
by inboard joint on link i, if link i is not to accelerate

Develop equations by induction

R.Parent, CSE788 OSU

 Base Case
Consider last link of linkage (link n)

Force/torque applied by inboard joint + gravity = inertia*accelerations of link

Newton-Euler equations of motion

I
f n  mn g  mn an
 n  I n n  n  I nn
I

R.Parent, CSE788 OSU

 Using spatial notation
I
Link n fn

Inboard joint
n I mn g

 fnI   0 M n   n    mn g 
 I     
 n   I n 0   an  n  I nn 
  A  A
f n I n an Z n
R.Parent, CSE788 OSU
 Inductive case
Assume previous is true for link i; consider link i-1

Link i-1 f i 1
O
 i 1
O
I
f i 1 outboard joint

Inboard joint
mi 1 g
 i 1 I

 f i 1I   f i 1O   0 M i 1   i 1    mi 1 g 
 I  O   
 i 1   i 1   I i 1 0   ai 1  i 1  I i 1i 1 
  

R.Parent, CSE788 OSU

 Inductive case
 f i 1I   f i 1O   0 M i 1   i 1    mi 1 g 
 I  O   
 i 1   i 1   I i 1 0   ai 1  i 1  I i 1i 1 
  
 I     O
f i 1 I i 1ai 1 Z i 1 fi 1
The effect of joint I on link i-1 is equal and
opposite to its effect on link i
 O I
fi 1 i 1 X ifi
Substituting…
 I    I
fi 1 I i 1ai 1 Z i 1i 1X i fi
R.Parent, CSE788 OSU
 Inductive case
 I    I
fi 1 I i 1ai 1 Z i 1i 1X i fi

I
Invoking induction on the definition of fi

 I     A  A
fi 1 I i 1ai 1 Z i 1i 1X i ( I i ai Z i )

R.Parent, CSE788 OSU

 Inductive case
 I     A  A
fi 1 I i 1ai 1 Z i 1i 1X i ( I i ai Z i )
Express ai in terms of ai-1 and rearrange
    
ai i X i 1ai 1  qi si  ci

 I  A    A  A  A
fi 1  ( I i 1i 1X i I i i X i 1 )ai 1 Z i 1i 1X i [Z i  I i ci  ( I i si )qi ]

Need to eliminate from the right side of the equation qi

R.Parent, CSE788 OSU

 Inductive case
 I  A    A  A  A
fi 1  ( I i 1i 1X i I i i X i 1 )ai 1 Z i 1i 1X i [Z i  I i ci  ( I i si )qi ]
Magnitude of torque exerted by revolute joint actuator is Qi

A force f and a torque  applied to

f u d link i at the inboard joint give rise
to a spatial inboard force (resolved
 in the body frame) of  I  fi 
fi   
 i  di  f i 


   I  ui   fi 
Qi  si f i      d    f i  (ui  d i )  ( i  d i  f i )  ui   i  ui
u 
 i i
d  i i fi 
Moment of force Moment of force
R.Parent, CSE788 OSU
 Inductive case
 I
Qi  si fi   i  ui
     I  A   A
previously ai i X i 1ai 1  qi si  ci
and f i  I i ai  Z i
I  A     A
fi I i (i X i 1ai 1  qi si  ci ) Z i

Premultiply both sides by si substitute Qi for s’f , and solve

 A     A  A 
Qi  si I i i X i 1ai 1  si ( Z i  I i ci )
qi   A 
si I i si
R.Parent, CSE788 OSU
 And substitute
 A     A  A 
Qi  si I i i X i 1ai 1  si ( Z i  I i ci )
qi   A 
si I i si
 I  A    A  A  A
fi 1  ( I i 1i 1X i I i i X i 1 )ai 1 Z i 1i 1X i [Z i  I i ci  ( I i si )qi ]

 A   A
 I   A I i si si I i 
f i 1  [ I i 1 i 1 X i ( I i     )i X i 1 ]ai 1 
 A
si I i si
 A   A      A  A
  A  A  I i si [Qi  si I i i X i 1ai 1  si ( Z i  I i ci )]
Z i 1 i 1 X i [ Z i  I i ci   A  ]
si I i si
R.Parent, CSE788 OSU
 And form I & Z terms
 A   A
 I   A I i si si I i 
f i 1  [ I i 1 i 1 X i ( I i     )i X i 1 ]ai 1 
 A
si I i si
 A   A      A  A
  A  A  I i si [Qi  si I i i X i 1ai 1  si ( Z i  I i ci )]
Z i 1 i 1 X i [ Z i  I i ci   A  ]
si I i si
I  A   A
To get into form: f i  I i ai  Z i
 A   A
 A   A I i si si I i
I i 1  I i 1 i 1 X i ( I i     )i X i 1
 A
si I i si
 A   A      A  A
 A   A  A  I i si [Qi  si I i i X i 1ai 1  si ( Z i  I i ci )]
Z i 1  Z i 1 i 1 X i [ Z i  I i ci   A  ]
R.Parent, CSE788 OSU
si I i si
 Ready to put into code
Using
• Loop from inside out to compute velocities
previously developed (repeated on next slide)
• Loop from inside out to initialize I, Z, and c
variables
• Loop from outside in to propagate I, Z and c
updates
• Loop from inside out to compute q  using I, Z, c

R.Parent, CSE788 OSU

 ComputeSerialLinkVelocities
(revolute)
// This is code from an earlier slide – loop inside out to compute velocities
0 , v0 ,  0 , a0  0
For i = 1 to N do

Rrotation matrix from frame i-1 to i

rradius vector from frame i-1 to frame i (in frame i coordinates)
i  Ri 1
vi  Rvi 1  i  r
i  i  qiui
Specific to revolute joints
vi  vi  qi (ui  di )
end
R.Parent, CSE788 OSU
 InitSerialLinks
(revolute)

// loop from inside out to initialize Z, I, c variables

For i = 1 to N do
A   mi g 
Zi   
 
 i i i
I 

A 0 Mi 
Ii  
Ii 0 

  i 1 i 
ci   
i 1  (i 1  ri )  2i 1  (i  di )  i  (i  di )
end
R.Parent, CSE788 OSU
 SerialForwardDynamics
// new code with calls to 2 previous routines
Call compSerialLinkVelocities
Call initSerialLinks
// loop outside in to form I and Z for each linke
For i = n to 2 do  A   A
 A  A   A I i si siI i 
I i 1  I i 1  i 1X i [ I i    A  ]i X i 1
siI i si
 A   A A 
 A  A   A  A  I i si [Qi  si(Z i  I i ci )]
Z i 1  Z i 1  i 1X i [ Z i  I i ci    A ]
siI i si
// loop inside out to compute link and joint accelerations

a0  0
For i = 1 to n do
 A     A  A
Qi  siI i i X i 1ai 1  si( Z i  I i ci )
qi    A
 siI i si
   
a i X i 1ai 1  ci  qi si
R.Parent, CSE788 OSUi

 And that’s all there is to it!

R.Parent, CSE788 OSU

i  i 1  rel ri  vi  vi 1
vi  vi 1  i 1  ri  vrel ri  i 1  ri  vrel
 i   i 1   rel
ai  ai 1   i 1  ri  i 1  ri  vrel
ai  ai 1   i 1  ri  i 1  (i 1  ri )  i 1  vrel  vrel
 rel  i  i 1 i i  qi ui rel  i
vrel  i 1  (i  di )  i  di  i  (i  di ) i  qi ui vrel  i  di
d d
(vrel )  (i  d i )  i  d i  i  di
dt dt i  i  i 1 i
di  i  di  (i 1  i )  di
vi  vi 1  i 1  ri  i  di
vrel  2i 1  (i  di )  i  di  i  (i  di ) i  i 1  i

R.Parent, CSE788 OSU

 ai  ai 1   i 1  ri  i  d i  i 1  (i 1  ri ) 
2i 1  (i  d i )  i  (i  di )
 i   i 1  i  i 1 i

R.Parent, CSE788 OSU

         R 0
v  a  G X F  ~ 
v a   r R R 
 f 
f  
 
 a 
x      bT aT 
b 
  ui  
x y
si   
u 
 i i
d

R.Parent, CSE788 OSU