Articulated Body Dynamics

1
Articulated Body Dynamics

• Chris Oates
• 03/29/04

2
Definition

• Articulated Series of rigid links connected by
  movable joints
• Body Not just particles need to be concerned
  with angular velocity and momentum
• Dynamics Want to know about the forces acting on
  the articulated body more than kinematics

3
Two Problems

• Forward Given a set of forces and torques on the
  joints, calculate accelerations
• Inverse Given an end effector trajectory,
  calculate joint forces and torques
• (Didnt I say three problems in my preview?)

4
Historical Motivation

• Traditionally comes from field of robotics.
  There, you have a robot arm with certain joints
  controlled by motors
• You cannot necessarily control the position of
  the motors, just the force applied
• ABD needed to understand how to apply the forces
  to get the robot arm in the desired position

5
My Motivation

• Want to be able to simulate part of human body
  given the position of the head and the hand, what
  are the most likely positions for the
  intermediate body parts?
• This can be seen as a complicated (10
  degree-of-freedom) robot arm if we ignore
  soft-body nature of human tissues

6
Canonical Robot Arm

• Easiest to focus on canonical case, the 6-DoF
  robot arm
• This example has 4 1-DoF rotational joints, and1
  2-dof joint, but other configurations possible
• Focusing on this makes concepts easier, but does
  not restrict future extension

7
Some Basic Language

• Linki Links start at 0 and go to N
• Jointi Connects Linki-1 to Linki some papers
  have this defined differently, so be careful
• Base Link0 Fixed link, root of hierarchy/chain
• End-Effector LinkN final link in chain,
  sometimes called wrist of robot arm

8
More Basic ideas

• Links in a robot arm may be much more complicated
  than simple lines, so some extra notation was
  created to describe the various fixed and
  variable parameters between links and joints in
  as few numbers as possible
• See Denavit-Hartenberg notation
• We have computing power now to just treat
  everything as homogeneous 4x4 matrix transform,
  so I will do that for simplicity

9
On to the Math
10
But Before we do that, a few more terms and
definitions

• Spatial Vectors
• 6 1 vector of both linear and angular
  components
• Spatial Inertia Matrix of a link
• 6 6 matrix
• System Mass Matrix
• N N matrix mapping joint accelerations to
  forces
• Used in Forward Dynamics simulation

11
Methods

• Recursive Newton-Euler Algorithm
• Solves inverse dynamics problem in linear time
• Composite-Rigid-Body Algorithm
• Generates a matrix to solve the forward
  simulation problem
• Articulated-Body Algorithm
• Also gives matrix for forward simulation

12
Recursive Newton-Euler Algorithm (RNEA)

• Consider link i in a chain. Link i-1 is the
  parent of link i.
• vi is the velocity of link i. vji is the
  velocity across joint i
• vji vi vi-1
• Can also be derived as
• vji hiqi
• hi is 6 x d matrix, the motion freedom subspace
  of joint i. for our one-dof joints, this is a 6
  1 vector representing the joints axis of motion
• qi is a d x 1 vector of joint positions. In our
  case, a scalar. So, qi is the joint velocity,
  and qi is the joint acceleration

13
More RNEA

• Combining, we can say
• vi vi-1 hiqi
• For accelerations
• ai ai-1 hiqi hiqi
• (just the product rule derivative of the above)
• Can compute hi as vi hi
• As an algorithm, just compute from the base out
  to the tip iteratively

14
More More RNEA

• This gives us velocity and acceleration of the
  links given velocities and accelerations of the
  joint variables plus the velocity and
  acceleration of the base
• Thats not really inverse dynamics, is it?
• But, we can calculate the forces from the
  accelerations simultaneously by making a few
  additions

15
Calculating forces

• fi is the net force applied to link i through the
  joints
• fxi is the sum of external forces acting on that
  link
• Ii is the 6 x 6 spatial inertia matrix of link i
• fi fxi Iiai viIivi
• fji is the force applied to link i through joint
  i
• fi fji fji1
• fji Iiai viIivi fxi fji1

16
Almost done

• So we have the spatial joint force vector for
  link i, just need to convert it to joint force
  variables
• ti hTifji
• Lastly, lets do the whole thing in link
  coordinates. Need to transform by matrices
• xmi is the coordinate transform from link i-1 to
  link i for motion vectors, and xfi is the
  transform for force vectors

17
RNEA Pseudocode

• for (i 1 i lt N i )
• vi xmivi-1 hiqi
• ai xmiai-1 hiqi hiqi
• fji Iiai vi Iivi fxi
• for (i N i gt 0 i --)
• taui hTifji
• if (i-1 ! 0)
• fji-1 xf-1ifji

