Simbody™

Theory Manual
Release 3.1
March, 2013
Website: https://simtk.org/home/simbody

SimTK Simbody™ 3.1
Theory Manual
Michael Sherman
Stanford University

March, 2013

Simbody™ is part of SimTK, the open source biosimulation toolkit originating from Simbios, the NIH National Center for Physics-Based Simulation of Biological Structures at Stanford, funded under the NIH Roadmap for Medical Research, grant U54 GM072970. Information on the National Centers for Biomedical Computing can be obtained fromhttp://nihroadmap.nih.gov/bioinformatics.
Simbody home page: https://simtk.org/home/simbody
Simbios home page: http://simbios.stanford.edu
That handsome devil on the cover is my hero, Sir Isaac Newton.

Copyright and Permission Notice
Portions copyright (c) 2005-2013 Stanford University and Michael Sherman.

Permission is hereby granted, free of charge, to any person obtaining a copy of this document (the "Document"), to deal in
the Document without restriction, including without limitation the rights to use, copy, modify, merge, publish, distribute,
sublicense, and/or sell copies of the Document, and to permit persons to whom the Document is furnished to do so, subject
to the following conditions:This copyright and permission notice shall be included in all copies or substantial portions of the Document.

THE DOCUMENT IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED,INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS, CONTRIBUTORS OR
COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN
ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE
DOCUMENT OR THE USE OR OTHER DEALINGS IN THE DOCUMENT.

ii

Preface
SimTK Simbody provides a powerful multibody mechanics capability for use in biosimulation. It is designed for use by programmers who are not experts in multibody mechanics.
Simbody provides a sophisticated, robust, high performance, open source option for mechanical simulation, including biomechanical simulation, as well as the additional functionality
and performance needed for effective modeling of large molecular systems in internal
coordinates. It is accessible through a stable C++ API. The full capability of this package,
including bindings for other languages, will be built up in layers over time; this document
covers the current capabilities and discusses future directions.
A complete multibody mechanics simulation (a biomechanical gait simulation or a molecular
dynamics simulation of a protein/RNA interaction, for example) requires many layers. At thelowest level are hardware-dependent, computationally intense “inner loop” numerical methods like basic linear algebra and molecular force field computations. Built on those are
numerical mathematics methods like numerical integration, nonlinear root-finding, optimization, and higher-level linear algebra. The next layer supports physics and mechanics andincludes Simbody’s multibody dynamics capability, as well as a variety of models for forces
and constraints.
Simbody’s distribution contains multibody dynamics, generally useful force and constraint
models, some crude visualization tools, and the lower-layer software it needs to run efficiently. However, that is still by no means a complete simulation system since it does not include
domain-specific modeling or a GUI. Thus in any complete simulation tool there are typicallytwo more layers built above Simbody: a modeling layer and a user interface that provides
model building and editing, execution, and visualization of results.
Consequently, Simbody itself is intended for use by primarily by programmers who are
writing domain-specific modeling tools and/or user interfaces and would like to incorporate
high-quality multibody mechanics into their work. Some current examples are: the OpenSim™ musculoskeletal modeling layer and GUI https://simtk.org/home/opensim and the
Molmodel™ coarse-grained molecular modeling layer https://simtk.org/home/molmodel.

iii

How to use this document
This Simbody Theory Manual contains background information on simulation in general andmultibody dynamics in particular, as well as detailed mathematical theory discussion andliterature references describing Simbody’s implementation. Generally the equation-denseparts are kept separate from the overview, so readers who want just one or the other can skim
over large chunks of material.
This is not a programming manual or user guide so you will not find detailed information
here about using Simbody in a program. For that, see the latest Simbody User Guide,
Simbody Advanced Programming Guide, and Doxygen API documentation available fromthe Simbody distribution project at https://simtk.org/home/simbody (go to the Documentstab). Simbody also provides a public forum where you can get help, and bug report andfeature request tools (go to the Advanced tab).

Document conventions
In order to allow myself the pleasure of delivering the occasional opinionated diatribe,
while permitting the easily offended reader to avoid them, I have placed a “pontification
warning” symbol like the one at the left at the beginning of such sections in the text.
The end of these sections is marked with the “off my soapbox” symbol to the right.

The symbol to the left is used to highlight sections which summarize earlier material.

This one is used to mark discussions of capabilities which are planned but not yet
implemented.

iv

Table of Contents
1 Background............................................................. 1
1.1
1.2
1.3
1.4

What is “multibody dynamics”? ................................ 1
Structure of a simulation in Simbody ........................ 2
Structure of a System ................................................. 4
Structure of a multibody system ................................ 5

2 Fundamental concepts of multibody mechanics ....... 7
2.1
2.2
2.3

Coordinate frames ...................................................... 7
Bodies ......................................................................... 8
Mobilizers ................................................................. 10

2.3.1
2.3.2
2.3.3
2.3.4
2.3.5

2.4
2.5
2.6
2.7

Mobilizers are not joints ...........................................10
Types of Mobilizers ..................................................12
Mobility space ..........................................................13
Parameterization of mobility ....................................14
A comment on deformable (flexible) bodies ............15

Constraints ................................................................ 16
Forces........................................................................ 17
Kinematics ................................................................ 17
Dynamics .................................................................. 18

3 Basic Simbody numerical types ............................ 19
3.1
3.2

Vectors and Matrices ............................................... 19
Geometry .................................................................. 19

3.2.1
3.2.2
3.2.3
3.2.4

3.3

Stations .....................................................................19
Directions .................................................................20
Rotations ...................................................................20
Transforms ................................................................22

Mechanics ................................................................. 23

3.3.1
3.3.2
3.3.3
3.3.4

Spatial Notation ........................................................24
Cross product matrix ................................................26
Spatial mass properties .............................................27
Spatial rotation, shifting, and transform ...................28

4 Constructing a Simbody multibody system ........... 31
4.1
4.2

Topology................................................................... 31
Bodies and their Mobilizers ..................................... 32

4.2.1

4.3
4.4

The reference configuration .....................................33

Constraints ................................................................ 35
Forces........................................................................ 37

5 Theory for Mobilizers ........................................... 39
5.1
5.2

Reverse mobilizers ................................................... 42
Mobilizers in body frames ....................................... 44

6 Theory for Constraints .......................................... 47
6.1

Explicit calculation of constraint matrices .............. 51

7 State representation and realization ....................... 53
7.1

Computation – realization of the State .................... 53

7.1.1
7.1.2
7.1.3

7.2

State variables .......................................................... 58

7.2.1

7.3
7.4

Responses, operators, and solvers ............................53
Caching of computed results ....................................54
Computing in stages .................................................57
State partitioning by stage ........................................58

State resources .......................................................... 59
Allocation of state resources .................................... 60

7.4.1
7.4.2
7.4.3
7.4.4

Mobilized bodies ......................................................60
Dynamic variables z .................................................61
Structured variables d ...............................................61
Constraints ................................................................61

8 Equations of motion .............................................. 63
8.1
8.2

8.3 Unconstrained systems with prescribed, fast, andslow variables .....................................................................67
8.4 Constrained systems with prescribed motion ..........69
8.5 Constrained systems as specified to Simbody .........73
8.6 Unilateral constraints ................................................75
8.6.1
8.6.2
8.6.3

8.7

Solving for impacts .................................................. 77
Sliding friction forces and impulses ........................ 79
Event detection for unilateral constraints ................ 88

Dynamic solution method ........................................89

9 Scaling and and accuracy ...................................... 95
9.1
9.2
9.3
9.4
9.5

Relative vs. absolute accuracy .................................95
Weighting and absolute accuracy.............................97
Scaling of constraint errors ................................... 100
Scaling at the acceleration level ............................ 102
Accuracy ................................................................ 103

10 Time Stepping .....................................................105
10.1 Coordinate projection ............................................ 105
10.2 Simplified equations .............................................. 110
10.3 Update rates for state variables ............................. 110
10.3.1

Coupling ................................................................ 113

10.4 How to take a time step ......................................... 115
10.4.1
10.4.2

Setting the values of prescribed variables ............. 118
Relaxation of fast variables ................................... 118

11 Simbody Force Subsystems reference guide.........121
11.1 General Force Subsystem ...................................... 121
11.2 Hertz/Hunt and Crossley contact model subsystem121
11.2.111.2.211.2.3

Motivation ............................................................. 121
The model .............................................................. 122
Extension to include Friction ................................. 127

11.3 DuMM — Molecular mechanics force field ........ 128
11.3.1
11.3.211.3.311.3.411.3.511.3.6them
11.3.711.3.8Background ............................................................ 128
Basic concepts ....................................................... 129
Units....................................................................... 131
Defining a force field ............................................. 132
Defining the molecules .......................................... 132
Defining bodies and attaching the molecule to132
Running a simulation ............................................. 132
Theory.................................................................... 132

12 Appendix: derivations ..........................................133
12.1 Notation for multibody theory .............................. 133
12.2 Re-expressing spatial quantities ............................ 136
12.3 Rigid body shifting of spatial quantities ............... 137
12.3.1

Rigid body shift of rigid body spatial inertia ......... 137

12.4 Inversion of rigid body spatial inertia ................... 138
12.5 Articulated body inertia ......................................... 139
12.5.1
12.5.212.5.3

Rigid body shift of articulated body inertia ........... 140
Articulated shift of an articulated body inertia ...... 141
Terminal bodies and base bodies ........................... 142

12.6 Modal analysis and implicit integration ................ 143
12.7 Root finding and optimization .............................. 144
12.8 Operator form of Simbody interface ..................... 145
12.9 Misc ........................................................................ 146

13 Acknowledgments ...............................................151
14 References ...........................................................152

Unconstrained dynamic systems.............................. 64
Constrained systems ................................................. 67v

1 Background
This is general material hopefully providing enough background for the rest of the document
to make sense. Even for those familiar with multibody dynamics, it is probably worth reading
to see how we are characterizing it for the broad uses it will serve for SimTK users.

1.1 What is “multibody dynamics”?
Multibody mechanics (of which multibody dynamics is a component) is the field studying theclassical mechanical properties, especially motion, of systems of bodies interconnected byjoints, influenced by forces, and restricted by constraints. The key feature of a system thatmakes it suitable for multibody treatment is the observation that its motion is localized, thatis, it is well-described as a set of independently identifiable parts which undergo large motion
with respect to one another, but are themselves rigid or nearly rigid. Figure 1 shows someexamples of the breadth of applicability of multibody mechanics, which has been usedeffectively to model machines, skeletal motion and gait, coarse-grained biopolymers, andmany other systems relevant to a wide variety of scientific and engineering disciplines.

Figure 1: Some multibody
systems.

Mechanical U-joint

Articulated
skeleton
Proteinbackbone1

Multibody mechanics is a generalization of several more-familiar modeling methods. It
includes as special cases, for example, systems of point masses represented in Cartesian
coordinates (e.g. molecular dynamics models) and systems of freely moving extended bodies(typically, rigid bodies) and these can be intermixed into systems which also contain bodies
whose motion is defined with internal (relative) coordinates, that is, with respect to one
another rather than with respect to the Cartesian frame. Multibody mechanics should beviewed as a basic numerical capability fundamental to any simulation system. It is in thesame category as, say, a linear algebra library, not an end-user application. Simbody is foruse by modelers and application developers as a basic building block. Computational researchers working to improve multibody simulation methods can use Simbody as a baseline
source of correct answers for debugging and as a performance baseline to demonstrate thesuperiority of their new methods. However, Simbody itself is not a research project; it isintended instead as a reliable, industrial-grade tool on which researchers may depend.

1.2 Structure of a simulation in Simbody
The figure below shows the primary objects involved in computational simulation of aphysical system in SimTK, the infamous “three S’s of simulation”: System, State, and Study.
Here’s our first equation:
Simulation(SimTK) = System + State + Study
A System is a computational embodiment of a mathematical model of the physical world. A
System typically comprises several interacting, separately meaningful subsystems. A System
contains models for physical objects and the forces that act on them and specifies a set ofvariables whose values can affect the System’s behavior. However, the System itself is anunchangeable, state-free (“const”) object. Instead, the values of its variables are stored in a
separate object, called a State, more about which below.* Finally, a Study couples a Systemand one or more States, and represents a computational experiment intended to reveal some-*We will frequently use “state” (lower case) to refer to the values stored within a State object. This isn’t as

confusing as it might seem—even if we get the capitalization wrong the meaning will be obvious from context.

2

thing about the System. By design, the results of any Study can be expressed as a State value
or set of State values which satisfies some pre-specified criteria, along with results which the
System can calculate directly from those State values. Such a set of State values is often
called a trajectory.

Study

ResultsState

statesSystem

It is important to note that our notion of “state” is somewhat more general than the common
use of the term. By state, we mean everything variable about a System. That includes not
only the traditional continuous time, position and velocity variables, but also discrete variables, memory of past events, modeling choices, and a wide variety of parameters that we callinstance variables. The System’s State has entries for the values of all of these variables.
This design allows the conceptually simple model depicted above to express every kind ofinvestigation one may wish to perform. Here are some examples. The simplest Study merely
asks the System to evaluate itself using values taken from a particular State. More interestingly, a dynamic Study produces a series of time, position and velocity State values which
result from solving the classical dynamic equations representing Newton’s 2 nd law, F=ma.
An energy minimization is a Study which seeks values for the State’s position coordinates atwhich an energy calculation yields its minimum value. A Monte Carlo simulation is a Study
yielding a series of states which satisfy an appropriate probability distribution. Design
studies, also used for parameter fitting, are Studies which find values for instance variables
such as lengths, masses, material properties, or coefficients which meet specified criteria.
Modeling Studies select among models or algorithmic choices to improve defined measures3

of behavior, such as accuracy, stability, or execution speed. And so on. Since we know that
all System variability is contained in the State, we can guarantee that any answers you seek
regarding the System can be expressed in terms of state values, provided that a corresponding
System is available to interpret them.

1.3 Structure of a System
Looking a little closer at a System, you will find that it is composed of a set of interlocking
pieces, which we call subsystems.

Study

State

subsystem

System

In this jigsaw puzzle analogy, you can think of the System as providing the “edge pieces”
which frame the subsystems into a complete whole.
In general any subsystem of a System may have its own state variables, as can the Systemitself. The System ensures that its subsystems’ state needs are provided for within the overall
System’s State. The calculations performed by subsystems are interdependent in the sense of
having interlocking computational dependencies. However, these dependencies can always
be untangled by performing computations in “stages” as will be discussed below. It is the
System’s responsibility to properly sequence its subsystems through the stages.
Note that by design this is not a hierarchical structure. It is a flat partitioning of a System intoa small number of Subsystems. In a higher-level modeling layer, one would expect to findhierarchical models, which are a powerful way to represent the physical world. However,computational resources are flat, not hierarchical, and the System/Subsystem scheme is a4

computational device, not a modeling system. The intent is that a modeling layer (or user
program) assembles a System from a small library of Subsystems just at the point when it isready to perform resource-intense computations.

1.4 Structure of a multibody system
Simbody provides some computational components (puzzle pieces) of a complete multibody
mechanics System. Simbody’s primary piece, the SimbodyMatterSubsystem, manages therepresentation of interconnected massive objects (that is, bodies interconnected by joints).
Simbody can use this representation to perform computations which permit a wide variety ofuseful Studies to be performed. For example, given a set of applied forces, Simbody can very
efficiently solve a generalized form of Newton’s 2 nd law F=ma. On the other hand, Simbody
is agnostic about the forces F, which come from domain-specific models. That is, Simbody
fully understands the concept of forces, and knows exactly what to do with them, but hasn’t
any idea where they might have come from. Muscle contraction? Molecular electrostaticinteractions? Galactic collisions? Whatever.
A complete System thus consists both of the matter subsystem, and force subsystems that
may be Simbody-provided, user-written, or application-provided. So for a multibody system,Statethe general System described above is specialized to look something like this:Forces #1

MatterForces #3

Forces #2

Simbody Multibody System

5

Although both the Simbody matter subsystem and the force subsystems require state variables, as discussed above any System (including of course a MultibodySystem) is a statelessobject once constructed. Its subsystems collectively define the System’s parameterization,
but the parameter values themselves are stored externally in a separate State object.
The force and mechanical subsystems are computationally interlocked. For example, a userprovided force will typically depend on position and velocity information (kinematics)
returned by the Simbody matter subsystem, while accelerations (dynamics) calculated by
Simbody will in turn depend on the values of the forces. Section 7.1 provides details on how
these interlocking computations are performed.

6

2 Fundamental concepts of multibody
mechanics
There are only a few general concepts required to completely specify a multibody system.
These are closely related to physical concepts for which the reader is likely already to have agood intuition. This is both blessing and curse, since our intuitive understanding of theseconcepts is almost, but not quite, general enough or precise enough to serve as a basis forgeneral simulation. Nevertheless we will plunge ahead using familiar concepts, adding
precise definitions and suitable generalizations where needed.
The concepts we’ll need to define a multibody system are: coordinate frame, body, mobilizer,
constraint, and force. We’ll also discuss the fundamental ideas of kinematics and dynamics
of a multibody system.

2.1 Coordinate frames
We define a coordinate frame (syn: reference frame or just frame) F to be a set of threemutually orthogonal directions (called axes) and a point (called the frame’s origin). We will
denote the axes as unit vectors Fx, Fy, Fz and follow a right-handed (“dextral”) convention sothat Fz = Fx Fy. We label frame F’s origin point FO.
Coordinate frames are used for measuring things. We can express the location of a point P in
frame F, for example, by measuring the vector r from F’s origin to P, that is r=P–FO, andthen expressing it in frame F by writing down the components of r in each of the three axis
directions. These numerical values are called the measure numbers of r in F denotedFr  (rx , ry , rz )  (r  Fx , r  Fy , r  Fz ) . That is, the measure numbers are the scalars obtained bytaking the dot product of a vector with each of the three axis directions of the expressed-inframe. Here’s a picture:

7

Fy
P

ry

FO

r

rx

Fx

rz

Fz

Figure 2: Coordinate frame F, and how to
express the location of a point P in F.

Note that r is a unique physical quantity (the vector from FO to P) but its measure numbers
would be different if it were expressed in a different frame. In general we will use the F [Q]notation to indicate that some physical quantity Q is being expressed in frame F, whenever
the frame is not already obvious.
I suspect the above has not been much of a stretch for most readers, since this is a perfectly
ordinary example of a conventional coordinate frame. Possibly the notation and the term“measure number” are new, but everyone is familiar with these concepts. We are just being
excruciatingly precise in distinguishing the physical quantities of direction and location fromtheir expression in a particular frame of reference.
This next idea may seem a bit odd if you haven’t encountered it before: the concept of aframe makes perfect sense even if we can’t say where it is or which way its axes are pointing.
Once we have a frame F, for example, like the one defined above, we can start measuring
things in frame F without the slightest idea how F is placed with respect to other things. We
can even measure frame F in itself—the measure numbers of its axes are F[Fx] =(1,0,0),F[Fy]=(0,1,0), F[Fz]=(0,0,1) and its origin point is F[FO]=(0,0,0). In a sense F defines its own

self-consistent universe without reference to anything else. Note that this universe extends
infinitely in every direction. In multibody mechanics we have another name for such an
independent universe: a body.

2.2 Bodies
Fundamentally, a body B is just a moving reference frame, called the body frame B. You
probably aren’t used to thinking of a body this way! We will shortly connect this back to8

more intuitive “body” concepts like mass and geometry; however, it is the frame that is abody’s most fundamental characteristic. One implication of this is that a body extends
infinitely in all directions. Before you completely reject this idea, answer this question: is thehole a part of a doughnut?* In any case the infinite extent of bodies will turn out to be very
convenient when we start connecting them together.
We call the ordered set of all bodies in a multibody system[i]  .

[i]’s body frame is[i] with origin

O, with the ith body designated as[i ] . In practice we’ll only have to talkabout a few bodies at a time so we can use different letters for them and avoid subscript
bloat. In particular, body G is the distinguished body Ground representing the inertial (nonaccelerating, non-rotating) reference frame.† The ground frame provides a global origin GO(we’ll usually drop the frame in this case and just say O) and fixed orthogonal directions
x Gx , y Gy , z Gz . By convention, we identify ground with the “0th” body, that is,[0]  G .
Bodies typically have associated features which can be measured in and expressed in the
body frame. These include other frames, directions (unit vectors) and stations (point locations). The body frame B origin is the station whose measure numbers when expressed in the
body frame are (0,0,0), and its axes are the directions with measure numbers (1,0,0), (0,1,0),and (0,0,1). Mass properties include the total mass (a scalar), the center of mass (a station,
represented numerically by a vector), and an inertia tensor (numerically a 33 symmetricmatrix) which expresses rotational inertia about a particular station. When the inertia tensor
is defined about the center of mass it is called the central inertia. For rigid bodies, massproperties are constant; for deformable bodies (not presently supported by Simbody) the
mass is constant but the center of mass and inertia will be seen to vary when measured in the
body frame.

*

Thanks to Paul Mitiguy for the doughnut analogy.

†

Other names sometimes used for the ground frame are: Cartesian frame, Newtonian frame, world frame,
inertial frame, laboratory frame, and experimenter’s frame.

9For practical purposes we assign each body a fixed property called that body’s mass structure. The possible mass structures are: (1) ground, (2) massless, (3) particle (inertialess), (4)line (inertialess in one direction), (5) rigid body, and (6) flexible body.

2.3 Mobilizers
A mobilizer connects a body to its unique parent body, * and provides the relative mobility(degrees of freedom or “dofs”) allowed between those bodies. Mobility expresses the permitted motion of a body’s frame with respect to its parent’s frame. Don’t confuse this with theidea of constraining the motion of otherwise free bodies—in Simbody, bodies start out withno mobility at all, meaning that the body’s frame and its parent’s frame are locked together
and would stay that way forever. Thus there is no motion to be constrained. Instead, Mobilizers are used to grant to a body the ability to move relative to its parent, allowing translation and/or rotational motion of the body frame and providing a parameterization of that
motion. We call these unrestricted, parent-relative degrees of freedom a body’s mobilities.
The unique Ground body has no parent and no mobility.
2.3.1 Mobilizers are not joints
When describing a multibody system, a joint is a higher-level (more abstract) concept than amobilizer, although they are easily confused. We reserve the term “joint” to refer to thephysical-world concept of that name, as illustrated in Figure 3. In general, joints are implemented as a combination of mobilizers and constraints, and may also introduce force elements (e.g. friction or soft stops). It is possible to create topological loops with joints but notwith mobilizers, as the latter are restricted to connections between bodies and their uniqueparents. So a mobilizer can only add degrees of freedom to a system, while a joint may add
or remove them.

*

Recall that Ground is a body.

10

Figure 3: a mechanism with four joints; at most three can bemobilizers unless you break one of the bodies.

A mobilizer is one way to implement a joint, but not all joints are mobilizers. For example,
when a joint forms a loop as in the figure, it reduces the total mobility, requiring implementation as a constraint rather than a mobilizer. While the physical system is uniquely describedin terms of its bodies and joints, in general there will be many ways to decompose thatsystem into mobilizers and constraints for purposes of building a Simbody model. In particular, for a case like the one illustrated in Figure 3, there is a nice way to make the mobilizers
correspond to the joints—break the loop by cutting one of the bodies instead of the moreintuitive means of breaking one of the joints. The split body’s mass properties should be
divide 50/50 between the two halves (don’t use a massless body – that risks making the tree
part of the system singular which is not allowed in Simbody). Then a Weld constraint is
added to weld the two halves back together into the original body. With this approach all the
joints are mobilizers so are treated uniformly, with the motion of every joint being represented explicitly by mobilities. The alternative of modeling one of the joints with a 5-constraint
equation Pin constraint can work but is unappealing; one of the joints is then modeleddifferently, and there are no coordinates in the system directly corresponding to the motion ofthat joint.
One case where it is reasonable to split a loop at the joint is when that joint is a Ball joint
modeled with quaternions – in that case you can use a Ball constraint rather than a Ball
mobilizer and easily obtain the quaternions from the relative orientations of the two con-11

strained bodies. Other than that, though, we recommend splitting loops by cutting bodies
rather than joints.
2.3.2 Types of Mobilizers
The most common mobilizer types are sliding, torsion, and orientation. A sliding mobilizer(syn: prismatic) provides a single degree of freedom representing translation along a defined
axis, and adds a single coordinate with units of length to the system’s set of generalized
coordinates. A torsion mobilizer (syn: pin) provides a single degree of freedom representing
rotation about a defined axis and adds a single generalized coordinate with angular units. An
orientation mobilizer (syn: ball, spherical) permits unrestricted relative orientation between
its pair of bodies, that is, three degrees of freedom and at least three corresponding generalized coordinates (for dynamics these require a four-element quaternion).
Most other mobilizer types can be viewed as compositions of the three basic types. For
example, a cartesian mobilizer is a composition of three sliding joints with orthogonal axes
and thus permits unrestricted relative translation (three degrees of freedom) between thebodies it connects. A free mobilizer is a composition of a cartesian and an orientation mobilizer and permits six degrees of freedom (completely unrestricted motion) between its bodies.
A free mobilizer serves to introduce independent rigid bodies into the system and simply
provides a convenient reference frame and corresponding coordinates with which to express
their motion. Note that, like all other mobilizers, a free mobilizer can be placed between any
two bodies—it does not have to connect a body to ground. This allows very convenient
relative coordinates to be used for collections of independent bodies. For example, one can
express a protein domain that carries its local waters and ions along with it when it is moved
kinematically.
Complex joints can be built up from mobilizers and constraints (see below). A “screw joint”
for example could be composed of a coaxial sliding and torsion mobilizer, providing one
translational and one rotational coordinate, plus a constraint enforcing a defined relationship(the screw’s “pitch”) between the time derivatives of these coordinates. However, the Simbody mobilizer concept is extensible in the sense that arbitrarily complicated ones can beconstructed without the use of constraints. There is, in fact, already a screw mobilizer thathas only a single generalized coordinate and no constraints, but can only represent screw
12motion. A more elaborate, data-driven example is a subject-specific knee joint which can bebuilt as a 1-dof mobilizer so that a single unconstrained coordinate is used to represent thecomplicated coordinated rotational and translational motion of a knee.
2.3.3 Mobility space
A body can have from 0 to 6 relative mobilities (degrees of freedom) with respect to its
parent body. Summing the mobilities of each body in a multibody system, the total of nmobilities defines an n-dimensional mobility space for the multibody system. The n mobilities are independent by construction and thus form a basis for mobility space. Only configurations in mobility space are representable by the multibody system. Typically there are many
conceivable configurations which simply cannot be expressed. For example, consider asystem composed of Ground and one moving body, a wheel, having a single mobility withrespect to Ground consisting of just a rotation about a fixed axis. One can imagine a configuration in which the wheel is removed from the axis, but the chosen multibody system simply
can’t express that. With just one coordinate, an angle, we can only talk about rotations of thewheel about an axis. Additional mobilities would have to have been granted to the wheel inorder to express more general configurations.
This ability to limit the mobility space of a multibody system is extremely powerful if you
happen to know something about the space containing the solutions of interest to you. To
continue the above example, if you are a car designer rather than a crash-test engineer, then
you know that correct solutions to your vehicle simulation problems will always exhibit
wheels that are attached to their axles. Solutions in that smaller space are much easier to findthan solutions in the much larger space where wheels may be found anywhere. Similarly, in
molecular mechanics if you know that certain groups of atoms are always observed to move
together as rigid bodies, problems are much easier to solve in a reduced space in which only
those groupings can be expressed, than one in which the atoms could be anywhere. We knowthat correct solutions would always “rediscover” the known groupings (at great, and unnecessary, expense).

132.3.4 Parameterization of mobility
The mobilities of the bodies in a multibody system, taken together, define its mobility space.
However, we must choose a particular parameterization of this space (that is, a basis) in order
to express a particular configuration and motion of the multibody system and this choice is
not unique. Conveniently, body mobilities are mutually independent so we may choose theparameterization for each body separately. The set containing all these parameters is then theparameterization of mobility for the multibody system as a whole.
The independence of body mobilities localizes the parameterization issue to the mobilizer foreach body. Each mobilizer must define two sets of scalar parameters to express particularvalues for its mobilities, one set to specify the relative positioning (configuration) and theother to specify the relative velocity (motion) between the parent and child bodies. Parameters used for positioning are conventionally called generalized coordinates; parameters forvelocity are called generalized speeds.* The symbol q is used to represent a vector of generalized coordinates, and u is a vector of generalized speeds. Generalized coordinates are sometimes referred to as “internal coordinates,” “relative coordinates,” or “torsion coordinates.”
In Simbody, the number of a body’s generalized speeds u is always the same as that body’s
mobility—e.g., if a body has five degrees of freedom with respect to its parent, then it will
also have five u’s. The u’s are thus mutually independent. u’s have interpretations with direct
physical meaning, and the system equations of motion are written in terms of the time
derivatives of u, which we denote u and refer to as generalized accelerations. The generalized coordinates q, on the other hand, must at times be chosen for convenience or computational stability and do not always map directly to physical quantities, so in general q  u . In
fact, for many bodies there will be more q’s than u’s in which case the q’s are not always
independent. However, the interdependence among a body’s q’s is always a localized relationship among only those q’s, and never involves other bodies. At any particular configura-*

