Convex and analytically-invertible dynamics with contacts and

constraints: Theory and implementation in MuJoCo

Emanuel Todorov

Abstract— We describe a full-featured simulation pipeline im- A. Complementarity-free contact dynamics

plemented in the MuJoCo physics engine. It includes multi-joint
dynamics in generalized coordinates, holonomic constraints, dry The shortcomings of the now-standard complementarity
joint friction, joint and tendon limits, frictionless and frictional approach motivated complementarity-free methods, devel-
contacts that can have sliding, torsional and rolling friction. oped independently in [4] and [5]. The idea is to relax
The forward dynamics of a 27-dof humanoid with 10 contacts the strict complementarity condition and instead enforce it
are evaluated in 0.1 msec. Since the simulation is stable at 10
msec timesteps, it can run 100 times faster than real-time on
approximately. This relaxation transforms the NP-hard prob-
a single core of a desktop processor. Furthermore the entire lem into a convex optimization problem: a conic program
simulation pipeline can be inverted analytically, an order-of- when the friction cone is treated exactly, and a quadratic
magnitude faster than the corresponding forward dynamics. program when the cone is approximated with a pyramid. A
We soften all constraints, in a way that avoids instabilities and systematic analysis of the effects of this approximation for
unrealistic penetrations associated with earlier spring-damper
methods and yet is sufficient to allow inversion. Constraints are
complex robots remains to be done. Nevertheless, synthetic
imposed via impulses, using an extended version of the velocity- tests [6] as well as experience with model-based control [7]
stepping approach. For holomonic constraints the extension indicate that the approximation is accurate. Note also that
involves a soft version of the Gauss principle. For all other the complementarity formulation is not necessarily a gold
constraints we extend our earlier work on complementarity-free standard [8]. All contact models presently used in rigid-
contact dynamics – which were already known to be invertible
via an iterative solver – and develop a new formulation allowing
body simulations are phenomenological, and the only way to
analytical inversion. validate them is to measure the contact interactions between
real robots and their environment – which is rarely done.
Contact softness in particular may be better modeled by the
I. I NTRODUCTION
complementarity-free approach. Such softness arises from
Contacts enable robots to interact with the environment regularization terms that are also essential for inversion.
and get a job done. Yet their discontinuous nature compli-
cates simulation, planning and control. One way to sidestep B. Inverse dynamics with contacts and constraints
these complications is to focus on the smooth dynamics The goal of inverse dynamics it to compute the control
in between contact events, and handle the transitions via forces and constraint impulses given the positions, velocities
problem-specific and often adhoc methods that tend to limit and accelerations. This is complicated by the fact that rigid-
the capabilities of the robot. Another way is to smooth body contact dynamics are not actually invertible. Consider
the contacts using spring-dampers. Here we do not refer to pushing against a wall. The contact force cannot be recov-
detailed models of material deformations (which are accurate ered from the kinematics, unless of course we measure the
yet slow), but rather to phenomenological spring-damper material deformations – but such deformations are ignored
contact models in the context of rigid-body dynamics. That in the rigid-body approximation and in the complementarity
approach has been superseded by the velocity-stepping com- approach. Indeed one of the notable advantages of the
plementarity approach [1], [2], where the contact impulse complementarity approach is that it considers the control
(i.e. the integral of the contact force over the simulation force (as well as all other non-contact forces) before deciding
timestep) is computed by solving a linear or nonlinear what contact impulse to apply. This avoids penetration, but
complementarity problem. Apart from its mathematical el- also makes it impossible to invert the dynamics or smooth
egance, this approach avoids instabilities and unrealistic the contacts. If on the other hand we were to use spring-
penetrations and handles interactions among simultaneous dampers, the contact dynamics would be smooth and trivially
contacts – allowing much larger timesteps. However it also invertible – but as mentioned above, spring-dampers have
has shortcomings: it calls for a computation that is NP- their own limitations that tend to outweigh their advantages.
hard in its exact form [3], and furthermore it only solves Our complementarity-free approach [4] combines the best
the simulation problem while planning and control remain of both worlds: it allows smoothing and inversion, and at the
difficult due to contact discontinuities. same time considers both the inertia and the control forces
in computing the contact impulses. The related approach
This work was supported by the NSF and DARPA. E. Todorov is with the [5] is not amenable to inversion because it uses a two-step
departments of Applied Mathematics and Computer Science & Engineering,
University of Washington. Thanks to Yuval Tassa for discussions and optimization method, reminiscent of the staggered projection
comments on the manuscript. method developed for complementarity problems [3].
 II. F ORWARD DYNAMICS J (q) is computed using collision detection. The quantities
