Articulated body dynamics

Transcription

1 Articulated rigid bodies Articulated body dynamics Beyond human models How would you represent a pose? Quadraped animals Wavy hair Animal fur Plants

2 Maximal vs. reduced coordinates How are things connected? Maximal coordinates (x 1, R 1 ) (x 0, R 0 ) Reduced coordinates θ 0, φ 0, ψ 0 θ 1, φ 1 Maximal coordinate Treat each body part as a separate rigid body Use explicit constraints to connect body parts Reduced coordinate (x 2, R 2 ) state variables: 6m Assuming there are m links and n DOFs in the articulated body, how many constraints do we need to keep links connected correctly in maximal coordinates? θ 2 state variables: n Use joint angles directly as state variables Hard to derive the equation of motion for articulated bodies Maximal coordinates Reduced coordinates Direct extension of well understood rigid body dynamics; easy to understand and implement Operate in Cartesian space; hard to evaluate joint angles and velocities enforce joint limits apply internal joint torques Joint space is more intuitive when dealing with complex multi-body structures Fewer DOFs and fewer constraints Well suited for character motion and motion control Inaccuracy in numeric integration can cause body parts to drift apart

3 Forward simulation Featherstone s algorithm Given current state, current velocity, external forces, and joint torques, compute the current acceleration of the articulated body Featherstone s algorithm Use reduced coordinates to represent motion Given the current joint q, current joint velocity q, external forces F E and joint torque G, compute the joint acceleration q in linear time q = f(q, q, F E, G) Lagrangian method link λ(i) joint i connects link i and its parent link i Spatial notation Spatial velocity of each link Spatial notation combines linear and angular quantities Two ordinary 3-dimensional vectors are replaced by a single 6-dimensional spatial vector v i If we let be the velocity of link i, and vi J be the velocity across joint i then v J i = v i v λ(i) [ ωẋ v = [ ωẍ a = ] ] angular velocity of the body linear velocity of the body The joint velocity can also be described in the form v J i = h i q i where h i is a 6 by d i matrix, q is a d i by 1 vector and d i is the degree of freedom of joint i

4 Spatial acceleration of each link Newton-Euler equations Velocity of link i: Equation of motion for link i: v i = v λ(i) + h i q i f i + f E i = I i a i + v i I i v i Acceleration of link i: net force applied to link i through the joints sum of all other forces actin on link i a i = a λ(i) + ḣi q i + h i q i f i = f J i j µ(i) f J j fi J is the force transmitted from link λ(i) µ(i) is the set of children of link i f J i = I i a i + v i I i v i f E i + j µ(i) f J j Acceleration-force relation Featherstone s algorithm The acceleration of bodies are always linear functions of the applied forces a = Φf + b The equation can be inverted to f = I A a + p A where I A = Φ 1 p A = I A b proc ABM_accelerations( q, q, F E, G) /* first outbound loop */ v 0 = 0 for i = 1 to N - 1 v i = v λ(i) + h i q i /* inbound loop */ Compute_Inertia_Bias(); I A p A articulated body inertia bias force, the force required to bring the body s acceleration to zero /* second outbound loop */ Compute_joint_accel();

5 Inbound loop Second outbound loop Starting at the terminal links, calculate the inertia and bias force for each link in turn I A i p A i = I i + (I A j I A j h j (h T j I A j h j ) 1 h T j I A j ) j µ(i) = p i + (p α j + I A j h j (h T j I A j h j ) 1 (G i h T j p α j )) j µ(i) Compute joint acceleration from the root to the terminal link τ 0 = 0 a λ(0) = 0 for i = 0 to N-1 q i = (h T i I A i h i ) 1 (τ i h T i (I A i a λ(i) + p α i )) a i = a λ(i) + ḣi q i + h i q i where p i = v i I i v i f E i p α j = p A j + I A j ḣj q j f J i = I i a i + v i I i v i f E i + j µ(i) f J j f J i = I A i a i + p A i τ i = h T i f J i Lagrangian method Generalized coordinates d T T Q j = 0 dt q j q j T denotes the kinetic energy Q j is the generalized force associated with coordinate j The configuration of a multi-body system is identify by a set of variables called generalized coordinates These generalized coordinates are independent and completely determine the location and orientation of each body in the system one particle: x, y, z j is the index for DOFs in generalized coordinates one rigid body: x, y, z, θ, φ, ψ articulated bodies: θ 1, φ 1 x, y, z, θ 0, φ 0, ψ 0 θ 2

