Transcript Constrained Body Dynamics Chapter 4 in: Mirtich Impulse-based Dynamic Simulation of Rigid Body Systems Ph.D.

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
1
R.Parent, CSE788 OSU
…
n
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
R.Parent, CSE788 OSU
vi
Linear velocity of link i
ai
Linear acceleration of link i
i
Angular velocity of link i
i
Angular acceleration of link i
Joint variables
qi
q i
ui
joint position
joint velocity
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
di
ri
Fi-1
ui
[ q , q]
Axis of articulation
R.Parent, CSE788 OSU
State vector
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 velocity and
acceleration of current link
To be computed
Computed from base outward
vi , , ai , i ,i
vi 1, , ai 1, i 1,i 1
vrel , , arel , rel ,rel
R.Parent, CSE788 OSU
From local joint
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:
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
(relative velocity)
and
vi  vi 1  i 1  ri  vrel
(from previous slide)
ai  ai 1  i 1  ri  i 1  (i 1  ri )  i 1  vrel  vrel
R.Parent, CSE788 OSU
Define wrel and vrel and
their time derivatives
Joint velocity vector
Axis times parametric velocity
i  qiui
prismatic
rel  0
vrel  i
R.Parent, CSE788 OSU
Joint acceleration vector
Axis times parametric acceleration
i  qiui
revolute
rel  i
vrel  i  di
Velocity propagation formulae
(revolute)
linear
vi  vi 1  i 1  ri  vrel
vrel  i  di
vi  vi 1  i 1  ri  i  di
angular
R.Parent, CSE788 OSU
i  i 1  rel
rel  i
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  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
i  qiui
(revolute)
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  i 1  (i  di )  i  di  i  (i  di )
R.Parent, CSE788 OSU
Propagation formulae
(revolute)
linear
ai  ai 1  i 1  ri  i 1  ri  vrel
vrel  i 1  (i  di )  i  di  i  (i  di )
ai  ai 1   i 1  ri  i  di  i 1  (i 1  ri ) 
2i 1  (i  di )  i  (i  di )
angular
R.Parent, CSE788 OSU
i  i 1   rel
 rel  i  i 1 i
i  i 1  i  i 1 i
First step in forward dynamics
• Use known dynamic state (q, q-dot)
• Compute absolute linear and angular
velocities
• Acceleration propagation equations
involve unknown accelerations
But first – need to introduce notation to facilitate equation writing
Spatial Algebra
R.Parent, CSE788 OSU
Spatial Algebra
Spatial velocity
Spatial acceleration
  
v 
v
  
a 
a
R.Parent, CSE788 OSU
Spatial Transform Matrix
r – offset vector
 

vG G X FvF
 

aG G X FaF
R.Parent, CSE788 OSU
R– rotation
  R
0
G X F  ~

 r R R 
(cross product operator)
Spatial Algebra
Spatial force
 f
f  
 
Spatial joint axis
  ui 
si  

u

d
 i i
(used in later)
R.Parent, CSE788 OSU
Spatial transpose


 a 
T

x    b
b 
a
T

Spatial inner product

x y
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
vi  vi  qi (ui  di )
end
R.Parent, CSE788 OSU
Specific to revolute joints
Spatial formulation of
acceleration propagation
Previously:
i  i 1  i  i 1 i
(revolute)
ai  ai 1   i 1  ri  i  di  i 1  (i 1  ri ) 
2i 1  (i  di )  i  (i  di )
Want to put in form:
  
 
ai i X i 1ai 1  qi s  ci
R.Parent, CSE788 OSU
Spatial Coriolis force
(revolute)
i  i 1  i  i 1 i
ai  ai 1   i 1  ri  i  di  i 1  (i 1  ri ) 
2i 1  (i  di )  i  (i  di )
  
 
ai i X i 1ai 1  qi si  ci
These are the terms involving
  u 
si   i 
u i  d i 
i  qiui
i 1 i



ci  



(


r
)

2


(


d
)



