# Exponential Integration for Efficient and Accurate Multi-Body Simulation
with Stiff Viscoelastic Contacts

Bilal Hammoud1, Luca Olivieri2, Ludovic Righetti1, Justin Carpentier3, Andrea
Del Prete2 1Tandon School of Engineering, New York University, New York, USA.
<EMAIL_ADDRESS><EMAIL_ADDRESS>of Industrial
Engineering, University of Trento, Italy<EMAIL_ADDRESS><EMAIL_ADDRESS>Paris, France<EMAIL_ADDRESS>
###### Abstract

The simulation of multi-body systems with frictional contacts is a fundamental
tool for many fields, such as robotics, computer graphics, and mechanics. Hard
frictional contacts are particularly troublesome to simulate because they make
the differential equations stiff, calling for computationally demanding
implicit integration schemes. We suggest to tackle this issue by using
exponential integrators, a long-standing class of integration schemes (first
introduced in the 60’s) that in recent years has enjoyed a resurgence of
interest. We show that this scheme can be easily applied to multi-body systems
subject to stiff viscoelastic contacts, producing accurate results at lower
computational cost than classic explicit or implicit schemes In our tests with
quadruped and biped robots, our method demonstrated stable behaviors with
large time steps (10 ms) and stiff contacts ($10^{5}$ N/m). Its excellent
properties, especially for fast and coarse simulations, make it a valuable
candidate for many applications in robotics, such as simulation, Model
Predictive Control, Reinforcement Learning, and controller design.

##  I Introduction

The interest of the robotics community for fast and reliable methods to
simulate multi-body systems subject to frictional contacts has been constantly
growing in the last two decades [1, 2, 3, 4, 5]. This is reasonable
considering that simulation is at the core of many robotics applications, such
as the development and testing of novel controllers before deployment on
hardware. Moreover, many advanced control and planning techniques, such as
Model Predictive Control [6] (MPC) and Optimal Control [7], rely on the
ability to predict the future behavior of the system. Finally, the current
bottleneck of many learning algorithms [8, 9] is their need for huge amounts
of data, which therefore can greatly benefit from fast and accurate simulation
methods.

The simulation of articulated rigid multi-body systems without contacts is a
solved problem [10]. The same is not the case for systems with stiff contacts,
which can be treated in two ways, each leading to a hard (but different)
numerical problem. The first approach, which more closely follows the physical
phenomenon of contacts, consists in expressing contact forces as a function of
the penetration between bodies. Often, linear spring dampers have been used in
this context [5, 11]. This leads to stiff differential equations that are
simple to evaluate, but difficult to integrate because of their _numerical
stiffness_ [12]. The second approach tries to circumvent these numerical
challenges by assuming contacts to be _infinitely_ rigid. This approach
effectively gets rid of the numerical stiffness, but in exchange for _non-
smoothness_. One method that has been particularly successful for dealing with
the resulting non-smooth equations is velocity-impulse time-stepping [13, 1,
4, 14]. This has become the standard for robot simulation [15], demonstrating
stable behaviors with large time steps (several ms)—even though it must solve
a numerically hard Linear Complementarity Problem (LCP). Several authors have
tried to improve this approach by getting rid of the strict complementarity
constraints [2, 16, 3], which are the source of the numerical challenge.
However, none of these approaches is currently widely accepted in the robotics
community, mainly because of the unclear effects of the introduced numerical
regularization/relaxations on the physics (i.e., relaxations can be
interpreted as implicit spring/dampers but are not an explicit part of the
modeling).

Figure 1: Snapshots from our simulation tests with a biped and a quadruped
robot.

The approach we advocate for in this article is based on a well-known soft
contact model: the linear spring damper. Instead of using explicit integration
schemes, which require small time steps, or implicit schemes, which require
solving nonlinear systems of equations and introduce artificial viscosity
leading to nonphysical behaviors, we use _Exponential Integrators_ (EI) [17,
18]. EI are a long-standing class of integration schemes [19] that are
particularly suited for stiff differential equations. EI were initially
considered unpractical because of the computational challenges related to the
matrix exponential [20]. However, novel numerical methods to compute the
matrix exponential [21, 22, 23] have recently unlocked the potential of EI.
This has already been used in computer graphics for simulating deformable
objects, modeled as systems of particles [24, 25, 26]. This model is
particularly suited for EI because the stiff part of the dynamics is linear,
which however is not the case for articulated systems in contact with the
environment.

Our main contribution is a simulation algorithm that exploits EI to simulate
articulated robots in contact with a stiff visco-elastic environment.
Particularly, this paper addresses the following questions. (1) Does the
proposed simulation scheme provide an improvement in terms of speed vs
accuracy when compared to classic explicit and implicit methods? (2) Is it
possible to develop a simulation scheme that is less sensitive to the choice
of contact stiffness and damping? (3) Is it possible to get stable simulations
with increased time step (integration interval)? The last question is of
particular importance for using MPC and reinforcement learning.

In order to address all the above questions in an efficient way, we apply EI
only to the contact dynamics (which is stiff) while using an explicit Euler
scheme for the remaining terms (which are not stiff). Our simulation results
on quadruped and biped robots (see Fig. 1) show the superior performance of
our method compared to standard integration schemes in terms of accuracy,
speed and stability. To our knowledge, this class of integrators has never
been used before by the robotics community. The article is organized as
follows. Section II introduces the problem of multi-body simulation and the
basic theory of EI. Section III explains how EI can be used for multi-body
simulation with bilateral contacts. This method is then extended to frictional
contacts in Section IV. Section V discusses the implementation details of the
algorithm. Section VI presents the results and Section VII concludes the
article.

##  II Background

###  II-A Multi-Body Dynamics and Soft Contact Model

We want to simulate a multi-body mechanical system with the following dynamics
[10]:

$\displaystyle M(q)\dot{v}=u(q,v)+J(q)^{\top}\lambda,$ (1)

where $q$ is the robot joint configuration, $v$ is the robot joint velocity,
$M$ is the joint space mass matrix, $u$ contains gravity, nonlinear effects
and actuator forces, $J$ is the contact point Jacobian, and $\lambda$ contains
the stacked 3D contact forces. We assume a linear spring-damper contact model,
which means that the contact forces $\lambda$ are proportional to the inter-
penetration of contacting bodies:

$\displaystyle\lambda=-K\,(\underbrace{p(q)-p_{0}}_{\Delta
p})-B\,(\underbrace{\dot{p}(q,v)-\dot{p}_{0}}_{\Delta\dot{p}}),$ (2)

where $p$ and $\dot{p}$ contain the stacked 3D contact point positions and
velocities, $p_{0}$ and $\dot{p}_{0}$ contain the stacked 3D anchor point
positions and velocities, and $K$ and $B$ are the diagonal stiffness and
damping matrices, respectively. The anchor point $p_{0}$ is a _virtual_ point
to which the _virtual_ spring and damper are attached. It is typically set to
the contact point location when contact is first detected, and as long as
contacts are sticking $\dot{p}_{0}=0$. However, when slipping occurs then
$\dot{p}_{0}\neq 0$. A limitation of the “anchor point” model is that to
generate tangential forces, some lateral motion of the contact point is always
necessary. Consequently, pure static friction cannot be modeled with this
approach, but it can be well approximated by using large lateral stiffnesses.
Dependencies on $q$ and $v$ are dropped in the following to ease notation.

###  II-B Explicit Integration Schemes

The classic approach to integrate this dynamical system starts by writing it
in standard form. Defining the state as $x_{q}\triangleq(q,v)$, its dynamics
is:

$\displaystyle\underbrace{\frac{d}{dt}\begin{bmatrix}q\\\
v\end{bmatrix}}_{\dot{x}_{q}}=\underbrace{\begin{bmatrix}v\\\
M^{-1}(u+J^{\top}\lambda)\end{bmatrix}}_{f(x_{q},u)}$ (3)

We can integrate (3) with any numerical integration scheme, such as a high-
order Runge-Kutta scheme, or even a simple explicit Euler [12] (very common in
robotics):

$\displaystyle x_{q}^{+}=x_{q}+\delta t\,f(x_{q},u)$ (4)

where $x_{q}^{+}$ represents the next value of the state and $\delta t$ is the
integration time step. The problem with this approach is that for large values
of $K$ and $B$ the differential equations (3) are _stiff_ [12]. This means
that they require very small integration steps for numerical stability. This
is the main reason why soft contact models have been mostly abandoned in the
last decade, in favor of complementarity-based models (and their relaxations)
and time-stepping integration [2, 27, 4].

###  II-C Exponential Integrators (EI)

EI [25, 26, 24] are integration schemes particularly suited for stiff
dynamical systems for which the _stiffness_ comes from a linear part of their
dynamics:

$\displaystyle\dot{x}=\underbrace{f(x)}_{\text{nonstiff nonlinear
function}}+\underbrace{Ax}_{\text{stiff linear function}}$ (5)

In this case, using an explicit integration scheme would result in the
problems mentioned above. Instead, EI exploit the linearity of the stiff part
of the dynamics, which can be solved _analytically_ using the matrix
exponential, thanks to the well-known solution of linear dynamical systems:

$\displaystyle\dot{x}(t)$ $\displaystyle=Ax(t)+b$ (6) $\displaystyle x(t)$
$\displaystyle=e^{tA}x(0)+\int_{0}^{t}e^{\tau A}\,\text{d}\tau\,b$

First-order EI apply the solution (6) to the nonlinear system (5), by
interpreting $f()$ as $b$ and assuming it remains constant during the
integration step:

$\displaystyle x(t)=e^{tA}x(0)+\int_{0}^{t}e^{\tau A}\text{d}\tau\,f(x(0))$
(7)

Since the stiff part of the equations is integrated via the matrix
exponential, large integration steps can be taken.

##  III Bilateral Contacts

Our approach consists in using EI to simulate the system (3). The standard
approach to apply EI to arbitrary dynamics is to use a 1-st order Taylor
expansion:

$\displaystyle\dot{x}_{q}(t_{0}+t)\approx\dot{x}_{q}(t_{0})+\frac{\partial
f}{\partial x_{q}}\,(x_{q}(t_{0}+t)-x_{q}(t_{0}))$ (8)

However, this would require two demanding computations: the dynamics Jacobian,
and a matrix exponential with the size of $x_{q}$. In the following we present
instead an approach that i) does not require the dynamics Jacobian, and ii)
only computes a matrix exponential with twice the size of $\lambda$, which is
typically smaller than $x_{q}$ in legged locomotion.

To get a differential equation with the form (5) we start by projecting (1) in
contact space pre-multiplying both sides by $JM^{-1}$:

$\displaystyle
J\dot{v}-\underbrace{JM^{-1}J^{\top}}_{\Upsilon}\lambda=J\underbrace{M^{-1}u}_{\dot{\bar{v}}}$
(9)

Then we use the relationship $\ddot{p}=J\dot{v}+\dot{J}v$ to express the
contact point accelerations as functions of the robot accelerations:

$\displaystyle\ddot{p}-\Upsilon\lambda=J\dot{\bar{v}}+\dot{J}v$ (10)
$\displaystyle\ddot{p}+\Upsilon K\Delta p+\Upsilon
B\Delta\dot{p}=\underbrace{J\dot{\bar{v}}+\dot{J}v}_{\ddot{\bar{p}}}$

Since for bilateral contacts $\dot{p}_{0}$ is always null, we have that
$\ddot{p}_{0}=0$ and thus we can write the contact point dynamics as:

$\displaystyle\frac{d}{dt}\begin{bmatrix}\Delta p\\\
\Delta\dot{p}\end{bmatrix}=\underbrace{\begin{bmatrix}0&I\\\ -\Upsilon
K&-\Upsilon B\end{bmatrix}}_{A}\underbrace{\begin{bmatrix}\Delta p\\\
\Delta\dot{p}\end{bmatrix}}_{x}+\underbrace{\begin{bmatrix}0\\\
\ddot{\bar{p}}\end{bmatrix}}_{b}$ (11)

This dynamical system does not have the same form as (5) because $\Upsilon$
(and thus $A$) depends on $q$. However, $\Upsilon$ is typically a well-
conditioned function, meaning that it changes little for small variations of
$q$. The same holds for $\ddot{\bar{p}}$ (and thus $b$), which is why multi-
body systems without contacts can typically be integrated with large time
steps ($\approx$5 ms). This means that we can approximate $A$ and $b$ as
constants during the integration step, and therefore treat (11) as linear. We
can now express the contact forces as:

$\displaystyle\lambda(t)$
$\displaystyle=\underbrace{\begin{bmatrix}-K&-B\end{bmatrix}}_{D}x(t)=De^{tA}x(0)+D\int_{0}^{t}e^{\tau
A}\text{d}\tau\,b$ (12)

Substituting (12) in (3) we can compute the robot accelerations:

$\displaystyle\dot{v}(t)$
$\displaystyle=M^{-1}(u+J^{\top}\lambda(t))=\dot{\bar{v}}+M^{-1}J^{\top}Dx(t),$
(13)

where we consider all terms constant during the integration step, except for
$x(t)$. Now we can integrate to get the new velocities $v^{+}$:

$\displaystyle v^{+}$ $\displaystyle=v+\int_{0}^{\delta
t}\dot{v}(\tau)\,\text{d}\tau=$ (14) $\displaystyle=v+\delta
t\,\dot{\bar{v}}+M^{-1}J^{\top}D\int_{0}^{\delta t}x(\tau)\,\text{d}\tau$

Finally we integrate twice to get the new configuration $q^{+}$:

$\displaystyle q^{+}$ $\displaystyle=q+\int_{0}^{\delta
t}v(\tau)\text{d}\tau=$ (15) $\displaystyle=q+\delta t\,v+\frac{{\delta
t}^{2}}{2}\dot{\bar{v}}+M^{-1}J^{\top}D\int_{0}^{\delta
t}\int_{0}^{\tau}x(\tau_{1})\,\text{d}\tau_{1}\,\text{d}\tau$

###  III-A Integration of Matrix Exponentials

Eq. (14) and (15) are straightforward to compute, except for their last terms,
which are:

$\displaystyle x_{int}(t)$
$\displaystyle\triangleq\int_{0}^{t}x(\tau)\,\text{d}\tau$ (16) $\displaystyle
x_{int2}(t)$
$\displaystyle\triangleq\int_{0}^{t}\int_{0}^{\tau}x(\tau_{1})\,\text{d}\tau_{1}\,\text{d}\tau,$

where

$\displaystyle x(t)$ $\displaystyle=e^{tA}x(0)+\int_{0}^{t}e^{\tau
A}\,\text{d}\tau\,b$ (17)

When $A$ is invertible we can express the integral of $e^{tA}$ as an algebraic
function of $e^{tA}$:

$\displaystyle\int_{0}^{t}e^{\tau A}\text{d}\tau=A^{-1}(e^{tA}-I)$ (18)

However, $A$ is not invertible if the contact Jacobian $J$ is not full-row
rank. Luckily, the computation of integrals involving matrix exponentials has
been thoroughly investigated [28, 29]. In Section V we show how to compute
these integrals indirectly, by simply computing the matrix exponentials of an
augmented system.

###  III-B Extension to non-Euclidian spaces

When $q$ does not belong to an Euclidian space (as in the case of legged
robots, where $q$ includes the orientation of the base link) the integration
of $q$ is slightly more complicated (while the integration of $v$ remains
unchanged). Given the following definition:

$\displaystyle v_{mean}$ $\displaystyle\triangleq v+\frac{\delta
t}{2}\dot{\bar{v}}+\frac{1}{\delta t}M^{-1}J^{\top}Dx_{int2}(\delta t)$ (19)

The integration step of $q$ is computed as:

$\displaystyle q^{+}$ $\displaystyle=\text{integrate}(q,\delta t\,v_{mean}),$
(20)

where the function _integrate( $\cdot$)_ performs integration in the non-
Euclidian space of $q$.

##  IV Frictional Contacts

So far we have assumed that contact forces were bilateral. However, we
typically want to simulate unilateral contacts, where forces oppose
penetration but do not oppose detachment of bodies. Assuming that the contact
forces are expressed in a local reference frame with the z direction aligned
with the contact normal, unilateral forces must satisfy:

$\displaystyle f_{i}^{z}\geq 0\qquad\forall i$ (21)

Moreover, tangential forces are typically limited as well. Assuming a Coulomb
friction model we have:

$\displaystyle\sqrt{(f_{i}^{x})^{2}+(f_{i}^{y})^{2}}\leq\mu
f_{i}^{z}\qquad\forall i,$ (22)

where $\mu\in\mathbb{R}^{+}$ is the coefficient of friction111We assume that
static and dynamic coefficients of friction are equal.. We can represent the
constraints (21) and (22) as $\lambda\in\mathcal{K}_{\mu}$, with
$\mathcal{K}_{\mu}$ being a second-order cone.

###  IV-A Force Projection

To account for these constraints, when the value of $\lambda(t)$ computed by
(12) is outside $\mathcal{K}_{\mu}$, we should project it on the boundaries of
$\mathcal{K}_{\mu}$. However, we do not know how to check this constraint in
continuous time. In the same spirit of time-stepping simulators [1], we
suggest to check friction constraints on the average value of $\lambda(t)$
during the integration step, which is:

$\displaystyle\bar{\lambda}\triangleq\frac{1}{\delta t}\int_{0}^{\delta
t}\lambda(\tau)\,\text{d}\tau=\frac{1}{\delta t}Dx_{int}(\delta t)$ (23)

If $\bar{\lambda}\notin\mathcal{K}_{\mu}$, then we compute its projection on
the boundaries of the friction cone
$\lambda_{pr}=\text{proj}_{\mathcal{K}_{\mu}}(\bar{\lambda})$ and we use it to
compute the next state:

$\displaystyle\dot{v}_{pr}\triangleq M^{-1}(u+J^{\top}\lambda_{pr})$ (24)
$\displaystyle v^{+}=v+\delta t\,\dot{v}_{pr},\qquad q^{+}=q+\delta
t\,v+\frac{\delta t^{2}}{2}\dot{v}_{pr}$

Note that in case $\bar{\lambda}\in\mathcal{K}_{\mu}$, then
$\lambda_{pr}=\bar{\lambda}$ and the velocity update in (24) is equivalent to
(14). However, the position update in (24) approximates the double integral of
$\lambda(t)$ assuming a constant force ($\lambda_{pr}$), and so it is not
equivalent to (15) in general. In order to exploit also the double integral of
$x(t)$, we can check the friction cone constraints on the average of the
average $\lambda(t)$, computed as:

$\displaystyle\bar{\bar{\lambda}}\triangleq\frac{2}{\delta
t^{2}}\int_{0}^{\delta
t}\int_{0}^{\tau}\lambda(\tau_{1})\,\text{d}\tau_{1}\text{d}\tau=\frac{2}{\delta
t^{2}}Dx_{int2}(\delta t)$ (25)

If $\bar{\bar{\lambda}}\notin\mathcal{K}_{\mu}$, then we project it on the
boundaries of the friction cone
$\lambda_{pr2}=\text{proj}_{\mathcal{K}_{\mu}}(\bar{\bar{\lambda}})$ and we
use it to compute the next position:

$\displaystyle\dot{v}_{pr2}$ $\displaystyle\triangleq
M^{-1}(u+J^{\top}\lambda_{pr2})$ (26) $\displaystyle q^{+}$
$\displaystyle=q+\delta t\,v+\frac{\delta t^{2}}{2}\dot{v}_{pr2}$

Using (26) for the position update and (24) for the velocity update, both
updates are equivalent to the original ones in case of no slippage.

###  IV-B Anchor point update

When slippage occurs, the tangent anchor point state
$(p_{0}^{t},\dot{p}^{t}_{0})$ (where the index $t$ indicates the tangent
directions) changes, which has two main implications. First, the assumption
$\ddot{p}_{0}=0$ that we took to write the contact point dynamics as (11) is
no longer valid. This means that, during slippage, (11) is an approximation of
the contact point dynamics, based on a “business as usual” assumption (i.e.,
that the anchor point $p_{0}$ continues slipping at constant velocity).
Second, the anchor point state should be updated so that the contact forces at
the end of the time step are inside the friction cones. When a contact is
slipping, the tangent anchor point velocity converges to $\dot{p}^{t}$. We
show it now for the case of a 2D contact, but a similar reasoning can be
applied to the 3D case. While a contact is slipping, the tangential force
$\lambda^{t}$ remains on the boundary of the friction cone, so we have:

$\displaystyle\dot{\lambda}^{t}$ $\displaystyle=\mu\dot{\lambda}^{n}$ (27)
$\displaystyle
K^{t}(\dot{p}_{0}^{t}-\dot{p}^{t})+B^{t}(\ddot{p}_{0}^{t}-\ddot{p}^{t})$
$\displaystyle=\mu\dot{\lambda}^{n}$
$\displaystyle(\ddot{p}_{0}^{t}-\ddot{p}^{t})$
$\displaystyle=-(B^{t})^{-1}K^{t}(\dot{p}_{0}^{t}-\dot{p}^{t})+\mu(B^{t})^{-1}\dot{\lambda}^{n}$

The last equation shows that, if $\dot{\lambda}^{n}=0$, we have an exponential
convergence to zero of $(\dot{p}_{0}^{t}-\dot{p}^{t})$, with rate
$(B^{t})^{-1}K^{t}$. Since typically $(B^{t})^{-1}K^{t}$ is large, whereas
$\mu(B^{t})^{-1}\dot{\lambda}^{n}$ is small, we can expect this convergence to
be fast. For instance, if $(B^{t})^{-1}K^{t}=10^{3}$ and
$\dot{\lambda}^{n}=0$, then after 3 ms $(\dot{p}_{0}^{t}-\dot{p}^{t})$ will be
$5\%$ of its initial value. Given this fast convergence, we neglect the
transient and as soon as slippage starts we set
$\dot{p}_{0}^{t}:=\dot{p}^{t}$. Then, we compute $p_{0}^{t}$ so that the
contact force is on the boundary of the friction cone:

$\displaystyle\lambda$
$\displaystyle:=\text{proj}_{\mathcal{K}_{\mu}}(\lambda)$ (28) $\displaystyle
p_{0}^{t}$ $\displaystyle:=p^{t}+(K^{t})^{-1}\lambda^{t}$

##  V Computational Aspects

The computational bottleneck of the presented approach is the computation of
$x_{int}$ and $x_{int2}$ defined in (16). This section shows how to compute
these quantities with a matrix exponential, and how this computation can be
sped up.

###  V-A Computing $x_{int}$ and $x_{int2}$

Using the results presented in [29] we can compute $x_{int}$ and $x_{int2}$
as:

$\displaystyle\begin{bmatrix}x_{int}(t)&x_{int2}(t)\end{bmatrix}=\begin{bmatrix}I_{n}&0_{n\times
3}\end{bmatrix}e^{t\bar{A}}\begin{bmatrix}0_{(n+1)\times 2}\\\
I_{2}\end{bmatrix}$ (29)

where $n$ is the size of $A$, and $\bar{A}\in\mathbb{R}^{(n+3)\times(n+3)}$ is
an augmented matrix:

$\displaystyle\bar{A}\triangleq\begin{bmatrix}A&b&x(0)&0\\\ 0&0&1&0\\\
0&0&0&1\\\ 0&0&0&0\end{bmatrix}$ (30)

###  V-B Computing the Matrix Exponential

Using (29) we have transformed the problem of computing (16) into a matrix
exponential evaluation. Computing the matrix exponential is a challenging but
well-understood numerical problem [21, 30, 22, 23]. We have used as starting
point the scaling&squaring method, as revisited by Higham [21], a widely used
method for computing the exponential of small-medium size dense matrices. The
method scales the matrix by a power of 2 to reduce the norm to order 1,
computes a Padé approximant to the matrix exponential, and then repeatedly
squares to undo the effect of the scaling. A Padé approximant of a function is
its “best” approximation achievable by a ratio of two polynomials
$D_{j}(\cdot)$, $N_{j}(\cdot)$ of order $j$:

$\displaystyle e^{A}\approx D_{j}(A)^{-1}\,N_{j}(A)$ (31)

These approximants are only accurate around zero, so they cannot be used
directly if $\|A\|$ is large. When that is the case, the scaling&squaring
method is used to reduce $\|A\|$ by exploiting this property of the
exponential:

$\displaystyle e^{A}=(e^{A/(2^{s})})^{2^{s}}$ (32)

The integer scaling parameter $s$ is chosen so that $\|e^{A/(2^{s})}\|$ is
sufficiently small.

###  V-C Boosting the Matrix Exponential Computation