Generalized here refers to the use of the mobility space basis where the meaning of the coordinates and speeds
depends on the definition of the associated mobility. They can represent translations, rotations, or more generalmotions. We similarly use generalized forces to mean both forces, torques, or more general loads which areapplied along or about mobilities.

14

tion, there is always a linear, invertible relationship between q and u, and each Mobilizer
provides the necessary conversions. As a specific example, during dynamic calculations
Simbody Mobilizers that permit unrestricted relative orientation between a body and its
parent use four quaternions to stably represent the orientations, while the three generalized
speeds are just the elements of the relative angular velocity vector. The four quaternions mustsatisfy a normalization constraint, leaving only the expected three degrees of freedom for thefour coordinates.
For the whole multibody system, the generalized speeds are aggregated in a vector whose
length is the sum of the mobilities of each body. This vector is the set of generalized speeds
for the multibody system and is also designated u. A vector q aggregating the individual
bodies’ generalized coordinates forms the generalized coordinates for the whole multibody
system. Together, q and u constitute the instantaneous state of the matter component of amultibody system. It will usually be clear from context whether we are referring to thecoordinates of the whole system or just one body, but if we need to be specific we use qB anduB to indicate the sets of mobilizer parameters for body B.
2.3.5 A comment on deformable (flexible) bodies
In general, the bodies of a multibody system do not have to be rigid. It is sometimes desirable
to allow the bodies themselves to undergo small internal motions, called deformations. These
add a new set of independent coordinates to the overall system coordinates and speeds, but
we distinguish them from the generalized coordinates and generalized speeds introduced bymobilizers and refer to them instead as deformation coordinates and deformation rates.
Various techniques can be used to determine the appropriate representation of deformablebodies. Typically, structural mechanic methods are used to aggregate large nearly-rigid
subsystems into deformable bodies with “assumed mode” linear deformations.
We do not provide direct support for deformable bodies yet in Simbody, but it is always
possible to model body flexibility by partitioning the body into Mobilizer-connected rigid
bodies, with internal forces and constraints modeling the deformation behavior. Reference 1
describes how deformable bodies can be incorporated into a computational methodology like
Simbody’s.

15

2.4 Constraints
As discussed above, Simbody’s mobilizers allow construction of a very small mobility space
to represent all possible motion of the bodies of a multibody system. However, we will often
find that even this reduced space is substantially larger than our known solution space. For
example, in a multibody system where the joints form closed loops like the one shown in
Figure 3, mobility space would permit solutions where the loops are not closed. Instead, wewould like to focus on a lower-dimensional subspace of mobility space, called constrained
space. The dimensionality of constrained space is the net number of degrees of freedom
possessed by the multibody system. * So a multibody system’s net degrees of freedom (or net
mobility) can be smaller than the sum of its bodies’ individual mobilities.
One might wish simply to redefine the mobility of the bodies so that only constrained spacecan be expressed (that is, make mobility space=constrained space), and that is a very goodthing to do if you can. Unfortunately, in general constrained space cannot be parameterized
directly. Instead we create a system with a small but convenient-to-define mobility space,
and then add a set of constraints whose satisfaction implicitly defines the constrained space.
Constraints may represent arbitrary restrictions on the generalized coordinates and generalized speeds, and linear restrictions on accelerations. Each Simbody constraint generates one
or more constraint equations. Each independent constraint equation removes one degree of
freedom from the system. In this sense constraints are the complement of mobilizers, whose
generalized speeds each add one degree of freedom to the system. And in fact any n-dof
mobilizer can be represented instead as a free mobilizer plus 6−n constraint equations.
Constraints among the moving bodies of a physical system act by introducing internal forces
and moments. These forces act in the same manner as the applied forces described below—
they can act on bodies or along mobilities (joint axes), and as with applied forces they can*

When a multibody system represents a mechanism, the net number of degrees of freedom after accounting forconstraints is conventionally called the mechanism’s “mobility.” We use the terms mobility and mobility space
exclusively to mean the number of degrees of freedom in the unconstrained system. We’ll say “net dofs” or“net mobility” whenever we’re referring to the post-constraint leftovers.

16

always be reduced to a system of forces acting only along the mobilities. The only difference
between constraint forces and externally applied forces is that the constraint forces are
unknown and must be solved for simultaneously with the system accelerations.

2.5 Forces
By forces we mean to include both forces and moments (torques).* Force vectors can beapplied to the multibody system at any station on a body and moment vectors can be applied
to any body (or implemented as pairs of forces). Scalar forces or moments can also be
applied to the system’s mobilities, that is, directly along the generalized speeds; these arecalled generalized forces or mobility forces. All systems of forces and moments can be
reduced to an equivalent set of generalized forces, and Simbody provides an operator whichefficiently performs this useful conversion.
Forces can be functions of time, position, velocity, and their own internal states. They may
be local effects or result from spatially distributed fields or a constant gravitational field, or
act pairwise between distant stations (e.g. atoms) in the system. Forces which depend only on
configuration are called conservative forces, and are the gradient of some potential energyfunction. Non-conservative forces dependent on time and velocity exist as well but may notcontribute to potential energy.

2.6 Kinematics
Kinematics is usually defined as the study of motion without regard to mass or force. In
practice, however, it is entirely concerned with the mapping between the mobility coordinates and spatial positions, velocities, and accelerations of the bodies. The mobility coordinates and speeds uniquely determine the spatial quantities so the mapping in that direction isfast and direct; this is called forward kinematics. Given a q we can immediately say where all
the bodies are; with q and u we can say how they are moving; and with q, u, and u (where u
is the time derivative of u) we can say how they are reacting (accelerating). The reverse*

The term loads is often used as an alternative with less ambiguity. However we will continue to use the morefamiliar term forces, usually meaning both forces and torques.

17direction is called inverse kinematics and is more difficult unless all bodies have been given
unrestricted mobility (i.e., they are “free”). Given a set of observed spatial kinematic quantities, the goal of inverse kinematics is to find the “best fit” mobility coordinates and speedsthat satisfy both the observations and the constraint equations generated by the multibody
systems’ own Constraints. Such problems arise, for example, when fitting a reducedcoordinate molecular model to a set of atom positions determined with X-ray crystallography. Simbody provides an ObservedPointFitter solver that can solve many common inverse
kinematics problems. More generally, there is a broad assortment of useful initial condition
analyses which must be performed prior to the start of a dynamic analysis, and these arebased on kinematic calculations.

2.7 Dynamics
Dynamics is concerned with the relationship between forces and accelerations at a fixedvalue of the state variables q and u. This is determined by Newton’s second law, f=ma.
Forward dynamics attempts to calculate accelerations and internal constraint forces, given a
set of applied forces (which is equivalent to some set of generalized forces f). Inverse dynamics (also called prescribed motion) attempts to determine what set of generalized forces
explains a given set of generalized accelerations. In practice it is often useful to specify someaccelerations and some forces and calculate the remaining unknowns.
Given this definition of dynamics, advancing the state through time to produce a trajectory,or searching through the state to satisfy particular objectives, are higher-level operations(“Studies”) which are facilitated by the multibody dynamics capabilities described here.
SimTK::TimeStepper and SimTK::Integrator objects are designed to employ Simbody
dynamics calculations to generate a trajectory through time.

18

3 Basic Simbody numerical types
This chapter presents the basic numerical types used repeatedly in the Simbody API and intheory discussions. We’ll present both the mathematical notation and definitions for theseobjects and the C++ classes used to manipulate them programmatically.

3.1 Vectors and Matrices
Simbody makes use of lower-level SimTK toolsets to simplify its interface and internals. The
SimTK general purpose Simmatrix™ package (part of the SimTKcommon library that is partof Simbody) is used to handle basic vector and matrix objects. We follow the Simmatrix
convention of using names containing “Vector” and “Matrix” to refer to large objects ofvariable dimension, and names containing “Vec” and “Mat” to mean small, fixed-size objectsof known dimension. The types we use most are the fixed-size Vec3 and Mat33 types and thevariable length Vector type. We use the basic Simmatrix types to build up a set of specialized vectors and matrices of particular use in manipulating physical objects, as described in
the next sections.

3.2 Geometry
We provide a small set of specialized types for dealing with geometric quantities of interest
in multibody dynamics. This is not intended to be a general purpose geometry package. For
example, we happily assume that all geometry of interest is 3D.
Given the fundamental existence of a rigid body frame B, we are primarily interested instations, directions, and other frames fixed in B. These are represented by positions, rotations, and transforms (xforms) respectively, which locate these objects with respect to an
existing frame.
3.2.1 Stations
Stations are simply points which are fixed in a particular reference frame (i.e., they are“stationary” in that frame). They are specified by the position vector which would take theframe’s origin to the station. A position is represented by a Simmatrix Vec3 type. Simbody
does not provide an explicit Station class; Vec3’s are adequate whenever a station is tobe specified.
19

3.2.2 Directions
Directions are unit vectors, which are Vec3s with the additional property that their lengths arealways 1. We define a class UnitVec3 which behaves identically to Vec3 in most respects but
restricts the ways in which values can be assigned to ensure that the length is always 1. Thishas concrete performance benefits because this unit length guarantee means that we can track
length-preserving operations at compile time and avoid unnecessary normalization checks, orworse, unnecessary normalizations which are very expensive.
3.2.3 Rotations
There are many ways to express 3D rotations. Examples are: pitch-roll-yaw, azimuthelevation-twist, axis-angle, and quaternions. Many others are in common use. Each way ofwriting orientation has its own quirks and complexities. However, all of these are equivalent
to a 3x3 matrix, called a rotation matrix (synonyms: orientation matrix, direction cosine
matrix). Rotation matrices have a particularly simple definition and straightforward physicalinterpretation, and are very easy to work with. At the API level, Simbody uses the rotation
matrix as a least common denominator, embodied in a class Rotation. Rotation provides aset of methods which can be used to construct a rotation matrix from a wide variety ofcommonly-used rotation schemes.
Rotation matrices are simply 3x3 matrices whose three columns are mutually perpendicular
directions (unit vectors) representing the axes of one coordinate frame, expressed in another.
These are represented internally in objects of type Rotation as an ordinary Simmatrix
Mat33, and behave identically except that their construction and assignment is restricted toensure that certain properties are maintained. Those properties are: each column and row is aunit vector, the columns are mutually perpendicular, and the rows are mutually perpendicular, forming a right-handed set. That means that the third column (row) is the positive crossproduct of the first two columns (rows). Such a matrix is orthogonal; hence its transpose is its
inverse. Its determinant is +1, meaning that it is a pure rotation and not a reflection or scaling
operation.
We use the symbol R with left and right superscripts from Rto to represent the orientation of the“to” frame (the right superscript) measured with respect to the “from” frame (the left superscript), like this:
20

G

RB 

 B GGx

 By G

 Bz 

(Bx is the x-direction unit vector of frame B, with measure numbers expressed in B’s frame,
while the operatorF  indicates that the measure numbers of some physical quantity are re-expressed in coordinate frame F.) So the symbol G R B should be read “the axes of frame B
expressed in frame G,” or “the orientation of frame B in G,” or just “B in G.” We never use“R” alone for a rotation matrix; that is a recipe for certain disaster. Instead, we always
provide the two frames. (When under tight typographical restrictions, as in source code, wewrite G R B as R_GB.) Using this notation, one can simply match up superscripts to rotate
vectors or compose rotations. Also, since these are orthogonal, the inverse of a rotation
matrix is just its transpose, which serves simply to swap the superscripts. Using the Simmatrix “~” operator to indicate matrix transpose: ~G R B  B RG . As an example, if you have arotation G R B and a vector Bv expressed in B, you can re-express that same vector in G like
this: G v  G R B B v . To go the other direction, we can write B v  B RG G v  ~ G R B G v . As aC++ code fragment, this can be written
Rotation R_GB; //orientation of frame B in G
Vec3
v_G;
//a vector expressed in G…
Vec3
v_B = ~R_GB*v_G; //re-express v_G in frame B

Composition of rotations is similarly accomplished by lining up superscripts (subject to orderreversal with the “~” operator). So givenBG

R B andGRC

we can get

B

RC as

RC  B RG G RC  ~ G R B G RC . Note that the “~” operator has a high precedence like unary“–” so ~ G R B G RC is (~G R B ) G RC , not ~ ( G R B G RC ) .
As is typical for Simmatrix operations on small quantities, the transpose operator is actuallyjust a change in point of view and involves no computation or copying of data. That is, theoperations B RG G v and ~G R B G v are exactly equivalent in both meaning and performance:
the cost is 15 floating point operations (three inline dot products), with no wasted data
copying or subroutine calls.

21

3.2.4 Transforms
Transforms combine a rotation and a position (translation) and are used to define the configuration of one frame with respect to another. (Recall that we consider a frame to consist ofboth a set of axes and an origin point.) We represent a frame B’s configuration with respect
to another frame G by giving the measure numbers in G of each of B’s axes, and the measure
numbers in G of the vector from G’s origin point to B’s origin point, for a total of 4 vectors,
which can be interpreted as a 3x3 Rotation (see above) followed by the origin point location(a Vec3). Following computer graphics convention, we call this object a transform (abbreviated xform) and conceptually augment the axes and origin point to create a 4x4 linear operator which can be applied to augmented vectors (4 th element is 0) or points (4th element is 1),
or composed using matrix multiplication. We define a type Transform which conceptually
represents transforms as follows:
G(The notation

G

X

B

 G [ Bx ]

 0


G

[ By ]
0G[ Bz ] G p B

0
1 

p B  GO p BO , that is, the vector from the origin of the G frame to the origin of

the B frame.) Note that we use the symbol X for transforms, with superscripts fromX to soG

X B means “the transform from frame G to frame B,” or “frame B measured from andexpressed in frame G.” Another way to interpret G X B is that it represents the operations thatmust be performed on G to bring it into alignment with B (a rotation and a translation). Thenas for rotation matrices described above, we can interpret G X B B X C as a composition ofoperators yielding G X C , and ~G X B is defined to yield the inverse transform B X G .*The above transform matrix can be considered a matrix of four columns as shown: threeaugmented vectors and an augmented point. An alternate, and entirely equivalent, way toview this is as a rotation matrix, translation vector, and an extra row:*

Note that this is actually a different definition for the “~” operator than is normally used in Simmatrix, since
the inverse of a transform is not simply its transpose. However, the analogy with ~R (which is both the transpose and inverse of rotation matrix R), combined with the lack of any practical use for the transpose of atransform, makes this use of “~” very attractive and natural to use in practice.

22



G
B
X   


 0


G

R
0

B

  
 G B   p     01  

In our implementation, the physical layout of a Simbody Transform is just the three columnsof the rotation matrix followed immediately in memory by the translation vector, that is,G

X B  G RB

GpB

 . There is no need for the fourth row to be stored in memory since it is34

always the same.
Given a Transform, you can work with it as though it were a 4x4 matrix, or work directlywith the rotation matrix R and translation vector p individually, without having to makecopies. Although a transform defined this way is not orthogonal, its inverse is easy to apply
with no additional calculation. As described above, we overload the normal matrix transpose
operator “~” to recast a Transform to its inverse so that either the transform or its inverse
can be used conveniently in an expression, for example, B X C  ~G X B G X C . As is typicalusing Simmatrix objects, this inverse operator is just a change of point of view at zero cost,so the total cost is the same in either direction. For example, to transform a point measured
and expressed in one frame to the equivalent one re-measured and re-expressed in anotherframe costs one 3x3 matrix-vector multiply and one addition of 3-vectors per transformed
point, for a total of 18 floating point operations (flops), and the cost is the same if we transform it back using a Transform inverse. A straightforward implementation of a 4x4 transform (i.e., as an actual 4x4 matrix times a 4-vector) would require 28 floating point operations per transformed point. Composition of Transforms (using the ‘*’ operator for matrix
multiply) is done in 63 flops but would take 112 using a 4x4 matrix multiply. Thus Transform provides the convenience of a 4x4 transformation matrix at substantially lower cost.

3.3 Mechanics
Some additional specialized quantities arise in mechanics for dealing with mass properties,
which consist of a mass, center of mass, and inertia matrix for each body. Mass is a simple
scalar and center of mass just a point so we do not define special classes for them. Inertia,
however, is a tensor quantity (a 3x3 matrix) which is expected to exhibit certain properties.
23

Among these, it is symmetric, and the values of its elements must satisfy certain relationships. In addition, there are common operations on inertias which can be most efficiently andconveniently provided with a distinct inertia class. So we provide a class Inertia which isstored physically as a 3x3 symmetric matrix, i.e., a Simmatrix SymMat33 containing six realvalued numbers. This behaves like an ordinary matrix for read-only operations but its construction and assignment is restricted to enforce physical relevance, and additional operations
are provided, such as shifting inertia taken about one point to the equivalent inertia aboutanother point.
For convenience we combine all the mass properties into a MassProperties class, whichcontains a mass, a center of mass location, and an inertia matrix. Note that there is implicitlya reference frame in whose axes the vector and tensor are expressed, and from whose origin
the center of mass location and inertia distribution are measured.
3.3.1 Spatial Notation
We also build on the Simmatrix types to define some specialized vectors and matrices usefulin mechanics. Following Jain and Rodriguez5, we use spatial notation which combinestranslational and rotational quantities into a single object. Using Simmatrix we define theconvenient type SpatialVec to mean a stacked vector of two ordinary 3-vectors, and SpatialMat to mean a 2x2 matrix of ordinary 3x3 matrices, that is
typedef Vec<2,Vec3>

SpatialVec;

typedef Mat<2,2,Mat33> SpatialMat;

Note that these convenient types have well-defined interpretations as packed arrays of real
numbers, which means they have equivalent descriptions in C and FORTRAN, which we’ll
address later. There is zero overhead in C++ for using the more expressive types.
The first sub-vector of a SpatialVec is always the rotational component, and the second is
the translational one. Some examples of spatial vectors: spatial velocity V, spatial acceleration A, and spatial force F, defined like this:
V

24

 
 ,
v

 
A  , F
a


 
f

where ω is an angular velocity vector, v a linear velocity, β an angular acceleration*, a is alinear acceleration, μ a moment (torque), and f is a force. Each of these elements is an ordinary 3-vector (Vec3). Sadly, orientation is not a vector quantity so we can’t use an analogous   to represent configuration (orientation and position) of a rigid body p

SpatialVec P(that is, of a reference frame). However, it can be useful to think of position this way in somecircumstances.
Unless otherwise indicated, all quantities are measured with respect to the ground frame G,
and linear quantities are referred to the body origin. That is, the default symbols above
represent
V  GV B

 G B  G BO  , v A  GAB G B G B
 G BO  , F  F a  G B  G BO  f 

For spatial position, instead of the fanciful P we use the Transform class described above,
where
G



X B  G RB

GO

p BO



with rotation matrix R playing the role of P’s θ.
The above notation and somewhat atypical use of Greek symbols was chosen so that there
would be an obvious way to represent these using the restrictive typographical capability of aprogramming language. For Greek letters we use the correspondence w=ω, b=β, m=μ, q=θ,
so we can represent the above symbols in code with
V=[w,v], A=[b,a], F=[m,f], P=[q,p], X=[R,p](Although as mentioned above there is no actual P like this, orientation angles and quaternions are part of the generalized coordinates q so this notation is conceptually right even if
pragmatically flawed.)*

We use β rather than the more conventional α for angular acceleration because a and α are too similar in many

fonts, and we can use b in code rather than spelling out alpha.

25

3.3.2 Cross product matrix
For any vector quantity v, we use the notation v to indicate a 3x3 skew-symmetric crossproduct matrix such that for any vector w, v  w  v  w . Spelled out in scalars, the crossproduct matrix is 0
v   vz
 v y


v z
0
vx

vy 

v x 
0 (3.1)

Note that the matrix is skew-symmetric, so vT   v .
We will occasionally make use of the following identities:( v + w)  v  w

v2

(3.2)

vw  wvT  wT v13(3.3)

vT v   v v  vT v13  vv T

(3.4)

( 13 is a 3x3 identity matrix.) Note that vT v is a symmetric matrix with non-negative diagonal elements. Spelled out in scalars,v2

 v y2  vz2
vT v   vx v y v x v zTvx2  vz2
v y v z



T

2
2
vx  v y 
T

(3.5)

where T indicates that the element is the same as the transposed one. This can also be viewed
as the inertia (or gyration) matrix of a unit-mass particle located at v, measured about theorigin.
U  v  U T  (U  v) ,where U33 is orthogonal.

(3.6)

Since rotation matrices are orthogonal, equation (3.6) is particularly useful when transforming spatial quantities from one frame to another.

v  ( v)

(3.7)

where the overdot indicates a derivative with respect to time taken in some frame understoodfrom context.
26

Identity (3.7) is primarily useful to allow us to write v unambiguously without concern for
the typographical details of the overdot placement.
3.3.3 Spatial mass properties
The mass properties of a rigid body conventionally consist of the body’s mass m, the masscenter location p, and its inertia tensor J. It is convenient to view the inertia tensor as the
product of the mass and a gyration tensor G, such that J=mG. Then a spatial inertia matrix M
can be written as a spatial gyration matrix (giving the mass distribution) scaled by the total
mass:
M

 G
m
  p

p 

13 

For the spatial inertia matrix M B of a body B about its origin BO we have p  BO p BC so G
M B  mB  B B BOC  p

pBC 

13 BONote that when the spatial mass properties are given about the center of mass BC we havep  0 soM

BCB

G BBC mB 
 0

0

13 

Where the central gyration matrix is

G BBC  G B  (pTp13  ppT )  G B  pT p  G B  p2
using the parallel axis theorem and then cross product matrix identities (3.4).

ω
If we have the spatial velocity VC also referred to the center of mass, i.e. V C   C  , then wev 

can define another spatial vector quantity, spatial momentum of a body “referred to” itscenter of mass:

27

G C
PC  M CV C  m 
 0G C ω 
0 ω 

m
 
 C 
1   vC  v In the more general (and typical) case where the body origin BO≠BC we compute spatial
momentum the same way with the result being the spatial momentum referred to BO, which is
not the same quantity:p   ω  p vC Gω  p v C   m  P  m
1  v  v  pω 
 0 

 G
P  MV  m   p

(Because v  vC  pω and G  G C  pT p .) A body’s kinetic energy (a scalar) is calculated
from spatial momentum like this:KE  12 V T MV  12 V T P  12 m(ωTGω  v 2   )  (ωT p v  v T pω)  2ωT p v
Note that although the angular momentum must be referred to a specific point, kinetic energy
is independent of that point. That is

KE  12 V T MV

 12 V T P  12 m(ωTG ω  v 2  2ωT p v)

 12 V C T M CV C  12 V C T PC  12 m(ωTG C ω  vC 2 )These can be shown equivalent by substituting G C  G  pT p and vC  v  pω into the last
expression.
3.3.4 Spatial rotation, shifting, and transform
Objects of type Rotation and Transform have equivalent interpretations as spatial matrices:28

spatial rotationA

spatial shiftPA

spatial transform AR BRB  
 0

 1
SQ   P Q
 p

0 

RB 

(3.8)

0

1

(3.9)

A

 AR BX B  AO S BO A R B   A B A BOO p R

A

0 

RB 

(3.10)

Then a spatial vector or spatial matrix can be rotated, shifted, or transformed using thesematrices, their inverses, and their duals (inverse transpose). From the definitions above you
can check that swapping the superscripts produces the inverse of each of these matrices: BRA
RA   0
 1
Q P
S Q P
 p
B

B

B

0 

RA 

0

1

 BRA
XA B A B AOO p R ( A R B ) 1

 ( A R B )T

B

0 

RA 

0
 1
 P Q  p 1 
B AR B A A B
O
p O
 R

 ( P S Q ) 1

B

0 

RA 

(3.11)

 ( A X B ) 1

The dual operators are given by transposing the inverses, soA

R *B( A R B ) T  ( B R A )T

P

S *Q

( P S Q ) T  ( Q S P )T

A

X *B

( A X B ) T  ( B X A )T

 AR B 1 P pQ 


1 
0
 A R B AO pBO A R B 


A B
R
 0


(3.12)

Defined this way, operators R, S, and X apply to spatial vectors in the “motion” basis, likevelocities and accelerations. The dual operators R*(=R), S*, and X* apply to spatial vectors
in the “force” basis, like forces and impulses. These definitions follow Featherstone2 buthave the reverse sense from Jain3 and Schwieters4 where the force basis is primary and the
motion basis is dual (their ϕ operator is our S* operator).
Using these definitions you can rotate, shift, or transform a spatial inertia matrix (rigid orarticulated) M like this:29

A

R* B M B B R A  ( B R A )T M B B R A  A R B M B B R AAAS* B M B B S A  ( B S A )T M B B S A

X * B M B B X A  ( B X A )T M B B X A

See Chapter 12 for details.

30

4 Constructing a Simbody multibody
systemThe Simbody API (application programmer interface) assumes that the caller has made allmodeling decisions and simply wants to perform calculations on the model. The primarydecisions to be made are (1) how the physical model is to be decomposed into a particular setof rigid bodies, (2) what kinds of mobilizers are to be used to interconnected them in a tree
structure, and (3) what constraints, if any, should be present to restrict the allowable mobility.
A variety of higher-level automated modelers for specific domains can be provided which
can make these decisions and then use the low-level interface.

4.1 Topology
In describing the “matter” side of a multibody system, the most fundamental property is thesystem topology. By topology we mean just these properties:

A set of bodies (that is, reference frames). One distinguished body Ground is always
present.



The mass structure of each body. The possible mass structures are: (1) ground, (2)massless, (3) particle (inertialess), (4) line, (5) rigid body, and (6) flexible body.

For each body except Ground, a unique “parent” body with respect to which the
body’s mobility will be defined. This leads to a tree topology for the system as awhole, with the ground body at its root.

A set of topological constraints, that is, constraints which are always present and active. These can impart a closed-loop topology to the system as a whole.

A body’s mass structure defines the most general form that the body’s mass properties cantake on. Ground and massless bodies have only a single predefined set of mass properties:
infinite and zero respectively. Particles can take only a point mass, and never have inertia
about that point. A line body can be thought of as a linear arrangement of particles, and thus
has mass, a meaningful center of mass along the line, and equal central inertias in two
directions perpendicular to the line, but none about the line. A rigid body (representing a

31

mass distribution on a surface or in a volume) can have a full inertia. A flexible body has amass distribution that is not constant in the body’s frame.

4.2 Bodies and their Mobilizers
The primary Simbody representation of matter is a multibody tree, that is, a tree-structured
collection of interconnected bodies, which we call a SimbodyMatterSubsystem. On initialconstruction, a SimbodyMatterSubsystem contains just a single body, the inertial frame
Ground (body 0) which is the root of the multibody tree. To add a body B to an existing
SimbodyMatterSubsystem, you will need to be able to specify the following properties:

The parent body P (with body frame P), which must already be in the multibody tree.

A reference frame (axes and origin) for the body (this is implicit, but you need tohave it in mind). We call that the body frame B. (See Figure 2 for an example.)

Mass properties for the body, with the center of mass location measured from BO andexpressed in B, and the inertia (actually the unit inertia or gyration matrix G) measured about BO and expressed in B.



The mobilizer’s moving frame M attached to B. You must be able to express M’s configuration on B as a transform B X M from B to M.

The mobilizer’s fixed frame F, attached to P, which will be connected to M by themobilizer. You must be able to express F’s configuration on P as a transform P X Ffrom P to F.

The kind of mobilizer to be used to connect B to its parent body P, and whether toreverse the interpretation of the generalized coordinates.

Figure 4 shows a body B being added to a tree already containing its parent P. Not shown are
the body’s mass mB, its inertia JB= mBGB about BO and the transforms B X M and P X F .
When this information is supplied to the appropriate Simbody method, the new body becomes part of the growing tree, and a unique, small integer body number is assigned which
can be used to refer to the body later. The specified mobilizer is the unique inboard mobilizerof body B, that is, the mobilizer which connects it to a body which is closer (in a graph path32

length sense) to the Ground body. When defining the sense (sign) of mobilizer coordinates
later we will refer to the frame F on P as the “fixed” frame, and frame M on B as the “moving” or “mobilized” frame, although these terms are arbitrary and do not imply anything ofphysical significance except when P is ground in which case it really is “fixed.”

B
BC
M

BO

MO

F
FO

Figure 4: Adding body B to a
multibody tree already containing
parent body P.

PPO

4.2.1 The reference configuration
The frames M and F are used to define a reference configuration for each body with respect
to its parent. For most mobilizers, that is the configuration in which M and F are overlaid,and corresponds to a value of zero for the mobilizer’s generalized coordinates*. Figure 5
shows the reference configuration for the mobilizer defined in Figure 4. For any mobilizer
type, the values of the generalized coordinates q express a transform F X M which gives the*

Certain sets of mobilizer coordinates may define their own “zero” which does not necessarily correspond tonumerical values of zero for all coordinates. For example, zero (“no rotation”) for a quaternion is a four-vector(1,0,0,0).

33

