arXiv:1609.02898v2  [cs.RO]  5 Feb 2018
A Linear-Time Variational Integrator
for Multibody Systems
Jeongseok Lee1, C. Karen Liu2, Frank C. Park3, and Siddhartha S. Srinivasa1
1 Carnegie Mellon University, Pittsburgh, PA, USA 15213
{jeongsel, ss5}@andrew.cmu.edu
2 Georgia Institute of Technology, Atlanta, GA, USA 30332
karenliu@cc.gatech.edu
3 Seoul National University, Seoul 151-742, Korea
fcp@snu.ac.kr
Abstract. We present an efficient variational integrator for simulating
multibody systems. Variational integrators reformulate the equations of
motion for multibody systems as discrete Euler-Lagrange (DEL) equa-
tion, transforming forward integration into a root-finding problem for
the DEL equation. Variational integrators have been shown to be more
robust and accurate in preserving fundamental properties of systems,
such as momentum and energy, than many frequently used numerical
integrators. However, state-of-the-art algorithms suffer from O(n3) com-
plexity, which is prohibitive for articulated multibody systems with a
large number of degrees of freedom, n, in generalized coordinates. Our
key contribution is to derive a quasi-Newton algorithm that solves the
root-finding problem for the DEL equation in O(n), which scales up well
for complex multibody systems such as humanoid robots. Our key insight
is that the evaluation of DEL equation can be cast into a discrete in-
verse dynamic problem while the approximation of inverse Jacobian can
be cast into a continuous forward dynamic problem. Inspired by Recur-
sive Newton-Euler Algorithm (RNEA) and Articulated Body Algorithm
(ABA), we formulate the DEL equation individually for each body rather
than for the entire system, such that both inverse and forward dynamic
problems can be solved efficiently in O(n). We demonstrate scalability
and efficiency of the variational integrator through several case studies.
Keywords: variational integrator · discrete mechanics · multibody sys-
tems · dynamics · computer animation & simulation
1
Introduction
We address the problem of accurately and efficiently simulating the dynam-
ics of complex multibody systems, often referred to as the forward dynamics
problem. Existing state-of-the-art approaches use the Lagrangian formalism, ex-
pressing the difference between kinetic and potential energy (the Lagrangian) in
generalized coordinates, and derive the Euler-Lagrange second-order differential
equations from them via the principle of least action. The state of the system
2
at any time t is then obtained by integrating these differential equations from
initial conditions.
However, the long-term conservation of conserved quantities like energy and
momentum of the system remains a key open challenge. In particular, discrete-
time simulations, even with advanced algorithms for solving differential equa-
tions, eventually produce alarming and physically implausible behaviors, even
for simple dynamical systems like N-link pendulums, due to the accumulation
of numerical errors.
To address this problem, Marsden and West [1] introduced the discrete La-
grangian, which approximates the integral of the Lagrangian over a small time
interval. They then derived its variation via the principle of least action, creating
the discrete Euler-Lagrange (DEL) equations. They also showed that variational
integrators based on the DEL formulation were symplectic (energy-conserving)
and crucially decoupled energy behavior from step size [1,2].
Unfortunately, despite their benefits for stability, variational integrators suf-
fer from computational complexity. Variational integrators transform the inte-
gration of the equations of motion into a root-finding problem for the DEL equa-
tion. This introduces complexity in three places as most nonlinear root-finding
algorithms require: (1) the evaluation of the DEL equation, (2) computation
of their gradient (Jacobian), and (3) the inversion of the gradient. Although
there exist efficient algorithms for evaluating the DEL equation, they do not
use generalized coordinates but instead treat each link as a free-body and apply
constraint forces to enforce joints [3,4,5]. This becomes especially complicated
with branching multi-body systems and joint constraints.
Recently Johnson and Murphey [6] proposed a scalable variational integrator
that represents the DEL equation in generalized coordinates. By representing the
multibody system as a tree structure in generalized coordinates, they showed
that the DEL equation, as well as the gradient and Hessian of the Lagrangian,
can be calculated recursively. However, the complexity of their algorithm is O(n2)
for evaluating the DEL equation, and O(n3) for computing the Jacobian. When
coupled with traditional root-finders, e.g., Newton’s method, that require the
inverse of the Jacobian, this adds an approximately O(n3) complexity for matrix
inversion.
In this paper, we introduce a new variational integrator for multibody dy-
namic systems. The primary contribution is an O(n) algorithm which solves the
root-finding problem for the DEL equation. Our key insight is that the evalua-
tion of DEL can be cast into a discrete inverse dynamics problem [7,8] while the
root updating can be cast into a continuous forward dynamics problem. Both
inverse and forward dynamics problems can be solved efficiently in O(n) using
a recursive Lie group formulation of the dynamics [9,10,11,12].
Inspired by Recursive Newton-Euler Algorithm (RNEA) and Articulated
Body Algorithm (ABA), we formulate the DEL equation individually for each
body rather than for the entire system. By taking advantage of the recursive
relations between body links, it becomes possible to evaluate the DEL function
using a discrete inverse dynamics algorithm in linear-time. The same recursive
3
representation is applied to update the root using an impulse-based forward
dynamics algorithm. Together with these two algorithms, we propose an O(n)
quasi-Newton method specialized for finding the root of DEL equation, resulting
in a Linear-Time Variational Integrator.
We compare our method with the state-of-the-art variational integrator in
generalized coordinates [6]. The results show that, for the same computation
method of root updating, the performance of our recursive evaluation of the
DEL equation (linear-time DEL algorithm) is 15 times faster for a system with
10 degrees of freedom (DOFs) and 32 times faster for 100 DOFs. For the same
evaluation method of the DEL equation (i.e., linear-time DEL algorithm), our
results show that the performance of our new quasi-Newton method is 3.8 times
faster for a system with 10 DOFs, and 53 times faster for 100 DOFs. Further
analysis shows that for higher DOF systems, the impulse-based Jacobian ap-
proximation becomes increasingly more effective compared to our linear-time
DEL algorithm.
2
Background
Our work is built on the concepts of discrete mechanics and variational integra-
tors. In this section, we will briefly describe the standard formulation of discrete
mechanics [6], followed by a reformulation using the Lie group representation for
the Special Euclidean group SE(3) of rigid body motions [11].
2.1
Variational Integrators in Generalized Coordinates
We begin with the definition of Lagrangian, L(q, ˙q) ∈R, the difference between
the total kinetic energy and the total potential energy of a system characterized
by generalized coordinates q ∈Rn where n denotes the degrees of freedom of
the system. For continuous-time systems, the principle of least action states
that the system will follow the trajectory that minimizes the action integral
R t2
t1 L(q(t), ˙q(t))dt.
However, when we simulate the mechanical system on a computer, the me-
chanical system takes discrete time steps rather than following the continuous
trajectory. Loosely speaking, the idea of discrete mechanics is that the system
will follow the discretized trajectory that minimizes the approximated action
integral defined on the discretized trajectory. If we discretize a continuous tra-
jectory q(t) into a sequence of configurations q0, q1, · · · , qN, we can define a
discrete Lagrangian that approximates the integral of L(q(t), ˙q(t)) over a short
interval ∆t:
Ld(qk, qk+1) ≈
Z (k+1)∆t
k∆t
L(q(t), ˙q(t))dt.
(1)
Using the discrete Lagrangian, we can define the action sum PN−1
k=0 Ld(qk, qk+1)
as an approximation of the action integral. Minimizing the action sum with re-
spect to {qk} (k = 1, 2, · · · , N −1), we arrive at the discrete Euler-Lagrange
4
(DEL) equation:
D2Ld(qk−1, qk) + D1Ld(qk, qk+1) = 0,
(2)
where Di : R →Rn denotes differential operator with respect to the i-th param-
eter of the function, and the differentials of Ld can be analytically computed [6].
Note that the boundary configurations q0 and qN are not varied.
Instead of numerically integrating the Euler-Lagrange equation to simulate
the trajectory, discrete mechanics solves a root-finding problem to obtain the
next configuration. Specifically, given two previous configurations qk−1 and qk,
we solve the next configuration qk+1 by finding the root of the following function:
f(qk+1) = D2Ld(qk−1, qk) + D1Ld(qk, qk+1) = 0.
(3)
The superior energy behavior of variational integrators compared to the tra-
ditional integrators like Euler and Runge-Kutta methods have been shown using
a discrete version of Noether’s theorem [1]. One geometric interpretation of varia-
tional integrators is that the DEL equation plays the role of constraints, enforcing
the discrete system to evolve on the constraint manifold such that f(qk+1) = 0,
i.e., satisfying the least action principle on the approximated action. In that
sense, the process of root-finding can be seen as a feedback controller to find the
physically correct configuration for the next time step, with the DEL equation
being used by the feedback law to indicate how far away the given configuration
is from the manifold. Traditional integrators do not have such indicators, only
account for the rate of change based on the current state, which leads to the
numerical error accumulation.
This nonlinear, high-dimensional, continuous root-finding problem can be
solved efficiently by Newton’s method, provided that the partial derivatives of
f, Jf(q) (i.e., the Jacobian matrix), can be evaluated:
Algorithm 1 Newton’s Method for Solving DEL Equation
1: Initial Guess q0
2: do
3:
Evaluate f(qk+1)
⊲O(n2) time
4:
if ∥f(qk+1) < ǫ∥
return qk+1
5:
Update qk+1 ←qk+1 −