6 Peaucellier mechanism Generalized forces The purpose of this mechanism is to generate a straight-line motion This mechanism has eight bodies and yet the number of degrees of freedom is one Represent a point r i on the articulated body system by a set of generalized coordinates: r i = r i (q 1, q 2,..., q n ) The virtual displacement of r i can be written in terms of generalized coordinates δr i = r i q 1 δq 1 + r i q 2 δq r i q n δq n The virtual work of force F i acting on r i is r i F i δr i = F i δq j = q j j j Q j = F i r i q j = j Q j δq j Define generalized force associated with coordinate q j r i q j δq j Kinetic energy Lagrangian method T i = 1 ṙ T ṙτ i dx dy dz 2 T i = 1 r T 0 2 ẆT i Ẇ i r 0 τ i dx dy dz T i = 1 tr (Ẇi r 0 r T 0 2 ẆT i T i = 1 ( [ 2 tr Ẇ i T i = 1 2 tr (Ẇi M i Ẇ T i ) τ i dx dy dz ] ) r 0 r T 0 τ i dx dy dz Ẇi T ) r = Wr 0 M i = r 0 r T 0 τ i dx dy dz Put it all together d T i T ( i Wi = tr M i Ẅi T dt q j q j q j ) = Q j Represent external forces f k in terms of generalized coordinates ( ) Wi tr M i Ẅi T = q j k f k p k q j d T Compute i T and i by yourself dt q j q j where f k is acting at the point p k on the articulated body system

7 Acceleration of transformation Constraints Represent Ẅ(q) in terms of q, q and q Compute Ẅ(q) recursively W i = W i 1 R i Ẇ i = Ẇi 1R i + W i 1 Ṙ i Ẅ i = Ẅi 1R i + 2Ẇi 1ṘiW i 1 Ri R(q) = Penalty methods Use proportional derivative (PD) controllers Analytical methods Solve a linear system Acceleration constraints Multiple constraints Given a desired joint acceleration q c, what is the torque that gives rise to it? Force - Acceleration relationship a f = kf + a 0 1. Use test torque g t to compute k q t = kg t + q 0 k = qt q 0 2. Use k to compute the desired joint torque g c = g t qc q 0 q t q 0 h( q f ) h( q 0 ) = kf g t g c Compute the force magnitudes f that satisfy all the acceleration constraints simultaneously h 1 ( q) h 1 ( q 0 ) = k 11 f 1 + k 12 f k 1m f m h 2 ( q) h 2 ( q 0 ) = k 21 f 1 + k 22 f k 2m f m. h m ( q) h m ( q 0 ) = k m1 f 1 + k m2 f k mm f m h h 0 = Kf k ji = 1 fi t (h j ( q t i) h j ( q 0 ))

8 Impact constraints Impact constraints Use impulse to instantaneously change the body s velocities Compute desired acceleration on the body point a p = (v + p v p )/δt v p v + p p Compute the current joint velocities, q! that changes the velocity of body point p instantaneously from to v p v + p Find the appropriate constraint magnitude that satisfy the acceleration constraint f p h( q) = a( q) a p Evaluate the default joint acceleration, q 0,and the acceleration, q p, after f p is applied Compute the joint velocity after the impulse 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

9 Joint limits Summary UpdateConstraintSet(): Compare current joints against joint limits ResolveImpact(): Neutralize the joint velocity that violates the joint limit ComputeConstraintForces(): Use constraint torque to set joint acceleration to zero Define phase space a set of coordinates that fully determine the motion Write down the equation of motion Deal with constraints penalty methods vs. analytical methods Particle system Rigid body system phase space: [ x v ] phase space: x R P L equation of motion: f = mẍ constraints: compute the constraint forces equation of motion: f = mẍ τ = I ω + İω constraints: compute impulse and impulsive torque compute contact forces

10 Articulated body system What s next? phase space: [ q q ] Is this enough to simulate animal s natural motion? equation of motion: constraints: f i + f E i = I i a i + v i I i v i or d T T Q j = 0 dt q j q j use force - acceleration relationship to compute the constraint torques How do animals use muscles to generate locomotion? We will have our first guest lecture on Thursday Prof. Petros Faloutsos from UCLA