Articulated Body Dynamics The Basics

1. Comp 768 October 23, 2007 Will Moss Articulated Body DynamicsThe Basics

2. Overview • Motivation • Background / Notation • Articulate Dynamics Algorithms • Newton-Euler Algorithm • Composite-Rigid Body Algorithm • Articulated-Body Algorithm (Featherstone)‏ • Lagrange Multiplier approach (Baraff)‏

3. History • Originally a problem from robotics • Given a robotic arm with a series of joints that can apply forces to themselves (called motors)‏, find the forces to get the robot arm into the desired configuration

4. Applications • Computer Graphics • Humans, animals, birds, robots, etc. • Wires, chains, ropes, etc. • Trees, grass, etc. • Many more • http://vrlab.epfl.ch/~alegarcia/VHOntology/long.html

5. Basics • An articulated body is a group of rigid bodies (called links) connected by joints • Multiple types of joints • Revolute (1 degree of freedom)‏ • Ball joint (3 degrees of freedom) • Prismatic, screw, etc.

6. Notation • In rigid body dynamics we had two equations • Fs is the vector of spatial forces • Is is the spatial inertia matrix (6 x 6) • as is the spatial acceleration • This is called spatial algebra • Combines the linear and angular components of the physical quantities into one 6 dimensional vector

7. Notation • Transitioning this to articulated bodies • Qi is the force on link i • H is the joint-space inertia matrix (n x n) • are the coordinates, velocities and accelerations of the joints • C term produces the vector of forces that produce zero acceleration

8. Forward vs. Inverse Dynamics • Inverse Dynamics • The calculation of forces given a set of accelerations • Forward Dynamics • The calculation of accelerations given a set of forces

9. Algorithms • Inverse Dynamics • Newton-Euler Algorithm • Forward Dynamics • Composite-Rigid-Body Algorithm • Articulated-Body Algorithm • Lagrange Multiplier Algorithm

10. Newton-Euler Algorithm • Goal • Given the accelerations and velocities at the joints, find the forces required at the joints to generate those accelerations • Recursive approach • Finds the accelerations and velocities of link i in terms of link i - 1

11. Newton-Euler Algorithm • Method • Calculate the velocities and accelerations at each link • Calculate the required net force acting on each link to generate those accelerations • Calculate the joint forces required to generate the net forces on each link

12. Newton-Euler Algorithm • Find the velocities and accelerations of the links

13. Newton-Euler Algorithm Find the forces on each link

14. Newton-Euler Algorithm • Find the forces on the joints • This can be reformulated in link coordinates to speed up the calculation • Runs in O(n)

15. Forward vs. Inverse Dynamics • Inverse Dynamics • The calculation of forces given a set of accelerations • Forward Dynamics • The calculation of accelerations given a set of forces

16. Composite-Rigid-Body Algorithm • Q is the vector of the forces on the links • H is the joint-space inertia matrix (n x n) • C vector of forces that produce zero acceleration • Algorithm • Calculate the elements of C • Calculate the elements of H • Solve the set of simultaneous equations

17. Composite-Rigid-Body Algorithm • Solve for C • Setting the acceleration to zero, we get • We can, therefore, interpret C as the forces which produce no acceleration • We can use a forward-dynamics solver (like Newton-Euler) to solve for the forces given the position, velocity and an acceleration of zero

18. Composite-Rigid-Body Algorithm • Solve for H • If we set C to 0, we observe that is the vector of joint forces that will impart an acceleration of onto a stationary robot • Therefore, the ith column of H is the vector of forces required to produce a unit of acceleration about joint i and no other acceleration. • Treat the links i…n as a rigid-body with inertia defined by • Treat the links from 1…i-1 are therefore unmoving

19. Composite-Rigid-Body Algorithm • Solve for H (cont.) • Given that • Since none of the links from 1 … i-1 are moving, every joint transmits onto the subsequent link, so we can solve for H by solving • Which is a complete solution for H since it is symmetric • Runs in O(n2)