A. Notation p (q, v) , JE (q) are computed from analytical formulas or
user callbacks. The extra diagonal inertia D models the
Throughout the paper we use the following notation: armature inertia of motors as well as implicit damping.
q joint position We do not use Featherstone’s O(n) forward dynamics.
v joint velocity This is because the impulse phase needs the inertia matrix
u control force to be computed and factorized, and once this is done,
h discrete timestep using RNE is faster. Thus our method has O(n3 ) worst-
D armature, implicit damping inertia case performance. However, as Featherstone showed in [10],
M (q) total joint-space inertia branch-induced sparsity typical for robotic systems makes
c (q, v) gravity, Coriolis, centripetal forces the present method very similar to his O(n) method.
p (q, v) spring-dampers, other passive forces In Phase II we compute (fE , f ) given (q, v, u) as explained
JE (q) equality constraint Jacobian in the following subsections. This is done in two stages: we
fE (q, v, u) equality constraint impulse first eliminate fE by expressing it as a function f , and then
J (q) contact Jacobian project the dynamics in contact space and solve for f .
f (q, v, u) contact impulse Before proceeding with the impulse computation, we need
v+ , v− , v∗ impulse-space velocities defined later to transition to discrete time. Let h > 0 be the timestep.
Replace dv (t) with v (t + h) − v (t). All relevant quantities
Since the treatment of equality constraints and contacts is except v (t + h) are defined at time t, thus we omit t and
related, we use the same symbols (f, J) and the subscript write v (t + h) as v 0 . The discrete-time dynamics are
E to distinguish between the two. In addition, the following
model parameters will be defined later: M (v 0 − v) = (p + u − c) h + J T f + JET fE (3)
 impulse regularization scaling 0
Since M is symmetric positive-definite, v can be found as
κ error reduction time constant
v 0 = v + M −1 (p + u − c) h + J T f + JET fE


η loss due to dry joint friction
d number of contact friction dimensions Now that we have transitioned to discrete time, we can
µ1 , · · · , µd coefficients of elliptical friction cone clarify how the implicit damping terms in D are computed.
Multiple friction coefficients are needed to handle tangential, Recall that “implicit” refers to evaluating quantities at the
torsional and rolling friction; thus d can be up to 5. next timestep rather than the current timestep, and makes
numerical integration more stable. In the case of a damped
B. Overall computation 2nd-order system M v̇ = −Bv , implicit damping is imple-
We consider multi-joint systems subject to holonomic mented in discrete time as
constraints such as loop joints, and contact impulses arising M (v 0 − v) = −Bv 0 h
from dry joint friction, joint and tendon limits, frictionless
and frictional contacts. The continuous-time dynamics are These dynamics can be written in the familiar (explicit) form

M dv + c dt = (p + u) dt + J T f + JET fE (1) M (v 0 − v) = −Bvh

We have not divided by dt because fE , f are impulses. by modifying the inertia as M = M + Bh. Thus we imple-
The computation is carried out in two phases. Phase I ment implicit damping by defining D ≡ armature+Bh, and
corresponds to smooth dynamics and preparation for impulse adding −Bv to the passive force p (q, v). We require D to
dynamics, and relies on standard methods [9]. Phase II cor- be diagonal and non-negative, so as to preserve the sparsity
responds to impulse dynamics; it is the more the challenging and positive-definiteness of M .
part and is also where the novelty of our approach lies.
C. Constraint dynamics
In Phase I we compute M, D, c, p, JE , J. In our implemen-
tation in the MuJoCo physics engine, M (q)−D is computed The dynamics of equality-constrained systems can be
with the Composite Rigid Body (CRB) algorithm, then D obtained from the Gauss principle [11], which we now recall.
is added and the resulting M is LDL-factorized taking Suppose we have unconstrained continuous-time dynamics
advantage of branch-induced sparsity. c (q, v) is computed M a = τ subject to acceleration constraints JE a = a∗E .
with the Recursive Newton-Euler (RNE) algorithm – which Then, given M, τ, JE , a∗E , the constrained acceleration is the
is actually an algorithm for inverse dynamics: solution to the convex optimization problem
T
RNE (q, v, v̇) = (M (q) − D) v̇ + c (q, v) (2) min (M a − τ ) M −1 (M a − τ ) (4)
a : JE a=a∗
E

Here we use it to compute RNE (q, v, 0) = c (q, v). Note Since we aim to invert the dynamics later, and hard con-
that CRB and RNE do not take into account the extra inertia straints are non-invertible, we must soften the constraints
D, which is why we have to add it to the output of these somehow. We propose to do this by softening the Gauss prin-
algorithms to obtain the total inertia M . The contact Jacobian ciple, i.e. replacing the hard constraint with a soft penalty.
 Proposition. The acceleration of the constrained system is and v+ ≡ Jv 0 is the next-step velocity after the contact
