Beruflich Dokumente
Kultur Dokumente
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
Fi
ri di
Fi-1
ui
Need to determine: q
[q, q ]
ai ai 1 i 1 ri i 1 ri vrel
Rewritten, using ri vi vi 1 and vi vi 1 i 1 ri vrel
(relative velocity) (from previous slide)
ri i 1 ri vrel
i i 1 rel
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)
rel i i 1 i
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 )
2i 1 (i d i ) i (i di )
Spatial Algebra
R.Parent, CSE788 OSU
Spatial Algebra
v a
v a
R 0
vG G X FvF G X F ~
r R R
aG G X Fa F (cross product operator)
(used in later)
R.Parent, CSE788 OSU
ComputeSerialLinkVelocities
(revolute)
0 , v0 , 0 , a0 0
For i = 1 to N do
i i 1 i i 1 i
ai ai 1 i 1 ri i d i i 1 (i 1 ri )
2i 1 (i d i ) i (i di )
Want to put in form:
ai i X i 1ai 1 qi 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 )
2i 1 (i d i ) i (i di )
ai i X i 1ai 1 qi si ci u
si i
ui d i
These are the terms involving i qi ui
i 1 i
ci
i 1 (i 1 ri ) 2 i 1 ( i d i ) i ( i d i
)
O
fi Spatial force exerted on link i through its outboard joint
I
f n mn g mn an
n I n n n I nn
I
Inboard joint
n I mn g
fnI 0 M n n mn g
I
n I n 0 an n I nn
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 1i 1
I
Invoking induction on the definition of fi
I A A
fi 1 I i 1ai 1 Z i 1i 1X i ( I i ai Z i )
I A A A A
fi 1 ( I i 1i 1X i I i i X i 1 )ai 1 Z i 1i 1X i [Z i I i ci ( I i si )qi ]
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 qi si ci
and f i I i ai Z i
I A A
fi I i (i X i 1ai 1 qi 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 )
qi 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 )
qi A
si I i si
I A A A A
fi 1 ( I i 1i 1X i I i i X i 1 )ai 1 Z i 1i 1X i [Z i I i ci ( I i si )qi ]
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
A 0 Mi
Ii
Ii 0
i 1 i
ci
i 1 (i 1 ri ) 2i 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 siI i
I i 1 I i 1 i 1X i [ I i A ]i X i 1
siI 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 ]
siI i si
// loop inside out to compute link and joint accelerations
a0 0
For i = 1 to n do
A A A
Qi siI i i X i 1ai 1 si( Z i I i ci )
qi A
siI i si
a i X i 1ai 1 ci qi si
R.Parent, CSE788 OSUi
And that’s all there is to it!