Jf(qk+1)
−1 f(qk+1)
⊲O(n3) time
6: while num iteration < max iteration
To avoid the computation of the Jacobian and its inversion, various quasi-
Newton methods can be applied to approximate

Jf(qk+1)
−1. In Section 3.2, we
introduce a linear-time algorithm to approximate the product of

Jf(qk+1)
−1
and f(qk+1) for finding the root of DEL equation.
5
2.2
Variational Integrators in SE(3)
The linear-time root-finding algorithm we will introduce in the next section
leverages the idea of reformulating DEL equation for each rigid body rather
than for the entire system. We begin with the expression of the DEL equation
in SE(3) for a single rigid body.
The configuration of the rigid body can be represented by matrices of the
form:
T =

R p
0 1

∈SE(3),
(4)
where R ∈SO(3) is a 3 × 3 rotation matrix, and p ∈R3 is a position vector. The
spatial velocity of the rigid body V = (w, v) ∈se(3) or twist can be represented
in six-dimensional vector or 4 × 4 matrix form:
V =

w
v

,
[V ] =

ˆw v
0 0

,
(5)
where w ∈so(3) and v ∈R3 denote the angular velocity and linear velocity,
respectively, and ˆw is the 3×3 skew symmetric matrix for w such that ˆwT = −ˆw.
In this paper, we use brackets [·] to denote matrix representations.
The Lagrangian of a rigid body can be compactly expressed using the Lie
group representation ([9,13]) in the space of SE(3):
L(T, V ) = 1
2V T GV −P(T ),
(6)
where P : SE(3) →R is the potential energy. G is the spatial inertia matrix that
has the following structure:
G =

