Simbody™ Simbody Theory Manual
User Manual:
Open the PDF directly: View PDF 
.
Page Count: 160
| Download | |
| Open PDF In Browser | View PDF |
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 from
http://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 the
lowest 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 and
includes 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 typically
two 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 and
multibody dynamics in particular, as well as detailed mathematical theory discussion and
literature references describing Simbody’s implementation. Generally the equation-dense
parts 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 from
the Simbody distribution project at https://simtk.org/home/simbody (go to the Documents
tab). Simbody also provides a public forum where you can get help, and bug report and
feature 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, and
slow 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.1
11.2.2
11.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.2
11.3.3
11.3.4
11.3.5
11.3.6
them
11.3.7
11.3.8
Background ............................................................ 128
Basic concepts ....................................................... 129
Units....................................................................... 131
Defining a force field ............................................. 132
Defining the molecules .......................................... 132
Defining bodies and attaching the molecule to
132
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.2
12.5.3
12.6
12.7
12.8
12.9
Rigid body shift of articulated body inertia ........... 140
Articulated shift of an articulated body inertia ...... 141
Terminal bodies and base bodies ........................... 142
Modal analysis and implicit integration ................ 143
Root finding and optimization .............................. 144
Operator form of Simbody interface ..................... 145
Misc ........................................................................ 146
13 Acknowledgments ...............................................151
14 References ...........................................................152
Unconstrained dynamic systems.............................. 64
Constrained systems ................................................. 67
v
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 the
classical mechanical properties, especially motion, of systems of bodies interconnected by
joints, influenced by forces, and restricted by constraints. The key feature of a system that
makes it suitable for multibody treatment is the observation that its motion is localized, that
is, 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 some
examples of the breadth of applicability of multibody mechanics, which has been used
effectively to model machines, skeletal motion and gait, coarse-grained biopolymers, and
many other systems relevant to a wide variety of scientific and engineering disciplines.
Figure 1: Some multibody
systems.
Mechanical U-joint
Articulated
skeleton
Protein
backbone
1
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 be
viewed as a basic numerical capability fundamental to any simulation system. It is in the
same category as, say, a linear algebra library, not an end-user application. Simbody is for
use 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 the
superiority of their new methods. However, Simbody itself is not a research project; it is
intended 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 a
physical 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 of
variables whose values can affect the System’s behavior. However, the System itself is an
unchangeable, 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 System
and 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
Results
State
states
System
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 call
instance 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 of
investigation 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 at
which 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 measures
3
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 System
itself. 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 into
a small number of Subsystems. In a higher-level modeling layer, one would expect to find
hierarchical models, which are a powerful way to represent the physical world. However,
computational resources are flat, not hierarchical, and the System/Subsystem scheme is a
4
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 is
ready 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 the
representation of interconnected massive objects (that is, bodies interconnected by joints).
Simbody can use this representation to perform computations which permit a wide variety of
useful 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 electrostatic
interactions? 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,
State
the general System described above is specialized to look something like this:
Forces #1
Matter
Forces #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 stateless
object 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 a
good intuition. This is both blessing and curse, since our intuitive understanding of these
concepts is almost, but not quite, general enough or precise enough to serve as a basis for
general 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 three
mutually 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 so
that 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, and
then 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 denoted
F
r (rx , ry , rz ) (r Fx , r Fy , r Fz ) . That is, the measure numbers are the scalars obtained by
taking the dot product of a vector with each of the three axis directions of the expressed-in
frame. 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 from
their 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 a
frame 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 to
8
more intuitive “body” concepts like mass and geometry; however, it is the frame that is a
body’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 the
hole 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 talk
about 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 33 symmetric
matrix) 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, mass
properties 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 the
idea of constraining the motion of otherwise free bodies—in Simbody, bodies start out with
no 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 a
mobilizer, although they are easily confused. We reserve the term “joint” to refer to the
physical-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 not
with mobilizers, as the latter are restricted to connections between bodies and their unique
parents. 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 be
mobilizers 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 described
in terms of its bodies and joints, in general there will be many ways to decompose that
system 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 more
intuitive 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 modeled
differently, and there are no coordinates in the system directly corresponding to the motion of
that 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 the
bodies 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 be
constructed without the use of constraints. There is, in fact, already a screw mobilizer that
has 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 be
built as a 1-dof mobilizer so that a single unconstrained coordinate is used to represent the
complicated 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 n
mobilities 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 a
system composed of Ground and one moving body, a wheel, having a single mobility with
respect 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 the
wheel about an axis. Additional mobilities would have to have been granted to the wheel in
order 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 find
than 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 know
that 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 the
parameterization for each body separately. The set containing all these parameters is then the
parameterization of mobility for the multibody system as a whole.
The independence of body mobilities localizes the parameterization issue to the mobilizer for
each body. Each mobilizer must define two sets of scalar parameters to express particular
values for its mobilities, one set to specify the relative positioning (configuration) and the
other to specify the relative velocity (motion) between the parent and child bodies. Parameters used for positioning are conventionally called generalized coordinates; parameters for
velocity 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 general
motions. We similarly use generalized forces to mean both forces, torques, or more general loads which are
applied 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 must
satisfy a normalization constraint, leaving only the expected three degrees of freedom for the
four 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 a
multibody system. It will usually be clear from context whether we are referring to the
coordinates of the whole system or just one body, but if we need to be specific we use qB and
uB 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 by
mobilizers and refer to them instead as deformation coordinates and deformation rates.
Various techniques can be used to determine the appropriate representation of deformable
bodies. 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, we
would 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 space
can be expressed (that is, make mobility space=constrained space), and that is a very good
thing 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 for
constraints 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 be
applied 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 are
called 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 which
efficiently 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 energy
function. Non-conservative forces dependent on time and velocity exist as well but may not
contribute 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 is
fast 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 more
familiar 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 speeds
that 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 are
based on kinematic calculations.
2.7 Dynamics
Dynamics is concerned with the relationship between forces and accelerations at a fixed
value 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 some
accelerations 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 in
theory discussions. We’ll present both the mathematical notation and definitions for these
objects 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 part
of 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 of
variable dimension, and names containing “Vec” and “Mat” to mean small, fixed-size objects
of known dimension. The types we use most are the fixed-size Vec3 and Mat33 types and the
variable 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 in
stations, 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 the
frame’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 to
be specified.
19
3.2.2 Directions
Directions are unit vectors, which are Vec3s with the additional property that their lengths are
always 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. This
has concrete performance benefits because this unit length guarantee means that we can track
length-preserving operations at compile time and avoid unnecessary normalization checks, or
worse, 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 of
writing 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 physical
interpretation, 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 a
set of methods which can be used to construct a rotation matrix from a wide variety of
commonly-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 to
ensure that certain properties are maintained. Those properties are: each column and row is a
unit 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 cross
product 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
G
Bx
G
By
G
Bz
(Bx is the x-direction unit vector of frame B, with measure numbers expressed in B’s frame,
while the operator
F
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, we
G
write
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 a
rotation G R B and a vector Bv expressed in B, you can re-express that same vector in G like
this:
G
v G RB
B
v . To go the other direction, we can write
B
v B RG
G
v ~ G RB
G
v . As a
C++ 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 order
reversal with the “~” operator). So given
B
RC B RG
G
“–” so ~ G R B
RC ~ G R B
G
G
G
R B and
G
RC
we can get
B
RC as
RC . Note that the “~” operator has a high precedence like unary
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 actually
just a change in point of view and involves no computation or copying of data. That is, the
operations 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 of
both 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 ]
0
G
[ Bz ]
G
0
p B
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
G
from to
X
so
X B means “the transform from frame G to frame B,” or “frame B measured from and
expressed in frame G.” Another way to interpret G X B is that it represents the operations that
must be performed on G to bring it into alignment with B (a rotation and a translation). Then
as for rotation matrices described above, we can interpret
G
XB
B
X C as a composition of
operators 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: three
augmented vectors and an augmented point. An alternate, and entirely equivalent, way to
view 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 a
transform, makes this use of “~” very attractive and natural to use in practice.
22
G
B
X
0
G
R
0
B
G B
p
0
1
In our implementation, the physical layout of a Simbody Transform is just the three columns
of the rotation matrix followed immediately in memory by the translation vector, that is,
G
XB
G
RB
G
pB
34
. There is no need for the fourth row to be stored in memory since it is
always the same.
Given a Transform, you can work with it as though it were a 4x4 matrix, or work directly
with the rotation matrix R and translation vector p individually, without having to make
copies. 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 typical
using 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 another
frame 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 and
conveniently provided with a distinct inertia class. So we provide a class Inertia which is
stored 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 about
another point.
For convenience we combine all the mass properties into a MassProperties class, which
contains a mass, a center of mass location, and an inertia matrix. Note that there is implicitly
a 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 useful
in mechanics. Following Jain and Rodriguez5, we use spatial notation which combines
translational and rotational quantities into a single object. Using Simmatrix we define the
convenient 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 a
linear 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 some
circumstances.
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
XB
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 a
programming 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 cross
product matrix such that for any vector w, v w v w . Spelled out in scalars, the cross
product 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 vT v .
We will occasionally make use of the following identities:
( v + w) v w
v2
(3.2)
vw wvT wT v13
(3.3)
vT v v v vT v13 vv T
(3.4)
( 13 is a 3x3 identity matrix.) Note that vT v is a symmetric matrix with non-negative diagonal elements. Spelled out in scalars,
v2
v y2 vz2
vT v vx v y
v x v z
T
vx2 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 the
origin.
U v U T (U v) ,
where U33 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 understood
from 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 mass
center 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
G
M B mB B B B
O
C
p
BO
p BC so
pBC
13
BO
Note that when the spatial mass properties are given about the center of mass BC we have
p 0 so
M
BC
B
G BBC
mB
0
0
13
Where the central gyration matrix is
G BBC G B (pTp13 ppT ) G B pT p G B p2
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” its
center 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 pT 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 pT 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 rotation
A
spatial shift
P
A
spatial transform
XB
AO
AR B
RB
0
1
SQ P Q
p
A
0
RB
(3.8)
0
1
AR B
S BO A R B A B A B
O
O
p R
(3.9)
A
0
RB
(3.10)
Then a spatial vector or spatial matrix can be rotated, shifted, or transformed using these
matrices, 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 A
O
O
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, so
A
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 pQ
1
0
A R B AO pBO 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, like
velocities 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 but
have 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 or
articulated) 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 A
A
A
S* 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
system
The Simbody API (application programmer interface) assumes that the caller has made all
modeling decisions and simply wants to perform calculations on the model. The primary
decisions to be made are (1) how the physical model is to be decomposed into a particular set
of 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 the
system 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 a
whole, 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 can
take 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 a
mass 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 initial
construction, 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 to
have 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 and
expressed 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 the
mobilizer. You must be able to express F’s configuration on P as a transform
P
XF
from P to F.
The kind of mobilizer to be used to connect B to its parent body P, and whether to
reverse 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 mobilizer
of 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 of
physical significance except when P is ground in which case it really is “fixed.”
B
BC
BO
M
MO
F
FO
Figure 4: Adding body B to a
multibody tree already containing
parent body P.
P
PO
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 to
numerical 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 those
of 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 be
arbitrary 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 and
F frames are in their reference configuration.
Figure 5: The reference
configuration for the
mobilizer added in the
previous figure.
F
M
BC
P
PO
BO
B
Those users familiar with SD/FAST’s reference configuration should note that the above is a
different 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 then
defines the mobilizer (SD/FAST joint) frames from the reference configuration. We think it is
34
much more natural to express the joint frames separately in their bodies’ frames, and then
define 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 by
introducing 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 on
another 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 less
mobility 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 a
redundant constraint would be adding the same Constraint twice—nothing changes since
mobility coordinates which satisfy the first Constraint also satisfy the second. An example of
an ineffective constraint would be restricting the distance between a point on the outside of a
wheel 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 the
system 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 a
station 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 in
space, generating six constraint equations and thus removing six degrees of freedom from the
system. 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 body
index that was returned at the time they were added.
(Distance or Coincident Points Constraint) A station point PA fixed on body A and
station 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 constant
during 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 transform
36
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 by
inspection 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 and
velocity 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 variety
of 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 the
generalized 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 same
accelerations. Calculating this equivalent set is an important Simbody capability, since the
equations of motion are written in terms of the mobilities, while forces are typically known in
terms 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 is
37
to provide the information needed by the modeler to calculate the forces, and then to respond
to 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 n
mobilities (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 and
mobilizers; 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 B
q,u
body B
FO
P
Figure 6: Coordinate frames for use
in describing the mobility of
MobilizedBody 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
Ground
These 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)
F
R M ( q)
F
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
F
F
F M
V M F M F HM u F HM u
a
AM ( q , u , u )
F
q 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
( V
F
F
F
M
F RM
F
pM
M
M
) F R M
F
vM
(5.6)
VvM
F
This implies relationships that must hold among X, H, and N:
F
R M FM F R M ( F HM 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 is
stating the requirement that the spatial velocity produced from u by the action of H is the
time 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 the
definitions of the individual q’s.
Equation (5.5) specifies additional constraints that q must satisfy if there are not enough
equations (5.4) to uniquely specify q. That occurs when there are more q’s than u’s, typically
because 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 parentchild along each branch going
away from Ground, the role of parent and child may be reversed from the desired sense for
some mobilizers. When the generalized coordinates and speeds are intended to have a
particular 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 of
whether 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
HF ,
M
M
XF ,
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
XM ,
F
HM ,
F
H M , and N describing the mobilizer with
identical generalized coordinates and speeds in frame F fixed on the parent body (and with
time derivatives taken in F).
N is easy enough—since we want q and u to retain their original meanings, N must stay the
same. 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 to
work to get the other two matrices. First F H M :
From Equation (5.2) (with the frames swapped) we have
M
M F M HF
V M F M F u
v Hv
F
(5.9)
We want to find F H M such that
F
V
M
F M F HM
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 the
physical quantities. We’ll re-express at the end. We know that F M M F so it follows that
F
HM M HF . But the linear velocity cannot simply be negated. We have
42
M
M
vF
M
pF
F
M
F
M
d
dt
M
pF
(5.11)
d F M
p
dt
(5.12)
F
v
p
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 pF M HF )u
Since F v M F HvM u we see that
F HvM ( M HvF M pF M HF )
M
(5.14)
In the above analysis we left the quantities expressed in the M frame as emphasized in
equation (5.14) (although we took the derivative in F). Re-expressing in the F frame completes the computation of
F
F
HM :
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 FM 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
HF
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
F
HM F R M M HF
F
H vM F R M M H vF F R M
M
pF M HF
F R M M H vF F R M
M
pF ( M R F F HM )
(5.18)
F R M M H vF
( R
F
M M
F
RM
M
H p
F
v
F
M F
pF
F
HM
(5.19)
M
H )
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
HM F R M M HF FM F R M M HF
F R M M HF FM F HM
F
(5.20)
H vM ( F R M M H vF F M F R M M H vF F pM F HM F vM F HM )
( F R M M H vF F M ( F H vM F pM F HM )
F pM F HM F vM F HM )
( R
F
M M
H
p
F
F
v
M F
F
M F
H
(5.21)
M
v
H ( v F M F pM ) F HM )
M
F
M
Or, collecting terms
F
H M F R M M H F FM 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 these
computations.
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 constant
transforms
P
X F and
M
X B depicted in Figure 6. First, perform a rigid body shift of the
spatial 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)
where
F
Mp B FR M M p B
1
( v )
0
A
B
A
v B
1
Note that although we are shifting from one point on body B to another, the effect is time
varying 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 the
reference 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
A
B
v B
0
A
(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)
G
H*
d *
H G RP P HB G RP P HB
dt
G
G
(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 the
expressed-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 a
very small number nb of bodies and nm of mobilizers, typically one, two, or three, which we
call 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 of
all 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 of
mobilities 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 the
most-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 the
path from the kth constrained body inward to AC the kth branch of the Constraint; these
branches 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’s
nC 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 be
determined 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
bodies
B0
branch 1
u b ,1
branch 2
u b ,2
branch 0
u b ,0
ub ub,0 ub,1 ub,2
u ub um
outmost
common
ancestor
A
Figure 7: Constraint topology. This shows
a single Constraint C with three constrained
bodies Bk , the corresponding branches, and
the outmost common ancestor body A . The
branches determine the participating
mobilities u b ; mobilities of the two blackoutlined bodies are shared between
Ground
branches 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 n
participating generalized speed derivatives u ), called an acceleration constraint. Every
48
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 scalar
acceleration 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 the
physical consequences of ub having been supplied by Simbody after an O(n) computation.
Similarly, the meaning of the Lagrange multipliers λ is given by
fi (q, i ) gTi i
(6.2)
where fi is a column vector function giving the n generalized forces generated by the scalar
multiplier λi allocated to the ith constraint equation. Every defined Constraint must provide a
method for efficiently calculating its forces given its m multipliers λ. Again, except for
participating 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 to
bodies. The user-written routine is written as a constant-time function in those terms, and
then 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-only
49
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
NC
(6.9)
(6.10)
u
a j
(6.11)
u
and the remainder terms produced by differentiation are
c j (t , q)
p j
(6.12)
t
bp, j (t , q, u) c j p j u
v j v j
bv , j (t , q, u )
q
t
(6.13)
q
(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 by
differentiation 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 for
constraint 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 the
constraint 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 a
straightforward matrix multiply would be O(nm), much more expensive.) Simbody uses the
methods that define the constraint in combination with O(n) operators to perform these
computations 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 m
calls 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 acceleration
level, that is, equations (6.5), (6.7), or (6.8). Taken together, these equations are just the
equations (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 u
of 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 of
taking a new set of values from a State and performing the computations that those new
values enable. A simple example would be to take new position coordinate values from a
State 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 of
computations: 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 the
State. 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 the
accelerations that would be produced by those forces. Neither responses nor operators make
changes 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 a
particular 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
Inputs
Solvers
Back to State
Figure 8: After realizing a State, a
subsystem 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 State
values. Given that a System is a read-only object, and that realization results are associated
with 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 as
well, 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 in
simulation 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 to
differ 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 the
54
way, say, small measurement errors affect real-world experiments. Simulation software is the
most 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 with
molecular 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 are
simply intermediate calculations which have been derived from the state, and can easily be
discarded 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 call
this 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 the
State to realize responses, operators
and solvers. The realization cache adds
no new information but shadows the
state 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 and
solvers can have additional inputs (the blue arrows in the figure). Operators and solvers then
differ 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 produced
by
realizing 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 the
positions 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 for
Simbody, 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) each
force 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 approach
allows 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.
Report
System or
subsystem
State
Figure 10: The conceptual organization
of a computation into ordered stages. A
given stage is fully realized by each
subsystem in a System before the next
stage is realized by any subsystem. For
many purposes, construction of the (read
only) System can be viewed as the initial
stage 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 state
is stored in a SimTK State object, where it is partitioned into time
t , 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 the
types 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, and
structured 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 the
lowest affected computation stage the state variable’s stage. As shown in Figure 10 stages
that can be affected by state variables in
are: 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
,
report
model
,
x pos y pos
x pos q
vel
x vel y vel
x vel u
x force y force
x force z
For the other stages, only variables in the x partition can be used, so we have
report
time
,
x time {t}
pos
force
xinstance , and
,
with
time
instance
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 and
58
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 and
cache 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 the
set of resources in a System’s State can vary based on the current stage, and the settings of
state 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 of
those 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 the
values of existing state resources may be changed.
Here are the resources supported by a State object.
Resource
Allocation
Description
Built-in
resources
Topology
Every State has some built-in resources,
allocated at Topology stage. Note that the
System stage can never be higher than
the lowest Subsystem stage.
Dynamic
variable group
(q,u,z)
Topology
Model
size: Instance
A contiguous array of real-valued
“conventional” state variables. These are
pooled into a global array y, which is
itself 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 notes
t , t prev
System current stage
Per-Subsystem current stages
Low-water mark
y {q, u, z}, q
y prev {q prev , u prev , z prev }
sizes
Subsystem-private group Id
Pool slots assignment
59
Structured
variable x
Topology
Model
Constraint
group (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 are
contiguous arrays of m scalars for
holding the current constraint errors and
multiplier values. An additional state
variable is allocated to hold the size m of
the group which can be set at Instance
stage.
Allocates contiguous array of scalar
cache entries for event trigger function
values, in global pool. The number of
entries can be changed up to Instance
stage.
Allocates a group of event ids for a
subsystem, and a corresponding global
pool of System-unique Ids.
Previous value
Private variable belonging to a single
subsystem. Can hold any designated
object type. Has depends-on stage and “is
valid” flag. Must be allocated before
depends-on stage.
Subsystem-private Id only
x prev
Subsystem-private Id only
yerr {qerr , uerr }
uerr , λ
m
Subsystem-private group Id
Pool slots assignment
e, 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 resources
State variables are allocated by the various elements of a System. Here are the System
elements 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 ] are
sets 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 their
time derivatives, with nq [ j ] n[ j ] . q and u are related via an nq [ j ] n[ j ] kinematic coupling
60
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
j
u[ j ] and q
their elementwise time derivative variables u and q . Sizes are n
nq
q[ j ] , and sets of
j
u j n[ j ] and
q j nq [ j ] . We also define block diagonal nq n matrix N=diag(N[j]) so that
q 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 d
There 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 a
particular 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
subsets
p[i], v[i], a[i] g[i]
of
cardinality
m[i] mp [i] mv [i] ma [i] . We also define aggregate sets
mp [i], mv [i], ma [i]
, ,
and
such
that
with
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 , an
implicit relationship among a subset of generalized speeds with explicit dependence on t and
q. Note that there is not necessarily any correspondence between the sets qk and uk in a
velocity 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 and
uk in a velocity constraint; uk can depend on time and any sets of q’s and u’s. If uk 1 then
ak defines uk explicitly as uk uk (t , qk , uk ) , this is called a “prescribed acceleration”. Each
acceleration 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 a
different 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 unless
otherwise specified. We use a right superscript to denote a quantity which applies only to a
particular body or its mobilizer.
As detailed in section 7.4.1, the equations of motion will be written in terms of the set of n
generalized speeds u
b
u[b] where b ranges over all the mobilized bodies, and the nq
generalized coordinates q
b
q[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 which
must 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-time
equations 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 same
as 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 do
that). 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 are
q N(q)u
n(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 )
t
u (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 Mnn 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) is
the set of all applied force and torques (including gravity) mapped into an equivalent set of n
generalized 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 is
zero 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 the
forces 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 gravitational
model. 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 we
need 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, we
consider this an unconstrained system since these constraints do not affect the physical
solution.
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 may
themselves be modeled as differential equations. z’s cannot directly affect positions and
velocities, although of course they do affect accelerations which will ultimately affect
velocities and then positions. Note that z’s time derivatives z can depend on u but not vice
versa.
Formally, we can solve equation (8.4) for the accelerations u with
u M1 (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, while
a 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 of
dimension 3na3na 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 is
simply the Cartesian forces acting on each coordinate of each atom, typically resulting from
taking the gradient of the potential energy function. This represents a set of 3na uncoupled
scalar equations for the Cartesian accelerations of each atom, which can clearly be solved in
O(n)!
In a more general multibody system M will be dense as a result of coupling produced by the
internal coordinates. Use of quaternions for orientation results in there being more q’s than
66
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 of
multibody 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 Gmn=G(q) and bm1=b(t,q,u), m is the number of constraint equations and n=nu is the
number 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 M1 (fapp fbias ) , uC M 1G T , and (GM1G T )1(Gu0 b) . In general the constraint matrix GM G can
be 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 the
unknown 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 for
the generalized forces that implement the prescribed motions:
uf 0
M(q) f fapp (t , q, u, z ) fbias (q, u )
up
upd[i] pres
u [i](t , q pf , u pf ),
u p [i] p
0,
upd[i] fast or slow
z[i] z[i](t , q, u, z, u, )
(8.13)
(8.14)
(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, for
a subset of the prescribed speeds we will also know prescribed coordinates q p as a function
of 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 fp
M fp u f 0 f
M pp u p
f f f bias,f
f f
p bias,p
(8.19)
Multiplying out the blocks and moving the known quantities to the right hand side gives
u p u p (t , q, u)
M ff u f f f fbias,f M fpu p
f p fbias,p MTfpu f M ppu p
(8.20)
(8.21)
(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 prescribed
motion 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 fp
M fp
, G G f
M pp
Gp
(8.25)
Partitioning the right hand sides analogously, the equations of motion are now
u p u p (t , q, u)
M ff
T
M fp
M fp u f G Tf
0 f f f fbias,f
T
M pp u p G p
f p f bias,p
G
f
uf
G p b
up
Multiplying the blocks out and moving the known terms to the right hand side gives
70
(8.26)
(8.27)
(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)
where
fˆ f f f fbias,f M fpu p
bˆ b G pu p
fˆp f p fbias,p MTfpu f M ppu p
(8.33)
(8.34)
(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-singular
matrix 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:
u
dyn(f , u p ) f
1
M ff (fr f bias,f M fp u p )
f f
T
p bias,p M fp u f M pp u p
u f M ff1 0 f f
minv(f )
0 fp
0p 0
(8.39)
(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
Xnm 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 a
column Xi :
M ff1G iT
T
Xi
minv(G i )
0
p
and then
Wf (i) G f Xi aerr (Xi ) aerr0
(8.46)
(8.47)
to build up Wf G f M ff1G Tf in O(m n) operations as needed to solve equation (8.37) for a
least 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 the
velocity level as restrictions on u. In these cases we time-differentiate the constraints twice or
once, resp., until we have corresponding acceleration constraints, and then use them in
equation (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 or
velocity level, we differentiate twice or once and then use the result as equation (8.29).
Following this procedure yields correct accelerations, but with approximate numerical
integration 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 gradually
drift 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 in
a 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)u
n(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 and
prescribed 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 have
application, 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, like
a, they restrict the allowable values of u and in general they will be coupled to a. Prescribed
velocity 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 like
this 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 about
solving equations (8.48)-(8.54).
8.6 Unilateral constraints
Normally a constraint generates whatever constraint forces are necessary in order to
satisfy the acceleration-level expression of that constraint. Some constraints are
limited, 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 must
first 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 hold
75
for every acceleration-level unilateral constraint equation: if the force is nonzero then the
constraint error must be zero (constraint is active), or if the constraint error is nonzero then
the 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 , or
pi 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 efficient
solution 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:
State
violated
separated
separating
inactive
impacting
candidate
active
Conditions
p tol
p tol
Action
Illegal; must correct.
p tol ,
p vtol
p tol ,
p vtol ,
Ignore constraint.
p 0 and 0
p tol ,
p vtol ,
p, unknown
Perform impulsive MCP; new
p 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 the
velocity state variables due to impacts. The response to an impact requires user specification
of 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 is
small 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:
State
violated
separated
inactive
candidate
active
v vtol
v vtol
Conditions
Action
Illegal; 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:
State
candidate
inactive
active
Conditions
a, unknown
a 0 and 0
a 0 and 0
Action
Perform MCP to determine a,
Ignore constraint.
Obey constraint.
Note that a unilateral acceleration-only constraint is always a candidate and can never be
violated.
8.6.1 Solving for impacts
When we have determined that an impact has occurred, we need to make a discontinuous
change 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 so
that 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 of
constraints (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 the
square 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 will
often 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 constraint
c.
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, call
that 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 constraint
c.
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 time
and 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
M fp u f G Tf
M pp 0 G Tp
G f
c
f
cTf
0 fI , f
f
T I
c p
I fI , p
G p u f
c p 0
0
vc
(8.56)
(8.57)
(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 the
constraint violation velocity. For Poisson restitution, we will always want to calculate the
impulse 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 the
same 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
WI G f 1
M f vc
c f ff I , f
u f Mff1 ( f I , f G Tf
where
cTf I )
(8.59)
(8.60)
I f I , p MTfp u f G Tp cTp I
(8.61)
G f
W
cf
(8.62)
1 T
M ff G f
cTf
Note that we do not need to determine the prescribed motion impulses in order to solve for
the constraint impulses and changes to the generalized speeds. As before, W may be singular
so 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 that
are: 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 when
sticking, 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 and
position, 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, these
coefficients 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 in
turn 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, and
sliding 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 the
previous 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, although
with a switching condition (a discrete nonlinearity). There are two cases, depending on
whether 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 violation
of a unilateral constraint (i.e., a negative constraint multiplier), a higher-level algorithm will
revise the set of active constraints; we do not make that choice here.
The friction force multipliers are given by
81
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 that
element 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, defined
sign( 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 is
just 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 for
d, 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 to
82
generalized forces (that is, S=S(t,q,u) is a kxn matrix formed from the slip directions v and
kinematic 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 unique
solution we want:
W (GM1 f b)
(8.74)
where superscript + indicates pseudoinverse. This is identical to Simbody’s usual calculation
of λ except for the new term TT DμS , which makes W non-symmetric. That doesn’t matter for
us 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 d
and 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 the
error 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 is
chosen for it. The dotted blue lines show the incorrect values of σ. Note that the error is twice
as 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
Mu G T ST 0
Gu 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, and
the 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 is
dependent on a normal force that is the magnitude of a vector. Examples are a 2-multiplier
point-on-line constraint (with sliding friction along the line), or joint-like constraints such as
a 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 λi
for λ, we can approximate N locally to first order as
85
N ( i ) N ( i ) N i O( 2 )
N i
where
N
(8.78)
(k m)
i
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 i
i
(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 Ni 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 with
each 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 to
determine 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 of
static friction may be much larger than the coefficient of dynamic friction, so there may be a
large change in magnitude at the stictionsliding transition. This may cause a change in
direction that could make the final stiction force direction differ substantially from the
direction 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, and
the 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)
ea a
where 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 the
last 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 be
violated, 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 unilateral
contact’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 the
active set after the instantaneous velocity change has been completed. Figure 13 outlines this
flow of control during a time stepping study.
88
Start
Advance time
using current
active set
Event?
No
Contact event
Determine new
active constraint
set
Contact
Handler
Impact
event
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, the
handlers 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 or
reverses).
8.7 Dynamic solution method
The previous section glossed over some details of the system formulation that we’ll need to
deal 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 and
represent meaningful physical properties of the system. Holonomic constraint equations
involve only state variables at position stage or below, that is, q, t, parameters (instance
89
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 meaningful
physical properties. Typical examples are “non slip” conditions like rolling or gears, but
these 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 to
accelerations 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), each
of which involves only the coordinates of a single mobilizer and is present for numerical
reasons rather than physical. These are produced by mobilizers which use quaternions to
permit 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 three
physical 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 by
construction 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 to
quaternion 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:
q Nu
n( q ) 0
kinematics
dynamics
M G T u f fbias
G 0 b
z z (t , q, u, z )
(8.85)
(8.86)
velocity manifold
Pu c 0
v(t , q; u ) 0
(8.87)
position manifold
p(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) are
satisfied, 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
auxiliary
q Nu
n( 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
0
p
Pf u b q
f
uu p bu p
0
v
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 time
fast )
r pos (t , d , ydyn ;
d pos
fast , q fast ) 0
r vel (t , d , ydyn , q fast ;
d vel
fast , u fast ) 0
r acc (t , d , y;
(8.98)
(8.99)
0
r time (t , d , ydyn ;
r force (t , d , ydyn , q fast , u fast ; d force
fast , z )
(8.97)
(8.100)
0
d acc
fast , u fast , z ) 0
Relaxation constraints (8.100) should be solved in stage order as shown, so that once fast
variables have been calculated at one stage they can be dependencies in a later stage. Note
that 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, and
doesn’t show which variables in the state’s d partition can actually serve as dependencies; the
rule is: any d variable is allowed except those in dfast which haven’t yet been computed in the
relaxation 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 and
algebraic 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 are
arbitrary. 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, the
resulting physically equivalent models are not numerically equivalent so can affect the actual
solutions we obtain when doing computations, which are necessarily approximate. Such
computations involve significant tradeoffs between CPU time and accuracy, so we usually
want to use the loosest accuracy that is adequate for our purposes. The goal of scaling is to
ensure that an accuracy specification (e.g., “1% accuracy”) can be applied in a physically
meaningful way so that the behavior of a study is not dominated by arbitrary modeling
choices. That is, we would like a given accuracy specification to yield the same physical
results 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 each
state 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 state
variables to some fraction of that unit change, and satisfaction of algebraic equations to some
fraction 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 avoid
95
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 without
bound, adding 2π with each revolution. There is no configuration change associated with
those 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 a
block should behave the same way in one place as another. Similarly, any change in the
spatial location of a system involves changes to translational variables, yet should not affect
the precision of results. Hence Simbody solves all configuration coordinates q to an absolute
accuracy level independent of their current values. Note that this does mean that there can be
a 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 not
appear 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 absolute
accuracy requirement should be used instead. The absolute accuracy requirement for a
variable 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 involve
the 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 variables
y {q, u, z} vary widely in effect, or weight, by which we mean the degree to which a unit
change in the numerical value of a state variable affects a physically meaningful quantity of
interest. 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 even
more pronounced in internal coordinate formulations like Simbody’s, since the effect of a
state variable depends strongly on its position in the multibody tree. A unit change δqi to an
angular 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 more
accurately 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 than
97
q2 with respect to the location of P. If we consider a unit change of P to be 1nm, we can
define 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 “small
change.” 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 not
constant but change as a function of system configuration. Thus weights may need to be
recalculated periodically as a system moves during a study. Fortunately, in practice
weighting 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; in
practice 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 not
allowed 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 a
system in which q u . In general, however, we have q N(q)u (and u N (q)q ). That
means 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 mobilities
u, which are physical quantities, rather than on the generalized coordinates q which can be
chosen somewhat arbitrarily. So we work only with the nxn diagonal weighting matrix Wu;
there is no separate Wq. Instead, it may be calculated as
98
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 of
quaternions 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 near
singular configurations have one coordinate whose weight approaches infinity; proper
weighting ensures the simulation remains accurate in near-singular configurations, although
possibly 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 the
unitless 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
ny
E
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 some
residual 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 be
measured 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 to
machine 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 might
find it useful to scale the acceleration errors. In any case we do need to actively control the
errors 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 the
constraint 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 a
holonomic constraint, speed for a nonholonomic constraint, and accelerations for acceleration-only constraints. Each ti should represent the violation that is to be considered a “unit
100
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 a
typical “time of interest” over which we expect to see significant behavior. This should be
roughly the rate at which you could expect interesting reporting data, not necessarily the
highest 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 acceleration
levels (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 the
constraint weighting matrices are defined:
Tp diag(1. / t p ) , Tv diag(1. / tv ) , Ta diag(1. / ta )
tc Tp
Tpv
Tpva
tc Tpv
Tv
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 each
constraint equation at each level. We can now calculate the tolerance norms p
and p
T
Tp p
T
Tp p
2
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
1
mp
p
T
or
rather than the 2-norm to remove effects due just to problem size, and define
the phrase “meeting tolerance to accuracy α” to mean
max( 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 in
theory provide a weighting W to influence the norm in which that least squares calculation
is 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 in
practice – if you really care about how the forces are distributed you will probably want to
use compliant elements. However, here is a proof that acceleration-level weighting has no
effect except when there are redundant constraints.
Scaling on the constraint forces would be incorporated as follows. First, we want to solve for
the scaled multipliers (using just W here to represent W ):
Mu GT (WW ) f
Mu (WG)T W f
(9.6)
(W is symmetric.) This shows how the scaling transfers to the constraint Jacobian, so the
acceleration 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)
W1W .
(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 the
ordinary 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 1U
where U is just the multiplier we would have calculated without scaling. Unscaling W
using equation (9.8) gives WW WW1U U , that is, the final λ is just the unscaled one.
That proves that weighting the acceleration constraints and multipliers has no effect for
systems 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-constant
weighting matrix W on changes to the state variables u and z (and implying a
weighting 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” for
each state variable, and T defines a “unit error” for each constraint equation. We also have
information that defines what we consider “full scale” for each state variable, so that we can
interpret 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
max( Tp p
Enorm
Tnorm
, Eu eu Enorm , E z ez
, Tpv pv
)
Tnorm
Enorm
) 0.001
0.001
(9.12)
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 to
emphasize 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 of
accuracy 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 such
accuracy 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 of
equation (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. These
equations 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 can
always 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 (typically
prior 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 case
that 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 the
value for λ that minimizes |λ|2 in the redundant situation, and the value which minimizes the 2-norm of the
residual 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)
p
pv (u ) (t, q; u)
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 are
unweighted 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 to
modify 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 be
done separately from everything else, although no projection is done for quaternions that are
contained 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 we
106
are working only with free variables, and matrix columns corresponding to prescribed
variables are removed. All constraint equations remain, however, even as the number of
variables 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
)uELS
u (i 1)
(10.5)
(10.6)
tc Tp p (u (i ) )
(i )
Tv v (u )
u (i ) Eu1uELS
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 NWu1N )
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 the
iteration (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 treated
separately 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 those
elements. 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 like
to remove the component of that vector which is normal to the constraint manifold, in the
weighted 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
)nfinal
k
k
V(q final )ewu
V(q final )Eu eu
eˆu eu Eu1ewu
quaternions
no iteration
(10.12)
(10.13)
(10.14)
(Here as above we are working only with the error slots corresponding to free variables, not
prescribed 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. Note
that 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 block
108
structure in P and V should be exploited for efficiency. After this projection the integrator
should use the revised estimate ê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)
and torques, convert them
bodyForces, Vector& jointForces)
V J[ q ]u
Position
to hinge forces ignoring
constraints.
Given a set of generalized
speeds, compute the
equivalent spatial
velocities of each body.
ke=ke[q](u)
Position
Real calcKineticEnergy(State,
Given a set of generalized
Vector u)
speeds, calculate the
resulting kinetic energy.
12.9 Misc
This is material that should probably be removed.
146
Quaternion position level
normalization
only
holonomic
n=0
Given: position
(index 3)
Prescribed
motion
Local to each
prescribed
mobilizer i.
prescribed
coordinates
q q
u u
nonholonomic
(index 2)
acceleration
only (index 1)
velocity (index 2)
acceleration
(index 1)
Given: velocity
acceleration
(index 1)
Given: acceleration
Given: position
p(qˆ ) 0
holonomic
(index 3)
General
constraints
velocity p(uˆ ) 0
(index 2)
acceleration p(uˆ ) 0
(index 1)
free coordinates q̂ q
Given: velocity
v(uˆ ) 0
û u
Nonholonomic
(index 2)
acceleration
only (index 1)
acceleration v (uˆ ) 0
(index 1)
Given: acceleration
a(uˆ ) 0
ni (q) qiT qi 1
qi qi (t )
ui up,i (t , qi )
up,i (t , qi ) Ni1 (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)
u ma (t , q, u )
p(t , q , qˆ ) 0
Pˆ uˆ c(t , q, u ) 0
collect contributions from all the
shaded rows above
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ˆ û
v(t , q, u , uˆ) 0
ˆ uˆ b (t , q, u, u ) 0
V
v
ˆ v
V
û
V
v
u
v v
Nu Vu
t q
bv
ˆ uˆ b (t , q, u, u ) 0 Note that a must be linear
A
a
in û .
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
G
v
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 level
normalization
only
ni (q) qiT qi 1
n=0
Given: position
qqp,i qi (t )
holonomic
(index 3)
Prescribed
Local to each
motion
prescribed
mobilizer i.
prescribed
coordinates
velocity (index 2)
acceleration
(index 1)
uqp,i up,i (t , qqp,i )
uqp ,i m p,i (t , qqp ,i ,
up u
nonholonomic
(index 2)
acceleration
only (index 1)
holonomic
General
constraints
Given: velocity
acceleration
(index 1)
Given: position
p( qqf ) 0
p(t , qqp , qqf ) 0
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: velocity
v (uuf ) 0
Nonholonomic
(index 2)
acceleration
v (uuf ) 0
(index 1)
148
uup mv (t , q, u)
u p ma (t , q, u)
(index 2)
u p,i (t , qqp ,i )
uup u v (t , q)
Given: acceleration
velocity p(uqf ) 0
m p,i (t , qqp ,i , uqp ,i )
uqp ,i )
qqp qup
uup u p
up,i (t , qqp ,i ) Ni1 (qqp ,i )qi (t )
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: acceleration
a (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 )
0
Note 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 public
domain 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 to
bring the best of these ideas together (and avoid some earlier mistakes) in a form that is
practical for use in physics-based simulation of biological structures over a wide range of
scales.
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 field
of 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).
5
Jain, 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 Journal
of 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 original
form under an unrestrictive license on Simtk.org here: https://simtk.org/home/tao_de.
152
Source Exif Data:
File Type : PDF File Type Extension : pdf MIME Type : application/pdf PDF Version : 1.5 Linearized : No Page Count : 160 Language : en-US Tagged PDF : Yes Title : Simbody™ Author : Michael Sherman Creator : Microsoft® Word 2010 Create Date : 2013:03:22 15:42:47-07:00 Modify Date : 2013:03:22 15:42:47-07:00 Producer : Microsoft® Word 2010EXIF Metadata provided by EXIF.tools