Transcript Document

Articulated Body Dynamics
The Basics
Comp 768
October 23, 2007
Will Moss
Overview
• Motivation
• Background / Notation
• Articulate Dynamics Algorithms
–
–
–
–
Newton-Euler Algorithm
Composite-Rigid Body Algorithm
Articulated-Body Algorithm (Featherstone)
Lagrange Multiplier approach (Baraff)
October, 23 2007
2
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
October, 23 2007
3
Applications
• Computer Graphics
– Humans, animals, birds,
robots, etc.
– Wires, chains, ropes, etc.
– Trees, grass, etc.
– Many more
•
October, 23 2007
http://vrlab.epfl.ch/~alegarcia/VHOntology/long.html
4
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.
October, 23 2007
5
Notation
• In rigid body dynamics we had two equations
F(t)  Ma(t)
τ(t)  I(t)ω(t)
F (t )  I (t )a (t )
s
s
s
– 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
October, 23 2007
6
Notation
• Transitioning this to articulated bodies
n
n
n
j 1
j 1 k 1
Qi   H ij qj   Cijk q j qk  gi
– Qi is the force on link i
– H is the joint-space inertia matrix (n x n)
– q, q and q
 are the coordinates, velocities and accelerations
of the joints
– C term produces the vector of forces that produce zero
acceleration
October, 23 2007
7
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
n
n
n
j 1
j 1 k 1
Qi   H ij q j   Cijk q j q k  g i
October, 23 2007
8
Algorithms
• Inverse Dynamics
– Newton-Euler Algorithm
• Forward Dynamics
– Composite-Rigid-Body Algorithm
– Articulated-Body Algorithm
– Lagrange Multiplier Algorithm
October, 23 2007
9
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
October, 23 2007
10
Newton-Euler Algorithm
• Method
1.Calculate the velocities and accelerations at each
link
2.Calculate the required net force acting on each
link to generate those accelerations
3.Calculate the joint forces required to generate the
net forces on each link
October, 23 2007
11
Newton-Euler Algorithm
1. Find the velocities and accelerations of

the
links
where s is a unit vecto r describing the
 

vi  vi - 1  si qi
(v0  0)
 
 

ai  ai-1  vi  si qi  si qi (a0  0)
October, 23 2007
i
allowed motion of joint i,
q, q and q are the physical variables for the
joint and


v and a are the velocity and accelerati on of
the link
12
Newton-Euler Algorithm
2. Find the forces on each link
 
  
d 
fi 
I i vi  I i ai  vi  I i vi
dt
l
October, 23 2007
13
Newton-Euler Algorithm
3. Find the forces on the joints
j
j
l
fi  fi1  fi
fi  fi1  fi where f n  f n
j
j
l
j
l
• This can be reformulated in link
coordinates to speed up the calculation
• Runs in O(n)
October, 23 2007
14
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
October, 23 2007
15
Composite-Rigid-Body Algorithm
n
n
n
j 1
j 1 k 1
Qi   H ij qj   Cijk q j qk  gi
  C(q,q )
Q  H(q)q
–
–
–
–
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
q, q and q
 are the coordinates, velocities and accelerati ons
• Algorithm
– Calculate the elements of C
– Calculate the elements of H
– Solve the set of simultaneous equations
October, 23 2007
16
Composite-Rigid-Body Algorithm
• Solve for C
)
– Setting the acceleration to zero, we get Q  C (q,q
– We can, therefore, interpret C as the forces which
produce no acceleration
– We can use a forward-dynamics solver (like NewtonEuler) to solve for the forces given the position, velocity
and an acceleration of zero
October, 23 2007
17
Composite-Rigid-Body Algorithm
• Solve for H
– If we set C to 0, we observe that H (q)q is the vector of
joint forces that will impart an acceleration of q 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
n
by C
C

 

I i   I j I i 1  I i
j i
– Treat the links from 1…i-1 are therefore unmoving
October, 23 2007
18
Composite-Rigid-Body Algorithm
• Solve for H (cont.)
– Given that
 C
f i  I i si

where f i is a unit force at joint i and

si is a unit accelerati on
– Since none of the
 links from 1 … i-1 are moving, every
joint transmits f i onto the subsequent link, so we can
solve for H by solving
S
H ji  s j f i for j  i


Where s S is the spacial transpose of s
– Which is a complete solution for H since it is symmetric
– Runs in O(n2)
October, 23 2007
19
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
October, 23 2007
20
Articulated-Body Algorithm
• (Re)consider the equation of motion of
an articulated
body

 
f I a p
A

where f is a force applied to a link,
A
I is the articulate body intertia and

p is the bias force, or the force required to keep the link from accelerati ng
• This is true for any link in the
articulated body
October, 23 2007
21
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)
October, 23 2007
22
Articulated-Body Algorithm
• Solving the one-joint robot problem
S  A   

Q  s I ab  vb  s q   p 
q 
 S  A  I  articulated body inertia of the
s I s
A

p, Q, q , q
rest of the robot

p  bias force

s  is the unit vecto r describing the
allowed motion of the joint
S

s  spatial transpose of s

v b  velocity of the base

a b  accelerati on of the base
Q  force at the joint
q  velocity at the joint
October, 23 2007
q  accelerati on at the joint
23
Articulated-Body Algorithm
• Finding the articulated-body inertia (IA) and bias
force (p)
  
 A   I2ss S I2
I  I1  I 2   S  
s I1 s

  v

S
  v  v      Q  s I 2v1  v2  p2
p  p1  p2  I 2  v1  v2  s
S  
s I2s


v

Q, p
October, 23 2007
Where pi is called the
velocity-product force
and is defined
   to be
v
pi  vi  I i vi
24



Articulated-Body Algorithm
• These formulas
so allow us
A
 recursively,
can be reformulated

to find I i and pi in terms of only I i 1 A and pi 1
• 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)
October, 23 2007
25
Lagrange-Multiplier Method
• The preceding methods are reducedcoordinate 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
October, 23 2007
26
Lagrange-Multiplier Method
• Consider the equation of motion of i bodies
Mi v i  Fi
– 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
ji1v 1   jik v k   jin v n  ci  0
– 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
October, 23 2007
27
Lagrange-Multiplier Method
• To simplify the notation, we replace the q
individual constraint equations
j11 v 1    j1k v k    j1n v n  c1  0
j21 v 1    j2k v k    j2n v n  c 2  0

jq1 v 1    jqk v k    jqn v n  c q  0
• With
Jv  c  0
– 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
October, 23 2007
28
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:
 jT i1 


c
Fi    λ i  J T λ
 jT in 


– 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
October, 23 2007
29
Lagrange-Multiplier Method
• If we introduce an external force acting on the
system and combine the equations, we get
Mv  J T λ  Fext
• Solving for v and plugging into our constraint
equation, we get
JM 1 J T λ  JM 1F ext  c 
• 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
October, 23 2007
30
Lagrange-Multiplier Method
• For more complicated constraints, JM 1J T is no
longer sparse and we reformulate the equation as
0
 M  J T  y  

   
1 ext
 J
 λ

JM
F c
0

  





• If we required acyclic constraints, then sparsematrix theory tells us that  MJ  0J  has perfect
elimination order
T
M
– This means that if we factor   J
and can be computed in O(n)
 JT 

0 
into three matrices LDLT, L will be as sparse as H
y
– We can then solve LDLT    
0

 for λ, by solving each piece of LDLT
1 ext
 λ    JM F  c 
separately, each also in O(n) time and combining the solutions
October, 23 2007
31
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)
October, 23 2007
32
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
October, 23 2007
33