I 0
0 mI

∈R6×6,
(7)
where I is the inertia matrix, m is the mass, and I is 3 × 3 identity matrix when
the center of mass is at the origin of the body frame.
Analogous to Equation (1), the discrete Lagrangian for a single rigid body
can be expressed as
Ld(T k, T k+1) ≈
Z (k+1)∆t
k∆t
L(T, V )dt.
(8)
In this paper, we use the trapezoidal quadrature approximation for the dis-
crete Lagrangian of the single body system as
Ld(T k, T k+1) ≜∆t
2 L(T k, V k) + ∆t
2 L(T k+1, V k),
(9)
where the average velocity V k can be defined as
V k = 1
∆t log(∆T k),
(10)
6
with the log map log : SE(3) →se(3), the inverse of the exponential map exp :
se(3) →SE(3) [11,13], and ∆T k = T k −1 T k+1, the displacement of the rigid
body’s configuration during the discrete times of tk and tk+1.
To derive the DEL equation for a single rigid body in SE(3), we need to take
the variational calculus on V k with respect to T k and T k+1. This requires the
derivative of log map defined as
 ∂
∂T log(T )

[W] = dlogV ([W] exp(−[V ])) ,
(11)
where V = log(T ), and W ∈se(3) is an arbitrary twist, and dlogV : se(3) →se(3)
is the inverse of the right trivialized tangent dexpV : se(3) →se(3) as an linear
operator [11,14]:
dlogV (W) =
∞
X
j=0
Bj
j! adj
V (W).
(12)
The Lie bracket operator adV : se(3) →se(3) is defined as adV (W) =
[V ][W] −[W][V ]. dlogV can be alternatively represented in matrix form as
[dlogV ] =
∞
X
j=0
Bj
j! [adV ]j,
[adV ] =

ˆw 0
ˆv ˆw

,
(13)
where Bj are the Bernoulli numbers (B0 = 1, B1 = −1/2, B2 = 1/6, B3 = 0, . . . )
[15].
Using Equation (10) and (11), we can now express the variation of V k as
δV k = 1
∆tdlog∆tV k

−T k −1 δT k + Adexp(∆t[V k])

T k+1 −1 δT k+1
,
(14)
where δT k and δT k+1 are variations, and AdT : se(3) →se(3) is the adjoint
action of T ∈SE(3) on V ∈se(3) defined as AdTV = T [V ]T −1. The adjoint
action can be regarded as an linear operator in the 6 × 6 matrix form of:
[AdT] =
 R 0
ˆpR R

