bra A Short Course on Spatial Vector Algebra The Easy Way to do Rigid Body Dynamics Spatial Vector Alge 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 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 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 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 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 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 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 ω 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 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 a couple, nP 10 bra Force nP f P nO f OP O general force (f,nP) at P Spatial Vector Alge 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 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 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 + 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 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 . 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 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 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 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 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 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 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 What's the difference? Spatial acceleration is a vector. 23 bra Acceleration Example ω fixed axis vO Spatial Vector Alge 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 Look, no Coriolis term! 25 bra Now try question set B Spatial Vector Alge 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 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 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 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 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 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 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 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 . 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 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 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 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 .