20. Composite-Rigid-Body Algorithm • Once you have H and C, solve the system of equations using any solver • O(n3), but the constant is small enough that for n less than ~12 the O(n2) term dominates • Like Newton-Euler, this can be reformulated in link coordinates • Faster for n ≤ 16

21. Articulated-Body Algorithm • (Re)consider the equation of motion of an articulated body • This is true for any link in the articulated body

22. Articulated-Body Algorithm • Consider an articulate robot as a single joint attached to an articulated body • The problem simplifies to the forward dynamics of a one-joint robot (much simpler than the general case) • The first joint is simply a one-joint robot • The second joint is a one-joint robot with a moving base (slightly more complicated, but still much simpler that the general case) • Solving this requires two tasks • Solving the one-joint robot forward dynamics problem • Finding the articulated-body inertias (I) and bias forces (p)

23. Articulated-Body Algorithm • Solving the one-joint robot problem

24. Articulated-Body Algorithm • Finding the articulated-body inertia (IA) and bias force (p) Where is called the velocity-product force and is defined to be

25. Articulated-Body Algorithm • These formulas can be reformulated recursively, so allow us to find and in terms of only and • Our algorithm is then • Calculate the series of articulated body inertias and bias forces • Using these inertias and bias forces, calculate the joint accelerations • Since these are both defined recursively, they each take O(n), making the entire algorithm O(n)

26. Lagrange-Multiplier Method • The preceding methods are reduced-coordinate formulations • These methods remove some of the dof’s by enforcing a set of constraints (a joint can only rotate in a certain direction constraining the motion of the joint and the link) • Finding a parameterization for the generalized coordinates in terms of the reduced coordinates is not always easy • The Lagrange Multiplier Method considers all the d.o.f.’s of the system

27. Lagrange-Multiplier Method • Consider the equation of motion of i bodies • M describes the mass properties of the system is an d x d matrix where d is the number of dof’s of body i when not constrained • Also consider a constraint i that removes m dof’s from the system, we can write it as • Where each jik is a m x d matrix that represents the constraint on link k where • d is again the number of dof’s of body k and • m is the number of dof’s removed by the constraint

28. Lagrange-Multiplier Method • To simplify the notation, we replace the q individual constraint equations • With • Where J is a q x n matrix of the individual jik matrices • Where q is the total number of constraints on the system and • n is the number of bodies • c is a q dimensional vector

29. Lagrange-Multiplier Method • Just as we did when solving the constrained particle dynamics problems, we require that the constraint does no work. This results is a constraint force of the form: • Where λi is an m (dof’s removed by constraint i) dimensional column vector and is referred to as the Lagrange multiplier • The problem is now just to find a λ so the constraint forces and any external forces satisfy the constraints

30. Lagrange-Multiplier Method • If we introduce an external force acting on the system and combine the equations, we get • Solving for and plugging into our constraint equation, we get • For constraints that act on two bodies, the matrix system is tightly banded and can be solved in O(n) • Using banded Cholesky decomposition, for example

31. Lagrange-Multiplier Method • For more complicated constraints, is no longer sparse and we reformulate the equation as • If we required acyclic constraints, then sparse-matrix theory tells us that has perfect elimination order • This means that if we factor into three matrices LDLT, L will be as sparse as H and can be computed in O(n) • We can then solve for λ, by solving each piece of LDLT separately, each also in O(n) time and combining the solutions

32. Summary • Inverse Dynamics • Newton-Euler is the standard implementation in O(n) • Forward dynamics • Composite-rigid-body algorithm is simpler and faster for n < 9, runs in O(n3) • Articulated-body algorithm is faster for n > 9, runs in O(n) • Lagrange multiplier method is somewhat simpler than ABA and speed is comparable, runs in O(n)

33. References / Thanks • R. Featherstone, Robot Dynamics Algorithms, Boston/Dordrecht/Lancaster: Kluwer Academic Publishers, 1987. • D. Baraff, "Linear-Time Dynamics using Lagrange Multipliers," Proc. SIGGRAPH '96, pp. 137-146, New Orleans, August 1996. • Thanks to Nico for his slides from last year