.
(15)
By the least action principle with Equation (9), (10), and (14), we can derive
the DEL equation for a single rigid body in SE(3), which is the well known
discrete reduced Euler-Poincar´e equations [11,16]:
D2Ld(T k−1, T k) + D1Ld(T k, T k+1) = 0
∈R6,
(16a)
where
D2Ld(T k−1, T k) = −[Adexp(∆t[V k−1])]T [dlog∆tV k−1]T GV k−1 + ∆t
2 T k ∗P(T k)
(16b)
D1Ld(T k, T k+1) = [dlog∆tV k]T GV k + ∆t
2 T k ∗P(T k).
(16c)
7
(a) Displacement of body i’s configuration
(b) Impulses acting on body i
Fig. 1: Recurrence relationships of configuration displacement and impulses
By Lagrange-d’Alembert principle, Equation (16a) can be straightforwardly
extended to a forced system [11]:
D2Ld(T k−1, T k) + D1Ld(T k, T k+1) + F k = 0,
(17)
where F k ∈se∗(3) is the integral of the virtual work performed by the force over
the time interval ∆t.
3
Linear-Time Variational Integrator
We introduce a new linear-time variational integrator which, at each time in-
stance tk, solves for the root of Equation (3). Our variational integrator consists
of two linear-time algorithms for evaluating the DEL equation and updating
the root, which, as shown in Algorithm 1, determine the time complexity of
the root-finding algorithm. We first derive the DEL equation for multibody sys-
tems in a recursive manner, resulting a linear-time procedure to evaluate the
function f(q). Next, we introduce an impulse-based dynamics algorithm, which
is also linear-time, to estimate the next configuration. Replacing Line 3 and
Line 5 in Algorithm 1 with these two algorithms, we present a new linear-time
quasi-Newton root-finding method for finding the root of DEL equation.
3.1
Linear-Time Evaluation of the DEL Equation
If we view the function f(q) = 0 as a dynamic constraint that enforces the
equation of motion, any nonzero value of f(q) indicates the residual impulse
that violates the equation of motion. As such, evaluating f(q) can be considered
a discrete inverse dynamics problem which solves the residual impulse of the
system given qk−1, qk, and qk+1. We derive a recursive DEL equation using
8
similar formulation as recursive Newton-Euler algorithm (RNEA) [7,8], which
solves the inverse dynamics for continuous systems in linear time with respect
to the degrees of freedom of the system.
Assuming that the multibody system can be represented as a tree-structure
where each body has at most one parent and an arbitrary number of children,
connected by joints, our goal is to expand Equation (17) to account for the
dynamics of entire tree-structure.
We begin with the recursive definition for a rigid body’s configuration and
the displacement of the configuration. Let us denote {0} as an inertial frame
which is stationary in the space, {i} as body frame of i-th body in the tree
structured system, and {λ(i)} as a body frame of the parent of the i-th body.
The configuration of a body in the system can be represented as
T k
i = T k
λ(i)T k
λ(i),i,
(18)
where T k
i and T k
λ(i) denote the transformations from the inertial frame to {i}
and {λ(i)}, respectively, while T k
λ(i),i denotes the relative transformation from
{λ(i)} to {i} represented as a function of the i-th joint configuration qk+1
i
. From
Equation (18), the configuration displacement of a rigid body can be written as
∆T k
i = T k
λ(i),i
−1 ∆T k
λ(i)T k+1
λ(i),i.
(19)
Fig. 1 (a) gives a geometric interpretation of the recurrence relationship of
the configuration displacements between ∆T k
i and ∆T k
λ(i).
Plugging Equation (19) into Equation (10), we can obtain the average velocity
of i-th rigid body as V k
i =
1
∆t log(∆T k
i ). Unlike the continuous velocity of i-th
body Vi = Si ˙qi where Si is the joint Jacobian [13], the equation for the average
velocity is implicit with respect to qk+1 due to the log map. The use of log map,
with dlogV , is the key reason that makes the DEL equation implicit with respect
to qk+1.
For a rigid body in a multibody system, the impulse term F k in Equation
(17) includes the impulse transmitted from the parent link F k
i , impulses trans-
mitting to the child links F k
c , and other external impulses F ext,k
i
applied by the
environment as (Fig. 1 (b)):
F k = F k
i −
X
c∈σ(i)
Ad∗
T k
i,c
−1F k
c + F ext,k
i
.
(20)
Note that F k
i is expressed in the i-body coordinates so the coordinate frame
transformation is required for F k
c as
h
Ad T k
i,c
−1
iT
F k
c .
Plugging these forces into Equation (17) and using the definitions in Equation
(16b) and (16c), we express the equations of motion for the i-th body as
F k
i = µk
i −
h
Adexp(∆t[V k−1
i
])
iT
µk−1
i
+
X
c∈σ(i)
h
Ad T k
i,c
−1
iT
F k
c −F ext,k
i
(21a)
µk
i =
h
dlog∆tV k
i
iT
GiV k
i ,
(21b)
9
where µk
i is the discrete momentum of body i and σ(i) denotes the set of child
bodies to body i. The required generalized impulse of joint i to achieve the
motion qk+1 is simply the projection of F k
i onto the joint Jacobian as ST
i F k
i
where Si ∈R6×ni is the i-th joint Jacobian [13]. The residual impulse then can
be obtained by subtracting the joint impulses, Qk
i , such as joint actuation or
joint friction, from the required impulse:
fi = ST
i F k
i −Qk
i
∈Rni.
(22)
Algorithm 2 summarizes the recursive procedure, which we call discrete recur-
sive Newton-Euler algorithm (DRNEA). DRNEA consists a forward pass from
the root of the tree structure to the leaf nodes and a backward pass in the reverse
order. The forward pass computes the velocity of each body while the backward
pass computes force transmitted between joints. By exploiting the recursive re-
lationship between a parent body and its child bodies, the computation for each
pass is O(n), where n is the number of rigid body links in the system assuming
the degree of freedom of each joint is one.
Algorithm 2 Discrete recursive Newton-Euler algorithm (DRNEA)
1: for i = 1 →n do
2:
T k+1
λ(i),i = function of qk+1
i
3:
∆T k
i = T k
λ(i),i
−1 ∆T k
λ(i)T k+1
λ(i),i
4:
V k
i =
1
∆t log
∆T k
i


