bra A Short Course on Spatial Vector Algebra

The Easy Way to do Rigid Body Dynamics

Spatial Vector Alge Vector Spatial Roy Featherstone Dept. Information Engineering, RSISE The Australian National University

1 © 2005 Roy Featherstone bra Spatial vector algebra is a concise vector notation for describing rigid−body velocity, acceleration, inertia, etc., using 6D vectors and tensors. fewer quantities fewer equations

Spatial Vector Alge Vector Spatial less effort fewer mistakes

2 bra Mathematical Structure

spatial vectors inhabit two vector spaces:

M6 −− motion vectors F6 −− force vectors

with a scalar product defined between them

Spatial Vector Alge Vector Spatial m . f = work ‘‘.’’ : M6 F6 R

3 bra Bases

T A coordinate vector m=[m1,...,m6] represents a motion vector m in a basis 6 {d1,...,d6} on M if 6 m = Σ mi di i=1

T Likewise, a coordinate vector f=[f1,...,f6]

Spatial Vector Alge Vector Spatial represents a force vector f in a basis 6 {e1,...,e6} on F if 6 f = Σ fi ei i=1 4 bra Bases

6 If {d1, ..., d6} is an arbitrary basis on M then there exists a unique reciprocal basis 6 {e1, ..., e6} on F satisfying 0 : i = j di . ej = { 1 : i = j

Spatial Vector Alge Vector Spatial With these bases, the scalar product of two coordinate vectors is

m . f = mT f 5 ω bra Velocity vP

The velocity of a rigid body P can be described by

choosing a point, P, in the body

specifying the linear velocity, vP, of that

point Spatial Vector Alge Vector Spatial specifying the angular velocity, ω, of the body as a whole

6 ω bra Velocity vP P

The body is then deemed to be

translating with a linear velocity vP

while simultaneously Spatial Vector Alge Vector Spatial rotating with an angular velocity ω about an axis passing through P

7 ω bra Velocity vP P

vO

OP

O ω

velocity ( ,vP) at P Spatial Vector Alge Vector Spatial ω is equivalent to ( ,vO) at O ω where vO = vP + OP 8 ω bra Velocity vP P

vO ω OP x ωy ω ω O ^ z Spatial velocity: vO = =

vO vOx Spatial Vector Alge Vector Spatial vOy .. These are the Plucker coordinates vOz of v^ in the frame Oxyz 9 bra Force nP f P A general force acting on a rigid body is equivalent to the sum of

a force f acting along a line passing

through a point P, and Spatial Vector Alge Vector Spatial a couple, nP

10 bra Force nP f P

nO f OP O

general force (f,nP) at P Spatial Vector Alge Vector Spatial

is equivalent to (f,nO) at O

where nO = nP + OP f 11 bra Force nP f P

nO f OP nOx nOy O ^ nO nOz Spatial force: fO = =

f fx Spatial Vector Alge Vector Spatial fy .. These are the Plucker coordinates fz ^ of f in the frame Oxyz 12 bra .. Plucker Coordinates

A Cartesian coordinate frame Oxyz defines twelve basis vectors:

dOx, dOy, dOz, dx, dy, dz : rotations about the Ox, Oy and Oz axes, translations in the x, y and z directions

Spatial Vector Alge Vector Spatial ex, ey, ez, eOx, eOy, eOz : couples in the yz, zx and xy planes, and forces along the Ox, Oy and Oz axes 13 bra ω ^ ^ nO .. If vO = and fO = are the Plucker vO f ^ coordinates of v^ and f in Oxyz, then

^ ω ω ω v = x dOx + y dOy + z dOz + + vOx dx + vOy dy + vOz dz ^ f = nOx ex + nOy ey + nOz ez + Spatial Vector Alge Vector Spatial + fx eOx + fy eOy + fz eOz and ^ ^ ^ T ^ v . f = vO fO 14 bra Coordinate Transforms B r A transform from A to B E for motion vectors:

0 −rz ry B E 0 1 0 ~ X = where r = rz 0 −rx A 0 E ~rT 1

−ry rx 0 Spatial Vector Alge Vector Spatial

corresponding transform B F B −T for force vectors: XA = ( XA) 15 bra Basic Operations with Spatial Vectors Composition of velocities

If vA = velocity of body A vB = velocity of body B vBA = relative velocity of B w.r.t. A

Then vB = vA + vBA Scalar multiples Spatial Vector Alge Vector Spatial . If s and q are a joint motion axis and velocity variable, then the joint velocity . is vJ = s q 16 bra Composition of forces

If forces f1 and f2 both act on the same body then their resultant is

ftot = f1 + f2 Action and reaction

Spatial Vector Alge Vector Spatial If body A exerts a force f on body B, then body B exerts a force −f on body A (Newton's 3rd law)

17 bra

Now try question set A Spatial Vector Alge Vector Spatial

18 bra Spatial Cross Products There are two cross product operations: one for motion vectors and one for forces ω ω ^ ^ m m vO mO = = ω vO mO mO + vO m ω ω ^ ^ nO nO + vO f vO fO = = ω Spatial Vector Alge Vector Spatial vO f f

^ ^ where vO and mO are motion vectors, and ^ fO is a force. 19 bra Differentiation

The derivative of a spatial vector is itself a spatial vector d s(t+δt)−s(t) in general, s = lim dt δt 0 δt

The derivative of a spatial vector that is Spatial Vector Alge Vector Spatial fixed in a moving body with velocity v is d s = v s dt 20 bra Differentiation in Moving Coordinates

d d s = sO + vO sO dt O dt velocity of coordinate frame componentwise derivative of coordinate Spatial Vector Alge Vector Spatial vector coordinate vector representing ds/dt 21 bra Acceleration

. . . is the rate of change of velocity:

ω. ^ d ^ a = v = . dt vO

but this is not the linear acceleration of Spatial Vector Alge Vector Spatial any point in the body!

22 bra Classical acceleration is ω. . where v'O is the derivative of vO . v'O taking O to be fixed in the body

Spatial acceleration is ω. . where vO is the derivative of vO . taking to be fixed in space

vO O Spatial Vector Alge Vector Spatial What's the difference? Spatial acceleration is a vector. 23 bra Acceleration Example

ω

fixed axis vO

Spatial Vector Alge Vector Spatial A body rotates with constant angular velocity ω, so its spatial velocity is constant, so its spatial acceleration is zero; but almost every body−fixed point is accelerating. 24 bra Basic Operations with Accelerations

Composition

If aA = acceleration of body A

aB = acceleration of body B

aBA = acceleration of B w.r.t. A

Then aB = aA + aBA Spatial Vector Alge Vector Spatial Look, no Coriolis term!

25 bra

Now try question set B Spatial Vector Alge Vector Spatial

26 bra Rigid Body Inertia mass: m CoM: C C c inertia O at CoM: IC

~ ^ IO m c spatial inertia tensor: IO = Spatial Vector Alge Vector Spatial m ~cT m 1 ~~ where IO = IC − m c c

27 bra Basic Operations with Inertias

Composition

If two bodies with inertias IA and IB are joined together then the inertia of the composite body is

Itot = IA + IB

Spatial Vector Alge Vector Spatial Coordinate transformation formula

B F A A T A IB = XA IA XB = ( XB) IA XB 28 bra Equation of Motion

d f = (I v) = I a + v I v dt

f = net force acting on a rigid body I = inertia of rigid body

v = velocity of rigid body Spatial Vector Alge Vector Spatial I v = momentum of rigid body a = acceleration of rigid body 29 bra Motion Constraints

If a rigid body's motion is constrained, then its velocity is an element of a subspace, S M6, called the motion subspace

degree of (motion) freedom: dim(S)

degree of constraint: 6 − dim(S) Spatial Vector Alge Vector Spatial

S can vary with time

30 bra Matrix Representation

Let S be any 6 dim(S) matrix such that range(S) = S, then

the columns of S define a basis on S

any vector v S can be expressed in the form v = S α, where α is a dim(S) 1 Spatial Vector Alge Vector Spatial vector of coordinates

31 bra Constraint Forces motion constraints are enforced by constraint forces

constraint forces do no work: if fc is the constraint force, and S the motion subspace matrix, then

T

S fc = 0 Spatial Vector Alge Vector Spatial constraint forces occupy a subspace T F6 satisfying dim(S) + dim(T) = 6 and ST T = 0 32 bra Constrained Motion a f I A force, f, is applied to a fc body with inertia I that is constrained to a subspace S = Range(S) of M6. Assuming the body is initially at rest, what is its acceleration?

α Spatial Vector Alge Vector Spatial velocity v = S = 0 . . . acceleration a = S α + S α = S α T constraint force S fc = 0 33 bra a equation of motion f I

f + fc = I a + v I v fc = I a solution . f + fc = I S α .

ST f = ST I S α Spatial Vector Alge Vector Spatial . T −1 T apparent α = ( ) S I S S f inverse inertia a = S (ST I S)−1 ST f 34 bra

Now try question set C Spatial Vector Alge Vector Spatial

35 bra Inverse Dynamics

joint 1 joint 2 joint n

base link 1 link n

. .. qi, qi, si joint velocity, acceleration & axis

vi, ai link velocity and acceleration Spatial Vector Alge Vector Spatial fi force transmitted from link i−1 to i

τi joint force variable

Ii link inertia 36 bra velocity of link i is the velocity of link i−1 plus the velocity across joint i . vi = vi−1 + si qi acceleration is the derivative of velocity . . .. ai = ai−1 + si qi + si qi equation of motion

Spatial Vector Alge Vector Spatial fi − fi+1 = Ii ai + vi Ii vi active joint force τ T i = si fi 37 bra The Recursive Newton−Euler Algorithm

(Calculate the joint torques τi that will .. produce the desired joint accelerations qi.)

. vi = vi−1 + si qi (v0 = 0) . . .. ai = ai−1 + si qi + si qi (a0 = 0)

fi = fi+1 + Ii ai + vi Ii vi (fn+1 = fee) Spatial Vector Alge Vector Spatial τ T i = si fi

38