Dynamics of Linked Hierarchies 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 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 n 1 R.Parent, CSE788 OSU

The Problem Given: the positions q and velocities 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: 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 – global variables Linear velocity of link i Linear acceleration of link i Angular velocity of link i Angular acceleration of link i R.Parent, CSE788 OSU

Joint variables joint position joint velocity Unit vector in direction of the axis of joint i vector from origin of Fi-1 to origin of Fi 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 ri di Fi-1 Need to determine: ui Axis of articulation State vector R.Parent, CSE788 OSU

From base outward Velocities and accelerations of link i are completely determined by: the velocities and accelerations of link i-1 and the motion of joint i R.Parent, CSE788 OSU

First – determine velocities and accelerations From velocity and acceleration of previous link, determine total (global) velocity and acceleration of current link Computed from base outward To be computed Motion of link i Motion of link i-1 Motion of link i from local joint R.Parent, CSE788 OSU

Compute outward Angular velocity of link i = angular velocity of link i-1 plus angular velocity induced by rotation at joint i Linear velocity = linear velocity of link i-1 plus linear velocity induced by rotation at link -1 plus linear velocity from translation at joint i R.Parent, CSE788 OSU

Compute outward Angular acceleration propagation Linear acceleration propagation Rewritten, using and (relative velocity) (from previous slide) R.Parent, CSE788 OSU

Compute outward Need In terms of joint axis motion Angular acceleration propagation Linear acceleration propagation Need In terms of joint axis motion R.Parent, CSE788 OSU

Define wrel and vrel and their time derivatives Joint acceleration vector Joint velocity vector Axis times parametric velocity Axis times parametric acceleration (unkown) prismatic revolute R.Parent, CSE788 OSU

Velocity propagation formulae (revolute) linear angular R.Parent, CSE788 OSU

Time derivatives of vrel and wrel (revolute) Joint acceleration vector Change in joint velocity vector 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 (revolute) R.Parent, CSE788 OSU

Acceleration propagation formulae (revolute) linear Previously derived angular R.Parent, CSE788 OSU

Spatial formulation of acceleration propagation (revolute) But remember is an unknown R.Parent, CSE788 OSU

First step in forward dynamics Use known dynamic state: Compute absolute linear and angular velocities: Remember: Acceleration propagation equations involve unknown joint accelerations But first – need to introduce notation to facilitate equation writing Spatial Algebra R.Parent, CSE788 OSU

Spatial Algebra Spatial velocity Spatial acceleration R.Parent, CSE788 OSU

Spatial Transform Matrix r – offset vector R– rotation (cross product operator) R.Parent, CSE788 OSU

Spatial Algebra Spatial transpose Spatial force Spatial joint axis Spatial inner product (used in later) R.Parent, CSE788 OSU

ComputeSerialLinkVelocities (revolute) For i = 1 to N do Rrotation matrix from frame i-1 to i rradius vector from frame i-1 to frame i (in frame i coordinates) Specific to revolute joints end R.Parent, CSE788 OSU

Spatial formulation of acceleration propagation (revolute) Previously: Want to put in form: Where: R.Parent, CSE788 OSU

Spatial Coriolis force (revolute) These are the terms involving R.Parent, CSE788 OSU

Featherstone algorithm 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 Spatial articulated inertia of link I; articulated means entire subchain is being considered Spatial articulated zero acceleration force of link I (independent of joint accelerations); force exerted by inboard joint on link i, if link i is not to accelerate 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 R.Parent, CSE788 OSU

Using spatial notation Link n Inboard joint R.Parent, CSE788 OSU

Inductive case Assume previous is true for link i; consider link i-1 outboard joint Inboard joint R.Parent, CSE788 OSU

Inductive case The effect of joint I on link i-1 is equal and opposite to its effect on link i Substituting… R.Parent, CSE788 OSU

Inductive case Invoking induction on the definition of R.Parent, CSE788 OSU

Inductive case Express ai in terms of ai-1 and rearrange Need to eliminate from the right side of the equation R.Parent, CSE788 OSU

Inductive case Magnitude of torque exerted by revolute joint actuator is Qi A force f and a torque t applied to link i at the inboard joint give rise to a spatial inboard force (resolved in the body frame) of u f d t Moment of force Moment of force R.Parent, CSE788 OSU

Inductive case previously and Premultiply both sides by substitute Qi for s’f , and solve R.Parent, CSE788 OSU

And substitute R.Parent, CSE788 OSU

And form I & Z terms To get into form: 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 initialize I, Z, and c variables Loop from outside in to propagate I, Z and c updates Loop from inside out to compute using I, Z, c R.Parent, CSE788 OSU

ComputeSerialLinkVelocities (revolute) // This is code from an earlier slide – loop inside out to compute velocities For i = 1 to N do Rrotation matrix from frame i-1 to i rradius vector from frame i-1 to frame i (in frame i coordinates) Specific to revolute joints end R.Parent, CSE788 OSU

InitSerialLinks For i = 1 to N do end (revolute) // loop from inside out to initialize Z, I, c variables For i = 1 to N do 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 // loop inside out to compute link and joint accelerations For i = 1 to n do R.Parent, CSE788 OSU

And that’s all there is to it! R.Parent, CSE788 OSU

R.Parent, CSE788 OSU

R.Parent, CSE788 OSU

R.Parent, CSE788 OSU