5: end for
6: for i = n →1 do
7:
µk
i =
h
dlog∆tV k
i
iT
GiV k
i
8:
F k
i = µk
i −
h
Adexp(∆t[V k−1
i
])
iT
µk−1
i
−F ext,k
i
+ P
c∈σ(i)
h
Ad T k
i,c
−1
iT
F k
c
9:
fi = ST
i F k
i −Qk
i
10: end for
For clarity, the mathematical symbols used in DRNEA are listed below.
• i: index of the i-th body.
• λ(i): index of the parent body of the i-th body.
• σ(i): set of indices of the child bodies of the i-th body.
• qk
i ∈Rni: generalized coordinates of the i-th joint which connects the i-th
body with its parent body where ni denotes the dimension of the coordinates.
• Qi ∈Rni: generalized force exerted by the i-th joint.
• Tλ(i),i ∈SE(3): relative transformation matrix from the {λ(i)} to {i}.
• V k
i ∈se(3): the spatial average velocity of the i-th body, expressed in {i} at
time step k
• Sk
i ∈R6×ni: Jacobian of Tλ(i),i expressed in {i}.
• Gi ∈R6×6: the spatial inertia of the i-th body, expressed in {i}.
10
• F k
i ∈se∗(3): the spatial impulse transmitted to the i-th body from its parent
through the connecting joint, expressed in {i}.
• F ext,k
i
∈se∗(3): the spatial impulse acting on the i-th body, expressed in {i}.
3.2
Linear-Time Root Updating
Besides function evaluation, Newton-like methods also require the update of
Jacobian to estimate the root, which is usually the computation bottleneck in
each iteration. Here we describe a recursive impulse-based method to efficiently
update the root in linear-time.
Let us denote the current iteration in Newton’s method as l and the current
estimate of the configuration at next time step as qk+1
(l) . Evaluating the forced
DEL equation (17) gives the residual impulse, f(qk+1
(l) ) = e(l), in the system. If
the magnitude of e(l) is zero or less than the tolerance, qk+1
(l)
is the next config-
uration that satisfies the forced DEL equation. Otherwise, e(l) can be regarded
as the residual impulse needed to result in qk+1
(l)
at the next time step. If we
apply the negative residual force, −e(l)/∆t, to the system, we should arrive at a
configuration closer to the root of f(qk+1). Applying such a force to the system
can be done by continuous forward dynamics in linear-time [8].
Given the approximation of ˙qk as
1
∆t
qk −qk−1

, the continuous forward
dynamics equation can be used to evaluate the generalized acceleration:
¨qk = M −1(qk)
−C(qk, ˙qk) ˙qk + Q


,
(23)
where M(qk) is the mass matrix and C(qk, ˙qk) is the Coriolis force in generalized
coordinates. Q indicates the sum of other external and internal forces applied to
the system in generalized coordinates.
Using the 2nd order central difference to approximate qk+1 = ∆t2¨qk +2qk −
qk−1, we can apply the negative residual force to improve the estimate of root:
qk+1
(l+1) = ∆t2M −1(qk)
 
