articulated body dynamics - Geometric Algorithms for Modeling ...

Advanced Topic p

ARTICULATED BODY DYNAMICS Tim Johnson & Michael Su

Agenda ƒ Inverse Dynamics in general I  D i  i   l ƒ Efficiency ƒ A power tool for designing algorithms – ƒ ƒ ƒ ƒ ƒ

Recursion Recursive Newton‐Euler Algorithm Forward Dynamics in general Featherstone’s Algorithm g Conclusion R f Reference

Inverse Dynamics (1) ƒ Given the kinematic representation of 

motion, inverse dynamics calculates the  motion  inverse dynamics calculates the  forces necessary to achieve that motion ƒ Inverse kinematics will tell us what the  I  ki ti   ill t ll    h t th   motion is ƒ Inverse dynamics will tell us how to do it

Inverse Dynamics (2) ƒ The calculation of the forces required at a 

robot's joints to produce a given set of joint  robot s joints to produce a given set of joint  accelerations ƒ Applications A li ti à Robot control à Trajectory planning

ƒ Rapid execution is essential as it is used 

heavily for real‐time control

Efficiency (1) ƒ The classic approach to inverse dynamics 

involved a Lagrangian formulation, which  involved a Lagrangian formulation  which  was O(n4) ƒ Non‐recursive approaches of either  N i   h   f  ith   Lagrangian or Newton‐Euler formulations  result in equations such as:

O(n2)

3 O(n )

Efficiency (2) ƒ Optimization technique: use recursion à Requires a reformulation of the equations à Can reduce complexity down to O(n) à Reduces computational requirement as well R d   i l  i     ll

Recurrence Relations ƒ Equations defining a member of a sequence 

in terms of its predecessors ƒ Example à xn+1 = xn + xn‐1 à x0 = x1 = 1 à Fibonacci sequence

Example: Recurrence Relation ƒ Let the matrix B be defined as à B = A1A2...A An, where A  where Ai is a matrix

ƒ How do we compute the derivative of B à Brute force ‚ Gets expensive fast à Use recursion

Example: Brute Force ƒ Say B = A1A2A3A4A5 ƒ Then B Then B' =   = 

A1'A2A3A4A5 + A1A2'A3A4A5 + A1A2A3'A4A5 + A1A2A3A4'A5 + A1A2A3A4A5'

ƒ Computational 

requirement is n2‐n  n  matrix multiplications  aand n‐1 matrix  d at additions

Example: Recursion ƒ Recall B = A1A2...An ƒ Define Bi+1 = B  BiAi+1 à It follows that Bn = B, thus Bn' = B'

ƒ So

Bi+1' = B    Bi'A Ai+1 + B  BiAi+1' ƒ To calculate B', we start with B1' = A1' and  iterate up to Bn

Example: Recursion ƒ So we iteratively compute B2,...,Bn‐1 à n‐2 iterations à 1 matrix multiplication

ƒ Then we iteratively compute B2',...,B Bn' à n‐1 iterations à 2 matrix multiplications, 1 matrix addition   t i   lti li ti     t i   dditi

ƒ Total: 3n‐4 matrix multiplications, n‐1 

additions à Non‐recursive: n2‐n matrix multiplications and n‐1  p

additions

Example: Results ƒ By using recurrence relations, we were able 

to reformulate the solution using recursion ƒ O(n2) to O(n) improvement ƒ We can get much more dramatic results with  inverse dynamics à O(n4) to O(n)

Robot System Model ƒ N movable links, labeled 1,...,N (from the root 

to the terminal) ƒ A fixed base link, labeled 0 ƒ λ(i) is the parent link of link i à Links are numbered so that λ(i) 9

Featherstone s Algorithm Featherstone’s Algorithm ƒ Linear relation between the acceleration and 

the force (Newton Euler equation):

Articulated‐body  y Inertia

Test force

Link acceleration Known Unknown

Bias force (the  value of test  force when a = 0)

ƒ No kinematic connection with ground, for ex: 

a floating system ƒ If there are external forces such as gravity,  the equation becomes 

Featherstone’s Algorithm:  Featherstone s Algorithm:  Inertia & Bias forces (1)  ( ) net force on link 1 + force transmitted  to link 2 through the joint Joint Axis s Link 2

… (I)  (I) Constraint imposed by the joint:

… (II)  (II) Active joint force:

Link 1

Scalar joint acceleration 

… (III) Active joint force

Test force f

Featherstone’s Algorithm:  Featherstone s Algorithm:  Inertia & Bias forces (2)  ( ) From (I), (II), and (III), I1A

p1

The above derivation is for a system with 2 links  only. They can be generalized to multiple links  by considering the following scenario:  I1A Root

I2A

Featherstone’s Algorithm:  Featherstone s Algorithm:  Inertia & Bias forces (3)  ( ) I1 is still a simple rigid body but I2 becomes an  articulated body  By a similar derivation  we can  articulated body. By a similar derivation, we can  find out the following recursions for the inertia  and the bias force: Use it’s child’s  information. So if we  can start from the  terminal link….

Featherstone’s Algorithm:  Featherstone s Algorithm:  Joint acceleration (1)  ( ) Definition of joint velocity: …(IV)

Joint Axis s

Link l

Take derivatives: …(V) ( ) Force f applied through  the joint: …(VI) ( )

Moving  Base b

Featherstone’s Algorithm:  Featherstone s Algorithm:  Joint acceleration (2) ( ) From (IV), (V), (VI), and our linear relationship

We can obtain  Joint Axis s

Link l

Moving  Base b

Featherstone’s Algorithm:  Featherstone s Algorithm:  Put all steps together p g FUNCTION ABA_acceleration(q, q_dot, s, Q) {

R Root

v(1) = 0;

Link 1

// Compute velocity for all links.

FOR link i=2 TO N FOR link_i=2 TO N v(link_i) = p_link(v(link_i))+s*q_dot();

J i t  Joint 1 Link 2

// Compute the inertia and the bias forces.

Joint 2

FOR  link_i=N TO 1 { compute_I(link_i, link_i‐1);

Link 3

compute_p(link_i , link_i‐1); } // Compute acceleration for all the joints. // p j a(1) = 0; // link 1’s acceleration FOR joint_i=1 TO N‐1 { compute_q_dotdot(joint_i); // Compute link acceleration for the next joint // Compute link acceleration for the next joint. a(joint_i+1) = a(joint_i)+v(joint_i).cross(s(joint_i))*q_dot(joint_i)+ s(joint_i)*q_dot_dot(joint_i);  } }

Link N‐11 Link N Joint N‐1 Link N

Conclusions ƒ Inverse Dynamics Vs. Forward Dynamics ƒ Recursive Newton‐Euler Algorithm for solving  Recursive Newton Euler Algorithm for solving 

Inverse Dynamics problems ƒ Featherstone’s Algorithm for solving Forward  Dynamics issues ƒ Use the recursion trick to make your program  faster. faster

References Highly recommend!

ƒ Roy Featherstone, “Robot Dynamics 

Algorithm,” Kluwer Academic Publishers 1987 Algorithm  Kluwer Academic Publishers 1987 ƒ Featherstone & Orin, “Robot Dynamics:  E Equations and Algorithms, “ ICRA 2000 ti   d Al ith  “ ICRA  ƒ Karen Liu, “Articulated Rigid Bodies,” slides  from CS7496/4496 Computer Animation class  at Georgia Tech g