current location and orientation of the M frame, measured from and expressed in the F frame.
The definition of each mobilizer type specifies the meaning of each of the q’s for that mobilizer and the kinds of transforms that can be expressed. For example, a Cartesian mobilizer
would permit arbitrary translation of M, but its axes must remain forever aligned with thoseof F. A ball (spherical) mobilizer’s coordinates express the complementary motion in which
the origins of the two frames must remain coincident forever, but the orientation of M can bearbitrary with respect to F. A sliding mobilizer permits translation along one axis only, and a
torsion (pin) mobilizer permits only rotation about a single axis. Other mobilizers permit
various combinations of rotation and translation, with the extremes being the Free mobilizer
which permits all possible motion (six degrees of freedom) and the Weld (im)mobilizer
which permits no motion at all (zero degrees of freedom).
Regardless of the mobilizer type, setting all the coordinates to zero expresses that the M andF frames are in their reference configuration.
Figure 5: The reference
configuration for themobilizer added in theprevious figure.

F

M
BC

P
PO

BO

B

Those users familiar with SD/FAST’s reference configuration should note that the above is adifferent method for defining the reference configuration. It is in fact the opposite approach:
SD/FAST requires the bodies to be entered already in the reference configuration, and thendefines the mobilizer (SD/FAST joint) frames from the reference configuration. We think it is34

much more natural to express the joint frames separately in their bodies’ frames, and thendefine the reference configuration from the joint frames. It is always possible to choose
mobilizer frames to reproduce the ones used by SD/FAST if you want, but it is no longer
necessary to calculate them that way.

4.3 Constraints
Constraints in Simbody are the complement of Mobilizers. Mobilizers add mobility to a
multibody system; Constraints reduce mobility by introducing one or more constraint
equations. Mobilizers are local, granting degrees of freedom to a single body, while Constraints are global and remove degrees of freedom from the multibody system as a whole byintroducing restrictions on the allowable relationships among the generalized coordinates,
speeds, or accelerations. A simple example is a distance constraint which says that a particular point fixed on one body must always be at a certain distance d from a point fixed onanother body. If those bodies are far apart in the graph of the multibody topology, this simple
restriction is actually expressing a complicated relationship that must hold among the mobility coordinates of many bodies. As mentioned earlier, it is much more efficient to define lessmobility in the first place than to grant the bodies their freedom and then take it away later!
However, as with the distance constraint above, that is not always possible or convenient, so
we have Constraints.
In the same way that a single Mobilizer may introduce several mobilities, a single Constraint
may generate multiple constraint equations. Unlike mobilities, which are globally independent, the constraint equations generated by Constraints may be mutually interdependent
making some of the constraints ineffective, redundant or inconsistent. A trivial example of aredundant constraint would be adding the same Constraint twice—nothing changes sincemobility coordinates which satisfy the first Constraint also satisfy the second. An example ofan ineffective constraint would be restricting the distance between a point on the outside of awheel and the point of the parent at the wheel’s center. If the specified distance is equal to
the wheel’s radius, the single mobility automatically meets this restriction at all times and thesystem has the same net mobility with or without the restriction. Changing the required
distance to anything other than the wheel’s radius creates an inconsistent constraint which
can never be satisfied by any setting of the mobility coordinates.
35

Simbody supports a variety of built-in Constraints, and arbitrary user-defined Constraints.
Some examples of built-ins are: Rod (distance) Constraint, Ball (coincident points) Constraint, and Weld (coincident frames) Constraint. A Rod constraint generates one constraint
equation which maintains a user-specified constant, non-zero separation distance between astation on one body (that is, a point fixed on the body) and a station on another body, as
measured along the line between the two stations. Each nonredundant distance constraint
removes one degree of freedom from the system. A Ball or “coincident points” constraint
generates three constraint equations which together hold a station from each of two distinct
bodies together at the same location in space, i.e., at a separation distance of zero, exactly
like a Ball joint. A nonredundant Ball constraint thus removes three translational degrees of
freedom from the system (all translation between the two points), while a Ball mobilizer adds
three rotational degrees of freedom (all rotation about the connected points). A Weld constraint maintains frames (both location and orientation) from each of two bodies coincident inspace, generating six constraint equations and thus removing six degrees of freedom from thesystem. Weld Constraints are the primary means by which we take a system that has loop
topology and make it a tree—we cut one of the bodies in two to break the loop and then weld
the two halves back together with a Weld constraint.
The information needed for adding one of the above Constraints to a Simbody multibody
system is as follows:

Two distinct bodies A and B. Either one (but not both) may be Ground. Both bodies
must already be part of the multibody tree and are identified by the mobilized bodyindex that was returned at the time they were added.

(Distance or Coincident Points Constraint) A station point PA fixed on body A andstation point PB fixed on body B. These are measured and expressed in the bodies’ local frames, that is, PA is measured from AO and expressed in A while PB is measured
from BO and expressed in B. The measure numbers of these vectors are thus constantduring simulation.

(Weld Constraint) A frame FA fixed on body A and a frame FB fixed on body B.
These are expressed in the bodies’ local frames, that is, FA is given by a transform36

A

X FA while FB is given by transform B X FB . The measure numbers of the transforms

are thus constant.
For a Rod (constant distance) Constraint you also need to supply a scalar distance.
This is the physical separation d  PB  PA between the stations that you would like
Simbody to maintain at all times. This separation must be significantly larger than zero; zero distance between stations is obtained using a Ball Constraint rather than a
Rod Constraint.

Note that nonredundant constraints will not be satisfied by arbitrary values of the mobility
coordinates. Prior to a simulation, you must find an initial set of generalized coordinates q
and speeds u that satisfies all the constraint equations. Occasionally this can be done byinspection or hand calculation, but in general it is a difficult nonlinear problem to be solved
numerically prior to beginning a simulation (this is called assembly analysis for q andvelocity analysis for u). Given any set of mobility coordinates q and u, Simbody can efficiently calculate the constraint equation violations those entail. Simbody provides a varietyof numerical methods that can be used to drive constraint violations to below a desired
tolerance, at which point the associated constraints will be considered to be satisfied. After
that, valid numerical studies maintain the constraint equations, and thus satisfy the Contraints, as they advance from step to step.

4.4 Forces
We can apply forces to bodies, or directly to the mobility coordinates represented by thegeneralized speeds u. In general these include both linear and rotational forces (torques).
Forces applied to mobilities are called generalized forces or mobility forces. Forces applied
to the bodies are called spatial forces or body forces. There is always a unique set of mobility
forces equivalent to any set of body forces, in the sense that both sets will produce the sameaccelerations. Calculating this equivalent set is an important Simbody capability, since theequations of motion are written in terms of the mobilities, while forces are typically known interms of their effects on the bodies.
It is important to note that calculation of applied forces is not limited to the force types
provided Simbody. Force calculation is a domain-specific modeling issue; Simbody’s job is37

to provide the information needed by the modeler to calculate the forces, and then to respondto those forces in accordance with Newton’s laws of motion. For convenience, the Simbody
distribution does include a set of basic force subsystems to use in calculating simple forces
such as gravity, springs, and atomic forces; however, this is by no means an exhaustive set
and it is easily extended.

38

5 Theory for Mobilizers
A Simbody Mobilizer defines the permitted mobility of a body B with respect to a moreinboard (closer to Ground) body P, called its parent body. A given mobilizer provides nmobilities (degrees of freedom) for body B with respect to body P, with 0  n  6 .
Each body has a unique parent so there is a one-to-one correspondence between bodies andmobilizers; in Simbody we call the combination of a body with its unique mobilizer a MobilizedBody. The permitted mobility is described in terms of n scalar velocity coordinates u(called generalized speeds), and nq  n scalar position coordinates q (called generalized
coordinates). The time derivatives of the generalized speeds serve as the generalized accelerations u . The meanings of these quantities are defined by the following equations, which
express the body’s allowed motion with respect to its parent in terms of q and u. This relative
motion is defined using a pair of coordinate frames associated with the MobilizedBody B at
the time it is added to the multibody tree: the unique mobilizer “moving” frame M attached to
B with constant transform B X M and “fixed” frame F attached to P with constant transform
P

XF.

39

B
B

M

XM

BO

MO
F

X M(q)

F

mobilizer Bq,ubody BFO

P

Figure 6: Coordinate frames for use
in describing the mobility ofMobilizedBody B with respect to its
inboard parent body P. Everything
blue is associated with B. The origin
point O of each frame is labeled.

XF

P
PO
parent body P
GroundThese are the equations that define the generalized coordinates and speeds. (The overdot
notation below indicates time derivatives that are taken in the F frame.)40F

X M (q) R ( q)F

MF

p M ( q)

 FV M   F M 
V M (q, u )  F M    F M   F H M ( F X M (q))u Vv   v FF F M 
V M   F M   F HM u  F HM u a AM ( q , u , u )Fq  N(q)u

(5.1)
(5.2)

(5.3)
(5.4)

n(q)  0(5.5)

Note that X and H cannot be chosen arbitrarily, because the time derivative of X is related to
V:
F

XM

R p    Rv  ( V )  R
V 
F

M

F

M

F

F

M

M

F

M



F

M



F

M



F

(5.6)

M

v

This implies relationships that must hold among X, H, and N:FR M  FM  F R M  ( F HM u )  F R MF F RM F RM
q
Nu
q
q

(5.7)

p M  F v M  F H vM u

 F p M (q) F p M (q)
q
Nu
q
q

(5.8)

where H and H v are the upper and lower 3xn partitions of hinge matrix H. Intuitively, this isstating the requirement that the spatial velocity produced from u by the action of H is thetime derivative of the spatial position and orientation produced from q by the nonlinear
function X(q), with matrix N serving to mediate between u and q . Note from (5.2) that H
depends only on the transform (spatial position) represented by the set of q’s, not on thedefinitions of the individual q’s.
Equation (5.5) specifies additional constraints that q must satisfy if there are not enoughequations (5.4) to uniquely specify q. That occurs when there are more q’s than u’s, typicallybecause q is a quaternion in which case n(q) is the quaternion normalization constraint.

41

5.1 Reverse mobilizers
Because a tree of mobilized bodies must be ordered parentchild along each branch goingaway from Ground, the role of parent and child may be reversed from the desired sense forsome mobilizers. When the generalized coordinates and speeds are intended to have aparticular physical meaning, we would like to preserve that meaning even when exchanging
the roles of parent and child. For example, if you make a “knee” mobilizer with the generalized coordinate being knee flexion, you would like to preserve that meaning regardless ofwhether the femur or tibia is the parent body.
So we will at times be given the mobilizer specification from frame M on body B to frame F
on body P, but a mobilizer specification must go from F to M. That is, we’re given M X F ,M

H F , M H F , and N for a mobilizer in frame M fixed to the child body B (with time deriva-

tives taken in M), but we want F X M , F H M , F H M , and N describing the mobilizer withidentical generalized coordinates and speeds in frame F fixed on the parent body (and withtime derivatives taken in F).
N is easy enough—since we want q and u to retain their original meanings, N must stay thesame. Almost as easy, F X M  ( M X F )1   F R M

 F R M  M p F  . But we’re going to have towork to get the other two matrices. First F H M :From Equation (5.2) (with the frames swapped) we haveM

 M  F   M HF 
V   M F    M F u
 v   Hv 
F

(5.9)

We want to find F H M such thatF

V

M F M   F HM   F M    F M u
 v   Hv 

(5.10)For the moment we are going to leave the expressed-in frame as M and work only with thephysical quantities. We’ll re-express at the end. We know that F M   M  F so it follows thatFHM   M HF . But the linear velocity cannot simply be negated. We have

42

M
M

vF

M

pF

F

M

F

M

d M F
p
dt

(5.11)

F

v

p

d F M
p
dt

(5.12)

Since F p M   M p F we can substitute into (5.12) and get
F

F

d M F
p
dt
 ( M p F  F  M  M p F )vM  

(5.13) ( v  p   )
M

F

M

F

M

F

 ( M H vF  M pF M HF )u

Since F v M  F HvM u we see that F HvM   ( M HvF  M pF M HF )M(5.14)In the above analysis we left the quantities expressed in the M frame as emphasized inequation (5.14) (although we took the derivative in F). Re-expressing in the F frame completes the computation of F H M :
F

M


HF
H M   F R M  M F M F M F  H v  p H 

(5.15)

We can differentiate equation (5.15) in F to get F H M :
F

M


HF
H M   FM  F R M  M F M  F M F  H v  p H M
HF
 F R M  M F M F M F M F M F  H v  p H  p H 

(5.16)

Substituting from (5.12) and (5.15) gives this form for F H M :
F

M


HF
H    H  R  M F M F M F M F M F  H v  p H  v H 
M

F

M

F

M

F

M

(5.17)

Algorithmically, we can avoid duplicate computations by separating the two rows of (5.15),
and rearranging to use already-computed terms:

43FHM   F R M M HFF

H vM   F R M M H vF  F R M M pF M HF(5.18)

  F R M M H vF  F R M M pF ( M R F F HM )  F R M M H vF  F R M M p F
 ( RF

M M

H  p
F
v

F

M F

 H
F



M

(5.19)

MH )using identity (3.6) in the second-to-last step.
To create an algorithmic version of (5.17), differentiate equations (5.18) and (5.19) in F to
get F H M in terms of already-computed quantities:F

HM   F R M M HF  FM F R M M HF  F R M M HF  FM F HM

F(5.20)

H vM  ( F R M M H vF  F M F R M M H vF  F pM F HM  F vM F HM ) ( F R M M H vF  F M ( F H vM  F pM F HM ) F pM F HM  F vM F HM ) ( R
F

M M

 p
F

H  

M F

F
v

F

M F

H

(5.21)

Mv

H  ( v  F M F pM ) F HM )M

FM

Or, collecting termsF

H M   F R M M H F  FM F H M
0 F M F M
F M
F
M F
M F
M 
 p H  ( v   p ) H 

(5.22)

Simbody uses equations (5.18), (5.19), (5.20), and (5.22) in that order to perform thesecomputations.

5.2 Mobilizers in body frames
At times it is more convenient to deal with the mobilizer hinge matrix describing the allowed
motion of the body frame B with respect to the parent body’s frame P, rather than between
the two mobilizer frames. This is related to the hinge matrix H defined above by the constanttransforms P X F and M X B depicted in Figure 6. First, perform a rigid body shift of thespatial velocity from M’s origin outward to B’s, using the kinematic shift operator  T :
44

F

HB

F
 FV B  T (  M p B )  F H Mu(5.23)

whereFMp B  FR M  M p B1 (  v )  

0
A

B

  v   
A

B



1




Note that although we are shifting from one point on body B to another, the effect is timevarying since we are expressing the shift vector in the parent body using the cross-mobilizer
rotation matrix FR M(q) .
Next, re-express the resulting spatial velocity (currently in F) to P:
P

HB PV B P F F B R  Hu(5.24)

This transformation involves only a constant rotation matrix, and the translation of thereference frame from F to P doesn’t affect the velocity.
The time derivative taken in P is then
P
P

HB

F

B

d P B P F F B P F B
H  R  H   H 
dt

(5.25)

where
F

H

F
F
d F B
H   T (  M p B )  F H M   T (  M p B ) F H M
dt(5.26)and0 (  v )  

0
A

B

    v   
A

B

A

B



0




(5.27)

These matrices are related to the hinge matrix H* in reference 3 as follows:

45

G

H*

  PV B  G P B  H   G R P  P H Bu(5.28)GH*d *H  G RP  P HB  G RP  P HB
dt
GG

(5.29)    H    H 
G

P


P

B

P

B

Note that P H B is not shifted to Ground to form H*, but only re-expressed in Ground. That is,
it still represents motion of B with respect to P (not with respect to G), however it has been
re-expressed in the Ground frame. (Time derivatives are taken in the frame indicated by theexpressed-in frame of the differentiated quantity.)46

6 Theory for Constraints
A Simbody Constraint C is modeled with a set of mC scalar constraint equations which
restrict the allowable values for mobilizer coordinates by enforcing algebraic relationships
among them or their time derivatives. Constraints are usually written to directly affect only avery small number nb of bodies and nm of mobilizers, typically one, two, or three, which wecall the constrained bodies and constrained mobilizers. For efficient processing, Simbody
must know the complete set BkC , M lC  of nbC constrained bodies and nmC constrained mobilizers for each Constraint C. The set of constrained bodies and mobilizers is considered
topological information and is thus frozen after the Constraint is specified.
The set of mobilities which can appear in the corresponding constraint equations consists ofall the mobilities umC associated with the constrained mobilizers, plus all mobilities ubC which
can affect the relative motions of any the constrained bodies. Note that while the number ofmobilities associated with a mobilizer is very small, the number which may affect a set of
constrained bodies can be much larger, potentially including all the mobilities on the paths
from the constrained bodies back to Ground.
To avoid unnecessarily including a large number of mobilities in the constraint calculations
for a Constraint C, Simbody searches the multibody tree from the constrained bodies in the
inboard direction (towards Ground) to find the outmost common ancestor AC, which is themost-outboard (highest numbered) body shared by the inboard paths of all the constrained
bodies. Ground can always serve as A if no other common body can be found. We call thepath from the kth constrained body inward to AC the kth branch of the Constraint; thesebranches may overlap and may also overlap with constrained mobilizers. We call the set of
all generalized speeds on the kth branch ub,C k , with ubC  ub,C k ; the complete set of generalk

ized speeds which can affect Constraint C is then u C  umC , ubC . These are the Constraint’snC  u C participating mobilities. The nqC participating coordinates are similarly defined as
qbC 

k

qb,C k and qC  qmC , qbC , with nqC  qC and nqC  nC .

47

Figure 7 depicts these quantities for a single Constraint C with three constrained bodies. The
figure does not show the mC constraint equations that this Constraint generates; mC can’t bedetermined just from the number of constrained bodies. However, it does show how the
body-affecting mobilities ubC are determined. Note that the mobilizers for the two black
highlighted bodies are shared by branches 0 and 1.

B1

B2

constrained
bodiesB0

branch 1
u b ,1branch 2
u b ,2branch 0
u b ,0ub  ub,0  ub,1  ub,2u  ub  um
outmostcommonancestorA

Figure 7: Constraint topology. This showsa single Constraint C with three constrained
bodies Bk , the corresponding branches, andthe outmost common ancestor body A . The
branches determine the participating
mobilities u b ; mobilities of the two blackoutlined bodies are shared between

Groundbranches 0 and 1. Additional mobilities u m
are introduced explicitly for constrained
mobilizers.

For the rest of this section we’ll drop the superscript “C” except when necessary for clarity.
Without the C, our Constraint generates m constraint equations in n mobilities.
The most fundamental constraint equation is a relationship among the accelerations (the nparticipating generalized speed derivatives u ), called an acceleration constraint. Every48

Simbody constraint equation ultimately restricts accelerations, and these m acceleration
constraint equations form part of the dynamical equations of motion. The ith acceleration
constraint equation has the following form:gi (t , q, u, u) gi u  bi (t , q, u)  0(6.1)

Where g i is a scalar function, gi=gi(q) is a row vector of length n, and bi is a scalar function.
Every defined Constraint must provide a method for efficiently evaluating its m scalaracceleration error functions gi. For constraint equations involving only constrained mobilizers
this can be done directly in terms of the mobilities um. But in the case of participating mobilities ub due to constrained bodies, the constraints are not normally known explicitly as in (6.1)
but rather in terms of some physical consequence of ub , such as body accelerations. The
user-written routine is expected to calculate the error in those terms in constant time, with thephysical consequences of ub having been supplied by Simbody after an O(n) computation.
Similarly, the meaning of the Lagrange multipliers λ is given byfi (q, i ) gTi i(6.2)

where fi is a column vector function giving the n generalized forces generated by the scalarmultiplier λi allocated to the ith constraint equation. Every defined Constraint must provide amethod for efficiently calculating its forces given its m multipliers λ. Again, except forparticipating coordinates due to constrained mobilizers, this is normally not known explicitly
in generalized forces as in equation (6.2), but in terms of forces and torques applied tobodies. The user-written routine is written as a constant-time function in those terms, andthen Simbody converts the result to generalized forces with a single O(n) computation.
Constraint equations may differ in the level at which they are first defined: position, velocity,or acceleration. When a constraint equation is introduced at the position level (such constraints are called holonomic and are typically nonlinear), it is differentiated once to yield a(linear) constraint on velocities, and again to yield a (linear) constraint on accelerations.
When a constraint is first introduced at the velocity level (a nonholonomic constraint, which
can be nonlinear in velocities) it is differentiated once to yield a (linear) constraint on accelerations. A constraint which appears only at the acceleration level (an acceleration-only49constraint; not common) is required by Simbody to be linear in the generalized accelerations
u . Here are the equations defining each of the three types of constraint equation:holonomic (position) constraints (0  j  mp )p j (t , q)  0

(6.3)



p j (t , q, u)

p j u  c j (t , q)  0

(6.4)



p j (t , q, u, u)

p j u  bp, j (t , q, u)  0(6.5)

nonholonomic (velocity) constraints (0  j  mv )v j (t , q, u, u)v j (t , q, u)  0(6.6)

v j u  bv, j (t , q, u)  0(6.7)

a j u  ba, j (t , q, u)  0(6.8)

acceleration-only constraints (0  j  ma )a j (t , q, u, u)

where the row vectors are
p j (q) v j (q) a j (q) 

p j
u

v j
u





p j
u



p j
q

v j
u

a j
u

NC

(6.9)

(6.10)(6.11)and the remainder terms produced by differentiation arec j (t , q)  

p j
t

(6.12)

bp, j (t , q, u)  c j  p j u(6.13)

 v j v j 
bv , j (t , q, u )   

q
q 
 t

(6.14)NC is an nqC  nC matrix assembled from a subset of the rows and columns of the global N
such that qC  NC u C . Note that the equations marked with blue arrows are implied bydifferentiation of the modeled constraint equations; they are not independent. These add
another 2mp+mv equations to the mC modeled ones.
50

All mp rows pj stacked together form matrix PC, and all mv rows vi form matrix VC, which
together are used for initial satisfaction of position and velocity constraints, as well as forconstraint projection during numerical integration. All ma rows aj together form matrix AC,
 PC 
and P ,V ,A stacked together form the m rows of constraint matrix G   V C  as dis AC 
C

C

C

C

C

cussed above. Note that each of the mC rows gi in (6.1) is actually one of the rows pj, vj, or aj.

6.1 Explicit calculation of constraint matrices
For efficient calculation of constraint forces and for performing constraint projections,
Simbody needs to be able to efficiently calculate matrix-vector products involving theconstraint matrices and their transposes. We expect to be able to calculate both Gv and GTw
in O(n+m) time, where G is mXn and v and w are conformant column vectors. (Note that astraightforward matrix multiply would be O(nm), much more expensive.) Simbody uses themethods that define the constraint in combination with O(n) operators to perform thesecomputations efficiently.
With the O(n+m) matrix-vector multiplies available, Simbody can calculate the constraint
matrices P, V, and A (collectively G) explicitly in constant time per element. By making mcalls to the provided routines, mXn matrices can be calculated in O(nm+m2)=O(nm)* time
which is within a constant factor of optimal if you have to form these matrices.
Regardless of whether a constraint equation is initially specified at position, velocity, or
acceleration level it will contribute a row g to the acceleration constraint matrix G above,
which will also be a row of P, V, or A. So all the terms we need can be obtained by examining the constraint equation’s error function once it has been expressed at the accelerationlevel, that is, equations (6.5), (6.7), or (6.8). Taken together, these equations are just theequations (6.1), that is, gi (t , q, u, u) gi u  bi (t , q, u)  0 . So the rows of the explicit matrices
we need are just gi (t , q, u, u) u . An alternative is to use the constraint force functions (6.2)*

because m≤n, mn+m2≤2mn

51

which can equivalently provide a column of gTi at O(n) cost per column. Simbody thus
calculates the constraint matrices a row at a time by mC repeated calls to the O(nC) constraint
force function (6.2), yielding an explicit GC matrix to machine precision in O(mC nC) operations, which is within a constant factor of optimal since the matrix has mC nC elements. The b0 
 from each Constraint form the vector b in equation (6.1), and can if
mC scalars bC  bmC 1 

necessary be determined explicitly in O(n) time using equation (6.1) with all u ’s set to zero.
As discussed above, it is rare that an acceleration constraint equation will be conveniently
written directly in terms of the generalized accelerations u (prescribed motion is an exception). Instead, it will be written in terms of physically meaningful acceleration-derived
quantities involving the constrained bodies. These may be complicated expressions, but they
are always built from the following fundamental quantities:the accelerations of points and angular accelerations of vectors fixed on the constrained bodies, relative to the ancestor or to other constrained bodies in this Constraint, orthe cross-mobilizer accelerations directly in terms of the generalized accelerations uof the constrained mobilizers.

Simbody’s Constraint base class provides utilities to efficiently obtain the constrained
bodies’ accelerations relative to the outmost common ancestor A given a set of u ’s (for fixed
q and u), relative velocities given u’s (for fixed q), and relative positions given q’s. The
user’s constraint equation error functions are written using these utilities.

52

7 State representation and realization
The State concept was presented in Section 1.2. In this section we will take a closer look at
how we represent the state of a Simbody System and how we operate on that state when
performing a Study.

7.1 Computation – realization of the State
This section provides some details about how computations are performed in the SystemState-Study architecture described in Section 1.2.
During a Study, the System is used to realize a State. By realize we mean the process oftaking a new set of values from a State and performing the computations that those newvalues enable. A simple example would be to take new position coordinate values from aState and use them to calculate new spatial locations for the bodies, and then distances
between designated points on different bodies. Realizing a State enables three kinds ofcomputations: responses, operators, and solvers, defined next.
7.1.1 Responses, operators, and solvers
A response is a numerical result which can be computed knowing only the values in theState. The above calculation of distance from position coordinates is an example of a response. An operator is a computation which requires knowledge of certain State variables,
but then can be applied repeatedly to other input data (i.e., data not from the State) to produce
numerical results. For example, once we know positions and velocities from the State, we
can realize an operator which, when applied to a set of forces, efficiently calculates theaccelerations that would be produced by those forces. Neither responses nor operators makechanges to the State. A solver, on the other hand, both reads from and writes to the State. A
given solver requires certain values from the State, and may make use of those values or
responses and operators calculated from them. It then performs a computation which updates
the State in some well-defined way. The simplest kind of solver is a method which just sets aparticular State variable to a given value. A more elaborate example is a solver which takes
current positions from the State and modifies them to find the nearest set of positions which
satisfies particular constraints.

53

Subsystem

Responses
Results

Operators
InputsSolvers

Back to State

Figure 8: After realizing a State, asubsystem provides responses,
operators, and solvers.

7.1.2 Caching of computed results
Realizing a State may require a large amount of expensive computation, and the computed
results are typically used many times in calculation of subsequent results. Consequently it is
crucial that these computations not be re-done once calculated for a particular set of Statevalues. Given that a System is a read-only object, and that realization results are associatedwith a particular State, the obvious place to store these results is in the State object. That way
when a Study provides a State to a System, all previously-calculated results are available aswell, and one may be certain that those results were calculated using the values from the
supplied State. This eliminates the possibility of bugs in which values computed at one state
are incorrectly used as results at a different state. That is an extremely common error insimulation programs and is very difficult to fix, primarily because it often goes completely
unnoticed. Errors of this type are hidden by the fact that sequentially-produced states tend todiffer very little, making the computed values only a “little bit” wrong.
To take a brief pontification opportunity, I want to emphasize in the strongest possible terms that “little” bugs in simulation programs do not leave them “nearly” valid the54

way, say, small measurement errors affect real-world experiments. Simulation software is themost nonlinear thing in existence—one wrong bit in a billion can completely destroy any
relevance it might have had to the real world. The resulting simulation results, unfortunately,
may still appear plausible, especially where human intuition is of limited use such as withmolecular systems. And statistical reduction methods used to calculate physical properties
from a simulation (e.g., population distributions, free energies, radii of gyration, transition times, etc.) are almost certain to turn meaningless garbage into “intriguing” results
which “should be researched further.”
Although cached results are stored in the State object, it is important to note that those results(that is, responses, operators, and solvers) are not logically part of the system state. They aresimply intermediate calculations which have been derived from the state, and can easily bediscarded and re-created when necessary. They are needed only for efficient computation
using the System-State-Study architecture, and so can be viewed as “merely” a hint. They
exist as a kind of shadow behind the actual state variables, whose values do matter. We callthis shadowy construct the realization cache, or more often, just the cache.

55Subsystem

Responses
Operators
Solvers

Realization Cache

State Variables

Figure 9: A subsystem (part of a
System), showing its use of part of theState to realize responses, operatorsand solvers. The realization cache adds
no new information but shadows thestate variables for efficiency.

State

Figure 9 combines the concepts just described. It shows a subsystem (one of the pieces
making up a System) and how its responses, operators, and solvers make use of the realization cache. Note that responses require no input other than the State, while operators andsolvers can have additional inputs (the blue arrows in the figure). Operators and solvers thendiffer by the disposition of their outputs (red arrows), with only solvers’ output able to update
the State.
To summarize briefly: A System (or subsystem) by itself is stateless once constructed. The values of state variables stored in a particular State object
completely determine the behavior of the System. That behavior is producedbyrealizing the State. The results of realization, which are responses, operators,