−C(qk, ˙qk) ˙qk + Q −
l
X
m=0
e(m)
∆t
!
+ 2qk −qk−1.
(24)
Consolidating the quantities on the RHS of Equation (24) gives the update
rule for qk+1:
qk+1
(l+1) = qk+1
(l)
−∆tM −1(qk
(l))e(l),
(25)
where ∆tM −1(qk
(l))e(l) can be evaluated in O(n) using recursive impulse-based
dynamics (ABI algorithm: articulated body inertia algorithm) introduced by
Featherstone [8]. Specifically, ABI is a forward dynamics algorithm which com-
putes Equation (23). If we set ˙q ≡0 (to eliminate the Coriolis force) and
Q ≡∆te(l), ABI will return exactly ∆tM −1(qk
(l))e(l).
Comparing to the Newton’s method in Algorithm 1, the inverse of Jaco-
bian matrix is approximated by the inverse mass matrix multiplied by ∆t. We
11
name this algorithm RIQN (Recursive Impulse-based Quasi-Newton method)
and summarize it in Algorithm 3.
Algorithm 3 Recursive Impulse-based Quasi-Newton method (RIQN)
1: Initial Guess qk+1
0
2: do
3:
Use DRNEA to evaluate e ←f(qk+1)
⊲O(n) time
4:
if ∥e < ǫ∥
return qk+1
5:
Use ABI to compute ∆tM −1(qk)e
⊲O(n) time
6:
Update qk+1 ←qk+1 −∆tM −1(qk)e
7: while num iteration < max iteration
3.3
Initial Guess
Similar to other Newton-like methods, our algorithm requires the initial guess to
be sufficiently close to the solution. We propose three different ways to produce
an initial guess for RIQN.
• IG1: Directly use the current configuration as the initial guess of the next
configuration: qk+1
(0) = qk.
• IG2: Apply explicit Euler integration, qk+1
(0)
= qk + ∆t ˙qk, where ˙qk is
approximated by
1
∆t
qk −qk−1