18
Last Words on RNEA

• Can work on trees, not just chains
• i-1 terms become parent of link i
• i1 terms become set of children of link i
• So long as links are ordered so that parent node
  lt child node, the iterative nature of algorithm
  produces correct answer

19
Forward Dynamics

• Total forces can include positional, velocity and
  acceleration-based forces
• Positional and Velocity-based forces are constant
  w.r.t. joint accelerations
• Given small time steps, joint accelerations have
  linear relationship to joint forces
• Mq F q M-1F
• M is the System Mass Matrix

20
Mass Matrix

• Mq C(q,q) t
• t is vector of forces acting on the articulated
  body
• C is a vector of those components of tau not
  dependent on acceleration
• Notes t is different from RNEA t, which was
  joint forces (size d) this t is spatial vector
  of size 6n
• Also This is forward dynamics, so t is known
  and q is what we want to find

21
More Mass Matrix

• Using inverse solver (like RNEA) we can find Mq
  as
• Mq D(q,q,q) D(q,q,0)
• q terms cancel, leaving
• Mq D(q,0,q) D(q,0,0)

22
More More Mass Matrix

• Artificially construct q vectors di for each
  d.o.f. of the articulated body such that the ith
  element of di 1, di 0 elsewhere
• So, Mdi is the force that causes a unit
  acceleration along that d.o.f.
• Mdi is the ith column of M
• O(N2) yuck

23
Composite-Rigid-Body Algorithm (CRBA)

• If we give the body an acceleration of di, then
  the subtree acts as a single rigid body with
  acceleration of hi, the rest of the tree is
  unmoving
• Call the moving subtree Ci, has inertia matrix
  ICi
• ICi is just the sum of the individual inertia
  matrices
• fCi is the force required to impart acceleration
  of hi to Ci

24
More CRBA

• For every element Mji where j lt i and j is on the
  chain from the root to i
• Mji hjTfCi
• For every Mji where j lt i but not an ancestor of
  I, set to 0
• For every Mji where j gt i, Mij because M is
  symmetric (in fact, M is positive definite)

25
CRBA Pseudocode

• M 0
• for (i 1 i lt N i )
• ICi Ii
• for (i N i gt 0 i --)
• fCii ICihi
• for (j i j lt N j ) Mij
  hTifCji
• if (i - 1 ! 0)
• ICi - 1 xf-1iICixmi
• for (j i j lt N j )
• fCji-1 xf-1ifCji

26
Last Words on CRBA

• Isnt this O(N3) ?
• Yes, but for small N, small order terms dominate
• Like RNEA, can change pseudocode to work on
  trees, but better off to go to original
  Featherstone / Orin paper, which has general form

27
Articulated Body Algorithm (ABA)

• If we apply a test force of f to one link in an
  articulated body, we get
• a Ff b
• a is the bodys new acceleration
• b is the acceleration if f were 0
• F is a 6 6 matrix that transforms forces into
  motions (sound familiar?)

28
More ABA

• Invert equations to get
• f IAa pA
• IA is F-1 Articulated-Body Inertia (ABI) of the
  link that receives the test force
• pA is IAb Bias Force
• Need to compute the ABI and bias for each link
  per frame, then compute accelerations
• Can be done for each link independent of other
  links in the body, so this is overall O(N)

29
ABA high-level

• Starting at the tip, compute for each link
• pi vi Iivi fxi
• paj pAjIAjhjqj
• IAi Ii ?(j in children of i)(IAj
  IAjhj(hjTIAjhj)-1hjTIAj)
• pAi pi ?(j in children of i)(pajIAjhj(hjTIAjh
  j)-1(ti-hjTpaj))
• Then, compute q for each link
• qI (hiTIAihi)-1(ti hiT(IAiai-1 pai))

30
Kinda More ABA

• Featherstone does not give any pseudocode for
  this in his 2000 paper, and his earlier papers
  are difficult to find (his 1987 book is checked
  out of the library right now)
• However, just last week at GDC, Kokkevis of Sony
  presented a paper that improves on the ABA and
  includes a full pseudocode and extensions for
  arbitrary constraints and joint limits

31
Conclusions

• RNEA is still standard for inverse dynamics, good
  O(N) runtime
• CRBA is fairly easy to understand, but only good
  for smaller chains
• ABA is more efficient than CRBA for N gt 9, but
  improvements to ABA make it better than CRBA for
  N gt 6
• Research still going on as we speak!

32
References

• Featherstone, R. and Orin, D. Robot Dynamics
  Equations and Algorithms 2000
• Craig, John J. Introduction to Robotics
  Mechanics and Control 1986
• Kokkevis, Evangelos. Practical Physics for
  Articulated Characters 2004