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