.
• IG3: Compute the acceleration via the equations of motion, ¨qk = M −1 (−C + Q)),
and apply semi-implicit Euler integration to integrate velocity, ˙qk+1 = ˙qk +
∆t ¨qk, followed by position, qk+1
(0) = qk + ∆t ˙qk+1.
4
Experimental Results
In this section, we describe the implementation of the proposed algorithms,
RIQN and DRNEA, and verify the algorithms in terms of efficiency and scal-
ability by comparing them to the state-of-are algorithms through case studies.
We used fixed time step of 1 millisecond for all the experiments.
4.1
Implementation
The algorithms introduced by this paper and several state-of-art algorithms were
implemented on top of DART [17,18], which is an C++ open source dynamics
library for multibody systems. All of the simulations were performed on a Intel
Core i7-4970K @ 4.00 GHz desktop computer.
All the source code of the implementations is available at the GitHub repos-
itory.4
12
0
200000
400000
600000
800000
1000000
Frames
200
300
400
500
600
700
800
900
1000
1100
Total Energy
Energy conservation behavior
Semi-implicit Euler Integrator
RIQN
Fig. 2: (a) Serial chain of N-bodies connected by revolute joints, (b) Energy
conservation behavior over simulation frames
4.2
Energy Conservation
We first show that our linear-time variational integrator inherits the energy
conservation property, which is one of the important features of variational in-
tegrators. We simulate a serial chain that consists of N-bodies connected by
revolute joints (Fig. 2 (a)) with RIQN (variational integrator) and semi-implicit
Euler method, which is an easy-to-implement standard method. In this experi-
ment, we use a 10-body serial chain with no joint actuation nor external forces
except for the gravity. The total energy (kinetic energy + potential energy) of
this passive system should remain constant.
Fig. 2 (b) shows the energy evolution of the serial chain over simulation
frames for both integration methods. RIQN does not artificially dissipate the
energy while the Euler method does.
4.3
Performance Comparisons
The major factors that affect on the computational time of variational integrator
are (1) evaluation of DEL equation and (2) the evaluation of Jacobian inverse. We
consider various of the root-finding algorithm that are combination of methods
for (1) and (2).
For (1), we compare our DRNEA to the scalable variational integrator (SVI)
[6]. For (2), we compare the proposed RIQN to Newton’s method and Broyden
method (quasi-Newton method) [19].
Newton’s method requires the (exact) Jacobian of the DEL equation. When
combining with DRNEA, for a fair comparison we also derive a recursive al-
gorithm to evaluate the derivatives of the DEL equation with respect to qk+1.
Please see the Appendix for the algorithm.
4 https://github.com/jslee02/wafr2016
13
10
20
30
40
50
60
70
80
90
100
DOFs
0
10
20
30
40
50
60
CPU time [sec]
Performance comparisons
RIQN with SVI
Newton with DRNEA
Broyden with DRNEA
RIQN with DRNEA
(a) Performance comparisons
101
102
DOFs (logarithm)
10-2
10-1
100
101
102
CPU time [sec] (logarithm)
Performance comparisons (log-log scale)
RIQN with SVI
Newton with DRNEA
Broyden with DRNEA
RIQN with DRNEA
(b) Performance comparisons (logarithm)
Fig. 3: Absolute computation time versus DOFs for the various root-finding
methods.
For all the root-finding methods, we measure computation time of serial
chain forward dynamics simulations for 10k frames. To reveal the scalability of
the methods, we vary the number of bodies of the serial chain (Fig. 3). RIQN
method with DRNEA shows the best performance. We also noticed that, for
the same method for (2), DRNEA shows better performance than SVI. Further
analyses show that the impulse-based Jacobian approximation contributes more
than our linear-time DEL algorithm for the higher DOFs systems.
4.4
Convergence
We consider the convergent rate of RIQN comparing to Newtons method. We
inspect the convergence of error f(qk+1
(l) ) = e during the iterations in solving
the DEL equation for one simulation time step. For quantitatively visible con-
vergence, we use the zero configurations as the initial guess qk+1
0
= 0 instead of
the proposed initial guesses in Section 3.3.
Fig. 4a shows that under the tolerance RIQN converges more slowly than
Newton’s method. This observation is expected because Newton’s method has a
quadratic convergence rate which is in theory faster than that of Quasi-Newton
methods. However, in Section 4.3, we observed that the absolute computation
time of the proposed method (DRNEA+RIQN) showed the best performance.
Fig. 4b shows the average iteration numbers per each simulation step in
the root-finding process. As expected, Newton’s method requires less iteration
numbers than RIQN.
5
Conclusion
We introduced a novel linear-time variational integrator for simulating multi-
body dynamic systems. At each simulation time step, the integrator solves a
14
0
1
2
3
4
5
6
7
8
9
Iterations
10−9
10−8
10−7
10−6
10−5
10−4
10−3
10−2
10−1
100
101
102
103
104
105
106
The error norm of f(qk+1
(l) )
tolerance = 10−8
Convergence comparisons
Newton with DRNEA
RIQN with DRNEA
(a)
0
2000
4000
6000
8000
10000
Frames
0
2
4
6
8
10
Number of iterations in root-updating
Iterations
Newton with DRNEA
RIQN with DRNEA
(b)
Fig. 4: (a) Convergence rate comparison for Newton’s method and RIQN (b)
Iteration numbers over simulation frames. Newton’s method: mean = 4, σ = 0.0.
RIQN: mean = 5.69, σ = 1.16
root-finding problem for the DEL equation using our quasi-Newton algorithm,
RIQN, which consists of two primary contributions:
• DRNEA: Based on the variational integrator on Lie group and inspired by
RNEA, we derived an O(n) recursive algorithm that evaluates DEL equa-
tions of tree-structured multibody systems. Unlike the previous work, which
formulates and solves the DEL equation for the entire system, in our ap-
proach the DEL equation for each body is solved recursively.
• Root updating:
By leveraging existing forward dynamic algorithm for
multibody systems, we introduced an O(n) impulse-based dynamic algorithm
to estimate the configuration at next time step.
We evaluated our linear-time variational integrator on a n-DOF open chain
system and compared the results with existing state-of-art algorithms. The re-
sults show that, for the same computation method of root updating, the perfor-
mance of our recursive evaluation of the DEL equation (linear-time DEL algo-
rithm) is 15 times faster for a system with 10 degrees of freedom (DOFs) and 32
times faster for 100 DOFs. For the same evaluation method of the DEL equation
(i.e., linear-time DEL algorithm), our results show that the performance of our
new quasi-Newton method is 3.8 times faster for a system with 10 DOFs, and 53
times faster for 100 DOFs. Further analysis shows that for higher DOF systems,
the impulse-based Jacobian approximation becomes increasingly more effective
compared to our linear-time DEL algorithm.
One of the future directions is to apply the linear-time variational integrator
on constrained dynamic systems. This paper demonstrates the performance gain
on multibody systems with joint constraints, but does not address other types of
constrains, such as contacts or closed-loop chains. The standard way to handle
constraints in a dynamic system is to solve the DEL equations and constraints
15
simultaneously using Lagrangian multipliers [1,2]. To preserve the performance
gain achieved by RIQN, one possible extension to constrained systems is to solve
constraint force using the similar idea of impulse-based forward dynamics [8,20].
Our current implementation of RIQN can be improved by using variable
time step size. Although the variational integrator allows for larger time step
size than other numerical integrators for the same accuracy, the variable time
step size can still be exploited to achieve further stability and time performance.
However, naively changing the time step size can have negative impact on the
qualitative behavior of a simulation [15,21]. Previous work has shown that addi-
tional constraints are needed when using the scheme of variable time step size.
Integrating this line of work to our linear-time variational integrator can be a
fruitful future research direction.
Acknowledgments
This work was (partially) funded by the National Science Foundation IIS (#1409003),
Toyota Motor Engineering & Manufacturing (TEMA), and the Office of Naval
Research.
References
1. Marsden, J.E., West, M.: Discrete mechanics and variational integrators. Acta
Numerica 2001 10 (2001) 357–514
2. West, M.: Variational integrators. PhD thesis, California Institute of Technology
(2004)
3. Betsch, P., Leyendecker, S.: The discrete null space method for the energy consis-
tent integration of constrained mechanical systems. part ii: Multibody dynamics.
International journal for numerical methods in engineering 67(4) (2006) 499–552
4. Leyendecker, S., Marsden, J.E., Ortiz, M.:
Variational integrators for con-
strained dynamical systems. ZAMM-Journal of Applied Mathematics and Mechan-
ics/Zeitschrift f¨ur Angewandte Mathematik und Mechanik 88(9) (2008) 677–708
5. Leyendecker, S., Ober-Bl¨obaum, S., Marsden, J.E., Ortiz, M.: Discrete mechanics
and optimal control for constrained systems. Optimal Control Applications and
Methods 31(6) (2010) 505–528
6. Johnson, E.R., Murphey, T.D.:
Scalable variational integrators for constrained
mechanical systems in generalized coordinates. Robotics, IEEE Transactions on
25(6) (2009) 1249–1261
7. Luh, J.Y., Walker, M.W., Paul, R.P.: On-line computational scheme for mechanical
manipulators. Journal of Dynamic Systems, Measurement, and Control 102(2)
(1980) 69–76
8. Featherstone, R.: Rigid body dynamics algorithms. Springer (2014)
9. Park, F.C., Bobrow, J.E., Ploen, S.R.: A lie group formulation of robot dynamics.
The International Journal of Robotics Research 14(6) (1995) 609–618
10. Lee, T.: Computational geometric mechanics and control of rigid bodies. ProQuest
(2008)
11. Kobilarov, M.B., Marsden, J.E.: Discrete geometric optimal control on lie groups.
Robotics, IEEE Transactions on 27(4) (2011) 641–655
16
12. Kobilarov, M., Crane, K., Desbrun, M.: Lie group integrators for animation and
control of vehicles. ACM Transactions on Graphics (TOG) 28(2) (2009) 16
13. Murray, R.M., Li, Z., Sastry, S.S.: A mathematical introduction to robotic manip-
ulation. CRC press (1994)
14. Bou-Rabee, N., Marsden, J.E.: Hamilton–pontryagin integrators on lie groups part
i: Introduction and structure-preserving properties. Foundations of Computational
Mathematics 9(2) (2009) 197–219
15. Hairer, E., Lubich, C., Wanner, G.: Geometric numerical integration: structure-
preserving algorithms for ordinary differential equations. Volume 31.
Springer
Science & Business Media (2006)
16. Fan, T., Murphey, T.: Structured linearization of discrete mechanical systems on
lie groups: A synthesis of analysis and control. In: 2015 54th IEEE Conference on
Decision and Control (CDC), IEEE (2015) 1092–1099
17. Liu, C.K., Stillman, M., Lee, J., Grey, M.X.: DART - Dynamic Animation and
Robotics Toolkit. (2011 (accessed October 30, 2016)) http://dartsim.github.io.
18. Liu, C.K., Jain, S.: A short tutorial on multibody dynamics. Technical Report GIT-
GVU-15-01-1, Georgia Institute of Technology, School of Interactive Computing
(08 2012)
19. Broyden, C.G.: A class of methods for solving nonlinear simultaneous equations.
Mathematics of computation 19(92) (1965) 577–593
20. Mirtich, B., Canny, J.: Impulse-based simulation of rigid bodies. In: Proceedings
of the 1995 symposium on Interactive 3D graphics, ACM (1995) 181–ff
21. Kharevych, L.: Geometric interpretation of physical systems for improved elasticity
simulations. PhD thesis, Citeseer (2009)
Appendix: Derivative of DRNEA
Algorithm 4 Derivative of DRNEA for computing ∂f(qk+1)
∂qk+1
∈Rn×n
1: for j = 1 →n do
2:
for i = 1 →n do
3:
∂T k+1
λ(i),i
∂qk+1
j
= T k+1
λ(i),i[Si]δij
⊲δij =
(
1,
if i = j
0,
otherwise
4:
∂∆T k
i
∂qk+1
j
= T k
λ(i),i
−1 ∂∆T k
λ(i)
∂qk+1
j
T k+1
λ(i),i + ∆T k
i [Si]δij
5:

∂V k
i
∂qk+1
j

=
1
∆tdlog∆tV k
i

∂∆T k
i
∂qk+1
j
exp
−∆t[V k
i ]


6:
end for
7:
for i = n →1 do
8:
∂µk
i
∂qk+1
j
=
∂
∂qk+1
j
h
dlog∆tV k
i
iT
GiV k
i +
h
dlog∆tV k
i
iT
Gi
∂V k
i
∂qk+1
j
9:
∂F k
i
∂qk+1
j
=
∂µk
i
∂qk+1
j
+ P
c∈σ(i)

Ad(T k
i,c)
−1
T
∂F k
c
∂qk+1
j
−
∂F ext,k
i
∂qk+1
j
10:
∂f(qk+1)
∂qk+1
j
= ST
i
∂F k
i
∂qk+1
j
−
∂Qk
i
∂qk+1
j
⊲j-th column of ∂f(qk+1)
∂qk+1
11:
end for
12: end for