Order(N) Multibody DynamicsPart 1: Spatial Notation (draft!)

Michael Sherman (Sherm)Revised: 7 Aug 2014

O(N) Topics• O(n) intro

– How not to form matrices (think operators)– Featherstone vs. Jain

• Look at algorithms– Simple, but some mystery symbols

• Notation and basic abstractions– Spatial notation, mobilizers, work in Ground …

• Demystify symbols• Look at Simbody implementation• Write code!• (NOT YET: constraints, prescribed motion)

Structure of O(N) algorithm

Abhi Jain

Articulated body inertia P

Jain, Alg. 6.1, pg 106

Forward dynamics

Jain, Alg. 16.2, pg 126

6

Notation Motivation• Good abstractions good code• You can’t reason about spatial algorithms if

you treat translation & rotation differently• Algorithms should look line-by-line just like the

ones in the books• Debugging much easier, API smaller• We need some common notation to

communicate clearly

7

Spatial Vector

x

y

z

x

y

z

vvv

Velocity

x

y

z

x

y

z

aaa

x

y

z

x

y

z

fff

Acceleration Force

Vec<2, Vec3>

V[0] rotational

V[1] translational

NOT Plücker vectors! Abhi Jain, not Roy Featherstone

8

Spatial Inertia Matrix

mcx

–mcx

0

0

mm

m

(sym)

xx xy xz

yy yz

zz

I I II I

I

Mat<2,2, Mat33>

Equivalent to MassProperties object

9

Beautiful to work withusing namespace SimTK;SpatialMat M; // spatial inertiaSpatialVec V; // spatial velocity

…SpatialVec P = M*V; // spatial momentum

// angular momentum is P[0], linear momentum is P[1]

Real ke= ~V*M*V/2; // kinetic energy

=ke

~V M V

/2

ωω

v

v

m

mcx

−mcx

I

= ½ (ω•(Iω+mcxv)+ v•(mv−mcxω))

10

Notation that works• A physical quantity

– Has a type, e.g. angular momentum– Is usually associated (“fixed to”) a body– Is measured with respect to something– Is expressed in some coordinate frame– Has units

• Many programming errors here• Good notation can almost eliminate

these problems

11

Basic notation

QiB

Type of quantity Which

instance (optional)

Fixed to which body?

• Implies several defaults– Measured in B’s frame, from B’s origin– Expressed in the B frame– Details depend on type of Q

• Code equivalent: Qi_B, v_B

BvA vector attached to body B, expressed

in B

12

Add explicit measured-in (“from”) frame

QMiB

Fixed to which body?

• New defaults– Measured in M’s frame, from M’s origin

– Expressed in the M frame

• Code equivalent: Qi_MB, w_GB

Measured with respect to which

body? G BAngular velocity of body B with respect to Ground,

expressed in GroundQFromiTo

13

Change expressed-in frame

QMiB

F

Fixed to which body?

• New defaults– Still measured in M’s frame, from M’s origin

– But expressed in the F frame

• Code equivalent: Qi_MB_F, w_PB_G

Measured with respect to which

body? PG

BAngular velocity of body

B with respect to its parent P, but expressed in

Ground

Expressed-in frame

14

Rotation matrix

~[ ] ~

~

B G

G B G B G B G B B G

B GR

xx y z y

z

(~ )

P B P F F M M B

P F F M B M

R R R R

R R R

q,u

M

F

MO

FO

mobilized body B( )F MR q

parent body P

B

P

15

For translations, specify points if not origin

BAP QpTo what point on

what body?

• New defaults– “From” frame implied by left superscript

– I.e., A frame above left, G frame above right

• Code equivalent: p_PaQb; p_GCb, p_GoCb

Measured from what point on what

body? BG CpVector from Ground

origin (implied) to body B’s mass center,

expressed in Ground

Point name conventions:O origin (implied)C mass center

16

Transforms

(0 0 0 1)

BG B G OG B R pX

Transform X_GB;Rotation& R_GB=X_GB.R();Vec3& T_GB=X_GB.p();

17

Transforms (cont.)

Transform X_PB=…;Vec3 p_BCb=…; // from B origin, in B frame impliedVec3 p_PoCb = X_PB*p_BCb; // implies P frame

B BBC CPP BX pp

q,u

M

F

MO

FO

parent body P

P

( )F MX q

( , )F MV q u

( , , )F MA q u u

Bmobilized body B

CB

18

Notational reality• Goal is correctness and clarity• Develop recognizable “cliches”• Use comments for anything unusual• Not perfectly unambiguous – this is for

humans– Ascii text is not very expressive

• Many variants possible, but we should try to be consistent

– Improvements solicited

19

A real example (as is)From MatterSubsystem.h

/// Return the angular and linear velocity of body B's frame in body A's frame,/// expressed in body A, and arranged as a SpatialVec.SpatialVec calcBodySpatialVelocityInBody(const State& s, BodyId objectBodyB, BodyId inBodyA) const{ const SpatialVec& V_GB = getBodyVelocity(s,objectBodyB); if (inBodyA == GroundId) return V_GB;

// Body A is not Ground so we'll have to compute relative velocity.

const SpatialVec& V_GA = getBodyVelocity(s,inBodyA); const Vec3 w_AB_G = V_GB[0]-V_GA[0]; // angular velocity of B in A, exp in G

// Angular velocity was easy, but linear velocity needs an wXr term. const Transform& X_GB = getBodyTransform(s,objectBodyB); const Transform& X_GA = getBodyTransform(s,inBodyA); const Vec3 p_OA_OB_G = X_GB.p() - X_GA.p(); // vector from OA to OB, exp in G

// linear velocity of OB in A, exp in G const Vec3 v_AB_G = (V_GB[1]-V_GA[1]) + w_AB_G % p_OA_OB_G;

// We're done, but the answer is expressed in Ground. Reexpress in A and return. const Rotation& R_GA = X_GA.R(); return SpatialVec(~R_GA*w_AB_G, ~R_GA*v_AB_G);}

20

Acknowledgments• Mathematical notation: Tom Kane, via

Dan Rosenthal and Paul Mitiguy • Guillermo Rodriguez & Abhi Jain (JPL)

– Spatial notation

• Charles Schwieters (NIH)– Templatized implementation in IVM

• NIH Roadmap grantU54 GM072970

21

Mobilizer kinematics

q,u

M

F

MO

FO

mobilized body B( )F MX q

parent body P

( ) O OF MF M F MRX q p

( , ) ( )F M F MV q u q u H

F M F M F M F MA V u u H H

( )q q u N

( , )F MV q u

( , , )F MA q u u define q

define u

relate q,u

Seth, et al. Nonlinear Dynamics 62, 2010

22

Discussion