(


d
)
i 1
i
i 1
i
i
i
i
i 
 i 1
R.Parent, CSE788 OSU
Featherstone algorithm

ai
I
fi
O
fi
Spatial acceleration of link i
Spatial force exerted on link i through its inboard joint
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
fi I i ai Z i
A
Ii
A
Zi
Spatial articulated inertia of link i
Spatial articulated zero acceleration force of link I
(independent of joint accelerations)
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
I
 n  I nn  n  I nn
R.Parent, CSE788 OSU
Using spatial notation
Link n
fn
Inboard joint
 fn I   0
 I
 n   I n
I
n
I
mn g
M n   n    mn g 




0   an  n  I nn 
  A  A
f n I n an Z n
R.Parent, CSE788 OSU
Inductive case
f
Link i-1
fi 1
Inboard joint

I
 i1
O
i 1
outboard joint
I
 f i 1I   f i 1O   0
 I  O  
 i 1   i 1   I i 1
R.Parent, CSE788 OSU
O
i 1
mi 1 g
M i 1   i 1    mi 1 g 





0   ai 1  i 1  I i 1i 1 
Inductive case
 f i 1I   f i 1O   0
 I  O  
 i 1   i 1   I i 1
M i 1   i 1    mi 1 g 




0   ai 1  i 1  I i 1i 1 
 I  
 O

fi 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 1X 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
Invoking induction on the definition of
I
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

i ]
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
Need to eliminate from the right side of the equation
R.Parent, CSE788 OSU
qi
Inductive case
 I

A

 A  A
 A

i ]
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
Magnitude of torque exerted by revolute joint actuator is Qi
f
u
A force f and a torque applied to
link i at the inboard joint give rise
to a spatial inboard force (resolved
in the body frame) of  I 
fi

d

   I  ui 
Qi  si f i  

u

d
 i i

fi  

 i  d i  f i 
fi

  d 
i
 i

 f i  (ui  di )  ( i  di  f i )  ui   i  ui

fi 
Moment of force
R.Parent, CSE788 OSU
Moment of force
Inductive case
previously
 I
Qi  si f i   i  ui
  
 
ai i X i 1ai 1  qi si  ci and
I  A   A
fi I i ai Z i
I  A  
  A
i si  ci ) Z i
fi I i (i X i 1ai 1  q
Premultiply both sides by

si
substitute for Qi , 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

i ]
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
f i 1
 A   A

 A I i si si I i

 [ I i 1 i 1 X i ( I i     )i X i 1 ]ai 1 
 A
si I i si

 



 A  A  I i A si [Qi  si I i A i X i 1ai 1  si ( Z i A  I i A ci )]
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
 I
f i 1
 A   A

 A I i si si I i

 [ 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
To get into form:
I  A   A
fi 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 1X i [ Z i  I i ci 
]
 A 
si I i si
R.Parent, CSE788 OSU
Ready to put into code
Using
• Loop from inside out to compute velocities
previously developed (repeated on next slide)
• Loop from inside out to initialization of I, Z, and c
variables
• Loop from outside in to propagate I, Z and c
updates
• Loop from inside out to compute q-dot-dot using
I, Z, c
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
vi  vi  qi (ui  di )
end
R.Parent, CSE788 OSU
Specific to revolute joints
InitSerialLinks
(revolute)
For i = 1 to N do
A
  mi g 
Zi  



I

 i i i
A
0
Ii  
Ii
Mi 
0 
i 1 i



ci  

i 1  (i 1  ri )  2i 1  (i  d i )  i  (i  di )
end
R.Parent, CSE788 OSU
SerialForwardDynamics
Call compSerialLinkVelocities
Call initSerialLinks
 A   A
 A
 A
  A I i si siI i

I i 1  I i 1  i 1 X 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 1 X i [ Z i  I i ci 
]
  A
siI i si

a0  0
For i = n to 2 do
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





ai i X i 1ai 1  ci  qi si
R.Parent, CSE788 OSU