defined as the solution to the convex optimization problem impulse, all expressed in contact space.
T To complete the forward dynamics computation we have
min (M a − τ ) M −1 (M a − τ ) + (5) to solve (8) for (f, v+ ) given (A, v− ). This involves twice
a
(JE a − a∗E )
T
R−1 ∗ as many unknowns as the number of equations in (8), thus
E (JE a − aE )
we need additional information – which comes in the from
where the regularizer RE is a diagonal positive matrix. of inequality constraints reflecting the laws of contact and
In the limit RE → 0 the solution to problem (5) converges friction. We now introduce these inequalities, and at the same
to the solution to problem (4), which is non-invertible. How- time explain what exactly is included in the contact space.
ever for any RE  0 the resulting dynamics are invertible, In our current implementation in MuJoCo the contact
as shown by construction later. solver can handle three types of objects: friction loss in the
We now return to discrete time and impose a soft con- joints, limits on joint angles and distances, and frictional
straint on velocity rather than acceleration: JE v 0 = v∗E (q, v). contacts. The elements fi of the vector f satisfy different
Here v∗E is the desired next-step velocity in constraint space, inequality constraints depending on the type of object they
computed by any suitable constraint stabilization mechanism. represent, as follows:
Our specific choice will be described later.
friction: η (i) − fi ≥ 0
Theorem 1. The dynamics under the proposed soft Gauss
principle are η (i) + fi ≥ 0
v 0 = vb + M
c−1 J T f (6) limit: fi ≥ 0
(9)
where M
c and vb are defined as
contact: fi ≥ 0
d(i)  2
c ≡ M + JET R−1 JE
M (7) X fi+j
E
fi2 − ≥0
−1 T −1 ∗ µj (i)


vb ≡ v + M
c (p + u − c) h + JE RE (vE − JE v) j=1

Proof. Apply the soft Gauss principle with Assembling the left hand sides of the above inequalities in
the vector φ (f ), we can write (9) as φ (f ) ≥ 0.
v0 − v ∗ v∗ − JE v JT f In (9) subscripts denote vector elements as usual, while
a= , aE = E , τ =p+u−c+
h h h i in brackets denotes parameters of the object whose data
Solving (5) analytically yields (6, 7).  starts at position i in the vector f . In the case of friction
We can interpret M
c as the apparent inertia that takes into loss, η (i) is the (load-independent) joint torque that is lost
account the constraints, and vb as the next-step velocity that to friction before the joint starts accelerating. Limits can
takes into account all forces except for the contact impulse. be defined for revolute, prismatic and ball joints (the latter
For hard constraints the dynamics remain in the general limits are cylinders in the angle-axis representation of the
form (6), but the definitions (7) are replaced with joint quaternion), as well as for tendon lengths (tendons
are strings whose spatial path is defined by via points and
c−1 ≡ M −1 − M −1 JET A−1 JE M −1
M E wrapping objects), and distances between geometric shapes
vb ≡ v + M (p + u − c) h + M −1 JET A−1 (v∗E − JE v)
c−1
E (i.e. frictionless contacts).
In the case of frictional contacts, d (i) ∈ {2, 3, 5} is
where AE ≡ JE M −1 JET is the inverse inertia in constraint the dimensionality of the friction space and µj (i) are the
space. This can be shown by solving (4), or by taking the friction coefficients in the d (i) dimensions. The contact
limit RE → 0 and using the matrix inversion lemma. contributes 1 + d (i) elements to the vector f with the
Equations (6, 7) represent modified dynamics which im- following semantics. Consider a 3D frame whose first axis
plicitly take the constraints into account. We did not compute is aligned with the contact normal. Impulses (fi , fi+1 , fi+2 )
the impulse fE explicitly because it can only be computed cause relative translation along the contact frame axes, while
after v 0 is known, i.e. in the context of inverse dynamics. (fi+3 , fi+4 , fi+5 ) cause relative rotation around the axes.
D. Contact dynamics Thus fi is the normal impulse, (fi+1 , fi+2 ) is the tangential
friction impulse, fi+3 is the torsional friction impulse, and
We now have everything in place for the contact com- (fi+4 , fi+5 ) is the rolling friction impulse. The contact
putation stage. First we project (6) in contact space via inequalities in (9) specify that the normal impulse must be
multiplication by the contact Jacobian J: non-negative, and that the impulse vector must lie within the
Jv 0 = J vb + J M
c−1 J T f elliptical friction cone. In the special case when d (i) = 2
and µ1 (i) = µ2 (i) = µ (i), this reduces to the more familiar
Then we write the contact-space dynamics as definition of a friction cone:
q
Af + v− = v+ (8) 2
µ (i) fi ≥ fi+1 2
+ fi+2
c−1 T
where A ≡ J M J is the inverse of the apparent inertia, We now return to the computation of the contact impulse
v− ≡ J vb is the next-step velocity before the contact impulse, f and next-step velocity v+ . Conditions (8, 9) are still
insufficient to determine a unique solution. There are two Proof. Combining (6) and (7) yields
paths forward: complementarity-based and complementarity-
M + JET R−1

0
free. The former approach introduces additional constraints E JE (v − v) =

(complementarity conditions) to obtain a unique solution. For (p + u − c) h + J T f + JET R−1 ∗

