Constrained Body Dynamics Chapter 4 in Mirtich Impulsebased
Constrained Body Dynamics Chapter 4 in: Mirtich Impulse-based Dynamic Simulation of Rigid Body Systems Ph. D. dissertation, Berkeley, 1996 R. Parent, CSE 788 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 Do. F Vector of joint positions: q=(q 1, q 2, …qn)T 2 1 R. Parent, CSE 788 OSU … n
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, CSE 788 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, CSE 788 OSU
Notation Linear velocity of link i Linear acceleration of link i Angular velocity of link i Angular acceleration of link i R. Parent, CSE 788 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, CSE 788 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 Axis of articulation R. Parent, CSE 788 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, CSE 788 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 R. Parent, CSE 788 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 Linear velocity: R. Parent, CSE 788 OSU
Compute outward Angular acceleration propagation Linear acceleration propagation Rewritten, using and (relative velocity) R. Parent, CSE 788 OSU (from previous slide)
Define wrel and vrel and their time derivatives Joint velocity vector Axis times parametric velocity prismatic R. Parent, CSE 788 OSU Joint acceleration vector Axis times parametric acceleration revolute
Velocity propagation formulae (revolute) linear angular R. Parent, CSE 788 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 vector from joint to Co. M R. Parent, CSE 788 OSU
Derivation of (revolute) R. Parent, CSE 788 OSU
Propagation formulae (revolute) linear angular R. Parent, CSE 788 OSU
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, CSE 788 OSU
Spatial Algebra Spatial velocity R. Parent, CSE 788 OSU Spatial acceleration
Spatial Transform Matrix r – offset vector R– rotation (cross product operator) R. Parent, CSE 788 OSU
Spatial Algebra Spatial force Spatial joint axis (used in later) R. Parent, CSE 788 OSU Spatial transpose Spatial inner product
Compute. Serial. Link. Velocities (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, CSE 788 OSU
Spatial formulation of acceleration propagation (revolute) Previously: Want to put in form: R. Parent, CSE 788 OSU
Spatial Coriolis force (revolute) These are the terms involving R. Parent, CSE 788 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, CSE 788 OSU
Serial linkage articulated motion Spatial articulated inertia of link i Spatial articulated zero acceleration force of link I (independent of joint accelerations) Develop equations by induction R. Parent, CSE 788 OSU
Base 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, CSE 788 OSU
Using spatial notation Link n Inboard joint R. Parent, CSE 788 OSU
Inductive case Link i-1 outboard joint Inboard joint R. Parent, CSE 788 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, CSE 788 OSU
Inductive case Invoking induction on the definition of R. Parent, CSE 788 OSU
Inductive case Express ai in terms of ai-1 and rearrange Need to eliminate from the right side of the equation R. Parent, CSE 788 OSU
Inductive case Magnitude of torque exerted by revolute joint actuator is Qi f u d t 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 Moment of force R. Parent, CSE 788 OSU Moment of force
Inductive case previously Premultiply both sides by R. Parent, CSE 788 OSU and substitute for Qi , and solve
And substitute R. Parent, CSE 788 OSU
And form I & Z terms To get into form: R. Parent, CSE 788 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 using I, Z, c R. Parent, CSE 788 OSU
Compute. Serial. Link. Velocities (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, CSE 788 OSU
Init. Serial. Links (revolute) For i = 1 to N do end R. Parent, CSE 788 OSU
Serial. Forward. Dynamics Call comp. Serial. Link. Velocities Call init. Serial. Links For i = n to 2 do For i = 1 to n do R. Parent, CSE 788 OSU
- Slides: 39