Articulated Body Dynamics

Articulated rigid bodies

Articulated body dynamics

Beyond human models How would you represent a pose?

Quadraped animals

Wavy hair

Animal fur

Plants Maximal vs. reduced coordinates How are things connected?

Maximal coordinates Reduced coordinates Maximal coordinate θ0, φ0, ψ0 Treat each body part as a separate rigid body (x0, R0) Use explicit constraints to connect body parts θ1, φ1

(x1, R1) Reduced coordinate

θ2 (x2, R2) Use joint angles directly as state variables state variables: 6m state variables: n Hard to derive the equation of motion for articulated Assuming there are m links and n DOFs in the articulated body, how many bodies constraints do we need to keep links connected correctly in maximal coordinates?

Maximal coordinates Reduced coordinates

Direct extension of well understood rigid body dynamics; easy to understand and implement Joint space is more intuitive when dealing with Operate in Cartesian space; hard to complex multi-body structures evaluate joint angles and velocities Fewer DOFs and fewer constraints enforce joint limits Well suited for character motion and motion control apply internal joint torques

Inaccuracy in numeric integration can cause body parts to drift apart Forward simulation Featherstone’s algorithm

Given current state, current velocity, external forces, and Use reduced coordinates to represent motion joint torques, compute the current acceleration of the articulated body Given the current joint q , current joint velocity q ˙ , E external forces F and joint torque G , compute the joint acceleration q¨ in linear time E Featherstone’s algorithm q¨ = f(q, q˙ , F , G) Lagrangian method link λ(i) joint i connects link i and its parent link i

Spatial notation Spatial velocity of each link

J Spatial notation combines linear and angular quantities If we v i let be the velocity of link i, and v i be the velocity across joint i then Two ordinary 3-dimensional vectors are replaced by a J v = vi − vλ i single 6-dimensional spatial vector i ( ) The joint velocity can also be described in the form ω v = angular velocity of the body ! x˙ " J linear velocity of the body vi = hiq˙ i

ω˙ a = ˙ ! x¨ " where hi is a 6 by di matrix, q is a di by 1 vector and di is the degree of freedom of joint i Spatial acceleration of each link Newton-Euler equations

Velocity of link i: Equation of motion for link i: f f E I a v × I v vi = vλ(i) + hiq˙ i i + i = i i + i i i

net force applied to link sum of all other forces Acceleration of link i: i through the joints actin on link i ˙ ai = aλ(i) + hiq˙ i + hiq¨i f = f J − f J f J λ i i i ! j i is the force transmitted from link ( ) j∈µ(i) µ(i) is the set of children of link i

f J I a v × I v − f E J i = i i + i i i i + ! fj j∈µ(i)

Acceleration-force relation Featherstone’s algorithm

E The acceleration of bodies are always linear functions of proc ABM_accelerations( q , q˙ , F , G ) the applied forces /* first outbound loop */ v = 0 a = Φf + b 0 for i = 1 to N - 1 = + ˙ The equation can be inverted to vi vλ(i) hiqi

A A /* inbound loop */ f = I a + p where IA = Φ−1 pA = −IAb Compute_Inertia_Bias();

A I articulated body inertia /* second outbound loop */ pA bias force, the force required to bring the body’s Compute_joint_accel(); acceleration to zero Inbound loop Second outbound loop

Starting at the terminal links, calculate the inertia and Compute joint acceleration from the root to the terminal bias force for each link in turn link τ0 = 0 IA I IA − IAh hT IAh −1hT IA i = i + ! ( j j j( j j j) j j ) aλ(0) = 0 j∈µ(i) for i = 0 to N-1 A α A T A −1 − T α pi = pi + ! (pj + Ij hj(hj Ij hj) (Gi hj pj )) T A −1 − T A α q¨i = (hi Ii hi) (τi hi (Ii aλ(i) + pi )) j∈µ(i) ˙ ai = aλ(i) + hiq˙ i + hiq¨i

J A A fi = Ii ai + pi = × − E where pi vi Iivi fi f J = I a + v × I v − f E + J T J i i i i i i i ! fj τi = hi fi α A A ˙ j∈µ(i) pj = pj + Ij hjq˙ j

Lagrangian method Generalized coordinates

d ∂T ∂T − − Q = 0 The configuration of a multi-body system is identify by a set of ˙ j dt ∂qj ∂qj variables called generalized coordinates These generalized coordinates are independent and completely determine the location and orientation of each body in the system T denotes the kinetic energy one particle: x, y, z Qj is the generalized force associated with coordinate j

x, y, z, θ, φ, ψ j is the index for DOFs in generalized coordinates one rigid body:

x, y, z, articulated bodies: θ0, φ0, ψ0 θ1, φ1

θ2 Peaucellier mechanism Generalized forces

Represent a point ri on the articulated body system by a set of generalized coordinates: ri = ri(q1, q2, . . . , qn)