and solvers, are stored in a hidden cache which is physically contained in the State object, but
is not logically part of the state in the sense that cache values are not permitted to alter the
behavior of the System, except for the speed with which it can perform computations involving that State.
56

7.1.3 Computing in stages
The computations performed by a System in realizing a State are naturally ordered in stages,
and realization is done one stage at a time, in order. For example, one must compute thepositions of the bodies before computing forces that may depend on those positions. This
structure allows for interdependencies among the subsystems in the System, without requiring any subsystem to know any internal details of other subsystems. Of specific relevance forSimbody, user-supplied forces depend on values provided by the Simbody multibody subsystem (such as positions and velocities), but Simbody dynamic calculations (e.g., accelerations)
likewise depend on the user-supplied forces. Thus complete realization of a State requires
sequences like (1) the SimbodyMatterSubsystem realizes its “Position” stage, then (2) eachforce subsystem independently realizes its Position stage to calculate position-dependent
forces (repeat for Velocities), and then (3) SimbodyMatterSubsystem realizes its accelerations (reactions) using computations cached by the force subsystems. This staging approachallows a composite System computation to be performed efficiently from isolated subsystems, with each subsystem mediating access to its own state variables and cache.

Stages1.

Topology

2.

Model

3.

Instance

4.

Time

5.

Position

6.

Velocity

7.

Dynamics

8.

Acceleration

9.

ReportSystem or
subsystem

State

Figure 10: The conceptual organizationof a computation into ordered stages. A
given stage is fully realized by eachsubsystem in a System before the nextstage is realized by any subsystem. For
many purposes, construction of the (readonly) System can be viewed as the initialstage of computation.

Study

57

7.2 State variables
The complete state of a Simbody System is represented by a set
variables. Physically,of broadly-defined stateis stored in a SimTK State object, where it is partitioned into timet  , and two disjoint subsets x, y , so that {t}  x  y . During time stepping, t is

the independent variable; x and y both contain dependent state variables but differ in thetypes of variables they can contain. Partition y contains conventional real-valued, “smooth”
state variables q, u, and z representing system kinematic and dynamic quantities, while
partition x contains state variables of arbitrary value types including boolean, integer, andstructured types of any complexity.
7.2.1 State partitioning by stage
State variables may be usefully partitioned by the computation stages they affect; we call thelowest affected computation stage the state variable’s stage. As shown in Figure 10 stages
that can be affected by state variables inare: model, instance, time, position, velocity,

force, acceleration, and report. (Topology stage is only affected by the contents of the
System, not its State.) We denote state partitions with these effects
pos

,

vel

,

force

,

acc

,

reportmodel,

 xinstance , andreport

,

time

,

with

time

x time {t}pos

x pos  y pos

 x pos  qvel

x vel  y vel x vel  u

force

x force  y force

 x force  z

For the other stages, only variables in the x partition can be used, so we haveinstance

instance

(7.1)

model

 xmodel , xreport ,Partition y consists only of position-, velocity-, and force-stage variables as shown above. For
convenience, we provide more conventional names for those partitions:q  y pos , u  y vel , z  y force
with y  q  u  z(7.2)

q and u are the sets of generalized coordinates and speeds associated with the System’s
mobilized bodies. z is a set of generalized dynamic variables typically belonging to force and58

control elements in the System. z’s can directly affect forces, but not positions or velocities
of bodies.

7.3 State resources
The SimTK::State object manages a collection of resources including state variables andcache entries needed for realization. Resources are allocated at the request of the various
Subsystems, and the State object keeps them organized by Subsystem. In addition, certain
kinds of state resources are aggregated into global resources pools which represent the
System as a whole. Global pools support efficient numerical manipulation of the System’s
state as a whole in circumstances where the Subsystem composition is irrelevant.
Every State resource has an allocation stage, meaning that that resource is allocated during
realization of that stage, and unallocated if that stage is invalidated later. That means that theset of resources in a System’s State can vary based on the current stage, and the settings ofstate variables that were allocated earlier.
Resources that are always present for a given System are allocated at Topology stage. That
includes the set of state variables which can affect modeling options. Then realizing Model
stage causes more resources to be allocated, reflecting the modeling choices made. Some ofthose resources may be state variables representing parameters of the chosen model. Once
those are set, realizing Instance stage allocates the final set of resources needed for further
computation. No state resources may be allocated later than Instance stage; after that only thevalues of existing state resources may be changed.
Here are the resources supported by a State object.
Resource

Allocation

Description

Built-inresources

Topology

Every State has some built-in resources,
allocated at Topology stage. Note that the
System stage can never be higher thanthe lowest Subsystem stage.

Dynamic
variable group(q,u,z)TopologyModelsize: InstanceA contiguous array of real-valued“conventional” state variables. These arepooled into a global array y, which isitself partitioned into global q, u, and z
arrays. An additional state variable holds
the size of the group and can be set at
Instance stage.

Resource notest , t prev
System current stage
Per-Subsystem current stages
Low-water marky  {q, u, z}, qy prev  {q prev , u prev , z prev }sizes
Subsystem-private group Id
Pool slots assignment

59

Structured
variable x

Topology
ModelConstraintgroup (qerr,uerr, udoterr,
mults)

Topology
Model
Instance
m: Instance

Event trigger
group

Topology
Model
Instance
size: Instance

Event group

Topology
Model
Instance
size: Instance
Topology
Model
Instance

Cache entry

A private variable belonging to a single
subsystem and able to contain a designated value type of any kind, from boolean
flag to arbitrary object. Has “invalidates”
stage; must be allocated before that.
Allocates cache entries which arecontiguous arrays of m scalars forholding the current constraint errors andmultiplier values. An additional statevariable is allocated to hold the size m ofthe group which can be set at Instancestage.
Allocates contiguous array of scalarcache entries for event trigger function
values, in global pool. The number of
entries can be changed up to Instancestage.
Allocates a group of event ids for asubsystem, and a corresponding global
pool of System-unique Ids.

Previous value x prev

Private variable belonging to a single
subsystem. Can hold any designatedobject type. Has depends-on stage and “isvalid” flag. Must be allocated before
depends-on stage.

Subsystem-private Id only

Subsystem-private Id only

yerr  {qerr , uerr }uerr , λm
Subsystem-private group Id
Pool slots assignmente, e prev# of triggers
Subsystem-private group Id
Pool slots assignment
Subsystem-private group Id# events

The State maintains mapping information for the global resources so that one can determine
which particular entity is the owner of an entry in a global pool, for any local entity where in
the global pool it may be found.

7.4 Allocation of state resourcesState variables are allocated by the various elements of a System. Here are the Systemelements and the kinds of state variables they allocate:
7.4.1 Mobilized bodies
There are nB mobilized bodies
Each[ j ] (including Ground) .

[ j ] represents a unique body and its mobilizer providing 0  n[ j ]  6 unconstrained

mobilities (degrees of freedom). The ground body G  [0] , and n[0]=0. u[j] and u[ j ] aresets of n[j] scalar generalized speeds and corresponding generalized accelerations defined by[ j ]’s mobilizer. q[j] and q[ j ] are the mobilizer’s nq[j] generalized coordinates and theirtime derivatives, with nq [ j ]  n[ j ] . q and u are related via an nq [ j ]  n[ j ] kinematic coupling60

matrix N[j] such that q[ j ]  N[ j ]u[ j ] . There may be a local quaternion normalization constraint n[j] defined, where n[j] depends only on q[j].
For the System as a whole, we define ordered sets u ju[ j ] and q their elementwise time derivative variables u and q . Sizes are nnq

q[ j ] , and sets ofju   j n[ j ] andq   j nq [ j ] . We also define block diagonal nq  n matrix N=diag(N[j]) so thatq  Nu .
7.4.2 Dynamic variables z
There are nD scalar dynamic variable sets z[i]  z .
Any element in the System may allocate one or more sets of nz[i] scalar dynamic variables
z[i] and their corresponding time derivative variables z[i] . We collect these into nz-element
aggregate ordered sets z and z .
7.4.3 Structured variables dThere are nd structured-value variables d[i]  d .
These variables can be allocated by any element of the System. Each one holds an object of aparticular type, but that type is arbitrary and is different for each variable. These can be as
simple as boolean flags or integers to arbitrarily complex objects.
7.4.4 Constraints
There are nC constraints [i] which can restrict the mobility of the bodies.
Each constraint defines a set of m[i] constraint equations g[i]. These are classified as position(holonomic), velocity (nonholonomic), and acceleration constraint equations and a single
constraint can generate equations at different levels. g[i] is then partitioned into corresponding

subsetsp[i], v[i], a[i]  g[i]ofcardinalitym[i]  mp [i]  mv [i]  ma [i] . We also define aggregate sets

mp [i], mv [i], ma [i]

such

that



 

with

, ,

and

cardinalities mp , mv , ma and m, resp.; m  mp  mv  ma .
A position constraint equation pk 

has the general form qk : pk (t; qk )  0 where qk  q ,that is, it defines an implicit algebraic relationship among a subset of the elements of q,
possibly with an explicit dependence on time. If the set qk contains just one element ( qk  1 ),61then pk defines qk explicitly as qk  qk (t ) ; this is called a “prescribed position”. Each position
constraint pk requires an entry in the global qerr pool in the State, a corresponding entry in
the uerr pool for the time derivative equation pk  0 , an entry in udoterr for pk  0 , and a λslot in the multiplier pool.
Similarly, a velocity constraint equation vk 

is uk : vk (t , qk ; uk )  0 where uk  u , animplicit relationship among a subset of generalized speeds with explicit dependence on t andq. Note that there is not necessarily any correspondence between the sets qk and uk in avelocity constraint; uk can depend on time and any set of q’s. If uk  1 then vk defines uk
explicitly as uk  uk (t , qk ) , this is called a “prescribed velocity”. Each velocity constraint vk
requires an entry in the global uerr pool in the State, an entry in udoterr for vk  0 , and a λslot in the multiplier pool.
Finally, an acceleration constraint equation ak 

is uk : ak (t , qk , uk ; uk )  0 where uk  u ,an implicit relationship among a subset of generalized accelerations with explicit dependence
on t, q, and u. Note that there is not necessarily any correspondence between the sets uk anduk in a velocity constraint; uk can depend on time and any sets of q’s and u’s. If uk  1 thenak defines uk explicitly as uk  uk (t , qk , uk ) , this is called a “prescribed acceleration”. Eachacceleration constraint ak requires an entry in the global udoterr pool in the State, and a λslot in the multiplier pool.

62

8 Equations of motion
In this chapter we’ll present the equations of motion represented by a Simbody System. By
equations of motion we mean the equations that determine the instantaneous rates of change
for the state variables. Integrating those rates of change into a trajectory through time is adifferent topic and will be discussed in Chapter 10.
A few conventions: We use n and subscripted n’s to count quantities related to coordinates(mobilities or degrees of freedom) and m and subscripted m’s to count constraint equations.
We use overdot to represent differentiation with respect to time, in the Ground frame unlessotherwise specified. We use a right superscript to denote a quantity which applies only to aparticular body or its mobilizer.
As detailed in section 7.4.1, the equations of motion will be written in terms of the set of ngeneralized speeds u bu[b] where b ranges over all the mobilized bodies, and the nq

generalized coordinates q bq[b] , where u[b] and q[b] are the disjoint sets of n[b] speeds

and nq[b] coordinates for each body[b] , which arise from the presence of its mobilizer.*

Typically there will also be a set of differential equations associated with force models whichmust be integrated along with the matter model’s generalized coordinates and speeds; we’ll
call these nz auxiliary state variables z. In general a system will also include discrete-timeequations and associated discrete states (we call those “slow” variables) but we’ll only
consider the continuous system here.
The total number n of mobilities in a multibody system is just the sum of the bodies’ individual mobilities, that is n  b n[b] . Note that n is the number of unconstrained system
mobilities; the net number of degrees of freedom after constraints will be nnet  n  mnet
where mnet  m is the number of independent acceleration-level constraint equations generated by the system’s constraints.

*

We use n, representing the mobilizer’s of degrees of freedom, rather than nu to count generalized speeds, since

there is necessarily the same number of generalized speeds as degrees of freedom.

63

Generalized speeds u are fundamentally related to the physics of the system, while generalized coordinates q are chosen primarily to facilitate good numerical behavior during computation. Thus the number of generalized speeds introduced by a mobilizer is always the sameas the number of mobilities so that the generalized speeds are always mutually independent.
The number of generalized coordinates nq [b]  n[b] so the coordinates q[b] may not be
independent. In Simbody, that occurs only when a mobilizer uses a quaternion to represent
unrestricted orientation. For convenience we introduce the symbol nquat [b] defined as follows:1,nquat [b]  0,if mobilizer [b] uses a quaternion
otherwise(8.1)

Then the total number of quaternions in the system is nquat  b nquat [b] .
It should be emphasized that our presentation of the equations of motion below is a formal
description, rather than a computational algorithm. It would be extremely inefficient to set up
and solve the equations in the form they are presented here (although many lesser codes dothat). The techniques of Order(n) multibody dynamics provide the solution of these equations
without ever requiring their explicit formation.

8.1 Unconstrained dynamic systems
In a system with no constraints and where all state variables q, u, and z are defined dynamically by differential equations, the equations of motion areq  N(q)un(q)  0

M(q)u  fapp (t , q, u, z )  fbias (q, u)z  z(t , q, u, z, u)Then a time stepper study seeks to find trajectories q(t ), u(t ), z (t ) where

64

(8.2)
(8.3)
(8.4)
(8.5)

q(t )  q(t0 )  tu (t )  u (t0 )  

t

 t0

 t0

z (t )  z (t0 )  

t

 t0

q( ) d(8.6)

u ( ) d(8.7)

z ( ) d

(8.8)

Simbody forms equations (8.2)–(8.5) analytically while equations (8.6)–(8.8) must be solved
numerically. Time stepping is discussed in detail in Chapter 10.
Here Mnn is a symmetric, positive definite mass matrix in mobility space (u-space) which
captures all the inertial properties of the system in its current configuration, and fapp (nx1) isthe set of all applied force and torques (including gravity) mapped into an equivalent set of ngeneralized forces acting along the mobilities. fbias (nx1) is equivalent to the forces representing velocity-induced coriolis acceleration and gyroscopic terms. (fbias is quadratic in u, and iszero if u=0.) We partition the applied forces fapp as follows:
fapp  fmob  J T  Fbody(8.9)

Here fmob and Fbody are the user-supplied system of forces and torques, while the kinematic
Jacobian J=J(q) is managed internally by Simbody. Fbody is an nBx1 “stacked” vector of
spatial forces consisting of one element per body (that is, the per-body net result of all theforces and moments applied to each body), where each element is a 6-element spatial vector
combining body forces and moments as described in section 3.3.1. Fbody collects user-applied
forces and moments that act on bodies (such as Cartesian forces on atoms). If gravity has
been specified, then Fbody also includes the spatial forces resulting from the gravitationalmodel. fmob is an nx1 vector of user-supplied scalar forces applied directly to the mobilities,
such as would be used for bonded forces in a molecular model. JT• is the Cartesian-tointernal conversion operator, conceptually an nxnB matrix of spatial vectors that maps spatial
forces to their equivalent mobility forces (some authors call J the matrix of “partial velocities”). In practice the JT• operator is an O(n) algorithm, and J is never formed explicitly in
Simbody (unless you ask for it).
N nq n is a block diagonal, invertible mapping between generalized speeds and generalized

coordinate derivatives. In practice this is mostly used to convert angular velocities to scaled
quaternion derivatives or to Euler angle derivatives. The rectangular system of equations
65

represented by (8.2) has rank only n (≤nq), leaving quaternion lengths undetermined, so weneed nquat additional normalization conditions represented by (8.3) to ensure a unique solution for trajectory q(t). Note that although equation (8.3) is formally a set of constraints, weconsider this an unconstrained system since these constraints do not affect the physicalsolution.
Equation (8.4) is just a version of Newton’s second law F=ma, relating forces to accelerations. The z’s are nz additional state variables whose values can affect the forces, which maythemselves be modeled as differential equations. z’s cannot directly affect positions andvelocities, although of course they do affect accelerations which will ultimately affectvelocities and then positions. Note that z’s time derivatives z can depend on u but not viceversa.
Formally, we can solve equation (8.4) for the accelerations u withu  M1 (fapp  fbias )(8.10)

By formally we mean, “don’t actually do it that way!” There is always special structure to M
that can be exploited such that the accelerations can be calculated directly in O(n) time, whilea literal matrix inversion would take O(n3) time and be prohibitive for large systems. Even
forming M would take O(n2) time since it has (roughly) n2/2 unique elements, so Simbody
neither forms nor factors M while solving equation (8.10).
As an extreme example, consider the special case of a molecular system modeled with na
point mass atoms and Cartesian coordinates, so that n=3na. M is then a diagonal matrix ofdimension 3na3na with the atomic masses (each repeated three times) arrayed along the
diagonal. The q’s are the Cartesian x,y,z coordinates, and the u’s are the Cartesian velocities
so nq=n (=nu), N is an identity matrix, and q  u . fbias is always zero for this system. fapp issimply the Cartesian forces acting on each coordinate of each atom, typically resulting fromtaking the gradient of the potential energy function. This represents a set of 3na uncoupledscalar equations for the Cartesian accelerations of each atom, which can clearly be solved inO(n)!
In a more general multibody system M will be dense as a result of coupling produced by theinternal coordinates. Use of quaternions for orientation results in there being more q’s than66

u’s and N is no longer identity and in fact not even square. However, equation (8.10) provides the solution for the accelerations in this case just as well, and the special structure ofmultibody systems permits a solution in O(n) time regardless of the amount of coupling in
M.

8.2 Constrained systems
Constraints introduce unknown forces and moments into the system. Constraints are introduced, for example, if there are topological loops created by the set of bodies and joints. The
constraint forces involve additional unknowns (along with the non-prescribed accelerations).
We call these unknowns Lagrange multipliers and represent them as a vector λ of length m.
These are mapped to mobility forces with a coupling matrix G and thus modify acceleration
equation (8.4) like this:Mu  GT   fapp  fbias(8.11)Gu  b

(8.12)

where Gmn=G(q) and bm1=b(t,q,u), m is the number of constraint equations and n=nu is thenumber of generalized speeds. Equations (8.11) and (8.12) are a system of n+m equations in
n+m unknowns ( u and  ) so can be solved for the accelerations that satisfy the constraint
equations. The solution of this system makes use of the unconstrained result from equation(8.10). Note that because we can directly solve for u and eliminate λ, this is still just an
ordinary differential equation, with u  u(t , q, u, z) .*8.3 Unconstrained systems with prescribed, fast, and slow variables
A system may have motion where some or all of the generalized accelerations u are known
as functions of time and state, but the corresponding generalized forces are unknown. In this*

Knocking equations (8.11) and (8.12) around a little, one can verify that u  u0  uC , where1 T
u0  M1 (fapp  fbias ) , uC  M 1G T  , and   (GM1G T )1(Gu0  b) . In general the constraint matrix GM G canbe singular, so there may be no solution, or an unlimited number of solutions, in which case Simbody will
provide a least squares solution for λ.

67

case we’ll partition the generalized accelerations u as u  u p , u f  where u p are the n p
prescribed accelerations and u f are the n f free accelerations driven by forces. (For notational convenience we’ll treat these as though all prescribed variables follow any dynamic ones,
although that ordering is not required in practice.) We then use  (n p 1) to represent theunknown generalized forces associated with prescribed generalized accelerations u p . Any
accelerations that are associated with fast or slow variables are prescribed to be zero.
Equation (8.4) is then replaced with equation (8.13) where there are additional unknowns forthe generalized forces that implement the prescribed motions:uf  0 
M(q)     f   fapp (t , q, u, z )  fbias (q, u ) up    (8.13)

upd[i]  presu [i](t , q pf , u pf ),u p [i]   p
0,
upd[i]  fast or slow


(8.14)

z[i]  z[i](t , q, u, z, u, )(8.15)For a subset of the prescribed accelerations u p , we may also know the corresponding
generalized speeds u p analytically as a function of time and position, otherwise the generalized speeds will be free variables u f produced by numerical integration of u p . Similarly, fora subset of the prescribed speeds we will also know prescribed coordinates q p as a functionof time only; otherwise the coordinates will be part of the free position variables q f .
The trajectory equations (8.6) and (8.7) still hold for the free and prescribed dynamic variables, but only the free variables q f , u f , z need to be solved via numerical integration:

68

t


 q f (t )   q f (t0 )   t q f ( )d 0q p (t )  
q p (t )
q(t ) 
 q fast (t )  relax(
q
)
fast

 

(k )
 qslow (t )  
qslow



(8.16)t
 u f (t )   u f (t0 )   t u f ( )d 0u p (t )  u p (t , q pf )u (t )  u fast (t )   relax(q ; u ) 
fast
fast



(k )
 uslow (t )  
uslow



(8.17)z (t )  z (t0 )  

t

 t0

z ( )d

(8.18)

Note again that the partitioning of q, u, and u into free and prescribed variables can differ
because a higher-level coordinate (position or velocity) may need to be produced by integration even if a lower one (velocity or acceleration) is prescribed.
To solve for the dynamic unknowns u f and  , we view the mass matrix and right hand side
as being composed of partitions corresponding to the free and prescribed variables like this: M ff
 T
 M fpM fp  u f   0 f   f f   f bias,f      

M pp  u p      f p   fbias,p (8.19)

Multiplying out the blocks and moving the known quantities to the right hand side givesu p  u p (t , q, u)(8.20)

M ff u f  f f  fbias,f  M fpu p(8.21)

  f p  fbias,p  MTfpu f  M ppu p(8.22)

These equations can be solved recursively in O(n) time by the method described in reference
5.

8.4 Constrained systems with prescribed motion
Prescribed motion is similar to a constraint, and in fact one of Simbody’s built-in Constraint
types implements prescribed motion that way. There are two differences between a constraint-based prescribed motion and the direct form described here:69

1. Direct prescribed motion takes priority over any constraints. That is, first prescribedmotion is satisfied, and then constraints are satisfied by modifying only the unprescribed variables.
2. Direct prescribed motion can be implemented much more efficiently. In fact, adding
prescribed motion this way speeds up computation rather than slowing it down, because inverse dynamics is faster than forward dynamics.
With prescribed motion some of the generalized accelerations u are known explicitly as
functions of time, q, and u. As was done in section 8.3, we’ll partition the u ’s and generalized forces into two groups, subscripting those associated with prescribed motion with a p,
and the free (force-driven) ones with an f: u  u f , u p  , f  f f , f p  with u f ,  and  being
the unknown dynamic quantities. Substituting into equations (8.11) and (8.12) gives
uf 
0 
M    G T    f   f  f bias
  up 

(8.23)

uf G   b up 

(8.24)

Now we’ll partition M and G into blocks corresponding to the free and prescribed variables
as follows: M ff
M T M fpM fp 
 , G  G f
M pp 

Gp (8.25)

Partitioning the right hand sides analogously, the equations of motion are now M ff
 T
 M fpu p  u p (t , q, u)(8.26)

M fp  u f   G Tf 
 0 f   f f   fbias,f     T          M pp  u p   G p     f p   f bias,p (8.27)

G

f

uf 
G p    b
 up 

Multiplying the blocks out and moving the known terms to the right hand side gives70(8.28)

u p  u p (t , q, u)(8.29)

M ff u f  G Tf   fˆ f

(8.30)

G f u f  bˆ

(8.31)

  fˆp  GTp (8.32)

wherefˆ f  f f  fbias,f  M fpu p(8.33)bˆ  b  G pu p

(8.34)fˆp  f p  fbias,p  MTfpu f  M ppu p(8.35)

Equations (8.30) and (8.31) are solved for u f and  in the same manner as equations (8.11)
and (8.12), using the known value of u p to evaluate the right hand sides in equations (8.33)
and (8.34). Then the resulting values for u f and  are substituted into equations (8.35) and(8.32), giving the final unknown  .
 M ff1fˆ f 
 u f ,0   dyn(f , u p )   ˆ

f
(
u
)
 0 
 p f ,0 

(8.36)

 u f ,0 
(G f M ff1G Tf )  aerr   G f u f ,0  bˆu
 p 

(8.37)

 M ff1 (fˆ f  G Tf  ) 
uf 
T

   dyn(f  G  , u p )   ˆ
T


f
(
u
)

G

 
p
 p f


(8.38)As before the only matrix we must form and factor explicitly is the mxm possibly-singularmatrix G f M ff1G Tf , which takes worst case O(m  n f ) time to form and O(m3 ) time to factor,
with the worst case occurring when all the constraints are coupled. All the other matrixvector multiplies can be performed with recursive O(n) and O(m) operators and the prescribed constraints do not contribute to this matrix.

71

The operators we have available for performing the above calculations are as follows.
Prescribed motion provides direct calculation of u p (t , q, u) and applied forces, converted to fr 
equivalent generalized forces f (t , q, u, z )    are available also.
fp 

From the multibody system we have these two O(n) operators:
1 u   M ff (fr  f bias,f  M fp u p ) dyn(f , u p )   f   T    f p  fbias,p  M fp u f  M pp u p 

(8.39)

 u f   M ff1 0   f f minv(f )      0 fp  0p   0(8.40)

From the system of constraints we also have two O(n) operators:
aerr (u )   G f

uf 
G p    b
 up 

(8.41)

 fcons,f   G Tf frc( )     T  
f
cons,
p

 Gp (8.42)

These are used to calculate all the above terms efficiently, especially the m  m matrix
Wf  G f M ff1G Tf which we calculate by noting

Wf  G f M ff1G Tf   G f
where

Gp X

 M 1G T   M 1 0 fp  G Tf 
Xnm   ff f    ff

 0
 
 T 
 pm   0 pf 0 pp  G p (8.43)

Our goal is to calculate columns of X one column at a time and then use the aerr() operator
to calculate a column of Wf.
First make one call to the dyn() operator (8.39) to calculate u f ,0 in equation (8.36). Then
make one call to the aerr() operator (8.41) with a zero argument to calculate the bias term b:
0f 
aerr0  aerr    b
 0p 

Then use a call to equation (8.42) to form columns of G T explicitly, one at a time:72(8.44)

GiT  frc( i )  G T  i(8.45)

where  i is a unit vector of length m with the ith element 1 and the rest 0.
We then call the minv() operator in equation (8.40), followed by a call to aerr() to calculate acolumn Xi :and then M ff1G iT 
T
Xi    minv(G i )
 0
p



(8.46)Wf (i)  G f Xi  aerr (Xi )  aerr0(8.47)

to build up Wf  G f M ff1G Tf in O(m  n) operations as needed to solve equation (8.37) for aleast squares value for  . That is used to calculate constraint forces GT  using the frc()
operator again. These are subtracted from the applied forces for a final call to the dyn()
operator to solve equation (8.38) for u f and  .

8.5 Constrained systems as specified to Simbody
Equations (8.12) and (8.31) are written in terms of linear constraints on the accelerations u .
However, in most cases general constraints are known only at the configuration level, that is,
as nonlinear algebraic relationships which must hold among the q’s or among quantities fully
determined by the q’s. A constraint like “these two atoms must be a certain distance apart at
all times” would be an example. In other cases the constraints may be expressed at thevelocity level as restrictions on u. In these cases we time-differentiate the constraints twice oronce, resp., until we have corresponding acceleration constraints, and then use them inequation (8.12) or (8.31), along with any constraints which may have been defined directly at
the acceleration level. Similarly in the case of prescribed motions given at the position orvelocity level, we differentiate twice or once and then use the result as equation (8.29).
Following this procedure yields correct accelerations, but with approximate numericalintegration of those accelerations the original position or velocity constraints will not remain
satisfied over time. In practice, any constraints that are not actively enforced will graduallydrift apart during a dynamic simulation. To address this, we must keep the original algebraic
constraints in the problem and solve them along with the ODE (8.11), (8.12). That results ina system of mixed differential and algebraic equations, known as a DAE. Equations (8.48)73

(8.54) shows the complete set of equations, including the set of auxiliary, unconstrained
differential equations in z which may be required in the computation of forces.

q  N(q)un(q)  0
M(q)u  GT   f (t , q, u, z )  fbias (q, u)(8.48)(8.49)(8.50)a(t , q, u; u)  0(8.51)v(t , q; u)  0(8.52)

p(t; q)  0(8.53)z  z(t , q, u, z)(8.54)

Constraint coupling matrix Gmxn is obtained from equations (8.51)-(8.53) as discussed below.
Note also that the constraint equations (8.51)-(8.53) combine both general constraints andprescribed motion constraints, each subject to different restrictions on the allowable forms
for the constraint functions. We are showing only dynamic variables here; there are also
relaxation conditions for fast variables and discrete conditions for slow variables.
Equations (8.48)-(8.54) show the system as it is defined to Simbody, including all the constraints that must be obeyed during a dynamic simulation, starting with initial conditions t0,
q(t0), u(t0), z(t0) such that constraint equations (8.49), (8.52), and (8.53) are satisfied.
The function a ma 1 in equation (8.51) specifies ma acceleration (index 1) constraints, which
are required to be linear in the accelerations u , with maxn coefficient matrix A. These haveapplication, for example, in some models of Coulomb friction6 and in producing simulations
which must track measured accelerations or reaction forces. It is common to prescribe an
acceleration to zero to temporarily lock a joint.
The function v mv 1 in equation (8.52) specifies mv nonholonomic (velocity, index 2) constraints (usually, but not necessarily, linear or quadratic in u). These include, for example,“non slip” constraints like gears and rolling contact, and constraints involving kinetic energy.
The mv time derivatives v of the nonholonomic constraints v must also be obeyed since, likea, they restrict the allowable values of u and in general they will be coupled to a. Prescribedvelocity is often used to force a body to rotate at a constant rate.

