Transcript How to Simulate a Ponytail & a Raibert Hopper or Practical Lagrange Multiplier Constraints (for Games) or How to pack two complete lectures into 1

How to Simulate a Ponytail &
a Raibert Hopper
or
Practical Lagrange Multiplier Constraints (for Games)
or
How to pack two complete lectures into 1 hour.
Chris Hecker
definition six, inc.
[email protected]
This Lecture Might Be Boring
• you may...
– have already covered this stuff satisfactorily
– already know this stuff well
– be able to give this lecture yourself
• but it is...
– very practical, nitty-gritty information
– slightly game focused
– accompanied by running code examples
Road Map
• Very detailed Lagrange multipliers for
ponytail
• passive 3DOF position constraints
• Some high level controller stuff
• Slightly less detailed Lagrange multipliers
for hopper
• position and angular constraints
• Some lower level controller stuff
Ponytail Demo
• incentive to listen • constrained dynamics
– why use physics?
• set expectations
– no collisions
– no contact
• other uses (rope,
etc.)
– rigid body dynamics
– open loop chain
• kinematic control
– incremental adoption of physics
– animation plus physics for effects
Thanks to Vector Graphics (www.vectorg.com) for the model!
Lagrange Multipliers
• calculate an extra force to
keep the constraint satisfied
• algorithm:
–
–
–
–
calculate all non-joint forces
calculate joint forces
apply all forces back to bodies
simulate
• integrate state forward in time
fc
body a
body b
Prerequisites and Review
• mostly “on the fly” review during lecture
• comfortable with equations and derivations
– don’t hesitate to yell out questions during
derivations
• derivatives, vector algebra, solving
equations
Prerequisites and Review (cont.)
p
• kinematics
•
•
•
p=R+r
·
·p = R
+w×r
··
··
p=R+a×r+w×(w×r)
• dynamics
··
• f = mR , t = I a + w × L
• fcm= fp , tcm= r × fp
• state integration
• position, velocity, acceleration
r
R
Two Bodies, One Constraint
• derivation of dynamics equations
– figure out notation and conventions
– write dynamics equations with unknown constraint
force
– write constraint equation in terms of body accelerations
– plug ‘n’ chug to get constraint eqn with unknown
constraint force
– solve for constraint force
• our goal: A fc = b
The Goal: A fc = b
Two Bodies, One Constraint (cont.)
• figure out notation and conventions
• write dynamics equations
··
Fa = MaRa
fc
Rb
rb
pa = pb
ra
Ra
The Goal: A fc = b
Two Bodies, One Constraint (cont.)
• figure out notation and conventions
• write dynamics equations
··
Fa = MaRa
ta = Ia aa + wa × La
fc
Rb
rb
pa = pb
ra
Ra
The Goal: A fc = b
Two Bodies, One Constraint (cont.)
• figure out notation and conventions
• write dynamics equations
··
Fa = MaRa
ta = Ia aa + wa × La
FE, tE = gravity, springs, etc.
fc
Rb
rb
pa = pb
ra
Ra
The Goal: A fc = b
Two Bodies, One Constraint (cont.)
• figure out notation and conventions
• write dynamics equations
··
Fa = MaRa
ta = Ia aa + wa × La
FE, tE = gravity, springs, etc.
Fa = FEa + fc
fc
Rb
rb
pa = pb
ra
Ra
The Goal: A fc = b
Two Bodies, One Constraint (cont.)
• figure out notation and conventions
• write dynamics equations
··
Fa = MaRa
ta = Ia aa + wa × La
FE, tE = gravity, springs, etc.
Fa = FEa + fc , ta = tEa + ra × fc
fc
Rb
rb
pa = pb
ra
Ra
The Goal: A fc = b
Two Bodies, One Constraint (cont.)
• write dynamics equations (cont.)
Rb
• Newton’s 3rd Law
fca = fc , tca·· = ra × fc
fc
rb
pa = pb
ra
Ra
fcm= fp , tcm= r × fp
The Goal: A fc = b
Two Bodies, One Constraint (cont.)
• write dynamics equations (cont.)
Rb
• Newton’s 3rd Law
fca = fc , tca·· = ra × fc
fcb = – fc , tcb = – rb × fc
fc
rb
pa = pb
ra
Ra
fcm= fp , tcm= r × fp
The Goal: A fc = b
Two Bodies, One Constraint (cont.)
• write dynamics equations (cont.)
Rb
• Newton’s 3rd Law
fca = fc , tca·· = ra × fc
fcb = – fc , tcb = – rb × fc
··
Ra = Ma-1(FEa + fc )
fc
rb
pa = pb
ra
Ra
··
Fa = MaRa
The Goal: A fc = b
Two Bodies, One Constraint (cont.)
• write dynamics equations (cont.)
Rb
• Newton’s 3rd Law
fca = fc , tca = ra × fc
fcb = – fc , tcb = – rb × fc
··
Ra = Ma-1(FEa + fc )
aa = Ia-1(tEa + ra × fc – wa × La )
fc
rb
pa = pb
ra
Ra
ta = Ia aa + wa × La
The Goal: A fc = b
Two Bodies, One Constraint (cont.)
• write dynamics equations (cont.)
Rb
• Newton’s 3rd Law
fca = fc , tca = ra × fc
fcb = – fc , tcb = – rb × fc
··
Ra = Ma-1(FEa + fc )
aa = Ia-1(tEa + ra × fc – wa × La )
··
Rb = Mb-1(FEb – fc )
ab = Ib-1(tEb – rb × fc – wb × Lb )
fc
rb
pa = pb
ra
Ra
The Goal: A fc = b
Two Bodies, One Constraint (cont.)
• write constraint equation
Rb
pa – pb = 0
fc
rb
pa = pb
ra
Ra
The Goal: A fc = b
Two Bodies, One Constraint (cont.)
• write constraint equation
Rb
pa – pb = 0
Ra + ra – Rb – rb = 0
fc
rb
pa = pb
ra
Ra
The Goal: A fc = b
Two Bodies, One Constraint (cont.)
• write constraint equation
Rb
pa – pb = 0
Ra + ra – Rb – rb = 0
p· a – p·b = 0
fc
rb
pa = pb
ra
Ra
The Goal: A fc = b
Two Bodies, One Constraint (cont.)
• write constraint equation
Rb
pa – pb = 0
Ra + ra – Rb – rb = 0
rb
fc
pa = pb
p· a – p·b = 0
·
ra
·
Ra + wa × ra – Rb – wb × rb = 0
Ra
·
·
p=R+w×r
The Goal: A fc = b
Two Bodies, One Constraint (cont.)
• write constraint equation
Rb
pa – pb = 0
Ra + ra – Rb – rb = 0
pa = pb
p· a – p·b = 0
·
·
Ra + wa × ra – Rb – wb × rb = 0
p··a – p··b = 0
fc
rb
ra
Ra
The Goal: A fc = b
Two Bodies, One Constraint (cont.)
• write constraint equation
Rb
pa – pb = 0
Ra + ra – Rb – rb = 0
pa = pb
p· a – p·b = 0
·
·
Ra + wa × ra – Rb – wb × rb = 0
p··a – p··b = 0
fc
rb
ra
Ra
·· + a × r + w × w × r – R·· – a × r – w × w × r = 0
R
a
a
a
a
a
a
b
b
b
b
b
b
The Goal: A fc = b
Two Bodies, One Constraint (cont.)
• plug ‘n’ chug
Rb
• look at one side of constraint
··
p··a = Ra + aa × ra + wa × wa × ra
unknown acc.
known vel.
fc
rb
pa = pb
ra
Ra
The Goal: A fc = b
Two Bodies, One Constraint (cont.)
• plug ‘n’ chug
Rb
• look at one side of constraint
··
p··a = Ra + aa × ra + wa × wa × ra
unknown acc.
known vel.
fc
rb
pa = pb
ra
Ra
The Goal: A fc = b
Two Bodies, One Constraint (cont.)
• plug ‘n’ chug
Rb
• look at one side of constraint
fc
··
p··a = Ra + aa × ra + wa × wa × ra
unknown acc.
rb
pa = pb
ra
known vel.
··
Ra = Ma-1 fc + Ma-1FEa
Ra
··
Ra = Ma-1(FEa + fc )
The Goal: A fc = b
Two Bodies, One Constraint (cont.)
• plug ‘n’ chug
Rb
• look at one side of constraint
··
p··a = Ra + aa × ra + wa × wa × ra
unknown acc.
··
known vel.
Ra = Ma-1 fc + Ma-1FEa
aa = Ia-1(ra × fc) + Ia-1(tEa – wa × La )
fc
rb
pa = pb
ra
Ra
aa = Ia-1(tEa + ra × fc – wa × La )
The Goal: A fc = b
Two Bodies, One Constraint (cont.)
• plug ‘n’ chug (cont.)
··
··
pa = Ra + aa × ra + wa × wa × ra
p··a = Ma-1 fc + Ma-1FEa +
··
Ra = Ma-1 fc + Ma-1FEa
The Goal: A fc = b
Two Bodies, One Constraint (cont.)
• plug ‘n’ chug (cont.)
··
··
pa = Ra + aa × ra + wa × wa × ra
p··a = Ma-1 fc + Ma-1FEa + [Ia-1(ra × fc)] × ra +
[Ia-1(tEa – wa × La )] × ra +
aa = Ia-1(ra × fc) + Ia-1(tEa – wa × La )
The Goal: A fc = b
Two Bodies, One Constraint (cont.)
• plug ‘n’ chug (cont.)
··
··
pa = Ra + aa × ra + wa × wa × ra
p··a = Ma-1 fc + Ma-1FEa + [Ia-1(ra × fc)] × ra +
[Ia-1(tEa – wa × La )] × ra + wa × wa × ra
The Goal: A fc = b
Two Bodies, One Constraint (cont.)
• plug ‘n’ chug (cont.)
• how to get fc “out of” the cross term?
p··a = Ma-1 fc + [Ia-1(ra × fc)] × ra +
Ma-1FEa + [Ia-1(tEa – wa × La )] × ra +
wa × wa × ra
= ba
The Goal: A fc = b
Two Bodies, One Constraint (cont.)
• how to get fc “out of” the cross term? (cont.)
[ Ia-1 ( ra × fc ) ] × ra =
The Goal: A fc = b
Two Bodies, One Constraint (cont.)
• how to get fc “out of” the cross term? (cont.)
[ Ia-1 ( ra × fc ) ] × ra =
– ra × [ Ia-1 ( ra × fc ) ] =
x×y=–y×x
The Goal: A fc = b
Two Bodies, One Constraint (cont.)
• how to get fc “out of” the cross term? (cont.)
[ Ia-1 ( ra × fc ) ] × ra =
– ra × [ Ia-1 ( ra × fc ) ] =
~
– ra Ia-1 ( ra × fc ) =
~
x×y=xy=
0 -x3 x2
x3 0 -x1
-x2 x1 0
y1
y2 =
y3
x2 y3 - x3 y 2
x3 y1 - x1 y 3
x1 y2 - x2 y 1
The Goal: A fc = b
Two Bodies, One Constraint (cont.)
• how to get fc “out of” the cross term? (cont.)
[ Ia-1 ( ra × fc ) ] × ra =
– ra × [ Ia-1 ( ra × fc ) ] =
~
– ra Ia-1 ( ra × fc ) =
– r~a Ia-1 r~a fc
The Goal: A fc = b
Two Bodies, One Constraint (cont.)
• plug ‘n’ chug (cont.)
• coalesce fc terms…we’re almost done!
p··a = Ma-1 fc – r~a Ia-1 r~a fc + ba
The Goal: A fc = b
Two Bodies, One Constraint (cont.)
• plug ‘n’ chug (cont.)
• coalesce fc terms…we’re almost done!
p··a = Ma-1 fc – r~a Ia-1 r~a fc + ba
p··a = [ Ma-1 – r~a Ia-1 r~a ] fc + ba
The Goal: A fc = b
Two Bodies, One Constraint (cont.)
• plug ‘n’ chug (cont.)
• coalesce fc terms…we’re almost done!
p··a = Ma-1 fc – r~a Ia-1 r~a fc + ba
p··a = [ Ma-1 – r~a Ia-1 r~a ] fc + ba
p··a = Aa fc + ba
The Goal: A fc = b
Two Bodies, One Constraint (cont.)
• plug ‘n’ chug (cont.) - pb and final eqn.
p··a = Aa fc + ba
p··b = Ab(– fc ) + bb
The Goal: A fc = b
Two Bodies, One Constraint (cont.)
• plug ‘n’ chug (cont.) - pb and final eqn.
p··a = Aa fc + ba
p··b = Ab(– fc ) + bb
p··a – p··b = Aa fc + ba + Ab fc – bb = 0
p··a – p··b = 0
The Goal: A fc = b
Two Bodies, One Constraint (cont.)
• plug ‘n’ chug (cont.) - pb and final eqn.
p··a = Aa fc + ba
p··b = Ab(– fc ) + bb
p··a – p··b = Aa fc + ba + Ab fc – bb = 0
[Aa + Ab] fc = – ba + bb
A fc = b , a linear system
Two Bodies, One Constraint (cont.)
• simulation algorithm:
compute external forces: FEa , tEa , FEb , tEb
compute A matrix and b vector
solve for fc
apply fc back to objects
fca = fc , tca = ra × fc
fcb = – fc , tcb = – rb × fc
• calculate accelerations from forces
• integrate state forward
•
•
•
•
More Bodies, More
Constraints
• more fc’s to solve for
• each body’s acceleration affected by
multiple constraints
• the equations get real messy
• but the structure remains the same
– A matrices and b vectors relate
constraint forces and bodies
– more abstract derivations available
Kinematic Control
• kinematic bodies have already-known
accelerations
• assuming your animation functions are differentiable
• piecewise linear, splines, slerps, etc.
• plug in known acc. into constraint equations
··
R = known function
a = known function
Hopper Demo
• incentive to listen
– why use physics?
• set expectations
– not a walking human
Hopper Demo (cont.)
• physical features
– monopod
– prismatic knee
– massive body, light leg
Overview & Master PlanTM
• top down discussion
• control as it informs writing a simulator
• constrained dynamics
• joint types and equations
• not simulation review
• there will be math, but sometimes more implied than
explicit
Rough Hierarchy of Control
(I had to pick some names.)
• Cognition
– Where do I want to go today?
• Locomotion
– How do I move in a given direction?
• Actuation
– What torque do I exert to move my foot?
• Simulation
– What effect does the torque have on my body?
c
o
m
m
a
n
d
s
f
e
e
d
b
a
c
k
What does a controller need
from a simulator?
• at least two “callbacks”
• access to current state
• robust simulation of the controlled creature
• joints, contact, collisions, forces, etc.
Simulation Loop - No Control
while(CurrentTime < TargetTime)
Integrate(DeltaTime)
ComputeExternalForces(CurrentTime)
ComputeJointForces(CurrentTime)
IntegrateStep(DeltaTime)
Resolve Collisions/Penetration
CurrentTime += DeltaTime
Simulation Loop - Control
Think(CurrentTime)
while(CurrentTime < TargetTime)
Integrate(DeltaTime)
ComputeExternalForces(CurrentTime)
ComputeControllerForces(CurrentTime)
ComputeJointForces(CurrentTime)
IntegrateStep(DeltaTime)
Resolve Collisions/Penetration
CurrentTime += DeltaTime
Think
• Locomotion
• usually state machine
• eg. “if contacting, go to stance phase”
• goal changes for muscles
• eg. change setpoints for leg length to thrust leg
ComputeControllerForces
• Muscle Actuation
• directly exerts forces & torques on bodies
• must use only current state & time
• time independent, or explicity time dependent
• no internal state variables
• may be called at any time
• must continuously [smoothly] attain goals
• no if’s, abs, max, min, etc.
Access to Current State
• raw state
– CM positions, velocities, etc.
• derived state:
– e.g. scalar “leg length”
– derived state derivatives
Rb
Ra
“Intuitive” Lagrange Multiplier
Constraints Review
• “How to Simulate a Ponytail”
articles and lectures
• Summary:
– write equations to describe
constraint
– calculate forces to keep the
constraint satisfied
Rb
fc
rb
pa = pb
ra
Ra
Lagrange Multiplier Constraints
Review (cont.)
• the C(q) you write determines
the joint
– ponytail spherical:
C(q) = pa – pb = 0
• you can constrain bodies in
any way by writing the
appropriate C(q)
– fc may have torques in it
Rb
fc
rb
pa = pb
ra
Ra
Joint Types for Hopper
• spherical hip joint
– same as ponytail
– 3 DOF
• prismatic knee joint
– sliding joint, like telescope
– 1 DOF
Joints and Constraints in
General
• rigid bodies have 6 DOF
– 3 position, 3 orientation
– n bodies have n*6 DOF as a system
• joints reduce DOF between bodies
• joint vs. constraint DOF
• constraint equations mathematically express
DOF reductions
• each constraint is a signed error metric
• k constraints means fc will have k elements
Hopper Hip
• spherical joint – 3 DOF
pax – pbx
– C(q) = pa – pb = pay – pby = 0
paz – pbz
• 2 bodies + 1 spherical = 9 DOF
– 6 DOF for 1st body
– 3 DOF for 2nd
• orientation around the joint point
Rb
fc
rb
pa = pb
ra
Ra
Hopper Knee
• prismatic joint – 1 DOF
– joint subtracts 5 DOF
Rb
• 2 bodies + 1 prismatic = 7 DOF
– 6 DOF for 1st body
– 1 DOF for 2nd
• distance along prismatic axis
• C(q) = ?
– we know C(q) is a 5 x 1 vector
Ra
Hopper Knee (cont.)
• what are the 5 DOF we want to
constrain?
Rb
– 3 rotational DOF
– 2 translational DOF
Ra
Hopper Knee (cont.)
Rotational Constraints
• dot product constraints remove 1
DOF each
• how about C(q) = ta • nb = 0
Rb
nb
– 1 DOF like expected
• what force or torque will enforce
this?
– t in direction of ta  nb
– Newton’s 3rd law
– can derive cross product from
differentiation directly
– what about ta • tb = 1?
Ra
ta
Hopper Knee (cont.)
• 3 rotational DOF
– ta • n1 = 0, ta • n2 = 0
– n3 • n2 = 0
– t’s in direction of va  vb
– these dot products differentiate with w
• ta • n1  (wa × ta) • n1 + ta • (wb × n1)
• 2 translational DOF
– (Rb – Ra) • n1 = 0, (Rb – Ra) • n2 = 0
– forces in direction of n1, n2
– n’s are local coordinate frames
– contrast with pa - pb = 0
n2
Rb
ta
Ra
n1
n4
n3
Soft Constraints
• damped springs, PDCs, servos
• fc = ks (C – Cd) + kd (C’ – Cd’)
n2
Rb
n1
• fc is force or torque or both
• example: hip as soft constraint
– 3D spring
• example: constrain final knee DOF
– C = (Rb – Ra) • ta = 0
– C’ = (Rb’ – Ra’) • ta = 0
• where is (Rb – Ra) • ta’ term?
– fc in direction of t
ta
Ra
n4
n3
Low Level Hopper Muscle
Actuation
• knee length
– pick good gains for bouncing
– change leg setpoint for length control
n2
Rb
ta
Ra
n1
n4
n3
Low Level Hopper Muscle Actuation
(cont.)
• hip orientation
– roll & pitch: move za around in b’s
coordinate frame using za• xb and za• yb
– yaw: xa• yb
– problems with singularities
• euler angles, quaternions, etc.
zb
za
xb
Pros & Cons of Lagrange
Multipliers
• Pros
– flexibility, modularity, generality
• turn on and off, etc.
• Cons
– accuracy, stability, O(n3)
• Baumgarte Stabilization
• sparse matrices O(n)
Homework
•
•
•
•
•
do multibody derivation
collision detection
contact
different constraint types
closed loops
Math to the Rescue!
• Complete lecture in one page!
••
C(q) = 0
Mq = FE + FC
•
••
••
•
••
C(q) = Jq = 0 C(q) = Jq + Jq = 0
FC = JTl
••
q = M-1(FE + FC) = M-1(FE + JTl)
• •
JM-1JTl = - Jq - JM-1FE
Math Rules! (cont.)
•
•
•
•
•
q – 6n × 1, M – 6n × 6n
C – 3k × 1, J – 3k × 6n
JM-1JT – 3k×6n • 6n×6n • 6n×3k = 3k ×3k
all very sparse!
A is symmetric positive definite
What we didn’t talk about...
•
•
•
•
•
•
much control theory
Jacobian constraint derivations
other joint equations
stability
contact
generalized coordinate methods
References
• Shabana, Constrained Dynamics
• great for Jacobian methods
• My dynamics page
– http://www.d6.com/users/checker/dynamics.htm
• Witkin’s Siggraph Physically Based
Modeling notes