Impulse based Dynamic Simulation of Rigid Bo dy Systems
by
Brian Vincent Mirtich
B S E Arizona State University
M S University of California Berkeley
A dissertation submitted in partial satisfaction of the
requirements for the degree of
Do ctor of Philosophy
in
Computer Science
in the
GRADUATE DIVISION
of the
UNIVERSITY of CALIFORNIA at BERKELEY
Committee in charge
Professor John F Canny Chair
Professor David Forsyth
Professor Alan Weinstein
Fall
The dissertation of Brian Vincent Mirtichisapproved
Chair Date
Date
Date
University of California at Berkeley
Fall
Impulse based Dynamic Simulation of Rigid Bo dy Systems
Copyright Fall
by
Brian Vincent Mirtich
Abstract
Impulse based Dynamic Simulation of Rigid Bo dy Systems
by
Brian Vincent Mirtich
Do ctor of Philosophy in Computer Science
University of California at Berkeley
Professor John F Canny Chair
Dynamic simulation is a p owerful application of to day s computers with uses in elds rang
ing from engineering to animation to virtual reality This thesis intro duces a new paradigm
for dynamic simulation called impulse based simulation The paradigm is designed to meet
the twin goals of physical accuracy and computational e ciency Obtaining physically ac
curate results is often the whole reason for p erforming a simulation however in many
applications computational e ciency is equally imp ortant Impulse based simulation is
designed to simulate mo derately complex systems at interactive sp eeds To achieve this
p erformance certain restrictions are made on the systems to be simulated The strongest
restriction is that they comprise only rigid b o dies
The hardest part of rigid b o dy simulation is mo deling the interactions that o ccur
between b o dies in contact The most commonly used approaches are p enalty metho ds
followed by analytic metho ds Both of these approaches are constraint based meaning
that constraint forces at the contact p oints are continually computed and applied to deter
mine the accelerations of the b o dies Impulse based simulation is a departure from these
approaches in that there are no explicit constraints to be maintained at contact points
Rather all contact interactions b etween b o dies are a ected through collisions rolling slid
ing resting and colliding contact are all mo deled in this way The approach has several
advantages including simplicity robustness parallelizability and an ability to e ciently
simulate classes of systems that are di cult to simulate using constraint based metho ds
The accuracy of impulse based simulation has b een exp erimentally tested and is su cient
for many applications
The pro cessing of collisions is a critical asp ect of the impulse based approach E
cient algorithms are needed for detecting the large numb er of collisions that o ccur without
missing any Furthermore the physical accuracy of the simulator rests up on the accuracy of
the collision resp onse algorithms This thesis describ es these essential algorithms and their
underlying theory It describ es how the algorithms for simple rigid b o dy simulation maybe
extended to systems of articulated rigid b o dies To prove the metho d is truly practical the
algorithms have b een implemented in the prototyp e simulator Impulse Many exp eriments
p erformed with Impulse are describ ed
Professor John F Canny Dissertation Committee Chair
iii
Contents
List of Figures vi
List of Tables ix
Intro duction
Constraint based contact mo dels
Non p enetration contact forces
The impulse based approach
Impulses versus constraints
Overview of the thesis
Collision Detection
Intro duction and related work
Collision detection in Impulse
The Lin Canny algorithm
Collision detection and coherence
Extensions to the Lin Canny algorithm
Prioritizing collision checks
The collision heap
Estimating time of impact
Bounding ballistic angular velo city
Bounding b oxtechniques
Finding static b oxintersections
Coherence and the tiling scheme
Maintaining the collision heap
Spatial hashing versus co ordinate sorting
Generalizing collision detection
Collision Resp onse
Assumptions of collision resp onse mo del
Computing collision impulses
The equations of collision
Sliding mo de
Sticking mo de
iv
Collision integration
Work done by collision forces
Integrating collisions using di erent parameters
Sliding mo de under u and W parameterizations
z z
Handling sticking during collision integration
Sticking stabilityandrays of constant sliding
The u ellipse
Directions of constant sliding
Static contact and micro collisions
Constrained Bo dy Dynamics
Constrained forward dynamics
Velo city and acceleration propagation
Spatial algebra
Spatial formulation of acceleration propagation
The Featherstone algorithm
Base case
Inductive case
Forward dynamics algorithm
Extension to tree like linkages
Velo city and acceleration propagation through trees
Articulated inertias and z a forces for tree linkages
Forward dynamics algorithm for tree linkages
Extension to oating linkages
Hybrid Simulation
A sp ectrum of physical systems
Collision detection
Constrained b o dy swept volumes
Constrained b o dy TOI co e cients
Collision resp onse
Collision resp onse a rob otics p ersp ective
Articulated b o dy collision dynamics
Computing K
i
Propagating impulses through multib o dies
Supp orting control systems
Typ es of controllers
Controller scheduling
Impulse s control supp ort architecture
Computation of Mass Prop erties for Polyhedral Bo dies
Rigid b o dy mass parameters
Derivation of the algorithm
Reduction to surface integrals
Reduction to pro jection integrals
v
Reduction to line integrals
Evaluation of integrals from vertex co ordinates
Pseudo co de and on line C co de
Test results
Examples and Results
Pure impulse based simulation
Passivehybrid simulation
Controlled hybrid simulation
Execution times
Estimating p ose statistics
Background
Quasi static algorithms
Dynamic simulation
Exp erimental results and discussion
Other part feeding exp eriments
Future Work
Paradigm switching
Collision detection issues
Interpreted control and ob ject encapsulation
Physical simulation for animation and VR
A Mathematical Preliminaries
A Vectors matrices and frames
A Representing cross pro ducts as matrices
A Rigid b o dy dynamics
A Quaternions and integration of orientation
Bibliography
vi
List of Figures
A disassembled double p endulum
A system of falling dominos
A table with non unique contact force con gurations
A vibrational part feeder
Various contact mo des b etween ball and terrain
A blo ck diagram of Impulse s collision detection system
A p olygon and its Voronoi regions
The Lin Canny algorithm for p olygons
The three dimensional Lin Canny algorithm
The e ect of coherence on Lin Canny p erformance
A one sided approach to nding collision times
Impulse s narrow phase collision detection system
Closest p ointvelo cities and time of impact
Finding a lower b ound on the time of impact of con vex b o dies
The b o dy angular velo cityvector remains on an ellipsoid
A one dimensional example of a hierarchical spatial hash table
Avariant of the hierarchical hash table scheme
The swept volume b ounding b ox during a ballistic tra jectory
The advance algorithm
Collision heap hysteresis
Broad phase collision check culling during a coin simulation
Atwo dimensional version of I COLLIDE s b ounding b oxcheck
A bad case for co ordinate sorting
The axes aligned b ounding rectangles in the x y plane for a cycler b o dy
Velo city force impulse and work during collision
A collision b etween two b o dies
Solution tra jectories of the ODE system of Theorem
Relativetangential velo city that increases during a collision
Tra jectories through relativecontact velo city space for di erent collisions
The life cycle of a collision and the collision integration
Avelo city owwithtwo converging rays and no diverging rays
vii
Avelo city owwithtwo converging rays and two diverging rays
A sequence of transformations to generate the u ellipse
Testing whether the u ellipse circles the origin Lemma
The lighthouse analogy for rays of constant sliding direction
Proving there can b e no more than one diverging ray
The co e cient of restitution is arti cially increased
Standard collision impulses will cause the blo ck to creep down the ramp
Data from the blo ck on ramp exp erimentusingImpulse
The link and joint indexing conventions for serial linkages
Quantities for propagating velo cities and accelerations
Expressing velo city and acceleration in di erent frames
The compSerialLinkVelocities algorithm
The free b o dy diagram of the last link of the serial linkage
The free b o dy diagram of an inner link of the serial linkage
The initSerialLinks algorithm
The serialFwdDynamics algorithm
A complex p endulum with a tree like top ology
The numberLinks algorithm
The compTreeLinkVelocities algorithm
The free b o dy diagram of a tree linkage handle
The initTreeLinks algorithm
The treeFwdDynamics algorithm
A hinge joint
A sp ectrum of physical systems
Checking p erformed to detect aggressive b ounding b ox violations
The updateSlack algorithm
A rob ot in contact with its environment
The path for computing z a impulses
The impulseResponse algorithm
The b o dy frame of a colliding link and the collision frame
The compMultibodyKi algorithm
The propagateImpulse algorithm
The entire pro cedure for resolving collisions involving a multib o dy
A situation in which the multib o dy collision matrix is rank de cient
The coupling of a lowlevel controller to a multib o dy system
APDcontrolled joint loaded with a static weight
The control supp ort architecture of Impulse
The strategy for evaluating volume integrals
Maximizing a face s pro jected shadowinthe plane
Notation for computing a pro jection integral as a sum of line integrals
The CompVolumeIntegrals algorithm
The CompFaceIntegrals algorithm
viii
The CompProjectionIntegrals algorithm
Snapshots from the dominos simulation
Snapshots from the block drop simulation
Snapshots from the block on ramp simulation
Snapshots from the chain of bal ls simulation
Snapshots from the bal ls in dish simulation
Snapshots from the coins simulation
Snapshots from the pool break simulation
The frequency of collision checks during a p o ol break
Snapshots from the bow ling simulation
Bowling exp eriments p erformed with Impulse
Tra jectories of a ball on a spinning platter
Snapshots from the rattleback top simulation
Snapshots from the part feeder chute simulation
Snapshots from the exploding text simulation
Snapshots from the see saw simulation
Snapshots from the part sorter simulation
Snapshots from the triple pendulum simulation
Snapshots from the furniture arrangement simulation
Snapshots from the bicycle simulation
The kinematic structure of the bicycle
The forward velo city pro le as the biketraversed the obstacle course
The roll angle pro le as the biketraversed the obstacle course
Snapshots from the creature war simulation
Side view of a bug and kinematic tree
Side view of the rover and kinematic tree
The high level state machine controller for the rover
The C functions that implement the rover s high level control
The Adept Rob otics exible part feeding system
Estimating p ose statistics using a sphere pro jection metho d
The p erturb ed quasi static metho d
Parts used in the p ose statistics exp eriments
A snapshot taken during a MEM motion array exp eriment
The evolution of the states do es not have to pro ceed in lo ckstep
Interpreted ob ject sp eci cation allows generality and p ortability
A conception of a creature design environment
ix
List of Tables
Comparison of hashing schemes
Notation used in Chapter
Theoretical values of volume integrals for simple test p olyhedra
Data for successive approximations of a unit sphere
Simulation times for examples
How the simulation pro cessing time is sp ent
Orange insulator cap data
White stereo button data
Rectangular black stereo button data
Square black stereo button data
x
Acknowledgments
This thesis would not have been p ossible without the help of many p eople I thank my
advisor John Canny who was an endless source of wisdom and inspiration I thank the
many great friends who made my seven years at Berkeley an exp erience to be treasured
Finally I thank my family for their supp ort and esp ecially Caroline die mein Leb en mit
Freude erfullt
Chapter
Intro duction
The eld of classical dynamics is one of the success stories of mo dern mathematics
Dynamical systems exhibit a great deal of b eautiful structure and dynamicists have devel
op ed a b o dy of knowledge that can predict the b ehavior of many of these systems to great
accuracy The ability to know in advance how our universe or some part of it will evolve
is certainly one of the most powerful skills humans have learned The mo dern computer
with its p ower to p erform computations at blinding sp eed and display realistic images has
greatly increased our predictivepower But it has revealed holes in our knowledge as well
places where our predictivepower breaks down Predicting with certainty the outcome of
a simple coin toss is at the limits of our current abilities if not b eyond them
This thesis is ab out predicting the future or more precisely physical simulation
It deals with very simple systems and very simple mo dels The systems are comprised of
rigid b o dies macroscopic deformations o ccurring in materials like cloth or rubb er are not
allowed Di erent rigid b o dies may however be connected by joints to form articulated
b o dies Friction is mo deled according to the Coulomb law restitution is mo deled using a
single co e cient The latter provides a crude way of accounting for microscopic deformation
during a collision Despite these simpli cations the predictivepower is quite go o d for many
applications One common but signi cant simpli cation is also dropp ed the quali cation
that one is only interested in the b ehavior of the system over some limited modeling interval
Most textb o ok problems are concerned with the b ehavior of a system over an interval where
a single mo del applies determine the velo city of the wrecking ball before it collides with the
wall or determine the acceleration of the ho op down the ramp assuming slipping do es not
o ccur For the simulation problems addressed in this thesis the analysis is not restricted
in this manner The simulator must pro duce correct motion for whatever situation and
system the user describ es even in the presence of discontinuities pro duced by collisions or
changes in contact state
The external in uences that a ect the motion of physical ob jects can be divided
into two groups based on whether or not they arise through physical contact Forces due
to gravity external electric elds or air resistance are examples of non contact forces The
motion of isolated rigid b o dies sub ject only to these non contact forces is well understo o d
For example an accurate analytic mo del exists for the motion of a rigid b o dy in a uniform
gravitational eld The remaining in uences on the motion of ob jects result from contact
The contacts between di erent ob jects are where all of the interesting behavior arises
and correct mo deling of these contacts is critical to accurate physical simulation Contact
mo deling is also where all of the di cult problems are The mo dels for contact are less
accurate and more equivo cal than those for the dynamics of isolated b o dies Much of the
research in dynamic simulation has b een devoted to contact mo deling
In studying contact interactions there is the typical tradeo b etween accuracy and
e ciency Finite element metho ds FEMs and related approaches are among the most
accurate metho ds for studying contact interactions but they require signi cant computation
time They are not feasible for example in interactivesimulation where a user would like
to watch a system evolve at a sp eed near the true sp eed at whichitevolves in the physical
world This thesis is concerned with physically accurate simulation sub ject to the constraint
of interactiveoreven real time p erformance The accuracy of the metho ds is less than the
t This thesis accuracy of for instance nite element metho ds but the goals are di eren
demonstrates that dynamic simulations can b e computed at interactive sp eeds and retain
adequate physical realism and predictive power for real engineering applications One of
the fundamental assumptions made for computational e ciency is that all b o dies are rigid
Impulse based simulation is a departure from the typical approach for p erforming
interactive dynamic simulation The greatest di erence lies in how the contacts are mo deled
Rather than computing explicit constraint forces to be applied at contact p oints contact
interactions are mo deled exclusively through collision impulses applied b etween b o dies To
understand why this approach might b e useful it is necessary to understand the standard
constraint based approaches and their limitations
Air resistance might b e mo deled as a contact force resulting from collisions of a b o dy with air particles
But it is more common to mo del it as an aggregate non contact force based on the b o dy s state
Constraint based contact mo dels
Physically accurate interactive simulation has received muchattention in the elds
of engineering and computer science The standard approach uses a constraint based mo del
for contact an outgrowth of the classical mo del for constrained dynamics The latter is
useful in computing the forward dynamics of articulated b o dies given the current p ositions
and velo cities of all parts of a physical system nd the current accelerations of all of the
parts Consider for example the planar double p endulum shown in Figure Supp ose
f1
x1
x2 f3 f2 x3
x4
f4
Figure A disassembled double pendulum Although the pendulum is an articulated rigid
body its dynamics can be derived from the formulation for simple rigid bodies rst the
constraint forces f are computed and then the links are treated as individual rigid bodies
i
x and x are p oints that are on di erent b o dies of the p endulum but which are attached
at a joint However the two links move the constraint
x x
must b e enforced This is done byintro ducing constraint forces that are transmitted through
joints maintaining the constraints Once the constraint forces are known the links of the
system may be analyzed separately and their motion determined from the Newton Euler
equations of rigid body motion Some metho ds do not explicitly solve for the constraint
forces but pro duce the link accelerations directly Usually the value of the constraint forces
is not imp ortant as long as the constraints are enforced so these metho ds are suitable and
even preferable if computations can b e reduced General applications of the constraint based
approach to dynamic simulation and mo deling are in BB CS Gle IC WGW
Non p enetration contact forces
Sometimes b o dies are not p ermanently attached to each other but come into con
tact during the course of their motion The row of dominos in Figure is an example
These contacts give rise to non penetration constraints Unlike a non p enetration
Figure For this system of dominos there are no permanent constraints between bod
ies but the contacts indicated by dots give rise to temporary non penetration constraints
between bodies
constraint is fundamentally an inequality rather than an equality thatinvolves the acceler
ations of the b o dies In addition there are restrictions on the constraint force it can only
act in a direction which tends to push the ob jects apart not pull them together If a pulling
force is required to hold the ob jects in contact the contact will merely break For these
reasons non p enetration constraints can not b e handled using the classical approaches for
constrained dynamics
o main approaches to mo deling non p enetration with con There have been tw
straints The older and simpler approach is the family of penalty methods These metho ds
do not strictly enforce non p enetration instead they keep p enetrations small enough so
that they are not noticeable relative to the scale of the system With this practical goal
in mind it is not surprising that the easy to implement p enalty metho ds have dominated
the computer animation eld where motion that lo oks right is often su cient Penalty
metho ds vary in details however the rough idea is the same A sti spring is attached
at the contact so that as two b o dies move into one another the spring attempts to push
them apart The more p enetration the stronger the restoring force If the b o dies move
apart the spring is destroyed Penalty metho ds are used for rigid body simulation in
RH MW MZ and are also commonly employed in deformable b o dy simulation
Penalty metho ds have a few problems The very large spring constants that are
needed to keep the p enetration su ciently small generate large forces leading to sti equa
tions that are numerically intractable to integrate WGW Integration problems are
exacerbated by the fact that springs can be created and destroyed in a very transient
are manner Furthermore cho osing the spring constants is a black art Constants that
satisfactory for one situation may be to o high or to o low for another and the problem
b ecomes worse when multiple b o dies are in contact simultaneously These spring constants
do not corresp ond to familiar physical prop erties that can b e measured like the co e cient
of friction Another subtle problem of p enalty metho ds arises b ecause of the way collision
detection is typically p erformed Usually collision checks are done at discrete times rather
than continuously over time intervals As a result contact is not detected until a nite
amount of p enetration has o ccurred Under p enalty metho ds this causes forces that are
larger than the true forces in the continuous system causing instability
Alternatives to the p enalty metho ds are the analytic or exact metho ds for com
puting non p enetration contact forces rst studied byL otstedt L ot The idea is to cast
the non p enetration constraints as a linear complementarity problem LCP
a Af b
f
T
f a
Here a is a vector of the relative normal accelerations at the contact points and f is a
vector of corresp onding normal force comp onents at these contacts A and b are matrix
and vector constants which are determined from the known con guration of the system
For the cursory treatmentgiven here Bara s simpler notation is used Bar
The constraint a prevents motion that would cause p enetration at the contact points
The constraint f means that contact forces can only try to push b o dies apart not hold
T
them together The complementarity constraint f a means that at eachcontact p oint
either a force is acting and the relative acceleration is zero or the b o dies are separating
and the force is zero The former case means the contact will p ersist beyond the current
instant the latter means the contact is ending L otstedt gives an algorithm for solving the
LCP based on the principal pivoting metho d of Cottle and Dantzig see CPS for
a detailed treatment of LCPs L otstedt s algorithm in the context of rigid b o dy simulation
is describ ed in L ot Bara has also used LCP approaches for simulation of rigid b o dy
systems Bar Bar Bar
When there is no friction in the system the LCP alwa ys has a solution for the
contact forces and this solution is unique sub ject to certain non degeneracy constraints
Bar When friction is added to the mo del the corresp onding LCP may not have a
solution and if a solution exists it may not b e unique L otstedt L ot gives existence and
uniqueness conditions which Bara Bar generalizes The most comprehensive results on
the existence and uniqueness of solutions for rigid b o dy contact forces using the Coulomb
friction mo del are given byPang and Trinkle PT and Trinkle et al TPSL Roughly
sp eaking existence and uniqueness can b e guaranteed if the co e cients of friction are small
enough Certain problems such as the static stability problem in whichthe b o dies in the
system are at rest always have solutions
When there is no friction the LCP is convex and solutions can b e computed using
algorithms that run in worst case exp onential time but exp ected p olynomial time in the
tacts Convexity breaks down with the addition of friction Bara proved that number of con
the problem of determining the existence of a valid set of contact forces that ob ey Coulomb
friction laws at the contacts is NP hard when there is sliding friction Toavoid this problem
Bara suggests a new mo del for consistency that includes non colliding impulsive forces at
some of the contacts Under this mo del he gives an algorithm to nd a valid solution to
the contact force problem Its complexityisnotknown although it app ears to b e practical
in many examples Bar Bar None of the algorithms prop osed for computing contact
forces with friction are guaranteed to correctly terminate Trinkle et al give quantitative
results using an approach based on Lemke s algorithm this is also Bara s approach and
also a feasible interior p oint metho d TPSL In general the reliability of the algorithms
decrease as the number of contact points increase
Approximations are made to the Coulomb friction law in order to form an LCP
for the contact forces Under sticking conditions Coulomb friction imp oses the nonlinear
restriction
kf k kf k
t n
where f and f are the tangential and normal forces at the collision p oint To cast this
t n
into the LCP framework a linear approximation to the friction cone is typically employed
such as a friction pyramid This has the e ect of making friction anisotropic for three
dimensional systems Another common approximation is that up on the transition from
sticking to sliding the frictional forces are only partially instead of directly opp osed to
the tangential acceleration Bar PT
Even without friction analytic metho ds may not nd the correct set of contact
forces due to contact degeneracy Consider the idealized system of a table resting on a
o or where b oth ob jects are p erfectly rigid the table leg lengths are equal and the o or
is at Figure Here there are multiple sets of contact forces satisfying the constraints
of the LCP the forces are all p ositive and prevent acceleration of the table into the o or
Without friction the acceleration of the system is provably unique even when the contact
(a) (b) (c)
www
w/4 w/8 3w/8 w/4 w/2 w/4
w/2 3w/8 w/8 w/4
Figure For a symmetric table of weight w thereare in nite contact forcecon gurations
that satisfy the non penetration LCP Three are shown here
forces are not Bar in all three cases of Figure the net acceleration of the table
is Analytic metho ds may not be reliable however for computing the contact forces
Force con guration c of Figure is the correct one scales placed under each leg of the
table would all read the same weight Analytic metho ds can compute di erent solutions
such as those shown in parts a and b of the gure b ecause the contact con guration is
degenerate On the other hand p enalty metho ds and the impulse based metho d describ ed
in the next section always compute con guration c The idealized case is an imp ortant one
to handle correctly it is mathematically natural and the one often employed in simulations
It is an accurate approximation of certain real situations for instance when the table
lengths are approximately equal and the contacts are soft This would be the case if the
table leg tips are deformable as on a folding table or if the table rests on carp et or linoleum
If the table and o or are hardwo o d and the leg lengths are di erent or the o or is warp ed
then the idealized mo del is not accurate as the true forces on the legs may vary greatly
Such a system is not degenerate and any of the simulation metho ds describ ed will compute
the correct contact forces
With friction there maybemultiple contact force solutions that do not lead to the
same accelerations This indeterminacy that can arise in an analytic solution seems counter
intuitive to a deterministic view of the world The inconsistency is even more disturbing
how can there be no valid solutions to the contact force problem for a physical system
Certainly these forces always exist and are measurable for any physical system one can
construct These anomalies of the contact LCP re ect the limitations of the rigid body
assumption and the Coulomb friction law which are only approximations to reality
In dynamic simulation exploiting coherence of the problem instances b ecomes
critical for analytic metho ds Solving a linear program is more di cult than solving a
linear system of the same size and solving LCPs is harder still Solving a fresh linear
tarity problem at every time step would b e prohibitively slow Muche ort can complemen
be saved by using the results from the previous time step For example in the pivoting
algorithms of L otstedt and Bara basic solutions to the LCP can b e used from the previous
time step if they change relatively infrequently Even more imp ortant when friction is
present there may be multiple solutions to the LCP In order to preserve continuity of
the accelerations of the b o dies the solution at the current time step must be computed
with knowledge of the solution at the previous time step Bar Practically sp eaking this
dep endence on contact coherence is p erhaps the greatest limitation of the analytic metho ds
Tangentially related to this discussion is work of Peshkin and Sanderson PS who have elegantly
characterized the motion of parts sliding on a at surface in the presence of Coulomb friction Assuming
nothing ab out the pressure distribution on the part they compute an envelop e of p ossible instantaneous
centers of rotation thereby b ounding the p ossible motions of the part Unfortunately their metho d is based
on a quasi static assumption and the minimum power principle PS which prevents application to many dynamic systems
The impulse based approach
Consider the vibrational part feeder of Figure This machine shakes parts into
Figure A vibrational part feeder that shakes parts into recesses to be picked up by a
manipulator The system exhibits very transient contact modes
recesses so that they can be picked up by a manipulator Simulating this device requires
mo deling the extremely transient contacts between the parts and the feeder The contact
interactions are dominated by collisions The con gurations of non colliding contact exist
only for short time intervals before changing In short the coherence between time steps
that the analytic metho ds need for e ciency is destroyed This part feeder example was
one of the original examples that inspired the research describ ed in this thesis A metho d
was desired for e ciently simulating these typ es of systems The result is impulse based
simulation
The fundamental idea of impulse based simulation is that all contact between
b o dies is mo deled through collisions at contact points Non p enetration constraints do
not exist collisions are what enforce non p enetration between di erent b o dies Between
collisions the b o dies move along ballistic tra jectories in uenced only bygravity As might
be exp ected the metho d adeptly handles systems like the parts feeder describ ed ab ove
where the b o dies truly are unconstrained most of the time and collisions are frequent More
surprising is the fact that impulse based simulation can b e used to mo del cases of continuous
contact it is a viable and p owerful simulation paradigm for many applications In the case
of a book resting on a table the corners of the book exp erience rapid collisions with the
table whichprevent the b o dies from p enetrating The rolling ball of Figure rolls along
the ground and up the ramp b ecomes airb orne then b ounces along the ground eventually
settling into a roll sliding between the ball and terrain may also occur at various points
during this evolution Under an impulse based mo del all of these mo des and transitions are
handled by pro cessing collisions A macroscopic rolling constraintisnever enforced Rather
the macroscopic behavior results from treating the underlying collisions with a physically
accurate mo del
Figure Under impulse based simulation the various contact modes between the bal l and
terrain and the transitions between them are a ected through a physical ly accurate model
for col lisions
Hahn pioneered this simulation paradigm This thesis expands up on his work and
improves the metho ds for collision detection and collision resp onse describ ed in Hah
The collision detection and resp onse mo dules are critical comp onents of an impulse based
simulator due to the central role collisions play in the simulation These mo dules must b e
very fast due to the frequency of collisions and also physically accurate since collisions
are the sole means bywhich contact forces are transmitted
The top level impulse based simulation lo op is extremely simple It comprises
three steps
Collision detection Determine a maximum time interval over which the physical
system may be integrated such that no collision b etween two b o dies will b e missed
At the end of the interval a pair of b o dies called the critical pair must b e checked for
collision
Dynamic evolution Evolve the system forward in time byintegrating the equations
of motion for eachbody For rigid b o dies this tra jectory is a simple ballistic one
Collision resp onse Check to see if the critical pair of b o dies has collided If so
compute and apply a pair of collision impulses to them
These three steps are rep eated throughout the course of the simulation The
collision detection mo dule works by maintaining an estimated time of impact for each pair
of nearby b o dies This is only an estimate and it is re ned as the b o dies move closer but it
is guaranteed to b e conservative the b o dies can not collide any so oner than the computed
time of impact The integration step is determined bythe earliest time of impact b etween
any pair of b o dies The b o dies that have the minimum time of impact b ecome the critical
pair
Once the integration interval is determined all of the b o dies in the system can
be evolved without regard to collisions since none will occur during the interval More
precisely the surfaces of any b o dies that collide during the interval will still be touching
within some sp eci ed tolerance at the end of the interval The dynamic evolution step
is very simple b ecause the b o dies are treated as unconstrained and so their motion is
easy to compute For example isolated rigid b o dies follow ballistic tra jectories during the
integration step The dynamic integration step is highly parallelizable since the b o dies are
moving indep endently during the integration In a computer with multiple pro cessors the
b o dies can b e parceled among the pro cessors and the entire system can b e integrated with
out any need for inter pro cessor communication This can result in a substantial sp eedup
It is an advantage over constraint based metho ds for physical systems with many ob jects
which need the state information of many b o dies in order to compute the motion of each
one
If the critical pair of b o dies are touching after the integration step and have ve
lo cities that indicate a collision should occur at the contact point the collision resp onse
mo dule computes a pair of equal and opp osite collision impulses to apply at the contact
points These impulses instantaneously change the velo cities of the colliding b o dies send
This statement will b e relaxed slightly in the context of hybrid simulation But in b oth pure impulse
based simulation and hybrid simulation there is no interaction between b o dies through non p enetration
constraints
ing them along new tra jectories For physical realism the collision resp onse mo dule should
account for friction and energy loss during collision Much of the work in rigid b o dy sim
ulation uses a simple mo del for collision resolution The typical mo del assumes pre and
p ost collision velo cities can b e related with algebraic equations without regard to the un
derlying physical pro cesses that occur during collision Simple restitution laws that can
add energy to the system are typically used For impulse based simulation the collision
resp onse mo del is critical to the physical accuracy of the simulator and b etter metho ds are
needed Collisions are resolved by integrating the dynamic state according to di erential
equations that hold during the colliding contact These equations are derived directly from
the underlying frictional and restitutional laws they are more accurate than the algebraic
approximations
One might argue that impulse based simulation is not a valid contact mo del Arock
sitting on a table is not undergoing a series of col lisions It s just not moving period This
is certainly true although all approaches to simulation make approximations and deviate
from reality esp ecially those that achieve interactive simulation The question is what
information is desired from the mo del Summing the collision impulses delivered from the
ground to the ro ckover some time interval and dividing by the time interval will yield the
same average force delivered to the ro ck as a constraint based approachwould The average
velo city of the center of mass of the ro ck also vanishes as in constraint based metho ds
Finally the piecewise ballistic tra jectory of the ro ck results in little displacement of the ro ck
erage p osition With a small enough collision tolerance the displacementcanbe from its av
made less than a pixel so that the motion of the ro ck on a graphics display is imp erceptible
Although the impulse based mo del deviates from reality it preserves imp ortant physical
quantities related to forces and the motion of b o dies
Based on the literature the impulse based metho d has b een more carefully tested
against reality than the constraint based interactive simulation metho ds Many comparisons
of impulse based simulation to physical exp eriments or theoretical predictions are given
later in this thesis More comparisons to physical exp eriments are certainly needed for
b oth simulation paradigms
Impulses versus constraints
There are several advantages of the impulse based approach
No contact coherence required Analytic metho ds rely on contact coherence for
interactive sp eed and even more imp ortantly forevolving the system in a physically
plausible way when multiple solutions exist to the contact LCP Bar Contact
coherence is not required for impulse based simulation extremely transient contact
mo des cause no slowdown This is esp ecially useful in situations where ob jects settle
as they make a transition to a resting state
Computational robustness As with the p enalty metho ds nothing can go wrong
in computing contact interactions Given the dynamic states of the colliding b o dies
and the material dep endent parameters a unique impulse to resolve the collision can
always b e computed the collision resolution algorithm always terminates with a valid
solution The NP hard problem of computing non colliding contact forces is avoided
as are the indeterminacy and inconsistency asso ciated with analytic metho ds The
sti di erential equations of p enalty metho ds are also avoided
Colliding contact naturally handled With constraint based metho ds collisions
must be handled separately b efore any contact force analysis b egins But under
impulse based simulation colliding contact is naturally handled since collisions are
used to mo del all contact
Non p enetration strictly enforced Like analytic approaches but unlike p enalty
metho ds impulse based simulation strictly enforces non p enetration In addition if
aheavy ob ject and a light ob ject are sitting on a table b oth will exp erience collisions
with the table at roughly the same frequency the heavy ob ject do es not require more
impulses
Decoupling of b o dies Unlike constraint based approaches b o dies are evolved inde
pendently during dynamic integration under impulse based simulation This natural
decoupling facilitates parallelization Collisions are also pro cessed using a completely
needed to lo cal mo del so the global state of a system of contacting b o dies is not
compute the impulses A description of preliminary work in parallel impulse based
simulation is in Pau
Simple design The impulse based paradigm is conceptually simpler than analytic
metho ds making it easy to co de Advanced numerical metho ds are not required All
macroscopic contact mo des rolling sliding resting and colliding are handled with
a single lo cal collision resp onse algorithm
Physically valid contact forces Likepenalty metho ds the impulse based metho d
is not sub ject to the contact degeneracies that analytic metho ds are In the example of
Figure the physically valid contact forces will b e computed Forces are computed
by time averaging impulses over a short interval
Extension to constrained b o dies The metho ds of pure impulse based simulation
are easily extended to handle systems of rigid b o dies linked together byjoints
Though not an inherent prop erty of impulse based simulation the collision re
sp onse mo del presented in this thesis is a more accurate mo del than those typically em
ployed in interactivesimulation It would b e useful in other simulation paradigms as well
There are also disadvantages to the impulse based metho d as compared to constraint based
metho ds In particular
Po or handling of stable and simultaneous con tact Impulse based metho ds are
less e cient than constraint based metho ds for certain typ es of prolonged stable or
simultaneous contact in some cases the impulse based metho ds are not even feasible
The canonical example is a stack of blo cks at rest
Static friction creep Artifacts of the impulse based approach app ear when mo d
eling static friction in certain cases A blo ck will creep down a ramp even if static
friction should hold it in place
These limitations are signi cant and this thesis do es not claim that the impulse based
approach is a replacement for constraint based approaches There are cases where impulse
based simulation is the most natural approach for mo deling non p enetration and where
same can be said of it leads to faster simulation sp eeds based on rep orted data The
constraint based metho ds In the literature it is evident that constraint based approaches
have b een applied to problems they are well suited to and the same is true of impulse based
approaches Any contest between the two metho ds would be decided before it b egan by
the choice of physical system to simulate Furthermore sucha contest would b e p ointless
since the metho ds are complementary more than they are comp eting the situations for
which one approach works p o orly are exactly those for which the other approach works
well Previous work has demonstrated what can b e done with constraint based techniques
less has app eared on what can not b e done easily One purp ose of this thesis is to describ e
what can and can not b e done with impulse based techniques
Much of this thesis deals with theoretical issues surrounding impulse based simu
lation however there is a practical comp onent as well The ideas discussed in the thesis
have b een implemented in Impulse a prototyp e impulse based dynamic simulator Impulse
served as a pro of by example during the research phase of this thesis only through imple
mentation could the techniques b e proven practical While many of the discussions refer to
this sp eci c implementation they apply to any impulse based simulator
Overview of the thesis
The remainder of the thesis is organized as follows
Chapter describ es collision detection in the context of impulse based simulation
using the broad phase and narrow phase detection systems in Impulse as examples In
particular the chapter discusses how collision checking may be p erformed conservatively
so that collisions are guaranteed to b e detected
Chapter describ es the collision resp onse mo del used in Impulse A physically
accurate resp onse mo del is critical to accurate impulse based simulation The mo del incor
p orates friction and restitution and analyzes the dynamics that o ccur during a collision
Chapter derives Featherstone s algorithm for the forward dynamics of con
strained b o dies from the rst principles of Newtonian physics This is a key ingredient
for the simulation of constrained b o dies and also serves as the basis for constrained b o dy
collision resp onse
Chapter describ es hybrid simulation a metho d of applying impulse based sim
ulation to systems of constrained rigid b o dies It discusses necessary mo di cations to the
collision detection and collision resp onse algorithms describ ed earlier as well as a framework
for supp orting control and b ehavior systems
Chapter is self contained and presents algorithms for computing the center of
The algo mass and moments of inertia for arbitrary uniform density p olyhedral b o dies
rithms are very fast and naturally minimize numerical errors they are useful for any rigid
b o dy simulator
Chapter discusses many examples of simulations and exp eriments p erformed
with Impulse illustrating the strengths and weaknesses of the impulse based approach
Accuracy is analyzed qualitatively and quantitatively execution times are provided and
several real applications from the part feeding domain are discussed
Chapter concludes the thesis and discusses future work
App endix A provides a variety of basic information relevant to dynamic simula
tion including the Newton Euler equations for rigid b o dy motion and how orientation is
parameterized and evolved using a quaternion representation
Chapter
Collision Detection
A fundamental problem in the simulation of physical systems is the collision detec
tion problem determining when and where two b o dies come into contact This problem has
its ro ots in computational geometry and rob otics Typically in the former setting a group
of static ob jects are tested for intersection while in the latter setting the time of contact
must be rep orted between b o dies following tra jectories that are closed form functions of
time In simulation the physical system is certainly not static and the paths of the ob jects
are describ ed with di erential equations instead of closed form tra jectories The sp eed re
quirements that interactive simulation demands are also formidable Hahn found collision
detection to b e the b ottleneckinsimulation of physical systems Hah in his exp eriments
collision detection often accounted for over of the computation time Collision detec
tion algorithms have improved since then how ever they remain the b ottleneck in many
situations
This chapter describ es how collision detection may be p erformed in an impulse
based simulator using the system in Impulse as an example The main contribution is an
algorithm than can e ciently detect a large numb er of collisions and that is guaranteed not
to miss any collisions To meet this guarantee lower b ounds on the time of impact of two
ballistic rigid b o dies are derived from the principles of dynamics Metho ds of generalizing
this approach to other typ es of motion are describ ed This chapter also develops a fast
bounding box technique for culling most of the collision checks in a simulation with many
b o dies Two variants of the algorithm are describ ed and compared and the algorithm is
also compared to other b ounding b oxtechniques
Intro duction and related work
Hubbard gives a comprehensive reference list of work in collision detection Hub
Early approaches to collision detection for simulation solved problem instances from scratch
at every time step An example is the p olyhedral collision detection algorithm of Mo ore
and Wilhelms that is O n where n is the numb er of features on b oth p olyhedra MW
Hahn s metho d tests every edge of one p olyhedron against every face of the other when
bounding boxes can not guarantee the absence of collision and therefore also has quadratic
complexity Hah These typ es of algorithms are to o slow for many simulation applica
tions Using b ounding b oxes or spheres helps but is not enough Gilb ert et al prop osed a
O n algorithm for determining the distance b etween convex p olyhedra which also provides
a measure of p enetration when p olyhedra overlap GJK The biggest improv ementover
these early algorithms came from the realization that coherence could be used to greatly
reduce the computations In simulation the collision detection system solves a series of re
lated problems each one only slightly di erent than the one b efore For convex p olyhedra
coherence is combined with lo cality using lo cal prop erties to verify separation between
b o dies or lack thereof Gilb ert et al describ e an adaptation of their algorithm to take
advantage of coherence when it exists however the rep orted sp eedups are fairly mo dest
GJK Coherence is used more e ectively in Bara s witness plane algorithm Bar
and is thoroughly exploited in the Lin Canny closest features algorithm LC Lin
collision detection algorithm is only called at discrete sample For simulation the
points Even if invo cation o ccurs more than once per frame it is still p ossible to miss
collisions One pathological example is a bullet sp eeding toward a thin wall No matter
what the minimum sampling p erio d of the collision detection system the minimum temporal
resolution Hub one can cho ose a bullet sp eed and wall thickness suchthat the bullet
passes completely through the wall between collision checks One correct solution is to
apply detection algorithms to the four dimensional hyp er p olyhedra swept out in space time
Can These metho ds are to o slow and have not been used in any dynamic simulators
describ ed in the literature Most simulators systems handle the problem of missing collisions
by ignoring it CS MW Hah Bar
Ignoring the problem is not a go o d solution esp ecially in an impulse based paradigm
where collisions are used to mo del contact forces Von Herzen et al HBZ present an
algorithm that uses Lipschitz b ounds to derive limits on how far parts of a parametric sur
faces can move over atimeinterval their system is guaranteed to catch all collisions The
b ounds must b e supplied by the user when the surface is de ned The algorithm of Snyder
et al uses an interval version of Newton s metho d for ro ot nding to achieve the same
goal guaranteeing that the very next collision will b e detected SWF Here Lipschitz
b ounds are not needed since the exact tra jectories of the surfaces over time are input data
for the problem These approaches are similar to the one used in Impulse and discussed in
this chapter Lower b ounds on the times of impact b etween b o dies are maintained and used
to ensure collisions are not missed The user is not required to provide Lipschitz b ounds
nor the exact tra jectories of the b o dies over time Physical laws such as the conservation
of momentum pro vide enough information to b ound the times of impact Like the systems
describ ed in HBZ SWF Impulse uses a heap structure to schedule collision checks
between b o dies
Collision detection in Impulse
For computational sp eed Impulse restricts b o dies in the simulation to be rigid
and p olyhedral Rigidity allows much computation to be p erformed ahead of time only
once The p olyhedral restriction is is fairly mild one since any more general shap e such
as a parametric surface or CSG style solid can be approximated to arbitrary closeness
with a p olyhedral mo del Of course the complexity of the p olyhedral mo del may growvery
large but the algorithms used by Impulse are quite insensitive to this complexity Impulse s
exact meaning that the underlying p olyhedral mo dels of the collision detection system is
ob jects are eventually used if the ob jects are close enough These metho ds are in contrast to
approximate metho ds which are less concerned with lo cating the exact collision p ointand
more concerned with p erformance Hubbard s metho d based on b ounding sphere hierarchies
is one approximate metho d which is able to p erform collision checking b etween very complex
b o dies at frame rates Hub Hubbard makes a go o d case for using approximate metho ds
in many time critical applications Such approximate metho ds were not used in Impulse
which was designed to have value as a simulation to ol Inaccuracies in the position of
contact points can have large rep ercussions in the course of the simulation
E cient collision detection systems employ some typ e of multi level strategy
Bounding boxes b ounding spheres o ctrees or similar metho ds are used to prune most
of the p otential collision checks between a group of b o dies A more sophisticated but ex
p ensive collision detection algorithm is applied to pairs of b o dies that can not b e dismissed
by the simple technique This chapter uses Hubbard s terminology referring to these two
levels of checking as the broad phase and the narrow phase For a simulation with n b o dies
the broad phase prunes most of the O n pairs and the narrow phase is applied to the
pairs that remain
In the broad phase Impulse computes b ounding boxes for the swept volumes
of b o dies over a time interval and nds intersections of these boxes using a hierarchical
hash table The swept v olume technique is di erent than a four dimensional space time
intersection test in two resp ects First the b ounding b oxes are only conservative estimates
of the swept volumes of the b o dies with much simpler geometry than the true swept
volumes Second since the b oxes are not b eing used to compute the time of collision but
only to signal that one mighthave o ccurred during some time interval one can pro ject the
true swept volume in space time along the time axis into the physical space The analysis
is simpli ed byworking in three dimensional physical space rather than in four dimensional
space time The hashing technique employed for detecting b ounding b oxoverlaps is based
on a strategy Overmars prop oses for solving the static p oint lo cation problem Ove
Impulse s narrow phase collision detection is based on the Lin Canny algorithm
closest features between convex It is one of the fastest algorithms known for tracking the
p olyhedra in a setting where coherence can be exploited The algorithm s output can
easily b e used to compute the distance b etween the p olyhedra which serves as a basis for
collision detection The very co de which implements the Lin Canny algorithm in Impulse
was subsequently used for the low level core of the I COLLIDE collision detection package
for large scale environments CLMP
Both the broad and narrow phases of Impulse s collision detection system rely
heavily on dynamics as well Dynamics are used to compute b ounding b oxes that enclose
b o dies swept volumes during the broad phase Dynamics are used to determine when the
next collision check should b e p erformed b etween a pair of ob jects during the narrow phase
b e missed This coupling b etween collision de in a way that guarantees collisions will not
tection and dynamics is unfortunate from a mo dularity standp oint Collision detection can
not b e isolated as a strict geometric problem the detection system must have information
ab out the dynamics of the simulated bodies The coupling however allows for a very
e cient collision detection system tailored to the needs of dynamic simulation
A blo ck diagram of Impulse s entire collision detection system is shown in Fig
ure The Lin Canny algorithm is used to compute geometric information ab out the
dynamic state
bounding box 123456 computer
2−5
1−2 Lin− 3 5−6 Canny 1 2 5 TOI 1−5 estimator 2 active collision heap
1 5 5 6 close 4 counters
hierarchical hash table
inactive object pairs
Figure A block diagram of Impulse s col lision detection system Arrows represent
data ows Numbers designate indices of bodies in the simulation The Lin Canny algo
rithm TOI estimator and active col lision heap perform narrow phase col lision detection
the bounding box computer hierarchical hash table and close counters are used for the broad
phase
separation of a pair b o dies it is describ ed in detail in Section This geometric informa
tion plus the dynamic state of the b o dies is used to estimate a time of impact TOI for the
pair Pending collision checks b etween pairs of b o dies are maintained in the collision heap
sorted by the pair s TOI Time of impact estimation and the collision heap are discussed in
Section The b ounding b ox computer hierarchical hash table and close counters form
the broad phase collision detection system They typically prune most of the pairs of b o d
ies from the active collision heap using inexp ensive tests These comp onents are describ ed
in Section The section also presen ts a comparison between the spatial hashing tech
nique used in Impulse s broad phase and a common technique based on co ordinate sorting
Throughout most of the chapter the fo cus is on collision detection b etween ballistic b o dies
which are ubiquitous in impulse based simulation Section describ es how the algorithms
may b e extended to more general typ es of motion
The Lin Canny algorithm
The heart of Impulse s collision detection scheme is the Lin Canny closest features
algorithm Lin LC an extremely fast metho d for tracking the features faces edges or
vertices between a pair of convex p olyhedra moving through space The principle b ehind
the algorithm is b est describ ed with a two dimensional example A fundamental concept
in the Lin Canny algorithm is that of a Voronoi region Consider the p olygon shown in
Figure The p olygon has eight features four vertices and four edges For each
V( v 1 )
e v1 4 v e1 4
v2 e3 v e2 3
V(e 3 )
Figure Apolygon and its Voronoi regions
feature F the set of p oints closer to F than to any other feature of the p olygon is called
the Voronoi region of F and denoted V F The shap es of the Voronoi regions are easily
deduced for p olygonal b o dies From eachvertex extend tworays outward from the p olygon
each p erp endicular to one of the edges incident to the vertex These rays form b oundaries
between the Voronoi regions The Voronoi region of a vertex is the in nite cone lying
between the two rays emanating from that vertex The Voronoi region of an edge is the
semi in nite rectangle lying b etween two parallel rays passing through the edge s endp oints
Collectively the Voronoi regions partition the space outside the p olygon
Theorem Given non intersecting polygons A and B let a and b be the closest points
respectively If a and b are the closest points between feature F of A andfeature F of B
a
b
between A and B then a V F and b V F
a
b
Proof Supp ose a V F Then a is in some other Voronoi region say V F and a is
c
b
closer to F than to any other feature on B Since b F b F andsoa and b can not
c c
b
b e closest p oints A similar results holds if b V F
a
The fundamental basis of the Lin Canny algorithm is the converse of Theorem
which is true for convex ob jects see Lin for the pro of
Theorem Given non intersecting convex polygons A and B let a and b be the closest
points between feature F of A and feature F of B respectively If a V F and b
a
b b
V F then a and b are the closest points between A and B
a
points between convex p olygons Theorem suggests an algorithm for nding the closest
Consider the situation on the left of Figure Here closest p oint candidates a and b
polygon A a
b y b v
polygon B a e polygon B
polygon A
Figure Left Theorem implies a and b are the closest points between A and B Right
a V F and so a and b are no longer closest points The closest feature on B wil l be
b
updated from vertex v to edge e
For simplicity degenerate cases where the p oints are on the b oundary of Voronoi regions are ignored
here Lin for more details
each lie in the Voronoi region of the other s containing feature By Theorem they are the
closest p oints In the situation depicted on the right b is still in the Voronoi region of F
a
however a is no longer in the Voronoi region of F Sp eci cally a lies on the wrong side of
b
ray y In this case the Lin Canny algorithm sp eci es that feature F should b e up dated to
b
the feature on the other side of ray y namely the edge e At this p oint the closest points
between the new features are computed and the Voronoi check is made again This pro cess
can continue for many iterations however it is guaranteed to eventually terminate with
a F b F and a and b the closest points b etween A and B The three dimensional
a
b
version of the algorithm is a natural extension of the two dimensional case The b o dies in
question are p olyhedra the features are vertices edges and faces the Voronoi regions are
in nite regions of space b ounded by constraint planes rather than rays Figure The
voronoi plane
old closest feature
closest point
new closest feature
Figure Tracking closest features of polyhedra with the Lin Canny algorithm
basic algorithm remains the same For details see Lin LC
Although designed to track the closest features the Lin Canny algorithm is easily
extended to a collision detection algorithm The distance between two p olyhedra is com
putable from simple geometric formulas given the closest features The closest points are
obtained as a by pro duct of these calculations With nite precision arithmetic a col lision
epsilon must b e used The collision detection system rep orts a p ossible collision when the
c
inter p olyhedral distance falls b elow The particular value of is not critical a value is
c c
chosen based on how large a gap is tolerable For animation purp oses the collision epsilon
should b e smaller than a pixel width so that the ob jects app ear to touch when colliding
For the simulation examples describ ed later in the thesis was two to three orders of mag
c
nitude smaller than the dimensions of the ob jects For example in the b owling simulation
which used a standard fo ot alley and inch pins was one millimeter
c
Collision detection and coherence
F or e cient collision detection for simulation it is extremely imp ortant to take
advantage of geometric coherence also called temporal or frame to frame coherence The
problem instances presented to the collision detection algorithm are a series of closely related
problems In Lin Canny terms the closest features b etween a given pair of ob jects usually
change relatively infrequently Even if the features are changing up on every invo cation
of the algorithm due to highly discretized p olyhedral mo dels or high velo cities the pair
p oint for of closest features from the last invo cation of the algorithm is a good starting
the search for the current pair of closest features In Impulse a two dimensional table of
closest feature pairs is maintained For every pair of bodies in the simulation there is a
corresp onding table entry containing the closest feature pair for these b o dies computed
from the last invo cation of the algorithm Figure illustrates the e ect of coherence on
tracking the closest features
The Lin Canny algorithm has b een describ ed as taking exp ected constant time to
rep ort a pair of closest features This claim stems from coherence often the closest features
do not change between successive calls and the algorithm veri es this fact in constant
This is a bit misleading Consider tracking closest features b etween a small satellite time
orbiting the Earth over its equator If the Earth is mo deled as a tessellated sphere with N
facets then during one orbit of the satellite tracking on the Earth must progress through
p
O N features As the resolution of the Earth mo del increases more work is clearly
Running Time of Lin−Canny Algorithm
55
50 1280 faces 3842 features 45
40
35 320 faces 962 features 30
25
20 80 faces 242 features 15
average time per call (microseconds) 10
5
0 0 5 10 15 20
rotation between calls (degrees)
Figure This graph shows the e ect of coherence on the performance of the Lin Canny
algorithm The algorithm was used to track the closest features between a xed cube and a
polyhedral model of a sphere as the sphere rotated on an axis paral lel to the nearest surface
of the cube The amount of sphere rotation between successive cal ls to the algorithm was
variedfrom one to degrees in one degr ee steps This experiment was performed for three
di erent discretization resolutions for the sphere as indicated above The performance of
the algorithm decreases as the rotation speed increases due to a decrease in coherence
Also note the insensitivity of the algorithm to polyhedron complexity when coherencecan be
exploited Ata rotational speedofonedegreebetween cal ls a fold increase in complexity
results in a increase in execution time
b eing done to track the closest feature as it circumnavigates the planet even if the satellite
p
sp eed remains constant In this case the Lin Canny algorithm is O N Figure and
Graph in Cohen et al CLMP also illustrate that the running time of the Lin Canny
algorithm dep ends on the number of features One di culty of assigning a complexity to
the algorithm is that it is very dep endent on how the ob jects are moving If the satellite
mentioned ab ove falls straight down toward Earth the algorithm is again constant time
The claim of exp ected constant time raises more questions than it answers however and
p
almost constant time is a b etter description In the satellite example the co e cienton N
is probably extremely small compared to the constant term In exp eriments with Impulse
there is negligible slowdown in simulation sp eed when p olyhedral mo dels of spheres with
a few hundred facets are replaced with p olyhedral mo dels with over facets over
features The latter mo dels are used in many of the simulations describ ed later in
the thesis
Extensions to the Lin Canny algorithm
The extension of the basic Lin Canny algorithm to curved ob jects has b een studied
by Lin and Mano cha LM Curved b o dies are approximated with a p olyhedral mesh
and closest p oints are tracked b etween these meshes The closest p oints on the meshes are
pro jected onto the actual curved surfaces and a numerical ro ot nding metho d uses these
points as a starting p oint to lo cate the true closest p oints A general form of this algorithm
has not b een implemented
the extension to non convex b o dies As Another extension used in Impulse is
suming non convex b o dies can b e decomp osed into a group of convex pieces Lin Cannycan
still b e used to compute the distance b etween a pair of b o dies If b o dy A is decomp osed into
m convex pieces A A andbodyB is decomp osed into n convex pieces B B
m n
then the distance b etween A and B canbecomputedas
d A B min d A B
i j
i m
j n
In other words the distance computation is broken into m n standard Lin Canny compu
tations b etween convex b o dies Because of this reduction of the non convex case to a group
of convex cases the rest of this chapter assumes convex b o dies
ver this naivescheme is p ossible by computing the convex Signi cantimprovemento
hulls of each of the non convex b o dies and computing the distance between b o dies as the
distances b etween their hulls This requires only one Lin Cannyinvo cation and since A is
enclosed by its convex hull the distance to the convex hull is a lower b ound a conservative
estimate on the true distance to A Only if the distance between the hulls reaches zero
must the b o dies b e unwrapp ed and treated as a collections of convex pieces For complex
b o dies this scheme can b e applied recursively to obtain an entire tree structure for a non
convex b o dy A The inner no des of the tree are convex hulls of various subsets of the entire
body and if a particular convex hull is pierced that tree no de is replaced by its children
The leaves of the tree represent the underlying convex decomp osition of the entire body
and the ro ot of the tree is the convex hull of the entire b o dy With sucha structure only
as much of the body is unwrapp ed as needs to be to determine distance the rest of the
concavities remained wrapp ed in convex hulls Details of this scheme may be found in
Ponamgi et al PML At present a general version of this algorithm do es not exist
although one is being develop ed as part of I COLLIDE CLMP In Impulse the naive
approach represented by Equation is used to handle non convex b o dies
checks Prioritizing collision
For simulation the collision detection algorithm must determine the time t at
c
which a collision o ccurs b etween b o dies The typical approach is akin to the approach taken
in numerical ro ot nding Assume there exists a function that takes two b o dies and returns
a b o olean value indicating if they are p enetrating or not If the b o dies are not p enetrating
at time t dynamic integration of the system state continues to some time t If at this
point the bodies are re tested and p enetration is detected than a collision has o ccurred
t t t t between them at some time t with t t t In this case a new p oin
m c c
is chosen and dynamic integration is p erformed from t to t Based on the result of a
m
collision test at the time t the pro cess is rep eated recursively on the interval t t or
m m
t t The pro cess is rep eated until the width of the interval falls b elow some tolerance
m
The time t maybechosen as the midp ointoftheinterval t t however Bara rep orts
m
much faster convergence using an interp olation metho d instead of simple interval bisection
Bar If the p enetration function returns a distance of separation or p enetration rather
than a simple b o olean value one can more accurately estimate the moment of collision by
The distance of separation is well de ned for convex examining this distance at t and t
b o dies distance of p enetration can b e de ned as in SSCK
A particular trait of the Lin Canny closest features algorithm as originally sp eci
ed had a strong impact on the way collision detection is p erformed in Impulse When the
algorithm is passed two p olyhedra that are p enetrating it cycles between pairs of closest
features never terminating The pro ofs which guarantee convergence and termination rest
on the assumption that the b o dies are non p enetrating Lin motivates the solution to this
problem based on constructing internal pseudo Voronoi regions that partition the space in
side of the p olyhedron and matching eachinterior region with a feature on the p olyhedron
Lin This extension to the original algorithm is nontrivial and it was not pursued at
the time the collision detection subsystem of Impulse was develop ed
Instead the collision detection subsystem in Impulse was designed to detect col
lision before it o ccurred This is done by obtaining successively closer approximations to
the exact collision time t without ever integrating b eyond t The situation is as depicted
c c
in Figure in which the function d t represents the distance b etween two b o dies nega
tive values and zero crossings corresp ond to p enetration and collisions resp ectively The
d(t)
t t t t t
1 2 3 4 tc t’c
Figure A one sided approach to root nding is used to nd col lision times in Impulse
The function d t is the separation distance between two bodies over time the zero at t
c
indicates a col lision The velocities of the two bodies at time t are used to compute a
i
of the parabola gives t parabola that is less than d t for t t The next zero crossing
i i
a closer approximation to t that is guaranteed not to exceed t
c c
derivativeof d t is a measure of the velo city of approach of the b o dies In Impulse a series
of approximations t t t is made to the actual collision time t such that no t exceeds
n c i
t At each t the current velo cities of the relevant b o dies are used to compute a second
c i
order function which is a lower b ound on the separation distance between the b o dies for
t t The abscissa of the next zero crossing of the corresp onding parab ola b ecomes t
i i
The series of closer approximations to t ends when the distance between the b o dies falls
c
b elow Using a Newton Rhapson approach to ro ot nding it is p ossible to miss a zero
c
crossing in d t for example the crossing at t in Figure is likely to be missed This
c
ro ot can also b e missed if there is a minimum time interval b etween collision checks With
the one sided approach every ro ot of t is found
The collision heap
When collision checking reveals that the distance b etween two b o dies exceeds the
collision epsilon alower b ound on their time of impact TOI is automatically computed
c
and returned In terms of Figure the current time is t and the value returned is t
i i
Assume a TOI estimator exists which takes as inputs the dynamic states of two b o dies
along with the closest p oints b etween them It returns a lower b ound on the time of impact
based on the assumption that the b o dies will follow ballistic tra jectories until the moment
of impact The returned TOI estimates can b e used to adaptively control the frequency of
ks while integrating the dynamic system collision chec
Collision checks are scheduled in a col lision heap Corresp onding to each pair of
b o dies in the simulation is an element in the heap containing a eld with the last computed
TOI for that pair The TOI eld is a lower b ound on the true time of impact for the b o dies
which is unknown The heap is sorted on this TOI eld so that no collisions can o ccur
b efore the time in the TOI eld of the top heap element Figure The top level
simulation lo op is very simple The system is evolved to the time in the TOI eld of the top
heap element at which p oint collision checking is p erformed for this top top pair of b o dies
If the distance b etween the b o dies is b elow the collision epsilon a collision is declared and
is handled by the collision resolution system In any case the TOI for the body pair is
recomputed p ossibly causing it to drop down in the heap and the pro cess is rep eated
This scheme adapts the frequency of collision checks appropriately when a pair of b o dies
are far apart or moving slowly collision checks between them will be infrequent as the
b o dies approach checks increase as necessary
dynamic state
1234 current time, t 0 − TOI 2−4 +
1−2 Lin− 3−4 Canny TOI 1−3 integration estimator 2−3 step, ∆ t closest points 1−4
collision heap
Figure Impulse s narrow phase col lision detection system prioritizes col lision checks in
aheap based on estimated times of impact Thetimeofimpact of the top heap element de
termines the size of the next integration step Numbers designate the indices of the di erent
bodies in the simulation
Estimating time of impact
For the remainder of this chapter the following notation is used
t A general time variable The p ositions and velo cities
of all b o dies are functions of t
t The current time in the simulation The p ositions and
velo cities of all b o dies are known at time t
t The time of the next collision b etween two b o dies
c
t Some time step into the future Often the motion of
b o dies over the interval t t t is reasoned ab out
ust b e solved in order to apply the collision heap scheme is The key problem that m
Problem Given The current positions and velocities of two bal listic convex bodies and
the closest points between the bodies Compute A lower bound on the time of impact of the
two bodies assuming they continue their current bal listic trajectories
One di culty is that the p oints on the bodies that will ultimately collide are not readily
found from the given information Figure illustrates a bad case the linear and angular
velo cities of the b o dies are such that the closest p oints with total velo cities u and u are
moving apart However another pair of p oints not even considered by the closest features
algorithm are almost as close as the closest p oints and are approaching each other quickly
body 1 ω v 1 1
u1
ω 2 u2
v2
body 2
Figure The velocities of the closest points only is insu cient for predicting time of
impact
One metho d of b ounding the time of impact relies on the convexity of the b o dies
Consider the bodies shown in Figure Call the closest points on the two bodies c
and c and let d c c Since b o dy is convex it must lie entirely on one side of the
plane passing through c and p erp endicular to the vector d The same is true for body
and the corresp onding plane through c Let x and x b e the p oints at which the b o dies
will ultimately collide No matter where these points are lo cated and what their path to
There are closed form solutions for the orientation of a rigid b o dy as a function of time that use elliptic
integrals MR Using these equations with some metho d of evaluating elliptic integrals might lead to
tighter b ounds than the ones discussed here
body 1 x1
c1 d x2
c2
body 2
Figure Finding a lower bound on the time of impact of convex bodies
impact will b e they will have to cover the distance d kdk in the direction of d Let d
be a unit vector along d Let D t be an upp er bound on the distance traveled by any
pointonbody along d during the interval t t Let D t similarly b ound the distance
t traveled by any point on b o dy along d If a collision o ccurs at time
c
D t D t d
c c
A lower b ound on the time of impact can be found by replacing this inequality with an
equality and solving for t To facilitate this the functions D t should be kept simple
c i
Impulse uses quadratic functions
Problem TOI co e cients Given The position and velocity of a body at the current
time t and a direction speci ed by the unit vector d Find Co e cients A and B such
that the distancetraveled by any point on the body in the direction d over the interval t t
satis es
D t A t t B t t
Consider the case of a ballistic b o dy The total velo city u t of an arbitrary p oint
x on the b o dy is given by
u t v t t r t
Here v and are the linear centerofmassvelo city and angular velo cityofthebody andr
is the p osition vector from the b o dy s center of mass to x all sp eci ed in a xed reference
frame Since the b o dy is ballistic
v t v t g t t
where g is the vector acceleration of gravity in the reference frame Thus the velo cityofx
in the direction d is given by
u t d v t g t t d t r t d
Letting r be the maximum distance of any pointonthe b o dy from the center of mass
max
and be the maximum magnitude of the b o dy s angular velo city during the current
max
ballistic phase
u t d v t g t t d r
max max
body The next section dis The quantity r may be pre computed and stored for each
max
cusses the computation of Integrating the ab ove equation over time
max
Z
h i
t
g d t t v t d r t t u d d
max max
t
The function on the right hand side is a suitable choice for D t For ballistic b o dies the
TOI co e cient routine returns
A g d
B v t d r
max max
Other typ es of motion b esides ballistic motion can b e accommo dated by the same scheme
and are discussed later
To compute a lower b ound on the time of impact b etween body and body a
TOI co e cient routine is called for each of the b o dies In computing the TOI co e cients
for b o dy d is used in place of d Call the co e cients for b o dy A and B call those
for body A and B The lower b ound TOI is the smallest real ro ot t t of the quadratic
equation
A A t t B B t t d
If both b o dies are ballistic a common case in impulse based simulation the gravitational
contributions for each b o dy cancel and A A In this case only a linear equation must
b e solved If the ab ove equation has no real ro ots greater than t then the b o dies can never
collide given their current tra jectories In this case the time of impact routine returns
in nity If one of the b o dies collides with some other body in the environment however
the time of impact b etween bodies and must b e recomputed and may b e reduced to a
nite value
Bounding ballistic angular velo city
The TOI calculations of the last section assume knowledge of the maximum mag
nitude of the angular velo city of a b o dy during a ballistic tra jectory
Problem Given The current angular velocity of a bal listical ly moving body Find A
bound on the maximum magnitude of while the body moves in its current trajectory
Since only the magnitude of the b o dy s angular velo cityvector is needed can b e expressed
in the most convenient frame in this case the b o dy frame Below vectors and tensors are
expressed in this frame unless otherwise noted
The angular momentum of the b o dy is given by
L t I t
where I is the b o dy frame inertia tensor see App endix A There is no time dep endence in
this matrix Since the only external force acting on the b o dy is gravity which acts through
the center of mass the angular momentum is conserved in the inertial frame Although
angular momentum is not conserved in the b o dy frame it is related to the constant inertial
time varying rotation matrix Hence the magnitude of the angular momentum through a
b o dy angular momentum is conserved Therefore
kL t k kI t k
The b o dy inertia tensor I is diagonal Calling its diagonal elements I I andI the ab ove
x y z
equation can b e rewritten
t t t
x y z
kL t k kL t k kL t k
I I I
x y z
Thus the body angular velo city vector is constrained to lie on an ellipsoid in R Fig
ure The maximum value of k t k is just the length of the semi ma jor axis of this ω z
ω y ω
x ω(t)
Figure The body angular velocity vector remains on an el lipsoid over a bal listic
trajectory
ellipsoid thus
q
I t I t I t
x y z
x y z
kL t k
k t k
min I I I min I I I
x y z x y z
This b ound was rep orted in MC b however a slight improvement is p ossible
The conservation of energy de nes a di erent ellipsoid on which the angular velo citymust
lie Since there is no net torque acting on a body in a ballistic tra jectory the rotational
kinetic energy is constant given by
T
t I t E
Since the b o dy inertia tensor is diagonal
E I t I t I t
x x y y z z
Writing this in the general form of an ellipsoid gives
t t t
y z x
q q q
E
E E
I
I I
y
x z
The maximum value of k t k is the length of the semi ma jor axis of this ellipsoid
s
s
E I t I t I t
x x y y z z
k t k
min I I I min I I I
x y z x y z
Let I min I I I and I max I I I The ratio of the angular
min x y z max x y z
kinetic energy b ound to the angular momentum b ound is
s
I t I t I t I
x x y y z z min
r
I t I t I t
x y z
x y z
This implies
s
I
min
r
I
max
The angular kinetic energy b ound is always at least as tight as the angular momentum
bound The b ounds are the same when the body is rotating exactly ab out the axis of
minimum inertia and the b ounds are most di erent when the rotation is exactly ab out the
axis of maximum inertia The two b ounds are always equivalentif the diagonal entries of
the b o dy inertia tensor are all equal as is the case for a uniform density sphere or cub e
Bounding box techniques
The TOI estimation describ ed ab ove assumes that b o dies follow ballistic tra jec
and in tories between collisions Supp ose collision detection is p erformed on b o dies
dicating they are not yet colliding and let t be the computed time of impact for these
c
two b o dies The time t re ects the so onest time these b o dies may collide assuming they
c
continue along their current ballistic tra jectories It is p ossible however that b o dy may
collide with some other body say body before t is reached A collision impulse will b e
c
applied to body sending it on a new ballistic tra jectory Thus t is invalid and must
c
b e recomputed In fact all TOIs that involve b o dy or b o dy must b e recomputed The
collision heap is actually a priority queue since the keys are not static but can increase or
decrease causing heap elements to rise or fall b efore ever reaching the top
Early versions of Impulse p erformed O n TOI up dates up on every collision in an
n b o dy simulation but the metho d scales p o orly Consider a group of coins tossed onto a
large at surface Even if the coins are separated by large distances as they b egin to settle
on the surface collisions b ecome frequent Every time a coin collides with the surface the
TOIs b etween that coin and every other coin must b e recomputed even when the coins are
in no danger of colliding The problem is that spatial lo cality is not b eing exploited When
a coin exp eriences a collision with the surface it is reasonable to recompute its TOI with
nearby coins but not with ones that are far away A related ine ciency is that collision
checks are p erformed b etween b o dies very far apart and in no danger of colliding The heap
scheme reduces the frequency of checks between such bodies however they are still made
at regular intervals Bounding box techniques may be used to reduce these sup er uous
collision checks and TOI up dates The metho d used in Impulse is based on a hashing
scheme prop osed byOvermars for solving p oint lo cation problems Ove The technique
is rst discussed in a static context and then the extension to moving b o dies is describ ed
Finding static box intersections
Point lo cation problems o ccur frequently in computational geometry One variant
is expressed as follows
Problem Point Lo cation Given a number of non intersecting cel ls in space store
the arrangement such that for a given query point p the cel l containing p if any can be
determined e ciently
Here a cel l is a connected region of space Overmars presents two solutions to this problem
e cient solution involves under the restriction that the cells are fat Ove The more
surrounding each cell by an axes aligned b ounding box and storing the lo cation of these
boxes in a hash table The technique can be extended to solve a more useful problem for
collision detection
Problem Static box intersections Given n axes alignedrectangular boxes B B
n
xedinspace store this arrangement such that the boxes that intersect a speci ed query box
B can be determined e ciently
q
Toattack this problem consider partitioning space into a cubical tiling with resolu
tion Anypoint x y z in space b elongs to a unique tile sp eci ed byinteger co ordinates
under the tiling map
x bx c
y by c
z bz c
The tiles that b ox B intersects are found by computing the images under of two
i
of B s corners the one of minimum x y and z co ordinates and the one of maximum x
i
y andz co ordinates The two tiles containing these corners and the other tiles b etween
them are the tiles that intersect B A tile with co ordinates a b c isbetween the two tiles
i
with co ordinates a b c and a b c if and only if a a a b b b and
c c c There are an in nite number of tiles in unb ounded space but only a nite
numb er that are intersected by at least one b ox For each tile that a b oxintersects the b ox s
lab el is stored in the hash table hashed under the tile s integer co ordinates The query
box B can also b e mapp ed to a set S of hash buckets according to which tiles it overlaps
q
The union of all b oxes whose lab els app ear in buckets of S can b e quickly determined and
this set forms a candidate set of b oxes for Problem The candidate set is further checked
against B using more exp ensiveboxintersection tests
q
Howtocho ose the tiling resolution is not obvious and in fact this can b e prob
lematic when the sizes of the b oxes vary widely If is small the larger b oxes mayintersect
ahuge numb er of tiles and thus require a large amount of storage in the hash table In ad
dition when the static assumption is relaxed up dating the p ositions of these large moving
t On the other hand if is large the tiling will have p o or resolution boxes will b e ine cien
power for the smaller b oxes Many small b oxes may hash to the same tile so the initially
computed set S is large and the more exp ensive box intersection test will be p erformed
on many pairs Overmars solves this problem by partitioning the set of cells into groups of
similar size and creating one hash table for each group For collision detection however
any one of the boxes may need to be checked for intersection with any other this is not
to disjoint groups is not true in the point lo cation problem and so partitioning boxes in
helpful The solution is to build a hierarchical hash table comprising several resolutions
and checking for intersections among b oxes at di erent resolutions
To understand the metho d consider the one dimensional example shown in Fig
ure Here the six b oxes that is line segments p opulating space are lab eled A
through F Let X denote an arbitrary box from this set and de ne sz X as the size in
this case length of X As a prepro cessing step one cho oses constants and and a
minimal sequence of tiling resolutions such that
n
ρ 1 1 C C
2 FF
3 EE
r e s o l u t i n 4 ABB D ρ
AB C DE4 F
Figure A one dimensional example of a hierarchical spatial hash table with four reso
lutions The value is the size of the tiles at resolution i The cel ls which must be checked
i
when box E is stored in the table are shaded
n
and so that for eachbox X there exists an integer k n with
sz X
k
The integer k is called the resolution of b ox X abbreviated res X In Figure
and This means that eachbox X must have a length that is from to times
the width of the cells at resolution res X The constraints are met bycho osing four
tiling resolutions as shown in the gure with res A res B res D res E
res F and res C The lo cation of box X is hashed at tiling resolution res X
In two or three dimensions the idea is the same The cells are squares or cub es of side
or box X length and the boxes to be stored are rectangles or rectangular prisms F
i
sz X is the maximum distance b etween two opp osite edges or faces of the b ox In what
follows d denotes the dimension of the b oxes and ambient space
When a b ox is stored in the hash table overlap with other b oxes must b e checked
some of whichmay be stored at other resolutions Assume the boxes are hashed in order
of increasing resolution When b ox X is hashed all enclosing cells at resolutions less than
or equal to res X must b e checked for other b oxes In Figure the cells that must b e
checked when box E is stored are shaded Since body C overlaps one of these cells the
b o dies E and C are rep orted as close meaning the hash table is unable to v erify that the
boxes do not overlap Formally
De nition Boxes X and Y are close if and only if they overlap a common cel l at
resolution min res X res Y
When b oxes are close full collision detection must b e p erformed b etween the corresp onding
b o dies The storage requirements and relevant time complexities for a hierarchical spatial
hash table scheme are now analyzed assuming p erfect hashing
Lemma Let sz and sz be the sizes of the smal lest and largest boxes to be stored in
min max
the hierarchical hash table If n is the the number of resolutions required
sz
max
n log
sz
min
Proof Cho ose sz and subsequent tile resolutions such that the constraint
min
i i
is satis ed for i n In this way an appropriate k can b e found to satisfy for any
box dimension in the interval sz an n is needed such that sz From
min n n max
n
n
Substituting sz for and using sz yields
min n max
n
sz sz
max min
The lemma follows
Lemma gives an imp ortant theoretical b ound but it is not always tight For
instance if all boxes are one of three sizes small medium or large than at most three
resolutions are required for the hierarchical spatial hash table regardless of the ratio of the
dimensions of the largest to smallest b ox
Theorem For a set of boxes to be stored in a hierarchical spatial hash table let R be the
ratio of the largest to smal lest box dimension Then the total number of hash buckets which
d
log R must be checked for other boxes when storing a box in the hash table is O
oof When b ox X is stored cells at resolutions i k where k res X must b e checked Pr
d
for other boxes By sz X and so box X overlaps at most cells at k
resolution k The number of cells overlapp ed at a given resolution i k can not be more
than this and since there are O log R resolutions by Lemma the number of overlapp ed
d
cells is O log R Each cell check corresp onds to one bucket check in the hash table and
the theorem follows
Theorem Treating and R as constants a hierarchical hash table can report al l pairs
of close boxes among n boxes in O n c time where c is the number of close pairs
Proof By Theorem a constantnum b er of hash buckets must b e examined up on storing
each box the total number of buckets checked is O n The time sp ent rep orting closest
pairs among these buckets is O c
A nal theorem relates the resolution p ower of the hierarchical hash table to the
parameter
Theorem The hierarchical spatial hash table can guarantee that two boxes X and Y do
not intersect if the distance between them exceeds
p
d max sz X sz Y
Proof Without loss of generality assume sz X sz Y so that res X res Y Assume
X and Y are rep orted as close Then they overlap a common cell at resolution res X The
p
d and so the distance d maximum distance between any two points in this cell is
resX
between b oxes X and Y satis es
p
d
res X
d sz X
sz X
From
res X
sz X
and the theorem follows
The tradeo s involved in cho osing the parameters and are now apparent
Recall that and By Theorem the closer is to the minimum value of
the fewer cells must be checked when storing b oxes By Theorem the closer is to the
upp er b ound the b etter the resolving p ower of the hash table Finally Lemma implies
that the larger the ratio the fewer resolutions are required to store all of the b oxes
Coherence and the tiling scheme
In simulation applications a slight variant of the hierarchical hash table scheme
can improve p erformance by taking advantage of coherence between problem instances
Instead of storing box X s lab el in buckets at res X and only checking the appropriate
buckets at lower resolutions the lab el is stored in all buckets that are checked Using the
example of Figure the lab els are stored as shown in Figure Boxes X and Y are ρ 1 1 A BC C D E F
2 A B D E EFF
3 A B DEE
r e s o l u t i n 4 A BB D ρ
AB C DE4 F
Figure In a variant of the hierarchical hash table scheme the labels of each box are
stored in every bucket that is checked for other boxes
rep orted as close if and only if their lab els app ear in a common hash bucket at resolution
min res X res Y the close pairs returned by the algorithm are exactly the same as with
the original version A two dimensional array of close counters tracks which boxes are
close Each time X is stored into a hash bucket at resolution i that already contains Y if
res X res Y then the counter corresp onding to the pair X Y is incremented i min
When X is removed from such a bucket the counter is decremented Pairs of boxes for
which the corresp onding counter is zero are not close and narrow phase collision checking
is not p erformed b etween the corresp onding b o dies When a counter is incremented from
zero to one the pair enters the set of bodies on which narrow phase collision checking is
p erformed
This scheme retains the state of the b oxes b etween invo cations If a b oxintersects
the same cells that it did on the last call no additional work needs to be done to store
that box on the current call If the box has moved into new cells or left old ones only
buckets corresp onding to these cells must b e changed Boxes corresp onding to xed b o dies
need only b e stored into the hash table once The disadvantage of this scheme is that more
pro cessing is sometimes required to store a box s lab el into a bucket In Figure even
though D and E are veri ed as not close at resolution they must b e stored in common
buckets at resolutions and requiring extra pro cessing and adjustment of close counters
For this reason the claim of Theorem is not valid or at least not readily apparent for
this variant on the algorithm However this variant is quite e cient in practice as shown in
Table The table shows the numb er of cycles sp ent on broad phase collision detection for
millions of cycles
standard coherence
example hashing hashing ratio
coins
bowling
rattlebacktop
part feeder chute
Table Comparison of hashing schemes
some example simulations using both the standard hashing algorithm and the coherence
hashing algorithm The simulations themselves are describ ed in detail in Chapter From
the table the coherence hashing algorithm is signi cantly b etter consistently running six
or more times faster than the standard hashing algorithm For this reason the coherence
hashing algorithm is used in Impulse
Maintaining the collision heap
The hierarchical spatial hash table describ ed in the last section can b e used to cull
unnecessary collision checks and TOI up dates for a group of moving b o dies in a simulation
Up on each call to the integrator the state of the system is evolved from the current time
t to some future time t t The step t is determined by the earliest TOI eld on the
After t is computed but b efore the integrator is called for each top of the collision heap
body an axes aligned b ounding box is computed that is guaranteed to enclose the swept
volume of the b o dy during the up coming integration
Consider ballistic body i with b ounding box B The center of mass of body i
i
follows a parab olic tra jectory that is known from the current state of the b o dy B is found
i
by noting the p osition of b o dy i s center of mass at the currenttime t at the time t t
and p ossibly at the ap ex of its parab olic tra jectory should this occur during the interval
t t t The b ox which b ounds these two or three p oints is grown by r the maximum
i
radius of b o dy i to obtain B Figure Clearly computing B involves a xed number
i i
Bi
ri
time t 0 ri
ri ri f ri ∆ time t 0 + t
ri
Figure The bounding box for body i s swept volume during a segment of a bal listic
trajectory
of op erations and can be done in constant time This fact and Theorem imply that all
bounding boxes can b e computed stored in the hash table and close pairs rep orted all in
O n c time for an n body simulation c is the number of close boxes This is done at
the b eginning of eachintegration step For static b o dies suchaswalls or platforms it is not
necessary to grow the b ounding b oxby the b o dy radius and the resulting b ounding b oxes
are much tighter also b oxes for xed b o dies need never b e up dated
For each pair of b o dies that the hash table deems close there is a corresp onding
t maintained in the collision heap Pairs not deemed close are in no danger of elemen
colliding during the next integration step When the close counter for a particular pair is
incremented from zero to one the TOI for that pair is computed and the pair is added
to the heap if it is not there already Since pairs in the heap are kept sorted on the TOI
eld no pair can collide any so oner than the pair at the top of the heap The advance
algorithm Figure is p erformed at the currentintegration step As can b e seen from
the algorithm new pairs are inserted into the active collision heap before any integration
o ccurs This is b ecause the computed TOI for suchapairmight precede the TOI previously
advance
t TOI at top of heap t
for each body i
compute bounding box B over interval t t t
i
store B in hierarchical hash table
i
for each j such that closeCounts i j was incremented to
if pair i j is not in heap
compute TOI between bodies i and j
store pair in collision heap
since top TOI might have decreased due to new pairs
t TOI at top of heap t
t t t integrate state of system over
Figure advance Advance the state of the system forward in time as far as p ossible
while guaranteeing no collisions are missed
at the top of the heap in which case the integration step must b e shortened
The question of when pairs should be removed from the heap remains One ap
proach would b e to remove pairs as so on as the hash table indicates the b o dies b oxes are
no longer close This can cause pairs of bodies to rapidly move in and out of the heap
esp ecially when one of the boxes is close to the b oundary of a tiling cell Figure To
avoid this ine cient behavior hysteresis is applied to the collision heap A body pair is
pair may not be removed added to the heap whenever the boxes become close but the
b efore it bubbles up to the top of the heap After a collision check for the top heap element
is made the closest counter status for that pair is examined If it is nonzero the boxes
are still close and the pair is reinserted into the heap after computing the new TOI If the
closest counter for that pair is zero the pair leaves the heap
The hierarchical hashing scheme also reduces TOI up dates up on collision Recall
that without the hashing scheme whenever bodies i and j collided all TOIs involving
either one of these b o dies must be recomputed However with the hashing scheme TOIs
spatial partitioning
body 1
body 2
Figure If body is bouncing on xed body the pair of boxes corresponding to these
bodies may rapid ly toggle between close and not close status To avoid this jittering hys
teresis is applied to the col lision heap
are only maintained for pairs in the heap As a result if b o dies i and j collide TOIs are
only recomputed b etween these b o dies and nearby b o dies This greatly reduces the cost of
pro cessing collisions
the The sizes of the b ounding boxes dep end on the current linear velo cities of
b o dies These can not b e known for all time at the b eginning of the simulation As a result
tiling resolutions can not b e chosen as describ ed in Lemma In practice this is not such
a problem In Impulse the tiling resolutions are based simply up on the maximum radii of
the b o dies Unless the b o dies are moving at an extremely fast sp eed the number of tiles
intersected by the various b oxes remains small Figure shows the reduction in narrow
phase collision detection due to the hierarchical hashing scheme
Spatial hashing versus co ordinate sorting
In addition to the hashing scheme describ ed ab ove there is another algorithm for
nding intersections of axes aligned b ounding b oxes The algorithm is based on sorting the
co ordinates of edges of the b ounding b oxes along each of the three co ordinate axes it is used
in the I COLLIDE system CLMP and also in Bar The algorithm works as follows
The minimum and maximum x co ordinates of each axes aligned box are maintained in a
and z co ordinates Two boxes overlap if and only sorted list The same is done for the y
if their co ordinates overlap in each of the three co ordinate directions A two dimensional
Figure Snapshots taken during the simulation of eight coins tossed ontoa atsurface
The lines indicate the tracking of closest points between bodies each line corresponds to
one element in the active col lision heap The left gure was produced with the broad phase
col lision detection deactivated and the right one with the broad phase activated The broad
phase greatly cul ls col lision checks Also when abody experiences a col lision impulse only
onnected to it by lines need be updated the TOIs with bodies c
example is shown in Figure For example since x the maximum x co ordinate of b ox
B is less than x the minimum x co ordinate of b ox B these boxes can not overlap On
the other hand x x x and y y y Thus boxes B and B overlap in b oth
the x and y co ordinates and therefore the b oxes themselves overlap Cohen et al discuss
a body at any the relative merits of using a xed size cubical b ox that can accommo date
orientation versus tighter tting b oxesthatchange in shap e as the b o dy rotates CLMP
Coherence is exploited by up dating previously sorted lists to obtain new sorted
lists In this way thenumb er of exchanges needed to obtain the new sorted list is exp ected
to b e O n It can however b e O n Consider the situation depicted in Figure The
maximum and minimum y co ordinates of all the b oxes are clustered closely together Even
with very small motions from one time step to the next O n exchanges result in resorting
the y co ordinates coherence breaks down This example is not contrived Imagine throwing
a group of dice onto a at horizontal surface As the dice come to rest their b ounding
Since co ordinate sorting is based boxes will tend to cluster along the vertical co ordinate
on dimension reduction the co ordinates may b e clustered even when the original b oxes are
not the clustering b ecomes worse in higher dimensions One way of handling the clustering
problem is to p erform a less drastic dimension reduction pro jecting the three dimensional
boxes rst into two dimensional rectangles in the plane and rep orting intersections among
y’4 4 y4
y’3
y’2 3
y’1 2 y 3 1 y2
y1
x1 x4 x3 x’1 x’4 x’3 x2 x’2
Figure A two dimensional version of I COLLIDE s bounding box check Two boxes
overlap if and only if their projections onto the x and y axes overlap
the rectangles in O n log n k time where k is the number of intersections Ede Hashing
schemes do not su er from the clustering problem Coherence always results in e cient
up dating of the hash table unless the number of boxoverlaps in three dimensions is large
Co ordinate sorting do es have one advantage over hashing no hashing scheme culls
as many b o dy pairs as co ordinate sorting Cohen et al claim that cho osing a near optimal
cell size is di cult and failing to do so results in large memory usage and computational
ine ciency These claims are largely mitigated with a hierarchical hash table based on
multiple cell sizes A very e cient collision detection scheme based on boxes that are not axes aligned is describ ed in GLM
y
1 3 2 4 5
x
Figure A bad case for coordinate sorting The dense clustering of box extrema along
the y axis results in O n exchanges for each new sort of the coordinates
Generalizing collision detection
The collision detection techniques describ ed thus far are tailored to ballistic b o dies
which are ubiquitous in impulse based simulation The techniques are extensible to other
typ es of motion with the de nition of two routines
A swept volume routine This takes the state of the body at the current time t
and a time interval t and returns an axes aligned box that encloses the center of
mass s tra jectory during the time interval t t t
A time of impact co e cients routine This takes the current state of the body
and a directional vector d It returns two co e cients A and B such that the distance
anypoint on the body travels in the direction of d is b ounded by the expression
A t t B t t t t
The swept volume routine is needed during the broad phase of collision detection and the
narrow phase Previous sections describ ed TOI co e cient routine is needed during the
how the swept volume and TOI co e cients are estimated for ballistic b o dies For xed
b o dies the routines are trivial the swept volume is always the currentvolume o ccupied by
the body and the TOI co e cients are both zero The swept volume and TOI co e cient
routines for b o dies connected byjoints are discussed in Chapter
Another typ e of motion is scripted motion Scripted bodies follow unalterable
tra jectories through state space they are imp ervious to external forces like gravity and
collisions with other b o dies An example application of a scripted b o dy is a vibrating part
feeder Since the mass of the feeder is much greater than the mass of the small parts it
vibrates along a track one could neglect the e ects on the feeder of collisions with the parts
Instead the feeder might execute some prescrib ed sinusoidal motion Impulse pro vides
several typ es of scripted b o dies and others could b e added for sp eci c applications Some
simulations with scripted b o dies are describ ed in Chapter One typ e of scripted b o dy is a
cycler A cycler s center of mass follows an elliptical path through space while the cycler s
orientation remains constant For illustration consider the case where the path is a circle
in the x y plane Let r b e the radius of this circle and let b e the angular sp eed at which
the centerofmassmoves around the circle This is not the same as the angular velo cityof
the body which is zero since the orientation is held constant
First consider the swept volume routine Since the center of mass remains in the
x y plane the minimum and maximum z co ordinates of the center of mass are b oth The
minimum and maximum x and y co ordinates are inferred from the smallest axes aligned
the rectangle in the x y plane which encloses several points p t the current lo cation of
center of mass p t t the lo cation of the center of mass at a time t in the future
and any crossings of the tra jectory with the x and y axes over the interval t t t
Figure All of these p oints are easily computed since the tra jectory in the x y plane
y y y p(t + ∆ t ) 0 p(t 0 )
p(t 0 ) x x x p ∆ (t 0 + t ) p(t 0 ) p ∆
(t 0 + t )
Figure The axes alignedbounding rectangles in the x y plane for a cycler body Three
di erent cases are shown The point p t is the current position of the center of mass and
the point p t t is the position of the center of mass a time t in the future
is known in closed form
r cos t x
r sin t y
Now consider the TOI co e cients for this cycler Since the angular velo city of
a cycler is zero every point on the body has the same velo city as the center of mass at
all times Thus it su ces to b ound the motion of the center of mass along the directional
vector d There are di erentways to do this Let v t bethelinear velo cityof the center
of mass of the body The magnitude of v t is a constant equal to r Therefore if D is
the distance that any p ointonthebodytravels in the direction d
D r t t
This b ound suggests cho osing the TOI co e cients in to b e A and B r
A second metho d for b ounding motion is to observe that the currentvelo city of the
center of mass in the direction d is given by v t d The magnitude of the acceleration of
the center of mass is b ounded by r This is the centrip etal acceleration always directed
toward the center of rotation Thus
D r t t v t d t t
r and B v t d This suggests cho osing the TOI co e cients A
The co e cients indicated by and are for a linear and quadratic mo del
for distance traveled resp ectively In general the quadratic mo del has a smaller linear term
and will b e a tighter b ound on the distance traveled up to some time After that time the
quadratic term will dominate and the quadratic bound will overtake the linear one To
decide which mo del to apply one can use the current distance d to the other b o dy Impulse
checks the distance at which the quadratic mo del overtakes the linear one If this is greater
than d the quadratic co e cients are returned otherwise the linear co e cients are returned
The time at which the two mo dels are equal is obtained by equating the right hand sides
of and
r t t r t t v t d t t
which yields
h i
r v t d
t t
r
At this time the distance b ound given by b oth mo dels is
h i
r v t d
D
If this b ound is less than the distance to the other b o dy the cycler TOI co e cients routine
returns the co e cients of t and t in otherwise the co e cients in are returned
This strategy gives whichever b ound is likely to b e tighter
Other typ es of motion may b e handled in the same way the analyses are similar
to the one for cyclers describ ed ab ove It is not necessary to p erform analyses based on
pairs of motion typ es for instance for cycler ballistic pairs cycler xed pairs ballistic xed
pairs and so on Rather the swept volume and TOI co e cient routines are de ned for
each single motion typ e and then collision detection can be p erformed between this typ e
and all other motion typ es For n typ es of motion only O n analyses are required instead
n This feature is also shared by the collision detection algorithm of Von Herzen of O
et al HBZ Lipschitz b ounds need only be computed on a per body basis not on a
p er b o dy pair basis The b ounds for individual typ es are combined to form the b ound for a particular pair
Chapter
Collision Resp onse
The collision resp onse problem is concerned with computing the pair of equal and
opp osite impulses that should b e applied to the colliding b o dies in order to prevent p ene
tration Like contact forces collision impulses are sub ject to frictional constraints as well
as other constraints governing the energy dissipation during collisions Collision resp onse
algorithms fall into two broad categories The rst category makes the constant sliding di
rection assumption the direction of the relative tangential velo citybetween colliding b o dies
at the contact p oint remains constant during a collision This assumption is convenientbe
cause the direction of the frictional force varies with the sliding direction Holding these
directions xed allows one to solve for the collision impulse and p ost collision velo cities of
the b o dies by solving a system of algebraic equations Many v ariations on this theme are
describ ed byBrach Bra In the context of interactive dynamic simulation this approach
is used almost exclusively Bar CS Hah LRK MW NM It is often a fairly
strong approximation particularly for collisions b etween three dimensional b o dies
Just as a collision impulse changes the relative normal velo city between colliding
b o dies it also changes the relative tangential velo cities the direction of the latter is not
constant The second class of collision resp onse metho ds use di erential equations that
describ e the collision pro cess They account for the change in sliding direction and the
p ossibility of transientsticking that can o ccur during impact These approaches date backto
Routh Rou and have b een studied bymany others BK Kel MC a Str WM
accurate collision mo del is required for physically valid For impulse based simulation an
results
This chapter describ es a suitable collision resolution algorithm for an impulse
based simulator which has been implemented in Impulse The main contribution is a
computational mo del for the di erential analysis of the collision pro cess this approach is
more accurate than the algebraic one Building on work of Routh and Keller new equations
are derived that are suitable for numeric integration The algorithm presented here also
incorp orates Strong s improved mo del for restitution To accomplish this the collision
integration is divided into three phases and two di erent parameterizations are used This
chapter also presents new results concerning the determinacy of collision dynamics under
the Coulomb friction mo del Recently Bhatt and Ko echling ha ve explored these issues
BK BK b and in BK a they rep ort some of the same results The geometric
pro ofs given in this chapter di er signi cantly from theirs which are based on the ro ots of
p olynomials in our opinion the geometric pro ofs are simpler and more intuitive Finally
this chapter discusses some approaches for mo deling static friction in an impulse based
context
Assumptions of collision resp onse mo del
The physical phenomena that o ccur between bodies in contact result from com
plex interactions at the atomic level Finite element metho ds are a rst step toward a
to be used in the context of tractable mo del but are still to o computationally exp ensive
interactive simulation A much greater approximation but one which signi cantly reduces
computational costs is the rigid b o dy mo del This mo del is reasonable for many everyday
physical systems it is most tenuous during collisions b etween b o dies whichinvolve surface
deformations Still manyphysical phenomena can b e captured with a rigid b o dy mo del it
has wide application in engineering and it is the mo del used by Impulse
Assumption Rigid b o dies Al l physical objects in the simulation are perfectly rigid
This mo del has several implications When two ob jects collide the duration of the collision
is in nitesimal and the forces needed to prevent p enetration are impulsive instantaneously
changing the velo cities of the colliding b o dies Since velo cities are nite the p ositions of
t during the collision Non impulsive forces like gravity the colliding b o dies are constan
have no e ect over in nitesimal intervals and may thus be ignored in collision analysis
The rigid b o dy assumption is made frequently in collision analysis
When two real b o dies collide there is a p erio d of deformation during which elastic
energy is stored in the b o dies followed by a p erio d of restitution during which some of this
energy is returned as kinetic energy as the b o dies reb ound o each other Figure The
1 1 1
2 2 2 compression restitution
uz
0 t tmc tf
p z
fz
0 t tmc tf Wz 0 t
tmc tf
Figure The normal velocity force impulse and work during the compression and
restitution phases of a col lision The z subscript denotes the normal direction at the
col lision point
rigid body assumption is usually coupled with a mo del to approximate this pro cess The
simplest mo del and the one used in most elementary physics texts is Newton s impact law
u t eu
z z
f
Here u is the normal comp onent of the relative velo city between the b o dies u t
z z
means the b o dies are moving toward another at time t and u t means they are
z
separating The collision starts at time zero and ends at time t The p oint of maximum
f
compression t is the p oint at which the normal velo city changes sign and the phase
mc
changes from compression to restitution The constant e is called the coe cient of resti
tution and make take values between and dep ending on material prop erties of the
colliding b o dies Newton s law is linear and lends itself nicely to an algebraic solution for
the p ost collision velo cities
Poisson s hyp othesis an alternative restitution law uses e as a ratio between
impulses rather than velo cities
p t p t ep t
z z mc z mc
f
Here p is the normal impulse the time integral of the normal force f delivered from
z z
one body to another Poisson s hyp othesis states that the normal comp onent of impulse
delivered during the restitution phase is e times the normal comp onent of impulse delivered
during the compression phase This is equivalent to Newton s law for frictionless collisions
Keller uses Poisson s hyp othesis to derive equations of collision with friction Kel
Both Newton s impact lawandPoisson s h yp othesis can cause the total energy of
the colliding b o dies to increase during a collision when friction is present To correct this
defect Stronge prop oses a new de nition of the co e cient of restitution as a ratio of the
work done by the normal comp onents of impulse Str
Assumption Stronge s hyp othesis Let W t be the work done by the normal com
z
ponents of the equal and opposite col lision impulses during a col lision Then
W t W t e W t
z z mc z mc
f
The p ositive work done during the restitution phase is e times the negative work done
during compression Unlikethe other mo dels Stronge s mo del guarantees that the e ects
of the normal forces like the tangential frictional forces are always dissipative they can
not add energy to the system This agrees with the description of the physical pro cess of
collision Stronge s mo del is employed by the simulator Impulse
The tangential comp onents of fore and impulse that develop during a collision are
governed by a friction law A very common friction formulation and the one employed in
Impulse is the Coulomb friction law
Assumption Coulomb friction law At some instant during a col lision between bod
ies and letu be the contact point velocity of bo dy relative to the contact point velocity
of body Let u be the tangential component of u and let u be a unit vector in the di
t t
rection of u Let f and f be the normal and tangential frictional components of force
t z t
exerted by body on body respectively Then
u f kf ku
t t n t
u kf k kf k
t t n
where is the coe cient of friction
In order the equations ab ove corresp ond to dynamic and static friction While the b o dies
are sliding relative to one another the frictional force is exactly opp osed to the direction
of sliding If the relative tangential velo city is zero all that is known is that the total force
lies in a friction cone
Computing collision impulses
Collision pro cessing is initiated when two criteria are met
The collision detection system rep orts that the distance between two b o dies is less
than the collision epsilon
The velo cities of these b o dies are such that the distance b etween the closest p oints is
decreasing
The job of the collision resolution system is to compute a pair of equal and opp osite collision
impulses to be applied to the colliding b o dies at the contact points These impulses must
prevent p enetration of the b o dies and also satisfy the physical laws discussed in the previous
section This section describ es a metho d for computing these impulses The situation is as
depicted in Figure The collision frame F has its origin at the collision p oint and its
coll
z axis aligned with the mutual surface normal at this p oint pointing from b o dy toward
body body For non smo oth b o dies such as p olyhedra the surface normal is de ned to
lie along the line b etween the closest p oints For b o dy i m is the mass of the b o dy I is
i i
the mass matrix describ ed in App endix A v is the linear velo cityof the enter of
i
is the angular velo city of the b o dy u is the absolute velo city of the contact p oint mass
i i
on the b o dy and r is the o set vector from the b o dy s center of mass to the contact p oint
i
Also p is the collision impulse delivered from body to body and p is the reaction
impulse delivered from body to body Throughout this section it is assumed that all
vectors and tensors are resolved in the collision frame
body 1
v1 m 1 , I1 ω r1 1
Fcoll z y −p +p x
u 1
body 2
Figure A col lision between two bodies indicating some of the quantities used during
col lision analysis The col lision frame F is chosen so that the z axis is aligned with the
col l
surface normals at the col lision point
The equations of collision
Basic Newtonian physics dictate how collision velo cities and impulses evolve Dur
ing a collision all non impulsive forces acting on body are negligible only the collision
force f t needs to b e considered which also induces a torque of r f t From the Newton
Euler equations App endix A
f t ma t
r f t I t t I t
where a and are the linear and angular accelerations of body The last term in
the second equation comprises inertial forces which are also negligible during collision it
may be dropp ed Also m I and r are constants With these facts in mind the ab ove
equations can b e integrated over time to give
p t m v t
r p t I t
where p is the collision impulse
Z
t
p t f d
These equations relate the impulse delivered up to some time t to b o dy s velo c
ity changes up to that point Using time as a parameter is valid for the ab o ve derivation
in which a collision is a event of nite duration during which large but b ounded forces
act For the rigid body mo del however a collision is really the limit of this pro cess as
t approaches zero and the collision forces become in nite Di culties in analysis arise
f
b ecause the collision o ccurs over a zero time interval and the velo cities b ecome discontin
uous functions of time These problems are remedied bycho osing a new parameterization
for the collision A collision parameter is chosen which monotonically increases during
the course of the collision All velo cities of the colliding b o dies as well as the accumulated
impulse are expressed as functions of One natural choice p the normal comp onent
z
of p Clearly p b egins at zero and monotonically increases during a collision it is the
z
integral of the normal comp onent of force exerted by body on body and this force
comp onent is always p ositive in F Figure because the b o dies can only push on
coll
each other Furthermore velo cities will be shown to be continuous functions of this pa
rameter during a collision Other collision parameters will b e used as well when generality
used to is required denotes an arbitrary collision parameter A prime is sometimes
denote di erentiation with resp ect to the collision parameter
Rewriting and and changing to the new collision parameter
v p
m
I r p t
The contact pointvelo city u is given by
u v r
Combining the ab ove three equations
u p I r p t r
m
p r I r
m
where r is the cross pro duct matrix corresp onding to r App endix A and is the
identity matrix The same derivation can be p erformed for body the only di erence is
that p is replaced with the reaction p The result is
u p r I r
m
Letting u denote the relative contact p ointvelo city that is u u
r r r I r I p u
m m
z
K
or more succinctly
u Kp
The matrix K is called the collision matrix it plays a central role in computing the
impulse to resolve the collision
Theorem prop erties of K For a given col lision the col lision matrix K de ned by
is constant nonsingular symmetric and positive de nite
Proof Constancy of K is evident from it dep ends only on the masses mass distribu
tions and contact p oint lo cations of the colliding b o dies all of which are constant during a
collision For the other claims note that K is the sum of three matrices a p ositive scalar
multiple of the identity matrix which is clearly symmetric p ositive de nite A r I r
and A r I r The mass matrices I are also symmetric p ositive de nite see Chap
i
T
ter therefore so are their inverses Since r is skew symmetric it follows that A A
i i
i
so A is symmetric For an arbitrary vector x
i
T
T T T
x A x x r I r x r x I r x w I w
i i i i i
i i i
where w r v Since I is p ositive de nite the right hand side is non negative and
i
i
therefore A is p ositive semi de nite Finally since K is the sum of three symmetric ma
i
trices of whichone is p ositive de nite and two are p ositive semi de nite K is symmetric
p ositive de nite which also implies nonsingularity
The main result of this section is emb o died in the following theorem
Theorem equations of collision During a col lision parameterized by the relative
contact point velocity and the col lision impulse evolve according to
d d
u K p
d d
d d
p K u
d d
Proof From u u u Kp Since K is constant di erentiating
with resp ect to yields the rst equation of the theorem Since K is also nonsingular this
equation maybeinverted yielding the second equation
Sliding mo de
Bo dies and are sliding relative to one another when the tangential comp onent
of relativecontact velo city u is nonzero In this case it is p ossible to derive a rst order
di erential equation for u usingp as the collision parameter
z
De nition Let be the relative sliding direction at some point during a col lision that
is Tan u u where the arctangent returns values in four quadrants De ne the
y x
sliding vector as
q
u u u
cos
x
x y
q
u u sin u
y
x y
where is the coe cient of friction
The signi cance of the sliding vector is seen in the next lemma
Lemma Let p be the relative sliding direction during a col lision While the bodies
z
are sliding relative to each other
d
p p p
z z
dp z
Proof By the chain rule
d d dt dt
p p p p p f p p
z z z z z
dp dt dp dp
z z z
where f p is the collision force the time derivative of impulse By the Coulomb friction
z
law
cos p f p
z z z
f p
sin p f p
z
z z z
f p
z z
under sliding conditions Combining these equations and noting that dt dp p f p
z z z z
proves the lemma
Combining Theorem and Lemma pro duces
sliding ODE w r t p While two col liding bodies are in sliding contact Theorem
z
the derivatives of u with respect to p are given by
z
q
u u u
x
x y
q
d
u K K
u u u
y
x y
dp
z
This system of three nonlinear rst order ODEs describ e how the relativevelo city
evolves during a collision By numerically integrating these equations using p as the inde
z
p endent variable u can be tracked over the course of the collision as long as the sliding
velo city is nonzero Figure shows solution tra jectories of the ODE system pro jected into
the u u plane For this example was chosen as and
x y
K
initial values for u and u at the b eginning The diamonds in the gure mark di erent
x y
of the collision The ow lines showhow the relative tangential velo cityevolves during the
collision For a given collision only one of the ow lines is followed
One counterintuitive asp ect of Figure is that manyofthe ow lines diverge from
the origin indicating that the magnitude of the sliding velo city increases during collision
The explicit dep endence of u and on p is eliminated for clarity z
Evolution of Tangential Velocity
10
8
6
4
2
0 y velocity -2
-4
-6
-8
-10
-10 -8 -6 -4 -2 0 2 4 6 8 10
x velocity
Figure Solution trajectories of the ODE system of Theorem projected into the u u
x y
plane
This b ehavior seems contrary to the laws of friction which stipulate frictional force opp ose
sliding The b ehavior is understo o d from Figure In this case the tangential velo city
is zero at the b eginning of the collision but nonzero at the end of the collision Although
the frictional force opp oses the motion it can not overcome the e ects of the normal force
which induces an angular velo city that causes the ro d to slip
Sticking mo de
When the tangential comp onent of relativecontact velo city u vanishes the b o dies
are sticking and the analysis of the previous section is not valid Under sticking
conditions the Coulomb friction mo del do es not completely determine the frictional force
but only requires that its magnitude not exceed times the magnitude of the normal force
a ω ω = 0 α v v u u t t = 0 ft
fn
before collision during collision after collision
Figure A situation where the tangential relative contact velocity of the rod u starts at
t
zero and increases during the course of the col lision even though the frictional forceresists
this change in velocity
When sticking occurs we assume as do es Routh that if the frictional forces are strong
enough to maintain the sticking condition they will do so Rou If they are not strong
enough sliding will resume The following theorem provides the means to test which of
these b ehaviors o ccurs
Theorem Stable sticking For a particular col lision with col lision matrix K the fric
tional col lision forces can maintain sticking if and only if
K K K
where K denotes the i j element of the inverse col lision matrix K
ij
Proof Consider the collision force f p at point p in the collision From the Coulomb
z z
friction law if there is no sliding the comp onents of f must satisfy
f p f p f p
x z y z z z
ulated impulse or expressing force as the time derivativeofaccum
dp dp dp
y z x
p p p
z z z
dt dt dt
Multiplying the ab ove inequalityby dt dp
z
dp dp dp
y z x
p p p
z z z
dp dp dp
z z z
If the sticking condition is stable the derivatives of relative tangential velo city vanish
u p u p In this case Theorem implies
z z
x y
K
d
p p K u p
K
z z
z
dp
z
u p K
z
z
Combining the previous two equations proves the theorem
Collision integration
The collision resp onse calculation is based on tracking the relative contact velo c
each p oint ity u during a collision Figure depicts relative contact velo city space
u u u in this space corresp onds to a particular relativecontact p ointvelo citybetween
x y z
body and body The gure also depicts the tra jectories u for three di erent col
lisions For all of these collisions u at the b eginning of the tra jectory since that the
z
b o dies are moving toward each other and u at the end of the tra jectory since the b o d
z
ies are separating The plane u is the plane of maximum compression as u crosses
z
this plane the collision phase changes from compression to restitution c f Figure
is evolved according to sliding equations such As long u and u are not b oth zero u
x y
as those given by Theorem Path A is a case for which sliding o ccurs throughout the
collision If while tracking u u and u both vanish then u lies on the u axis also
x y z
called the line of sticking In this case Theorem is not applicable instead the test of
Theorem is p erformed to determine if the sticking condition is stable If it is u will
remain on the line of sticking for the duration of the collision path C exhibits this b ehavior
If the sticking is not stable meaning the frictional forces are not strong enough to prevent
tangential slip the p oint u leaves the line of sticking again moving according to sliding
equations Path B exhibits this b ehavior The question of exactly how u leaves the line
of sticking has not b een discussed this is a deep question that will b e treated in Section
If the paths of Figure are pro jected into the u u plane one obtains ow lines like the
x y
ones in Figure
This pro cess of tracking the p oint u during collision is called col lision inte
gration Given u the initial relative contact point velo city and u the value of
f
uz
B medium friction
uy line of sticking plane of max. ux compression
A C low friction
high friction
Figure Trajectories through relative contact velocity space for three di erent col lisions
relativecontact p ointvelo city at the end of the collision the collision impulse may b e easily
determined Inverting
p K u K u u
f
Since p is the total impulse delivered during the collision computing p is the
f f
ultimate goal of the collision resolution system The collision integration pro cess describ ed
ab ove is somewhat simpli ed and the purp ose of this section is to describ e the pro cess in
more detail Pro cedures to handle stable and transient sticking are develop ed as well as
the termination conditions that indicate when the integration is to stop The latter are
based on Stronge s hyp othesis for restitution
Work done by collision forces
Consider a force f applied to a particle that moves along a continuous path x t
through space The total work p erformed by this force on the particle over the time interval
t isgiven by
f
Z
t
f
W f t x t dt
t
If x t is a line segment and f t a constant force directed along this segment this boils
collision f is a down to the familiar work equals forces times distance rule During a
collision force applied to a b o dy at the contact point and x is the absolute velo city of the
contact point for example u on body The formulation for work given ab ove is not
usable during a rigid body collision b ecause the collision forces are in nite and the time
interval is in nitesimal This can b e remedied with the familiar tactic of re parameterizing
the collision with a di erentvariable With this change of variables
Z
f
dt
W f u d
d
Replacing f with d dt p applying the chain rule and rearranging gives
Z Z
f f
d
W u p d u p d
d
This expresses the work done by the collision impulse on b o dy Similarly the work done
by the reaction impulse on b o dy is given by
Z
f
W u p d
Combining the ab ove two equations and recalling that u u u the total work done
by the collision impulse on b oth b o dies is
Z
f
u p d W
The di erential form of this equation will b e useful
Theorem The derivative of work done by the col lision impulse with respect to the col
lision parameter is given by
dW
u p u K u d
Since K is a known constant and the path u completely determines u
Theorem implies that knowing the complete path u is enough to sp ecify the total
work done on the colliding b o dies In fact knowing even less is su cient
Theorem path indep endence of W Suppose the relative contact velocity u pro
ceeds from u to u during a col lision over some arbitrary path let u
f f
u u The total work done by the col lision forces is independent of the path taken
f
and is given by
T
u u K u W
f f
Proof From Theorem
h i
dW
T T T
u K u u K u u K u
d
Transp osing the second term in brackets a scalar and observing the symmetry of K
h i
dW
T T
u K u u K u
d
Since K is a constant
h i
dW d
T
u K u
d d
By the Fundamental Theorem of Calculus the total work done over the interval is
f
h i
f
T
Evaluating this expression u K u j
T T
W u K u u K u
f f
T T T T
u K u u K u u K u u K u
f f f f
T T
u K u u K u
f
Finally transp osing the second scalar term of the nal equation yields the desired result
An imp ortant corollary concerns a sp ecial energy preserving collision
Corollary If during a col lision the r elative contact velocity is exactly reversed that is
u u then the col lision forces do no net work on the col liding bodies and the
f
energy of the system is therefore unchanged by the col lision
Stronge s hyp othesis is used to determine when the collision integration should
stop This hyp othesis is not a statement ab out the total work done by collision forces but
ab out the normal work that is the work done by the normal comp onent of the collision
force The normal work is denoted W
z
Theorem The derivative of the normal work with respect to the col lision parameter
is given by
dW
z
u p
z
z
d
Proof From Theorem and the chain rule
dt dW
u p u f
d d
The di erential work done only by the normal z comp onent of the force is therefore
dW dt
u f u p
z z z
z
d d
Integrating collisions using di erent parameters
To this p oint the only quantitymentioned as a p ossible collision parameter is p
z
the normal comp onent of impulse This is a natural choice since p is clearly monoton
z
ically increasing during the collision There are however other choices for the collision
parameter and most of the results derived thus far apply equally well for other valid pa
rameterizations For a parameter to b e valid it must b e monotonically increasing during
the collision and the quantities b eing integrated suchasu must b e continuous These
and bythechain rule implies that prop erties have already b een shown for the parameter p
z
any other parameter with the prop erty that d dp is p ositive and nite is also valid As
z
an example consider the parameter u the normal comp onent of relative velo city From
z
Theorem
d
u K K K
z
dp
z
As long as the right hand side is p ositive u is a valid parameter As a second example
z
consider the parameter W the normal work done during the collision Applying Theo
z
This is true for almost all collisions indep endent of the sliding angle Section deals with the case when it is not
rem with p
z
d d
W u p u
z z z z
dp dp
z z
Therefore W is a valid parameter as long as u this is the case for instance throughout
z z
the restitution phase of the collision
Since the equations of collision have already b een develop ed using p as a param
z
eter why b other with other parameterizations The reason has to do with how collision
integrations are terminated Stronge s hyp othesis Assumption dictates the pro cess by
which the collision integration should pro ceed
Integrate the compression phase of the integration which terminates at the p ointof
maximum compression when u Record the normal work done during this phase
z
Integrate the restitution phase of the integration which terminates when the nor
e times the normal work done during mal work done during restitution reaches
compression
Numerical integrators integrate over an interval a b and the endp oints must b e sp eci ed
when the integrator is called The values of p at the end of the compression phase and at
z
the end of the restitution phase are not known a priori hence p is not a useful parameter
z
On the other hand the value of u is known at the end of the compression phase it is
z
zero Thus u is a suitable parameter for the compression integration At the end of the
z
compression phase the normal work done during compression is known and from this the
normal total work which must be done over the entire collision can be computed The
work W is therefore a suitable parameter for the restitution integration The next sections
z
reformulates the equations of collision for these new parameters and also showhow sticking
is handled It will b e useful to partition the collision matrix K into rowvectors
k
x
K
k
y
k
z
The expressions K and K resp ectively denote the i j th entries of K and K
ij
ij
Sliding mo de under u and W parameterizations
z z
The equations of sliding must b e reformulated for the new parameterizations The
compression integration is p erformed with u as a parameter During this integration u
z x
and u are considered functions of u Since the value of W at the end of compression
y z z
is needed this quantity must also be tracked as a function of u Of course u need not
z z
be explicitly integrated since it is the indep endent variable From the chain rule and
Theorem
du du dp k
x x z x
du du dp k
z z z z
The derivative du du is derived analogously Lastly from Theorem with u
y z z
dp u dW
z z x
u
z
du du k
z z z
Summarizing the results
u k
x x
d
u k
y y
du k
z z
W u
z z
The restitution integration is p erformed with W as a parameter The three com
z
ponents of u must be integrated with resp ect to this variable From Theorem and the
chain rule
d d d dp
z
u K p K p
dW dW dp dW
z z z z
From dp dW u as a result
z z
z
k
x
d
u K
k
y
dW u u
z z z
k
z
The ab ove equation is valid for u but it is not well b ehaved near u
z z
Toavoid using this equation near this singularity u is retained as the integration variable
z
slightly beyond the compression phase into the early restitution phase After u is safely
z
p ositive the restitution integration can complete using W as a parameter Care must be
z
en that the small extension integration with resp ect to u do es not cause W to exceed tak
z z
its nal value The following lemma computes a safe upp er limit for u for the extension
z
integration The complete life cycle of a collision is depicted in Figure
Lemma Let W denote the normal work which to be done during the entire restitution
r
phase Suppose integration with respect to u is extended into the restitution phase until
z
u b where
z
s
q
K W K K b
r
Then the normal work wil l not exceed W during this extension integration
r
Proof Supp ose dW du Cu for some p ositive constant C Separating the variables
z z z
and integrating as u varies over b yields
z
W Cb
z
To insure that W do es not exceed W itsu cestocho ose
z r
q
b W C
r
All that is needed is C From the last of Equations
q
dW u
z z
u K K K
z
du k
z z
The co e cientofu on the right hand side is a valid choice for C the lemma follows z
start of max. compression end of collision u zz < 0 u = 0 u z > 0 collision
compression phase restitution phase
latch W z extension integration
compression integration restitution integration
integrate u xy , u , W z integrate u xy , u , u z
w.r.t. uz w.r.t. Wz
Figure The physical life cycle of a col lision and the corresponding col lision integration
The compression and extension integrations are performed using and the restitution
integration is performed using The value of W latched at the point of maximum
z
compression is used to determine the upper limit of W that ends the restitution integration
z
according to Stronge s hypothesis The value of u that terminates the extension integration
z
is given by Lemma
Handling sticking during collision integration
Wehave not yet discussed what happ ens if sticking o ccurs at some p oint during the
compression extension or restitution integrations Practically sticking is detected when
the magnitude of the tangential comp onent of relative contact velo city falls b elow some
tolerance At this p oint the collision resolution algorithm determines if the sticking is
s
stable or not Theorem and pro ceeds accordingly In either case the normal sliding
integration halts and the evolution of the variables for the remainder of the collision may
b e computed by solving simple algebraic equations The solutions of three rst order ODEs
will be useful the claims of the following observation are veri ed by separating variables
and integrating BD
Observation solutions of rst order ODEs Let y be a di erentiable function of an
independent variable that varies over the a b and C bea constant Then
dy
C y b y a C b a
d
dy
C y b y a C b a
d
q
dy C
y a C b a y b
d y
First consider the case of stable sticking There are two varieties dep ending on
whether the sticking was detected during integration with resp ect to u or W integration
z z
Supp ose a u integration was in progress Let a b e the currentvalue of u when sticking was
z z
detected and let b b e the upp er limit of u for the currentintegration b if sticking was
z
detected during the compression integration or a p ositivenumber if sticking was detected
during the extension integration The goal is to determine u b u b and W b Stable
x y z
b u b By Theorem sticking implies u
y x
dW dp
z z
u
z
du du
z z
T
Since du du under stable sticking Theorem implies dp du K Thus
z z z
dW
z
K u
z
du
z
and applying the second rule of Observation gives
W b W a K b a
z z
Next supp ose that stable sticking prevails at the b eginning of the restitution in
tegration or that it occurs during it Let a b e the current value of W and b be the nal
z
value The goal is to compute u b and again u b u b b ecause sticking is stable
x y
Rewriting as
K du
z
dW u
z z
and applying the third rule of Observation gives
s
u b u a b a
z z
K
Instable sticking o ccurs if the contact force required to maintain the sticking lies
outside the friction cone in this case sliding must resume How this happ ens is not obvious
but the tra jectory plots in Figure give a clue Sticking o ccurs for this collision if the
tangential velo city point reaches the origin For the K and that generate that plot
sticking is instable and the tangential velo city point must leave the origin In the plot
there are tworays along which the sliding direction is constant Referenced to the p ositive
there is one ray at and one ray at If the tangential velo city p ointison x axis
either of these rays the direction of sliding remains constant and the p ointnever leaves the
ray The ray is called a converging ray since velo citypointmoves along it toward the
origin and the ray is a diverging ray since the velo city point moves along it away
from the origin If the velo city point is not on one of these rays the direction of sliding
changing The tangential velo city p oint can only leave the origin along a is continuously
diverging ray Leaving in any other direction would mean crossing ow lines or owing in
the wrong direction along a converging ray For any K and the sliding plot resembles
the one in Figure although the number of converging and diverging rays may vary
implies that After instable sticking sliding can only resume along a diverging ray which
the direction of sliding is constant after instable sticking Fortunately resuming sliding is
a deterministic pro cess When sticking is instable there is exactly one diverging ray along
which sliding may resume The next section proves this claim and gives an algorithm for
computing the direction of the ray Taking the claim on faith for now call the direction
of the unique diverging ray whichcan take on anyvalue mo dulo To resume sliding
to lie on the diverging after instable sticking one option is to set the values of u and u
y x
ray very close to the origin
u cos
x
u sin
y
for some very small Then the normal sliding mo de integration could b e resumed as the
tangential velo city ows away from the origin A b etter approach is to use the knowl
edge that the sliding direction is constant after instable sticking in order to solve for the
subsequent motion of the velo city p oint algebraically
First consider the case of instable sticking o ccurring during integration with resp ect
to u Let a b e the currentvalue of u andb the upp er limit of the integration The goal
z z
is to compute u B u b and W b From the chain rule and Theorem
x y z
du du dp k
x x z x
du du dp k
z z z z
Since after instable sticking the right hand side is constant Applying the rst rule
of Observation
k
x
u b u a b a
x x
k
z
The value u b is derived analogously
y
k
y
u b u a b a
y y
k
z
From Theorems and
dW dp u u
z z z z
u
z
du du du dp k
z z z z z
Applying the second rule of Observation gives
b a
W b W a
z z
k
z
Finally consider instable sticking o ccurring b efore or during the restitution inte
gration Let a b e the currentvalue of W andb b e the upp er limit of the integration The
z
goal is to compute u b Inverting
du k
z z
dW u
z z
Applying the third rule of Observation gives
q
u a k b a u b
z z z
Once u b is known u b and u b can again b e computed using the same technique as
z x y
b efore
k
x
u b u a u b u a
x x z z
k
z
k
y
u b u a u b u a
z z y y
k z
Sticking stability and rays of constant sliding
The previous section intro duced the problem of deciding how to resume sliding
when instable sticking is detected during a collision integration the tangential velo city
point leaves the origin along a diverging ray of constant sliding direction For the plot of
Figure this is p ossible b ecause there is exactly one such ray Consider the tangential
velo city ow in Figure Here there are no diverging rays and so it is imp ossible for the
Evolution of Tangential Velocity
10
8
6
4
2
0 y velocity -2
-4
-6
-8
-10
-10 -8 -6 -4 -2 0 2 4 6 8 10
x velocity
Figure A tangential velocity ow plot with two converging rays and no diverging rays
tangential velo citypoint to escap e from the origin For the K and used to generate this
plot sticking is stable and so the lackofdiverging rays is of no consequence But can this
situation arise in an instable sticking case Next consider the velo city ow in Figure
Here there would app ear to b e indeterminacy after instable sticking b ecause there are two
diverging rays The K used to generate this plot is symmetric but not p ositive de nite
Evolution of Tangential Velocity
10
8
6
4
2
0 y velocity -2
-4
-6
-8
-10
-10 -8 -6 -4 -2 0 2 4 6 8 10
x velocity
Figure A tangential velocity ow plot with two converging rays and two diverging rays
hence it is not even a valid choice But it is imp ortant to verify that such indeterminacy
can never o ccur with valid K This section addresses these questions
The issues addressed in this section have also been extensively studied by Bhatt
and Ko echling BK BK b BK a Their approach is to parameterize the space of all
three dimensional rigid b o dy collisions into regions that are qualitatively similar such as the
set of collisions where there are no diverging rays From their partitioning the qualitative
character of the collision can b e determined a priori from the initial conditions One of the
main results of this section is a pro of that the behavior is determinate in cases of instable
proved this long standing sticking Theorem Bhatt and Ko echling have also recently
conjecture using a purely algebraic metho d the pro of in this section is more geometric
Wang and Mason also partitioning collision space based on qualitative b ehavior but for the
caseoftwo dimensional collisions WM
The u ellipse
For the remainder of this section the entries of the collision matrix are given by
a f e
K
f b d
e d c
To study the evolution of sliding velo city during collision the most convenient parameteri
zation to use is the one based on p and sp eci ed by Theorem Using the ab ove de nition
z
of K the theorem implies
u a f cos e
x
u f b sin d
y
z z
s
M
where M and s are the designated sub matrices of K To simplify notation the explicit
dep endence of u and on p is dropp ed in the rest of the analysis primes denote derivatives
z
with resp ect to p Since only the tangential velo city is b eing studied the the third normal
z
T T
comp onentofrelativevelo city is dropp ed so that u u u andu u u Since
x y
x y
M is a ma jor sub matrix of the p ositive de nite matrix K it is also positive de nite
Therefore
T
M R M R
d
diagonal matrix with p ositiveentries along the diagonal and R is the where M is a
d
orthogonal two dimensional rotation matrix for some angle
cos sin
R
sin cos
See HJ for justi cation of these assertions concerning p ositive de nite matrices Com
bining results
u cos
x
T
R M R s
d
u sin
y
As the sliding direction varies over u traces out an ellipse in a counter
clo ckwise direction It is not the case that or u actually vary in this manner during a
collision but the analysis to come is based on howthepoint u behaves as a function of
Calling the diagonal entries of M a and b the series of transformations indicated by the
d d d
ab ove equation is shown in Figure
(C) (A) (B) µ bd
θ−α µ θ µ µ ad
θ − cosθ − cos − cosθ µ T µ θ µ T MdRα − sin θ − sin Rα − sin θ
(D) (E)
α d
θ e T − cos µRα Rα θ Md − sin θ µ T − cos
RαMdRα − sin θ + s
T
Figure A sequence of transformations applied to the point cos sin in order
T
to produce the velocity derivatives u u The latter traceoutacounter clockwise el lipse
x y
as varies over
Lemma The u el lipse shown in Figure E circles the origin if and only if the
entries of the col lision matrix K satisfy
b f e a f d a b fde jMj
Proof Establish a new frame G lo cated at the center of the u ellipse and rotated an angle
relative to the original frame F as shown in Figure The co ordinates of the origin
of frame F expressed in frame G are given by
o
x
T
o R s R s
o
y
The ellipse encircles this p oint if and only if
o o
y x
a b
G µa’ µb’ α d
F
e
Figure Testing whether the u el lipse circles the origin Lemma
Since the diagonal elements of M are a and b this criterion can b e written
d
T T
kM o k M o M o o M o
d d d d
T
From M R M R Again rephrasing the criterion
d
T
T T T
R s R M R R s
whichconveniently reduces to
T
s M s
M is given by
b f
M
jM j
f a
and so the criterion b ecomes
b f
T
s jM j s
f a
T
Straightforward computation with s e d gives the lemma
The criterion of this lemma seems cryptic but it can b e expressed much more simply
Theorem The u el lipse circles the origin if and only if sticking is stable
Proof By the direct inversion formula
K
jKj
f e a e a f
b d f d f b
where the dots mark entries which are irrelevant Notice that the determinant in the lower
right corner of the matrix is jMj In terms of the entries of K the stable sticking criterion
given by Theorem can b e written
f e a e
B C B C
B C B C
B C B C
b d f d
jMj
B C B C
B C B C
B C B C
jKj jKj jKj
B C B C
A A
Multiplying through by jKj and expanding the determinants on the left hand side leads to
the same criterion given in Lemma
Directions of constant sliding
Let Tan y x denote the four quadrant arctangent function which returns the
unique angle in such that y r sin and x r cos for some p ositive constant r
The sliding direction is given by
Tan u u
y x
If u is in the same direction as u or exactly opp osed to it Then one of the following holds
Tan u u
y x
mo d Tan u u
y x
If either of these relations hold then the direction of u remains constant during collision
integration Values of which satisfy these relations de ne the lo cation of diverging and
T
converging rays resp ectively Let u cos sin be the unit vector aligned with u
Then one of the ab ove relations is satis ed if and only if u u The particular relation
which holds can be determined by the sign of u u if it is p ositive the rst one holds
if it is negativ e the second one holds These results along with yield the following
theorem
Theorem lo cation of rays Rays of constant sliding direction are located at angles
satisfying the equation
u u a b cos sin f sin cos d cos e sin
At such values of if
f cos sin d sin e cos u u a cos b sin
is positive the ray is diverging otherwise it is converging
Corollary There can be no more than four rays of constant sliding direction
Proof The cross pro duct criterion of Theorem can b e converted to a p olynomial equa
tion by making a standard change of variables t tan cos t t
ving at most sin t t This leads to a fourth order p olynomial equation in t ha
four ro ots
The ab ove corollary also gives an e cient algorithm for computing the directions
of the converging and diverging rays The ro ots of a fourth order p olynomial are computed
algebraically These give the directions of the rays which are then easily classi ed as
converging or diverging based on the test of Theorem The lighthouse analogy of
Figure is useful in understanding the rays of constant sliding direction A lighthouse
at the origin rotates counter clo ckwise sweeping a white beam across the plane It also
emits a red beam in the opp osite direction of the white b eam The lo cation white beam
in the plane and as the represents the sliding direction The u ellipsoid is also lo cated
varies the point u moves around this ellipsoid in a counter clo ckwise direction Let
b e the angle this p ointmakes with the p ositive x axis If at a particular value the
u’ ellipse
u’( θ )
white beam β(θ) θ lighthouse
red beam
Figure The lighthouse analogy for rays of constant sliding direction
white b eam directly illuminates the p oint u there is a diverging rayat If at a particular
value the red b eam directly illuminates the p oint u then there is a converging ray
Lemma Consider the point u moving counter clockwise around the el lipse shown in
Figure as goes from to Then implies d d In other words
am can not be increasing when the point is il luminated by the white be
Proof Tan u u and so
y x
u du d u du d u
d d
y y x x y
d d u
u u
x
y x
u u
y x
Therefore
du
d du
y
x
sgn sgn u u
x y
d d d
Di erentiating gives
a sin f cos u
d
x
d
u f sin b cos
y
and so
h i
d
sgn sgn f sin b cos u a sin f cos u
x y
d
If then u r cos and u r sin for some p ositive constant r Substituting
x y
these values into the ab ove equation dividing the argument of the right hand sgn by the
p ositive quantity r yields
d
sgn b cos a sin f cos sin sgn
z
d
D
p
ab jf j Thus M is p ositive de nite so its determinant ab f is p ositive and
p
D b cos a sin ab j cos jjsin j
p
p
b j cos j a j sin j
This implies d d
Theorem If sticking is stable there arenodiverging rays of constant sliding direction
Proof If sticking is stable the u ellipse circles the origin lighthouse by Theorem For
this case d d must b e p ositive during the entire counter clo ckwise circuit of the p oint u
around the ellipse and therefore byLemma can never equal
Theorem If sticking is instable there is exactly one diverging ray of constant sliding
direction
Proof If sticking is stable the u ellipse do es not circle the origin lighthouse by Theo
rem In this case with Clearly as the white
min max max min
b eam sweeps over the range it must illuminate the p oint u at least once and
min max
so there is at least one value of for which To see that there is not more than
one consider the graph shown in Figure Lemma implies that the graph of can
β(θ)
2π
β(θ) = θ
β max β(θ) < π
β min
β(θ) = θ dβ/ dθ < 0
0 2π θ
Figure Proving therecan be no more than one diverging ray when sticking is instable
never cross the line while is increasing and therefore it can cross only once
More crossings would b e p ossible if the graph of wrapp ed around over an interval
of but this is imp ossible since The conclusion is that there is exactly
max min
one value of for which
Theorem guarantees that Figure is representative of all ow plot when
sticking is stable rays only converge to the origin none diverge Theorem proves that
when sticking is not stable there is exactly one diverging ray as in Figure Thus there
is a well de ned direction along which sliding resumes after instable sticking This result
had been previously conjectured in BK MC a Recently Bhatt and Ko echling have
devised an alternative algebraic pro of of this result BK a The previous two theorems
also prove that ows like the one of Figure never o ccur for valid K which are p ositive de nite
A nal lo ose end to tie up is how to handle situations where du d is nega
z
tive at the b eginning of a collision This paradoxical situation occurs when the collision
force initially accelerates the b o dies into each other instead of away from each other The
phenomenon is well known see Bar for a sp eci c example
Theorem For values of for which the dot product discriminant of Theorem is
positive du dp is also positive
z z
Proof The hyp othesis can b e rephrased as
a cos b sin f cos sin d sin e cos
T
Since K is p ositive de nite K is p ositive Expanding this pro duct yields
a cos b sin f cos sin c e cos d sin
Adding these two inequalities
e cos d sin c
From Theorem du dp is p ositive exactly when the ab ove inequality is satis ed
z z
As the collision is integrated the relative tangential velo citypoin t asymptotically
approaches the rays of constant sliding direction This is evident in the ows of Figures
and If it approaches a diverging ray then in some neighborhood of the ray the
conditions of Theorem will b e met and u will increase for the remainder of the collision
z
If it approaches a converging ray the point will eventually reach the origin If sticking is
not stable the p ointleaves along a diverging ray andu will again increase for the duration
z
T
of the collision If sticking is stable then by de nition u travels in the direction
for the duration of the collision Hence in any case u eventual ly increases and remains
z
for the duration of the collision To handle cases for which u is is initially increasing
z
decreasing and these are extremely rare p is chosen as the initial integration param
z
eter and is used until du dp becomes p ositive At this point the normal compression
z z
integration with resp ect to u can b egin and the rest of the collision integration is handled
z
as b efore
In the vast ma jority of simulations p erformed with Impulse including all of those describ ed in the
Chapter collisions in which u is initially decreasing never o ccur z
Static contact and micro collisions
Under impulse based simulation continuous contact suchasthatbetween a b o ok
and the table is rests on is mo deled through trains of collisions In this case the collision
impulses mo del a constant reaction force which do es no work on the stationary ob jects
The collision resolution metho d describ ed thus far is not completely suitable for mo deling
such static contact forces To understand the problem consider the b o ok on table example
Through collisions the energy of the b o ok steadily decreases as the b o ok sinks further and
further into the collision envelop e of the table The decreased distance b etween the b o dies
causes time of impact estimates to decrease increasing collision checks b etween the b o dies
The simulator would b e brought to a grinding halt as the separation distance and the time
between impacts approached zero
To alleviate this problem microcol lisions are prop osed as a way of mo deling static
contact forces Micro collision impulses are not computed in the standard way For a collision
to be a micro collision candidate the initial relative normal velo city at the contact p oin t
must be small since static contact o ccurs only as ob jects settle onto one another high
relative normal velo cities at the contact point corresp ond to colliding contact In Impulse
this criterion is de ned precisely as
p
u g
z c
where g is the acceleration of gravity The right hand side is the velo city that an ob ject
acquires as it falls from rest through the collision envelop e Since the distance between
ob jects in static contact is approximately this seems a reasonable velo city b ound
c
For micro collisions the co e cient of restitution is arti cially increased based on
the p enetration of the b o dies within the collision envelop e as shown in Figure The
graph shows e the actual co e cient of restitution used in resolving the collision as a
function of the p enetration depth which is in the range The value e
c
def
e is the default value for e based on material prop erties of the ob jects The value
def
e is the maximum value e may assume As shown in the graph e is a second order
max
curve with vanishing derivativeat This is so that e can b e made fairly large for deep
p enetrations while still remaining close to its default value for most collisions whichhave
small p enetrations In Impulse e was chosen as This value is enough to prevent the
max
simulation sp eed from slowing down noticeably when ob jects come to rest on other ob jects
emax
edef coefficient of restitution (e)
0 ε
envelope penetration ( δ )
Figure The coe cient of restitution is arti cial ly increasedtoprevent prolonged deep
penetration into the col lision envelope
The resting ob ject vibrates on its supp orting ob ject with a vibration amplitude on the order
of for most views this amplitude is less than a pixel and so the ob ject app ears at rest
c
A related problem is that of a blo ck on a ramp Figure If the co e cient
ft gt g
fn n g
Figure Standard col lision impulses wil l cause the block to creep down the ramp no
matter what the coe cient of friction is
of friction is high enough the blo ck should be brought to rest on the ramp the static
friction force opp osing the force of gravity Again the contact forces do no work once the
b o dies are stationary At most collisions the resp onse algorithm describ ed thus far will
bring the tangential velo city at the contact pointto zero As the collision ends the blo ck
again b ecomes airb orne following a ballistic tra jectory During this interval gravity is
unchecked and will impart a linear acceleration with a comp onent do wn the ramp Up on
the next collision the downward velo city will again b e halted but the blo ck motion during
the ballistic phase remains uncorrected Since the velo city of the blo ck down the ramp
varies between and some small p ositive the blo ck will creep slowly down the ramp
much as a real blo ckwould if placed on a vibrating ramp
To correct this problem a second typ e of micro collision is employed When the
initial relative normal velo city satis es the unique impulse p that reverses this
r
u velo city is computed This impulse is given by with u
p K u
r
The cost of computing p is much less than that of p erforming a standard collision inte
r
gration Furthermore by Corollary p do es no net work on the colliding b o dies like
r
the static force is mo dels If p lies within the friction cone it is applied otherwise the
r
impulse is computed through collision integration with a b o osted co e cient of restitution
ramp canceling The reversing micro collisions give the blo ck a small kick back up the
the e ects of gravity during the ballistic phase Creep is still present but is very small
Figure
The decision to classify a collision as a micro collision is based only on lo cal in
formation at the contact point This is in keeping with the impulse based approachofnot
explicitly classifying macroscopic state but instead generating correct macroscopic b ehavior
through collisions When one ob ject settles on another the pro cess is a smo oth transition
from a situation in which virtually all collisions are ordinary collisions to a situation in which
virtually all collisions are micro collisions Having to make explicit decisions ab out when an
ob ject is resting on another or whether a contact is of a rolling or sliding nature creates
arti cially sharp transitions in system b ehavior and leads to the consistency problems that
arise in constraint based simulation
The ordinary collision resp onse computations derived in this chapter are on a solid
theoretical framework given the assumptions made Micro collisions are harder to justify
Position of Block Velocity of Block 600 550 550 500 500 450 450 400 400 350 350 300 300 250 250 position (cm)
200 velocity (cm/s) 200 150 150
100 100
50 50
0 0
0 12345 0 1 2 3 4 5
time (s) time (s)
Figure Data from the block on ramp experiment using Impulse A block is launched
down a ramp with an initial sliding velocity of cm s with and g
cm s Theoretical ly the block should experience constant deceleration for seconds
coming to rest after traveling cm The simulator s results agree closely although the
plots reveal slight creep in the position of the block
from a physical standp oint Imp ortant prop erties are preserved when using micro collisions
for example the contact forces on the b o dies still lie within friction cones even when micro
collisions are used But ultimately micro collisions are an ad ho c waytomake impulse based
simulation feasible for mo deling prolonged contact b etween b o dies There are limitations of
what can b e done even using micro collisions The creeping problem although slight may
b e unacceptable for some applications The static contact b etween a b o ok and a table p oses
no problem nor do es a stack of two books By three b o oks the simulation is noticeably
slower and four or more causes it to grind to a halt Too many collision impulses must
b e propagated up and down the stack of b o oks and the weight of the other b o oks on the
deep envelop e p enetration between the b ottom book and the table bottom book causes
This p enetration could be reduced by increasing e but to o high a value adds instabil
def
ity to ordinary situations Furthermore parameter tuning has been eschewed through the
development of Impulse A stack of books on a table is a situation which is much more
naturally and e ciently handled with a constraint based approach
Chapter
Constrained Bo dy Dynamics
Impulse based simulation can not e ciently mo del tightly constrained contact
such as that o ccurring at a hinge joint It is useful however when combined with a classical
metho d for mo deling this constrained contact This combination is called hybrid simula
tion and is discussed in Chapter The present chapter develops the forward dynamics
algorithms for several classes of multibodies collections of rigid b o dies connected byjoints
The algorithms presented here are the original work of Featherstone Fea
Featherstone s algorithm is a generalized co ordinate approach meaning that there
are as many state variables as degrees of freedom in the system The advantage of this
approach is that there are no inv alid state con gurations the constraints are automatically
enforced Maximal co ordinate metho ds also called multiplier metho ds can also b e used to
derive the dynamics of constrained b o dy systems These employ more state variables than
there are degrees of freedom and so constraints must b e continuously enforced Multiplier
metho ds are more naturally applied to more general kinematic structures such as those
with kinematic lo ops They su er however from drift problems due to the co ordinate
redundancy Some di erential algebraic equation DAE solvers such as MEXX LNPE
employ sophisticated metho ds for maintaining constraints at the price of added complexity
A summary of the pros and cons of generalized and maximal co ordinate approaches is in
Bar Clear comparisons of the sp eed and accuracy of the two metho ds have not b een
published the b est choice may b e application dep endent
o reasons The The results of this chapter are not original but are included for tw
rst is tutorial Featherstone s algorithm has been presented by him as well as by several
other dynamicists BJO Lil JR yet the algorithm remains confusing and di cult to
implement for many who are not sp ecialists in the area The sources mentioned ab ovehave
a tendency to describ e the algorithm from a very theoretical viewp oint or to sacri ce detail
and completeness for brevity This chapter derives Featherstone s equations by taking a very
di erent path the equations are derived from the basic principles of rigid b o dy dynamics
taughtinanyphysics course Spatial notation is adopted only after a thorough non spatial
treatment of the underlying quantities Extensions of the basic serial chain algorithm to
handle tree like linkages and oating linkages are explicitly describ ed The second reason
for this chapter is to provide a foundation for multib o dy dynamics which will be used in
Chapter to develop the collision resp onse mo del for hybrid simulation
Section describ es the basic forward dynamics problem for constrained linkages
Section details how the absolute velo cities of all links are determined from the joint
p ositions and velo cities a pro cedure which is omitted in most exp ositions on constrained
dynamics Acceleration propagation equations are also derived Section intro duces
spatial algebra Section derives Featherstone s complete algorithm for grounded serial
linkages from rst principles of rigid b o dy mechanics Sections and extend the basic
algorithm to tree like linkages and oating linkages resp ectively
Constrained forward dynamics
Consider the serial linkage shown in Figure The links are numb ered from
to n where link is attached to a xed base called link and link n is the outermost
i n link i has an inboard joint link The joints are also lab eled from to n For
whichis closer to the base and an outb oard joint whichis closer to the other end of the
linkage link n hasonlyaninb oard joint The inb oard joint of link i is joint i i n
and the outb oard jointoflinki is joint i i n When each joint only allows one
degree of freedom a compact parameterization of the con guration space of the linkage is
T
the vector of joint p ositions q q q The problem to b e solved is
n
Problem Forward dynamics Given The positions q and velocities q of the n joints
of a serial linkage the external forces acting on the linkage and the forces and torques being
applied by the joint actuators Find The resulting accelerations of the joints q
Bara also makes this p ointin Bar
joint i joint i+1
link i
link 1 link n
joint n joint 1 link 0
(base) inboard outboard
Figure The link and joint indexing conventions for serial linkages
A ma jor hurdle in understanding the dynamics of linked rigid body systems is
the complex notation that is required For convenience Table summarizes the notation
whichisintro duced and used throughout this chapter
Velo city and acceleration propagation
To solve the forward dynamics problem one must rst determine the absolute
motion of all of the links of the linkage The linear and angular velo cities of the links
dep end only on q and q The linear and angular accelerations of the links dep end on these
accelerations can still quantities and also on q Although q is initially unknown the link
b e expressed in terms of it
De nition Attached to each link i is a body frame F with origin at the link s center
i
of mass and axes aligned with the principle axes of inertia for the link Vectors expressed
in link co ordinates have coordinates resolved relative to this body frame
The immediate goal is to solve this subproblem
Problem Velo city and acceleration propagation Given The joint positions q
link the linear velocity v angular velocities q and accelerations q Compute For each
i
velocity linear acceleration a and angular acceleration al l relative to an inertial
i i i
frame
n numb er of links of serial linkage
v linear velo city of link i
i
angular velo city of link i
i
a linear acceleration of link i
i
angular acceleration of link i
i
v relative linear velo cityoflinki De nition
rel
relative angular velo city of link i De nition
rel
m mass of link i
i
M matricized mass of link i Equation
i
I diagonalized b o dy frame inertia tensor of link i App endix A
i
d vector from link i inb oard joint to link i c o m Figure
i
r vector from link i c o m to link i c o m Figure
i
u unit vector in direction of joint i axis Figure
i
I spatial isolated inertia of link i De nition
i
A
I spatial articulated inertia of link i Theorem
i
Z spatial isolated zero acceleration z a force of link i De nition
i
A
Z spatial articulated zero acceleration z a force of link i Theorem
i
I
f spatial force applied byinb oard joint to link i De nitions
i
O
f spatial force applied by outb oard jointtolinki De nitions
i
v spatial velo city of link i De nition
i
a spatial acceleration of link i De nitions
i
s spatial joint axis of joint i De nition
i
c spatial Coriolis force for link i De nition
i
q scalar p osition of joint i
i
q scalar velo cityofjoint i
i
q scalar acceleration of joint i
i
vector velo cityofjoint i Equation
i
vector acceleration of joint i Equation
i
O inertial reference frame
F b o dy frame of link i De nition
i
X spatial transformation from frame F to frame G De nition
G F
X spatial transformation from frame F to frame F
j i i j
Table Notation used in Chapter
The quantities v a and describ e the motion of frame F relative to an
i i i i i
inertial frame O The strategy for solving this problem is straightforward The velo cities
and accelerations of link i are completely determined by the velo cities and accelerations of
link i and the motion of joint i Since the velo cities and accelerations of the base link
are the base link is a starting p oint and one pro ceeds inductively from there
A few additional vectors are needed as shown in Figure Let u denote the i
link i ui link i−1 joint i
d i
F Fi
i−1 ri
Figure Quantities for propagating velocities and accelerations
unit vector in the direction of the axis of joint i r denote the vector from the origin of
i
F to the origin of F andd denote some vector from the axis of joint i to the origin of
i i i
F
i
The motion of link i may b e divided into two parts one due to the motion of link
i and one due to the motion of joint i The latter comp onent is called the relative
motion
ar cen De nition Relative velo cities The linear relativevelo cityv is the line
rel
ter of mass velocity of link i due only to the motion of joint i that is the linear center of
mass velocity that link i would have if link i were held xed The angular relative
velo city is the angular velocity of link i due only do the motion of joint i that is the
rel
angular velocity that link i would have if link i were held xed
The angular velo city of link i is the angular velo cityoflinki plus the angular
velo city induced byrotationatthejoint
i i rel
Propagating linear velo city is similar but there is an added term due to the rotation of
link i
v v r v
i i i i
rel
The formula for propagating angular acceleration is obtained by di erentiating
i i
rel
The formula for propagating linear acceleration is obtained by di erentiating
a a r r v
i i i i i i
rel
Since r v v whichby is r v the ab ove equation can b e rewritten
i i i i i
rel
a a r r v v
i i i i i i i i
rel rel
Equations express link i s motion in terms of link i s motion and
the relative motion due to joint i All that remains is to de ne v and their time
rel rel
derivatives for prismatic and revolute joints To this end de ne twovectors along the axis
of joint i
q u
i i i
q u
i i
i
Supp ose joint i is prismatic Alone it imparts no angular velo city to link i only a linear
center of mass velo city From Figure and De nition
rel
v
i
rel
On the other hand a revolute joint i imparts b oth an angular velo city and a linear center
of mass velo city to link i From Figure and De nition
i
rel
v d
i i
rel
The following lemma is useful in computing and v
rel rel
Lemma For prismatic or revolute joints
i i i
i
Furthermore for revolute joints
d
d d d d
i i i i i i i i i
i
dt
Proof Di erentiating
u u q q u q
i i i i i i i
i
Since the joint axis u rotates with link i u u Substituting this result into
i i i
the ab ove equation proves the rst claim of the lemma For the second claim note
d
d d d
i i i i i i
dt
Now is given by the rst claim and d rotates with link i
i i
d d d
i i i i i i
The second equality ab ove follows from and Substituting for and d into
i i
distributing the cross pro ducts and applying the identity
A B C B C A C A B
proves the second claim
The results of Equations plus Lemma are summarized b elow
joint i prismatic joint i revolute
i
rel
v d
i i i
rel
i i
i
rel
v d d d
i i i i i i i i i
i i
rel
Substituting these quantities into equations pro duce the propagation formulae
that givelinki s motion in terms of link i s motion and the motion of joint i If joint i
is prismatic
i i
v v r
i i i i i
i i
a a r r
i i i i i i i i i i
If joint i is revolute
i i i
v v r d
i i i i i i
i i i i
i
a a r d
i i i i i
i
r d d
i i i i i i i i i
In Equations all velo city and acceleration vectors are absolute mean
ing they are taken relative to the inertial frame O In computations however the co
ordinates of the vectors may be expressed relative to any convenient co ordinate frame
Computations are simpli ed if all vectors are expressed in F the b o dy frame attached to
i
link i
As an example consider the planar linkage of Figure Assuming joint
link 1
y
F1 v1 x
d1 a1 q 1
y
O
x
Figure The vectors v and a describe the motion of the center of mass of link relative
to the inertial frame O But the coordinates of these vectors may beexpressed in any frame
rotates counter clo ckwise with constant angular velo city the expressions for v and a in
O s co ordinates are
q d cos q q d sin q
a v
q d sin q q d cos q
The same vectors expressed in F s co ordinates are
i
q d
a v
q d
These latter expressions are simpler b ecause F rotates with link Also the vectors d and
i
u are xed in F somany of the sub expressions app earing in may b e precom
i i
puted The only catchisthatvector co ordinates expressed in F must b e transformed to
i
frame F b efore they can b e used in computations with other vectors expressed in F This
i i
amounts to multiplying the four vectors v and a by a rotation matrix
i i i i
b efore using them in If joint i is prismatic this rotation matrix is constantand
may also b e precomputed if join i is revolute the matrix dep ends on q
i
The rst step in the forward dynamics algorithm is the computation of the absolute
linear and angular velo cities of all links These are determined from the known dynamic
centrip etal and Coriolis forces acting on the links The state q q and determine the
algorithm for computing these velo cities follows directly from Equations and
and is shown in Figure The derived acceleration propagation equations
involve the unknown joint accelerations through the term These will be be needed in
i
deriving the rest of the forward dynamics algorithm
Spatial algebra
Spatial algebra is a p owerful notation for describing quantities and relations rele
vant to the dynamics of three dimensional systems A spatial vector is six dimensional and
replaces two ordinary vectors in R The spatial acceleration of a rigid b o dy describ es b oth
its linear and angular accelerations The spatial force applied to a rigid b o dy describ es b oth
the applied ordinary D force and the applied D moment Throughout this chapter
Featherstone s convention of denoting spatial quantities with a caret is used
space with a frame F attached to it let De nition For a rigid body moving through
v a and be the linear velocity angular velocity linear acceleration and angular
compSerialLinkVelocities
v a
for i to n
R rotation matrix from F to F
i i
r radius vector from F to F in F coordinates
i i i
R
i i
v Rv r
i i i
if joint i is prismatic
v v q u
i i i i
else joint i is revolute
q u
i i i i
v v q u d
i i i i i
Figure compSerialLinkVelocities Compute the absolute linear and angular velo c
ities of all links of a serial linkage in link co ordinates Input data are the dynamic state
q q Output data are the velo cities and v for eachlink
i i
acceleration of frame F relative to an inertial frame O Then the spatial velo city v and
spatial acceleration a of the body expressed in frame F are given by
v a
v a
Now consider a rigid body moving through space with two frames F and G at
tached to it where G is displaced but not rotated relative to F Let v and v be the
F G
linear velo cities of the origins of these frames let be the common angular velo city of
the frames and let r be the o set vector from the origin of F to the origin of G Then
v v r and so
G F
v
G
v v r r v
G F F
is the identity matrix see App endix A for a de nition of r The matrix on
the right hand side is a spatial matrix If G is also rotated relative to F and R is
the rotation matrix that transforms vector co ordinates in F to vector co ordinates in G the
vector on the right hand side must b e premultiplied by the spatial matrix
R
R
in order to account for the rotation of G relativetoF This leads to the following de nition
De nition Let F and G be two frames let r be the o set ve ctor from the origin of F
to the origin of G expressed in G s coordinates and let R be the rotation matrix
transforming non spatial vectors from F to G Then the spatial transformation
matrix from F to G is given by
R R
X
G F
r R rR R
With this notation the spatial velo city transformation shown ab ove can b e written
v X v
G G F F
A spatial transformation matrix is the analog of an ordinary rotation matrix it transforms
X transforms the co ordinates of a spatial vector from one frame to another The matrix
F G
spatial accelerations from frame F to frame G in the same way as it transforms velo cities
Spatial vectors are also useful for describing external in uences acting on rigid b o dies Given
a rigid b o dy with an attached frame F one can express the total external in uences on the
b o dy as a force with line of action passing through the origin of F and a torque
De nition For a rigid body with attached frame F let the total external in uences on
the body be given by a force f with line of action passing through the origin of F and a
torque Then the spatial force acting on the body expressed in frame F is
f
f
Let G b e a second frame attached to the rigid b o dy and let r b e the o set vector
from the origin of F to the origin of G The force acting through the origin of F may be
displaced to act through the origin of G if a correction torque of r f is added the torque
requires no adjustment see MK for details Therefore
f f f
f
G
r f r
F F G
X that trans The parallel to is obvious the same spatial transformation matrix
F G
forms spatial accelerations from frame F to frame G also transforms spatial forces from
frame F to frame G
De nition Let
a
x
b
be the coordinate representation of a spatial vector with a b R The spatial transp ose
of x denoted by x is
h i
T T
x b a
The spatial inner pro duct of two spatial vectors x and y is given by x y
To transp ose a spatial vector one swaps its two halves and transp oses each of them this
y of a body and f is a is typically what one wants For example if v is the spatial velo cit
spatial force applied to the b o dy the inner pro duct of spatial force and spatial velo cityis
i h
f
T T
f v f v f
v v
whichispower as with ordinary D vectors
Spatial formulation of acceleration propagation
The acceleration propagation equations develop ed in Section are compactly
expressed with spatial algebra If joint i is prismatic equations can b e written
i i
q
i
r u r a a
i i i i i i i i i i
If joint i is revolute equations can b e written
u
i i i
q
i
u d r a a
i i i i i i
i i
r d d
i i i i i i i i i
Equations and are uni ed with the help of the following de nitions
De nition The spatial joint axis of joint i is the spatial vector
s
i
u
i
if the joint is prismatic and the spatial vector
u
i
s
i
u d
i i
if the joint is revolute
De nition The spatial Coriolis force of link i is the spatial vector
c
i
r
i i i i i
if the inboard joint i is prismatic and the spatial vector
i i
c
i
r d d
i i i i i i i i i
if the inboard joint i is revolute
The rst vector on the right hand sides of and is just the spatial
acceleration of link i transformed to frame F Using the shorthand X for X this
i j i F F
j i
vector can b e written X a Thus and can b oth b e written as
i i i
X a q s c a
i i i i i i i
This compact result encapsulates the acceleration propagation equations derived in Sec
tion One advantage of the spatial form is that prismatic and revolute joints are put
into a common framework di erences only app ear in the underlying expressions for s and
i
c By the established convention vectors subscripted i are expressed in frame F Hence
i i
s is constant and may be precomputed The vector c comprises Coriolis and centrip etal
i i
acceleration terms It involves velo cities of link i whichmust b e transformed into frame
F andvelo cities of link i It dep ends on the p ositions and velo cities of joints through i
i
but not on anyjoint accelerations
The Featherstone algorithm
The Featherstone algorithm is a metho d of solving the forward dynamics problem
in O n time for an n link manipulator It is one of a family of metho ds called structural ly
recursive algorithms These metho ds are alternatives to the O n algorithms that explicitly
build the mass matrix for the system and must invert it to solve for joint accelerations
WO
De nition Articulated body of a serial linkage For a serial linkage with n links
sever the linkage at joint i and consider the connected subchain comprising links i n
in isolation detached from the xed base This subchain is cal led an articulated body
of the original linkage and link i is cal led the handle of the articulated body The trivial
articulated body with link n as a hand le is simply the isolated rigid body of link n
Featherstone s algorithm works by considering successive articulated b o dies of the
original linkage b eginning with the trivial case of link n alone and adding inb oard links
one by one The key idea is to relate the spatial acceleration of the handle to the spatial
force applied at its inb oard joint The links outb oard to the handle will have an e ect on
this relation
De nition Per link spatial vectors The spatial acceleration of link i is denoted
I
a the spatial force exerted on link i through its inboard joint is f and the spatial force
i
i
O
exerted on link i through its outboard joint is f The co ordinates of all of these spatial
i
vectors are expressed in frame F
i
I
Even though the inb oard in uences on link i are exerted at the joint i the spatial force f
i
I I
I T I
are the equivalent force and torque applied at the and where f is given by f
i i
i i
I
origin of F the line of action of f passes through the origin of F The same is true for
i i
i
O
the outb oard spatial force f
i
Theorem Serial linkage articulated motion Consider the articulated body of a
A
serial linkage that has link i as a hand le i n There exists a spatial matrix I and
i
A
a spatial vector Z such that
i
I
A A
f I a Z
i
i i i
A
I is cal led the spatial articulated inertia of link i and is independent of the joint
i
A
velocities and accelerations Z is cal ledthespatial articulated zero acceleration z a
i
force of link i and is independent of the joint accelerations
The adjective articulated indicates that the entire sub chain b eginning at the handle
A
is b eing considered as opp osed to the handle in isolation Z is so named since by it
i
is the force whichmust b e exerted by the inb oard jointonlinki if link i is not to accelerate
a The pro of of Theorem is involved but constructive an algorithm will follow
i
directly from the pro of The pro of is by induction on i b eginning with the trivial sub chain
comprising only the outermost link and then adding links on the inb oard side one by one
until the entire linkage is considered
Base case
For the base case of the induction i n and the sub chain being considered is
simply the last link of the chain as shown in Figure This free b o dy diagram depicts
all of the forces and torques acting on the link those applied by the inboard joint but
resolved at the origin of F and those due to external in uences likegravity The motion
n
of the body is given by the Newton Euler equations describ ed in App endix A In link
co ordinates
I
f m g m a
n n n
n
I
I I
n n n n n
n
where m is the mass of the link n andI is the inertia tensor in link co ordinates These
n n
equations can b e written as a single spatial equation If
M m
i i
Featherstone calls these spatial articulated z a forces bias forces and denotes them P i
τI n
m n g
I inboard fn
joint link n
Figure The freebody diagram of the last link of the serial linkage It is the hand le of a
trivial subchain
then
I
M m g f
n n n
n
I
I a I
n n n n n
n
or more compactly
A A
f I a Z
n n
n n
A
Note that Z is indep endent of the acceleration of link n and therefore indep endent of the
n
A
is indep endent of the join joint accelerations I t velo cities and accelerations In fact it
n
dep ends only on the mass prop erties of link n and may b e precomputed Thus Theorem
holds for the base case i n Two expressions ab ove will be useful later and are given
sp ecial names
De nition The spatial isolated inertia of link i is de ned as the spatial matrix
M
i
I
i
I i
The spatial isolated zero acceleration z a force of link i is de ned as the spatial
vector
m g
i
Z
i
I
i i i
A A
except that and Z These two quantities are similar to their articulated counterparts I
i i
I and Z apply to link i in isolation with no outb oard links attached For the sp ecial case
i i
A A
Z I and Z of i n the de nitions coincide I
i i
i i
Inductive case
Assume Theorem is true for i and consider link i Figure In addition
outboard link i−1 joint
fO i−1 τO i−1
I fi−1 m i−1 g τI inboard i−1 joint
O
Figure The free body diagram of an inner link of the serial linkage It is the hand le of
a non trivial subchain
to gravity and the force and torque applied through the inboard joint there are also the
O
O
and As b efore all forces force and torque applied through the outb oard joint f
i
i
and torques are resolved at the origin of the link frame Again using the Newton Euler
equations the equations of motion of link i are given by
I O
f f m g M
i i i
i i
I O
a I I
i i i i i
i i
Using spatial notation and rearranging yields
I O
f I a Z f
i i i
i i
I O
Hence is the equal but opp osite reaction force to f The force f
i i
O I
f X f
i i
i i
Combining the ab ovetwo equations
I I
f I a Z X f
i i i i i
i i
Invoking the inductivehyp othesis for link i gives
A A I
a Z I a Z X I f
i i i i i i
i i i
To put this equation in the form of the acceleration a must be eliminated from
i
the right hand side This is done by expressing a in terms of a using the acceleration
i i
propagation equation Substituting into the ab ove equation and rearranging gives
i h
A A A A I
s q c I I X a Z X Z I X I f
i i i i i i i i i i i i
i i i i i
Now q must b e eliminated from the right hand side The following lemma will help
i
Lemma Atevery joint
I
Q f s
i
i i
where Q is the magnitude of the force exerted by the prismatic joint actuator or the mag
i
nitude of the torque exerted by the revolute joint actuator
Proof Supp ose a force f and a torque are applied to link i at the inboard joint These
give rise to a spatial inb oard force resolved in the b o dy frame of
f
I
f
i
d f i
If joint i is prismatic
f
I
s f f u
i
i i
u d f
i i
The right handsideisthe comp onent of the applied force along the joint axis This force
must b e supp orted by the actuator hence it is Q If joint i is revolute
i
f u
I
i
s f u f d d f u
i i i i
i i
u d d f
i i
The right hand side reduces to u the comp onent of the applied torque along the joint
i
axis This torque must b e supp orted by the actuator hence it is Q
i
Substituting equation for link i s spatial acceleration into yields
I
A A
f I X a q s c Z
i i i i i i
i i i
Premultiplying b oth sides by s and applying Lemma gives
i
A A
Z I X a q s c s Q s
i i i i i i i
i i i i
from which q may b e determined
i
A A A
Z Q s I X a s I c
i i i i i
i i i i i
q
i
A
s I s
i
i i
Substituting this expression for q into and rearranging gives
i
A A
I s s I
I
A
i
i i i
A
X a f I X I
i i i i i i
i i
A
s I s
i
i i
h i
A A A
I s Q s Z I c
i i i
i i i i
A A
Z X Z I c
i i i i
i i
A
s I s
i
i i
Comparing this to the desired form
A A
I s s I
A A
i
i i i
A
X I I I X
i i i i i
i i
A
s I s
i
i i
h i
A A A
I s Z I c Q s
i i i
i i i i
A A A
c I X Z Z Z
i i i i
i i i
A
I s s
i
i i
These equations compute the articulated inertia and articulated z a force for link i of
X X and the chain given those same quantities for link i I and s are constants
i i i i i i
dep end only on joint p ositions and c dep ends only on joint p ositions and velo cities It
i
A A
follows that if I is indep endent of jointvelo cities and accelerations then so is I Also
i i
A A
Thus if Theorem is true for is indep endent of joint accelerations then so is Z if Z
i i
a handle at link i it is also true for a handle at link i By induction the theorem is
true for a handle at any link
Forward dynamics algorithm
The pro of of Theorem gives an algorithm for computing forward dynamics of
a serial linkage Problem The algorithm comprises four main steps
Iterating from the base to the tip of the linkage compute the velo cities of all links
algorithm compSerialLinkVelocities Figure
Initialize each link s articulated inertia and articulated z a force to their isolated coun
terparts Also compute the Coriolis vector for each link algorithm initSerialLinks
Figure
Iterating from the tip to the base of the linkage compute the articulated inertia and
articulated z a forces of each link algorithm serialFwdDynamics Figure
Iterating from the base to the tip of the linkage compute the acceleration of each joint
and the spatial acceleration of each link algorithm serialFwdDynamics Figure
A A A
s and Q s Z I c should only b e computed once I The common sub expressions s
i i i
i i i i i
per call
Extension to tree like linkages
The previous sections solve the forward dynamics of serial linkages such as the one
in Figure The results can b e extended to encompass a more general class of kinematic
structures tree shaped linkages Consider the complex p endulum shown on the left side of
Figure This is not a serial linkage but it do es have a tree like top ology Attached
initSerialLinks
for i to n
m g
A
i
Z
i
I
i i i
M
A
i
I
i
I
i
if joint i is prismatic c
i
r
i i i i i
i i
else c
i
r d d
i i i i i i i i i
Figure initSerialLinks Initialize the articulated spatial inertias and articulated
z a forces to their isolated counterparts for all links in a serial linkage Also compute the
spatial Coriolis vector for eachlink
to each link i is a unique link h that is closer to the xed base link Letting h be the
parent of i one can construct a tree representing the linkage as shown on the right side
of Figure Each link b ecomes a no de of the tree and each joint b ecomes an edge The
links are numb ered using the depth rst recursion shown in Figure
De nition Tree linkage indices Let the links of a tree linkage be indexed using the
of the unique link outboard to it algorithm of Figure and assign each joint the index
Index the inertial frame as link Then the fol lowing hold
The n moving links of the structure and the n joints are numbered n
For any joint i the link outboard to the joint the child link is indexed i and the link
inboard to the joint the parent link is indexed h with h i
The forward dynamic algorithm for serial linkages was studied in terms of links
h link in tip to base or base to tip order The Iterations were p erformed by visiting eac
algorithm can also be viewed as enumerating over joints since in a serial linkage there is
exactly one joint between every two links and vice versa In studying tree linkages there
serialFwdDynamics
call compSerialLinkVelocities compute all link velocities
A A
call initSerialLinks initialize I Z and c for all links
i
i i
for i n downto
A A
I s s I
A A A
i
i i i
X I X I I
i i i i
i i i
A
s I s
i
i i
A A A
I s Q s Z I c
A A A A
i i i
i i i i
Z Z X Z I c
i i i
i i i i
A
s I s
i
i i
a
for i to n
A A A
Q s I X a s Z I c
i i i i i
i i i i i
q
i
A
s I s
i
i i
a X a c q s
i i i i i i i
Figure serialFwdDynamics Compute the accelerations of joints n of a se
rial linkage Input data are the joint p ositions q joint velo cities q and joint actuator
i i
forces torques Q output data are the joint accelerations q i n
i i
is an advantage to the joint centric viewp oint For these there is not always a unique
choice for the next link in a base to tip recursion nor is there a unique starting link for
joint still has exactly one inb oard link and exactly one a tip to base recursion But every
outb oard link By describing the algorithms in terms of computations to be done at each
joint and visiting the joints in a particular order the serial linkage algorithm generalizes
to tree linkages Using the joint indexing scheme describ ed ab ove the algorithm visits the
joints in increasing or decreasing order Pointers to the unique inb oard and outb oard links
to eachjoint should b e stored with the joint
0 1 1
1 9 2 2 2 9 8 389 12 3 4 10 5 4 11 4 7 10 6 56 11 12
7
Figure Left A complex pendulum with a tree like topology The moving links are
numbered through as are the joints Four of the joint indices areindicated with circled
numbers Right The corresponding tree representing the kinematic structure
int numberLinks tree idx
r root link of tree
r idx idx
idx idx
for each child c of r
idx numberLinks c idx
return idx
Figure int numberLinks tree idx Recursively number the links of a kinematic
tree so that every link has higher index than its parent The routine returns the integer
one greater than the highest index in the tree The routine should initially b e called with
the entire original tree and idx
Velo city and acceleration propagation through trees
The rst step of the forward dynamics algorithm is to compute the absolute ve
lo cities of all the links in the structure This was done in Section for serial linkages
A little re ection on Figure reveals that Let h be the index of the parent of link i
the absolute velo city of link i may be determined from the absolute velo city of its parent
link h and the velo city of joint i Thus the velo city propagation equations
and still hold with the subscript i replaced by h
The order in which the velo cities are propagated through links must b e chosen so
that the velo city of a link is always computed b efore it is needed This is accomplished by
iterating over the joints in increasing order and at each joint propagating the velo cityfrom
its inb oard link to its outb oard link The algorithm is shown in Figure The same
quantities may b e precomputed as in the serial linkage algorithm
compTreeLinkVelocities
v a
for i to n
h index of link inboard to joint i
h index of parent link i index of joint child link
R rotation matrix from F to F
i
h
r radius vector from F to F in F coordinates
i i
h
R
i
h
v Rv r
i i
h
if joint i is prismatic
v v q u
i i i i
joint i is revolute else
q u
i i i i
v v q u d
i i i i i
Figure compTreeLinkVelocities Compute the absolute linear and angular velo cities
of all links of a tree linkage in link co ordinates Input data are the dynamic state q q
Output data are the velo cities and v for each link
i i
Propagating accelerations through tree linkages is also analogous to the case for
serial linkages If link h is the parent of link i the acceleration of link i can b e determined
from the p ositions velo cities and accelerations of link h and joint i The derivation for
acceleration propagation is identical to that for the serial linkage case the end result is c f
Equation
X a q s c a
i i i i i
h h
Articulated inertias and z a forces for tree linkages
The next step of the forward dynamics algorithm is to compute the articulated
inertias and z a forces for all links For serial linkages this is done using a tip to base
recursion For a tree linkage articulated inertias and z a forces may b e computed directly
for links that are the leaves of the tree for an inner link the quantities are computed by
combining the quan tities from the link s children
De nition Articulated body of a tree linkage For a tree linkage break the tree
at joint i and consider the connected subtree rooted at link i in isolation detached from
the xed base This subtree is an articulated body of the original tree and link i is the
ody is trivial handle of the articulated body If link i is a leaf of the tree the articulated b
and is simply the the isolated rigid body of link i
The central result to b e proved is the counterpart to Theorem
Theorem Tree linkage articulated motion Consider the articulatedbody of a tree
A
linkage that has link h as a hand le h n There exists a spatial matrix I and a
h
A
spatial vector Z such that
h
I
A A
f I a Z
h
h h h
A A
is independent of the joint velocities and accelerations and Z is indepen Furthermore I
h h
dent of the joint accelerations
Theorem will b e proved by induction on the height of the articulated b o dy the maximum
numb er of no des along a simple path from the handle to some leaf of the subtree For the
base case consider the heighttobe Then the articulated b o dy is trivial it is simply an
isolated rigid b o dy that is a leaf of the original linkage There is no di erence b etween this
case and the case of the isolated tip link of a serial linkage Thus the pro of of Section
go es through unchanged for this base case
For the inductive case consider the more general articulated b o dy handle shown
in Figure The handle link h may haveseveral links outb oard to it these links are
joint h
IIτ link h O1 τO1 fh , h fh , h Om τOm fh , h
O2 τO2 fh , h m h g
joint i1 joint im
joint i2
Figure The free body diagram of a non trivial hand le of an articulated body from a
tree linkage
indexed i i i The in uences applied to link h come from gravity as well as forces
m
and torques transmitted through the one inb oard and m outb oard joints Resolving these
in uences at the origin of frame F the equations of motion of link h are given by the
h
Newton Euler equations
O
j
m I
X
f M f m g
h h h
h
h
O
j
I
I a I
j h h h h h
h
h
where the sup erscript O denotes forces or torques imparted through the outb oard joint i
j j
Using spatial notation and rearranging yields
m
X
I O
j
f I a Z f
h h h
h h
j
O I
j
Since f is the reaction force to f
h i
j
I O
j
f f X
i
h
i h
j
j
Combining the ab ovetwo equations
m
X
I I
X f f I a Z
i
h h h h
i h
j
j
j
Each of the child links i is the handle of a subtree of smaller height than the subtree with
j
link h as a handle The inductive hyp othesis may be invoked for these child links
giving
m
X
A A I
a Z X I I a Z f
i i
h h h h
i i h
j j
j j
j
Using the acceleration propagation equation to express a in terms of a and rear
i
h
j
ranging gives
m m
h i
X X
I
A A A A
A
f I X I a Z X X Z I c q I s
i i i i i i
h h h h h h
h i i i i
j j j j j j
j j j j
j j
The derivation of joint accelerations see Lemma and the subsequent discussion makes
no assumptions ab out the top ology of the linkage it applies equally well to tree linkages
The main result stated in the current notation is
A A A
Q s I X a s Z I c
i i i
h h
i i i i i
j j j
j j j j j
q
i
j
A
s I s
i
i i
j
j j
Combining the ab ovetwo equations and rearranging gives
A A
m
X
I I s s
i
I
A
i i i
j
j j j
A
X a I X I f
i i
h h h h
i h
j j
j
A
s I s
j
i
i i
j
j j
i h
A A A
m
Z I c I Q s s
X
i i i
i i i i
j j j
A A
j j j j
Z I c X Z
i i
h h
i i
j j
j j
A
s I s
j
i
i i
j
j j
Comparing this to the desired form
A A
m
X
s s I I
i
A A
i i i
j
j j j
A
I X I I X
i i
h h h
i h
j j
j
A
I s s
j
i
i i
j
j j
i h
A A A
m
c I Z s Q s I
X
i i i
i i i i
j j j
A A A
j j j j
c I Z Z Z X
i i
h h
i i h
j j
j j
A
s s I
j
i
i i
j
j j
These equations compute the articulated inertia and articulated z a forces for link h of the
A
are indep en linkage given those same quantities for its children links i i If the I
m
i
j
A A
are indep endent of joint dent of jointvelo cities and accelerations than so is I If the Z
h i
j
A
accelerations then so is Z By induction on the height of the subtrees of the handles
h
Theorem is true for a handle at anylink
Forward dynamics algorithm for tree linkages
At rst glance the recursive equations seem to imply a more complex
computation for tree linkages than for serial ones multiple articulated inertias and z a
forces of child links must be combined to compute the corresp onding quantities for the
parent But in fact the same numb er of back propagations are p erformed one for each joint
the algorithm is still O n The only di erence from the serial algorithm is the order in which
inertias forces and accelerations are propagated The forward dynamics algorithm for tree
linkages is shown in Figure It iterates using a jointcentric approach The articulated
inertia and z a force of a given link h are accumulated over several iterations as the
contributions from all of link h s c hildren are propagated back Link h s articulated inertia
and z a force will reach their nal values b efore they are propagated back to h s parent
When propagating accelerations link h s acceleration is known b efore the accelerations of
its children are computed These claims follow from the joint indexing scheme describ ed in
De nition The algorithm comprises four steps
Iterating over the joints in increasing order of index compute the spatial velo cities of
all links algorithm compTreeLinkVelocities Figure
Initialize each link s articulated inertia and articulated z a force to their isolated coun
terparts Also compute the Coriolis vector for each link algorithm initTreeLinks
Figure
Iterating over the joints in decreasing order of index back propagate the articulated
inertia and z a forces from the link outb oard to the jointto the link inb oard to the
joint algorithm treeFwdDynamics Figure
the acceleration of Iterating over the joints in increasing order of index compute
each joint and the spatial acceleration of the link outb oard to the joint algorithm
treeFwdDynamics Figure
Extension to oating linkages
The algorithms discussed thus far assume grounded linkages link is attached
to the inertial frame through a one degree of freedom joint A oating linkage is one in
initTreeLinks
for i to n
m g
A
i
Z
i
I
i i i
M
A
i
I
i
I
i
h index of link inboard to joint i link h is link i s parent
if joint i is prismatic c
i
r
i i
h h h
i
h
else c
i
r d d
i i i i i i
h h h
Figure initTreeLinks Initialize the articulated spatial inertias and articulated z a
forces to their isolated counterparts for all links in a tree linkage Also compute the spatial
Coriolis vector for each link
which link is not attached to the inertial frame An example of such a system is a
mechanical bug jointed legs are attached to a b o dy link which is free to move around
an environment Dynamics of such systems are easily mo deled with only slightchanges to
the previous algorithms These changes are discussed below for the tree linkage forward
dynamics algorithm which is more general than the serial linkage one
A minimal description of the dynamic state of an n link oating linkage comprises
the p osition orientation linear velo city and angular velo city of the base link plus the
p ositions and velo cities of the joints
q v q r
Here r is the p osition of the origin of b o dy frame F and is some parameterization of
the orientation of this frame b oth of these are relative to some xed frame O The joint
p osition vector q is now only n dimensional The vectors v and are the linear and
angular velo cities of frame F and q is the n dimensional jointvelo cityvector If unit
quaternions see App endix A are used to parameterize the orientation of the base link the
treeFwdDynamics
call compTreeLinkVelocities compute all link velocities
A A
call initTreeLinks initialize I Z and c for all links
i
i i
for i n downto
h index of link inboard to joint i link i is outboard
A A
I s s I
A A A
i
i i i
X I I X I
i i
h h
h h i
A
s I s
i
i i
A A A
c I Z s Q s I
A A A A
i i i
i i i i
Z I c Z Z X
i i
h
i i h h
A
s I s
i
i i
a
for i to n
h index of link inboard to joint i link i is outboard
A A A
c I X a s Z Q s I
i i i
h h
i i i i i
q
i
A
s I s
i
i i
a X a c q s
i i i i i
h h
Figure treeFwdDynamics Compute the accelerations of joints n of a tree link
age Input data are the joint p ositions q jointvelo cities q and joint actuator forces torques
i i
Q output data are the joint accelerations q i n
i i
total dynamic state of an n link oating linkage has n parameters as opp osed to the n
parameters of a grounded linkage For a grounded linkage the forward dynamics algorithm
uses the dynamic state to compute q the n dimensional joint acceleration vector For a
oating linkage the algorithm must compute a and the linear and angular accelerations
of frame F plus q the n dimensional joint acceleration vector
Changes to the velo city propagation algorithm to handle a oating linkage are
minimal The strategy for computing link velo cities is to calculate v and from v
i i
h h
and q where link h is the parent of link i For grounded linkages the inertial frame serves
i
as the parent of link and v starts the iteration For oating linkages the
velo cities v and are already part of the dynamic state vector and so do not need to b e
computed The iteration simply starts at joint and pro ceeds to joint n at which p oint
the velo cities of all the other links will b e known
The calculation of the articulated inertias and articulated z a forces for the links
needs no adjustment for oating linkages Featherstone s algorithm always considers artic
ulated b o dies which are collections of connected links that are detached from the inertial
frame Note that in the lo op which computes articulated inertias and z a forces in Fig
ure information ab out joint the connection between link and the inertial frame
is never used
In the second lo op accelerations are propagated outward toward the leaf links of
the tree For grounded linkages the algorithm b egins b y setting a to and then computes
q a q a and so on For a oating linkage joint do esn t exist and the iteration must
b e b o otstrapp ed in a di erent manner The solution comes from Equation applied to
link
I A A
f I a Z
Since link is disconnected from the inertial frame the spatial force applied through the
inb oard jointmust b e Thus
A A
a I Z
As long as the individual links all have non singular mass matrices which is the case for
A
all real ob jects I is invertible Fea Once the spatial acceleration a is determined
the acceleration propagation pro ceeds exactly as in the grounded case The second lo op
continues b eginning with joint The algorithm remains O n since the inversion of the
A
I takes constant time
Chapter
Hybrid Simulation
While there is a large range of contact interactions that can b e mo deled via trains of
impulses there are also limitations of this approach Constraints are the natural vehicle for
describing many typ es of contact interactions This chapter intro duces hybrid simulation
a metho d of combining the impulse and constraint based simulation paradigms Impulses
are used to transmit unilateral contact forces that arise between separate b o dies while
constraints are used to mo del forces which can push or pull such as those o ccurring in a
joint b earing Each metho d is applied to the contact it is most adept at handling One
main contribution of this chapter is a metho d for computing lower b ounds on the times of
impact b etween rigid b o dies that are part of linked structures Unlike the case for isolated
rigid b o dies the initial b ounds are sometimes violated however these violations can always
b e detected The collision detection algorithms of Chapter can thus b e extended to handle
linked b o dies A second ma jor contribution is a di erential approach for computing collision
impulses arising b etween rigid b o dies that are part of linked structures This extension of
the results presented in Chapter dep ends on computing a multib o dy collision matrix
that encapsulates the collision dynamics at the contact p oint An algorithm for computing
this matrix is given that runs in O n m time where n and m are the numb er of links in
the two colliding linked structures Finally the chapter describ es how amultilevel control
architecture can be easily added to an impulse based simulator using the one in Impulse as an example
A sp ectrum of physical systems
Consider a simple hinge joint Figure In principle the joint could be
Figure In practice it is not feasible to model the contact interactions at a hinge joint
using an impulse based approach
mo deled in an impulse based way enforcing the hinge constraint through collisions b etween
the hinge pin and sheath Due to the enormous amount of collision pro cessing that would
be necessary however impulse based simulation would be far to o slow Constraint based
metho ds should b e used instead
Figure depicts a sp ectrum of physical systems On the right end are jointed
manipulators a class of systems for which constraint based simulation approaches are clearly
appropriate The motion of these systems of rigid links is governed by bilateral equality
constraints imp osed at the joints Techniques to handle bilateral constraints are more de
ts velop ed and robust than those for unilateral inequality constraints The joint constrain
are also permanent facilitating e cient constraint based simulation Computing the for
ward dynamics of these systems is a classical problem of rob otics and a varietyofmethods
exist for doing so The interaction b etween the manipulator and the environment typically
o ccurring at the end e ector is not so clearly a constraint based interaction it is often
transient and only expressible through unilateral constraints Over limited intervals of mo
tion however the contacts can be mo deled through bilateral constraints and closed lo op
metho ds exist for computing the resulting manipulator dynamics Lil
Slightly to the left on the sp ectrum lie mechanisms a class of systems which
colliding contact constrained contact
impulse−based domain constraint−based domain
billiards & interactive jointed
part feeders bowling environments mechanisms manipulators
Figure Aspectrum of physical systems
are also highly constrained Unilateral constraints are more common in this case The
constraints are also more transient top ologically distinct contact con gurations may o ccur
during a complete cycle of the mechanism Nonetheless there is still a fair amount of contact
coherence and constraint based approaches can therefore work well Bara s blockfeeder
and double action jack are go o d examples of constrained based simulation of D mechanisms
Bar
On the other end of the sp ectrum are highly unconstrained collision intensive
systems like part feeders In these typ es of systems the almost complete lack of contact
coherence makes constraint based metho ds di cult to apply Mo deling frictional collisions
accurately is crucial to correct simulation For these applications impulse based simulation
tly to the right on the sp ectrum lie systems likea bowling alley or is the natural choice Sligh
a billiards table In these systems collisions still o ccur frequently and accurate mo deling of
them is imp ortant Some prolonged contact do es o ccur but contact coherence is still fairly
low particularly in critical intervals of the simulation such as when the b owling ball strikes
the pins Impulse based simulation can adeptly handle these typ es of systems including
the rolling and sliding contacts that o ccur
From a dynamic simulation standp oint the middle ground of the sp ectrum has
been the least studied This is probably because no existing simulation paradigms are
a natural t for the interesting applications that lie there Consider simulating a user
controlled agent within an interactive virtual environment A simulator must resp ond to
erratic unpredictable b ehavior on the part of the user controlled agent or other autonomous
agents in the environment Collisions provide muchofthe interaction b etween agents and
their environment The agents may bump into eachotheror other ob jects and maymove
or throw ob jects around Contact mo des are transient as agents grab push or pull ob jects
Settling phases as ob jects come to rest after b eing p ositioned or dropp ed byagents involve
more transient contact There are also manytyp es of contact b est mo deled with constraints
Hinged do ors b etween ro oms simple mechanical structures likelevers and the joints of the
agents all en tail p ermanent bilateral constraints There maybestacks of ob jects or other
situations where contact coherence is high and constrained based metho ds can e ectively
mo del the non p enetration b etween b o dies
An interactive virtual environment is just one example of an application that seems
to require b oth impulse based and constraint based techniques for e cient simulation ways
are needed to blend the approaches This chapter discusses hybrid simulation which is a
rst step in this direction Hybrid simulation extends the pure impulse based metho ds
discussed thus far to accommo date p ermanent bilateral constraints Thus systems of rigid
b o dies connected byjoints may b e handled Adjustments to the collision detection scheme
are required Also the calculation of collision impulses must account for the dynamics of
the colliding b o dies A connection to the simple rigid body the b o dies that are linked to
collision resp onse algorithm is made through the collision matrix K much of the the theory
discussed in Chapter then carries over to the case of linked b o dies Finally once the
impulses are computed they must be propagated through the linked systems of b o dies
instantaneously changing the velo cities of all b o dies in the linkage
Collections of linked rigid bodies are called multib o dies In this chapter multi
b o dies are restricted to the following de nition
De nition A multib o dy system or multibody is a col lection of rigid bodies linked
gree of freedom revolute or prismatic joints with tree like topology together through one de
That is the system is a single connected piece with no loops among the links One link is
speci ed as the base link and this link may beconnected to an inertial frame via a revolute
or prismatic joint or via a ful l six degreeoffreedom joint resulting in a oating multibody
The restrictions imp osed by this de nition are not essential but facilitate use of the forward
dynamics algorithms discussed in the last chapter There are extensions of Featherstone s
algorithm for more general joints Lil JR There are also metho ds of computing linkage
dynamics in the presence of kinematic lo ops Lil LNPE dJB Bar Multibody
impulses could still b e computed in cases of these more general systems the construction of
the collision matrix K would involve using more the general forward dynamics algorithms
instead of those presented in the last chapter
Collision detection
For collision detection purp oses a multib o dy is broken into its comp onent rigid
b o dies When a body passes in the vicinity of a multib o dy the narrow phase collision
detection system will track closest p oints b etween the b o dy and the nearbymultib o dy links
Links that are not close to the body will be discarded from consideration b y the broad
phase detection The collision detection system can be extended to handle multib o dies
using the twointerface routines describ ed in Section aswept volume routine and a TOI
co e cients routine
The motion of a rigid b o dy that is part of a multib o dy is much more complex that
that of a rigid b o dy or scripted b o dy Conservative b ounds on swept volumes and TOIs are
di cult to derive and the requirement that these b ounds b e conservative is abandoned for
multib o dies Instead Impulse makes aggressive predictions that are hop efully valid b ounds
in the average case If an aggressive b ound is violated this violation must b e detected
and the simulator must recover gracefully
Constrained body swept volumes
Recall the goal of the swept volume routine Given the state of a body at the
and a time interval t the routine returns an axes aligned b ounding b ox currenttimet
which encloses the b o dy s center of mass during the interval t t t For b o dies moving
ballistically or along simple scripted tra jectories this can be done For b o dies which are
part of a multib o dy itisvery di cult But one can assume the co ordinates of the center
of mass r will follow second order tra jectories
r t r t r t t t r t t t
x x x x
r t t t r t r t r t t t
y y y y
r t r t r t t t r t t t
z z z z
The quantities r t andr t are inferred from the current dynamic state of the multib o dy
and r t isaby pro duct of the forward dynamics algorithm it is just the linear acceleration
a of the link These accelerations can be stored so that their current values are always
available
Using these second order approximations make it easy to predict the minimum
t t It makes sense to pad and maximum r r and r values over the interval t
x y z
these values somewhat b ecause of the higher order terms which are neglected in the ab ove
equations this means returning a slightly larger b ounding b ox than that predicted by the
mo del The tradeo is that as b ox size increases so do es the chance that more b o dy pairs
will b e promoted to the active collision heap p erhaps unnecessarily The padding scheme
used in Impulse is to scale r t and r t by a factor b efore using them in the ab ove
equations
The p osition and velo cityofamultib o dy is numerically integrated using a deriva
tive evaluation routine such as treeFwdDynamics Chapter Each time the derivative
evaluation routine is called the p ositions of the links are determined The minimum and
maximum co ordinate values of each link s center of mass are tracked over the entire integra
tion interval at the end of the integration it is easy to check if these co ordinates remained
within the b ounding b ox predicted bytheswept volume routine
When a violation do es o ccur the correct b ounding b ox for the interval is known
since the minimum and maximum center of mass co ordinate values are known The b ound
ing b ox in the spatial hash table is up dated to re ect this new information If the new b ox
intersects no other b oxes than the old b ox did then no b o dy pairs should b e added to the
collision heap and the integration is still valid If new p ossible collision pairs are discovered
then the times to impact between these new pairs are computed and the pairs are added
to the collision heap If this do es not cause the b o dy pair at the top of the heap to change
the integration is still valid Otherwise a new collision check whichwas previously missed
b ecause of the incorrect b ounding b ox has b een discovered and this collision check should
ending time of the last integration In this case have been p erformed earlier than the
the simulation must be backed up The bodies states are restored to their values at the
b eginning of the last integration and the integration is redone this time stopping in time
for the earlier collision check After this second integration there will b e no b ounding b ox
violations since all of the invalid a priori boxes have b een replaced by the valid a posteri
ones Figure details the entire pro cess
update predict bounding box in bounding box hash table
integrate dynamics, tracking actual have new possible collision pairs bounding box n been detected?
y
was predicted y box violated? recompute TOIs for new pairs
n
n has soonest y continue collision time back up simulation
changed?
Figure The checking performed to detect aggressive bounding box violations and to
determine if the simulation should bebacked up as a result of the violation
Constrained body TOI co e cients
The narrow phase collision detection system requires a time of impact co e cient
routine given the dynamic state of a b o dy at the currenttimet and directional vector d
return co e cients A and B such that the distance that any p oint on the b o dy travels along
the direction d is b ounded ab oveby
A t t B t t
can b e calculated for ballistic b o dies and certain scripted Conservativevalues for A and B
b o dies for constrained bodies this remains an op en problem The approach taken in
Impulse is to abandon the requirement that the predictions be conservative and attempt
to compute reasonable co e cients which are usually valid A second order mo del of motion
is again used The co e cients computed for constrained b o dies are
A a t d
B v t d r k t k
max
These are very similar to the co e cients for ballistic rigid b o dies One di erence
is that the gravity vector g is replaced with the current acceleration of the body a t a
more general acceleration which also includes the e ects of gravity The term is
max
replaced with k t k the current angular velo city since it is di cult to compute a tight
b ound the angular velo city ofamultib o dy link over some future time interval Because of
the approximations made in this mo del the co e cients are padded by a safety factor In
Impulse A and B are computed as ab ove and then increased by times their resp ective
absolute value This reduces the chance that the co e cients will b e violated
The strategy for detecting time of impact violations is similar to that for detecting
bounding box violations the discrete data provided during numerical integration is tracked
TOI predictions are monitored using slack structures For each TOI estimate made for a
constrained b o dy there is a slack structure that tracks the progress of p oints on this b o dy
in the direction d The comp onents of a slack structure are
The unit vector d along whose direction motion of p oints of the b o dy is monitored
ts on the b o dy in The constants A and B which b ound the distance traveled by p oin
the direction d
The base time t at which the slackwas initialized
b
An upp er b ound on the distance traveled byanypoint on the b o dy in the direction
d since time t This is a truly conservative b ound based on values tracked during
b
dynamic integration
The time t at which the slack b ound as last up dated
l
When a TOI prediction involving a constrained b o dy B is made a slack is initial
ized The co e cients A and B are computed as describ ed ab ove the base time t and last
b
time t are set to the current time and the slack b ound is set to During integration the
l
maximum and minimum values of each comp onent of the b o dy s linear and angular velo city
y point on the body could have are tracked After the integration maximum distance an
traveled along d is computed using the tracked integration values this distance is added
to Acheck is made to insure
A t t B t t
b b
where t is the current time If this check fails a TOI violation has o ccurred The algorithm
to up date the slackisshown in Figure
updateSlack
dt t t
l
if dt return
v minimum components of linear velocity over last integration
min
v maximum components of linear velocity over last integration
max
maximum magnitude components of angular velocity
max
over last integration
k
if d k k v d else k k v d
x min x max x
x x
else k k v d if d k k v d
max y y min y
y y
if d k k v d else k k v d
z min z max z
z z
q q
q
k k r d d d
max max max max
x y z
x y z
k dt
if A t t B t t signal violation
b b
else t t
l
Figure updateSlack Up date a slack structure and signal a TOI violation if necessary
The variables t A B andt are all elds of the slack structure The variable t is the
l b
currentsimulation time
When a violation is detected the o ending TOI is recomputed using the now
known values for its motion during the integration step The simulation is backed up to the
state at the b eginning of the integration interval in whichthe violation o ccurred and the
integration is redone In this way the collision detection system avoids missing collisions
do to overly aggressive TOI co e cients
Collision resp onse
The algorithms develop ed in Chapter address the dynamics of multib o dies in
isolation but say nothing ab out their dynamics when they come into contact with other
b o dies Contact forces can be added to the mo del if one has a way to compute them
Impulse based simulation adopts an alternative approach all contacts are mo deled through
trains of collision impulses Chapter describ ed how to compute these impulses for the
case of rigid b o dy collisions this section handles the case of multib o dy collisions
Problem Multib o dy collision resp onse Given two col liding rigid bodies each of
which may be part of a larger multibody compute the impulse pair to be applied at the two
contact points and the instantaneous changes in velocity of the col liding bodies as wel l as
any changes in the velocity of other links of the multibodies
The resp onse of a multib o dy to an impulse is simpler than its resp onse to an applied
The joint accelerations dep end force Consider applying some force to a link of a multibody
on this force but b ecause of other forces due to gravity and p ossibly joint actuators and
b ecause of the velo city dep endent Coriolis and centrip etal forces the joint accelerations
are not linear functions of the applied force On the other hand when a collision impulse
is applied to the multibody it dominates all other ordinary magnitude forces that are
acting on the multib o dy including the external forces and the velo city dep endent forces
Unlike impulses ordinary magnitude forces have no e ect on dynamics over an in nitesimal
interval As a result all of the nonlinear terms drop out of the dynamic equations during
interval of collision resulting in a linear relationship b etween the applied impulse and the
the instantaneous changes in joint velo cities This implies a linear relationship between
applied impulse and change in contact p oint velo city and the linearity can be exploited
just as it was for rigid b o dy collision resp onse
For physical realism friction and non elastic b ehavior at the contact p oint should
be included in the mo del just as they were for rigid body collisions Much of physical
derivation b ehind rigid body collision resp onse can be applied directly to the multibody
case The key is computing the multib o dy collision matrix the counterpart to the rigid
b o dy collision matrix
Collision resp onse a rob otics p ersp ective
The existence of the multib o dy collision matrix can be shown using the classical
rob otics formulation of multib o dy dynamics Consider a rob ot in contact with its envi
ronment as in Figure The equations of motion for the rob ot in this situation are
f
Figure Arobot in contact with its environment
Lil
h i
T
q t H q t Q t C q t q t q t G q t J q t f t
Here q is the vector of joint p ositions H is the joint space inertia matrix C is the Coriolis
matrix and Cq contains all of Coriolis and centrip etal acceleration terms G describ es the
e ects of external forces likegravity J is the n Jacobian matrix at the end e ector f is
the external force and torque applied to the end e ector the caret is a reminder that this
is a spatial force and Q is a vector of the magnitudes of forces and torques applied
by the joint actuators All quantities on the right hand side are known or computable from
the dynamic state so this equation can be directly integrated to determine the forward
dynamics of the rob ot This equation also shows that the joint accelerations are not a
linear function of the force f applied to the end e ector
Supp ose in Figure that the end e ector is col liding with the blo ck The collision
pro duces a very large external force f acting on the multib o dy for a short p erio d of time t c
For rigid b o dies the collision time is in nitesimal consider the limit as t The change
c
in jointvelo cities during the collision is
Z
t
c
q t dt q lim
t
c
In the limit the collision force grows arbitrarily large and the terms Q t C q t q t
and G q t b ecome negligible The same assumption is made in rigid b o dy collisions when
nite magnitude forces likegravity are ignored over the duration of a collision Substituting
into the ab oveintegral and dropping negligible terms
Z
t
c
T
q lim H q t J q t f t dt
t
c
In the limit q t is a constant just as the p ositions of the b o dies are constant during
isolated rigid b o dy collisions Pulling constants outside the integral
Z
t
c
T T
q H q J q lim f t dt H q J q p
t
c
where p is the spatial collision impulse
hange in jointvelo cities The change in the end e ector velo city is related to the c
through the Jacobian
T
J q q J q H q J q p
v
T
The matrix J q H q J q on the right hand side is the inverse of the op erational
space inertia matrix develop ed by Khatib Kha This matrix describ es the apparent
mass of the end e ector to an observer exerting forces and torques on it
The rob ot is one of two bodies involved in the collision on the rob ot call the
velo city at the con tact point u The change in u is related to the change in the end
e ector s velo cities by
u v r
where r is the o set vector of the collision p oint in the end e ector frame Letting denote
the identity matrix this can b e written as a matrix equation
u r
v
T
r J q H q J q p
z W
The last equation shows the linear relationship between the collision impulse p and the
change in contact pointvelo city u they are related by a constant matrix W The
impulse p is a spatial quantity containing b oth force and torque comp onents Both com
ponents might b e useful for example when mo deling torsional friction during impact For
rigid b o dy collisions a simpler mo del is typically used only pure forces can be generated
T
at the contact p oint For this case p p where p is a pure force impulse with only
three comp onents Denoting the left sub matrix of W by K
u K p
The other body involved in the collision also exp eriences a collision impulse and
a corresp onding change in contact p oint velo city By Newton s third law the impulse
exp erienced by the second body must be p Calling the contact point velo city on the
second b o dy u one nds
u K p
by the pro cess If the second colliding body is a link of a multib o dy K can be derived
describ ed ab ove for K ifitisjustarigidbody the formulas from Chapter may b e used
Since the relative contact p ointvelo city u between the two bodies is given by
u u u
The ab ove three equations imply
p u K K
z
K
Compare this equation with its counterpart derived for simple rigid b o dies
they are identical In both cases the change in relative contact point velo city is related
to the total impulse applied at the contact point through a constant matrix Once this
connection is made the collision impulse can be found by forming the collision ODE and
p erforming a collision integration as describ ed in Section The same algorithm for
computing collision impulses b etween rigid b o dies can b e applied to colliding multib o dies
The ab ove derivation is valid for multib o dies that collide at any of their links not only the
In the current derivation the explicit dep endence of p and u on a collision parameter has b een
suppressed for clarity
at end e ectors The only change to the ab ove analysis is that the Jacobian in must
b e referenced to the colliding link
One nice feature of hybrid simulation is that some kinematic lo ops are handled
automatically Consider again the manipulator of Figure When it contacts the environ
ment it forms a kinematic lo op with the environment With a constraint based approach
this situation requires sp ecial handling the manipulator dynamics must b e combined with
the kinematic constraint imp osed at the end e ector Lil Under hybrid simulation the
contact force between the end e ector and the environment is mo deled with collision im
pulses that are propagated through the manipulator Once the collision resp onse algorithm
is develop ed no changes are needed in the manipulator forward dynamics algorithms This
same technique is used to mo del the dynamics of wheeled vehicles that form kinematic lo ops
with the ground examples are describ ed in Chapter
Articulated body collision dynamics
The previous section demonstrated the existence of the multib o dy collision matrix
K an algorithm for computing it is now develop ed Only half the problem is considered
computing the matrix K which relates the impulse applied to one of the colliding
i
b o dies to the change in contact point velo city on that body This problem is solved once
for each body and K and K are combined to form the nal multib o dy collision matrix
K as in One way to compute K has been shown already it is the left half of the
i
matrix
T
J q H q J q W r
Computing this pro duct explicitly however results in an O n algorithm for an n link
multib o dy due to the inversion of H q Since collisions o ccur so frequently in an impulse
based simulation computing K e ciently is imp ortant By studying the dynamics of
i
articulated b o dies of the full multib o dy during a collision an O n algorithm for computing
K may b e derived The algorithm will b e derived for tree like multib o dies these include
i
serial multib o dies as a sub class
Consider the dynamics of a tree like articulated body during a collision From
Z Z Z
t t t
c c c
I
A A
Z dt I a dt lim dt lim f lim
h
h h h
t t t
c c c
In the limit as t approaches zero the collision force acting on the body is in nite which
c
gives rise to in nite collision forces acting through joints of the linkage Call the limit of
I
the integral on the left hand side p the spatial impulse exerted on link h through the
h
A
inb oard joint The articulated inertia I dep ends on link masses and joint p ositions which
h
are constantast thus
c
Z Z
t t
c c
A A
I
a dt lim p I Z dt
h
h h h
t
c
Z
t
c
A A
I v lim Z dt
h
h h
t
c
The quantity v is the total change in spatial velo city of link h during the collision it is
h
the limit of in nite accelerations integrated over an in nitesimal interval The integral of
z a forces contain some terms which vanish in the limit and others which remain From
the last term in the ab ove equation is
i h
A A A
Z
m
t
Z I c I Q s s
X c
i i i
i i i i
j j j
A A
j j j j
Z dt X lim Z I c
i i
h h
i i
j j
j j
A
t
c
I s s
j
i
i i
j
j j
Velo cities remain nite in magnitude during a collision therefore so do the spatial Coriolis
forces c The joint actuators can not apply impulses so the Q are also nite The
i i
j j
A
quantities X I and s are all constant as t Pulling these constants outside the
i i c
h
i
j j
j
integrals and dropping the nite terms that vanish in the limit
A
Z Z Z
m
t t t
c c X c
s s I
i
A A
i i
j
j j
lim Z dt lim Z dt lim X Z dt
i
h h
h i
j
j
A
t t t
c c c
s I s
j
i
i i
j
j j
where is the identity matrix De ning the isolated and articulated zero acceleration
z a impulses as
Z
t
c
Y lim Z dt
h h
t
c
Z
t
c
A A
Y lim Z dt
h h
t
c
b ecomes
A
m
X
s I s
i
A A
i i
j
j j
Y X Y Y
i
h h
h i
j
j
A
I s s
j
i
i i
j
j j
Lemma Consider atree linkage that experiences acol lision impulse at link k Let p
col l
be the spatial vector comprising the impulsive col lision force and torque resolved at the
center of the body frame of link k Then the isolated z a impulses of the links are given by
i k
Y
i
p i k
col l
Proof By de nition Z is the force applied at the inb oard joint that cancels out the Cori
i
olis gravitational and external forces acting on link i when link i is considered detached
from the rest of the linkage Y is the integral of this force over time as t The force
i c
needed to opp ose the nite magnitude Coriolis and gravitational forces may be ignored
since they vanish in the limit If i k than there are no other external forces acting on
the link and so Y If i k then Z must also cancel the spatial collision force This
i i
force b ecomes the impulse p in the limit thus Y p
i
coll coll
Consider a tree linkage that experiences a col lision impulse at link k If link k Lemma
A
is not in the subtree rooted at link h Y
h
Proof The pro of is by induction on the height of the subtree ro oted at link h If the
A
height is link h is a leaf link with no descendents By Y Y Since h k
h
h
by Lemma Supp ose now the height of link h is greater than Index link h s Y
h
children i i Each of the children is the ro ot of a subtree of lesser height which also
m
A
do es not contain link k By the inductive hyp othesis Y j m From
i
j
A
Y Y
h
h
Lemma implies that articulated z a impulses need only b e calculated along the
chain from the colliding link k up to the base link the articulated z a impulse for all other
A A
for all children i of the colliding link k Y Y Figure Since Y links is
j
k
k i
j
by By Lemma
A
Y p
k
coll
Now consider link h a prop er ancestor of link k It has exactly one child link which
is a not necessarily prop er ancestor of link k any other children are ro ots of subtrees which
A
by Lemma Thus only one do not contain link k For these other children i Y
j
i j
1 propagation of z.a. impulses 2
3 89 colliding link 4 7 10
56 11 12
Figure The z a impulses need only be computed along the path from the col liding link
to the base of the linkage The gray links are not the col liding link or ancestors of it for
these links the articulated z a impulse is
term of the summation in is non vanishing Since h k Y by Lemma The
h
simpli ed expression for the articulated z a impulse of link h a prop er ancestor of k is
A
I s s
A A
i
i
i
Y X Y
h
i
h i
A
s I s
i
i i
where i is the unique child of link h whichisonthepathfromh to k
Computing the articulated z a impulses is half of the work it is also necessary to
compute the instantaneous changes of velo city due to a collision impulse This is also done
by taking the limit of dynamic equations for articulated b o dies From
A A A
Z Z
t t
I c X a s Z I Q s
c c
i i i
h h
i i i i i
lim dt q dt lim
i
A
t t
c c
s I s
i
i i
A
X and s are constants that can The nite magnitude Q and c vanish in the limit I
i i i i
h i
b e pulled outside of the integral The resulting change in link velo cityis
h i
s
A A
i
q I X v Y
i i
h h
i i
A
s I s
i
i i
where v is the limit of the in nite acceleration a integrated over an in nitesimal interval
h h
The change in link i s spatial velo city is obtained by applying the same technique to
Z Z
t t
c c
lim a dt lim dt X a q s c
i i i i i
h h
t t
c c
and simplifying Again dropping terms that vanish as t
c
X v q s v
i i i i
h h
At this point a pro cedure can be describ ed for determining the instantaneous
change in a link s velo city when a spatial collision impulse P is applied to it
coll
Compute the articulated inertias of all links of the multib o dy
Compute the articulated z a impulses of the chain b eginning at the colliding link
and ending at the base link Equations
Propagate instantaneous changes in joint and link velo cities along the path b eginning
at the ro ot link and ending at the colliding link Equations
Although most pro cessing o ccurs along the path from the base link to the colliding link the
e ects of the other links enter through the articulated inertias of the path links An algo
rithm for implementing the strategy is shown in Figure Step is not included b ecause
articulated inertias of all links are assumed available from the last dynamic integration
Computing K
i
of Figure relates a spatial collision impulse The impulseResponse algorithm
to a spatial change in velo city But the collision matrix K relates non spatial quantities
i
Figure depicts the body frame F of the colliding link as well as the collision frame
k
F Let R be the rotation matrix taking vectors in F to vectors in F and let
k
coll coll
r b e the vector from the origin of F to the origin of F in F s co ordinates In frame
k k
coll
F there is an impulsive force p R but no impulsive torque In frame F there is an
k
coll
r Rp or in spatial notation impulsiveforce Rp and an impulsive torque
T
p R
p
coll
T T
rR R
impulseResponse
A
Y p
k
coll
i k
while link i has a parent
h index of parent of link i
A
s s I
A A
i
i i
Y Y X
i
h
i h
A
s I s
i
i i
i h
a
h
repeat
i index of child of link h on path to link k
i h
s
A A
i
I X v Y q
i i
h h
i i
A
s s I
i
i i
v X v q s
i i i i
h h
h i
until i k
Figure impulseResponse Compute v the instantaneous change in spatial velo city
k
of link k when it exp eriences a collision impulse p This routine assumes that the
coll
articulated inertias of all links are already computed
The matrix is just the spatial transformation X This equation shows how the
k col l
non spatial impulse p is mapp ed into a spatial impulse p The spatial change in velo city
coll
T
v v computed by impulseResponse must then be converted into a non
k k k
Since and v are expressed in frame F spatial change in velo city at the contact p oint
k k k
the change in velo city at the contact p ointis
T
v r u R
k k
T T
r R v R
k k
link k
y Fcoll
y z x p r Fk x
z
Figure The body frame of a col liding link and the col lision frame
This is just the lower comp onent of the spatial pro duct
T
R
k
T T
R r R v
k
The upp er comp onent of the pro duct is the change in angular velo city at the con
tact point which is not used in the collision mo del It is easily veri ed that the spatial
transformation matrix in the ab ove equation is X the spatial transformation from F
col l k k
to F
coll
The ab ove observations along with the algorithm impulseResponse provide a
waytocompute u R the change in contact p ointvelo city given p R an impulse
applied at the contact p oint But the matrix K relates changes in velo city to an arbitrary
i
collision impulse Since velo city changes at the contact point are linear in the applied
collision impulse it is only necessary to compute the resp onse to asetof basis impulses at
the collision point Let R R be the mapping from applied impulse to change in
contact pointvelo city An arbitrary impulse p may b e decomp osed as
p p e p e p e
x x y y x z
where e e and e are the canonical basis vectors of the collision frame Then
x y z
p p e p e p e
x x y y z z
By building the matrix K as
i
K
e e e
i
x y z
it follows that
p K p
i
for any p R Thus the impulse resp onse of the colliding body may be completely
characterized by the impulse resp onse to three test impulses each a unit vector in one of
the canonical basis directions
Finally the algorithm for computing K may be completely written It is shown
i
in Figure After K and K are computed the complete multib o dy collision matrix is
given by their sum Once K is known a collision integration can b e p erformed to determine
the collision impulse whether either of the colliding b o dies are multib o dies or simple rigid
b o dies all of the necessary dynamic information is enco ded in K
Propagating impulses through multib o dies
impulse p is determined it must be propagated through the After the collision
multib o dy to determine the instantaneous change in jointvelo cities The machinery to do
this has already b een develop ed The algorithm propagateImpulse Figure is similar
to the impulseResponse algorithm except that the e ects are propagated throughout the
entire tree linkage rather than just along the path to the colliding link The iteration is
p erformed by enumerating over the joints in increasing order as in the forward dynamics
algorithms of Chapter
Figure summarizes the entire pro cess of resolving a collision that involves
multibody collide the a link of a multib o dy When and n link and an n link two
compMultibodyKi
compute the articulated inertias for all links of the multibody
X from R and r X and compute
k col l col l k
for i to
e
i
X p
col l k
coll
call impulseResponse to compute v
k
lower component of X v
col l k k
i
K
i
Figure compMultibodyKi Compute K half of the collision matrix corresp onding to
i
one of the b o dies in the collision Inputs are k the index of the colliding link and R and
r which sp ecify the relative p ositions of the collision and b o dy frame
total cost of pro cessing the collision is O n n C where C is the cost of p erforming
a collision integration A linear time multib o dy collision resp onse algorithm allows the
e cientsimulation of large articulated structures in an impulse based setting
For oating linkages the same adjustment that was made for the forward dynam
ics algorithm must be made in the collision resp onse algorithms No change is required
hanges for articulated inertia and z a impulse propagation When computing velo city c
however the base link must b e handled as a sp ecial case Since there is no inb oard impulse
applied to the base link
A A
Y v I
For all joints and other links velo citychange propagation needs no adjustment
With simple rigid b o dies the collision matrix is always p ositive de nite however
the multib o dy collision matrix is only guaranteed to be p ositive semide nite Thus the
invertibilityof K b ecomes an issue Figure shows an example where K can b e singular
propagateImpulse
A
Y p
k
coll
i k
while link i has a parent
h index of parent of link i
A
s s I
A A
i
i i
Y Y X
i
h
i h
A
s I s
i
i i
i h
A
for all links i not on path from base link to link k Y
i
a
for i to n
h index of link inboard to joint i link i is outboard
h i
s
A A
i
q I X v Y
i i
h h
i i
A
s I s
i
i i
v X v q s
i i i i
h h
Figure propagateImpulse Compute the instantaneous change in spatial velo city of
joints of a tree linkage when link k exp eriences a spatial collision impulse p p is
coll coll
computed from p via Equation This algorithm assumes that the articulated inertias
of all links are already computed
The two link multib o dy falls and collides with the xed horizontal plate Because there are
only two degrees of freedom in the multib o dy motion no matter what impulse p is applied
at the contact p oint the total change in relative contact p oint velo city u can have no
comp onent in the y direction Since u Kp the matrix K only has rank two But
K the collision integration algorithms require K This is not a problem in practice Let
b e the canonical inverse of K obtained by singular value decomp osition SVD PTVF
Then K u gives the minimum norm p that causes the given u thep computed in this
manner has no comp onent in the null space of K This gives the exp ected results in the
1 1 A B z.a. impulses articulated 2 2 inertias
3 89 3 89
4 7 10 test impulse 4 7 10
56 11 12 56 11 12
1
C velocity D changes 2
3 89 collision K p integration 4 7 10
56 11 12
1 1 E F z.a. impulses velocity 2 changes 2
3 89 3 89
4 7 10 4 7 10
p
56coll 11 12 56 11 12
Figure The entireprocedure for resolving col lisions involving a multibody here link
is the col liding link A Propagate articulated inertias This step can be eliminated by
saving inertias from the forward dynamics algorithm B Apply test impulse to the col liding
link and propagate z a impulses up to the base link C Propagate joint and link velocity
changes back to the col liding link Steps B and C areperformedthree times for test impulses
in the x y and z directions afterwards K is known K is found similarly and these are
combined to give K D Col lision integration determines p E Propagate z a impulses
from p up to base link F Propagate velocity changes throughout the tree
col l
collision of Figure the collision impulse will lie in the x z plane The same approach can b e used when the collision matrix has rank one
z y x
p
Figure A situation in which the multibody col lision matrix is rank de cient
Supp orting control systems
To animate multib o dies it is usually necessary to apply control forces or torques
at the joints in order to achieve the desired physical motion For the rest of this section
force is used in a generalized sense to mean force or torque dep ending on whether a joint
is prismatic or revolute Using the collision check scheduler that is already present in an
impulse based simulator a exible control architecture can b e built on top of the dynamic
simulator
Typ es of controllers
There are several typ es of controllers that can be used to control m ultib o dies
More information than app ears here can b e found in Cra Many systems contain passive
elements such as springs and damp ers attached at the joints of a multib o dy These are not
controllers in a strict sense however they in uence the dynamics of the system in a similar
way They may b e undesirable but unavoidable characteristics of the system such as friction
in joint b earings or they may b e elements intentionally added to the system by a designer
such as the sho ck absorb ers on a car Passive controllers are characterized by the force f
applied given the p osition q and velo city q of a joint such as
f sgn q
f q
f k q q
n
c
f
q q
n
In order these equations corresp ond to constant friction viscous damping a linear spring
and a hard spring which arises for example in a sealed prismatic cylinder this latter force
has b een exploited for hopping rob ot control Rai
Passivecontrollers are most accurately mo deled by placing the force computations
within the inner lo op of the dynamic integration For example every time the integrator
calls treeFwdDyn to compute q passivecontrollers forces are recomputed based on q and
q These control forces may be lump ed in with actuator forces Q since they mo dify the
dynamics in the same way The control laws for these passive elements are simple so
even in the inner integration lo op the cost of recomputing the control forces is a negligible
part of computing the forward dynamics
At the lowest level of a control system are elements whichcontrol external forces to
ts in order to maintain desired joint p ositions or velo cities Prop ortional b e applied at join
derivative PD or prop ortional integral derivative PID controllers are two common typ es
PD controllers are used for example in the human athlete mo dels of Ho dgins et al
HWBO The basic op eration of a lowlevel controller is depicted in Figure
Supp ose a particular joint is to be maintained at some desired p osition q
des
One approach would be to attach a spring with natural length q to the joint The
des
sti er the spring the harder the joint would be forced to the desired value To prevent
oscillations a damp er could also be added to the system A p ositional PD controller
mimics the b ehavior of this spring damp er system In control parlance the spring constant
and damping constant are called k and k resp ectively p for p osition and v for velo city
p v
The actuator force calculation for a p ositional PD controller is given by
Q k q q k q
p v des
actuator forces/torques Q
physical low level q system controller des or
qdes or q q q , , qdes
joint positions, velocities, & accelerations
Figure The coupling of a low level control ler to a multibody system The control ler
takes as inputs the position velocity and possibly acceleration of a particular joint and
computes a torque to be applied at that joint based on the control law and desired motion
In some cases a PD controller is insu cient for maintaining q Consider the
des
prismatic joint in Figure It is easily veri ed that the external force results in an
equilibrium p osition of
mg
q
des
k
p
The static error is made arbitrarily small bycho osing a very large k however this can lead
p
to instability in the system and numerical sti ness in the equations Instead a prop ortional
derivative integral PID controller can b e used This typ e of controller augments the spring
and damp er comp onents with an integral term that is based on the static error accumulated
over time
Z
t
q q d Q t k q t q k q t k
p v i
des des
A PID controller provides eventual convergence to the q in cases such as the one in
des
Figure although they are more di cult to analyze and intro duce new stability concerns
Another typ e of low level controller is a computed torque controller which shields
a higher level controller from the internal dynamics of a system The dynamics of a p oint
mass are trivial To make the mass s acceleration equal to a a force f ma is applied For a
multib o dy the dynamics are more complicated and joint accelerations are not prop ortional
to joint forces A computed torque controller hides this complexity so that a higher level
controller can directly sp ecify the desired accelerations of the system If the desired joint
mg
qdes
kp
Figure APDcontrol ler with position constant k causes the unweighted joint to attain
p
a value of q butit can not do so when the joint is loaded with a static weight
des
accelerations of a multib o dy system are q the computed torque lawis
des
T
Q H q q C q q q G q J q f
des
Substituting into Equation shows that with these joint forces q q A computed
des
torque controller requires much more computation than PD and PID controllers since a
dynamic mo del of the multib o dy must b e analyzed to determine the joint forces
Unlike passive elements like springs controllers run at nite rates A controller is
up dated with new values of q and q at regular intervals and it commands actuator forces
which will p ersist until the next up date even though q and q change over the interval The
up date rate must b e fast enough so that the controller has reasonably current information
otherwise instability results The required rate dep ends roughly on the mass of the system
and on the controller gains Less massive systems and higher gains require higher up date rates
High level controllers are needed to give multib o dy systems intelligent behavior
While a lowlevel controllers might allowahuman gure to stand a high level controller is
necessary to makeitwalk toward a goal High level controllers are less concerned with the
dynamic stability of the system and more concerned with the overall b ehavior They run at
rates slower than the rates of the lowlevel controllers For example a rob otic controller that
examines machine vision input would have a fairly slow up date rate High level controllers
are more varied than low level controllers and there are few canonical typ es Finite state
machine controllers have enjoyed recent success as high level controllers for hopping rob ots
Rai and mo dels of human athletes HWBO These track the phase of the o verall
system such as ight or loading and send commands to lowlevel PD controllers based on
the phase Use of state machines for higher level b ehavior control is describ ed by Bro oks in
the context of rob ot planning Bro and by Ahmad et al for scenario control ACH
Controller scheduling
There maybemany di erentcontrollers for a system running in parallel at di er
ent rates In simulation controllers are software routines that compute applied forces from
the current state These routines must b e called to recompute the control forces at arate
matching that of the real controller they mo del Scheduling control up date events is simple
in an impulse based simulator As discussed in Chapter the integration intervals are
determined by the collision heap The system is integrated to the p oint in time indicated
top heap element at which point a collision check is p erformed by the TOI eld of the
between two b o dies The simulator guarantees that no heap events are missed
Control up date events can be added to the collision check events in the heap
Supp ose a PD controller is to up date joint torques at a kHz rate A structure for this
control event is placed into the heap with a time eld set to the current time plus
seconds Using the same scheduling algorithm the simulation will b e stopp ed at the appro
k priate time and the PD up date pro cedure can b e called The control event is placed bac
into the heap with the time eld incremented by s In this way the simulator can
manage avarietycontrollers running indep endently and at di erent rates Adding control
up date events to the heap do es not signi cantly a ect the size of the integration steps
collision check events still dominate the top of the heap For simulations with a only few
b o dies collision checks can b e made at rates on the order of kHz This is far higher than
typical controller frequencies The passivecontroller forces are computed within the inner
integration lo op so they are not scheduled in the heap
Impulse s control supp ort architecture
Impulse shields the designer of a control system from the inner workings of the
control lawscheduler and the dynamic integration lo op through a layer of interface routines
Standard typ es of low level controllers are provided The designer writes high level control
routines as C functions that are compiled and linked with the rest of the simulator co de
When a high level controller is instantiated it is registered with the scheduler so that it
may be called back at the desired rate The high lev el control routine can instantiate low
level controllers and interact with them Impulse s control library contains
Routines to create passiveelements and attach them to sp eci ed joints of a multib o dy
Parameters like spring constants and damping co e cients are user sp eci ed
Routines to create low level controllers PD and PID controllers are available in
b oth p osition and velo city control varieties Computed torque controllers are also
provided The rates and gains of these controllers are sp eci ed when they are created
Communication with these controllers is handled through routines that command
desired p ositions velo cities or accelerations to an existing controller and disable or
enable the controller Ahighlevel controller typically calls these interface routines
Routines to execute general high level controllers at sp eci ed rates through a call back
mechanism The high level controller routines currently C functions are written by
the designer When called these high level controller routines are passed a p ointer to
the ob ject b eing controlled so that the same control function can be used for many
clones of a particular ob ject
With these library routines one can build up interesting control systems with a
few lines of C co de Figure is a system level view of the control supp ort architecture
The lowest level is the simulation engine whichcontains mo dules to compute the dynamics
of rigid bodies and multib o dies and to p erform collision detection and collision resp onse
between b o dies On top of this layer is the control library describ ed ab ove The designer
sp eci es high level controllers on top of the library A high level controller may do nothing
tiate some lowlevel controllers and send them initial commanded values more than instan
scheduled
high level p, v control f
control computed P D P I D library torque
simulation collision dynamics collision
engine detection engine response
Figure The control support architecture of Impulse Lower level control lers are pro
vided on top of the simulation engine and the designer writes the high level control lers
The dotted line indicates control lers that are scheduled in the heap
Or it can p erform complex computations involving the state of the system and environment
to determine b ehavior The designer is free to create new typ es of lowlevel controllers or a
control layer b etween the levels shown Examples that use this control supp ort architecture are describ ed in Chapter
Chapter
Computation of Mass Prop erties
for Polyhedral Bo dies
For dynamic simulation involving rigid b o dies it is necessary to know several pa
rameters describing the mass distribution of these b o dies A common formulation comprises
ten parameters the total mass a scalar the lo cation of the center of mass three param
eters and the moments and pro ducts of inertia ab out the center of mass six parameters
One can always nd a b o dy frame with origin at the b o dy s center of mass and axes aligned
with its principle axes of inertia In this way theentire mass distribution can b e describ ed
with a reduced set of four parameters however the larger parameterization is still needed
as a starting p oint
The problem of computing mass prop erties of solid ob jects has been studied ex
tensively Lee and Requicha note that the algorithms are closely tied to the underlying
representation of solid ob jects and give an excellentsurvey of the various families of algo
rithms in existence LR a Representations based up on primitive instancing and simple
sw eeping have small domain of application only simple canned ob jects may b e describ ed
Algorithms based on decomp osition fall into two categories according to whether the de
comp osition is exact or approximate Exact decomp ositions are di cult to obtain for general
ob jects and usually require considerable human lab or LK Approximate decomp ositions
such as spatial enumerations or octree metho ds may be easier to generate but yield only
estimates of the desired integral prop erties LR b The resolution of these representations
determine the accuracy of the estimate and errors in the representation tend to dominate
numerical errors LR a Algorithms that op erate directly on CSG representations are
computationally exp ensive indeed the b est algorithms for CSG op erate by rst converting
the CSG representations into approximate cell decomp ositions Integrals over solid regions
can also be evaluated using Monte Carlo techniques based on sampling p oints inside a
volume containing the solid Accuracy increases with the number of sample points but
convergence is slow and long execution times are required for accurate results LR a
Algorithms based on b oundary representations Breps remain the metho d of
choice for exact e cient computation of integrals over solids This is fortunate since most
graphical rendering algorithms require Breps as do many e cient collision detection algo
rithms Lin Van Thus for applications such as dynamic simulation Breps are likely
to be a vailable Lien and Ka jiya give an algorithm for computing integrals over arbitrary
nonconvex p olyhedra based on a Brep LK It is O n in the p olyhedron complexity and
works by pro jecting triangular faces to the origin forming tetrahedra and summing the
contributions of these tetrahedra to the comp osite integrals Numerical errors increase in
near degenerate situations where the tetrahedra are long and thin These errors are reduced
by shifting the pro jection origin to the barycenter of the ob ject however tetrahedra of high
asp ect ratio can still o ccur esp ecially for highly tessellated ob jects Also the algorithm
essentially triangulates any non triangular faces of the original p olyhedron and the manner
in which this is done may also generate long thin tetrahedra Nevertheless the algorithm
prop osed by Lien and Ka jiya is fairly easy to co de and generalizes to higher dimensions It
i j k
ver the solid region in may b e easily adapted to compute anyintegral of the form x y z o
addition to the integrals arising when computing mass prop erties
This chapter presents an exact algorithm for computing mass prop erties of uniform
density p olyhedra based on a b oundary representation of the solid Like Lien and Ka jiya s
algorithm it is exact and linear in the number of vertices edges or faces of the p olyhedron
The p olyhedron may be nonconvex and the faces may have any number of sides The
only assumptions made on the representation are that one can enumerate over all faces of
the b oundary and that given a face one can enumerate over all edges in counterclo ckwise
order In contrast to Lien and Ka jiya s algorithm the algorithm is optimized for computing
the mass prop erties required for dynamic simulation namely mass center of mass and
during a moments and pro ducts of inertia These quantities are all computed in parallel
single traversal of the p olyhedron so that common sub expressions are exploited it is very
fast In addition the algorithm presentedhereadaptively changes the pro jection direction
minimizing degeneracy and reducing numerical errors over those in Lien and Ka jiya s and
other algorithms
Rigid b o dy mass parameters
See App endix A for an overview of rigid b o dy dynamics Key quantities in rigid
b o dy dynamics are a b o dy s mass m center of mass r and inertia tensor I also called the
o
mass matrix In order to formulate the equations of motion for the body these quantities
must b e determined The initial problem may b e expressed as follows
Problem Given A rigid body comprising N parts B B each a uniform density
N
polyhedron There are no restrictions on the convexity or genus of the polyhedra nor on
the shape of the bounding faces For e ach polyhedron B either its density or mass m is
i i i
speci ed and the geometries of al l of the polyhedraarespeci edrelative to a single reference
frame Compute The mass m and the referenceframe coordinates of the center of mass r
and inertia tensor I for the entire rigid body
o
The mass m and density of p olyhedral part B are related by m V where
i i i i i i
V is the volume of the p olyhedron Assuming one can compute
i
Z
V dV
i
B
i
P
N
the masses and densities of each p olyhedron can b e found The total mass is m m
i
i
The co ordinates of the center of mass r for the entire b o dy are
Z Z Z
N
T
X
r xdm y dm z dm
m
B B B
i i i
i
The integrals ab ove are the sums over all di erential mass elements of the body but for
uniform density p olyhedra these are easily converted to volume integrals For body B
i
dm dV Thus
i
Z Z Z
N
T
X
z dV y dV xdV r
i
m
B B B
i i i
i
The inertia tensor is a symmetric matrix containing the moments and pro ducts
of inertia
I I I
xx xy zx
I
I I I
xy yy yz
I I I
zx yz zz
The moments of the inertia are given by
Z Z
N N
X X
I y z dm y z dV
i
xx
B B
i i
i i
Z Z
N N
X X
I z x dm z x dV
i
yy
B B
i i
i i
Z Z
N N
X X
I x y dm x y dV
i
zz
B B
i i
i i
Finally the pro ducts of inertia are given by
Z Z
N N
X X
xy dm xy dV I
i
xy
B B
i i
i i
Z Z
N N
X X
I yz dm yz dV
i
yz
B B
i i
i i
Z Z
N N
X X
I zx dm zx dV
i
zx
B B
i i
i i
not exactly the values app earing in the inertia The primed inertia quantities ab ove are
tensor The primed values are computed relative to the given reference frame but the
values in the inertia tensor must b e computed relative to a frame with origin at the center
of mass One way to accomplish this is to rst compute the lo cation of the center of mass
in the given frame and then to apply a translation to the body which brings the center
of mass to the origin After p erforming this transformation the primed quantities can be
directly inserted into the inertia tensor
A b etter solution is to use the transfer of axis relations for transferring a moment
or pro duct of inertia ab out one axis to a corresp onding one ab out a parallel axis To transfer
the primed values computed ab ove to a frame at the center of mass one uses MK
I I m r r
I I mr r
xx
xy x y
xx y z
xy
I I m r r
I I mr r
yy
yz y z
yy z x
yz
I I m r r
I I mr r
zz
zx z x
zz x y
zx
The unprimed quantities are inserted into the inertia tensor If the transfer of axis relations
are used it is not necessary to explicitly transform the vertices of the p olyhedron after
computing the center of mass hence all of the integrals can b e computed simultaneously
Although not explicitly allowed in the problem statement p oint masses placed at
various lo cations on the rigid b o dy are easily handled Supp ose the comp onent body B is
i
a p oint mass rather than a p olyhedron lo cated at p osition r in the reference frame All of
i
the required mass integrals can still b e computed since for a p ointmass
Z
f dm m f r
i i
B
i
where f is any scalar function of x y and z This substitution may b e used to compute all
integrals over B
i
Rigid b o dy dynamics can b e computed more e ciently with a reduced set of mass
prop erties based on a b o dy frame This is a co ordinate system attached to the body with
origin at the body s center of mass and axes aligned with the b o dy s principle axes of
inertia In this case all of the pro ducts of inertia vanish and the inertia tensor is diagonal
To compute the b o dy frame one must rst compute r and I as ab ove and then nd and
o
a rotation matrix R such that
T
I R I R
o
is diagonal In this case I is the diagonalized mass matrix and R is the rotation matrix
transforming vectors in the body frame to vectors in the original reference frame the
columns of R are the i j and k unit vectors of the b o dy frame expressed in the reference
co ordinates The diagonal elements of I are the moments of inertia ab out the principle axes
of the b o dy Since these are p ositive I and I are p ositive de nite Diagonalizing a matrix
o
that is nding R is a classical problem of linear algebra The Jacobi metho d works quite
well for this application since I is real symmetric and of mo derate size See PTVF for
o
a complete discussion of this technique as well as an implementation
Derivation of the algorithm
From the previous section all required mass prop erties can be found from ten
integrals over volume for each of the individual p olyhedral comp onents To simplify no
tation the p olyhedron index is henceforth dropp ed and only a single p olyhedral b o dy is
considered The domain of integration is written V as a reminder that it is a volume The
remainder of this chapter describ es an e cient exact algorithm for calculating these ten
integrals
Z
Z Z
T dV
V
Z
T x dV T xy dV
xy
x
V V
Z Z
T xdV
x
V
Z
T y dV T yz dV
yz
y
V V
Z Z
T y dV
y
V
Z
T z dV T z x dV
zx
z
V V
T z dV
z
V
Note that each isanintegral of a monomial in x y and z The basic idea is to use
the divergence theorem to reduce each of the volume integrals to a sum of surface integrals
over the individual faces of the p olyhedron Eac h of these surface integrals are evaluated
in terms of integrals over a planar pro jection of the surface For p olygons in the plane
Green s theorem reduces the planar integration to a sum of line integrals around the edges
of the p olygon Finally these line integrals are evaluated directly from the co ordinates of
the pro jected vertices of the original p olyhedron Figure illustrates the approach the
volume integral is ultimately reduced to a collection of line integrals in the plane and the
values from these integrations are propagated back into the value of the desired volume
integration
integral reduction
divergence projection Green’s theorem theorem
value propagation
Figure The strategy for evaluating volume integrals Complicated integrals are decom
posed into successively simpler ones and the values from evaluating the simplest integrals
are combined and propagated back to evaluate the original ones
Reduction to surface integrals
The rst reduction is from an integral over the three dimensional p olyhedral vol
ume to a sum of integrals over its two dimensional planar faces This reduction is easily
accomplished with the divergence theorem Ste
Theorem Divergence Let V bea region in space bounded by the surface V Let n
denote the exterior normal of V along its boundary V Then
Z Z
r F dV F n dA
V V
for any vector eld F de ned on V
For a p olyhedral volume the right hand side of can be broken up into a
summation over faces of constant normal so n can be pulled outside the integral The
R
integrals to be computed for example x dV do not immediately app ear to be of the
V
form in the theorem But one can nd manyvector elds whose divergence is the function
T
x a particularly simple choice is F x y z x This choice has the added
advantage that two of its comp onents are identically zero so that the dot pro duct on the
right hand side of b ecomes a scalar multiplication By making similar choices for the
other integrals and applying the divergence theorem Equations b ecome
Z
X
T n x dA
x
x
F
F V
Z Z
X X
n xdA T n y dA T
x y
y
F F
F V F V
Z Z
X X
T n x dA T n z dA
x x z
z
F F
F V F V
Z Z
X X
T T n n y dA x y dA
y xy y x
F F
F V F V
Z Z
X X
n n z dA y z dA T T
z y z yz
F F
F V F V
Z
X
n T z xdA
z zx
F
F V
Reduction to pro jection integrals
Green s theorem reduces an integral over a planar region to an integral around its
one dimensional b oundary however one must start with a region in the plane Although
the planar faces of the p olyhedron are in three space one can pro ject them onto one of
the co ordinate planes The next theorem relates integrations over the original face to
integrations over the pro jection
Theorem Let F bea polygonal region in space with surface normal n and lying
in the plane
n n n w
Let be the projection of F into the plane Then
Z Z
dA f f h d d
jn j
F
where
h n n w
n
Proof The p oints h lie in the plane of F soF is the graph of h over From
EP Section Formula
s
Z Z
h h
d d f dA f h
F
The square ro ot in the integrand reduces to jn j and the theorem follows
This theorem provides the reduction of the integral of a p olynomial in and
over the face F to the integral of a p olynomial in and over the pro jected region
From the constant w can b e computed w n p where p is any p oint on the face
F Numerical inaccuracy or oating point errors can occur when the face normal n has
little or no comp onent in the pro jection direction in the extreme situation n the
face pro jects to a line segment To reduce such errors for a given face the co ordinates
are always chosen as a right handed permutation of the of the x y z co ordinates such that
jn j is maximized This choice maximizes the area of the pro jected shadowinthe plane
p
see Figure Note that a choice can always b e found such that jn j
Recall the needed integrals over the region F Indep endent of the three
p ossible corresp ondences between the x y z and co ordinates they all can be found
This means
by computing the following integrals
Z Z Z Z
dA dA dA dA
F F F F
Z Z Z Z
dA dA dA dA
F F F F
Z Z Z Z
dA dA dA dA
F F F F
Using Theorem these face integrals can all b e reduced to integrals over the pro jection
region For instance
Z Z
n n w
dA jn j d d
n
F
Z Z Z
jn j n n d d n d d w d d
jn j n n n w
where
Z
f dA
f
The complete set of face integrals reduced to pro jection integrals with Theorem
is shown b elow
Z
dA jn j
F
Z
dA jn j
F
Z
dA jn j n n n w
F
Z
dA jn j
F
Z
dA jn j
F
Z
dA jn j n n n n n n w n w w
F
Z
dA jn j
F
Z
dA jn j
F
Z
n n n n n n n w dA jn j n
F
n n w n w n w n w w
Z
dA jn j
F
Z
n n w dA jn j n
F
Z
dA jn j n n n n n n w n w w
F
Reduction to line integrals
The nal step is to reduce an integral ov er a p olygonal pro jection region in the
plane to a sum of line integrals over the edges b ounding the region The following notation
is used see Figure The edges of are lab eled E through E Edge E is the directed
K e
line segment from to and note
e e e e e e e e e e
that Finally edge E has length L and exterior unit normal m
K K e e e
Green s theorem Ste provides the nal integration reduction
Sometimes more formally called Green s theorem in the plane Additionally some texts call this Green s
lemma reserving Green s theorem for a more general D result WB
γ n
F
β
Π
α
Figure The axes are a right handed permutation of the x y z axes chosen to
maximize the size of the face s projected shadow in the plane
Theorem Green s Let be a region in the plane bounded by the single curve
Let m denote the exterior normal along the boundary Then
Z I
r H dA H m ds
de nedon where the line integral traverses the boundary counter for any vector eld H
clockwise
This theorem is a two dimensional version of the divergence theorem and the p olyhedral
application again provides simpli cation Since is p olygonal the right hand side of
maybebroken into a summation over edges of constant normal and byalways cho osing H
so that one comp onentisidentically zero the dot pro duct b ecomes a scalar multiplication
From and all integrals of the form
Z
p q
p q
dA
β (α , β ) (α , β ) 1 1 K K ε K ε 1
(α , β ) Π e+1 e+1 ε e ∆β e m e (α , β ) ∆α e e e
α
Figure Notation for computing a projection integral as a sum of line integrals
are needed for nonnegative integers p and q with p q Consider rst the case q
p T
By cho osing H and applying Green s theorem to the p olygonal region
p
Z Z
K
X
p p
m dA s ds
e
p
E
e
e
In the right hand integral the integration variable s is arc length and runs from to L
e
the length of the edge s is the co ordinate of the p oint on the edge that is a distance s
from the starting p oint Letting the variable be s L one can change integration variables
e
ds L d toget
e
Z Z
K
X
p p
m L L d dA
e e e
p
e
Now m L where the plus or minus dep ends on whether the vertices of are
e e e
indexed in counterclo ckwise or clo ckwise order resp ectively By convention assume that the
vertices of the original face F are indexed in counterclo ckwise order thus the vertices of
will b e indexed in counterclo ckwise order exactly when the sign ofn is p ositive Figure
helps in visualizing this situation Hence m L sgn n and
e e e
Z Z
K
X
sgn n
p p
L d dA
e e
p
e
p T
The case with q is similar except one cho oses H Finally one can
p
derive analogous equations for the cases when p or p The results are
Z Z
K
X
sgn n
q q
L d dA
e e
q
e
Z Z
K
X
sgn n
p p
dA L L d
e e e
p
e
Z Z
K
X
sgn n
q q
dA L L d
e e e
q
e
Evaluation of integrals from vertex co ordinates
The original volume integrals have been successively reduced to face integrals
pro jection integrals and nally line integrals The latter can b e directly evaluated in terms
of vertex co ordinates with the help of the following theorem
Theorem Let L be the length of the directedlinesegment from to
e e e e e
Let s and s bethe and coordinates of the point on this segment a distance s from
the starting point Then for nonnegative integers p and q
p q
A A
Z
p q
X X i j
j
p q i p i q j
L L d
e e
e e e
e
p q
p q
i j
A
i j
Proof Denoting the integral on the left hand side by I
Z
p q
I d
e e e e
Z
p q
X X
j p q
q j i p i
d B B
e e e
e
i j
i j
where the B s are Bernstein polynomials
n
n k n k
A
B
k k
Two imp ortant prop erties of these p olynomials are FR Far
p q
A A
i j
q p p q
B B B
j i i j
p q
A
i j
Z
n
B d
k
n
Expanding the pro duct in and applying gives
p q
A A
Z
q p
i j X X
j p q
i p i q j
I d B
e e e
e
i j
p q
j i
A
i j
Evaluating the integrals using proves the theorem
All of the line integrals app earing in are sp ecial cases of Theorem
with either p or q setto or Sp eci cally
Z
p
X
i p i p
d
e e
p
i
Z
q
X
j
q q j
d
e
e
q
j
Z
p p
X X
p i p i i p i
d i p i
e e
e e e e
p p
i i
Z
q q
X X
j j
q j q j q
j q j d
e e
e e
e e
q q
j j
Making these substitutions into give all of the needed pro jection integrals in
terms of the co ordinates of the pro jection vertices
Z
K
X
sgn n
dA
e e e
e
Z
K
X
sgn n
dA
e e e
e e
e
Z
K
X
sgn n
dA
e e e
e e
e
Z
K
X
sgn n
a dA
e e e
e e e e
e
Z
K
h
X
sgn n
dA
e e e e
e e
e
i
e e e
e e
Z
K
X
sgn n
dA
e e e
e e e e
e
Z
K
X
sgn n
dA
e e e
e e e e e e
e
Z
K
h
X
sgn n
dA
e e e e
e e e e
e
i
e e e
e e e e
Z
K
h
X
sgn n
dA
e e e e
e e e e
e
i
e e e
e e e e
Z
K
X
sgn n
dA
e e e
e e e e e e
e
Pseudo co de and on line C co de
The derivations of the previous section sp ecify a complete algorithm for computing
the ten desired volume integrals The algorithm comprises three routines
CompVolumeIntegrals V Figure computes the required volume integrals for a
p olyhedron by summing surface integrals over its faces as detailed in Equations
CompFaceIntegrals F Figure computes the required surface integrals over a
p olyhedral face from the integrals over its pro jection as detailed in Equations
CompProjectionIntegrals F Figure computes the required integrals over a
face pro jection from the co ordinates of the pro jections vertices as detailed in Equa
tions
This algorithm contains a minor simpli cation over the presented derivation When Equa
tions are substituted into the computation of sgn n and jn j b ecomes un
necessary since these factors always app ear together in a pro duct giving simplyn Thus
no signs or absolute values are computed in the routines CompFaceIntegrals and Comp
ProjectionIntegrals
ANSI C source co de for the algorithm describ ed in this pap er and detailed in
Figures is publicly available from two web sites
http www acm org jgt
http www cs berkeley edu mirtich massProps html
Also included is an interface to build up p olyhedra using a simple data structure from
ASCI I sp eci cations examples are provided The co de is public domain and may b e used
as is or in mo di ed form
Test results
This section analyzes the sp eed and accuracy of the algorithm for various test
cases These tests were run on an SGI Indigo II with a MHz R CPU and double
ere used for the calculations precision oating p ointnumb ers w
The set of p olyhedral ob jects that have volume integrals which are commonly
tabulated or easy to compute by hand is rather limited The algorithm was run on two
such ob jects an axes aligned cub e units on a side and centered at the origin and a
tetrahedron de ned by the convex hull of the origin and the vectors i j and k The
theoretical values for these ob jects are shown in Table For these two examples all
T T ob ject T T T T T T T T
x y z xy yz zx
z y x
cub e
tetrahedron
Table Theoretical values of volume integrals for simple test p olyhedra
values computed by the algorithm were correct to at least signi cant gures The total
time required to compute all ten volume integrals was s for the tetrahedron and
s for the cub e
For a more interesting test the algorithm was applied to several p olyhedral ap
proximations of a unit radius sphere centered at the origin In this case there are two
sources of error numerical errors from the algorithm and approximation errors inherentin
the geometric mo del which is not a true sphere These latter errors should not b e attributed
to the algorithm itself For a p erfect unit sphere the integrals T T T T T and T
x y z xy yz zx
anish while T and T T T The algorithm was applied should v
x y z
to a series of successive approximations to the sphere b eginning with an icosahedron and
obtaining each ner approximation by pro jecting the midp ointofeach p olyhedral edge onto
the unit sphere and taking a convex hull The computed values of a representative set of
volume integrals for each p olyhedron are shown in Table
Without numerical error the integrals T and T would vanish for all six p olyhe
x yz
dral approximations of the sphere due to symmetry From Table the absolute values of
these computed values are all less than The theoretical values in the table corresp ond
to the sphere which circumscrib es the p olyhedra For each p olyhedron the corresp onding
values for the inscrib ed sphere were also determined and the computed values for T and
to lie between the b ounding values for these two spheres For approximation T veri ed
x
the di erence in values for the inscrib ed and circumscrib ed sphere is for T
These values are upp er b ounds on the numerical errors of the al and for T
x
gorithm Note that the deviations b etween theoretical and computed values for T and T
x
are reduced as the complexityof the p olyhedron increases while numerical error from the
algorithm should increase with complexity In light of the very small errors incurred in the
computation of T and T the deviations b etween the computed and theoretical values of
x yz
T and T are almost certainly due mainly to the p olyhedral approximation rather than to
x
numerical errors
approx verts edges faces T T T T time
x yz
x
s
ms
ms
ms
ms
ms
theoretical values for sphere
Table Data for successiveapproximations of a unit sphere
The execution times shown in Table are the total times for computing all ten
volume integrals for each p olyhedron The O n nature of the algorithm is evident from
approximation on the time ratios b etween successive re nements very closely follow the
ratio in the number of faces The algorithm is also very fast all ten integrals are
computed for a p olyhedron with over faces in only ms
Acknowledgments This chapter originally app eared as the pap er Mir We thank
Aristides Requicha for a valuable literature survey on this topic and David Bara for
useful commen ts on the initial draft of this pap er We esp ecially thank John Hughes for his
detailed review and many suggestions for improving the pap er
CompVolumeIntegrals V
T T T T T T T T T T
x y z xy yz zx
x y z
for each face F on the boundary of V
right handed permutation of x y z that maximizes jn j
compFaceIntegrals F
if x T T n F
else if x T T n F
else T T n F
T T n F
T T n F
T T n F
n F T T
T T n F
T T n F
T T n F
T T n F
n F T T
T T T T T T
x y z x y z
T T T T T T
x y z x y z
T T T T T T
xy yz zx xy yz zx
Figure CompVolumeIntegrals V Compute the required volume integrals for a p oly
hedron See Equations
CompFaceIntegrals F
computeProjectionIntegrals F
w n p for some point p on F
k n k k k k k k k k k
F k
F k
F k n n w
F k
F k
F k n n n n n w n w w
F k
F k
n F k n n n n n
n w n n w n w n w n w w
F k
F k n n w
F k n n n n n w n w w
Figure CompFaceIntegrals F Compute the required surface integrals over a p oly
hedral face See Equations
CompProjectionIntegrals F
b
for each edge E in CCW order around F
coordinate of start point of E
coordinate of start point of E
coordinate of end point of E
coordinate of end point of E
C
C C C C C C
C C C C C
C a a a K
C C K K
C K
C
C
C
C
C
C
C
C K
K C
C K
Figure CompProjectionIntegrals F Compute the required integrals over a face
pro jection See Equations
Chapter
Examples and Results
The ideas and theory presented in this thesis have b een put into practice through
the prototyp e Impulse an exp erimental impulse based dynamic simulator Impulse is writ
ten in ANSI C and runs on SGI and HP platforms This chapter describ es several examples
of simulations pro duced using Impulse which fall into three categories pure impulse based
simulation passive hybrid simulation and controlled hybrid simulation Snapshots of the
simulations are included and most of the simulations describ ed here can be viewed as
MPEG movies available from the Impulse web site
http www cs b erkeley edu mirtich impulse html
Many of these simulations were designed to test sp eci c asp ects of the impulse based ap
proach Beyond these test cases Impulse has been used as an engineering to ol for real
applications in the parts feeding domain These applications and some results are describ ed
here All of the simulations described here were run using the identical algorithms with
absolutely no tuning of parameters
Pure impulse based simulation
Pure impulse based simulation means that all contact interaction is handled through
trains of collision impulses there are no explicit constraints governing the relative motion of
di erent b o dies All b o dies in the simulation are either xed in space or execute piecewise
ballistic tra jectories between collisions This basic paradigm adeptly handles colliding and
transient contact and can also mo del manytyp es of continuous contact including sliding
Figure Snapshots from the dominos simulation
rolling and resting contact
Dominos
The dominos simulation involves a rowofseven dominos that fall over Figure
While the dominos are falling there are many simultaneous contacts between them which
are mo deled as individual collisions The e ects of the friction are evident in the simulation
after all dominos have fallen they come to rest on the supp orting surface Mo deling friction
is critical to realism as can be seen by comparing this simulation movie to a simulation
Figure Snapshots from the blo ck drop simulation
movie of frictionless dominos pro duced bytheSimpact system RK
Blo ck drop
The block drop simulation Figure involves dropping two blo cks onto a sup
p orting surface so that the blo cks come to rest in a stack The blo cks visibly b ounce several
times b efore settling into a resting state mo deled with micro collisions Performing this test
with more than twoblocks proved troublesome since collision impulses must b e propagated
up and down the stack in order to keep blo cks from p enetrating Stacks of ob jects sub ject to
Figure Snapshots from the blo ckonramp simulation
gravitational forces are one typ e of system p o orly mo deled with impulse based simulation
the ob jects p enetrate deeply into collision envelop es and the collision detection and colli
sion resp onse computations b ecome excessive Impulse based simulation can handle large
stacks which are in the pro cess of settling but once the system reaches a static state it is
easier to mo del with constraints
Blo ck on ramp
The block on ramp simulation involves a blo ck that slides down a ramp under
conditions where the friction between the blo ck and ramp is su cient to stop the sliding
Figure The ramp is at a angle with the horizontal the blo ckislaunched with a
y along the ramp of cm s and the co e cient of friction is This exp eriment velo cit
tests the accuracy of the friction mo del since the blo ck initially exp eriences sliding frictional
forces and later exp eriences static friction forces when it comes to rest Under Impulse the
blo ck reaches a resting state at very nearly the time and place predicted by theory Graphs
of the blo ck s p osition and velo city are shown in Figure
Figure Snapshots from the chain of balls simulation
Chain of balls
The chain of bal ls simulation illustrates the propagation of an impulse through a
chain of contacting bodies Figure Four balls are initially placed a collision epsilon
apart so that if anyballmoves toward another it will immediately p enetrate the collision
envelop e After the striking ball hits the chain the ball at the other end of the chain rolls
away while the rest remain basically motionless The moving ball rolls up a small ramp
then backdown to strike the chain The second collision impulse is propagated less cleanly
b ecause the balls have drifted slightly from their straight line con guration The drift is
primarily a result of the p olyhedral mo dels used for the spheres which prevent the collisions
from b eing exactly central
Under analytic approaches there are tw o ways to propagate impulses through
chains of contacting b o dies One metho d computes the impulses one at a time and the
other computes them simultaneously The latter is computationally preferable in some situ
ations but can generate non physical results in others For instance when the simultaneous
mo del is used for a ball chain simulation the striking ball launches the entire chain into
motion while it recoils backwards after impact CS Bar Howtocho ose b etween these
mo dels is not clear There is no ambiguity in the impulse based mo del which pro duces the
physically correct result
Balls in dish
The bal ls in dish simulation Figure involves dropping seven balls into a shal
low dish to let them interact at will After b ouncing around for a few moments the balls
come to rest with one ball at the center of the dish surrounded symmetrically by the other
six Although Impulse do es not explicitly try to minimize the energy of the system this is
the macroscopic result as collisions dissipate energy from the system
In the nal con guration there are simultaneous contacts within the cluster of
balls plus seven contacts between the balls and dish which are all easily maintained with
micro collisions These contacts are easier to mo del via collisions than those for the block drop
simulation describ ed ab ove for two reasons First there are not stacks of ob jects involved
b ecause of the shallow dish angle the balls are not pushed into one another as strongly
Acentral collision is one in which the collision p oint is on the line segment joining the centers of mass of the colliding b o dies
Figure Snapshots from the balls in dish simulation
Second collisions b etween spheres are central and impart little angular acceleration to the
colliding b o dies As a result the TOI b ounds are much tighter In contrast when one
corner of a cub e collides with a large at surface the angular acceleration of the cub es
cause the opp osite corner to strike the surface so on afterward
Coins
The coins simulation Figure illustrates the wide variety of macroscopic con
tact mo des that are pro duced by Impulse s collision mo del In this simulation four coins
are rolled toward the center of a plate and four coins are simultaneously tossed into the
center of the plate All of the coins collide in the middle Through the course of the simu
lation manytyp es of contact o ccur including colliding sliding prolonged rolling resting
contact betw een di erent coins and resting contact between the coins and plate Also
demonstrated is the familiar motion of a coin rolling along a path of increasing curvature
eventually falling over and rattling to rest The coin system is very representative of the
typ es of physical systems whicharewell suited to impulse based simulation
Pool break
Figure is a series of snapshots from the simulation of a pool break Like the
chain of bal ls simulation this simulation entails propagating collision impulses through
chains of contact but with a more complex initial contact con guration there are initially
contact p oints b etween the rack of balls The balls in the p o ol break simulation are
initially placed one collision epsilon apart so that any motion on the part of one ball toward
another results in an immediate collision
ust b e pro cessed at the moment of the initial A high numb er of collision checks m
break Figure shows the numb er of collision checks p er second of simulation time After
the balls b egin to separate the collision check frequency quickly drops as higher TOI b ounds
are achieved The collision check rate stabilizes at approximately kHz after the break is
over These collision checks are b etween the balls and the surface of the p o ol table and are
needed to keep the balls from p enetrating the table
The collision mo del supp orts spin on the balls By changing the initial angular
velo city of the cue ball it can be made to draw back or eventually roll forward after the
initial impact with the rack Other balls follow paths which deviate slightly from a straight
Figure Snapshots from the coins simulation
Figure Snapshots from the p o ol break simulation
Collision Rate
12 initial break 10
8
6
4
2 thousands of collisions per second
0 time (s)
0.0 0.5 1.0 1.5 2.0
Figure The frequency of col lision checks as a function of time during a pool break
Time refers to the time in the simulation not the execution time required to compute the
simulation
line due to spin
Bowling
A typical ten pin bowling throw involves b oth prolonged and transient contact
mo des The ball is lofted onto the alley and settles into continuous contact with the alley
sliding at rst Eventually contact with the alley causes the contact mo de to shift from
sliding to rolling as the relative tangential velo city at the contact between the ball and
alley is brought to zero If the b owler imparts the prop er initial translational and angular
velo cities to the ball the sliding phase will bring the ball toward the gutter and the rolling
phase will cause it to ho ok back toward the center of the alley Once the ball strikes the
pins the contacts are predominantly transient with many collision o ccurring Snapshots
from sucha bowling simulation are shown in Figure
Figure Snapshots from the bowling simulation
A series of b owling exp eriments was p erformed to test Impulse s ability to obtain
physically valid results In the rstbatch of simulations a straight ball was thrown down
the alley bylaunching the ball with zero angular velo city andacenter of mass velo cityin
the y direction Figure trials were p erformed keeping the initial ball velo city
Straight vs. Hooking Ball hooking 10.0 straight
9.5
9.0
8.5
8.0
felled pins 7.5
7.0 pin line 6.5 −30 −15 0 +15 +30
position (cm) 6.0 y −15 −10 −5 0 +5 +10 +15
x pin line position (cm)
Figure Left Bow ling experiments wereperformed by recording the horizontal position
of the ballasitcrossed the pin line and the number of pins that wereknocked down Right
Results of the bow ling experiments for straight and hooking bal ls
constant but varying the initial x co ordinate of the ball s center of mass overa centimeter
window In a second batch of trials the initial ball velo city conditions were altered
to pro duce a right hander s ho oking ball angular velo city of rad s in the y direction
and a linear velo city at an angle of from the y axis Accurate physical dimensions and
masses were used for all b o dies except for a slight approximation in the shap e of the pins
it crossed Figure shows the number of felled pins versus the ball p osition as
the pin line ordinates are averaged over mm wide abscissa windows The ho oking ball is
slightly b etter than the straight ball at most p ositions along the pin line and is signi cantly
b etter over a range between the head pin and rightmost second row pin to cm
on the pin line This agrees with the accepted wisdom that a right handed b owler s b est
strategy is to throw a ho oking ball between these two pins The plots also illustrate the
dip in felled pins due to splits when the ball hits the head pin dead on Obtaining these
results analytically would b e nearly imp ossible but the chaotic bowling system is an ideal
application for impulse based simulation the evolution is collision intensive with many
transient contacts between ob jects and there is a gradual shift in contact mo de between
the ball and the alley b ouncing to sliding to rolling
Ball on spinning platter
Consider a ball rolling on a spinning platter The nonholonomic mo del and the
Vakonomic mo del are two variational based approaches to deriving the dynamics of this
system which yield di erent answers Lewis and Murray computed simulations based on
each of these mo dels and then p erformed exp eriments to test their accuracy using a large
steel ball on a spinning plexiglas surface LM They note that the general exp erimental
behavior was in go o d agreement with certain nonholonomic simulations such as the one
a similar exp eriment using pictured in Figure Also in the gure is a snapshot from
Impulse Here the nonholonomic contact between the ball and platter is mo deled with
the usual collisions The tra jectories generated this way qualitatively match those of Lewis
and Murray This example illustrates the versatility of the impulse based metho d with no
extra e ort collisions mo del the nonholonomic constraintbetween the ball and platter in a
physically accurate way
Rattleback top
The rattleback top simulation demonstrates the ability of the impulse based metho d
to mo del nontrivial contact interaction A rattleback top is an elongated solid with a convex
bottom much likethehull of a ship If spun in its preferred direction the top will continue
to spin in that same direction until friction eventually stops it If spun in the other direction
however instability so on o ccurs as the top b egins to ro ck back and forth and eventually
vior results from a slight asymmetry in reverses its spinning direction This anomalous b eha
the top either in the shap e of the b ottom or in the mass distribution The key requirement
is that the principle axes of curvature at the contact pointmust b e skewed in a particular
Figure Left Trajectories of a bal l on a spinning platter simulated by Lewis and
Murray the physical experiments general ly exhibited this behavior LM reprinted with
permission Right A snapshot of an analogous simulation using Impulse the spiral lines
indicate the bal l s trajectory
manner relative to the principle axes of inertia of the top
A rattleback exp eriment was p erformed to test Impulse s ability to predict this
phenomenon First two identical balanced ellipsoids were spun with the same initial
conditions to verify that they remained in phase when spun The mass of each ellipsoid
was grams Next a gram point mass was attached to the surface of one of the
ellipsoids in a p osition which skewed the principle axes of inertia in the prescrib ed way
The exp eriment was redone spinning the ellipsoids in a manner such that the rattleback
was spun in its non preferred direction After approximately two revolutions the rattleback
top b egan ro cking and quickly reversed its spinning direction as predicted Snapshots from
the simulation are shown in Figure
Part feeder chute
ulation is one of a numb er of simulations related to part The parter feeder chute sim
feeding Six parts are dropp ed into an inclined tray funneled through a narrow op ening
in the lower end and land on the at surface below The parts are mo deled after an
actual industrial part a non convex insulator cap The parts slide along each other in the
chute b efore spreading out as they fall onto the surface Figure Exp eriments like
Figure Snapshots from the rattleback top simulation
Figure Snapshots from the part feeder chute simulation
Figure Snapshots from the explo ding text simulation
this can generate sto chastic data related to a particular feeding op eration The data can
determine the feeder s susceptibility to jamming and its ability to singulate the parts Such
an application using Impulse is describ ed in BC
Explo ding text
Physically based mo deling is not only a valuable to ol for simulation and virtual
prototyping but also for generating animations One example is the exploding text simula
tion The letters of the words impulse based simulation are given random initial velo c
ities and set free to move through the space in an enclosed volume with gravity turned
o Figure This example is a go o d test of the collision detection system there are
indep endently moving rigid b o dies many of them non convex It illustrates the use of
simulation to pro duce interesting visual e ects The animation is even more comp elling
when played back in reverse from random debris the words impulse based simulation
materialize
Passive hybrid simulation
As discussed in Chapter pure impulse based metho ds are not suitable for mo d
eling tightly constrained contact such as that o ccurring at joints Hybrid simulation allows
for certain con tacts to be mo deled as joints greatly expanding the range of systems that
can b e simulated This section describ es three examples that use passivehybrid simulation
Passive means that there are no control systems acting All external forces on multib o dies
are due to gravity collisions or passive elements like springs and damp ers
See saw
In the see saw simulation eight balls are placed in a shallow funnel like tray
and roll toward a rectangular op ening in the center of the tray Blo cking this op ening
is a see saw a one link multib o dy xed to the tray through a revolute joint The balls
w causing it to turn and ro ck as they pass through the op ening eventually land on the see sa
Figure Contacts b etween di erent balls b etween the balls and the tray and b etween
the balls and see saw are all mo deled with collisions
Part sorter
The part sorter simulation is another application from the part feeding domain
Six parts of three di erenttyp es slide down tracks arranged in a switchback fashion Two
trap do ors in the tracks allow parts with su cient mass to fall through to the track b elow
lighter parts slide across the trap do ors In this way parts are sorted based on their mass
Figure The trap do ors are single link multib o dies connected to the xed tracks
through hinges The hinges are equipp ed with springs to hold the do ors closed The spring
constants were chosen to let certain parts fall through but let others pass over Damp ers
Figure Snapshots from the see saw simulation
Figure Snapshots from the part sorter simulation
are attached at the jointtoprevent excessive oscillation of the do ors Interaction b etween
the parts and the tracks and trap do ors are mo deled with collisions the hinge contacts are
mo deled with constraints
The spring constants are adjusted so that individual ly one part typ e glides across
the rst trap do or one part typ e falls through the rst trap do or but glides across the second
and one part typ e falls through both trap do ors In practice the simple sorter p erforms
p o orly b ecause its dimensions are to o small Parts can not b e adequately separated so the
trap do ors do not have su cienttime to return to their resting state between parts This
typ e of information would b e very useful to the designer of a part sorter
Triple p endulum
A nal example of passive hybrid simulation is the triple pendulum simulation
Figure Six balls are dropp ed into a shallow dish Susp ended ab o ve the dish is a
p endulum with three links and three revolute joints The last link ends with a mallet This
p endulum is p ositioned so that it can never hit the dish but so that it can reach the balls in
the dish It is not actuated but dropp ed from a high p otential energy state so that gravity
drives it As it swings it collides with the balls o ccasionally kno cking one out of the dish
The interactions between pairs of balls between the balls and the dish and between the
balls and the p endulum are mo deled with collisions Eventually the p endulum comes to
rest in its minimum energy state with the mallet resting b etween the three remaining balls
The p endulum is not damp ed its decrease in energy results entirely from collisions with
the balls
Controlled hybrid simulation
Hybrid simulation is not restricted to passive systems External controllers may
also b e attached to ob jects making the simulator a p owerful to ol for animation as well as
simulation and testing Three examples using Impulse are discussed b elow
Furniture arrangement
The furniturearrangement simulation can b e viewed as an animation application
or as a design application with intelligent ob jects This example only used rigid b o dies a
Figure Snapshots from the triple p endulum simulation
Figure Snapshots from the furniture arrangement simulation
trivial case of multib o dies and do es not illustrate the full b ene ts of hybrid simulation
Butitisagoodtestofthecontrol architecture The moving ob jects are a table six chairs
and a wine b ottle that move through two adjoining ro oms Figure The ob jects are
all controlled by applying external forces and torques via the control architecture The table
controller steps through a sequence of eight user sp eci ed con gurations applying forces
and torques to gravitate the table toward each con guration Damping forces and torques
are also applied When a con guration goal is attained and the table velo city subsides the
control system advances to the next goal
Each chair is driven to gravitate toward a goal con guration sp eci ed relative to
the table again using a PD controller as the table moves the chairs attempt to followit
Chair goal p ositions are also mo di ed by the presence of nearbywalls so that they sco ot
in if the table moves close to a wall Finally the b ottle has a simple control law which
rep eatedly causes it to jump onto the table if not there already and gravitate toward
the center of the table To a ect a jump the controller computes initial velo city values
whichwould arc the b ottle in a parab olic tra jectory onto the table and writes these new
velo cityvalues to the b ottle s state as if it had exp erienced a collision This illustrates the
exibilityavailable to the control designer All ob ject controllers also apply torques to keep
the ob jects upright
A total of four controllers are required one for the table one for the b ottle and
two for the chairs one to control them to goal p ositions and one to up date these p ositions
in the presence of walls Eachcontroller is a single C function ab out one page long The
controller rates vary from Hz
Although real furniture is no more autonomous than a Luxo lamp this example
demonstrates the ease with whichinteresting motion can b e pro duced from simple control
systems built on top of a dynamic simulator The entertaining animation can be viewed
simulation From a more serious p ersp ective this example as the desired pro duct of the
illustrates the use of simulation as a design or layout to ol Consider the lab or intensive
job of arranging furniture in a large architectural virtual environment as in BS The
task is made easier if the user only sp eci es the p ositions of certain ob jects like tables and
other ob jects likechairs adjust themselves accordingly The chairs are intel ligent objects
with useful b ehaviors
Bicycle
The bicycle simulation involves an unmanned bicycle guided over an obstacle
course Figure A controller designed to balance the bike at a sp eci ed roll angle
was provided by Getz Get In contrast to the bike controller emplo yed by Ho dgins
et al HWBO Getz s control law is nonlinear employs a dynamic mo del of the bicycle
accounts for the nonholonomic constraint between the wheels and ground and do es not
decouple the control of the p edals and handlebars Ho dgins et al mo deled a rider on the
bike and built basic obstacle avoidance and clustering abilities into the control law these
Figure Snapshots from the bicycle simulation
are not present in Getz s controller
This example was chosen for two reasons First Getz s control law relies on correct
mo deling of b oth the bicycle dynamics and constraint forces with the road in order to
function prop erly Second the controller was develop ed indep endently without regard
to any particular style of dynamic simulation Prior to this exp eriment the control law
had b een implemented only as a MATLAB simulation of a bicycle on a at surface using
idealized motion constraints and without collisions or obstacles
The bicycle is a link multib o dy a frame a fork and two wheels geometry and
masses are based on a standard bike The kinematic top ology can be describ ed as a
serial linkage but the mo del w as built as a tree linkage with the frame as the oating base
link Figure The controller takes as input the current steering angle roll angle rear
frame
rear steering axle column
fork rear wheel front axle
front
wheel
Figure The kinematic structure of the bicycle
wheel velo city and the derivatives of these quantities Using a computed torque metho d
it calculates desired accelerations of the p edals and handlebars to achieve the user de ned
roll angle and forward velo city pro les The front wheel is not actuated and rolls freely
Getz s MATLAB co de was translated in essentially unchanged form to a C function called
by Impulse s control architecture at a Hz rate With this control law and the de ned
pro les the bike is guided over an obstacle course which includes a pylon slalom stair steps
awall an ice patch and a jump
This example demonstrates howcontrol strategies may b e tested under more var
ied conditions with an impulse based simulator that supp orts general collision and contact
mo deling Hazards or other agents can b e placed into the simulation without any mo di ca
tion of the control law a good controller causes the system to react in a natural way The
graphs shown in Figures and showhowwell the controller worked in the presence
of the various disturbances
Forward Velocity Profile
14
desired 12 actual jump 10
8 steps 6 ice forward velocity (m/s) 4 pylon slalom
2 wall crash
010 203040
time (s)
Figure The forward velocity pro le as the bike traversed the obstacle course
Creature war
The creature war simulation provides a detailed test of the multibody collision
resp onse algorithms and the control supp ort architecture In this test a rover vehicle chases
crawling bugs annihilating them by pushing them o the edge of the world Figure
Each bug has an ellipsoidal b o dy and two elliptical legs Figure The leg velo cities
Roll Angle Profile
+40
+30
+20
+10
0 jump
−10 steps roll angle (deg)
−20 desired actual −30 pylon slalom ice −40 wall crash
010 203040
time (s)
Figure The rol l angle pro le as the bike traversed the obstacle course
are regulated with individual PD controllers Bugs control their headings by commanding
di erentvelo cities to their legs Bug b ehaviors are generated by a high level controller that
computes a desired heading using a p otential eld approach They have a preference to
b e in the center of the square world and a stronger preference to avoid the rover when it
is nearby Each bug s high level controller uses the bug s current p osition and that of the
rover to compute a new desired heading
The rover is a link multib o dy Figure The front wheels turn on axes
connected directly to the chassis while two prismatic joints allow each rear wheel axis to
move vertically relative to the chassis These prismatic joints are equipp ed with sho ck
absorb ers springs and damp ers to smo oth out bumps in the terrain The rover uses four
PD controllers to regulate the velo city of its wheels It changes its heading by commanding
Figure Snapshots from the creature war simulation
body
left right
leg leg
Figure Left Side view of a bug Right Kinematic tree for a bug showing the
individual links and joints
chassis
left right rear rear axle axle left right front front wheel wheel
left right rear rear wheel wheel prismatic joint
revolute joint
Figure Left Side view of the rover in normal and jacked con gurations Jacking
al lows the rover to push bugs rather than running over them Right Kinematic tree for the
rover showing the individual links and joints
di erent desired velo cities to the left and right side wheels Two additional PD controllers
regulate the length of the rear prismatic pistons By activating these controllers the rover
can jack up its rear end and lower its front end in preparation to push bugs Deactivating
these controllers makes the rover resume its normal driving p osture The rover s behavior
is more complicated than that of the bugs It rep eatedly cho oses one of the surviving bugs
as a victim chases this bug down assumes jacked p osition and pushes the bug o the
edge of world and into the abyss It avoids driving itself over the edge This b ehavior was
implemented via a state machine controller with ve states shown in Figure
reverse
rover prey below? backing up? prey below? prey killed? prey below? prey ahead? prey to side?
spin forward push prey near & prey to side? dead ahead?
always prey killed?
pick
Figure The high level state machine control ler for the rover There are ve states
and the edges are labeled with the condition for the corresponding state transition
The PD controller rates for this example vary from to kHz and the high
level control routines run at Hz Two high level control routines are required for this
animation one for the bugs and one for the rover Both are C functions ab out one page
long The actual function for implementing the rover is shown in Figure The creature
war shows howinteresting b ehaviors can emerge from physics plus simple control laws and
interactions between agents The sequence of the nal animation was quite unimagined
when the control laws were designed
installSpring mb joint Ks Ls
void cli rover MB mb
installDamp er mb joint damping
f
installDamp er mb joint damping
FILE fp
REAL Kp Kd Ki period desired
mb period Kp Kd Ki createPID
REAL Kp Kd Ki period
mb period Kp Kd Ki createPID
REAL Ks Ls damping
createPID mb period Kp Kd Ki
mb period Kp Kd Ki createPID
fp fopen rover ctrl r
createPID mb period Kp Kd Ki
fscanf fp lf lf lf lf lf Kp Kd Ki period roverVmax
mb period Kp Kd Ki createPID
fscanf fp lf lf lf lf Kp Kd Ki period
fscanf fp lf lf lf Ks Ls damping
roverPrey
fclose fp
FWD roverState ROVER
mb REAL roverCtrl createGenCtrl
mb joint Ks Ls installSpring
g
case ROVER void roverCtrl CTRL LAW cl SPIN
f vRightDes roverVmax bearing spin
MB mb vLeftDes vRightDes
REV RB chassis prey if inc roverState ROVER
FWD else if deltaBear roverState ROVER REAL vLeftDes vRightDes d bearing deltaV k
break REAL deltaBear spin inc
int dead i
PUSH case ROVER VECT goal v
CRAWLERS froverState ROVER REV break g if roverPrey NUM JOINT jnt
deltaV roverVmax bearing spin SE Tib
vRightDes roverVmax deltaV
vLeftDes roverVmax deltaV static char states f fwd spin push rev pick g
vectCopy prey v v
k SQR v SQR v mb cl body
displacePoint prey Tbi d v k vectNorm v v chassis mb link
if v v v v prey MBtable CRAWLER roverPrey link
REV deltaBear inc roverState ROVER se inv chassis Tbi Tib
CRAWLERS fv v v g break if roverPrey NUM
else vectCopy prey Tbi d v
REV dead v case ROVER
vLeftDes vRightDes roverVmax
se xformVect Tib chassis v v se xformPoint Tib v goal
PICK if v inc roverState ROVER d sqrt SQR goal SQR goal
break vectNormalize goal goal
TO DEG PICK case ROVER inc acos goal RAD
TO DEG bearing atan goal goal RAD vLeftDes vRightDes
deltaBear fabs bearing if dead roverPrey
mb se xformVect chassis Tbi chassis w v disablePID
mb disablePID spin v
FWD roverState ROVER
break switch roverState f
g
FWD case ROVER
WHEEL RAD deltaV roverVmax bearing spin vLeftDes ROVER
WHEEL RAD vRightDes ROVER vRightDes roverVmax deltaV
vLeftDes roverVmax deltaV
PICK mb vLeftDes setPID if dead roverState ROVER
REV mb vLeftDes else if inc roverState ROVER setPID
SPIN mb vRightDes else if deltaBear roverState ROVER setPID
mb vRightDes else if d deltaBear f setPID
mb g setPID
mb setPID
PUSH roverState ROVER
g
break
Figure The C functions that implement the rover s high level control Func
tion cli rover is cal led once to instantiate control lers and set initial values Function
roverCtrl implements the high level state machine depicted in Figure It is cal led by
the simulator scheduler every seconds Cal ls to the control library are in underlined
and in bold font
Execution times
Table summarizes the execution times needed to simulate the examples de
scrib ed ab ove Simulation time is the length of time which passed in the simulation com
simulation computation
time time
example sec sec min sec slowdown
pure impulse based simulation
dominos
blo ck drop
blo ck on ramp
chain of balls
balls in dish
coins
p o ol break
bowling
ball on spinning platter
rattleback top
part feeder chute
explo ding text
passivehybrid simulation
see saw
part sorter
triple p endulum
controlled hybrid simulation
furniture arranging
bicycle
creature war
Table Simulation times for examples
putation time is the actual time needed to compute the simulation and slowdown is the
ratio of the latter to the former a slowdown of corresp onds to real time simulation
Simulations were p erformed on an SGI Indigo II with MHz R pro cessor Execution
times shown are the average taken over several runs with graphics turned o For the pure
impulse based simulation examples running times were generally less than ve times slower
than real time for a couple of the examples real time simulation was achieved
Impulse ran slower on the hybrid examples generally one to two orders of mag
art sorter and furniture arranging nitude slower than real time The slowdown in the p
examples was primarily due to collision detection involving several highly nonconvex bod
ies For the triple pendulum bicycle andcreaturewarexamples the bulk of the pro cessing
time was sp ent computing nontrivial multib o dy forward dynamics The see saw example
which had only convex ob jects and a very simple multib o dy ran relatively quickly
Table details how the pro cessing time was sp ent for the three controlled hybrid
simulation examples In all cases only a very small fraction of time was sp ent executing
forward collision collision control
example arrangement dynamics detection resp onse system
furniture
bicycle
creature war
Table How the simulation processing time is spent All gures are percentages of total
processing time
control laws Collision resp onse the time sp ent computing and propagating multib o dy col
lision impulses is also fairly small As mentioned b efore the nonconvex collision detection
uses most of the pro cessing time in the furniture arranging example and computing for
ward dynamics uses most of the time in the other examples Signi cant sp eed up is p ossible
by using commercial packages that precompile the dynamics computation for greater e
ciency as in HWBO Also under impulse based simulation the dynamics of separate
multib o dies may b e computed indep endently so parallelization is trivial
For rigid b o dy and multib o dy forward dynamics integration as well as for collision
integration Impulse uses a standard fourth order Runge Kutta integrator with adaptive
step size PTVF Totake one step forward the integrator calls the derivativeevaluation
routine times This can be quite exp ensive compared to say a non adaptive Euler
integration step which only requires one derivativeevaluation p er step An impulse based
simulator tends to call the dynamics integrator with fairly small intervals since the time
between collisions is short and with these small intervals the accuracy of a simpler and
faster integrator mightbesu cient Further study is needed
Estimating pose statistics
The initial problem that sparked the development of Impulse was the simulation
of vibrational parts feeders These machines commonly used in automated assembly use
high frequency small amplitude vibratory motion to move and orient parts which are
often subsequently handled by a manipulator More detailed descriptions of these typ es of
feeders may be found in BPM Vibrational parts feeders naturally led to the idea of
mo deling contact with impulses since the contact mo des between the part and feeder are
highly transient Many non vibrational feeders are natural applications for impulse based
simulation b ecause the parts still collide frequently with the feeder and collisions have a
dominant e ect on the dynamics of the parts
Impulse was used to predict the probabilities of rigid parts landing in their various
stable p oses when dropp ed onto a at surface The problem has sp eci c relevance to the
Adept Rob otics exible feeder but would also b e useful for other feeders Results from the
simulator and from quasi static metho ds were compared to data from physical exp eriments
for a numb er of real industrial parts these results are included b elow More details of the
metho ds used to estimate p ose statistics are in MZG
Background
In contrast to xed assembly lines exible assembly lines can be rapidly recon
gured to handle new parts This can dramatically reduce the time needed to bring new
pro ducts to market and p ermit the cost of assembly lines to be amortized over multiple
pro ducts Part feeders which singulate and orient the parts prior to packing and insertion
are critical comp onents of the assembly line and one of the biggest obstacles to exible
assembly Currently the design of part feeders is a black art that is resp onsible for up to
of the cost and of workcell failures NW BPM
Carlisle et al RG prop osed a exible part feeding system that combines ma
chine vision with a high sp eed rob ot arm In contrast to custom designed hardware such
as the b owl feeder only software is changed when a new part is to b e fed The idea is that
a collection of like parts are randomly scattered on a at worktable by the force of gravity
erhead vision system determines the p ose p osition and orientation of each part The An ov
rob ot arm then picks up each part and moves it into a desired nal p ose as illustrated in
Figure A metho d of estimating the throughput of this feeder is in GC but the
metho d requires knowledge ab out the distribution of p oses particularly orientation when
the parts are scattered on the at worktable More formally the problem to b e solved is
Problem Estimating Pose Statistics EPS For a rigid polyhedral part X with
known center of mass and inertia tensor denote the n faces of its convex hul l H by F
efrom some known distribution of F Assuming X is repeatedly dropped onto a at surfac
n
initial poses compute the values p p where p is the probability that X reaches a
n i
nal resting state with F down against the surface i
Figure A exible part feeder using machine vision a high speedrobot arm and pivoting
gripper This il lustration shows the system feeding car stereo pushbuttons
Additional assumptions are that the workspace is at and much larger than X and that X
do es not collide with other parts
Predicting the p ose distribution of rigid parts dropp ed onto a at surface is im
p ortant in evaluating part designs for assembly in estimating feeder throughput and in
determining how many rob ots and assembly lines are required to meet sp eci ed pro duction
rates This work in estimating pose statistics complements other ongoing work in auto
mated assembly Rao Kriegman and Goldb erg have studied the use of a pivoting gripp er
for Adept s exible feeder they give an O m n log n algorithm to generate pivot grasps
for a part with n faces and m stable con gurations RKG Christiansen Edwards and
Co ello Co ello give a genetic algorithm for designing e cient part feeders from comp onent
gates CEC Their algorithm takes p ose statistics suchas the ones computable by Im
pulse as input
Quasi static algorithms
For completeness Problem was also solved using two quasi static algorithms
The rst algorithm was prop osed by Wiegley et al WRG and is an improvement on the
early work done byBoothroyd BPM Motion of the part is assumed to b e quasi static
inertia and velo city are neglected
The idea of the algorithm is illustrated in Figure After computing the part s
β 0
δ 2
Fπ
v 0 β 1 d01 v d F 1 Hull H c0 dc1 β c 2
v2
Figure Estimating pose statistics using a sphere projection method
convex hull H the facets of H are pro jected onto a unit sphere centered at the center of
mass c If F is the pro jection of face F then the ratio of the area of F to the total surface
area of the sphere gives the probability that the part will land on face F under quasi static
conditions if its initial p ose is uniformly distributed over SO
Assuming triangular faces the ratio in question is given by
A
where the are the interior angles of F the can b e computed from standard spherical
i i
trigonometry
This pro cedure results in an initial estimate of each p To treat faces of H that
i
are statically instable the center of mass is pro jected onto the plane of eachfaceF If the
i
pro jected point lies outside face F gravity will cause the part to topple over to adjacent
i
face F In this case p is added to p andp is set to zero The algorithm to compute the
j i j i
p ose statistics is O n in the numb er of p olyhedral edges
Since the quasi static analysis do es not mo del dynamic disturbances it often over
estimates the probability of landing on afacet that is stable but easily dislo dged by small
vex hull F and F and the b ounding edge e vibration Consider two facets of the part s con
i j
between them Let g b e the gravityvector from the part s center of mass If g intersects F
i
when in contact the part will remain on facet F under the quasi static mo del However
i
dynamic energy may rotate across edge e when g points inside facet F but is close to edge
i
e Figure A second quasi static metho d called the p erturb ed quasi static metho d
computes a more accurate likeliho o d that the part might rotate from facet F to facet F
i j
and propagates probabilities accordingly see MZG for details In all the exp eriments
the cone half angle was although the rightwaytocho ose this half angle measure is not
completely clear
Dynamic simulation
using full dynamic simulation of the parts b eing The EPS problem was attacked
dropp ed onto a surface using Impulse For dynamic simulation many parameters not used
in the quasi static and p erturb ed quasi static algorithms are imp ortant The co e cients
of friction and restitution were b oth estimated to b e The feeder con guration is also
Recently Issa Nessnas of Adept has exp erimentally measured these co e cients He estimated the
co e cient of friction to b e and the co e cient of restitution to b e although there was a
large amount of noise in these results
C Fj ε
g
Fi
e
Figure The perturbed quasi static method uses a cone projected down from the center
of mass to estimate the likelihood that the part wil l topple from face F to face F This
i j
likelihood is used to propagate some of the probability from p to p
i j
imp ortant Adept s exible feeder system dumps the part from an upp er b elt onto alower
b elt where the parts are examined by the vision system and then picked and placed by
one or more manipulators Figure For dynamic simulation the height of the drop
was estimated at cm The horizontal velo city of the parts as they leave the upp er b elt
was estimated at cm s
The initial orientation of the parts p oses a problem since the parts are in a stable
resting state on the upp er b elt b efore b eing dropp ed onto the lower one Thus the initial
distribution of orientation is not uniform but similar to the nal initially unknown dis
tribution To circumvent this problem the initial orientations for the rst drops were
chosen randomly assuming a uniform distribution over SO This b o otstraps the pro cess
with a preliminary nal p ose distribution For all remaining drops the initial upp er b elt
p oses are chosen from the current distribution of nal lower b elt p oses so that the results
Figure Top CAD models of the four parts used in the experiments From left to right
insulator cap large white rectangular black and square black stereo buttons Bottom
Photographs of the rectangular black stereo button in its seven stable states
of the drop tests are continually fed back to determine initial conditions A slight p ertur
bation a rotation of up to degrees ab out a randomly chosen axis is also applied to the
initial p ose to intro duce noise into the system due to b elt vibration
Exp erimental results and discussion
The three algorithms describ ed were applied to four test parts The test parts
were all small plastic rigid pieces of the typ e typically used in automated assembly Fig
a lo cal hardware store Parts ure Part is an insulator cap purchased at
and are pushbuttons designed for a commercial car stereo system Geometric mo d
els of each part were constructed by measuring the parts with a ruler Centers of mass
and moments of inertia for the parts were computed automatically by Impulse using the
techniques of Chapter As a control physical exp eriments were p erformed by rep eatedly
running several hundreds of samples of each test part through Adept s exible feeder sys
tem Figure the nal resting poses were recorded by a human observer Tables
through show the results All quantities in the tables are p ercentages
The error p ercentages included in the tables indicate the overall p erformance of
each algorithm for each sample part They are computed as the average deviation of the
algorithm s predictions from the physical test p ercentage weighted by the frequency with
which that state actually o ccurs Let p p represent the probabilityofeachofn states
n
as measured in the physical test Let a a represent the corresp onding probabilities
n
computed by one of the algorithms The error p ercentage for that algorithm is given by
n
X
e p ja p j
i i i
i
Quasi P turb ed Dynamic Physical
Pose Static Q S Sim Tests
error
trials p erformed in physical exp eriments
Table Orange insulator cap data
Quasi P turb ed Dynamic Physical
Pose Static Q S Sim Tests
error
trials p erformed in physical exp eriments
Table White stereo button data
The quasi static and p erturb ed quasi static algorithms are extremely fast requir
ing less than a second of computation time for parts with facets and the p erturb ed
quasi static algorithm s predictions are consistently more accurate than those of the quasi
static one Dynamic simulation is slower For each part drops were simulated taking
approximately minutes per part however greater accuracy is also achieved The dy
namic simulation algorithm is the most accurate for all sample parts except the insulator
Quasi P turb ed Dynamic Physical
Pose Static Q S Sim Tests
error
trials p erformed in physical exp eriments
Table Rectangular black stereo button data
Quasi P turb ed Dynamic Physical
Pose Static Q S Sim Tests
error
trials p erformed in physical exp eriments
Table Square black stereo button data
cap Table for which the p erturb ed quasi static algorithm slightly outp erforms it
The dynamic simulation algorithm s prediction accuracy is also the most consistent the
comp osite error is less than in all cases
In an interactive setting where a designer is p erhaps editing the CAD mo del of a
part in order to improve feeder throughput the p erturb ed quasi static algorithm is clearly
the best choice The designer need only wait seconds to see how changing a part s CAD
mo del alters the pose distribution and feeder throughput The dynamic simulation algo
rithm is useful for obtaining a more accurate estimate once the design has b een determined
or for analyzing the e ects of more subtle design changes It mo dels several factors that
aren t considered by the standard and p erturb ed quasi static algorithms including friction
collisions with energy loss mass moments of inertia height of drop and initial conditions of
the part prior to drop To study the e ects of varying these parameters dynamic simulation
is appropriate
Our simulation exp eriments involved drops tests Often fewer trials maybe
su cient reducing the computational cost of this metho d Supp ose the true unknown
probability that a part lands in a particular p ose is p The numb er of times the part lands
in this p ose over n trials is a binomial random variable whichmay often b e approximated
by a normal distribution A con dence interval statement is of the form p lies within the
range with certainty Here is the probability estimate obtained
from the n trials is the allowable error tolerance and is the level of the statistical test
Given and one can b ound the numb er of trials necessary by
n
where x is the cumulative normal distribution function For example to pinp oint the
probability of a particular nal p ose to within with certainty and
From trials are su cient See Dev for more information
Other part feeding exp eriments
Impulse is currently b eing used for several other studies in part feeding Impulse
based simulation is a go o d match to this domain b ecause the interactions between feeder
and parts are often collision intensive or of a vibratory nature An accurate friction mo del
is also required for accurately predicting the b ehavior of many feeders
Berkowitz and Canny have develop ed a to ol for designing industrial part feeders
basedonstochastic data from dynamic simulation BC In many feeders parts are pro
p elled along a feeder track through vibration conveyor b elt or gravity Various protrusions
and cut outs also called gates along the path reorient the passing parts or reject them
parts passing successfully through all of the gates are correctly oriented Feeder designs
can be parameterized by the p ositions angles and dimensions of the gates Berkowitz s
A common rule of thumbis that the normal approximation is valid if the numbers of successes and
failures during the trial series b oth exceed ve
and Canny s to ol allows a designer to search through the space of p ossible feeder designs
searching for the optimal one with resp ect to a metric the designer sp eci es Impulse is used
to compute the tra jectories of the parts through the feeders the impulse based approachis
very e cient for this typ e of simulation
Another way to apply this idea would b e for the algorithm of Christiansen et al
They describ e an approach to near optimal design of vibratory b owl feeders using a genetic
algorithm to search through the space of all gate sequences CEC For each p ossible gate
their algorithm requires as input a sto chastic matrix that characterizes the gate op eration
element i j of the matrix is the probability that a part entering a gate in state i will exit
in state j Impulse based simulation can easily gather this typ e of information A similar
application is the algorithm of Wiegley et al that generates a sequence of passive fences
to align parts moving on a conveyor b elt In WGPB they analyze their feeders using a
frictionless quasi static mo del
A completely di erenttyp e of part feeding involves micro actuated motion arrays
a typ e of micro electro mec hanical MEM device These devices also have applications in
video displays B ohringer et al have fabricated a motion array comprising several thou
sand torsional resonators distributed over a surface of a few square centimeters BDMM
however di culties in pro ducing and testing these motion arrays have hindered exp erimen
tal results Reznik and Brown are using Impulse to analyze motion arrays based on the
mo del of B ohringer et al Their study fo cuses on determining feed rates for the arrays and
how the p erformance is a ected by variations in certain design parameters and op erating
conditions RB Results such as these might increase understanding of these devices and
suggest future improvements Figure depicts one of Reznik s and Brown s exp eriments
Electrostatic stepp er motors are another typ e of MEM device that could b e improved with
simulation Since their op eration dep ends on transientcontact between com results from
ponents analytic results are di cult to derive Zhuang is using Impulse to analyze MEM
stepp er motors and the e ects of varying design parameters Zhu
Acknowledgments Thanks to Neil Getz for providing his bicycle controller and explain
ing its op eration and to Je Wendlandt for many enlightening discussions Section
was largely extracted from MZG thanks to Randy Brost and Bruce Shimano for use
ful feedback on the initial draft of that pap er Versions of the quasi static algorithm of
Figure A snapshot taken from Impulse during one of the MEM motion array exper
iments described in RB Here two di erent resonator tilings are compared to determine
possible di erences in feed rates reprinted with permission
Section were implemented at USC Wiegley in Zheng Yeh in and Yan
Zhuang in The convex hull routine is due to Ioannis Emiris and John Canny Sp ecial
thanks to Stephen Chenney for the renderings of the controlled hybrid simulations shown in Figures and
Chapter
Future Work
This thesis has intro duced impulse based simulation a paradigm for dynamic sim
ulation of rigid b o dy and articulated b o dy systems The approach pioneered by Hahn has
b een extended re ned and implemented with the prototyp e simulator Impulse Tests with
Impulse show that the metho d is capable of interactive simulation sp eeds for mo derately
complex physical systems and that the physical accuracy of the metho d is suitable for many
applications This nal chapter mentions some challenges that remain and some avenues
for future research
Paradigm switching
Hybrid simulation as presented in this thesis is a somewhat limited blending of
constraint and impulse based simulation techniques Constraints hold together the joints
of a multibody while an impulse based approach is used for the contacts between di erent
multib o dies This expands the domain of what can b e done using impulse based approac hes
but fails to address some basic de ciencies in the metho d What can be done in the case
of a stack of books on a table Here a constrained mo del for contact between bodies is
appropriate but the presented hybrid metho ds do not employ one One way to solve this
problem would b e to detect the onset of static contact b etween b o dies This might b e done
by observing that a high number of low velo city impacts are o ccurring between adjacent
books in the stack At this point a paradigm shift could be made to mo del the contact
interactions b etween b o oks with constraint forces
The transition need not be a sharp one A contact force f that is less than the
true contact force f might initially b e applied b etween the b o oks the slackwould b e picked
up by collisions Even the force f would reduce the number of collisions and increase
the simulation sp eed As f approaches f the number of collisions decreases to zero
and pure constraint based mo deling is achieved A gradual paradigm shift like this seems
less susceptible to anomalous b ehavior that might result from discontinuities in simulation
strategy This impulse to constraint based shift could naturally and e ciently handle a
very common case ob jects coming into initial contact through collision followed by some
period of low impact collisions as the ob jects settle in to continuous contact
Constraint to impulse based paradigm shifts are also appropriate Consider an
arrangementofbowling pins in resting contact with the ground until struckby the inevitable
ball a stack of blo cks at rest until the bottom one is suddenly removed or a skateb oard
in steady rolling contact with the ground until a sp eed bump sends it airb orne In all
of these situations a switch from a constraint based to an impulse based approach seems
appropriate in preparation for p erio ds of transient collision intensive contact An abrupt
t change in the normal force applied b etween a pair of b o dies might b e the triggering even
for this shift from constraint based to impulse based mo deling of the contact
We b elieve that a fuller blending of the impulse and constraint based approaches
through paradigm shifts is a very fruitful area for future research By applying each tech
nique in the cases to which it is well suited the range of physical systems that can be
e ciently simulated can b e signi cantly increased
Collision detection issues
Of ma jor in uence in the design of Impulse s narrow phase collision detection sys
tem was the fact that the Lin Canny algorithm do es not op erate correctly on intersecting
p olyhedra This motivated the one sided approach to nding the moment of collision In
the more common two sided approach the simulation regularly pro ceeds past the moment
of collision at whichpoint the collision detection system detects p enetration and the sim
search or similar techniques can pinp oint the moment of ulation is backed up Binary
collision to within a prescrib ed spatial or temp oral tolerance A disadvantage to the two
sided approach is that it is susceptible to missing collisions But the two sided approachis
likely to converge to the collision time faster than the one sided approach which requires
conservativelower b ounds on the time of impact The two sided approach is certainly easier
to implement when the ob jects have complex dynamics making conservative b ounds hard
to obtain If a way to avoid missing collisions was devised a two sided approach mightbe
more e cient for impulse based simulation
Parallelization is an imp ortant advantage of the impulse based metho d but it
extends only to the dynamic integration phase Parallel collision detection algorithms could
greatly improve p erformance on parallel machines Using the single collision heap scheme
describ ed in this thesis whenever a collision check o ccurs b etween any pair of b o dies the
dynamic integration of every body stops This is clearly ine cient If the b o dies were
partitioned into separate heaps based on spatial lo cality the dynamic integration of a
body would not be halted by irrelevant collision check events the individual heaps could
be parceled among the di erent pro cessors This partitioning is advantageous in single
pro cessor settings as well since certain b o dies could b e evolved further in time for eachcall
to the integrator resulting in larger integration steps and faster simulation The situation
would b e as depicted in Figure All b o dy integrations need not stop for every collision
body n
body i
body 2 body 1
0 1/30tc 2/30 3/30 time (s)
Figure If the dynamic integrations of every body are not required to stop for every
col lision check then the evolution of the states does not have to proceed in lockstep
check so the evolution of the b o dies do es not pro ceed in lo ckstep The bars in the gure
indicate the time to which each b o dy s state has b een evolved for example b o dy has b een
evolved to almost seconds Whenever all b o dies have been evolved to the next frame
refresh time the screen is redrawn with their p ositions at that time Some consistency
problems must b e addressed Referring to the gure if when body i is integrated to time
t a p ossible collision with b o dy is detected then the states of b oth b o dies must b e reset
c
to the last p ointatwhichtheywere veri ed as separate The work p erformed to integrate
body to its current state would be wasted in this case Still this scheme would likely
result in a signi cant p erformance improvementonaverage
Interpreted control and ob ject encapsulation
One p owerful application of simulation is the design and testing of control systems
Exp erience with Impulse has demonstrated that many potential problems with a control
scheme can b e detected and corrected through simulation alone A disadvantage of Impulse s
current architecture is that control laws must b e sp eci ed in C compiled and linked with
the rest of the simulator co de b efore they can be tested This delay in the design test
lo op quickly b ecomes a burden Even more serious is the p ortability problem created when
di erentversions of Impulse have di erent compiled control laws A b etter approachwould
be to sp ecify high level control laws in an interpreted language such as Scheme or Java
GM At a control up date event the necessary dynamic states would be passed to the
interpreter whichwould execute the control law and pass backcontrol forces and torques
More general state information such as that arising during planning could b e kept in static
data structures on the interpreted side For e ciency thelowlevel controllers and springs
be implemented as compiled C routines called by the high level and damp ers could still
controller The interpreter provides a completely general language for sp ecifying control
laws One concern is the impact on execution time although from Table the outlo ok
app ears go o d The table indicates that a very small fraction of the time is sp ent executing
control laws Of this fraction most of the time is devoted to executing low level control
functions such as the PD up date routine For these examples a reasonable estimate is that
less than of the time is sp ent executing user de ned control routines If the interpreted
versions of these routines ran ten times slower than the compiled versions execution time
would only increase
Once the control laws are b eing written in an interpreted language it makes sense
to combine the entire ob ject sp eci cation in such a language Rather than have the geom
etry of an ob ject sp eci ed in some xed format data le it could be sp eci ed by a Java
program that executed calls to compiled library routines like createVertex x y z or
createEdge v v Such calls could b e collected in a list or put inside lo ops so that an
interpreted program could algorithmically build up the geometry The line b etween program
and data b ecomes blurred there is simply a paragraph of co de that generates the geometry
somehow Other imp ortant prop erties for dynamic simulation could also b e sp eci ed by the
same program such as masses or densities of ob jects co e cients of friction and so on Even
the rendering mo del could b e sp eci ed this way although there may b e more reason to keep
that mo del in a xed standardized format for e ciency purp oses Leveraging o the lan
guage interpreter this ob ject sp eci cation format is completely general and p ortable Users
at di erent sites could each create full descriptions of creatures including their geometries
physical prop erties rendering mo dels and b ehaviors implemented through control systems
Scripts describing these creatures could b e submitted to a common simulation engine the
creatures would inhabit a common world and b e free to interact with each other Graphical
results could b e sentback to the remote sites where the creature designs could b e evaluated and re ned Figure
Geometry Behavior attachSpring("knee"); createPID(...); Java case LIFTOFF: if (dist("foot", "ground") > eps) state = FLIGHT; createVertex("v1"); createVertex("v2"); createEdge("v1", "v2"); Graphics
Physics setDensity(5.0); simulator graphical attachPointMass(v5, 100); output Java setMaterial(STEEL); pointer to VRML/Inventor
Java object
description
Figure Interpreted object speci cation al lows generality and portability
Taking the logical next step why make the designer program at all Consider
building a creature to inhabit a virtual world A number of systems have been prop osed
that allow a user to interactively snap together kinematic structures sub ject to dynamic
constraints BB Gle IC WGW One could envision such a system augmented
with widgets representing various typ es of low level controllers that could be attached to
the creature Various typ es of sensors for example compasses or joint enco ders could
be attached as well The sensors could be very simple like enco ders that return a joint
p osition or quite complex like a vision sensor that returns a binary image from a camera
mounted on the front of the creature After the geometry and sensing and control systems
were constructed a high level control system could be sp eci ed again using a graphical
user interface One paradigm wouldbeinteractive design of state machine control systems
where the actions to b e taken at each state and the conditions for state transitions are built
up from primitives based on the control and sensing systems that are on b oard Figure
Finally at the push of a button a complete description of the creature in the form of a Java
N
range spin gyro compass bearing finder vision encoder left
chase
P D reverse
P I D push
computed torque push activate right wheels left wheels rear pistons full fwd full fwd
revolute prismatic spherical
link joint joint joint motor spring damper piston
Figure Aconception of a creature design environment Through a graphical user inter
face a designer completely describes the geometry physics control and sensing rendering
model and behavior for a creature A script describing the creature is automatical ly gener
ated and ready for use in a physical simulation
program is automatically generated and ready for deploymentinto a virtual world Many
details need to be worked out but such a system is entirely p ossible and would greatly
accelerate the creation of interesting virtual worlds p opulated byintelligent creatures
Physical simulation for animation and VR
In recentyears the computer graphics community has increasingly turned to phys
ical mo dels for generating realistic animations Several systems have b een develop ed which
mo del a particular typ e of creature often with intelligence and goals interacting within
its environment just a few examples are in Rey MZ RH Mil NM TT
Sim HWBO The animations these systems pro duce are extremely comp elling and
one can imagine the richness of a virtual world p opulated by these sorts of autonomous
creatures op erating under indep endent control systems and in teracting in complex ways
This has yet to happ en however largely b ecause physically based animation systems have
each been sp ecialized for particular typ es of creatures and environments Miller s system
is for animating snakes Tu s and Terzop oulos s system is for sh Raib ert s and Ho dgins s
system only mo dels contact with a ground plane and so on
This is in no way a criticism of these systems since traditionally such restrictions
have b een necessary to obtain reasonable sp eeds This condition is changing however An
impulse based simulator can e ciently and accurately simulate three dimensional physical
systems comprising rigid and articulated b o dies a fairly general class Fast and robust
general collision detection and physically accurate collision resp onse facilitate realistic in
come together teractions among di erent comp onents or creatures no matter how they
The designer of a creature do es not havetoworry ab out the varietyofcontact interactions
it might encounter in a virtual world correct contact forces will be computed The de
signer can then consider the b ehavior system at a higher level as one would if designing the
controller for a rob ot in the real world Many questions and challenges remain however
Impulse has shed some lighton the p ossibilities that a general purp ose dynamic simulator
could create Such a simulator is needed b efore the work of so many can b e combined in a
virtual environment inhabited bydiverse autonomous agents
App endix A
Mathematical Preliminaries
A Vectors matrices and frames
In this text a frame designates a right handed three dimensional co ordinate sys
tem Frames are usually designated by scripted upp ercase letters such as F The right
handed requirement means that the canonical basis vectors satisfy
i j k
The comp onentsofavector are always expressed relative to the basis vectors of some such
frame whichmay b e implied or stated explicitly
It is often necessary to convert the co ordinates of a vector expressed in one frame to
co ordinates expressed in another frame Let v and v be column vectors comprising
F G
the co ordinates of vector v relative to frames F and G resp ectively Let r r and r be
x y z
the column vector comprising the co ordinates of frame F s i j and k basis vectors
relative to frame G respectively Then
v v
r r r
G F
x y z
z
R
The matrix R is called a change of basis matrix Under the assumption that the two
bases are orthonormal right handed bases the change of basis is always a rotation matrix
This is an orthogonal matrix meaning that its columns are all of unit length and mutually
p erp endicular The same can b e said of its rows Furthermore its determinant is and
it p ossesses the extremely useful prop erty that
T
R R
Matrices must also be transformed to op erate on vectors sp eci ed in di erent
bases For example Let and L be the co ordinates of the rigid b o dy s angular velo city
and angular momentum relative to a frame attached to the b o dy These co ordinate vectors
are related to each other through the mass matrix I
L I
Supp ose now that the b o dy s angular velo city and angular momentum co ordinate vectors
are expressed relativetoadi erent frame xed in space Call these xed frame vectors
and L These two vectors can still b e related through a matrix I given by
T
I RIR
where R is the rotation matrix taking co ordinate vectors in the b o dy frame to co ordinate
vectors in the xed frame The matrix I represents the same linear mapping as I do es it
is just co ordinatized relative to a di erent basis Justi cation for the transformation ab ove
can b e seen from
L I
T
RIR R
RI
RL
Thus the angular momentum vector computed in the xed frame using I and is the
same as that computed in the b o dy frame using I and but with the co ordinates expressed
in the new frame
Matrices can also represent the co ordinates of second order tensors that map two
to a new frame in the vectors into a scalar These co ordinate matrices are transformed
same way See Cul for details
A Representing cross pro ducts as matrices
In the vector space R the cross pro duct isanoperatortakingtwovectors to
a third vector In co ordinates the op eration can b e describ ed by
a b a b
y z z y
a b
a b a b
z x x z
a b a b
x y y x
One can equivalently write
a b ab
where a is the skew symmetric matrix
a a
z y
a
a a
z x
a a
y x
In this way a can b e thought of as a transformation from R to itself taking each vector
v to the cross pro duct a v where a is a xed vector Representing the cross pro duct
op eration as a matrix is often useful in studying equations arising in dynamics this thesis
uses this representation frequently denoting cross pro duct matrices with the standard tilde
notation The matrix a is skew symmetric
T
a a
Let the matrix a corresp ond to the cross pro duct by a op eration in frame F
F
This same op eration may be expressed in a new frame G using the matrix transformation
rule describ ed ab ove If R is the rotation matrix transforming co ordinate vectors in F to
co ordinate vectors in G then the op eration is describ ed in the new frame by
T
a R a R
G F
The matrix a represents the same op eration as a except it op erates on co ordinate vectors
G F
resolved in frame G rather than frame F Since
a Ra
G F
the transformation rule for cross pro duct matrices is often written as
T
g
Ra RaR
Cross pro duct matrices provide a convenient way of writing the derivative of a
time varying rotation matrix
Theorem Derivative of a rotation matrix Let B be a frame rotating relative to
an inertial frame O with instantaneous angular velocity t Let R t be the rotation
matrix that transforms vector coordinates in B to vector coordinates in O If t is the
o
representation of t in frame O coordinates and t is the representation of t in
b
frame B coordinates then
R R
o
R R
b
Proof Let r be an arbitrary vector that is xed in and therefore rotates with frame B
compute the Let r and r be the co ordinate representations of r in O and B One can
o
b
derivative r in several ways
o
d
r Rr Rr R r Rr
o
b b b b
dt
r r Rr Rr
o o o o o
b b
r R r R r
o
b b b b
The rst equation follows from straight di erentiation the second and third follow from
the formula for the velo cityofapoint on a rotating b o dy p erforming the cross pro duct in
frames O and B resp ectively Equating the right hand sides of these three equations
r Rr Rr R
o
b b b b
Since r is arbitrary the result follows
b
A Rigid body dynamics
All of the external in uences on a rigid b o dy may b e summarized as a single force
f t acting at the center of mass of the b o dy and a single moment t applied to the b o dy
Let r t b e the p osition of the center of mass of the b o dy andm themassofthebody By
Newton s second law
d
f t mr t mr t A
dt
This relation can b e rephrased as a rst order ordinary di erential equation by letting v t
denote the velo cityofthecenter of mass of the b o dy Then
r t v t
v t f t
m
For any f t this rst order system can always be numerically integrated to de
termine the motion of the center of mass over time In many cases particularly those
describ ed in this thesis r t can be solved in closed form Consider the case of a bal listic
body where gravity is the only external in uence The gravitational force acts downward
is the gravitational through the center of mass with constant magnitude mg where g
constant Equation A b ecomes
mg k mr t
whichuponintegrating yields
r v t
x x
r t
r v t
y y
gt r v t
z z
The vectors r and v are initial data needed to completely solve the ODE
The other comp onent of rigid b o dy motion is rotation Let t b e the angular
o
velo cityofthebody and the total moment applied to the b o dy The rotational analog
of A is
d
I t t A t
o o o
dt
I t is often called the mass matrix or inertia tensor It describ es how mass is distributed
o
in the body The equation ab ove isonlyvalid when the vectors and matrices are expressed
relative to a frame that it not rotating like a frame O with origin at the center of mass of
the body but with axes that p oint in xed directions The o subscripts denote quantities
expressed in this frame It can b e shown that
T
I R t IR t o
where I is the body frame mass matrix This matrix describ es how mass is distributed in
the body relative to a frame B with origin at the center of mass and that rotates with the
body I is a constant diagonal matrix and R t is the rotation matrix that maps vectors
in B to vectors in O The ab ove equation is just an example of a matrix change of basis
transformation If t and t are the angular velo city and applied moment vectors
expressed in frame B A can b e rewritten
h i
d
T
R t R t IR t R t t
dt
R t I t R t I t
R t t I t R t I t
T
Multiplying b oth sides by R t and solving for t
t I t I t t A
Writing the diagonal elements of I as I I andI
x y z
I I
t
y z
x
t t
t
y z
x
I I
x x
t
y
I I
z x
t t
t
z x
y
I I
y y
I I
t
x y
z
t
t t
z
x y
I I
z z
These are the Euler equations they describ e how b o dy angular velo cityevolves over time
Often the equations of translation and rotation are group ed together and written
X
f t ma t A
X
t I t t I t A
Equation A is just A with a t denoting linear acceleration the second time deriva
tive of p osition Equation A is a restatement of A with t denoting angular
acceleration the time derivative of angular velo city The summation signs are reminders
on the body should be used Together these are that the total force and moment acting
called the Newton Euler equations they completely characterize the dynamics of a rigid
body More detailed treatments of these topics are in Gre MK
A Quaternions and integration of orientation
For dynamic simulation unit quaternions are usually the best representation for
the orientation of a rigid b o dy Unlike Euler angles and related parameterizations quater
nions provide a parameterization with no singularities in the mapping to SO Unlike
rotation matrices quaternions are a minimum dimension nonsingular parameterization for
SO and when the con guration p oint drifts o the manifold it can easily b e pro jected
back onto it by normalizing the quaternion This section derives the ODE that needs to
be integrated for the orientation of a rigid body as a function of time A more detailed
presentation of quaternions is in FP
Every elementofSO can b e identi ed with a rotation ab out some unit vector
u by an angle The matrix corresp onding to this rotation is
u vers cos u u vers u sin u u vers k sin
y x z z x y
x
R A
u u vers k sin u vers cos u u vers u sin
x y z z y x
y
u u vers u sin u u vers k sin u vers cos
z z y y z x
z
where vers cos This orientation can also b e represented by the unit quaternion
q q q q q cos u sin u sin u sin
s x y z x y z
This relation can b e inverted to express u and trigonometric functions of in terms of the
quaternion comp onents
q
x
p
u
q
y
q
s
q
z
cos q
s
q
q sin q
s
s
vers q
s
Substituting these equivalences into A gives R in terms of quaternions
q q q q q q q q q q
y x z s z x y s
s x
R A
q q q q q q q q q q
z y x s x y z s
s y
q q q q q q q q q q
x z y s y z x s
s z
If the quaternion comp onents vary with time
q q q q q q q q q q q q q q q q q q q q
s s x x y x y x z s z s z x z x y s y s
R
q q q q q q q q q q q q q q q q q q q q
s s y y z y z y x s x s x y x y z s z s
q q q q q q q q q q q q q q q q q q q q
x z x z y s y s y z y z x s x s s s z z A
Supp ose R t is the rotation matrix mapping vectors in the b o dy frame to vectors
in a xed frame If is the angular velo city expressed in the body frame then from
Theorem
w w
z y
T
R R
w w
z x
w w
y x
Substituting A and A into the ab ove equation pro duces a complex expression but
it simpli es greatly after applying the unit quaternion restriction and its derivative
q q q q
z y x s
q q q q q q q q
s s x x y y z z
With these substitutions the comp onents of are easily deduced
q q q q q
s x x s z y
q q q q q
x y y z s x
q q q q q
y z z y x s
q q q q q
z s x y z
The last row of the ab ove equation is just the derivative of the unit quaternion
constraint it is added to make the matrix square This matrix is always invertible the
determinantisalways Inverting the equation gives
x y z s w x y z q
x s
s z y x w s z y q
y x
z s x y w z s x q
z y
y x s z y x s q
z
This nal equation expresses the derivatives of the quaternion comp onents in terms of
the angular velo cities It is used by the integrator to evolve the orientation which is
parameterized by a quaternion
Bibliography
ACH O Ahmad J Cremer S Hansen J Kearney and P Willemson Hierarchi
cal concurrent state machines for behavior mo deling and scenario control In
Proceedings of Conference on AI Simulation and Planning in High Au
tonomy Systems Decemb er
Bar David Bara Analytical metho ds for dynamic simulation of non p enetrating
rigid b o dies Computer Graphics July
Bar David Bara Curved surfaces and coherence for non p enetrating rigid body
simulation Computer Graphics August
Bar David Bara Coping with friction for non p enetrating rigid b o dy sim ulation
Computer Graphics August
Bar David Bara Dynamic Simulation of Non Penetrating Rigid Bodies PhD
thesis Department of Computer Science Cornell University March
Bar David Bara Fast contact force computation for nonp enetrating rigid b o dies
In SIGGRAPH Conference Proceedings ACM Press
Bar David Bara Linear time dynamics using lagrange multipliers In SIGGRAPH
Conference Proceedings ACM Press
BB Ronen Barzel and Alan H Barr A mo deling system based on dynamic con
straints Computer Graphics August
BC Dina R Berkowitz and John Canny Designing parts feeders using dynamic
simulation In International Conference on Robotics and Automation pages
IEEE April
BD William E Boyce and Richard C DiPrima Elementary Di erential Equations
and Boundary Value Problems John Wiley Sons Inc New York fourth
edition
BDMM K B ohringer B Donald R Mihailovich and N MacDonald Sensorless ma
nipulation using massively parallel microfabricated actuator arrays In Inter
national Conference on Robotics and Automation IEEE May
BJO H Brandl R Johanni and M Otter A very e cient algorithm for the
simulation of rob ots and similar multib o dy systems In Proceedings of
IFAC IFIP IMACS International Symp osium on the Theory of Robots De
cemb er
BK Vivek Bhatt and Je Ko echling Classifying dynamic b ehavior during three di
mensional frictional rigid b o dy impact In International ConferenceonRobotics
and Automation IEEE May
BK a Vivek Bhatt and Je Ko echling Partitioning the parameter space according
to di erent b ehavior during d impacts Transactions of ASME Journal of
Applied Mechanics To app ear
BK b Vivek Bhatt and Je Ko echling Three dimensional frictional rigid b o dy impact
Transactions of ASME Journal of Applied Mechanics To app ear
BPM Geo rey Bo othroyd Corrado Poli and Laurence E Murch Automatic Assem
bly Marcel Dekker Inc
al Impact Dynamics Rigid Body Col lisions John Bra Raymond M Brach Mechanic
Wiley Sons Inc
Bro Ro dney Bro oks Arobustlayered intelligentcontrol system for a mobile rob ot
IEEE Journal of Robotics and Automation RA March
BS Richard W Bukowski and Carlo H S equin Ob ject asso ciations A simple and
practical approach to virtual d manipulation In Symposium on Interactive
D Graphics pages New York ACM Press
Can John Canny Collision detection for moving p olyhedra Technical Rep ort MIT
A I Lab Memo Massachusetts Institute of Technology Octob er
CEC Alan D Christiansen Andrea Dunham Edwards and Carlos A Co ello Co ello
Automated design of part feeders using a genetic algorithm In International
Conference on Robotics and Automation IEEE
CLMP Jonathan D Cohen Ming C Lin Dinesh Mano cha and MadhavK Ponamgi
I collide An interactive and exact collision detection system for large scaled
environments In Symposium on Interactive D Graphics pages ACM
Siggraph ACM Siggraph April
CPS Richard K Cottle Jong Shi Pang and Richard E Stone The linear comple
mentarity problem Academic Press
Cra John J Craig Introduction to Robotics Mechanics and Control Addison Wes
ley Series in Electrical and Computer Engineering Addison Wesley Reading
MA second edition
CS James F Cremer and A James Stewart The architecture of newton a general
purp ose dynamics simulator In International ConferenceonRobotics and Au
tomation pages IEEE May
Cul Charles G Cullen Matrices and Linear Transformations Dover Publications
Inc New York second edition
Dev Jay L Devore Probability Statistics for Engineering and the Sciences
Bro oks Cole Publishing Company Monterey California
dJB Javier Garcia de Jal on and Eduardo Bayo Kinematic and Dynamic Simulation
of Multibody Systems The Real Time Chal lenge Springer Verlag
Ede H Edelsbrunner A new approach to rectangle intersections part i Interna
tional Journal of Computational Mathematics
EP C H Edwards Jr and David E Penney Calculus and Analytic Geometry
Prentice Hall Inc Englewo o d Cli s second edition
Far Gerald Farin Curves and Surfaces for Computer Aided Geometric Design Academic Press Inc San Diego second edition
Fea R Featherstone The calculation of rob ot dynamics using articulated b o dy
inertias International Journal of Robotics Research
FP Janez Funda and Richard P Paul A comparison of transforms and quaternions
in rob otics In International Conference on Robotics and Automation pages
IEEE May
FR R T Farouki and V T Ra jan Algorithms for p olynomials in Bernstein form
Computer Aided Geometric Design June
GC Ken Goldb erg and John Craig Estimating throughput for a exible part
feeder Simulation and exp eriments In International Symposium of Experi
mental Robotics Stanford University June
Get N H Getz Control of balance for a nonlinear nonholonomic nonminimum
phase mo del of a bicycle In American Control Conference Baltimore June
American Automatic Control Council
GJK Elmer G Gilb ert Daniel W Johnson and S Sathiya Keerthi A fast pro cedure
for computing the distance b etween complex ob jects in three dimensional space
IEEE Journal of Robotics and Automation April
Gle Michael L Gleicher A Di erential Approach to Graphical Interaction PhD the
sis Scho ol of Computer Science Carnegie Mellon University Novemb er
GLM S Gottschalk M C Lin and D Mano cha Obb tree A hierarchical structure
for rapid interference detection In SIGGRAPH ConferenceProceedings ACM
Press
GM James Gosling and Henry McGilton The java language environment A white
pap er Technical rep ort Sun Microsystem
Gre Donald T Greenwood Principles of Dynamics Prentice Hall Inc Englewood
Cli s second edition
Hah James K Hahn Realistic animation of rigid bodies Computer Graphics
August
HBZ Brian Von Herzen Alan H Barr and Harold R Zatz Geometric collisions for
time dep endent parametric surfaces In SIGGRAPH Conference Proceedings
pages ACM Press
HJ Roger A Horn and Charles R Johnson Matrix Analysis Cambridge University
Press Cambridge
Hub Philip M Hubbard Col lision Detection for Interactive Graphics Applications
PhD thesis Department of Computer Science Brown University Octob er
Hub Philip M Hubbard Approximating p olyhedra with spheres for time critical
collision detection ACM Transactions on Graphics July
HWBO Jessica K Ho dgins Wayne L Wo oten David C Brogan and James F O Brien
Animating human athletics Computer Graphics pages Siggraph
Conference Pro ceedings
IC Paul M Isaacs and Michael F Cohen Controlling dynamic simulation with
kinematic constraints b ehavior functions and inverse dynamics In SIGGRAPH
Conference Proceedings pages ACM Press
JR A Jain and G Ro driguez Lecture notes on the spatial op erator algebra for
multib o dy dynamics
Kel J B Keller Impact with friction Journal of Applied Mechanics March
Kha Oussama Khatib A uni ed approach for the motion and force control of rob ot
manipulators The op erational space formulation IEEE Journal of Robotics
and Automation RA February
LC Ming C Lin and John F Canny A fast algorithm for incremental distance
calculation In International Conference on Robotics and Automation pages
IEEE May
Lil Kathryn W Lilly E cient Dynamic Simulation of Robotic Mechanisms
Kluwer Academic Publishers Norwell
Lin Ming C Lin E cient Col lision Detection for Animation and Robotics PhD
thesis University of California Berkeley December
LK Sheue ling Lien and James T Ka jiya A symbolic metho d for calculating the
integral prop erties of arbitrary nonconvex p olyhedra IEEE Computer Graphics
and Applications Octob er
LM M C Lin and Dinesh Mano cha Interference detection b etween curved ob jects
for computer animation In Models and Techniques in Computer Animation
pages Springer Verlag
LM Andrew D Lewis and Richard M Murray Variational principles in constrained
systems Theory and exp eriments International Journal of Nonlinear Mechan
ics
LNPE C Lubich U Nowak U Pohle and Ch Engstler Mexx numerical software
for the integration of constrained mechanical multib o dy systems Technical Re
port Technical rep ort SC Konrad Zuse Zentrum fur Informationstechnik
Berlin
L ot Per L otstedt Coulomb friction in two dimensional rigid body systems
Zeitschrift fur Angewandte Mathematik und Mechanik
L ot Per L otstedt Numerical simulation of time dep endent contact and friction
problems in rigid b o dy mechanics SIAM Journal of Scienti c Statistical Com
puting June
LR a Yong Tsui Lee and Aristides A G Requicha Algorithms for computing the
volume and other integral prop erties of solids I Known metho ds and op en
issues Communications of the ACM Septemb er
LR b Yong Tsui Lee and Aristides A G Requicha Algorithms for computing the
volume and other integral prop erties of solids I I A family of algorithms based
on representation conversion and cellular approximation Communications of
the ACM Septemb er
LRK Paul U Lee Diego C Ruspini and Oussama Khatib Dynamic simulation of
interactive rob otic environment In International Conference on Robotics and
Automation pages IEEE May
MC a Brian Mirtich and John Canny Impulse based dynamic simulation In K Gold
berg D Halp erin J C Latombe and R Wilson editors The Algorithmic
Foundations of Robotics A K Peters Boston MA Pro ceedings from
the workshop held in February
MC b Brian Mirtich and John Canny Impulse based simulation of rigid b o dies In
Symposium on Interactive D Graphics pages New York ACM
Press
Mil Gavin Miller Goal directed animation of tubular articulated gures or how
snakes play golf In Norman A Badler Brian A Barsky andDavid Zeltzer ed
itors Making Them Move Mechanics Control and Animation of Articulated
Figures pages San Mateo CA Morgan Kaufmann Publishers
Inc From the workshop held April
Mir Brian Mirtich Fast and accurate computation of p olyhedral mass prop erties
Journal of Graphics Tools
MK J L Meriam and L G Kraige Engineering Mechanics Volume Dynamics
John Wiley Sons Inc New York
MR J Marsden and T Ratiu Introduction to Mechanics and Symmetry a basic
osition of classical mechanical systems Springer Verlag New York exp
MW Matthew Mo ore and Jane Wilhems Collision detection and resp onse for com
puter animation Computer Graphics August
MZ Michael McKenna and David Zeltzer Dynamic simulation of autonomous
legged lo comotion Computer Graphics pages Siggraph Con
ference Pro ceedings
MZG Brian Mirtich Yan Zhuang Ken Goldb erg John Craig Rob Zanutta Brian
Carlisle and John Canny Estimating p ose statistics for rob otic part feeders In
International ConferenceonRobotics and Automation pages IEEE
April
NM J Thomas Ngo and Jo e Marks Spacetime constraints revisited Computer
Graphics pages Siggraph Conference Pro ceedings
NW James L Nevins and Daniel E Whitney Computer controlled assembly Sci
enti c American
Ove M Overmars Point lo cation in fat sub divisions Information Processing Let
ters
Pau Eric Paulos Parallel impulse based dynamic simulation Available from WWW
site http robotics eecs berkeley edu paulos IMP May
PML Madhav K Ponamgi Dinesh Mano cha and Ming C Lin Incremental algo
rithms for collision detection between solid mo dels In Proceedings of Third
ACM Symposium on Solid Modeling and Applications pages New
York May ACM Press
PS Michael A Peshkin and Arthur C Sanderson The motion of a pushed sliding
workpiece IEEE Journal of Robotics and Automation December
PS Michael A Peshkin and Arthur C Sanderson Minimization of energy in quasi
static manipulation IEEE Transactions on Robotics and Automation
February
PT J S Pang and J C Trinkle Complementarity formulations and existence of
solutions of dynamic multi rigid b o dy contact problems with coulomb friction
Mathematical Programming To app ear
Teukolsky William T Vetterling and Brian R PTVF William H Press Saul A
Flannery Numerical Recipes in C The Art of Scienti c Computing Cambridge
UniversityPress Cambridge second edition
Rai Marc H Raib ert LeggedRobots that Balance The MIT Press series in arti cial
intelligence MIT Press Cambridge
RB Dan S Reznik and Stan W Brown Dynamic simulation of micro actuated
motion arrays with application to design optimization Submitted to
IEEE International Conference on Robotics and Automation
Rey Craig W Reynolds Flo cks herds and scho ols A distributed b ehavioral mo del
Computer Graphics pages Siggraph Conference Pro ceedings
RG Anil Rao and Ken Goldb erg Planning grasps for a pivoting gripp er Technical
rep ort USC Institute for Rob otics and Intelligent Machines Octob er
USC Techrep ort IRIS
RH Marc H Raib ert and Jessica K Ho dgins Animation of dynamic legged lo
comotion Computer Graphics pages Siggraph Conference
Pro ceedings
RK Diego C Ruspini and Oussama Khatib Simpact Available from WWW site
http flamingo stanford edu users ruspini simpact html
RKG A Rao D Kriegman and K Goldb erg Complete algorithms for reorient
ing p olyhedral parts using a pivoting gripp er In International Conference on
Robotics and Automation IEEE May
Rou Edward J Routh Elementary Rigid Dynamics Macmillan London
Sim Karl Sims Evolving virtual creatures Computer Graphics pages
Siggraph Conference Pro ceedings
SSCK K Sridharan H E Stephanou K C Craig and S S Keerthi Distance measures
on intersecting ob jects and their applications Information Processing Letters
August
Ste Sherman K Stein Calculus and Analytic Geometry McGraw Hill Inc New
York third edition
xical theories for rigid b o dy collisions Journal Str W J Stronge Unraveling parado
of Applied Mechanics Decemb er
SWF John M Snyder Adam R Wo o dbury Kurt Fleischer Bena Currin and Alan H
Barr Interval metho ds for multi p oint collisions between time dep endent
curved surfaces In SIGGRAPH ConferenceProceedings pages ACM
Press
TPSL J C Trinkle J S Pang S Sudarsky and G Lo On dynamic multi rigid b o dy
contact problems with coulomb friction Zeitschrift fur Angewandte Mathematik
und Mechanik To app ear
TT Xiaoyuan Tu and Demetri Terzop oulos Arti cial shes Physics lo comotion
p erception behavior Computer Graphics pages Siggraph Con
ference Pro ceedings
Van George Vanec ek Jr Brep index Amulti dimensional spatial partitioning tree
International Journal of Computational Geometry and Applications
Septemb er
WB C Ray Wylie and Louis C Barrett Advanced Engineering Mathematics
McGraw Hill Inc New York fth edition
W GPB Je Wiegley Ken Goldb erg Mike Peshkin and Mike Brokowski A complete
algorithm for designing passive fences to orient parts In International Confer
ence on Robotics and Automation pages IEEE April
WGW Andrew Witkin Michael Gleicher and William Welch Interactive dynamics
Computer Graphics March
WM Yu Wang and Matthew T Mason Mo deling impact dynamics for rob otic
op erations In International Conference on Robotics and Automation pages
IEEE May
dynamic computer simulation of WO M W Walker and D E Orin E cient
rob otic mechanisms Journal of Dynamic Systems Measurement and Con
trol Septemb er
WRG Je Wiegley Anil Rao and Ken Goldb erg Computing a statistical distribu
tion of stable p oses for a p olyhedron In th Annual Al lerton Conference on
Communications Control and Computing
Zhu Yan Zhuang Simulation of linear electrostatic stepp er motor a mems de
vice using impulse Available from WWW site
http www cs berkeley edu yzhuang projects cs report html
May