E (vE − JE v)
example, the contact normal inequality fi ≥ 0 is augmented Subtracting this from (3) yields the result (13). 
with vi+ ≥ 0 and fi vi+ = 0. In the presence of frictional We can further recover the actual fE if we assume that the
contacts, this approach yields an NP-hard problem in the equality constraints are non-redundant (i.e. that JE has full
forward dynamics. Furthermore its inverse cannot be defined. rank). However this assumption is not needed to complete the
Thus we will not pursue it further in this paper. Instead we inverse dynamics computation, because (12) only depends on
will rely on the complementarity-free approach. JET fE which we already have from (13).
E. Complementarity-free contact dynamics C. Inverse contact dynamics: General case
In our approach to contact dynamics [4], the impulse f In our previous work [4] we showed how the convex
is defined via minimization of the (regularized and offset) contact model described above can be inverted in an uncon-
next-step kinetic energy subject to (9): strained setting, by converting the inequality constraints into

T
v+ − v∗ A−1 v+ − v∗ + f T Rf log-barrier penalty functions. Here we present a more general


min (10)
f : φ(f )≥0 version of this result, allowing hard inequality constraints –
The regularizer R is a diagonal positive matrix, which is which in turn make it possible to use projected and active-set
needed because A can be singular and also because it methods in the forward dynamics. We first state the abstract
introduces smoothing that is necessary to define the inverse result and then specialize it to dynamic simulation.
later. Kinetic energy is measured relative to a desired contact Theorem 3. Let A ∈ Rn,n be symmetric positive semi-
velocity v∗ which can be computed by any suitable contact definite, r, s : Rn → R be convex, φ : Rn → Rn be convex,
stabilization mechanism (details below). and b, c ∈ Rn . Define x∗b , x∗c ∈ Rn as the (unique global)
Substituting (8) in (10), the impulse f is found as solutions to the following convex optimization problems:
1 T 1
f (A + R) f + f T v− − v∗ x∗b : min r (x) + s (Ax + b) + xT Ax + xT b


min (11)
f : φ(f )≥0 2 x: φ(x)≥0 2
(14a)
This is a convex optimization problem and has a unique
global minimum. We solve it using our (yet unpublished) x∗c : min T
r (x) + x (A∇s (c) + c) (14b)
x: φ(x)≥0
generalization of the projected Gauss-Seidel method (GPGS)
that can handle cone and pyramid constraints. Then Ax∗b + b = c implies x∗b = x∗c .
Proof . Since x∗b , x∗c are defined as solutions to convex opti-
III. I NVERSE DYNAMICS mization problems, they can be equivalently characterized by
A. Overall inverse computation the corresponding Karush-Kuhn-Tucker (KKT) conditions.
Given (q, v, v 0 ), we compute J, JE , D, p, v∗ , v∗E as in the The KKT conditions for problem (14a) are:
forward dynamics, and define v̇ ≡ (v 0 − v) h−1 . Then we  T
∂φ
apply the RNE algorithm to compute the sum of all forces ∇r (x) + A∇s (Ax + b) + Ax + b + λ=0
∂x
acting on the system except for the Coriolis, centripetal and T
gravity force c (q, v) and the extra inertial force Dv̇. More φ (x) ≥ 0, λ ≥ 0, φ (x) λ = 0
precisely, from (1) and (2) we have The KKT conditions for problem (14b) are:
T
J f+ JET fE  T
∂φ
RNE (q, v, v̇) + Dv̇ = p + u + (12) ∇r (x) + A∇s (c) + c + λ=0
h ∂x
Once the impulses f, fE are computed as explained below, T
φ (x) ≥ 0, λ ≥ 0, φ (x) λ = 0
the control force u is recovered from (12) and we are done.
Note that we did not need to compute or factorize the These two sets of conditions are identical when Ax + b = c.
inertia matrix M . It will turn out that M is not needed to Therefore, if the solution x∗b to the first problem satisfies
recover the impulses either. Ax∗b + b = c, then x∗b will also be the solution to the second
problem, and so x∗b = x∗c . 
B. Inverse constraint dynamics
This theorem can be applied to complementarity-free
Recall that in the forward dynamics we did not compute contact dynamics by identifying x with f , b with v− , c with
the constraint impulse fE , but only modified the dynamics so v+ , and absorbing the linear term −f T v∗ in r (f ). Thus we
as to take it into account implicitly. Here fE can be computed have the following corollary.
explicitly because v 0 is known. Corollary. Given A, v∗ , v− , the impulse is computed by
Theorem 2. The constraint impulse fE which caused the solving the (forward dynamics) convex optimization problem
observed state transition (q, v) → v 0 satisfies
1
min r (f ) + s Af + v− + f T Af + f T v− − v∗


JET fE = JET R−1 ∗ 0
E (vE − JE v ) (13) f : φ(f )≥0 2
Given A, v∗ , v+ , the impulse is computed by solving the This general problem cannot be solved analytically be-
(inverse dynamics) convex optimization problem cause both the cone and the distance metric are elliptical,
making the problem equivalent to finding the roots of a
min r (f ) + f T A∇s v+ + v+ − v∗