The purpose of this mechanism is The virtual displacement of ri can be written in terms of to generate a straight-line motion generalized coordinates r r r r r ∂ i ∂ i ∂ i = ∂ i δ i = δq1 + δq2 + . . . + δqn ! δqj This mechanism has eight bodies ∂q1 ∂q2 ∂qn ∂q j j and yet the number of degrees of The virtual work of force Fi acting on ri is freedom is one ∂r F δr = F i δq = Q δq i i i ! ∂q j ! j j j j j

Define generalized force associated with coordinate qj ∂ri Qj = Fi · ∂qj

Kinetic energy Lagrangian method

1 T Ti = r˙ r˙τi dx dy dz r = Wr0 2 ! Put it all together 1 d ∂T ∂T ∂W T ˙ T ˙ i − i = tr i M W¨ T = Ti = r0 Wi Wir0τi dx dy dz i i Qj 2 ! dt ∂q˙j ∂qj ! ∂qj "

1 ˙ T ˙ T Ti = tr Wir0r W τi dx dy dz 2 ! 0 i " # Represent external forces fk in terms of generalized coordinates 1 ˙ T ˙ T T Ti = tr Wi r0r0 τi dx dy dz Wi Mi = r0r0 τi dx dy dz 2 ! " $ % ! # ∂W ∂p tr i ¨ T = k MiWi fk 1 ! ∂qj " ∂qj T = tr W˙ M W˙ T #k i 2 ! i i i "

d ∂Ti ∂Ti where fk is acting at the point pk on the articulated body system Compute ˙ and by yourself dt ∂qj ∂qj Acceleration of transformation Constraints

Represent W ¨ ( q ) in terms of q , q ˙ and q¨

Compute W ¨ ( q ) recursively Penalty methods Wi = Wi−1Ri Use proportional derivative (PD) controllers W˙ i = W˙ i−1Ri + Wi−1R˙ i Analytical methods W¨ i = W¨ i−1Ri + 2W˙ i−1R˙ iWi−1R¨ i Solve a linear system R¨ (q) =

Acceleration constraints Multiple constraints

Compute the force magnitudes f that satisfy all the Given a desired joint acceleration q ¨ c , what is the acceleration constraints simultaneously torque that gives rise to it? 0 Force - Acceleration relationship h1(q¨) − h1(q¨ ) = k11f1 + k12f2 + · · · + k1mfm 0 0 0 h2 q − h2 q k21f1 k22f2 · · · k2mfm af = kf + a h qf − h q kf (¨) (¨ ) = + + + (¨ ) (¨ ) = . . t k 1. Use test torque g to compute 0 ¨t − ¨0 hm(q¨) − hm(q¨ ) = km1f1 + km2f2 + · · · + kmmfm t t 0 = q q q¨ = kg + q¨ k t g h − h0 = Kf c 2. Use k to compute the desired joint torque g c 0 1 t − 0 c t q¨ − q¨ kji = t (hj(q¨i) hj(q¨ )) g = g fi q¨t − q¨0 Impact constraints Impact constraints

Use impulse to instantaneously change the body’s Compute desired acceleration on the body point + − − velocities ap = (vp vp )/δt Find the appropriate constraint magnitude that Compute the current joint velocities, satisfy the acceleration constraint fp h a − a q ˙ ! that changes the velocity of body (q¨) = (q¨) p p v− v+ point instantaneously from p to p ¨0 v+ Evaluate the default joint acceleration, q ,and the p p acceleration, q ¨ , after f p is applied p Compute the joint velocity after the impulse v− p − q˙ + = q˙ + (q¨p − q¨0)δt

The simulation step Collision response

UpdateConstraintSet(): Collision detection

ResolveImpact(): Instantaneously change the relative velocity at the contact point

ComputeConstraintForces(): Use constraint forces to prevent interpenetration Joint limits Summary

UpdateConstraintSet(): Compare current joints Define phase space against joint limits a set of coordinates that fully determine the motion ResolveImpact(): Neutralize the joint velocity that Write down the equation of motion violates the joint limit Deal with constraints ComputeConstraintForces(): Use constraint torque to set joint acceleration to zero penalty methods vs. analytical methods

Particle system Rigid body system

phase space: x phase space: x ! v "  R   P     L  equation of motion: equation of motion: f = mx¨ f = mx¨ τ = Iω˙ + I˙ω constraints: constraints: compute the constraint forces compute impulse and impulsive torque compute contact forces Articulated body system What’s next?

phase space: q ! q˙ " Is this enough to simulate animal’s natural motion? equation of motion: How do animals use muscles to generate locomotion? d ∂T ∂T f f E I a v × I v − − = 0 i + i = i i + i i i or Qj dt ∂q˙j ∂qj We will have our first guest lecture on Thursday constraints: Prof. Petros Faloutsos from UCLA use force - acceleration relationship to compute the constraint torques