74

The function p mp 1 in equation (8.53) specifies mp holonomic (position, index 3) constraints,
which are arbitrarily nonlinear in t and q. The mp time derivatives p , and mp second time
derivatives p must also be obeyed since they impose restrictions on u and u , respectively,
and in general will be coupled to v and a.
Then a, v , and p together constitute the acceleration-level constraints, so we have
m=ma+mv+mp as the total number of constraints at the acceleration level.
The system of equations (8.48)-(8.54) contains nq+nu+nz+m equations in the nq+nu+nz+m
unknowns q,u,z and λ, and should thus yield a unique solution for the resulting trajectories
q(t), u(t), z(t) and λ(t), given consistent t0, q(t0), u(t0), and z(t0) to start with. Unfortunately,
obtaining that solution is easier said than done! Numerical analysts describe a system likethis as a Differential Algebraic Equation (DAE) system of index 3, for which few entirely
satisfactory solution methods exist. For a survey of methods, see reference 7. For Simbody
we advocate and actively support the method known as coordinate projection,8 which is very
accurate and reliable in practice. It is also possible to use the popular but less robust technique called Baumgarte stabilization9. In the next section we’ll discuss how we go aboutsolving equations (8.48)-(8.54).

8.6 Unilateral constraints
Normally a constraint generates whatever constraint forces are necessary in order tosatisfy the acceleration-level expression of that constraint. Some constraints arelimited, however, in the forces they can generate. Most commonly this occurs
when constraints are used to represent contact between bodies. In those cases, the constraint
can produce “pushing” forces but not “pulling” forces. Other common examples are joint
stops, ratchets, and ropes. These constraints are capable only of producing forces λ of one
sign, and their characteristic constraint equations are inequalities rather than the equalities
shown in equations (8.51)–(8.53). By convention we’ll require that all the constraint errors
and λ are nonnegative; if they are actually nonpositive we can negate them so that we only
need to consider nonnegativity here. Now when solving for the constraint forces λ we mustfirst determine which of the unilateral constraints are active. These extra unknowns (that is,whether each constraint is active) are resolved by a complementarity condition that must hold75

for every acceleration-level unilateral constraint equation: if the force is nonzero then theconstraint error must be zero (constraint is active), or if the constraint error is nonzero thenthe force must be zero (constraint is inactive). Thus the product gi i  0 for every unilateral
constraint equation i, where g i is the acceleration-level constraint error (i.e., it is ai , vi , orpi depending on the kind of constraint). So we have the conditions
gi  0, i  0, gi i  0 i

(8.55)

Combined with the always-active acceleration-level equality constraint equations, (8.55) is in
the form of a mixed complementarity problem (MCP). In cases where g i and i are linear
functions, this is a (mixed) linear complementarity problem (LCP) for which very efficientsolution methods exist. In practice, all our inequality constraints will be linear except for
friction, which can be quadratic.
An enabled, unilateral holonomic constraint will be in one of these seven states:Stateviolated
separated
separating
inactive
impactingcandidateactiveConditionsp  tolp  tol

ActionIllegal; must correct.

p  tol ,p  vtolp  tol ,p  vtol ,Ignore constraint.
p  0 and   0

p  tol ,p  vtol ,p,  unknown

Perform impulsive MCP; newp  vtol  separating or
candidate.
Perform MCP to determine p, p  tol ,p  vtol ,p  0 and   0

Obey constraint.

p  tol , p  vtol
Or, impact declared by time stepper.

Holonomic unilateral constraints can generate impulses, that is, discontinuous changes to thevelocity state variables due to impacts. The response to an impact requires user specificationof the amount of dissipation that should occur; this results in a coefficient of restitution
0  e  1 that is used to impulsively change the velocities. If the resulting rebound velocity issmall enough, the constraint becomes a candidate to become active; otherwise the surfaces
separate after impact.

76

An enabled, unilateral nonholonomic constraint will be in one of these five states:Stateviolated
separatedinactive
candidateactivev  vtolv  vtolConditions

ActionIllegal; must correct.

v  vtol , v  0 and   0

Ignore constraint.

v  vtol , v,  unknown

Perform MCP to determine v, v  vtol , v  0 and   0

Obey constraint.

No state variable discontinuity occurs when a unilateral nonholonomic constraint becomes
active, although the acceleration will have a jump.
An enabled, unilateral acceleration-only constraint will be in one of these three states:Statecandidate
inactive
activeConditionsa,  unknowna  0 and   0
a  0 and   0

ActionPerform MCP to determine a, 
Ignore constraint.
Obey constraint.

Note that a unilateral acceleration-only constraint is always a candidate and can never beviolated.
8.6.1 Solving for impacts
When we have determined that an impact has occurred, we need to make a discontinuouschange to the system’s generalized speeds to avoid violating constraints. These speed changes result from applying an impulse to the system. An impulse is the integral of the forces
applied during the collision, which is considered to be infinitesimally fast for rigid contact sothat the system configuration (value of q) does not change during the impact. Further, during
an impact we assume that all applied forces are insignificant except contact forces, so only
those contribute to the impulse integral. Contact forces result either from the enforcement ofconstraints (including unilateral and bilateral constraints and mobilizers), or from the application of sliding friction forces.
Impact events have a coefficient of restitution e that determines how much energy is dissipated during the collision. When e=1, the collision is conservative, and when e=0 it is maximal77

ly dissipative. In between some fraction of the energy is lost and there are three common
ways to interpret e: as the ratio of rebound speed to impact speed (Newton’s coefficient); as
the ratio of expansion impulse to compression impulse (Poisson’s coefficient); and as thesquare root of the ratio of the final energy to the initial energy (Stronge’s coefficient). These
are all equivalent in simple collisions, but for multibody systems Newton’s interpretation willoften produce non-physical behavior. Poisson’s produces consistent behavior though Stronge
claims that only his interpretation can be guaranteed not to add energy. It is not clear whether
Stronge’s interpretation can be implemented in practice, however, so we use Poisson’s.
The basic procedure for a frictionless impact is as follows:
1. Determine that a contact event has occurred at time t  that would violate a constraintc.
2. Determine the collision velocity that must be eliminated to avoid constraint violation,
call it vc (< 0). If vc is below a small “capture velocity” we set coefficient of restitution e=0, otherwise we use a supplied value or velocity-dependent calculation for e.
3. Compression: calculate the consistent impulse that would just drive vc to zero, callthat I. (By consistent we mean that it satisfies the equations of motion and all constraint conditions.)
4. Expansion: Apply the impulse (1+e)I and calculate the resulting change in generalized speeds Δu.
5. Update u   u   u and use that to calculate the post-impact velocity vc .
6. If vc  0 (to a tolerance), examine its time derivative ac . If ac  0 activate constraintc.
Assume first that only constraint c is unilateral and that all other constraints are active. We
need to integrate the equations of motion over the infinitesimal interval t  to t  . Since timeand state don’t change during the impact, only contact force-dependent terms can change
during the integration interval. That leaves us with these impulsive equations of motion:78

u p  0
M ff
M T
 fp

(8.56)

M fp   u f  G Tf
M pp   0  G Tp

cTf  0   fI , f   f    T Ic p 
  I   fI , p (8.57)

G f
c
 f

G p   u f   0 c p   0   vc (8.58)

where the unknowns are shown in red. These are u f , the change in generalized speeds, I ,the constraint impulses, and  I , the prescribed motion impulses. The fI are the sliding friction
impulses applied during the impact. We have added rows to the constraint matrix corresponding to the new constraint c, with right hand side set to the change we want to make in theconstraint violation velocity. For Poisson restitution, we will always want to calculate theimpulse that will make vc  0 . Note that the prescribed motions in (8.56), applied and inertial
forces in (8.57), and the right hand side of (8.58) are all zero here, but otherwise these are thesame as the acceleration-level equations of motion with the unknowns relabeled. Thus we
can solve these equations using the same operators and techniques as for the original equations. Multiplying these out we get
0




WI   G f  1
M f  vc  c f  ff I , f

 


cTf  I )

(8.60)

 I  f I , p  MTfp u f  G Tp cTp  I(8.61)

G f 
W    M ff1 G Tf
 cf 

(8.62)

u f  Mff1 ( f I , f  G Tf

where(8.59)

cTf 

Note that we do not need to determine the prescribed motion impulses in order to solve forthe constraint impulses and changes to the generalized speeds. As before, W may be singularso we choose a least squares solution to I ; any choice for I that solves (8.59) will produce
the same u f .
8.6.2 Sliding friction forces and impulses
Using Coulomb’s friction law results in sliding friction forces or impulses of the form
79

Fslip   e(8.63)

with sliding friction multiplier σ a nonnegative scalar and e a unit vector in the slip direction(or impending slip direction). The sliding friction multiplier is defined as  N(8.64)

where scalar N≥0 is a normal force or impulse magnitude and μ≥0 is a scalar (often velocitydependent) representing the coefficient of friction. The dimensionality of slip direction e
determines the dimensionality and meaning of the friction force Fslip – for example, e may be
a scalar (a generalized speed), a translational 2-vector (planar friction), or a rotational 3vector such as the angular velocity of a ball joint. These would produce friction forces thatare: a generalized force, a planar force, or a 3d torque, respectively.
8.6.2.1 Coefficient of friction
The coefficient of friction μ in general may be velocity-dependent. Pure Coulomb friction
switches discontinuously between two values, a static coefficient of friction μs used whensticking, and a dynamic coefficient μd ≤ μs used when sliding. Continuous schemes may
blend these values based on velocity. Coefficients of friction may also change with time andposition, but must not depend on accelerations or constraint forces. A typical form for sliding
friction (where v is the slip velocity vector) is:   d  v v

(8.65)

where μv is a viscous coefficient of friction. For any given pair of contact surfaces, thesecoefficients must be calculated via some combination of the two sets of surface properties.
8.6.2.2 Normal force
N may depend nonlinearly (quadratically) on mobilizer or constraint reactions, which are inturn dependent on the calculated accelerations u and Lagrange multipliers λ. Because these
may then depend on friction forces, the equations of motion can become mildly nonlinear
and some iteration may be required to find a compatible set of accelerations, multipliers, andsliding friction forces for a given set of active constraints. During impact, similar iteration
may be required to find compatible velocity changes, impulse multipliers, and impulsive
sliding friction forces. We expect this iteration usually to converge very rapidly for several
reasons: the nonlinearity is very mild, at most the magnitude of a vector; friction forces are
80

orthogonal to N’s application direction so coupling must be indirect through the multibody
system; and we generally have a very good starting guess from the normal forces at theprevious time step. Nevertheless the coupling can be strong at times and there are circumstances in which convergence cannot be obtained without an impulsive change to velocities(see Painlevé’s paradox).
8.6.2.3 Sliding friction with linear dependence on multipliers
When each sliding friction multiplier  j is a linear function of a constraint multiplier  j  , as is
the case for point contact between surfaces, the equations of motion remain linear, althoughwith a switching condition (a discrete nonlinearity). There are two cases, depending onwhether the constraint multiplier belongs to a bilateral or a unilateral constraint, as illustrated
below.

σjσj

 j  0 j  0 j  0 j  0

λj

λj

λj

λj

j {bilateral}j {unilateral}

Figure 11: Dependence of sliding force magnitude σ on normal constraint multiplier λ.

Friction force multipliers are required to be nonnegative, but the constraint multipliers they
depend on are signed. This is true even for unilateral constraints here, because we are assuming a given active set (all active constraints are treated as bilateral). If that results in violationof a unilateral constraint (i.e., a negative constraint multiplier), a higher-level algorithm willrevise the set of active constraints; we do not make that choice here.
The friction force multipliers are given by81j  {bilateral}  j  j ,  j max( j  ,0), j  {unilateral}j (8.66)

where j  is the index of the constraint multiplier on which sliding friction force j depends.
There are a total of k sliding forces and m multipliers (and n generalized speeds). For convenience we’ll arrange the j  j  mapping in a kxm matrix T, which is all zero except thatelement T[ j, j ]  1 for j=1..k. Then Tλ selects just the k multipliers for which there are sliding
forces and reorders them if necessary. (T can select the same multiplier more than once if
several sliding forces depend on it, but that is unlikely.)
We can view this as a collection of linear equations, only one of which is active for each j,depending on the sign of  j  :  j  j , j    j  j  , 0,

 j  0 j   0 and j  {bilateral} j  0 and j  {unilateral}(8.67)

We don’t know in advance what the signs of the multipliers will be, so we now have k
additional unknowns d j  sense( j ) {1, 1,0} giving the sense (“direction”) for each sliding
force equation, definedsign( j  ),
d j  sense( j )  
max(sign( j ),0),j  {bilateral}j  {unilateral}(8.68)

where sign(x) is 1 if x≥0, −1 otherwise. Note that d does not represent sliding directions; it isjust the sense of the relationship between the normal constraint multipliers and their sliding
force multipliers. With that definition we have j  d j  j  j  ( 0)(8.69)

Assemble the sense scalars into a kxk diagonal matrix D=diag(dj). Given the right values ford, we can write the sliding friction magnitudes as the set of linear equations  DμT ( 0)(8.70)

where σ is a kx1 column vector containing the sliding friction force multipliers and
μ=diag(μj) is a kxk diagonal matrix. Now let ST be the matrix mapping those magnitudes to82generalized forces (that is, S=S(t,q,u) is a kxn matrix formed from the slip directions v andkinematic Jacobian). Then the equations of motion are
Mu  G T   ST   f
Gu  b  DμT

(8.71)d  sense(T ), D  diag(d )Given fixed values for d, the first three equations above define a linear subproblem for the
unknowns u,  ,  . Substituting the third line of (8.71) into the first eliminates σ:
Mu  (G  TT DμS)T   f
Gu  b

(8.72)

Then multiplying through by the inverse mass matrix and substituting eliminates u :W  GM 1 f  b
where

W  GM 1 (G  TT DμS)T(8.73)

This system is likely to be underdetermined, but the least-squares value of λ is the uniquesolution we want:  W (GM1 f  b)

(8.74)

where superscript + indicates pseudoinverse. This is identical to Simbody’s usual calculationof λ except for the new term TT DμS , which makes W non-symmetric. That doesn’t matter forus since we can’t easily exploit symmetry anyway here due to the potential for redundancy.
Now we can recalculate d and check whether we used the right set of equations. If so, then  DμT are the correct, nonnegative sliding force multipliers. If not we’ll have to revise dand re-solve the modified linear system.
In the case where we have chosen d incorrectly, we will have errors in σ due to having
chosen the wrong linear equation from (8.67). For bilateral constraints we will have had d=1
when it should have been −1 or vice versa; for unilateral constraints we will have had d=0
when it should have been 1 or vice versa. That means an incorrect choice leads to twice theerror for bilateral constraints as for unilateral ones. You can see that easily in Figure 12
below.

83

σj

σj

σerrj

σerrj

Bilateral

Unilateral

Figure 12: The error in a sliding friction force multiplier when the wrong linear equation ischosen for it. The dotted blue lines show the incorrect values of σ. Note that the error is twiceas large for bilateral constraints as for unilateral ones.

We propose the following algorithm to solve this system [TODO: is there a better way?]:
1. Assume we are given an initial set of signs dj (these will typically have been saved
from the previous time step).
2. Solve for λ using (8.74), then define derr such that derrj  d j  sense( j ) {0,1, 2} . This
will be 2 for an incorrect bilateral constraint, 1 for an incorrect unilateral constraint,
and zero if dj was correct.
3. If derr=0 we are done since we used the correct set of linear equations. Go to step 5.
4. Otherwise, from all j for which derrj  0 , choose the one for which  errj  derrj  j  j 
is largest, then revise dj using (8.68), and repeat from step 2. [TODO: is this the right
way to choose j?] Note that we only revise one sign at a time since all the unknowns
are coupled in general—changing more than one at a time can cause failure to converge. [TODO: what can we say about convergence and uniqueness?]
5. Verify that we have a solution. If not, the system is inconsistent which means there
must be an impulsive velocity change to change a sliding contact into a sticking one;the impact handler should be invoked.
This same set of equations with relabeling and removing non-impulsive forces can be used
for impact also:84Mu  G T   ST   0
Gu  0  DμT

(8.75)d  sense(T ), D  diag(d )where the unknowns are the set of impulse multipliers π (and σ), the velocity change Δu, andthe signs d.
8.6.2.4 Sliding friction with nonlinear dependence on multipliers
A bilateral constraint implemented with several multipliers may have sliding friction that isdependent on a normal force that is the magnitude of a vector. Examples are a 2-multiplierpoint-on-line constraint (with sliding friction along the line), or joint-like constraints such asa 3-multiplier ball joint constraint (sliding friction torque vector opposing angular velocity)or 5-multiplier pin joint constraint (sliding friction generalized torque about rotation axis). In
these cases the normal force scalar of interest is the norm of a vector which is some projection of the constraint reaction force. Then we can write the magnitude σj≥0 of the jth sliding
friction force as j   j N j ( j  )where

N j ( j  )  A j  j (8.76)

2

Subscript j  is now the set of multiplier indices on which σj depends and  j  is a vector,
typically with 2 or 3 elements; conceivably it could contain up to 5. Aj=Aj(q) provides
rotation, scaling, and projection as needed. Note that Nj(0)=0; you can’t have a sliding
friction force without a normal force. However, Aj is not necessarily full rank so A j  j  may
be zero even if  j   0 .
Now the equations of motion become:Mu  G T   ST   f
Gu  b  μN ( )(8.77)

where each element of vector function N is one of the Nj and μ=diag(μj). Given an estimate λifor λ, we can approximate N locally to first order as85

N ( i   )  N ( i )  N i   O( 2 )
N i

where

N
   i(8.78)(k  m)From (8.76) we can determine the general form for a block of the derivative:
N j
 j 

ATj A j  i j  

A j ii(8.79)

2

Note that this is undefined when A j  i  0 , requiring special handling there.
Define  i  μN ( i ) . Assume we have an initial estimate λ0, perhaps saved from the previous
time step, and let  0  μN ( 0 ) be our initial estimate of the sliding friction magnitudes. We
can refine this estimate using Newton iteration, terminating when
 i 1   i   tol(8.80)

for some tol based on user-requested accuracy. Substituting into (8.77) gives
Mu i 1  G T  i 1  ST  i 1  f
Gu i 1  b(8.81)

 i 1   i  μN i 
where i 1   i  

The unknowns are u and  . Rearranging, we get
Mu i 1  (G  N i T μS)T   f i
Gu i 1  b
where(8.82)f  f G  S 
i

T

i

T

i

We can solve now as for the linear case:  Wi  (GM 1 f i  b)
where now

Wi  GM 1 (G  Ni T μS)T

(8.83)

That gives us λi+1 so we can calculate  i 1  μN ( i 1 ) and check for convergence in (8.80). If
we converged, then the solution is [u, , ]  [ui 1 ,  i 1 ,  i 1 ] .
Note that the above describes a full Newton iteration, where the Jacobian is recalculated witheach iteration. Because of the mild nonlinearity, we are likely to be able to converge quickly
with a modified Newton, using only N 0 and W0 unless one of the λ changes sign. However,as mentioned above, Jacobian Nλ will be undefined at Aλ=0.
86

8.6.2.5 Stiction to sliding transition
When a contact was sticking but first begins to slide, there is no slip velocity to use todetermine the sliding force direction. Instead we want the sliding force to oppose the (unknown) impending slip direction. One reasonable approach is to record the last known
stiction force direction and use that as the initial sliding force direction. Provided that the last
stiction force and first sliding force are about the same, this is likely to be a good direction.
However, it is possible that the stiction force exceeded its limit by a lot, and the coefficient ofstatic friction may be much larger than the coefficient of dynamic friction, so there may be alarge change in magnitude at the stictionsliding transition. This may cause a change indirection that could make the final stiction force direction differ substantially from thedirection of impending slip.
A single friction element can be viewed as having three distinct “phase”:
1. Stiction. The sliding velocity is zero; the magnitude and direction of the stiction force
that will keep the sliding acceleration at zero are both unknown. This is implemented
as a set of workless constraint equations. If the magnitude exceeds μsN, we transition
to impending slip.
2. Impending slip. The velocity is currently zero but the acceleration is not constrained.
The magnitude is known as a function of the (unknown) normal force (μdN), and the
direction should oppose the (unknown) acceleration.
3. Sliding. The magnitude is known as a function of the (unknown) normal force, andthe direction is known as a function of state (it opposes the sliding velocity).
Here are the equations of motion with an impending slip friction element:
Mu  G T   S T   f
Gu  b i   d Ne i

(8.84)ea awhere a is the friction-space (usually tangential) acceleration direction which is linearly
dependent on the unknowns u . We can get a good initial guess for the direction e using thelast stiction force direction: e  λ / λ87

8.6.3 Event detection for unilateral constraints
Once the set of active constraints has been determined, a Simbody simulation proceeds
continuously with an unchanged active set until a contact event is detected. Then one of two
event handlers is invoked: either the Impact Handler, or the Contact Handler.
The Impact Handler is invoked when a velocity-level unilateral constraint condition would beviolated, requiring a discontinuous change in velocities. There are two ways this can happen.
Most commonly, a holonomic unilateral constraint like contact or a joint stop reaches its
limit with a negative velocity, meaning further simulation would cause violation of the limit.
Alternatively, failure to converge a sliding friction contact force requires an impulsive
transition from sliding to sticking.
The Contact Handler is invoked when an acceleration-level, or equivalently reaction forcelevel, unilateral constraint condition would be violated. For example, change of a unilateralcontact’s normal force from positive to negative requires invocation of the Contact Handler.
The solution involves only changes to the active constraint set; no changes to velocity occur
except very small projections of the active velocity constraints to satisfy velocity tolerances.
The Contact Handler is also invoked as the last step of the Impact Handler, to determine theactive set after the instantaneous velocity change has been completed. Figure 13 outlines thisflow of control during a time stepping study.

88

StartAdvance timeusing current
active set

Event?
No

Contact event

Determine newactive constraint
set

Contact
Handler

Impactevent
Apply impulsive
velocity change

Impact Handler
Figure 13: Flow of control during time stepping with unilateral contact and friction
constraints. Time stepping begins with a feasible active set. Then the normal time stepping
sequence is in blue and does not involve recalculation of the active set.

Although one or more particular constraint condition violations will trigger an event, thehandlers work with all relevant constraints simultaneously without treating the triggering
event special consideration.
TODO: Impact events: penetration, failure to converge sliding friction normal forces.
TODO: Contact events: liftoff (normal force goes negative), contact (normal acceleration
goes negative), stiction release (force exceeds μsN), slip-to-stick (slip velocity stops orreverses).

8.7 Dynamic solution method
The previous section glossed over some details of the system formulation that we’ll need todeal with here. Let’s first revisit the several types of constraint equations. Holonomic constraint equations p in equation (8.53) are those that are expressed at the q (position) level andrepresent meaningful physical properties of the system. Holonomic constraint equations
involve only state variables at position stage or below, that is, q, t, parameters (instance89

variables), and modeling choices. Holonomic constraint equations can be differentiated once
to produce holonomic velocity constraint equations p , and again to produce holonomic
acceleration constraint equations p .
The nonholonomic constraint equations v in equation (8.52) are those that are directly
expressed in terms of system velocities, that is, at the u level, and also represent meaningfulphysical properties. Typical examples are “non slip” conditions like rolling or gears, butthese can also include more global restrictions such as a conservation of energy constraint.
Nonholonomic constraint equations involve state variables at the velocity stage and below,
which includes the entire list given above for holonomic constraints plus the generalized
speeds u. Nonholonomic constraint equations can be differentiated once to produce nonholonomic acceleration constraint equations v .
The acceleration constraint equations a in equation (8.51) are those which are directly
specified in terms of the system accelerations u , or quantities which are linearly related toaccelerations such as reaction forces or constraint forces. Like holonomic and nonholonomic
constraints these are physically meaningful constraints.
Also at position level are the quaternion normalization constraints n in equation(8.49), eachof which involves only the coordinates of a single mobilizer and is present for numericalreasons rather than physical. These are produced by mobilizers which use quaternions topermit unrestricted orientation. Simbody’s implementation ensures that violation of quaternion normalization constraints has no physical effect on the system. That is, a change to q
which serves only to satisfy a quaternion normalization constraint is not permitted to cause
any change to the system configuration. Quaternion normalization constraints exist only to
reduce the number of degrees of freedom of a mobilizer’s four quaternions down to the threephysical rotational degrees of freedom represented by its three mobilities u.
Unlike the holonomic and nonholonomic constraints, there are no constraints at the velocity
or acceleration level corresponding to the quaternion normalization constraint. Equation(8.48) constructs the quaternion derivatives in terms of the three independent u’s, ensuring byconstruction that the velocity-level constraints are satisfied.

90

Note that the system equations include a block diagonal invertible linear mapping between
the u’s and the time derivatives of the q’s, shown in equation (8.48): q  N(q)u . Although N
is a rectangular matrix, it is invertible. Note that when the quaternion normalization constraints are not satisfied exactly, the 4x3 blocks Ni on the diagonal of N which correspond toquaternion qi will be scaled by |qi| so that the resulting q is the derivative of the unnormalized quaternion.
Here is the system in the form we actually solve in Simbody, with slow (discrete) variables
not shown:kinematics

q  Nun( q )  0(8.85)

dynamics

 M G T   u   f  fbias     G 0    b z  z (t , q, u, z )(8.86)

velocity manifold

Pu  c  0v(t , q; u )  0(8.87)

position manifoldp(t; q)  0(8.88)r(t , ydyn ; d f , y f )  0(8.89)

relax fast variables

We are given initial condition(t0 )  {t0 , d (t0 ), y(t0 )} such that equations (8.87)–(8.89) aresatisfied, and are asked in a time stepping study to solve for d(t), y(t) for t0≤t≤tfinal.

Showing prescribed motion and fast variables:
kinematics

dynamics
auxiliaryq  Nun( q )  0
 M G T   u   f  fbias     G 0    b z  z(t , q, u, z, u, , )(8.90)(8.91)(8.92)

91

constraint matrix












position manifold





 q p  p p (t ) 
p
0
p
(
t
,
q
;
q
)
f
p
f



velocity manifold









 uq p  c p p
uq p  N 1q p
  0,
 Pf u  cr 
 u p  v p (t , q ) v
0 v f (t , q, u p ; u f ) 






acceleration manifold 






relax fast variables











g

 up 
G b  0
uf 

 1p
G 
 G fp

 1f

0 f    P fp  G ff    V fp 
  A fp 





 P ff   V ff   A ff  
0f(8.93)(8.94)(8.95)(8.96)

 uq p  b q p 0p Pf u  b q f  uu p  bu p 0v
 V f u  bu f 
u p  a p (t , q, u )


a
0
 a f (t , q, u, u p ; u f ) A f u  b f 

d timefast )

r pos (t , d , ydyn ;d posfast , q fast )  0

r vel (t , d , ydyn , q fast ;d velfast , u fast )  0