Our problem has two features that we can exploit to speed up computation:

  1. 1.

We do not need double machine precision, i.e. $\approx 10^{-16}$, (which is
the target of the algorithm of [21]) because we are typically fine with much
larger numerical integration errors, e.g. $\approx 10^{-4}$.

  2. 2.

We do not need the whole matrix exponential, but only its product with a
2-column matrix, as shown in (29).

The first point is easily exploitable. The choice of the scaling parameter $s$
and the polynomial order $j$ is usually optimized to achieve double machine
precision with the minimum amount of matrix-matrix multiplications. We have
empirically found that for our tests we can set $s=0$ and use a relatively low
order $j\in[1,2,3,5,7]$, corresponding to $[0,1,2,3,4]$ matrix-matrix
multiplications, respectively. Which polynomial order is optimal depends on
the specific test, and is discussed in the next section.

Regarding the second point, given a matrix $V$, we can directly compute the
product $e^{A}V$ by performing operations in the following order:

$\displaystyle V_{1}$ $\displaystyle:=N_{j}(A)\,V$ (33) $\displaystyle e^{A}V$
$\displaystyle:=D_{j}(A)^{-1}\,V_{1}$

This is faster than computing $e^{A}$ and then multiplying it times $V$
because we have to solve the linear system with a much smaller right-hand-side
($V_{1}$ rather than $N_{j}$).

Finally, we have also observed that the preprocessing step suggested in [21],
which uses _matrix balancing_ , is extremely effective at reducing $\|A\|$ in
our tests. This is crucial to achieve accurate results with low polynomial
orders, therefore speeding up computations. Further details can be found in
our open-source online repository222https://github.com/andreadelprete/consim.

##  VI Results

We assess the performance of our simulation algorithm (_Expo_) comparing it to
Implicit Euler (_Eul-imp_), Runge-Kutta 4 (_RK4_) and explicit Euler (_Eul-
exp_). Our implementation of _Eul-imp_ is described in the Appendix. Our
implementation of _RK4_ is standard, whereas _Eul-exp_ was implemented as
follows:

$\displaystyle v^{+}=v+\delta t\,\dot{v},\qquad
q^{+}=\text{integrate}(q,\delta t\,v+\frac{\delta t^{2}}{2}\,\dot{v})$ (34)

Our results try to answer to the following questions:

  1. 1.

Can our approach (compared to the others) achieve higher accuracy for equal
computation time, or equal accuracy for smaller computation time? (Section
VI-C)

  2. 2.

How sensitive is the simulator accuracy to contact stiffness and damping?
(Section VI-D)

  3. 3.

What is the maximum integration time step that results in a _stable_ 333We say
that a simulation is “stable” if the robot state remains bounded. motion?
(Section VI-E)

  4. 4.

How accurately can (11) predict future contact forces when assuming constant
$A$ and $b$? (Section VI-F)

  5. 5.

How much computation time is spent in the different operations of our
simulator? Is there room for improvement? (Section VI-G)

###  VI-A Accuracy Metric

Following an approach similar to [15], we measure accuracy with a _local
integration error_. We compute the ground truth trajectory $x_{q}(t)$ using
the simulator under analysis with an extremely small time step $\delta t=1/64$
ms. Let us define $\hat{x}_{q}(t;t-\delta t_{c},x_{q}(t-\delta t_{c}))$ as the
state at time $t$ obtained by numerical integration starting from the ground-
truth state $x_{q}(t-\delta t_{c})$, where $\delta t_{c}(\geq\delta t)$ is the
time step of the controller. We define the _local_ integration error as the
error accumulated over one control time step:

$e(t)\triangleq\|x_{q}(t)\ominus\hat{x}_{q}(t;t-\delta t_{c},x_{q}(t-\delta
t_{c})\|_{\infty}$

where $\ominus$ is a difference operator on the space of $x_{q}$. In the
numerical integration literature [12] the _local_ integration (or truncation)
error is typically defined using the integration step $\delta t$ rather than
the controller step $\delta t_{c}$. We chose to use the controller step to
make errors comparable across tests with different integration steps (as in
[15]).

###  VI-B Test Description

TABLE I: Controller time steps. Test |  $\delta t_{c}$ [ms] |  Test |  $\delta t_{c}$ [ms]
---|---|---|---
Solo-squat |  40 |  Solo-trot |  2
Solo-jump |  10 |  Romeo-walk |  40

To evaluate the trade-off between accuracy and computation time we tested each
simulator with different time steps. For _Expo_ , _RK-4_ , _Eul-imp_ we have
started from $\delta t=1/8$ ms up to the controller time step $\delta t=\delta
t_{c}$ with a logarithmic step of 2 (i.e. 1/8, 1/4, …, $\delta t_{c}/2$,
$\delta t_{c}$). For _Eul-exp_ we have used the same approach, but starting
from a value of $\delta t$ resulting in roughly the same computation time of
_Expo_. For every test we have set $\delta t_{c}$ to the largest value that
still ensured control stability (see Table I). Since our main interest lies in
legged robots, our tests focused on quadrupeds and bipeds:

  * •

_Solo-squat_ : Quadruped robot Solo [31] performing a squatting motion.

  * •

_Solo-jump_ : Quadruped robot Solo jumping in place.

  * •

_Solo-trot_ : Quadruped robot Solo trotting forward.

  * •

_Romeo-walk_ : Humanoid robot Romeo [32] taking two walking steps.

It is important to note that the quadruped Solo has a total of 13 links, 18
degrees of freedom, 12 of which are actuated revolute joints, and 4 contact
points (one on each foot). As for the humanoid Romeo, it consists of 32 links,
37 degrees of freedom, 31 actuated revolute joints and a total of 8 contact
points (four on each foot). In all tests, the control torques have been
computed with a feedback controller, either a linear controller or a Task-
Space Inverse Dynamics controller. If not specified otherwise, we have used a
contact stiffness $K=10^{5}$ N/m, and a contact damping $B=300$ Ns/m, which
are reasonable values for contacts with a hard floor. For homogeneity we have
used the same value of friction coefficient $\mu=1$ across all our tests, even
though this large friction was only needed for control stability of the
quadruped jumping motion. Besides testing the default _Expo_ simulator, we
also tested 5 other versions of the same scheme where we used a reduced
polynomial order in the Padé approximant of the matrix exponential. This leads
to a reduced number of matrix-matrix multiplications (mmm), between 0 and 4
(see Section V-C). This results in a faster but potentially less accurate
computation of the matrix exponential.

All the code has been implemented in C++ and binded with Python. For all
dynamics computation we have used the Pinocchio library [33].

###  VI-C Accuracy-Speed Results

Fig. 2 and 3 summarize the results for the four tests. Fig. 2 plots _local
errors_ vs _real-time factor_ , which measures how many times the simulation
was faster than real time. Fig. 3 instead plots _local errors_ vs _integration
time step_. Even though our main interest is in the trade-off between
computation time and accuracy, which is depicted in Fig. 2, we decided to
report also the accuracy as a function of integration time in Fig. 3, to
provide more information about the behavior of the different methods.

(a) Solo squat

(b) Solo jump

(c) Solo trot

(d) Romeo walk

Figure 2: Local integration errors vs real-time factors. The label _mmm-1_ in
the legend corresponds to using the default number of matrix-matrix
multiplications (mmm) in the computation of the matrix exponential.

(a) Solo squat

(b) Solo jump

(c) Solo trot

(d) Romeo walk

Figure 3: Local integration errors vs integration time step.

Overall, _Expo_ outperformed the other methods in all tests, showing faster
computation for equal accuracy, or greater accuracy for equal computation
time. Surprisingly, the second best method overall was the simple _Eul-exp_ ,
even though it was partially beaten by _Eul-imp_ in “solo-squat”. _RK4_ was
comparable to _Eul-exp_ for small time steps, but surprisingly worse for large
time steps. These results show a sudden increase of integration error of _Eul-
exp_ and _RK4_ for large real-time factors—corresponding to large $\delta t$.
This is because of the poor stability of explicit methods.

_Eul-imp_ sometimes failed to converge to the desired error threshold
($10^{-6}$), in particular when using large integration steps. This is not
surprising because the system dynamics is non-continuous at impacts, and _Eul-
imp_ uses a gradient-based method (Newton) that is suited for smooth systems.
Despite this, Fig. 3 shows that in most cases _Eul-imp_ gives integration
errors similar to _Expo_ for the same integration time step. More precisely,
_Eul-imp_ is almost indistinguishable from _Expo_ in “solo-squat” for $\delta
t\leq 5$ ms, whereas it is unstable for larger $\delta t$. _Eul-imp_ is a bit
worse than _Expo_ in “solo-jump” and “romeo-walk” (but only for large time
steps), and slightly better in “solo-trot”. Overall, _Eul-imp_ performed
similarly to _Expo_ for the same $\delta t$, but resulted in much larger
computation times, making it often the worst one in terms of accuracy-speed
trade-off.

_Expo_ instead shows a graceful degradation of accuracy for large real-time
factors, making it an excellent candidate for fast low-accuracy simulations,
which are typically desirable in MPC. In general, the _Expo_ versions using a
reduced number of mmm outperformed the standard _Expo_ , but which number of
mmm is optimal depends on the specific test and time step. As expected, when
$\delta t$ is smaller we can use a lower number of mmm. Automatically finding
the optimal number of mmm is an interesting direction for future work.

###  VI-D Stiffness and Damping

(a) Varying contact stiffness (with fixed damping ratio of 0.5)

(b) Varying contact damping ratio (with fixed stiffness of $10^{5}$ N/m).

Figure 4: Local integration errors vs contact stiffness and damping ratio for
the “solo trot” test using a fixed integration time step for _Eul-exp_ (1/2
ms), _Expo_ (2 ms) and _Eul-imp_ (2 ms).fig:local˙errorvs˙stiff˙damp

This subsection investigates the sensitivity to contact stiffness and damping
ratio of _Expo_ , _Eul-exp_ and _Eul-imp_. The damping ratio is defined as
$\frac{B}{2\sqrt{K}}$. A damping ratio of 1 corresponds to a _critically
damped_ contact. These results are based on the “solo-trot” scenario. For
_Eul-exp_ we have used $\delta t$=1/2 ms (real-time factor $\approx$50). Then,
we have set $\delta t$=2 ms for _Expo_ so that it had roughly the same
computation time, and $\delta t$=2 ms for _Eul-imp_ , so that it performed
similarly to _Expo_ (even though with much larger computation times).

Fig. LABEL:fig:local_errorvs_stiff_damp shows the local integration error as
we vary the contact stiffness (with fixed damping ratio) and the damping ratio
(with fixed contact stiffness). _Expo_ performs consistently as damping ratio
and stiffness increase up to $K=10^{8}$, which roughly corresponds to a ground
penetration of 0.01 mm for 100 kg of weight on a single contact point. The
error of _Eul-exp_ instead is highly affected by both stiffness and damping.
_Eul-imp_ performed slightly better than _Expo_ in most cases (but at the cost
of being 50 times slower), except for very stiff contacts ($K\geq 10^{6}$),
where it led to larger errors.

###  VI-E Stability

TABLE II: Maximum integration time steps to achieve a stable motion. Test |  _Expo_ $\delta t$ [ms] |  _Eul-exp_ $\delta t$ [ms] |  _Eul-imp_ $\delta t$ [ms]
---|---|---|---
Solo-squat |  40 |  40/512 $\approx$ 0.08 |  40/2 = 20
Solo-jump |  10 |  10/64 $\approx$ 0.16 |  10/2 = 5
Solo-trot |  2 |  2/16 $\approx$ 0.13 |  2
Romeo-walk |  40/8 = 5 |  40/32 = 1.25 |  40/4 = 10

To test the stability of the simulators we have repeated the previous tests,
but without resetting the state to the ground truth after every control loop.
Table II reports, for _Expo_ , _Eul-exp_ and _Eul-imp_ , the largest
integration time step for which the system remained stable. _Expo_ and _Eul-
imp_ showed similar stability, both remaining stable for large time steps,
mostly between 5 and 40 ms. In the tests “solo-squat” and “solo-jump” _Expo_
was stable even with a larger time step than _Eul-imp_ , whereas the opposite
happened in “romeo-walk”. _Eul-exp_ instead showed poor stability, needing a
time step between 4 and 512 times smaller than _Expo_ to remain stable.

###  VI-F Force Prediction

(a) Contact velocity at impact: 0.1 m/s.

(b) Zero contact velocity at impact.

Figure 5: Comparison of contact forces in normal direction with forces
predicted using matrix exponential.

To gain some insights into the internal computations of _Expo_ , we show in
Fig. 5 the normal contact forces predicted with (11) assuming constant $A$ and
$b$—which is the key assumption of our method. Since $A$ and $b$ depend on $q$
and $v$, which vary during the time step, one could expect that neglecting
their variations would result in significant force prediction errors. However,
Fig. 5 shows that the force prediction can be accurate over a rather long time
horizon (20 ms). These forces were generated at the beginning of the “solo-
squat” test, using different initial velocities. Since a linear spring-damper
model is used, a sudden discontinuity in the contact force is expected when a
point reaches contact with a non-zero velocity. To demonstrate this, the
normal contact force at a single contact point is plotted for the cases of the
trotting quadruped and the walking biped in Fig. 6. Depending on the velocity
at contact time, a finite jump in the contact force is observed.

(a) Quadruped Trotting.

(b) Biped Walking.

Figure 6: Normal contact force during quadruped trotting and biped walking.

###  VI-G Computation Times

TABLE III: Computation times of _Expo_ for “solo-trot”, using zero mmm for the matrix exponential (in parentheses the values using the standard matrix exponential routine). Operation |  Mean Time [$\mu$s] |  Percentage of Total Time [%]
---|---|---
step |  39 (94) |  100
computeIntegrals |  13 (67) |  33 (72)
prepareExpLDS |  13 |  33 (14)
computeContactForces |  8 |  20 (8)
_Eul-exp_ step |  9 |  -

We report here a breakdown of the computation time of our method. The times
shown in Tab. III are for the “solo-trot” test, which means that
$v\in\mathbb{R}^{18}$ and most of the times $\lambda\in\mathbb{R}^{6}$. Most
computation time (86%) is spent in three operations: computeIntegrals,
prepareExpLDS, and computeContactForces. computeIntegrals boils down to
computing a matrix exponential. This takes 72% of the total time when using a
standard expm routine (without balancing and reduced matrix-matrix
multiplications), but it goes down to 33% with our optimized version using
zero matrix-matrix multiplications—we have seen in Fig. 2 that often this
results in only a small loss of integration accuracy. The preparation of the
linear dynamical system (11) (prepareExpLDS, which includes the computations
of $h(q,v)$ with RNEA, $M(q)$ with CRBA, and $\Upsilon$ with a custom sparse
Cholesky decomposition) takes an equal amount of time: 13 $\mu$s on average,
namely 33% of the total time. The third operation (computeContactForces) takes
20% of the total time, and it includes the computation of all kinematic
quantities (contact point positions, velocities, accelerations, Jacobian) and
the contact detection.

We believe that computation times could be improved, especially for the first
two operations. In computeIntegrals we could test novel techniques [23] to
compute the matrix exponential, exploit the sparse structure of the matrix
$A$, and warm-start the computation using quantities computed at the previous
cycle. In prepareExpLDS, the inverse contact-space inertia matrix $\Upsilon$
could be computed faster using a customized algorithm, rather than with
products between $J$, $M^{-1}$ and $J^{\top}$ [34]. Overall, it seems
impossible to reach the same efficiency of a simple _Eul-exp_ step (9 $\mu$s),
but we think we could reach computation times in the range [20, 30] $\mu$s.

##  VII Conclusions

This paper has presented a new approach to simulate articulated systems
subject to stiff visco-elastic frictional contacts. The novelty of the
approach lies in the numerical integration, which applies a first-order
Exponential Integrator scheme to the contact point dynamics to obtain a time-
varying expression of the contact forces. These contact forces are then
integrated analytically, exploiting theoretical results on the integrals of
the matrix exponential [29], and advanced numerical algorithms for its fast
computation [21]. Comparison with standard integration schemes, both implicit
and explicit, highlighted the benefits of the proposed approach in terms of
speed-accuracy trade off, and stability. Overall, the proposed approach
performed similarly to an implicit scheme in terms of stability and accuracy,
but without the excessive computational burden.

Given its good behavior in the high-speed/low-accuracy regime, we believe that
this simulation technique could be an excellent candidate for MPC. To do that,
we will need to differentiate the integration scheme, which should be
feasible. We also plan to investigate the improvement, in terms of
computational efficiency, of the needed dynamics quantities and the matrix
exponential.

## References

  * [1] M. Anitescu and G. D. Hart, “A constraint-stabilized time-stepping approach for rigid multibody dynamics with joints, contact and friction,” _International Journal for Numerical Methods in Engineering_ , vol. 60, no. Jan 2003, pp. 2335–2371, 2004.
  * [2] E. Todorov, “Implicit nonlinear complementarity: A new approach to contact dynamics,” in _2010 IEEE International Conference on Robotics and Automation_ , no. 5. IEEE, may 2010, pp. 2322–2329.
  * [3] ——, “Convex and analytically-invertible dynamics with contacts and constraints: Theory and implementation in MuJoCo,” in _Proceedings - IEEE International Conference on Robotics and Automation_ , 2014, pp. 6054–6061.
  * [4] J. Hwangbo, J. Lee, and M. Hutter, “Per-Contact Iteration Method for Solving Contact Dynamics,” _IEEE Robotics and Automation Letters (RAL)_ , vol. 3, no. 2, pp. 0–8, 2018.
  * [5] E. Drumwright, “An Unconditionally Stable First-Order Constraint Solver for Multibody Systems,” _arXiv preprint arXiv:1905.10828_ , 2019.
  * [6] Y. Tassa, T. Erez, and E. Todorov, “Synthesis and stabilization of complex behaviors through online trajectory optimization,” in _Intelligent Robots and Systems (IROS), IEEE/RSJ International Conference on_ , 2012, pp. 4906–4913.
  * [7] D. M. O. Von Stryk, “Numerical solution of optimal control problems by direct collocation,” in _Optimal Control_. Springer, 1993, pp. 129–143.
  * [8] N. Mansard, A. Del Prete, M. Geisert, S. Tonneau, and O. Stasse, “Using a Memory of Motion to Efficiently Warm-Start a Nonlinear Predictive Controller,” in _IEEE International Conference on Robotics and Automation_ , 2018, pp. 2986–2993.
  * [9] J. Viereck, J. Kozolinsky, A. Herzog, L. Righetti, and R. O. Aug, “Learning a Structured Neural Network Policy for a Hopping Task,” _IEEE Robotics and Automation Letters (RAL)_ , vol. 3, no. 4, 2018.
  * [10] R. Featherstone, _Rigid body dynamics algorithms_. New York, NY: Springer, 2014.
  * [11] K. Yamane and Y. Nakamura, “Stable penalty-based model of frictional contacts,” _Proceedings - IEEE International Conference on Robotics and Automation_ , vol. 2006, no. January, pp. 1904–1909, 2006.
  * [12] U. M. Ascher and L. R. Petzold, _Computer methods for ordinary differential equations and differential-algebraic equations_. Philadelphia, PA: Siam, 1998.
  * [13] M. Anitescu, “A Fixed Time-Step Approach for Multibody Dynamics with Contact and Friction,” in _IEEE International Conference on Intelligent Robots and Systems_ , vol. 4, 2003, pp. 3725–3731.
  * [14] A. Peiret, S. Andrews, J. Kövecses, P. G. Kry, and M. Teichmann, “Schur complement-based substructuring of stiff multibody systems with contact,” _ACM Trans. Graph._ , vol. 38, no. 5, Oct. 2019. [Online]. Available: https://doi.org/10.1145/3355621
  * [15] T. Erez, Y. Tassa, and E. Todorov, “Simulation Tools for Model-Based Robotics: Comparison of Bullet, Havok, MuJoCo, ODE and PhysX,” in _International Conference on Robotics and Automation_ , 2015.
  * [16] E. Drumwright and D. A. Shell, “Modeling contact friction and joint friction in dynamic robotic simulation using the principle of maximum dissipation,” in _Springer Tracts in Advanced Robotics_ , vol. 68, no. STAR, 2010, pp. 249–266.
  * [17] J. Loffeld and M. Tokman, “Comparative performance of exponential, implicit, and explicit integrators for stiff systems of ODEs,” _Journal of Computational and Applied Mathematics_ , 2013.
  * [18] Y. J. E. Chen, S. H. Sheen, U. M. Ascher, and D. K. Pai, “Siere: A hybrid semi-implicit exponential integrator for efficiently simulating stiff deformable objects,” _ACM Trans. Graph._ , vol. 40, no. 1, Aug. 2020. [Online]. Available: https://doi.org/10.1145/3410527
  * [19] J. Certaine, “The solution of ordinary differential equations with large time constants,” _Mathematical methods for digital computers_ , vol. 1, pp. 128–132, 1960.
  * [20] C. Moler and C. Van Loan, “Nineteen Dubious Ways to Compute the Exponential of a Matrix, Twenty-Five Years Later,” _SIAM Review_ , vol. 45, no. 1, pp. 3–49, 2003.
  * [21] N. J. Higham, “The scaling and squaring method for the matrix exponential revisited,” _SIAM Journal on Matrix Analysis and Applications_ , vol. 26, no. 4, 2005.
  * [22] A. H. Al-Mohy and N. J. Higham, “Computing the Action of the Matrix Exponential, with an Application to Exponential Integrators,” _SIAM Journal of Scientific Computing_ , vol. 33, no. 2, pp. 488–511, 2011.
  * [23] J. Sastre, J. Ibáñez, and E. Defez, “Boosting the computation of the matrix exponential,” _Applied Mathematics and Computation_ , vol. 340, no. August, pp. 206–220, 2019.
  * [24] D. L. Michels, G. A. Sobottka, and A. G. Weber, “Exponential integrators for stiff elastodynamic problems,” _ACM Transactions on Graphics_ , vol. 33, no. 1, 2014.
  * [25] Y. J. Chen, U. M. Ascher, and D. K. Pai, “Exponential Rosenbrock-Euler Integrators for Elastodynamic Simulation,” _IEEE Transactions on Visualization and Computer Graphics_ , vol. 24, no. 10, pp. 2702–2713, 2018.
  * [26] V. T. Luan and D. L. Michels, “Explicit Exponential Rosenbrock Methods and their Application in Visual Computing,” _arXiv preprint arXiv:1805.08337_ , pp. 1–18, 2018.
  * [27] E. Todorov, T. Erez, and Y. Tassa, “MuJoCo: A physics engine for model-based control,” in _Intelligent Robots and Systems (IROS), IEEE/RSJ International Conference on_ , 2012.
  * [28] C. F. Van Loan, “Computing Integrals Involving the Matrix Exponential,” _IEEE Transactions on Automatic Control_ , vol. 23, no. 3, pp. 395–404, 1978\.
  * [29] F. Carbonell, J. C. Jímenez, and L. M. Pedroso, “Computing multiple integrals involving matrix exponentials,” _Journal of Computational and Applied Mathematics_ , vol. 213, no. 1, pp. 300–305, 2008.
  * [30] A. H. Al-Mohy and N. J. Higham, “A New Scaling and Squaring Algorithm for the Matrix Exponential,” _SIAM Journal on Matrix Analysis and Applications_ , vol. 31, no. 3, 2010.
  * [31] F. Grimminger, A. Meduri, M. Khadiv, J. Viereck, M. Wuthrich, M. Naveau, V. Berenz, S. Heim, F. Widmaier, T. Flayols, J. Fiene, A. Badri-Sprowitz, and L. Righetti, “An Open Torque-Controlled Modular Robot Architecture for Legged Locomotion Research,” _IEEE Robotics and Automation Letters_ , vol. 5, no. 2, pp. 3650–3657, 2020.
  * [32] “Project Romeo http://projetromeo.com.”
  * [33] J. Carpentier, G. Saurel, G. Buondonno, J. Mirabel, F. Lamiraux, O. Stasse, and N. Mansard, “The Pinocchio C++ library: A fast and flexible implementation of rigid body dynamics algorithms and their analytical derivatives,” _Proceedings of the 2019 IEEE/SICE International Symposium on System Integration, SII 2019_ , pp. 614–619, 2019.
  * [34] R. Featherstone, “Exploiting sparsity in operational-space dynamics,” _The International Journal of Robotics Research_ , vol. 29, no. 10, pp. 1–21, 2010.