f : φ(f )≥0 polynomial of order 2 (d + 1). Our goal then is to identify
conditions that simplify the problem, in particular make the
Both computations yield the same impulse f .
cone circular and the metric Cartesian. The metric is made
D. Inverse contact dynamics: Analytical special case Cartesian by the change of variables
We now focus on the specific formulation (11) used in 1/2 1/2
bk ≡ rk xk ,
x ybk ≡ rk yk (16)
the forward dynamics, where instead of a general impulse
regularizer r (f ) we had The minimization problem now becomes
X 2
1 T min xk − ybk )
(b
f Rf − f T v∗
r (f ) = x b∈Cb
b:x k
2
In this case the inverse can be computed analytically. In where the transformed friction cone Cb is
particular, problem (14b) can be written in least-squares form
n X o
Cb = x b0 ≥ 0, r0−1 x
b ∈ Rd+1 : x b20 ≥ µ−2
j rj
−1 2
x
b j
1 j
T
min (f − y) R (f − y) (15)
f : φ(f )≥0 2 This cone is circular when µ2j rj
= const, which can be
enforced by requiring that there exists a scalar µ
b such that
where the vector y is defined as
µ2j rj = µ
b2 r0 (17)
y ≡ R−1 v∗ − v+

The transformed friction cone then takes the familiar form

Including a general velocity regularizer s (v+ ) would merely n X o
add a constant to y and still allow the inverse to be computed Cb = xb ∈ Rd+1 : x b2 x
b0 ≥ 0, µ b20 ≥ b2j
x
j
analytically, but we omit it here so as to match the forward
dynamics formulation (11) used by our GPGS solver. The resulting optimization problem can now be solved
We now make a key observation: the terms corresponding analytically. If yb ∈ Cb then x
b = yb and we are done. Otherwise
to the different objects used to construct the contact space b lies on the surface of the cone Cb and can be found via
x
are decoupled, both in the objective function and in the Lagrange multipliers: there exists a scalar λ such that
constraints. Thus problem (15) decomposes into a collection b − yb + λ µ
x b2 x
b0 , −b
x1 , · · · , −b

T
xd = 0
of smaller problems – one for each friction loss, limit, and
frictional contact object. These smaller problems can be We can find λ by expressing x
b as a function of yb and λ:
solved analytically as follows. yb0 ybj
Theorem 4. The solution to the inverse dynamics opti- x
b0 = , x
bj = (18)
1+ µ b2 λ 1−λ
mization problem (15) is
Substituting this x
b in the surface equality constraint and
friction: fi = max (−η (i) , min (η (i) , yi )) solving for λ yields two general solutions:
limit: fi = max (0, yi ) P 2 !1/2
± 1∓β jybj
contact: fi , · · · , fi+d(i) = ConeProject (y, i) λ = , β= (19)
1±µ 2
b β b yb02
µ2
ConeProject extracts the contact-specific data yi , · · · , yi+d(i)
from y, and then calls Algorithm 5 below to compute the with corresponding vectors x b± given by (18). If

both vectors
nearest vector within the friction cone. have non-negative first components x b±
0 ≥ 0 , the optimal
solution is the one closer to yb. If only one vector has non-
E. Projection on a friction cone negative first component, it is the optimal solution. If both
Here we develop the analytical procedure for projecting a have negative first components, the optimal solution is x
b = 0.
vector on a friction cone with certain properties. Let C be an We must also handle two special cases where the above
elliptical cone defined as general method involves division by 0. If yb0 = 0 we have
 Xd  1+ µ b2 λ = 0. In that case the solution can be shown to be
d+1 2 −2 2
C≡ x∈R : x0 ≥ 0, x0 ≥ µj xj
j=1 b X 2 1/2
µ b2
µ
x
b0 = yj , xj = ybj (20)
b2 b2
b b
We seek the vector x ∈ C which minimizes the weighted 1+µ j 1+µ
distance to a given vector y ∈ Rd+1 : The other special case is ybj = 0 for all j ≥ 1, resulting in
Xd 1 − λ = 0. In that case the solution is the tip of the friction
2
min rk (xk − yk ) cone x b = 0.
x : x∈C k=0
We now clarify how the contact model can be constructed
Here r ∈ Rd+1 is a vector of positive weights, i.e. the so as to obey the restriction (17). The regularizing weights
diagonal of the matrix R above. Throughout this section k r0 , · · · , rd together with the friction coefficients µ1 , · · · , µd
is an index starting at 0 while j is an index starting at 1. and the transformed coefficient µ b appear to have 2d + 2
degrees of freedom, however d of them are removed due V. A NALYSIS AND TUNING OF THE IMPULSE DYNAMICS
to (17), leaving us with d + 2 degrees of freedom. The Here we analyze the behavior of the dynamics defined
friction coefficients clearly need to be under the control of above. We also show how to set the solver parameters
the user, thus only two of the regularizers can be specified R, RE , v∗ , v∗E so as to obtain a stabilization mechanism.
independently. It is then natural to construct the contact For contacts we focus on the case when all inequality
model by specifying the following independent parameters: constraints are inactive, i.e. φ (f ) > 0 at the solution f found
friction coefficients: µ1 , · · · , µd by the impulse solver. Physically this corresponds to non-
normal regularizer: r0 (21) sliding contacts (modulo contact softness). In that case, from
mean friction regularizer: rF (15) we have
f = R−1 (v∗ − Jv 0 ) (23)
Defining
rF µ−2
j rF Note how similar this contact impulse is to the constraint
rj ≡ , b2 ≡
µ (22) impulse (13), despite the fact that the two were defined and
µ−2
j r0 µ−2
j
computed differently. This observation motivates analysis
where h.i denotes the mean value, we can verify that (17) in the combined space of constraints and contacts, with
is satisfied and furthermore rF = hrj i as intended. We now coordinates x defined as follows. For constraints, limits and
summarize the algorithm. contact normals xi is the violation/penetration distance. For
Algorithm 5. Given the contact model parameters (21) frictional dimensions xi is defined relative to an arbitrary
and the vector y, compute the vector x as follows: offset (we only care about the velocity in that case).
1) if y is inside the friction cone, set x = y and return; Define the Jacobian J , regularization matrix R and
2) if yj = 0 for all j ≥ 1, set x = 0 and return; desired next-step velocity ẋ∗ in x-space by stacking the
3) compute rj and µ b2 from (22); corresponding quantities for constraints and contacts:
4) compute yb from (16);      ∗ 
JE RE 0 vE
5) if yb0 = 0, compute x b from (20) and go to step 8; J ≡ , R≡ , ẋ∗ ≡
J 0 R v∗
6) compute x b± from (18, 19);
7) choose the optimal x b among {b x± , 0}; Then the velocity and inverse inertia in x-space are
8) compute x by inverting (16).
ẋ = J v, A = J M −1 J T
As a sanity check, we generated random optimization
problems in this family and compared our analytical solution As before, ẋ0 = ẋ + hẍ will denote the actual next-step
to the solution found by the MATLAB fmincon iterative velocity. We will also need the x-space acceleration that the
solver. The two solutions agreed within the tolerance level non-impulsive forces cause:
specified for the iterative solver.
a ≡ J M −1 (p − c + u)
IV. C OMPUTATIONAL COMPLEXITY Finally we must decide how the desired next-step velocity
Let n = dim(v) be the number of degrees of freedom and ẋ∗ is computed; different choices give rise to different
m = dim(f ) the number of impulses. We will ignore holo- dynamics. Motivated by the idea of Baumgarte stabilization
nomic constraints in this analysis because the computational [12], we consider a virtual PD controller that causes accel-
cost is dominated by the contact impulses. eration −B ẋ − Kx. Thus ẋ∗ is defined as
The forward dynamics involve
computing and factorizing
the inertia M which is O n 3
. Computing A = JM −1 J T ẋ∗ ≡ ẋ − hB ẋ − hKx (24)
2 2


