1
Spa
tial V
ecto
r W
ork
shop
Spatial Vector Alge bra
The Easy Way to do Rigid Body Dynamics
Roy FeatherstoneDept. Information Engineering, RSISE
The Australian National University
© 2004 Roy Feathers tone
A Workshop on
2
Sp
atia
l Vec
tor
Wo
rksh
op
Spatial vector algebra is a concise vectornotation for describing rigid−body velocity,acceleration, inertia, etc., using 6D vectorsand tensors.
fewer quantities
fewer equations
less effort
fewer mistakes
3
Sp
atia
l Vec
tor
Wo
rksh
op
Mathematical Structure
spatial vectors inhabit two vector spaces:
M6
F6
−− motion vectors
−− force vectors
with a scalar product defined between them
‘‘.’’ : M6 F6 R
m . f = work
4
Sp
atia
l Vec
tor
Wo
rksh
op
Bases
Likewise, a coordinate vector f=[f1, ..., f6]T
represents a force vector f in a basis{e1, ...,e6} on F6 if
A coordinate vector m=[m1, ...,m6]T
represents a motion vector m in a basis{d1, ...,d6} on M6 if
f = Σi=1
6fi ei
m = Σi=1
6mi di
5
Sp
atia
l Vec
tor
Wo
rksh
op
Bases
If {d1, ..., d6} is an arbitrary basis on M6
then there exists a unique reciprocal basis{e1, ..., e6} on F6 satisfying
di . ej = { 0 : i = j1 : i = j
With these bases, the scalar product of twocoordinate vectors is
m . f = mT f
6
Sp
atia
l Vec
tor
Wo
rksh
op
vP
ω
P
Velocity
The velocity of a rigid bodycan be described by
choosing a point, P, in the body
specifying the linear velocity, vP, of thatpoint
specifying the angular velocity, ω, of thebody as a whole
7
Sp
atia
l Vec
tor
Wo
rksh
op
Velocity vP
ω
P
The body is then deemed to be
rotating with an angular velocity ω aboutan axis passing through P
while simultaneously
translating with a linear velocity vP
8
Sp
atia
l Vec
tor
Wo
rksh
op
Velocity
O
vO
OP
vP
ω
P
velocity
is equivalent to
where
(ω,vP) at P
(ω,vO) at O
vO = vP + OP ω
9
Sp
atia
l Vec
tor
Wo
rksh
op
Velocity
O
vO
OP
vP
ω
P
Spatial velocity:ωvO
=vO =^
These are the Plucker coordinatesof v in the frame Oxyz^
..
ωx
ωy
ωz
vOx
vOy
vOz
10
Sp
atia
l Vec
tor
Wo
rksh
op
Force
Pf
τP
A general force acting on arigid body is equivalent to thesum of
a force f acting along a line passingthrough a point P, and
a couple, τP
11
Sp
atia
l Vec
tor
Wo
rksh
op
P
Force
OP
fτP
general force
is equivalent to
where
(f,τP) at P
(f,τO) at O
τO = τP + OP f
τO
O
f
12
Sp
atia
l Vec
tor
Wo
rksh
op
P
Force
OP
fτP
τO
Spatial force: =O
f
fO =^ τO
f
These are the Plucker coordinatesof f in the frame Oxyz^
..
τOx
τOy
τOz
fx
fy
fz
13
Sp
atia
l Vec
tor
Wo
rksh
op
Plucker Coordinates..
A Cartesian coordinate frame Oxyz definestwelve basis vectors:
rotations about the Ox, Oy and Oz axes,translations in the x, y and z directions
couples in the yz, zx and xy planes, andforces along the Ox, Oy and Oz axes
dOx, dOy, dOz, dx, dy, dz :
ex, ey, ez, eOx, eOy, eOz :
14
Sp
atia
l Vec
tor
Wo
rksh
op
If andvO =ωvO
^ fO =^f
τO
f = τOx ex + τOy ey + τOz ez + + fx eOx + fy eOy + fz eOz
^
and
are the Plucker..
coordinates of v and f in Oxyz, then^ ^
v . f = vOT fO
^ ^ ^ ^
v = ωx dOx + ωy dOy + ωz dOz + + vOx dx + vOy dy + vOz dz
^
15
Sp
atia
l Vec
tor
Wo
rksh
op
Coordinate Transforms
transform from A to Bfor motion vectors:
corresponding transformfor force vectors:
0rz
−ry
−rz0rx
ry−rx0
r =~BXA = E0
0E where
BXAF = (BXA)−T
A
Br
E
1rT
01~
16
Sp
atia
l Vec
tor
Wo
rksh
op
Basic Operations with Spatial Vectors
If vA = velocity of body AvB = velocity of body BvBA = relative velocity of B w.r.t. A
Then vB = vA + vBA
Composition of velocities
Scalar multiplesIf s and q are a joint motion axis andvelocity variable, then the joint velocityis vJ = s q
.
.
17
Sp
atia
l Vec
tor
Wo
rksh
op
If forces f1 and f2 both act on the samebody then their resultant is
Composition of forces
ftot = f1 + f2
Action and reaction
If body A exerts a force f on body B,then body B exerts a force −f on body A(Newton’s 3rd law)
18
Sp
atia
l Vec
tor
Wo
rksh
op
Spatial Cross Products
There are two cross product operations:one for motion vectors and one for forces
ωvO
mmO
ω mω mO + vO mvO mO =^ ^ =
where vO and mO are motion vectors, andfO is a force.
^ ^^
ω τO + vO fω f
ωvO
τO
f=vO fO =^ ^
19
Sp
atia
l Vec
tor
Wo
rksh
op
Differentiation
The derivative of a spatial vector is itselfa spatial vector
The derivative of a spatial vector that isfixed in a moving body with velocity v is
in general, s = lim s(t+δt)−s(t)δt
ddt δt 0
ddt
s = v s
20
Sp
atia
l Vec
tor
Wo
rksh
op
Differentiation in Moving Coordinates
= sO + vO sOddt
ddt
sO
velocity of coordinateframe
componentwisederivative of coordinatevector
coordinate vectorrepresenting ds/dt
21
Sp
atia
l Vec
tor
Wo
rksh
op
Acceleration
v =^
. . . is the rate of change of velocity:
a =^
but this is not the linear acceleration ofany point in the body!
vO.ω.
ddt
22
Sp
atia
l Vec
tor
Wo
rksh
op
Classical acceleration is
Spatial acceleration is
vO.ω.
where v’O is the derivative of vO
taking O to be fixed in the body
.
v’O.ω.
What’s the difference?
Spatial acceleration is a vector.
where vO is the derivative of vO
taking O to be fixed in space
.
23
Sp
atia
l Vec
tor
Wo
rksh
op
Acceleration Example
vO
ω
A body rotates with constant angular velocityω, so its spatial velocity is constant, so itsspatial acceleration is zero; but almost everybody−fixed point is accelerating.
fixed axis
24
Sp
atia
l Vec
tor
Wo
rksh
op
If aA = acceleration of body AaB = acceleration of body B
aBA = acceleration of B w.r.t. A
Then aB = aA + aBA
Composition
Look, no Coriolis term!
Basic Operations with Accelerations
25
Sp
atia
l Vec
tor
Wo
rksh
op
O
C
Rigid Body Inertiamass:
CoM:inertiaat CoM:
m
C
IC
spatial inertia tensor:
where
c
IO = IC − m c c~~
IO =^ IO m cm cT m 1
~~
26
Sp
atia
l Vec
tor
Wo
rksh
op
Composition
If two bodies with inertias IA and IB arejoined together then the inertia of thecomposite body is
Itot = IA + IB
Basic Operations with Inertias
Coordinate transformation formula
IB = BXAF IA AXB = (AXB)T IA AXB
27
Sp
atia
l Vec
tor
Wo
rksh
op
Equation of Motion
f = (I v) = I a + v I v
f = net force acting on a rigid body
I = inertia of rigid body
v = velocity of rigid body
I v = momentum of rigid body
a = acceleration of rigid body
ddt
28
Sp
atia
l Vec
tor
Wo
rksh
op
Constrained Motion
A force, f, is applied to abody with inertia I that isconstrained to a subspaceS = Range(S) of M6. Assuming the bodyis initially at rest, what is its acceleration?
velocity
acceleration
constraint force
v = S α = 0
ST fc = 0
fa
fc
I
a = S α + S α = S α. . .
29
Sp
atia
l Vec
tor
Wo
rksh
op
fa
fc
Iequation of motion
solution
a = S (ST I S)−1 ST f
f + fc = I a + v I v= I a
I S α = f + fc.
ST I S α = ST f.
α = (ST I S)−1 ST f. apparentinverse inertia
30
Sp
atia
l Vec
tor
Wo
rksh
op
Inverse Dynamics
base
joint 1
link 1
joint 2 joint n
link n
vi, ai
qi, qi, si
link velocity and acceleration
joint velocity, acceleration & axis. ..
fi force transmitted from link i−1 to i
τi joint force variable
Ii link inertia
31
Sp
atia
l Vec
tor
Wo
rksh
op
vi = vi−1 + si qi.
acceleration is the derivative of velocity
velocity of link i is the velocity of link i−1plus the velocity across joint i
ai = ai−1 + si qi + si qi. ...
equation of motion
fi − fi+1 = Ii ai + vi Ii vi
τi = siT fi
active joint force
32
Sp
atia
l Vec
tor
Wo
rksh
op
The Recursive Newton−Euler Algorithm
(Calculate the joint torques τi that willproduce the desired joint accelerations qi.)
vi = vi−1 + si qi
ai = ai−1 + si qi + si qi. ...
. (v0 = 0)
(a0 = 0)
fi = fi+1 + Ii ai + vi Ii vi (fn+1 = fee)
..
τi = siT fi