r acc (t , d , y;(8.98)(8.99)

0r time (t , d , ydyn ;r force (t , d , ydyn , q fast , u fast ; d force
fast , z )(8.97)(8.100)0d acc
fast , u fast , z )  0

Relaxation constraints (8.100) should be solved in stage order as shown, so that once fastvariables have been calculated at one stage they can be dependencies in a later stage. Notethat a relaxation solver at any stage may involve repeated state realizations through Acceleration stage; the relaxation stage just determines which of the fast variables may be modified.
92

For compactness, equation (8.100) doesn’t show the always-present dependency on yslow, anddoesn’t show which variables in the state’s d partition can actually serve as dependencies; therule is: any d variable is allowed except those in dfast which haven’t yet been computed in therelaxation sequence. As with the dynamic constraints in (8.94)-(8.99), relaxation constraints
will in general consist of a mix of explicit and implicit equations, but we’re not breaking
them out since there is no special treatment for them outside the relaxation solver itself.

93

9 Scaling and and accuracy
A multibody system is modeled using a set of state variables, and a set of differential andalgebraic equations that those variables must satisfy. There are many mathematically equivalent ways to model the same system, and some of the modeling choices to be made arearbitrary. Some examples are: choice of units for various quantities; definitions of generalized coordinates and speeds; choice of which quantities to treat as independent and which
dependent; and choice of which body is to serve as the base body for a chain. However, theresulting physically equivalent models are not numerically equivalent so can affect the actual
solutions we obtain when doing computations, which are necessarily approximate. Suchcomputations involve significant tradeoffs between CPU time and accuracy, so we usuallywant to use the loosest accuracy that is adequate for our purposes. The goal of scaling is toensure that an accuracy specification (e.g., “1% accuracy”) can be applied in a physicallymeaningful way so that the behavior of a study is not dominated by arbitrary modelingchoices. That is, we would like a given accuracy specification to yield the same physicalresults for all the physically equivalent models, regardless of any arbitrary choices that may
have been made during construction of those models.
There are two ways in which arbitrary modeling choices interact with accuracy requirements.
These are: (1) scaling of system state variables, and (2) scaling of errors in the algebraic
constraints. Our goal is to be able to determine a physically meaningful “unit change” to eachstate variable, and a physically meaningful “unit error” for each algebraic constraint. Then
when solving the system equations we can define “accuracy” to mean calculation of statevariables to some fraction of that unit change, and satisfaction of algebraic equations to somefraction of that unit error. We deal with multiple variables and equations by defining a scalar
norm representing “overall change” and “overall error” and then requiring our computations
to maintain those norms at or below the requested accuracy.

9.1 Relative vs. absolute accuracy
For some quantities we are satisfied to achieve relative accuracy, e.g. a result that is within
1% of the correct value. In that case absolute errors of larger magnitude are acceptable when
the numerical magnitude of a quantity is larger. However, one must be careful to avoid95

situations in which a physically-meaningless change in magnitude of a measured quantity
would result in a physically-significant difference in accuracy or behavior. For example,
rotations are cyclical quantities, but the angular variables that define them can grow withoutbound, adding 2π with each revolution. There is no configuration change associated withthose factors of 2π and no reason that the absolute angle should be determined less accurately
as the revolution count increases. Less obviously, translational coordinates can also introduce
physically-insignificant numerical changes. Consider a block on a sliding joint that hits a stiff
spring stop at x=1 and a simulation that predicts its behavior as it encounters the stop. If you
now lengthen the block’s travel and put the stop at x=1000, it would be incorrect to calculate
x any less accurately because that could drastically change the calculation of the spring force,with significant consequences for the block’s predicted behavior. Common sense says ablock should behave the same way in one place as another. Similarly, any change in thespatial location of a system involves changes to translational variables, yet should not affectthe precision of results. Hence Simbody solves all configuration coordinates q to an absoluteaccuracy level independent of their current values. Note that this does mean that there can bea performance cost for choosing units far from unity; we deem that a more reasonable
consequence than producing bad answers.
Unlike configuration variables, velocity variables u may reasonably be solved to relative
accuracies in most cases*. Arbitrary constants in q are eliminated by differentiation, so do notappear in q or u. For auxiliary state variables z relative accuracy may or may not be appropriate; Simbody assumes that relative accuracy for z is sufficient unless told otherwise.
When a u or z is near zero, relative accuracy becomes too strict. In that case an absoluteaccuracy requirement should be used instead. The absolute accuracy requirement for avariable represents a “good enough” level that is acceptable no matter what fractional error of
the current value that represents. For example, say accuracy α=0.1% (10−3) has been requested for a forward dynamics study. If the current value of a variable x is 10−6 units, an error*

This may not be acceptable for u’s that are involved in constraints, since constraint errors generally involvethe differences between velocities.

96

estimate no larger than 10−9 units would be required to produce a relative accuracy of 0.1%,
requiring a very small step size. However, if the absolute error requirement for x were 10−5
units, then an error estimate as large as 10−5 units would be acceptable, despite that being
1000% of the current value.

9.2 Weighting and absolute accuracy
An important practical consideration for any multibody formulation is that the state variablesy  {q, u, z} vary widely in effect, or weight, by which we mean the degree to which a unitchange in the numerical value of a state variable affects a physically meaningful quantity ofinterest. There are several causes for the uneven weights of state variables. To begin with,they are expressed in different units—q’s are typically lengths, angles, or quaternions; u’s are
typically length/time or angle/time; z’s can be anything at all. Weighting differences are evenmore pronounced in internal coordinate formulations like Simbody’s, since the effect of astate variable depends strongly on its position in the multibody tree. A unit change δqi to anangular coordinate near the system base will have a much larger effect (on almost anything
you might care to measure) than the identical change made to a coordinate which rotates only
a lone terminal body.

d2

q2
P

d1
q1
Figure 14: Weighting of q1 and q2 are very different
with respect to the position of the end point P.

Figure 14 depicts this situation. If we hope to achieve a given level of accuracy with regard
to the positioning of the end point P marked with a red square, we need to calculate q1 moreaccurately than q2. For example, say d1=10 nm and d2=0.5 nm. Then an error ε=0.1 radians in
q1 induces an error of d1ε=1 nm in P’s location, while that same error in q2 induces only
d2ε=0.05 nm of positioning error. In this situation we say that state q1 has more weight than97

q2 with respect to the location of P. If we consider a unit change of P to be 1nm, we candefine a corresponding “unit change” of q1 in this case to be δq1=0.1 radians while δq2=2
radians. Each of those unit changes will move P by 1nm. States which are “more important”
will have a numerically smaller unit change. Note that “unit change” does not mean “smallchange.” Rather, this is the quantity to which the requested accuracy α is applied.
Even in the simple example of Figure 14 it is clear that the weights of state variables are notconstant but change as a function of system configuration. Thus weights may need to berecalculated periodically as a system moves during a study. Fortunately, in practiceweighting does not need to be done perfectly to yield substantial improvements over unscaled variables. That permits us to treat weights as constants in the discussion to follow; inpractice they are updated only occasionally. Also, in practice there is no single quantity P
that we can use to neatly define unit changes for each of the generalized coordinates.
This concept of “unit change”  yi  0 for each state variable yi is used to define the “always
good enough” absolute error requirement    yi for use when relative error   yi is notallowed or too stringent. We can then define a diagonal weighting matrix W, where the ith
diagonal element is wi  1/  yi , the “unit weight” of state variable yi. For example, referring
again to Figure 14, if we want to scale by the geometric consequences of the state variables
on P’s location we could use w1  1/  q1  d1 and w2  1/  q2  d2 . Then we would define W as
follows:W d1 0 
0 d 
2In the above discussion we have oversimplified the choice of weights for q by choosing asystem in which q  u . In general, however, we have q  N(q)u (and u  N (q)q ). Thatmeans the effective weights on q contain arbitrary factors due to choice of coordinates in
addition to physical considerations. However, given weights on u (which are physical) we
can easily determine the corresponding q weights. So instead of determining weights fromP q we instead look at P u . Thus “unit change” estimates are provided for the mobilitiesu, which are physical quantities, rather than on the generalized coordinates q which can bechosen somewhat arbitrarily. So we work only with the nxn diagonal weighting matrix Wu;
there is no separate Wq. Instead, it may be calculated as98

Wq  NWu N

(9.1)

which ensures that the appropriate kinematic relationship holds between the weighted
velocity variables, that is qw  N(q)uw where qw  Wq q and uw  Wu u . Note that although
Wu is diagonal, positive, and constant, Wq is block-diagonal, signed, and q-dependent since
N=N(q). (Note that although N N  I , NN  I .) If there are auxiliary state variables z they
will have independent weights Wz. Then we have
 NWu N 

W
0

0


0
Wu
0

0 

0 
Wz 

(9.2)

Even when Wu=I, Wq can be significant for orientation coordinates. Some configurations ofquaternions have one or more coordinates with zero weight (because they act only to change
the length of the quaternion and thus produce no rotation). Euler angle coordinates nearsingular configurations have one coordinate whose weight approaches infinity; proper
weighting ensures the simulation remains accurate in near-singular configurations, althoughpossibly with a significant performance penalty.
We can combine relative accuracy requirements with the absolute “unit change” estimates
into a matrix E to form the error norm:
Eq  NWu N 
 Eq

E



Eu


min( Wu ,i ,1 ui ), where Eu  
Wu ,i ,
E z 
min( Wz ,i ,1 zi ),Ez  
Wz ,i ,
ui solved to relative accuracy
otherwise(9.3)

zi solved to relative accuracy
otherwise

Now let ey represent the vector of ny unweighted error estimates introduced by a step in the
numerical solution for y(t). Then f y  Eey is the vector giving for each state variable theunitless fractional error represented by the value in ey. We can define the error 2-norm
ey

E

fy

2

and error ∞-norm ey

E

fy



. We don’t use the 2-norm directly, but instead

use the related RMS norm

ey

ERMS

1
ey
E
ny
99

where ny is the number of state variables, because the RMS norm does not grow with the
problem size. (The ∞-norm is already size-independent.) Now we can provide a precise
meaning for the notion “solve for y(t) to an accuracy α”: either ey
ey

E

ERMS

  or (stricter)  at each step during the study. Typically we’ll require that q, u, and z norms

separately meet the required accuracy to avoid mixing dissimilar variables in the RMS case.

9.3 Scaling of constraint errors
The nonlinear algebraic equations defining the system, such as equations (8.52) and (8.53),
cannot be solved exactly by a numerical computation. Instead, they will be met with someresidual error. We would like to keep that error below a specified “tolerance” level during a
study. As with the state variable weighting problem above, we have to deal with the issue
that there are many separate algebraic equations, and the errors they produce will not bemeasured in the same units. This is especially important when mixing position (holonomic)
and velocity (nonholonomic) constraints, since velocity constraint errors need to be in units
comparable to the time derivatives of the position constraint errors. Also, if any accelerationonly constraints are provided their errors must be in units comparable to the 2 nd time derivative of the holonomic constraint errors.
The formulation used by Simbody ensures that the acceleration-level constraints are solved tomachine precision at the same time we solve for the accelerations. So our numerical integration methods do not need to deal with acceleration constraint tolerances; we’ll just take what
we get from solving system (8.86). However, if you are using your own methods you mightfind it useful to scale the acceleration errors. In any case we do need to actively control theerrors in velocity, position, and quaternion normalization constraints. As with state variables,
we want to be able to provide a consistent physical meaning for a statement like “solve theconstraint equations to 1% accuracy.”
To do this we define a set of unit constraint errors tp , tv , ta  0 , one for each of the mp position(holonomic) and mv velocity (nonholonomic) and ma acceleration-only constraint equations.
Each of these has appropriate units for its constraint equation, e.g. angle or length for aholonomic constraint, speed for a nonholonomic constraint, and accelerations for acceleration-only constraints. Each ti should represent the violation that is to be considered a “unit100violation” of the ith constraint (“unit” doesn’t necessarily mean “small”). By default each unit
constraint error is 1, meaning 1 length unit, 1 radian, 1 radian/time unit, etc.
To estimate tolerances for holonomic first and second derivatives, and nonholonomic derivatives, we use a System-defined characteristic time scale parameter tc. This parameter gives atypical “time of interest” over which we expect to see significant behavior. This should beroughly the rate at which you could expect interesting reporting data, not necessarily thehighest frequency in the system. For example, given the time scale, we consider an appropriate unit velocity error for a distance constraint derivative to be one unit distance error per tc.
A biomechanical or robotic system might have a timescale of 100ms, a biomolecular system
100fs, and an orbital problem might have a timescale of 10 or 100s. Somewhat counterintuitively, a long time scale puts more stringent requirements on velocity constraints since they
have much longer to drift in that case. Our default time scale is 0.1 time units.
Now we can define diagonal constraint weighting matrices Tp, Tpv, Tpva whose diagonal
elements are the weights for the combined constraints at position, velocity, and accelerationlevels (that is, velocity level includes holonomic derivatives, etc.). Note that “unit error” has
the inverse sense to “weight”—while a larger weight means “more important” a larger unit
error means “less important,” so we invert unit errors in T to create weights. Here is how theconstraint weighting matrices are defined:

Tp   diag(1. / t p )  , Tv   diag(1. / tv )  , Ta   diag(1. / ta ) 
t T
Tpv   c p



Tv 

t T
Tpva   c pv


 tc2 Tp Ta  





.

tc Tv

(9.4)




Ta 


Unlike relative weights of state variables, which can change as the state variable values
change, we expect T to remain fixed once specified since tolerances are absolute quantities.
Now define  p ,  pv ,  pva as the vectors containing the current, unweighted errors for eachconstraint equation at each level. We can now calculate the tolerance norms  pand  pT

 Tp pT

 Tp p2

and similarly for pv and pva. These treat all constraint errors uniformly.

101

As in the previous section, in practice we will use the RMS norm  p∞-norm  p

T

TRMS

 m1  ppTorrather than the 2-norm to remove effects due just to problem size, and define

the phrase “meeting tolerance to accuracy α” to meanmax( Tp p

norm, Tpv pv

norm

) 

(9.5)

where norm=RMS or (stricter) norm=∞.

9.4 Scaling at the acceleration level
Acceleration-level constraint weightings Tpva do not matter when using Simbody’s standard
formulation since accelerations are calculated to machine precision. Even when the constraints are redundant accelerations are uniquely determined, however in that case the constraint forces are not unique. Simbody will calculate a least-squares solution to the constraint
forces which tends to spread the load evenly among the redundant constraints. You can intheory provide a weighting W to influence the norm in which that least squares calculationis performed. However, that is not the same as the acceleration error weighting Tpva. Instead,W is used to capture the relative stiffness of the compliant elements that are being modeled

rigidly with constraints. This is a rather obscure nuance and is unlikely to be useful inpractice – if you really care about how the forces are distributed you will probably want touse compliant elements. However, here is a proof that acceleration-level weighting has noeffect except when there are redundant constraints.
Scaling on the constraint forces would be incorporated as follows. First, we want to solve forthe scaled multipliers (using just W here to represent W ):Mu  GT (WW )  f

 Mu  (WG)T W  f(9.6)(W is symmetric.) This shows how the scaling transfers to the constraint Jacobian, so theacceleration constraint equation is scaled like this:(WG)u  Wb

Note that once we have W we can unscale this for use with the original constraints via

102

(9.7)

  W1W .

(9.8)

Multiplying (9.6) through by M−1 and then WG and substituting from (9.7) gives:(WGM 1GT W)W  W(GM 1 f  b)

(9.9)

In the underdetermined case we can solve for a least squares W using the pseudoinverse:W  (WGM 1GT W) W(GM 1 f  b)

(9.10)

But if GM 1GT has full rank then so does the scaled version, and the pseudoinverse is just theordinary matrix inverse so we can rewrite (9.10) as
W  ( WGM 1G T W)1 W(GM 1 f  b)
 W 1 (GM 1G T )1 W 1 W(GM 1 f  b)
 W 1 (GM 1G T )1 (GM 1 f  b)

(9.11)

 W 1U

where U is just the multiplier we would have calculated without scaling. Unscaling Wusing equation (9.8) gives   WW  WW1U  U , that is, the final λ is just the unscaled one.
That proves that weighting the acceleration constraints and multipliers has no effect forsystems with full rank.

9.5 Accuracy
To summarize, we now have a way to define what is meant by solving a multibody
system to a given accuracy, say α=0.1%. We will have defined a locally-constantweighting matrix W on changes to the state variables u and z (and implying aweighting on changes to q) and a constant reciprocal tolerance matrix T on the absolute
errors in the constraint equations, appropriately time-scaled. W defines a “unit change” foreach state variable, and T defines a “unit error” for each constraint equation. We also haveinformation that defines what we consider “full scale” for each state variable, so that we caninterpret accuracy as a request for precision as a fraction of the full scale value. The block
diagonal matrix E combines relative scaling with the absolute unit changes in W to define
the error norm.
Then we have solved a trajectory to an accuracy α =0.1% (for example) when both

103

and

max( Eq eq, Eu eu Enorm , E z ez Enorm )  0.001
Enormmax( Tp p, Tpv pv)
 0.001
Tnorm(9.12)

Tnorm

hold for each step of the solution, where Enorm and Tnorm are RMS or ∞ norms.
Although constraint accuracy is maintained throughout a simulation, it is important toemphasize that accuracy of the state variables is a local phenomenon. Many multibody
systems are inherently chaotic, meaning that their long term behavior is arbitrarily sensitive
to initial conditions and numerical errors and hence not predictable. Only local measures ofaccuracy make sense for such systems. One may think of this as ensuring that the simulation
accurately simulates some system which is very similar to the one under study. Without suchaccuracy control there is no guarantee that any such system is being simulated.

104

10 Time Stepping
The system described by equations (8.85)–(8.88) is an overdetermined system since there are
more equations than unknowns. The nq+n+nz+m unknowns are q,u,z and λ. The first line ofequation (8.85) provides only n independent equations, but the second adds nquat more for a
total of nq kinematic equations. Then equation (8.86) provides n+m* with the first line and nz
more with the second line. That leaves (8.87) and (8.88) as 2mp+mv “extra” equations. Theseequations define the position and velocity constraint manifolds on which the solutions q(t)and u(t) are expected to lie (that is, the values of q and u should always satisfy those equations). If equations (8.85) and (8.86) could be integrated perfectly, the solutions would
indeed stay on the manifolds since they start out that way and equations (8.85) and (8.86)
satisfy the constraint derivatives. However, truncation error inherent in methods for approximate numerical integration allows the solution to drift away from the manifolds. The “extra”
equations can be employed rigorously to eliminate this drift, and in fact improve the solution
overall, using the method of coordinate projection8 to be discussed below. We’ll make use
here of the scaling, tolerance, and accuracy theory from Chapter 9.

10.1 Coordinate projection
Given an arbitrary value for the state variables, some or all of the constraint equations may
fail to be satisfied. Since accelerations are computed quantities rather than states, we canalways calculate them to satisfy the acceleration constraint equations. However, since t, q,
and u are independent states we may find position and velocity constraints are not satisfied.
In cases where the equations are expected to be arbitrarily far from being satisfied (typicallyprior to the start of a study), we may need special analyses to attempt to find values which
satisfy the constraints. However, during a dynamic simulation it will typically be the casethat the constraints will almost be satisfied, meaning that q and u just need to be “cleaned up”
a little. This cleaning up process can be thought of as taking state variables which have left*

When G doesn’t have full row rank (meaning some of the constraints are redundant or inconsistent), we
introduce other conditions to select the “best” solution for the underdetermined λ. Specifically, we choose thevalue for λ that minimizes |λ|2 in the redundant situation, and the value which minimizes the 2-norm of theresidual error in equation (8.86) if the constraints are (slightly) inconsistent.

105the required constraint manifold and projecting them back to the manifold via the shortest
path (smallest change in a weighted norm) we can make.
We define q 1  1 n ( q )  n( q )   q nquat  1


(10.1)

 p (q)  p(t; q)(10.2)

 pv (u )    (t, q; u)

p 
v

(10.3)p 
 pva (u )   v  (t, q, u; u ) a 

(10.4)

where arguments before the semicolon are fixed at their current values. Note that these areunweighted errors; εp and εpv need to be weighted using the constraint weighting matrices Tp
and Tpv discussed in section 9.3. When some of the q and u values are known due to prescribed motion (see section 8.3), then q={qp, qf} and u={up, uf} and we are only able tomodify the free variables qf and uf to satisfy the violated constraints.
Then we would like to find the smallest change to qf that will drive εp to 0, and the smallest
change to uf that will drive εpv to 0. Those “smallest” changes correspond to a least squares
projection in a weighted direction we call the error norm (E norm), normal to the constraint
manifold, for which a theorem given in reference 8 guarantees that this projection also
improves the solution to the differential equations. See section 9.1 for a discussion of the E
norm. εpva is satisfied exactly when we solve equation (8.86), and εn is always satisfied
simply by normalizing the quaternions n k  q , which is a 2-norm projection that can bedone separately from everything else, although no projection is done for quaternions that arecontained in qp.
The projection equations are underdetermined, nonlinear equations, but we expect to be close
to a solution so they can be solved efficiently with Newton iteration or similar methods. To
avoid an excess of subscripts we will drop the subscript “f” here, but keep in mind that we106

are working only with free variables, and matrix columns corresponding to prescribedvariables are removed. All constraint equations remain, however, even as the number ofvariables available to solve them is reduced by prescribed motion.
The full Newton steps in projection are
Pq (q (i ) )qELS  Tp p (q (i ) )q (i 1)  q (i )  Eq qELS iteratenk  q(last ) : nfinal
 nk | nk |
k
V (q

final

(10.5)(10.6)

tc Tp p (u (i ) ) )uELS  (i ) 
 Tv v (u ) u (i 1)  u (i )  Eu1uELS iterate(10.7)

where
Pq (q)  Tp Pq (q) Eq

(10.8)

t T P(q)  1
V (q)   c p
 Eu
T
V
(
q
)
v



(10.9)

Eq  NWu Nand( Eq  NWu1N )Pq (q)  P(q) N .

(10.10)(10.11)(Columns of Pq and V corresponding to prescribed variables are removed.)
We iterate (10.5) until we have calculated a final value q (last ) that satisfies the holonomic
constraint equations (10.2) to within a specified tolerance, then using equation (10.6) project
the quaternions in q (last ) via their normalization constraints (10.1). That gives us q final which
satisfies all the constraints (10.1) and (10.2). We then iterate (10.7) with V calculated at
q final while solving for the final velocity value u final which satisfies the velocity constraints(10.3). Note that we must perform a least squares solution to the linear system at each
iteration, and that the diagonal weighting matrices Eu, Tp, and Tv must be constant during theiteration (although Eq may change a little). Typically, P and V are block-structured matrices(and weighting preserves that structure). For efficiency, uncoupled blocks should be treatedseparately in equations (10.5) and (10.7).

107Normalizing a quaternion as in equation (10.6) is the least squares projection of the fourdimensional quaternion onto its constraint manifold, a three-dimensional sphere of unit
radius. However, quaternion projection is done in an unweighted norm since it is a constraint
on the numerical values of the quaternion elements unrelated to the physical effect of thoseelements. By construction, the physical effect of a change in the length of a quaternion in
Simbody is zero. Note also that there are no velocity or acceleration constraints corresponding to the quaternion normalization constraint, because those constraints are satisfied exactly
by the quaternion derivatives we calculate from the generalized speeds u.
A very similar problem arises when we have a vector in the q or u basis, and we would liketo remove the component of that vector which is normal to the constraint manifold, in theweighted norm. For example, when an integrator has computed a pre-projection absolute
error estimate vector ey  eq , eu , ez  in its computation of integrated state variables
y={q,u,z}, we know that performing the above constraint projection will remove the component of the error in the weighted constraint-normal direction (for proof, see ref. 8 and ref. 10,§3.8.2), and also the component of error along the length of quaternions. So we can now
reduce that error estimate by subtracting out any component it might have had in the directions we just fixed, which may allow us to take a bigger step. In that case the projections arePq (q final )ewq
 Pq (q final )Eq eq  no iteration eq  eq  Eq ewq eq k  eq : eˆqk  eq k  (eq k nfinal)nfinalkkV(q final )ewu
 V(q final )Eu eueˆu  eu  Eu1ewu

quaternions no iteration(10.12)(10.13)(10.14)(Here as above we are working only with the error slots corresponding to free variables, notprescribed ones.)
Again we need to find least-squares solutions to the underdetermined systems (10.12) and(10.14). Then we set eˆy  eˆq , eˆu , eˆz  as the new (absolute, unweighted) error estimate. Notethat these use the same (final) iteration matrices as above with a different right hand side.
Equations (10.12) and (10.14) are linear systems so no iteration is needed, and again block108

structure in P and V should be exploited for efficiency. After this projection the integrator
should use the revised estimate êy as its error estimate instead of the original estimate ey(using the W norm).
We can use a pseudoinverse to find the least squares solution at each step. The pseudoinverse
A+ of an mxn matrix A, with m<=n and full row rank (i.e. rank(A)=m) is given by
A+=AT(AAT)–1, although computing A+ that way can be numerically inaccurate. Using an
SVD or faster complete orthogonal factorization (QTZ) we can compute a numerically wellconditioned pseudoinverse even in the case of redundant constraints, i.e., rank(A)<m.
Looking now at the weighted holonomic position constraint iteration matrix Pq in equation(10.8) we see that the pseudo inverse we need isPq  (Tp Pq Wq )  (Tp PNWu1N )

The corresponding velocity constraint projection from (10.9) istc Tp PWu1 
V 
1 
 Tv VWu When there are no non-holonomic constraints, we have V  P the velocity projection is justV   P  (tc Tp PWu1 )

The constraint projections can be performed sequentially (for proof, see ref. 10, §3.8.3). First,
with t fixed at tˆ we must find some q  q(last ) that satisfies the holonomic constraint equations to within a specified tolerance. Then we normalize the quaternions in q (last ) (which byconstruction cannot affect any of the holonomic constraints) and call the result q final . Afterthat we freeze q at q final and proceed to find u that satisfies the velocity constraint equations
to within a specified tolerance. Note that the holonomic velocity constraint equations (i.e.,
first time derivatives of the holonomic constraint equations) and nonholonomic constraint
equations must be dealt with simultaneously since they can be coupled.

109

10.2 Simplified equations
For use with a generic coordinate projection integrator, the Simbody equations can be viewed
in the following simplified form:
differential eqns.

y  f (t , y)(10.15)algebraic eqns.

c(t , y)  0(10.16)

initial conditionsc(t0 , y0 )  0(10.17)

with the guarantee that equation (10.15) is solved in such a way that any new constraints
introduced by time-differentiating equation (10.16) are satisfied automatically; that is,c(t , y, f (t , y))  0 whenever equations (10.15) and (10.16) are satisfied. There is an nyxny
block-diagonal weighting matrix E and an ncxnc diagonal tolerance matrix T, and corresponding norms as discussed in Chapter 9. To solve this system to accuracy α, the following
two conditions must be satisfied at each integration step: y ERMS  (10.18)c(t , y) TRMS  (10.19)where the  y are the post-projection local state errors introduced by an integration step.
When conditions (10.18) and (10.19) are met, the integrator can accept the step.

10.3 Update rates for state variables
During a time-stepping study, Simbody supports three distinct update rates for time-varying
variables in: slow (discrete), dynamic, and fast. Slow variables are updated only occasion-

ally upon occurrence of events and never change during a continuous interval. Dynamic
variables follow differential equations, evolve smoothly during continuous intervals, andhave well-defined time derivatives. Fast variables are those whose response may be considered instantaneous on the scale of the dynamic variables. The dynamic system is thus always
in quasi-static equilibrium with respect to the fast variables, whose values may be determinedby difference equations, equilibrium conditions, or algorithmically. Fast variables change
continuously in response to dynamic ones but can affect dynamic time evolution only indirectly by affecting the dynamic variables’ derivatives.

110

Only variables in the State’s y partition can be dynamic. So we can partition the state by
update rate as follows:slow

d slow  yslow

fast

d fast  y fastdyn

(10.20) ydyn

where
yslow

qslow  uslow

ydyn

qdyn  udyn  z

y fastq fast  u fastwith

(10.21)y  yslow  ydyn  y fast

There also exists a set of time derivatives y  q  u  z , withyslow  y fast  0
ydyn 

(10.22)dydyn
dt

Note that yslow is zero because yslow is constant during a step, while y fast is zero for the
opposite reason: y fast changes so quickly that by the time we look it has already reachedequilibrium and is thus no longer changing. There are no derivatives defined for variables inx, which are not permitted to be dynamic (although they can be fast or slow).
When we want to denote variables which affect a particular stage and also have a particular
update
pos
fast

rate,
pos



we
fast

combine

the

sub-

and

superscripts,

forexample, d pos
fast  q fast . Note that a variable’s stage effect is an inherent property ofthat variable, while the rate at which it is to be updated is a run time choice that may bedifferent under different circumstances.
At run time we must choose for every mobilizer:How motion is driven: forces, acceleration, velocity, or position

The rate at which the driven motion is updated: slow (discrete), dynamic, or fast. A
force-driven mobilizer is always dynamic.

When mobilizer j is updated dynamically, it is driven by differential equations relating
position q[j] and velocity u[j] to acceleration u[ j ] . Normally, u[ j ] is calculated as a re111

sponse to forces, with u[j] and then q[j] calculated from u[ j ] by numerical integration. In
that case the dynamic mobilizer j is said to be free. When motion is known analytically, themobilizer is said to be prescribed. Both free and prescribed mobilizers are considered dynamic mobilizers since they are both defined by time-varying differential equations. If instead thedriven motion is fast or slow, we have a fast or slow mobilizer, resp. All mobilities of amulti-dof mobilizer must have the same update rate (that is, slow, prescribed, free, or fast).
Note that for any mobilizer that is driven at acceleration or velocity level, higher levels are
determined by numerical integration and are thus free.
There are some restrictions on the use of “fast” motion, because fast variables may notdirectly affect dynamic ones: (1) If a mobilized body is fast, then all its children and descendents in the multibody tree must also be fast. (2) If a constraint involves any fast body ormobility, that makes it a fast constraint that is not part of the dynamic system. Instead it issolved taking the dynamic variables as fixed, with only the fast variables varying.

Driven by

Update
rate

u

u

q

forcesdynamic(free)

u f  t , q, u, z, u p 

u f   dt

q f   dt 2

acceleration

dynamic(prescribed)

u p (t , q pr , u pr )u f   dt

q f   dt 2

dynamic(prescribed)

u p  d dt

u p (t , q pr )q f   dt

fast

up  0u fast : algebraic

q f   dt

slow

up  0

uslow : eventdriven

q f   dt

dynamic(prescribed)

u p  d 2 dt 2

u p  d dt

q p (t )fast

up  0up  0q fast : algebraic

slow

up  0up  0qslow : eventdriven

velocity

position112

Structured variables d that can affect time-varying quantities can be evaluated either continuously (fast) or at discrete times (slow); variables that affect model or instance parameters or
are just used for reporting can’t be continuous. Here the continuous variables cannot be
defined by differential equations; they are always algebraic (or algorithmic).
dmodel,instance,report

dtime,pos,vel,dyn,acc

fast

n/a

d fast : algebraicslow

d slow : eventdriven

d slow : eventdriven

Also note that “end of step” can be an event, so event-driven (slow) variables can be updated
as frequently as every step. However, they will not be updated during integration stages andcannot affect integrator step size selection in the current step.
10.3.1 Coupling
Variables which are mutually coupled must be solved simultaneously which can be computationally expensive. In many cases, modeling considerations dictate that some variables are“stronger” than others so coupling is only in the direction from driving variables to driven
ones, lending an ordering to the computation that avoids some simultaneity. Simbody supports four levels of coupling for computations at a given realization stage:
1. Slow (discrete): these variables are event driven so are not updated at all during acontinuous interval. Their values are taken as given at the beginning of a continuousinterval and can affect all subsequent calculations but are themselves unaffected.
2. Dynamic, prescribed: these are q, u, z, or u variables defined by differential equations with explicit analytic solutions based only on already-available values. For example: positions as a function of time. This could represent an extremely highbandwidth controller, or a locked joint. Prescribed accelerations are not affected byforces.
3. Dynamic, free: these are q, u, or z variables calculated by numerical integration. Corresponding accelerations u can additionally be coupled by algebraic constraint equations, which equations can depend on prescribed accelerations. They can thus be driv113

en by prescribed variables at the same level (e.g. prescribed velocity can drive freevelocity) but not vice versa. A dynamic position can affect a prescribed velocity sincethat’s a later stage. z variables cannot be coupled via constraints.
4. Fast (quasi-static): these variables are calculated from the values of dynamic variables(free or prescribed). They can affect forces so will change the behavior of free variables indirectly, through subsequent accelerations or by interaction with the numericalintegration method’s error test.
Free dynamic variables yf are approximated numerically by an integration method, then anyof them that are subject to constraint equations may need to be projected back to the constraint manifold via the System’s project() solvers. Prescribed dynamic variables yp are
simply set to their appropriate values via the System’s prescribe() solvers, once theirdependencies are available. Finally, we must solve for the fast variables dfast and yfast using
the System’s relax() solver.
Depending on the operation we are performing, it will be convenient to partition
variety of different ways.
Variable type

={d,t,y}y={q,u,z}
stage(t)=Timestage(q)=Position
stage(u)=Velocity
stage(z)=Forcestage(x) varies

Simulation

={def, t, var, report}def ={topo, model,instance}
var ={pos, vel, force,acc }

scalarvariablesy114

pres ={qp, up}

Generalized coordinates(positions)uGeneralized speeds (velocities)z

Force variables

dstructured variables

t

time

def
t

variables that define the modeled
system: topology, model, instance
time

var

variant properties of the modeled
system: position, velocity, forces,
acceleration

report“output” states for reporting

Time steppingq

continuous
variablespresprescribed analyticallyfreedetermined by numericalintegrationin afree ={qf, uf, z}fast

fast ={qfast, ufast, dfast}slow={qslow, uslow, dslow}

slow

Quasi-static variables
determined algebraicallyafter pres and free

Discrete “slow” variables (updated
occasionally upon event occurrence)

10.4 How to take a time step
Here we deconstruct a single time step into its constituent parts.
Special handling is required at the start of a time stepping study. Time and all state variables
must be given initial values, and then these must be modified to satisfy all constraints,
starting with prescribed motion. The resulting state must be fully realized using to-beupdated values for discrete variables, as though we had just completed a zero-length step.
Then we can take the first step as though it were any arbitrary step.
1. Handle events as needed, updating discrete states, realize through Acceleration stage.
2. Take trial step of continuous system (with projection), with success metric.
3. If metric unacceptable, reduce step and return to 2.
4. Check for Time-stage event triggers and handle them; <= Instance stage change is a
restart meaning later event triggers are forgotten.
5. Realize(Positions); check for Position-stage event triggers and handle them; <= Instance stage change is a restart.
6. Repeat for Velocity, Force and Acceleration-stage event triggers.
7. If simulation is not done, go to step 1 to begin the next step.
Taking a trial step of the continuous system (we just updated discrete variables). Continuous
system consists of dynamic (prescribed and free), and “fast” variables qfast, ufast, zfast, and dfast.
There are a series of integrator stage evaluations and then a final setting of the state variables
in which differential variables are projected to their constraint manifold.

115

All steps begin like this:

If there are pending triggered events (at t  ):o Set t  t  . Call time-triggered handlers to change slow variables; re-evaluatey, e, c ; revise list of pending eventso Repeat for pending position, velocity, dynamics, acceleration-triggered events
If there are any beginning-of-step handlers, call them and re-evaluate. This cannot
cause any other events to occur.
Set t0=t, y0=y; invalidate d0
y = realize[A](d,t,y)Set y 0  y
Next step size to attempt is h

Fixed step explicit Euler (DAE step only):

Only stage:
 yfree  y 0free  h  y 0free
Set t  t 0  h ; realize[T](t)q pres = prescribeQ(t)q free : projectQ( q pres , qfree )u pres = prescribeU( t , q pres , q free )u free : projectU( u pres , ufree )Test convergence of projection. (bad: shrink h goto)d fast , y fast : relax[TPVFA]( t , y, y(d fast , y fast ); d fast , y fast )Test convergence of relaxation. (bad: shrink h goto)(Error test OK.) Check for events. If any:o Localize event(s) to window w  (t  , t  ]o Back up the state to t  t  :yfree (t  ) = interpolate( t  , y 0free , yfree )y pres , y free = presAndProjQU( t  , yfree )x fast , y fast : relax[TPVFA]( t  , y, y(d fast , y fast ); d fast , y fast )o (Projection and relaxation must converge or there is something seriously
wrong.)
Successful, event-free step taken to time t. May be events pending for time t  .

Explicit trapezoid (w/explicit Euler error estimator):
116First stage: y1free  y 0free  h  y 0freet  t 0  h ; realize[T]( t )y1pres = prescribeQU( t , y1free )d 1fast , y1fast : relax[TPVFA]( t , y1 , y(d fast , y fast ); d fast , y fast )Test convergence of relaxation. (bad: shrink h goto)Final stage:
y1 = realize[A]( t , d 1 , y1 )yfree  y 0free  h2 ( y 0free  y1free ) free  yfree  y1free
y pres , y free ,  free = presAndProjQU( y pres , yfree ,  free )Test  free and convergence of projection. (bad: shrink h goto)d fast , y fast : relax[TPVFA]( t , y, y(d fast , y fast ); d fast , y fast )Test convergence of relaxation. (bad: shrink h goto)(Error test OK.) Check for events. If any:o Localize event(s) to window w  (t  , t  ]o Back up the state to t  t  :yfree (t  ) = interpolate( t  , y 0free , yfree )y pres , y free = presAndProj[PVF]( t  , yfree )x fast , y fast : relax[TPVFA]( t  , y, y( x fast , y fast ); x fast , y fast )o (Projection and relaxation must converge or there is something seriously
wrong.)
Successful, event-free step taken to time t. May be events pending for time t  .

VerletFirst stage:2
 qfree  q0free  h  q0free  h2  q0freet  t 0  h ; realize[T]( t )q pres , q free = presAndProjQ( t , qfree );realize[P]( t , q )u pres = prescribeU( t , q pres , q free )u1free  u 0free  h  u 0free ; z1  z 0  h  z 0

realize[V]( t , q, u )d 1fast , q fast : relax[TP]( t , y1dyn , y(d fast , y fast ); d fast , q fast )k=1
Loop until u and z are converged:kd kfast , uz kfast : relax[VFA]( t , ydyn
, y( x fast , y fast ); x fast , uz fast )117

u k , z k =realize[VFA]( t , q, u k , z k ) expensive once++k1
u kfree  u 0free  h2 (u 0free  u kfree) ; z k  z 0  h2 ( z 0  z k 1 )

 u  u k  u k 1 ;  z  z k  z k 1
If δ not converged, continue looping.
ufree  u kfree ; z  z k

u and z have converged to 2nd orderqfree  Nufree
Compute low order estimates:qfree  q0free  h2 (q0free  qfree ); ufree  u1free ; z  z1

 free  yfree  yfree error estimate
 free,q = projErrEstQ(  free,q )uz free ,  free,uz = presAndProjU( y pres , uzfree ,  free,uz )Test  free and convergence of projection. (bad: shrink h goto)d fast , uz fast : relax[VFA]( t , ydyn , y(d fast , y fast ); d fast , uz fast )Test convergence of relaxation. (bad: shrink h goto)Successful continuous step to t. Now check for events.

10.4.1 Setting the values of prescribed variables
Prescribed variables y p  y and y p  y are defined by analytically-known explicit functions. These include q p (t ) , u p (t , qdyn ) , u p (t , qdyn , udyn ) . ( qdyn

q pres  q free , etc.) Note thatearlier-stage prescribed and dynamic variables can affect later-stage prescribed ones.
A System defines prescribeQ() and prescribeU() solvers which sets the values forthat stage’s prescribed variables, taking values of earlier-stage variables as given. Be sure tocall them in this order: prescribeQ(), projectQ(), prescribeU(), projectU().
10.4.2 Relaxation of fast variables
After the values of time and the differential variables y pres , y free  y have been calculated byan integrator, the fast variables y fast  y and d fast  d need to be allowed to respond to the
new values, to attain a new quasi-static equilibrium. We call this process “relaxation” although in general a System is permitted to use any method to solve for these variables,118

provided the correct order of dependencies is followed. A System provides a relax(stage,tol) solver which will drive a particular stage’s fast variables to their new equilibrium,
solving the associated equations to the given tolerance.
The time-stepping relaxation stages and the variables relaxed at each stage are as follows:
1. d time
fast :

relax(Time)

2. q fast , d

pos
fast

:

relax(Position)

3. u fast , d

vel
fast

:

relax(Velocity)

4. d

force
fast

5. d acc
fast ::

relax(Force)
relax(Acceleration)

A System may also support relaxation of report-stage variables d report and definition-stage
variables d model and d instance , but these are not relevant for time stepping.

119

11 Simbody Force Subsystems reference guide
Simbody comes with a predefined set of commonly-used force subsystems. Each of these isan independent, self-contained set of related features, and users may add their own force
subsystems as well.

11.1 General Force Subsystem
Simbody comes with a force subsystem called GeneralForceSubsystem. It can be used to
add a variety of standard forces to a system, such as linear springs and dampers. It also
provides a mechanism for adding user defined forces.

11.2 Hertz/Hunt and Crossley contact model subsystem
Simbody comes with a force subsystem class called HuntCrossleyContact. This section
describes the theory behind it.
11.2.1 Motivation
Most engineers, physicists and computer scientists are introduced to contact problems using
the concept of coefficient of restitution. The idea presented is that when two objects collide,
they will rebound in a predictable way with the rebound velocity being a known fraction e ofthe impact velocity.

Unfortunately, it is rarely mentioned that this concept is only usable in the mostlimited cases. Many difficulties arise trying to apply this in a multibody dynamics
context; in particular the presence and motion of the other bodies and the forces
applied to them (which change constantly and are not know in advance) change the reboundvelocity. Also, it is well known in the field of contact mechanics (and to anyone who has
watched closely as a ball bounces) that the coefficient of restitution is very sensitive to theimpact velocity. In fact, in contact mechanics the normal way to approximate the coefficientof restitution is e  1  cvi for small impact velocity vi , where c is a material property. An
enormous amount of empirical data supports that—at low velocities, normal materials have a121

coefficient of restitution that drops linearly with impact velocity. The classic work in thisfield is reference 13. Even with this improvement to the functional form of e, the results are
rarely applicable outside the realm of freely falling bodies. In multibody dynamics, thecoefficient of restitution is something to be computed, along with the rest of the system’s
motion, not something that can be known in advance!
To obtain usable results in a multibody context, we need a method that can calculate forces
produced during contact, rather than impulsive velocity changes. That permits contact to betreated as yet another force among the many that influence the behavior of multibody systems, ensuring that accurate (or at least reasonable!) behavior will result. Only once you canobtain physically correct results with some model, should an optimization like “treat
contact as an instantaneous event” be attempted, and even then one might wonder if it isworth the effort.
11.2.2 The modelThis model is based on Hertz theory of elastic contact,11 and the Hunt and Crossley model for
damping.12 The idea is to predict contact behavior during a dynamic simulation working only
from material properties and geometry. This is a frictionless model but it can be used as astarting point for several useful frictional models.
To apply Hertz theory, we need two linearly elastic materials in non-conforming contact,
where the dimensions of the contact patch are small compared to the curvatures, and small
compared to the overall dimensions of the object. Hertz theory can be used for generalcurved shapes (including cylinders) provided they can be approximated by paraboloids at thecontact point; however, we will discuss only sphere-sphere and sphere-halfspace contact
here. For Hunt and Crossley, the impact velocities should be small enough not to cause
permanent yielding of the materials. Within these regimes, the model produces a good matchfor empirical data, such as that found in reference 13. Outside these limits, the model can still
produce surprisingly useful results when fit to experimental data, because the form of themodel has a structure which captures the most significant aspects of contact for many purposes. It is especially well-suited for soft contacts such as are common in biology, eventhough those are well out of the range that the rigorous theory presented here can support. I
speculate that it works well in most applications because the results of interest don’t usually122depend on precise details of contact, only that it behaves in a qualitatively correct manner. As
an example, if you get a stiffness parameter too low, the model will compensate by allowing
more deformation with the result being that you get the same forces (such as are needed tokeep a foot going through the floor) although the precise deformation of the foot and floor
are not obtained. This may be acceptable for researchers who are more interested in studying
some other aspect of the model, knee flexion for example.
For the rest of this section, please refer to Figure 15 which defines the geometry of contact.
We will consider a collision between two bodies, B1 and B2, in which a sphere attached to B1
contacts a sphere or halfspace attached to B2. During the collision (which will occur over an
extended period of time, not impulsively), our goal will be to determine instantaneous values
for the contact force f, the contact patch orientation n and radius a, and a unique contact point
P at which we can apply equal and opposite forces to the two contacting bodies. We will begiven the spatial locations and velocities of the undeformed geometric objects in contact, andwill easily be able to determine the total deformation x that must have occurred because of
the apparent overlap between the undeformed objects. However, in order to find the contact
point P and the compression rates of each body (needed to compute dissipation), we have todetermine the individual deformations x1 of B1 and x2 of B2, where x  x1  x2 andx  x1  x2 .

The thin lines in the figure are intended to show the undeformed shape while the thicker lines
give a (crude) depiction of the deformed shape. Note the assumption that the contact patch isplanar, circular of radius a, centered at P and oriented with normal n pointing towards body
B1. With these conventions, the scalar force f that we will calculate (applied equal andopposite to the two bodies at P) is always positive, and the vector force fn is applied to body
B1 at P, while we apply −fn to body B2 at P. From the diagram one might think it doesn’t
matter where along the line between the centers we apply the force. However, it is importantto keep in mind that the colliding objects are in general only attached to a larger body—they
do not constitute the whole body. That means the applied force is also generating momentson the bodies, and those moments depend critically on exactly where the force is applied.

123

E1* , c1

B1

O1•

Figure 15: Contact geometry for the
Hertz/Hunt and Crossley model.

R1nx2 , x2
x1 , x1

P•

a 

x, x
x1  s1 xx2  x  x1  (1  s1 ) x  s2 x

R2
O2•

n

O1  O 2
O1  O 2

P  O1  (R1  x1 )n O 2  (R 2  x2 )nB2E2* , c2

We expect to be given the following material properties for each body:
Property

SymbolUnits

Comments

Radius of curvatureR

LengthMeasured at the contact point

Young’s modulus

E

Pressure

stress/strain = (force/unit area) / (% deformation)

Poisson’s ratio

ν

Unitlessratio

ratio of transverse contraction to deformation (0–½ fornormal materials); related to preservation of volume
during strain; rubber has ν=½

Dissipation coefficientc1/velocity−slope of coef. of restitution vs. velocity at low velocities; i.e., coef. of restitution e=1−cvi for impact velocityvi

For our purposes, we combine Young’s modulus E and Poisson’s ratio ν into a single “stiffness” property called the plane-strain modulus E*=E/(1−ν2). This is measured as the pressure
per unit area induced by a fractional deformation (strain). The MKS unit is Pascals which are
Newtons/m2. Below are some typical values as ballpark figures only; please don’t rely on
them. (Note that the stiffnesses are given in gigapascals, i.e. 109 N/m2!)
Material

124

Young’s

Poisson’s

modulus E

ratio

Plane-

Dissipation

ν straincoefficient(GPa)(unitless)modulus E*

(s/m)

(GPa)
Rubber

0.01

0.5

.0133

0.05?

Bacteriophage20.4(?)

2.4

?

Nylon

3

0.4

3.6

?

Lead

14

0.42

17

0.4?

Concrete

25

0.15

25.6

?

Steel

200

0.3

220

0.08?

Diamond

1100

0.2

1150

?

capsidOf these, Young’s modulus and Poisson’s ratio can be obtained easily from handbooks formost materials, but the dissipation coefficient is harder to get. It would be very useful torelate this to standard properties such as hardness and yield stress (if that’s possible) but for
now it has to be measured or estimated as the slope of the coefficient of restitution-vs.velocity curve at low velocities. References 12 and 13 provide or imply some values for c,but they should be taken with a grain of salt. Note that this situation is still better than thestandard approach of supplying a coefficient of restitution e directly—at least c is a material
property so can be expected to produce correct behavior over a range of velocities.
Hertz contact theory says the relationship between force fHz and displacement x depends only
on the relative curvature R of the two bodies at the contact point, and on an effective planestrain modulus E*, and the contact patch radius a is an even simpler function
f Hz  43 RE* x3 2 , a  R x1 2

Hunt and Crossley start with the above formula for fHz and add a dissipation term:f HC  f Hz (1  32 cx)

where c is an effective dissipation coefficient combining the material properties of the twocontacting materials.

125Note that although the materials are assumed linear, the force-displacement relationship is
nonlinear because of the changing geometry during contact. This complicates the calculationof the effective stiffness E*. The literature seems to suggest E*  E1* E2* ( E1*  E2* ) but thiswould be inconsistent with the Hertz relationship, by the following reasoning. First, therelative curvature is a geometric property and is straightforward to calculate:R  R1R2 ( R1  R2 ) . Looking at the figure, note that the contact situation depicted should beindistinguishable from one in which B1 (the top, red body) had met an infinitely rigid halfspace, with a displacement of x1 instead of x, provided that B1’s radius were R instead of R1.
The effective stiffness in that case would be just the stiffness E1* of B1. Hertz theory would
then give f1  43 RE1* x13 / 2 . By the same reasoning, we can view B1 as a rigid half space andsee that the force on B2 (with radius changed to R) would be unchanged at f 2  43 RE2* x23 / 2 .
But the forces must be the same on both bodies and the same as f  43 RE* x3 2 . Recallingthat x  x1  x2 , we now have enough information to write E* in terms of E1* and E2* :E1* x13 2  E2* x23 2  E * x1  x2 

3 2

 E1*2 / 3 x1  E2*2 / 3 x2  E *2 / 3 x1  x2 3

 E *2 / 3 E *2 / 3  2
 E   *21/ 3 2 *2 / 3  E1  E2 *

Note that this combining formula is close, but not identical, to E*  E1* E2* ( E1*  E2* ) . The
general scheme is that if your force/displacement dependency has an exponent n, as inf  kx n , then the combining scheme for the material stiffness is E *1/ n E *1/ n E   *1/1 n 2 *1/ n  E1  E2 n*

We can now rearrange this for our case where n=3/2 to determine how x is split into x1 andx2 given the stiffnesses of the materials, the result we need to determine the contact point

location P:

126

2

 E*  3
E2/3
x1   *  x  2 / 3 2 2 / 3 x
E1  E2
 E1 
2

 E*  3
E2/3
x2   *  x  2 / 3 1 2 / 3 x  x  x1
E1  E2
 E2 

By inspection, the time derivatives x1 and x2 are split in the same ratios, which gives us away to define an equivalent dissipation coefficient for x : c  c1s1  c2 1  s1  , wheres1  E22 / 3

E  E . To summarize, here are the combining rules we use:
2/3

2/3

1

2

3

 E1*2 / 3 E2*2 / 3  2
R1 R2
*
R
, E   *2 / 3*2 / 3 
R1  R2
 E1  E2 
s1 

E22 / 3
E12 / 3,s
 1  s1
2
E12 / 3  E22 / 3
E12 / 3  E22 / 3

x1  s1 x, x2  s2 x
cx  c1 x1  c2 x2  c  c1s1  c2 s2

Now we can apply the Hunt and Crossley model, which starts with Hertz contact and adds a
dissipation term:f  max( f HC , 0)
 max( 43 RE * x3 2 (1  32 cx), 0)

The max() is needed only when an active force is “yanking” two contacting bodies apart; theforce will never be negative in normal contact/response conditions (see reference 14 forproof). The “yanking” situation corresponds to pulling the bodies apart faster than they canundeform.

11.2.3 Extension to include Friction
TBD
Friction models need to know the normal force, and sometimes the contact patch dimensions,and the Hertz/Hunt and Crossley model provides those.

127

We hope to provide a simple, continuous model with functionality like that described inreference 15, which is able to accurately model sticking, pre-sliding, and sliding friction
behavior and exhibit empirically observed Stribeck, Coulomb and viscous friction effects
without adding intermittent constraints to the multibody model and event detection to thenumerical methods.

11.3 DuMM — Molecular mechanics force field
Simbody comes with a force subsystem class called DuMMForceFieldSubsystem,
which we’ll abbreviate “DuMM” below. This is intended to provide a straightforward implementation of conventional molecular mechanics force fields, for use in experimenting withrigid-body molecule models, and to serve as sample code for someone who would like towrite or port a good molecular mechanics force field for Simbody. It is not intended for
production work!
11.3.1 BackgroundMolecular mechanics (MM) uses classical approximations of molecular interactions. It isthus suited only for circumstances in which quantum effects are not dominant; in practicethat means simulations which do not form or break covalent bonds between atoms. Fortunately this includes a lot of biologically interesting behavior such as binding, aggregation,
protein folding, and other cases where molecules rearrange rather than form or break.
Atomic force models are conventionally divided into two categories: bonded and nonbonded. Bonded forces act between or among covalently-bound “neighbor” atoms. Since
each atom can form only a small number of bonds, the number of bonded interactions is
O(na) in the number of atoms na. Non-bonded forces, on the other hand, represent interactions
between each atom and all the other atoms. These are electronic in nature and comprise
Coulomb forces and van der Waals forces. Because the number of such forces is O(na2), theseterms dominate the computational cost of the force field for all but the smallest systems.

12811.3.2 Basic concepts
The primary concepts supported by DuMM are the force field, molecule, and body. The
resulting model permits matter to be coarse-grained (that is, large bodies interconnected bymobilizers and constraints) while retaining detailed atomic forces and geometry. The same
methods are used to produce systems from ones where atoms are free to move anywhere in
Cartesian space, to systems where all the atoms move together as a rigid body, to anything in
between. Different molecules or pieces of molecules can be modeled at different granularity
in the same simulation.
11.3.2.1 Force field
The force field provides broad atom classes providing van der Waals parameters for particular elements in particular covalent environments. All bonded terms are specified in terms ofthese atom classes. A larger set of charged atom types is defined which combine atom classes
with particular partial charges. Each atom in the molecule is classified as a particular charged
atom type, which implicitly provides the partial charge, van der Waals parameters, andelement. Then the force field provides bonded terms for stretch, bend, and torsion, defined asa pair, triple, or quad of atom classes.
The force field definition includes a few global parameters as well, such as how to scalecharge and van der Waals forces for closely-bonded atoms, and how to mix van der Waals
parameters for dissimilar atom classes.
11.3.2.2 Molecules
Molecules are built from three concepts: atoms, bonds, and clusters. The only information
required in the definition of an atom is its charged atom type as described above. An integer
atomId is assigned and returned to the caller, so that every atom in the system has a unique
atomId. A bond connects a pair of atoms, with at most one bond allowed between any pair.
A cluster is a rigid grouping of atoms. When a cluster is defined it is assigned a uniqueclusterId, which is returned to the caller as a handle for future references to that cluster. Eachcluster has its own reference frame, like a body, and when initially created a cluster consists
only of that reference frame. Whenever an atom is placed in a cluster, it is given a station(position) with respect to that cluster’s reference frame. Clusters may be placed within larger
129clusters, in which case a Transform is used to specify the configuration (location and orientation) of the child cluster’s reference frame with respect to the parent cluster’s frame. An atom
may appear only once within a cluster or any of its subclusters. However, an atom may beplaced in multiple clusters as long as those clusters are independent.
Once a cluster has been populated with atoms, it can calculate its own mass properties which
can then be used in the construction of bodies.
11.3.2.3 Bodies
Once molecules have been constructed by adding atoms and bonds and then partitioning theatoms into clusters, a mapping of the atoms to SimbodyMatterSubsystem bodies canbe made. Bodies serve as a “top level” cluster, and atoms and clusters can be attached tobodies. Any time an atom is attached to a body it is given a station in the body’s reference
frame, and a cluster is given a configuration (Transform).
Note that mass properties are not automatically determined by attaching atoms and clusters tobodies. Rather, bodies must have mass properties assigned at the time they are defined in the
SimbodyMatterSubsystem. Typically, the mass properties as calculated by clusters,
and the masses of individual atoms, will be used in calculating the appropriate mass properties but that is not required.
Once the bodies are assigned, DuMMForceFieldSubsystem will figure out which of its
atoms are on different bodies, and consequently which of the bonded terms cross bodies.
Bonded and nonbonded terms that act only within a single body are ignored.
There is no automatic mapping of mobilizer coordinates to bonds, and in fact there is notnecessarily any direct mapping possible. Optionally, you may assign particular mobilities toany of the cross-body bonded terms (such as a sliding mobility to a bond stretch term or arotating mobility to a bond torsion angle). Bonded terms which depend directly on mobilities
can be calculated very efficiently, and it can be very convenient to have a coordinate whichcorresponds directly to a bonded term. (TODO: bond mapping not implemented yet).

13011.3.3 Units
There are a number of molecular mechanics unit systems in popular use. DuMM supports asingle “native” unit system but provides conversions to and from the others. The native unit
system is sometimes called “MD units” and is defined by the following units: length innanometers (nm, 10-9 m), mass in daltons (Da, g/mol, atomic mass units), and time in picoseconds (ps, 10-12 seconds). Angles are measured as unitless radians. In this set of units, atypical bond has a length of about 0.15 nm, a hydrogen atom has mass about 1 Da, andsubstantial motion occurs on a scale of about 1 ps.
This is a particularly appealing set of units because when combined consistently into energy(mass x length2/time2) we get energy per mole in g-nm2/ps2=103kg-m2/s2 =1kJ. That is, ourenergy unit is 1 kilojoule/mol which is one of the energy units popular among molecularmechanics practitioners. (Our consistent unit of force is then the kJ/nm = 1 Da-nm/ps2.)
The other popular unit system, perhaps somewhat more chemist-friendly than ours, is the
kcal-Ångströms (KA) system. It uses the kilocalorie (kcal) for energy, where 1 kcal = 4.184
kJ, and the Ångström (Å, 0.1 nm) for length (those are both exact conversions), degrees forangles, and ps for time. However, there is no reasonable consistent set of units in whichenergy is measured in kcals, so there is always a conversion involved in this system. * The
DuMM subsystem provides alternate methods dealing directly in kcals, Ångstroms, anddegrees so that users who think better in KA units can continue to do so, hopefully resulting
in a smaller chance of errors being made. Whenever we use these nonstandard units we
include “KA” in the method and argument names; any time no unit system is specified you
may assume we are using MD units as described above. And no matter which methods werecalled initially, anyone who looks at internal data should be aware that our internal units arekJ, nm, ps, and radians.

*

Typically, energy is calculated in the consistent unit of decajoules/mol (Da-A2/ps2) and then divided by 418.4

when no one is looking.

13111.3.4 Defining a force field
TODO
11.3.5 Defining the molecules
TODO
11.3.6 Defining bodies and attaching the molecule to them
TODO
11.3.7 Running a simulation
TODO
11.3.8 Theory
TODO

132

12 Appendix: derivations
This collects detailed derivations for and discussion about some of the results presented
earlier.

12.1 Notation for multibody theory
When discussing physical quantities that arise in multibody dynamics, we must be very
precise. We need to describe exactly what quantity we mean, how it was measured, and inwhat coordinate system we have decided to express the result. In the worst case, this canresult in a complicated forest of super- and subscripts, however there are defaults which
cover most cases. Here is the worst-case, fully-decorated symbol:“From” point orframeFExpressed-in, if
not M“To” point or
frame

M



QiB 


Type of quantity
and which instance

The type of quantity (the central black symbol) is the only required piece. The right subscript
conveys the particular instance being measured. The superscripts are bodies, frames, orpoints. When useful, points are considered to be the origin of a frame that is parallel to thepoint’s body frame, but with the origin shifted to the point. That frame is then considered the“measured in” and “expressed in” frame unless otherwise stated.
Here are the symbols we conventionally use for particular quantities, shown in nice typography and then the crude equivalent we have to use in code.
GG

The unique Ground body, and the inertial (Cartesian) reference frame fixed to it. Technically this is a MobilizedBody,
although it doesn’t do a lot of moving.

B
Bi

B
Bi

The mobilized body under discussion. The same symbol isused to mean the body frame associated with that body.

P
PB

P
Pb

The parent (inboard) body of the mobilized body underdiscussion, or the parent of a particular body B.

133

M
MB

M
Mb

The mobilizer frame for the mobilized body under discussion, or for a particular mobilized body B. The M frame isfixed to the body, and is related to the body frame by theconstant transform B X M .

F
FB

F
Fb

The fixed (reference) frame for the mobilized body underdiscussion, or for a particular body B. The F frame is fixed tothe parent body P, and is related to the parent’s body frameby the constant transform P X F .

FOFoThe origin point of some frame F.

BC

Bc

The mass center (a point) of some body B. By default this isthe vector from the B origin to the mass center, expressed in
B.

mB

mb

The mass of some body B.

GBGb

The gyration matrix of body B. By default this is taken about
the B origin and expressed in the B frame.

JB

Jb

The inertia of body B, where JB=mBGB. By default this istaken about the B origin and expressed in the B frame.

A

RB

R_AB

The 3x3 rotation matrix whose columns are the B frame’saxes expressed in the A frame.

pS
A B
p

p_RS

R

G A

[ pB ]XB

X_AB

The spatial transform (rotation and translation) expressing
frame B in frame A. A X B   A R B A p B .

A

VB

V_AB

A

V_AQ

The spatial velocity of frame B in A, expressed in A. This
includes the angular velocity of B in A and the linear velocityof BO in A as a stacked pair of vectors expressed in A. A
different point Q (fixed in B) can be specified as shown inwhich case the linear velocity is of Q in A rather than of BO.
Q can be considered a coordinate frame parallel to B but with
its origin shifted to Q.

A

VQ

134

The position vector from point R to point S, expressed in thesame frame as R. If R or S are the names of bodies or coordip_AB
nate frames, the origins of those frames are used as thepoints; that is, A p B  AO p BO . If the position vector is exp_AB_G pressed in a frame other than the “from” point’s frame, we
use the bracket notation shown.

A

AB

A_AB

A

AQ

A_AQ

The spatial acceleration of frame B in A, expressed in A.
This includes the angular acceleration of B in A and the linear
acceleration of BO in A as a stacked pair of vectors expressedin A. A different point Q (fixed in B) can be specified asshown in which case the linear acceleration is of Q in A
rather than of BO. Q can be considered a coordinate frame
parallel to B but with its origin shifted to Q.

MORE
TODO

The right superscript defines the physical quantity by specifying the frame to which a physical quantity is attached, and optionally a point other than the frame’s origin to which thephysical quantity is referred. The inertia of body B, taken about B’s origin would be JB, but if
the inertia were instead taken about B’s center of mass point BC , the symbol would be J BBC .
The left superscript specifies how we are to take the measurement of the physical quantity.
Typically this is just a frame F, so that the measurement is done with respect to that frame’s
coordinate system and from the frame’s origin, and by default the resulting measure numbers
are expressed in F. However, a “measured about” point can be provided which is different
from the origin. As an example, if body B’s center of mass point BC is to be measured in thelocal frame of another body A, we would write A p BC (a vector from body A’s origin to body
B’s center of mass point). If instead we want the vector from A’s center of mass to B’s, thesymbol would be AC p BC where the expressed-in frame A is inferred from AC . In both cases
the vector would be expressed in A. If instead it was to be expressed in the ground frame G,
we would writeG A pB   G R A  A pB .
C

C

C

C

Time derivatives with respect to the expressed-in frame are denoted with an overdot. For
exampleG A pB 
C

G

d
dt

C



G



G

  p GACBC

 A pC    A p B 
G

C

B

C

C

 A pC   G A   A p B 
G

C

B

C

C

135

where for any quantity Q expressed in frame A and an arbitrary frame B:

 AQ 
B A
 Q
B

B

R A  AQ

B

R A  AQ  ( BA  B R A )  AQ

 BA 

B AQ 

A

d A
Q
dt

A

Q

B

B
d B A
B
Q    AQ    AQ dt

 AQ 

B

B

  AQ   BA 

 AQ 

B

12.2 Re-expressing spatial quantities
For any quantity Q we use the notationZQ  to mean “Q re-expressed in frame Z.” Note that

this never changes the physical quantity being represented, just the frame in which themeasure numbers of that quantity are expressed. If v is a vector currently expressed in frame
A, then

Z

v 

Z

R A  v . If M is a tensor (matrix) currently expressed in frame A, then



Z

M 

Z

RA  M  Z RA

V

is

a

spatialZ

R A V Z RA 0Z

  R  M  R . We use similar definitions for spatial quantities. If
TZ

vectorA

A

Z

currently

expressed

in

frame

A,

 Z RA 0

 AV B   Z R A  AV B

Z A B 
0  A B  Z R A  A B      .
V Z A A B  Z
Z A
O


R 
 R  v    Av BO    M   Z R A  M  ( Z R A )T
 Z RA 0

  ARZMZA R   00

 Z G  m Z  p 



136

we

0 
V . For example,
Z A
R 

To re-express a spatial inertia matrix M from frame A to frame Z, write
Z

then

Z

p 
13 

 Z R A G ARZ m Z AA
Z R  ( R p)
0( Z R Ap) 
13 

definewhere we have made use of identity (3.6), the fact that if U is a 3x3 orthogonal matrix, thenU  v  UT  (U  v) . (Recall that rotation matrices are orthogonal.)

12.3 Rigid body shifting of spatial quantities
Rigid body shifting is used during processing of the multibody tree to transfer the effect of
inboard kinematic quantities (velocities and accelerations) in an outboard direction, and to
shift applied forces and spatial inertias from outboard bodies in an inward direction. The
rigid body shift matrix P S Q is used to shift a spatial motion vector (e.g., velocity or acceleration) at point Q to the equivalent spatial motion vector acting at point P. The transpose of thatQ

matrix S*P S  is used to shift a spatial force vector (e.g., force or impulse) acting at
P

Q T

point P to the equivalent spatial force acting at point Q, and both forms are used whenshifting inertias. The operators areP

so that

and

SQ

VP

F

Q

 1
P Q
 p

0

1

and

Q

S*

P

 1  P pQ 
1  S   0
P

Q T


  P Q Q  P   S V   Q
P Q
v 
v   p    Q P P    P pQ  f P .
 Q   S * F  
fP
f 



12.3.1 Rigid body shift of rigid body spatial inertia
To shift a spatial inertia matrix about a point Q to another point P of the same rigid body, if
stored using a gyration matrix, use

137

1
Q P
M P  P S * Q M Q  S  mB 
0Pp   G

 Q C1    ppC   1
 P Q
13    p



 







Q

Q

Q

G Q  Q pC 2  P pC 2
 mB P C pG C  P pC 2

 mB 
  P pC
 GP mB  P C
  p

0



1

pC 

13 

P

pC 

13 

P

pC 

13 

P

where P pC  P pQ  Q pC .
Note that there is no mention of expressed-in frame; the shifting operators assume that all
quantities are expressed in the same frame. Op counts for the above arepC  P pQ  Q pC3

 p , p 

22 (11 each)

P

Q

C


2

P

C


2

GP
TOTAL

12 (2 adds)
37 flops

12.4 Inversion of rigid body spatial inertia
In case you find yourself with a need to invert a rigid body spatial inertia, you can do it very
efficiently. We have symmetric, positive definite
 G
M  m  p

p 

13 

(12.1)

Then its inverse is also a symmetric matrix

M 1 

1 m  p

pp  133 *T

(12.2)

where   (G - p2 ) 1
So even though this is a 6x6 matrix, only a symmetric 3x3 need be inverted.
This can be a very useful optimization for lone rigid bodies on free joints to Ground, where
the joint matrix H is a constant identity matrix in Ground. In that case the mobility space138

mass matrix D  H T PH (see below) is just the body’s rigid body spatial inertia M reexpressed in Ground and is itself a rigid body spatial inertia.
Because of the orthogonality of rotation matrices, we also have1 M B F( F R B M B B R F )1  F R B (M B )1 B R F

(M B )1 
F(12.3)

which may permit the inverse spatial inertia to be usefully precalculated in some cases.
Note that an articulated body inertia is a general symmetric matrix so there is no “trick” way
to invert it as there is for a rigid body spatial inertia.

12.5 Articulated body inertia
An articulated body inertia (ABI) matrix P(q) contains the spatial inertia properties that abody appears to have when it is the free base body of an articulated multibody tree in a given
configuration q. Despite the complex relative motion that occurs within a multibody tree, at
any given configuration q there is still a linear relationship between a spatial force F applied
to a point of the base body and the resulting acceleration A of that body and that point:F  P(q) A  c , where c is a velocity-dependent inertial bias force. P is thus analogous to arigid body spatial inertia (RBI), but for a body which has other bodies connected to it by
joints which are free to move.
An ABI P is a symmetric 6x6 spatial matrix, consisting of 2x2 blocks of 3x3 matrices,similar to the RBI. However, unlike the RBI which has only 10 independent elements, all 21
non-repeated elements of P are significant. For example, the apparent mass of an articulated
body depends on which way you push on it, and in general there is no well-defined center ofmass. This is a much more expensive matrix to manipulate than an RBI. In Simbody's
formulation, we only work with ABIs in the Ground frame, so there is never a need to rotate
or re-express them. (That is achieved by rotating RBIs prior to using them to construct the
ABIs.) Thus only shifting operations need be performed when transforming ABIs from bodyto body. Cheap rigid body shifting is done when moving an ABI within a body or across aprescribed mobilizer; otherwise we have to perform an articulated shift operation which isquite expensive. For a full discussion of the properties of articulated body inertias, see139

Section 7.1 (pp. 119-123) of Roy Featherstone's excellent 2008 book, Rigid Body Dynamics
Algorithms.2
As a spatial matrix, an articulated body inertia is composed these of 3x3 subblocks:
 J
PB   T
F

F

M

(12.4)

Here M is the mass distribution (symmetric), F is the first mass moment distribution (full),and J is an inertia (second mass moment, symmetric).
12.5.1 Rigid body shift of articulated body inertia
Rigid body shifting of ABIs is done when an ABI is shifted across a prescribed (or welded)
mobilizer. This is done with a rigid body shift operator P S B as above. Here we’re given an
ABI PB expressed in the Ground frame, taken about the origin BO of body B. We would liketo shift it to the origin PO of its parent body’s frame P, crossing the prescribed mobilizer
connecting B to P.
1B P
P P  P S * B P B  S  0p   J

 T
1  F J F  TF  M 

F  1M    p

0



1

(12.5)

whereF   F  pM(12.6)J   J  pF T  F p(12.7)

Note that symmetry of J is preserved in equation (12.7) becausepF T  F  p  pF T  Fp  pMp  pF T  ( pF T )T  pTMp(12.8)and each of the quantities in {} is symmetric. Therefore we need only calculate the lower
halves of pF T and F  p which, if done carefully, requires fewer calculations than calculating the symmetric terms in (12.8) directly would. Flop counts are:pM (full)

140

24

F   F  pM

9half of pF  F  p
J
TOTAL

33
6
72 flops

T

This is a little less than twice as expensive as a rigid body shift of a rigid body inertia, which
isn’t bad considering there are more than twice as many meaningful elements in an ABI thanan RBI. Unfortunately, this is the easy case!
12.5.2 Articulated shift of an articulated body inertia
This is the single most expensive operation in the Simbody kernel. We have the articulated
body inertia PB of a child body B which we would like to shift to its parent body A, but
accounting for the movable (free, non-prescribed) mobilizer connecting B to A. That meansthat prior to a final rigid body shift from child to parent we have to remove the inertia projected on the current directions of the mobilities, since the parent can’t “feel” that inertia
through the floppy mobilizer in those directions. The projection of the ABI onto the mobilities is P  P B HD1H T P B and the final result we’re looking for is P A   AS * B ( P B  P) B S A .
Here D  H T P B H is a symmetric, positive definite nxn mobility-space mass matrix; H is the
6xn hinge matrix associated with the connecting mobilizer.
Free joint

66n

Pin/slider
joint
66

n2  112 n

11

231

n3 +n/, / = 10 (?)

10

240

12n2–6n6396

42n–2121
72 (see above)21
2
5 3
6 n  17.5n  117.5n  93

21

231

21
72
21
228

21
72
21
1608

( P B H )6nsym: Dnn  H T ( P B H )sym: D 1 (use Cholesky)

G6n  ( P B H ) D1
sym: P66  G( P B H )T
sym: P  P B  P
sym: P  AS * B P B S A
sym: P A  Ptotal

ntotal/per dof

11
2
5
6

0
1
2
3
4
5
93 228 404/202 625/209 896/224 1222/245

396

6
1608/268
141

This is unlikely to be the optimal computation of an articulated shift but it is the best I could
come up with for now. The above is a general-case treatment; there are many special casesthat could be exploited some of which are discussed below.
In addition to PA, several of the intermediate quantities in the above calculation are needed
for subsequent operations, such as calculating accelerations. These are D, D–1, and G.
12.5.3 Terminal bodies and base bodies
In a typical multibody tree, a substantial fraction of the bodies are terminal, meaning they
have no outboard children, or base, meaning their parent body is Ground. For a terminal bodyG

we have P B   M B  , that is, the articulated body inertia is just the rigid body inertia of B,
re-expressed in the Ground frame (and that is still a rigid body inertia matrix). We would liketo use that fact to reduce the op count required for the articulated shift of PB to its parent.
Although PB in the terminal-body case is an ordinary rigid body inertia, from inspecting theabove table it is not obvious how to get much out of that except a modest savings in calculating PBH because D, D–1, and G still need to be calculated and Pʹ, Pʹʹ, Pʹʹʹ, and PA are allarticulated body inertias. A few possible special cases:


If

H

is

constant

when

expressed

in

the

body

frame

B,

then

D  H T ( P B H )  ( H B )T M B H B is constant. (It doesn’t matter what frame you calcu-

late in since D is in mobility space which is its own coordinate system.) Thus D andD 1 can be precalculated. Unfortunately it is much more common for H to be constant in the parent body frame than in the child.
For base bodies we don’t need to update the parent’s (Ground’s) articulated body inertia since that is already infinite. Thus Pʹ, Pʹʹ, Pʹʹʹ, and PA calculates are all unnecessary.

Particles (lone point masses) and spherical rigid bodies are common special cases thatcan be handled extremely efficiently since their spatial inertia matrices are constantunder rotation.

142



A lone, free rigid body is a common case that can be handled efficiently. Here H canbe made an identity matrix in Ground or in the body frame. With H identity in
Ground, D is a rigid body spatial inertia, which can be inverted very efficiently (seesection 12.4). With H identity in the body, D is a constant as discussed above so canbe precalculated.

12.6 Modal analysis and implicit integration
In this section we discuss the related needs of modal analysis (that is, normal modes ininternal coordinates) and implicit integration. Both of these require that the system equationsof motion be differentiated with respect to the generalized coordinates and speeds. That is wewant to calculate the dynamic, internal coordinate Jacobian J qqJ   J uq
 J zq


J qu
J uu
J zu

J qz  q q q u q z J uz    u q u u u z J zz   z q z u z z (5)Modal analysis is typically done with all speeds set to zero, so only the submatrix Juq is ofinterest. If q is such that the system is stable (at a local energy minimum), then the eigenvalues of this matrix are the normal modes of the system about that equilibrium point and thecorresponding eigenvectors are the modal basis (that is, they represent the coordinatedmotion involved in each of the normal modes).
Given the system equations of motion, note that one can easily obtain an approximation to J
by perturbing the state variables (this is called a finite difference approximation to J). Simbody 1.0 should, at a minimum, support that method. However, it is both inaccurate andextremely expensive to compute. Finite differencing loses about half the available precision,and requires O(n) calculations of the system accelerations to form an nn matrix. In molecular dynamics straightforward force calculations are typically O(n2), so this can mean the
Jacobian calculation is a prohibitive O(n3). In any case the force calculations are very expensive and doing O(n) of them to get a half-accurate Jacobian is not a very good deal. Analytical methods exist which allow Juq to be calculated from the spatial force derivatives (energy
Hessian), to full accuracy and in much less time, with the total calculation being O(n2). Notethat this is within a constant factor of optimal for filling in a matrix with n2 elements.
143If possible, Simbody 1.0 should include a good modern method for calculating J analytically,
but if that can’t be done it should at least provide an interface designed to support such a
calculation in the next release.
For implicit integration the required matrix is the full J (with nonzero velocities) rather thanjust Juq. However, that is not much worse. Calculating the Juq submatrix is by far the mostdifficult part since it involves the Hessian of the potential energy and (formally) the partial
derivatives of the mass matrix inverse with respect to the q’s.

12.7 Root finding and optimization
The needed computations here depend on the kind of problems being solved. They typicallyrequire Jacobians of various calculations with respect to the generalized coordinates andspeeds. J as defined above can be very useful for minimizations involving search for equilibria. For satisfying constraints, the partial derivatives of the constraint equations (8.52) and(8.53) are required and Simbody can provide those analytically.
Root finding problems can be difficult when the coordinates are constrained, so it is convenient to define a new set of fully-independent coordinates. It is easy to create a localized 3coordinate representation for orientation about a current set of q’s which will remain valid
even for large perturbations, using the N and N 1 operators provided by Simbody to workwith the kinematic coupling matrix N. Reduced sets of coordinates for more general constraints may have limited validity ranges and have to be recalculated periodically during aroot finding or optimization run.

144

12.8 Operator form of Simbody interface(NOT DONE YET) The Simbody subsystem follows the response/operator/solver scheme
described elsewhere. Arguments in brackets indicate the stage at which the operator isavailable; other symbols are the runtime arguments.
Operator

Stage

MethodDescriptionq  N[ q ]u

Position

void calcQDot(State, Vector

Convert generalizedu, Vector& qdot)

speeds to generalized
coordinate time derivatives.

q  N[ q,u ]u  N[ q ]u

Velocity

void calcQDotDot(State,

Convert generalized speed

Vector udot, Vector& qdotdot)

time derivatives togeneralized coordinate 2ndtime derivatives.

f a  M[ q ] aForce

f c  G[Tq ]

void calcMa(State, Vector a,

Inverse dynamics. Can use

Vector& f)

as residual (implicit) formof equations :f bias  τ[ q ,u ]finv (u,  )  fapplied  0

finv  f a  f c  f bias

[q,u]

Force

a  M[q1] fvoid calcMInverseF(State,[t,q,u]Forward dynamics.

Vector f, Vector& a)
void calcTreeUdot(State,utree  M[q1] ( f  f bias )Vector f, Vector& udot)[q,u]uloop  utree  ucons
ucons  M 1G T λ(ε a (utree ))) a  G[ q ]a  b[t ,q,u ]Forcevoid calcAccelerationCon-