is O m n + n m
. Applying the GPGS iterative solver We now have everything in place to obtain the dynamics.
is O m2 maxiter . Note however that the branch-induced Theorem 6. The x-space dynamics are
sparsity of M as well as the sparsity of J make the actual
I + AR−1 ẍ + AR−1 B ẋ + AR−1 Kx = a


performance better than these worst-case estimates. Our (25)
implementation exploits sparsity. Proof. Substituting (23) and (13) in (3) yields
The inverse dynamics involve RNE which is O(n), as well
as our new analytical impulse solver which is O(m). We use M (v 0 − v) = (p − c + u) h+
a precomputed approximation to the diagonal of A to set the JET R−1 ∗ 0 T −1
(v∗ − Jv 0 )
E (vE − JE v ) + J R
regularizer R, avoiding any higher-order operations. Thus
the worst-case performance is dominated by the computation Multiplying by J M −1 and using the above definitions,
of the contact Jacobian J which is O(mn). Again, sparsity
ẋ0 − ẋ = ah + AR−1 (ẋ∗ − ẋ0 )
makes the actual performance better.
The above computational complexity analysis does not Using the definitions of ẋ0 and ẋ∗ yields the result (25). 
include collision detection – which is identical in both the We can now gain a better understanding of what the
forward and inverse dynamics. In simulations relevant to impulse solver is doing. Suppose we set R = A for some
robotics, we have found collision detection to be a small positive . Since A and R have the same units (they were
fraction of the computational cost. added together in (11)),  is a dimensionless constant. Also
 Friction impulse

Contact impulse
Time (2 sec) Time (2 sec)

Fig. 1. The two MuJoCo models used in the simulations. Left: single-joint Fig. 2. Comparison of the friction impulse and contact impulse computed
mechanism with dry friction. The arrow shows the contact impulse. Right: by the forward (black) and inverse (orange) dynamics, while the single-
27-dof humanoid. The red cylinders show the active contacts. Joint limits joint system was being perturbed randomly. The two curves in each plot
prevent more contacts. are within 1E-5 of each other, thus the difference is not visible.

