Order(N) Multibody Dynamics
Part 1: Spatial Notation (draft!)
Michael Sherman (Sherm)
Revised: 7 Aug 2014
O(N) Topics
•
O(n) intro
–
–
•
Look at algorithms
–
•
Simple, but some mystery symbols
Notation and basic abstractions
–
•
•
•
•
How not to form matrices (think operators)
Featherstone vs. Jain
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
Notation Motivation
•
•
•
•
•
6
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
Spatial Vector
x
y
z
vx
vy
vz
7
Velocity
Vec<2, Vec3>
V[0] rotational
V[1] translational
NOT Plücker
vectors!
Abhi Jain, not Roy
Featherstone
x
y
z
x
y
z
ax
ay
az
fx
fy
fz
Acceleration
Force
Spatial Inertia Matrix
I xx I xy I xz
I yy I yz
(sym)
I zz
–mcx
Mat<2,2, Mat33>
mcx
m
0
m
0
8
Equivalent to
MassProperties
object
m
Beautiful to work with
using namespace SimTK;
SpatialMat M; // spatial inertia
SpatialVec 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
~V
ω
ke
9
=
=
M
v
I
V
mcx
ω
/2
−mcx
m
½ (ω•(Iω+mcxv)+ v•(mv−mcxω))
v
Notation that works
•
A physical quantity
–
–
–
–
–
•
•
10
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
Basic notation
Type of
quantity
Q
Fixed to
which body?
B
i Which
instance
(optional)
•
v
B
A vector attached to
body B, expressed
in B
Implies several defaults
– Measured in B’s frame, from B’s origin
– Expressed in the B frame
– Details depend on type of Q
•
11
Code equivalent: Qi_B, v_B
Add explicit measured-in (“from”)
frame
Measured with
respect to which
body?
M
B
i
To
i
Q
From
Q
•
•
12
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

G
B
Angular velocity of body
B with respect to Ground,
expressed in Ground
Change expressed-in frame
Measured with
respect to which
body?

Fixed to
which body?
M

B
i F
Q

Expressed-in frame
•
•
13
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

P
B
G
Angular velocity of body
B with respect to its
parent P, but expressed in
Ground
Rotation matrix
B G
~
 x 
G B
R  [ G x B G y B G z B ]  ~ B yG 
 ~ B z G 
M
F
F
FO
P
parent
body P
14
R M (q)
q,u
MO
mobilized
body B
B
P
RB  P RF F RM M RB
 P R F F R M (~ B R M )
For translations, specify points if
not origin
Measured from
what point on what
body?
PA
p
QB
To what
point on
what body?
Point name conventions:
O origin (implied)
C mass center
•
•
15
G
p
Vector from Ground
origin (implied) to body
B’s mass center,
expressed in Ground
New defaults
–
“From” frame implied by left superscript
–
I.e., A frame above left, G frame above right
CB
Code equivalent: p_PaQb; p_GCb, p_GoCb
Transforms


G
B

R 
G
X B   


 (0 0 0



B
 G pO  




1) 
Transform
Rotation&
Vec3&
16
X_GB;
R_GB=X_GB.R();
T_GB=X_GB.p();
Transforms (cont.)
M
F
X M(q)
F
FO
MO
q,u
CB
B
mobilized
body B
F
V M (q, u)
F M
A (q, u, u)
P
pCB  PX B  B pCB
P
parent
body P
Transform
Vec3
Vec3
17
X_PB=…;
p_BCb=…; // from B origin,
p_PoCb = X_PB*p_BCb;
in B frame implied
// implies P frame
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
18
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,
const Transform& X_GB
=
const Transform& X_GA
=
const Vec3
p_OA_OB_G =
but linear velocity needs an wXr term.
getBodyTransform(s,objectBodyB);
getBodyTransform(s,inBodyA);
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);
}
19
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
•
20
NIH Roadmap grant
U54 GM072970
Mobilizer kinematics
M
MO
F
X M(q)
F
FO
q,u
F
V M (q, u)
F M
A (q, u, u)
parent
body P
mobilized
body B
define q
define u
F
X M (q) 

F
RM
FO
p MO
V M (q, u)  F H M(q) u
F
q  N(q)u
relate q,u
A  FV M  F H M u  F H M u
F M
21
Seth, et al. Nonlinear Dynamics 62, 2010

Discussion
22