Maps accelerations a tostraintErr(State, Vector a,

the acceleration constraint

Vector& aerr)p  a   v  a [t ,q ,u ,u ]Acceleration

const Vector& getAccelerationConstraintErr(State)errors they entail.
Maps accelerations u tothe acceleration constraint
errors they entail.

145

p 
v    v  [ t , q ,u ]Velocity

const Vector& getVelocityCon-

Given a set of generalizedstraintErr(State)speeds u, return thevelocity constraint errors
they entail.

 p  p[t ,q ]Positionconst Vector& getPositionCon-

Given a set of generalizedstraintErr(State)coordinates q, return theposition constraint errors
they entail.

  (GM1G T )[q ]  a

Forcevoid calcMultipliers(State,

Given a set of acceleration

Vector aerr, Vector& lambda)

constraint violations,calculate the multipliers
needed to eliminate them.

f  J[Tq ] F

Position

void calcTreeEquivalentForc-

Given a set of body forceses(State, Vector_<SpatialVec>

and torques, convert them

bodyForces, Vector& jointForces)

V  J[ q ]u

Position

to hinge forces ignoring
constraints.
Given a set of generalized
speeds, compute theequivalent spatial
velocities of each body.

ke=ke[q](u)Position

Real calcKineticEnergy(State,

Given a set of generalized

Vector u)