set B = B (1 + ) , K = K (1 + ) for some B, K. Assuming To summarize, the user specifies the impulse regularization
for the moment that A is invertible, (25) becomes scaling  and the error reduction time constant κ, and then the
 solver parameters R, RE , v∗ , v∗E are computed automatically
ẍ + B ẋ + Kx = a (26) using (27, 28, 24) with the above modifications.
1+
Instead of using a diagonal approximation, we can obtain
Therefore in the limit  → ∞ we have a spring-damper
a stabilization mechanism that is closer to critical damping
driven by a. In the limit  → 0 the spring-damper becomes
as follows. Define a diagonal R as above. Now solve the
autonomous and removes constraint violations regardless of
following equations for B and K:
a. The latter limit corresponds to hard constraints and con-
AR−1 B = 2κ−1 I + AR−1


tacts. In practice we use small . Since our solver implements
the above dynamics implicitly, we can get very close to the AR−1 K = κ−2 I + AR−1


 → 0 limit without instabilities, numerical errors, or need
for small timesteps. If A is invertible these equations can be solved exactly,
While (26) is illuminating, it does not apply in general resulting in exact critical damping. If not, then a pseudo-
because A can be singular; for example, two frictional inverse can be used. The resulting B and K are no longer
contacts on the same body make A singular. Furthermore diagonal, but that does not complicate the computation.
we cannot set R = A because we want R to be diagonal,
VI. S IMULATIONS
so that the dynamics can be inverted analytically.
In our implementation we set R to a diagonal matrix: A. Correctness
We demonstrate numerically the correctness of the in-
Rii = max (Aii , rmin ) , (27)
version using a single-joint mechanism (Figure 1 left) with
Similarly the stiffness and damping are diagonal: two impulses: dry friction in the joint and contact with the
ground. Figure 2 compares these impulses as computed by
Bii = 2 (1 + ) κ−1 , Kii = (1 + ) κ−2 (28) the forward and inverse dynamics, while the mechanism is
We can now carry the analysis of (25) further by approx- perturbed with random control forces.
imating A with its diagonal, resulting in Recall that the impulses in the forward dynamics are
computed with our GPGS iterative solver which may not
 discover the optimal solution within the number of iterations
ẍ + 2κ−1 ẋ + κ−2 x ≈ a
1+ we allow in runtime. Indeed for more complex simulations
Note that the choice of stiffness and damping coefficients in the forward and inverse do not agree so closely, making
(28) made the autonomous part of the dynamics critically- the inverse dynamics approach more appealing for optimal
damped, where κ−1 is the natural frequency and so κ is the control and estimation applications.
time constant of the x-space dynamics. It is also informative
B. Speed of computation
to look at penetrations. If we consider a free-floating object
resting on the ground in the presence of gravity g, we have Figure 3 illustrates the computational efficiency of our
a = g, and so the penetration is algorithms and their implementation in MuJoCo. The tests
were done on an Intel i7-3930K processor, Windows 7,
κ2 g single-threaded computation taking advantage of AVX in-
x≈
1+ structions (with custom BLAS-like routines). We dragged
independent of the mass of the object. the humanoid model around in the virtual environment
There are two final caveats. First, the Rii corresponding using a 3D mouse. This generated many different contact
to the friction dimensions of each contact must be further configurations with different Jacobian size. We run both the
adjusted so as to satisfy (22). Second, in friction dimensions forward and inverse dynamics and timed them using high-
we only care about velocity, thus we set the corresponding resolution timers. The timing data and size of the Jacobian
Kii = 0. In that case the velocity decays to 0 at rate 2κ−1 , at each simulation step were saved in a file. Afterwards, we
and κ again has the meaning of a time constant. found all simulation steps in which the Jacobian had a given
 500 20
CPU time per step (microsec) the input, but the actual computation is not affected since it
400 Forward(50) / Inverse always relies on the same analytical procedure.
15
Our softness parameters , κ are generally related to the

