Upload
kennedy-lee
View
44
Download
1
Tags:
Embed Size (px)
DESCRIPTION
Comp 768 October 23, 2007 Will Moss. Articulated Body Dynamics The Basics. Overview. Motivation Background / Notation Articulate Dynamics Algorithms Newton-Euler Algorithm Composite-Rigid Body Algorithm Articulated-Body Algorithm (Featherstone) Lagrange Multiplier approach (Baraff) . - PowerPoint PPT Presentation
Citation preview
Articulated Body DynamicsThe Basics
Comp 768
October 23, 2007
Will Moss
October, 23 2007 2
Overview
•Motivation•Background / Notation•Articulate Dynamics Algorithms
– Newton-Euler Algorithm– Composite-Rigid Body Algorithm– Articulated-Body Algorithm (Featherstone)– Lagrange Multiplier approach (Baraff)
• 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
History
October, 23 2007 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
October, 23 2007 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.
October, 23 2007 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
)()()( tatItF sss I(t)ω(t)τ(t)
Ma(t)F(t)
October, 23 2007 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
ik
n
j
n
j
n
kjijkjiji gqqCqHQ
1 1 1
qqq and ,
October, 23 2007 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
ik
n
j
n
j
n
kjijkjiji gqqCqHQ
1 1 1
October, 23 2007 9
Algorithms
• Inverse Dynamics–Newton-Euler Algorithm
•Forward Dynamics–Composite-Rigid-Body Algorithm–Articulated-Body Algorithm– Lagrange Multiplier Algorithm
October, 23 2007 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
October, 23 2007 11
Newton-Euler Algorithm
•Method1.Calculate the velocities and accelerations at each
link2.Calculate the required net force acting on each
link to generate those accelerations3.Calculate the joint forces required to generate the
net forces on each link
October, 23 2007 12
Newton-Euler Algorithm
1. Find the velocities and accelerations of the links
link the
ofon accelerati and velocity theare and
andjoint
for the variablesphysical theare and ,
,joint ofmotion allowed
thedescribingr unit vecto a is where
av
qqq
i
si
)0( 01 vqsvv iii - i
)0( 01 aqsqsvaa iiiiii-i
October, 23 2007 13
Newton-Euler Algorithm
2. Find the forces on each link
iiiiiiil
i vIvaIvIdt
df
October, 23 2007 14
Newton-Euler Algorithm
3. Find the forces on the joints
•This can be reformulated in link coordinates to speed up the calculation
•Runs in O(n)
ljji1ii fff
ln
jn
ljj ff wherefff i1ii
October, 23 2007 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
October, 23 2007 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
ik
n
j
n
j
n
kjijkjiji gqqCqHQ
1 1 1
)qC(q,qH(q)Q
onsaccelerati and s velocities,coordinate theare and , qqq
October, 23 2007 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
)qC(q,Q
October, 23 2007 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
iC
i
n
ijj
Ci IIII
1
qqH )(q
October, 23 2007 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)
iC
ii sIf
ijfsH iS
jji for
if
sS of transposespacial theiss Where
onacceleratiunit a is
and joint at forceunit a is where
i
i
s
if
October, 23 2007 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
October, 23 2007 21
Articulated-Body Algorithm
• (Re)consider the equation of motion of an articulated body
•This is true for any link in the articulated body
paIf A
ngaccelerati fromlink thekeep torequired force or the force, bias theis
and intertiabody articulate theis
link, a toapplied force a is where
p
I
fA
October, 23 2007 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)
October, 23 2007 23
Articulated-Body Algorithm
• Solving the one-joint robot problem
sIs
pqsvaIsQq AS
bbAS
joint at theon acceleratiq
joint at thevelocity q
joint at the forceQ
base theofon acceleratia
base theofvelocity v
s of transposespatial s
joint theofmotion allowed
thedescribingr unit vecto theis s
force biasp
robot theofrest
theof inertiabody darticulateI
b
b
S
A
q ,q Q, ,p
October, 23 2007 24
Articulated-Body Algorithm
• Finding the articulated-body inertia (IA) and bias force (p)
sIs
pvvIsQsvvIppp S
vSvv
2
221221221
sIsIssI
III S
SA
1
2221
iiiv
i vIvp p Q,
Where is called the velocity-product force and is defined to be
vip
October, 23 2007 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)
AiI
ip A
iI 1
1ip
October, 23 2007 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
October, 23 2007 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
iii FvM
0cvjvjvj ininkik1i1
October, 23 2007 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
0cvjvjvj
0cvjvjvj
0cvjvjvj
nnkk11
nnkk11
nnkk11
qqqq
2222
1111
0cvJ
October, 23 2007 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
λJλ
j
j
F Ti
inT
i1T
ci
October, 23 2007 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
cFJMλJJM ext1T1
extT FλJvM
v
October, 23 2007 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
cFJM
0
λ
yLDL ext1
T
0J
JM T
T1JJM
cFJM
0
λ
y
0J
JMext1
T
0J
JM T
October, 23 2007 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)
October, 23 2007 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