1 / 40

Constrained Body Dynamics

This chapter explores the problem of determining the resulting accelerations of joints in a serial linkage, given the positions, velocities, external forces, and joint actuator forces. It introduces spatial algebra and develops equations for acceleration propagation using the Newton-Euler equations of motion.

sizemorea
Download Presentation

Constrained Body Dynamics

An Image/Link below is provided (as is) to download presentation Download Policy: Content on the Website is provided to you AS IS for your information and personal use and may not be sold / licensed / shared on other websites without getting consent from its author. Content is provided to you AS IS for your information and personal use only. Download presentation by click this link. While downloading, if for some reason you are not able to download a presentation, the publisher may have deleted the file from their server. During download, if you can't get a presentation, the file might be deleted by the publisher.

E N D

Presentation Transcript

  1. Constrained Body Dynamics Chapter 4 in: Mirtich Impulse-based Dynamic Simulation of Rigid Body Systems Ph.D. dissertation, Berkeley, 1996

  2. 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

  3. 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:

  4. 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

  5. Notation Linear velocity of link i Linear acceleration of link i Angular velocity of link i Angular acceleration of link i

  6. 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

  7. Basic terms • Fi – body frame of link i • Origin at center of mass • Axes aligned with principle axes of inertia ui Frames at center of mass Fi ri di Fi-1 Need to determine: Axis of articulation State vector

  8. 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

  9. 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

  10. 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

  11. Compute outward Angular acceleration propagation Linear acceleration propagation Rewritten, using and (from previous slide) (relative velocity)

  12. 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

  13. Velocity propagation formulae (revolute) linear angular

  14. 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

  15. Derivation of (revolute)

  16. Acceleration propagation formulae (revolute) linear angular

  17. First step in forward dynamics • Use known dynamic state (q, q-dot) • 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

  18. Spatial Algebra Spatial velocity Spatial acceleration

  19. Spatial Transform Matrix r – offset vector R– rotation (cross product operator)

  20. Spatial Algebra Spatial transpose Spatial force Spatial joint axis Spatial inner product (used in later)

  21. 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

  22. Spatial formulation of acceleration propagation (revolute) Previously: Want to put in form: Where:

  23. Spatial Coriolis force (revolute) These are the terms involving

  24. 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

  25. 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

  26. 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

  27. Using spatial notation Link n Inboard joint

  28. Inductive case Link i-1 outboard joint Inboard joint

  29. Inductive case The effect of joint I on link i-1 is equal and opposite to its effect on link i Substituting…

  30. Inductive case Invoking induction on the definition of

  31. Inductive case Express ai in terms of ai-1 and rearrange Need to eliminate from the right side of the equation

  32. 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 d f t Moment of force Moment of force

  33. Inductive case previously and Premultiply both sides by substitute Qi for s’f, and solve

  34. And substitute

  35. And form I & Z terms To get into form:

  36. 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 using I, Z, c

  37. 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

  38. InitSerialLinks (revolute) // loop from inside out to initialize Z, I, c variables For i = 1 to N do end

  39. SerialForwardDynamics // new code with calls to 2 previous routines Call compSerialLinkVelocities For i = n to 2 do Call initSerialLinks // loop outside in to form I and Z for each linke // loop inside out to compute link and joint accelerations For i = 1 to n do

  40. And that’s all there is to it!

More Related