Speedup factor
300 constraint-force mixing (CFM) and error reduction parameter
Forward(50)
10 (ERP) introduced in the Open Dynamics Engine (ODE),
200
although the specifics are different. CFM regularization in
5 ODE is applied to the full system while we only apply
100 Forward(5) Forward(5) / Inverse
Inverse
regularization in the impulse space, and furthermore the
0 0 problem solved by ODE does not become convex even after
0 20 40 60 80 100 0 20 40 60 80 100
Size of contact Jacobian Size of contact Jacobian
regularization. ERP implements a first-order constraint stabi-
lization mechanism while we use a second-order mechanism.
Fig. 3. Comparison of the speed of the forward and inverse dynamics Our results have many potential applications in data anal-
computations for the humanoid. Left: CPU time per simulation step is shown ysis, simulation, estimation and control. We have already
for different sizes of the contact Jacobian. The PGS iterative solver in the
forward dynamics was run for 5 or 50 iterations. Right: the speedup factor
leveraged the new analytical inverse dynamics in the context
is the ratio of forward dynamics CPU time to inverse dynamics CPU time. of state estimation, using it to enforce a physics consis-
tency prior [13]. Future applications to optimal control are
size (plotted on the x-axis in Figure 3), and computed the particularly exciting. We were recently able to synthesize
median of the corresponding CPU times. We performed the complex full-body movements with direct trajectory opti-
test twice, with 5 and 50 iterations of the GPGS solver in the mization automatically, without relying on motion capture
forward dynamics. To generate larger Jacobians we enabled or manual scripting [14]. While this was done offline, we
all contact friction directions. For 10 contacts with sliding estimate that in model-predictive control (MPC) mode our
friction only, the Jacobian size would be 30. existing optimizer will be an order of magnitude slower than
We find the results in Figure 3 quite remarkable. On a real time. The analytical inverse dynamics developed here
single core of a desktop processor, we can evaluate the may provide the missing order of magnitude speedup. If we
inverse dynamics of a 27-dof humanoid subject to 100 could replicate [14] in MPC mode, it is likely to transform
impulses in 30 microseconds. In the absence of impulses the robotic control as well as interactive games.
time goes down to 10 microseconds. Since the simulation
timestep is 10 milliseconds, the inverse dynamics are being R EFERENCES
computed between 300 and 1000 times faster than real time [1] D. Stewart and J. Trinkle, “An implicit time-stepping scheme for
rigid-body dynamics with inelastic collisions and coulomb friction,”
depending on the number of impulses. The forward dynamics International Journal Numerical Methods Engineering, vol. 39, pp.
are slower – by an order of magnitude – but still much faster 2673–2691, 1996.
than real time. Note that increasing the number of GPGS [2] M. Anitescu, F. Potra, and D. Stewart, “Time-stepping for three-
dimensional rigid body dynamics,” Computer Methods in Applied
iterations from 5 to 50 is expensive with large number of Mechanics and Engineering, vol. 177, pp. 183–197, 1999.
impulses, but the inverse dynamics are faster regardless of [3] D. Kaufman, S. Sueda, D. James, and D. Pai, “Staggered projections
the number of GPGS iterations. This is because computing for frictional contact in multibody systems,” ACM Transactions on
Graphics, vol. 164, pp. 1–11, 2008.
the full A matrix is avoided. [4] E. Todorov, “A convex, smooth and invertible contact model for
trajectory optimization,” ICRA, 2011.
VII. C ONCLUSIONS AND FUTURE WORK [5] E. Drumwright and Shell, “Modeling contact friction and joint fric-
tion in dynamic robotic simulation using the principle of maximum
We described a full-featured simulation pipeline for multi- dissipation,” International Workshop on the Algorithmic Foundations
joint dynamics subject to constraints and contacts. Forward of Robotics, 2010.
simulation requires an iterative solver, while inverse dynam- [6] E. Drumwright and E. Shell, “An evaluation of methods for modeling
contact in multibody simulation,” ICRA, 2011.
ics are computed analytically and faster. [7] Y. Tassa, T. Erez, and E. Todorov, “Synthesis and stabilization of
The inversion is possible because our formulation allows a complex behaviors through online trajectory optimization,” IROS,
certain amount of softness. One can think of the impulses as 2012.
[8] A. Chatterjee and A. Ruina, “A new algebraic rigid body collision
being generated by smart spring-dampers, which are aware of law based on impulse space considerations,” Journal of Applied
each other and furthermore scale their stiffness and damping Mechanics, 1998.
automatically with inertia. The forward formulation consid- [9] R. Featherstone, Rigid Body Dynamics Algorithms. Springer, 2008.
[10] ——, “Efficient factorization of the joint-space inertia matrix for
ers the interactions among all impulses, involving the dense branched kinematic trees,” International Journal of Robotics Research,
matrix A and the diagonal regularizer R. The mathematical 2005.
relation between the forward and inverse is such that the A [11] F. Udwadia and R. Kalaba, “A new perspective on constrained motion,”
Proceedings of the Royal Society, 1992.
matrix is no longer needed in the inverse, and we are left only [12] J. Baumgarte, “Stabilization of constraints and integrals of motion in
with the R matrix allowing us to decompose the problem. dynamical systems,” Computer Methods In Applied Mechanics And
Softness is controlled by the two parameters , κ whose Engineering, 1972.
[13] K. Lowrey, Y. Tassa, T. Erez, S. Kolev, and E. Todorov, “Physically-
effects we analyzed. Small values of these parameters make consistent sensor fusion for contact-rich behaviors,” manuscript under
the forward dynamics harder to simulate (requiring smaller review, 2014.
timesteps or larger number of solver iterations). They also [14] I. Mordatch, E. Todorov, and Z. Popovic, “Discovery of complex
behaviors through contact-invariant optimization,” SIGGRAPH, 2012.
make the output of the inverse dynamics more sensitive to