Constrained Body Dynamics Chapter 4 in: Mirtich Impulse-based Dynamic Simulation of Rigid Body Systems Ph.D.
Download
Report
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 ri vrel
Rewritten, using
ri 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 vrel
R.Parent, CSE788 OSU
Define wrel and vrel and
their time derivatives
Joint velocity vector
Axis times parametric velocity
i qiui
prismatic
rel 0
vrel i
R.Parent, CSE788 OSU
Joint acceleration vector
Axis times parametric acceleration
i qiui
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 i1 i
Time derivatives of vrel and wrel
(revolute)
Joint acceleration vector
Change in joint velocity vector
rel i i 1 i
vrel 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 vrel
i qiui
(revolute)
i qui qui i qu
ui i 1 ui
i i i 1 i
d
d
(vrel ) (i d i ) i d i i di
dt
dt
di i di (i1 i ) di
vrel 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 ri vrel
vrel i 1 (i di ) i di i (i di )
ai ai 1 i 1 ri i di i 1 (i 1 ri )
2i 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
Rrotation matrix from frame i-1 to i
rradius vector from frame i-1 to frame i (in frame i coordinates)
i Ri 1
vi Rvi 1 i r
i i qiui
vi vi qi (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 )
2i 1 (i di ) i (i di )
Want to put in form:
ai i X i 1ai 1 qi 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 )
2i 1 (i di ) i (i di )
ai i X i 1ai 1 qi si ci
These are the terms involving
u
si i
u i d i
i qiui
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 nn n I nn
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 nn
A A
f n I n an Z n
R.Parent, CSE788 OSU
Inductive case
f
Link i-1
fi 1
Inboard joint
I
i1
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 1i 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 1i 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 1i1X i fi
R.Parent, CSE788 OSU
Inductive case
I
I
fi 1 I i 1ai 1 Z i 1i1X i fi
Invoking induction on the definition of
I
fi
I
A A
fi 1 I i 1ai 1 Z i 1i 1X i (I i ai Z i )
R.Parent, CSE788 OSU
Inductive case
I
A A
fi 1 I i 1ai 1 Z i 1i 1X i (I i ai Z i )
Express ai in terms of ai-1 and rearrange
ai i X i 1ai 1 qi si ci
I
A
A A
A
i ]
fi 1 (I i 1i1X i I i i X i1 )ai 1 Z i1i1X i [Z i I i ci (I i si )q
Need to eliminate from the right side of the equation
R.Parent, CSE788 OSU
qi
Inductive case
I
A
A A
A
i ]
fi 1 (I i 1i1X i I i i X i1 )ai 1 Z i1i1X 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 qi 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 )
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
i ]
fi 1 (I i 1i1X i I i i X i1 )ai 1 Z i1i1X 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 si [Qi si I i A i X i 1ai 1 si ( Z i A I i A 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
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
Rrotation matrix from frame i-1 to i
rradius vector from frame i-1 to frame i (in frame i coordinates)
i Ri 1
vi Rvi 1 i r
i i qiui
vi vi qi (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 ) 2i 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 siI i
I i 1 I i 1 i 1 X 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 1 X i [ Z i I i ci
]
A
siI i si
a0 0
For i = n to 2 do
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
ai i X i 1ai 1 ci qi si
R.Parent, CSE788 OSU