speeds, calculate theresulting kinetic energy.

12.9 Misc
This is material that should probably be removed.

146

Quaternion position levelnormalization
onlyholonomic

n=0

Given: position(index 3)

PrescribedmotionLocal to eachprescribedmobilizer i.

prescribed
coordinates

q q
u u

nonholonomic(index 2)
acceleration
only (index 1)

holonomic(index 3)

Generalconstraints

velocity (index 2)
acceleration(index 1)
Given: velocity
acceleration(index 1)

acceleration
only (index 1)

up,i (t , qi )  Ni1 (qi )qi (t )ui  mp,i (t , qi , ui )mp,i (t , qi , ui )  up,i (t , qi )u  u v (t , q)u  mv (t , q, u)Given: positionp(qˆ )  0p(t , q , qˆ )  0

velocity p(uˆ )  0(index 2)

Nonholonomic(index 2)

ui  up,i (t , qi )u  ma (t , q, u )Given: velocityv(uˆ )  0û  uqi  qi (t )Given: acceleration

acceleration p(uˆ )  0(index 1)

free coordinates q̂  q

ni (q)  qiT qi  1

Pˆ uˆ  c(t , q, u )  0

p p ˆPˆ Nuˆ qˆ

P

p p

N
u q

 p

c  
 Pu tPˆ uˆ  bp (t , q, u, u )  0

b p  c  Pˆ ûv(t , q, u , uˆ)  0
ˆ  v
V
û

V

v

acceleration v (uˆ )  0(index 1)

ˆ uˆ  b (t , q, u, u )  0Vv

Given: accelerationa(uˆ )  0

ˆ uˆ  b (t , q, u, u )  0 Note that a must be linearAain û .

collect contributions from all the
shaded rows aboveu v vNu  Vu  t qbv   u  m(t , q, u )All index 1
constraints

mv (t , q, u)  u v (t , q)ˆ uˆ  b(t , q, u, u )  0
G Pˆ 
b p 
m p ˆ  Vˆ  b  b  m  m Gv
v
 Aˆ b a m a 

Table 1: the three classes of constraint equations dealt with by Simbody.

147

An in-progress attempt to include prescribed motion in the above table – please ignore for now.
Quaternion position levelnormalization
onlyni (q)  qiT qi  1

n=0

Given: position

qqp,i  qi (t )holonomic(index 3)

Prescribed
Local to eachmotionprescribedmobilizer i.

prescribed
coordinates

velocity (index 2)

acceleration(index 1)

uqp,i  up,i (t , qqp,i )

up,i (t , qqp ,i )  Ni1 (qqp ,i )qi (t )uqp ,i  m p,i (t , qqp ,i ,m p,i (t , qqp ,i , uqp ,i )uqp ,i ) u p,i (t , qqp ,i )qqp  qupuup  u p
up  unonholonomic(index 2)
acceleration
only (index 1)

holonomic

Generalconstraints

Given: velocity
acceleration(index 1)

uup  mv (t , q, u)Given: accelerationu p  ma (t , q, u)Given: positionp( qqf )  0p(t , qqp , qqf )  0

velocity p(uqf )  0(index 2)

Puqf  c(t , q, uqp )0

mv (t , q, u)  u v (t , q)

P

p
p

N qf
uqf qqf

Pp 

p
p

N qp
uqp qqp(index 3) p
c     Pp uqp  tfree coordinates

qqf  q  qqp

acceleration p(uqf )  0

uuf  u  uup

Puqf  b p (t , q, u, uqp )bp  c  Puqf

0

(index 1)

u f  u  up

Given: velocityv (uuf )  0
Nonholonomic(index 2)

accelerationv (uuf )  0(index 1)

148

uup  u v (t , q)v(t , q, uup , uuf )  0
Vuuf  b v (t , q, u, uup )
0

V

v
uuf

Vp 

v
uup

 v vNu  Vp uup  t qbv   

acceleration
only (index 1)

Given: accelerationa (u f )  0

Au f  ba (t , q, u, u p )0
u p  m(t , q, u )All index 1
constraints

collect contributions from all the
shaded rows above

Gu f  b(t , q, u, u p )0Note that a must be linear
in u f .
b p 
m p PG   V  b  b v  m  m v b a m a  A 

Table 2: the three classes of constraint equations dealt with by Simbody.

149

13 Acknowledgments
Simbody is built on the proverbial shoulders of giants. It inherits code from the publicdomain IVM molecular modeling module written at the NIH and kindly provided by Charles
Schwieters4 and the TAO robotics simulation and control code which was placed into open
source and contributed to SimTK by Arachi Corporation.16 It inherits ideas from earlier
efforts such as SD/FAST and Imagiro. In turn, these packages were based on fundamental
work in aerospace, robotics, and molecular dynamics by Dan Rosenthal and Michael Sherman at Symbolic Dynamics and Protein Mechanics, by Abhi Jain and Guy Rodriguez at JPL
and Cal Tech,3 and in Oussama Khatib’s lab at Stanford. The Simbody effort is intended tobring the best of these ideas together (and avoid some earlier mistakes) in a form that ispractical for use in physics-based simulation of biological structures over a wide range ofscales.
I thank Charles Schwieters for writing IVM, providing the source code, and helping me
understand the code. I am grateful to Bill Mydlowec for discovering the IVM paper in the
Journal of Magnetic Resonance and pointing it out to me. Thanks also to Oussama Khatib,
James Warren, K.C. Chang, and Diego Ruspini for help in obtaining TAO, and to Michael
Levitt and Vijay Pande for their guidance through the thicket of biomolecular simulation.
The Tinker molecular mechanics code, and in particular its author Jay Ponder, were very
helpful in constructing the DuMM subsystem, whose dummness is not Jay’s fault.
I thank Dan Rosenthal for patiently teaching me everything I know (plus much more which I
promptly forgot) about the fascinating field of multibody dynamics, and Linda Petzold for
inspiring me with her deep knowledge and intense enjoyment of the equally fascinating fieldof numerical integration and specifically for helping me learn to solve the system of equations (8.48)-(8.54).
This work was funded by the National Institutes of Health through the NIH Roadmap for
Medical Research, Grant U54 GM072970. Information on the National Centers for Biomedical Computing can be obtained from http://nihroadmap.nih.gov/bioinformatics.

151

14 References
1

Jain, A.; Rodriguez, G. Recursive flexible multibody system dynamics using spatial operators. J. Guidance,
Control, and Dynamics 15(6):1453-66 (1992).
2

Featherstone, R. Rigid Body Dynamics Algorithms, Springer (2008).

3

Jain, A.; Vaidehi, N.; Rodriguez, G. A fast recursive algorithm for molecular dynamics simulation. J.
Comput. Phys. 106(2):258-268 (1993).
4

Schwieters, C.D.; Clore, G.M. Internal coordinates for molecular dynamics and minimization in structure
determination and refinement. J. Magnetic Resonance 152:288-302 (2001).
5Jain, A.; Rodriguez, G. Recursive dynamics algorithm for multibody systems with prescribed motion. J.
Guidance, Control, and Dynamics 16(5):830-837 (1993).
6

Mitiguy, P.C.; Banerjee, A.K. Efficient simulation of motions involving Coulomb friction. J. Guidance,
Control, and Dynamics 22(1):78-86 (1999).
7

Ascher, U.M.; Chin, H.; Petzold, L.R.; Reich, S. Stabilization of constrained mechanical systems with DAEs
and invariant manifolds. Mechanics of Structures and Machines 23(2):135-157 (1995).
8

Eich, E. Convergence results for a coordinate projection method applied to mechanical systems with algebraic
constraints. SIAM J. on Numerical Analysis 30(5):1467-1482 (1993).
9

Baumgarte, J. Stabilization of constraints and integrals of motion in dynamic systems. Computer Methods in
Applied Mechanics and Engineering 1:1–16 (1972).
10

von Schwerin, R. Multibody System Simulation: numerical methods, algorithms, and software. SpringerVerlag Lecture Notes in Computational Science and Engineering (1999).
11

Johnson, K.L. Contact Mechanics, Cambridge University Press (1985). Chapter 4, especially section 4.2.

12

Hunt, K.H.; Crossley, F.R.E. Coefficient of restitution interpreted as damping in vibroimpact. ASME Journalof Applied Mechanics, Series E 42:440-445 (1975).
13

Goldsmith, W. Impact, London: Arnold (1960).

14

Marhefka, D.W.; Orin, D.E. Simulation of contact using a nonlinear damping model, Proc. of the 1996 IEEE
Intl. Conf. on Robotics and Automation, Minneapolis, Minnesota (1996).
15

Dupont, P.; Hayward, V.; Armstrong, B.; Altpeter, F. Single state elastoplastic friction models. IEEE Trans.
On Automatic Control, 47(5):787-792 (2002).
16

Chang, K.S.; Khatib, O. Efficient algorithm for extended operational space inertia matrix. Proc. of the 1999
IEEE/RSJ Intl. Conf. on Intelligent Robots and Systems (1999). The TAO source code is available in its originalform under an unrestrictive license on Simtk.org here: https://simtk.org/home/tao_de.

152