From Compliant to Rigid Contact Simulation:
a Unified and Efficient Approach
Abstract
Whether rigid or compliant, contact interactions are inherent to robot motions, enabling them to move or manipulate things. Contact interactions result from complex physical phenomena, that can be mathematically cast as Nonlinear Complementarity Problems (NCPs) in the context of rigid or compliant point contact interactions. Such a class of complementarity problems is, in general, difficult to solve both from an optimization and numerical perspective. Over the past decades, dedicated and specialized contact solvers, implemented in modern robotics simulators (e.g., Bullet, Drake, MuJoCo, DART, Raisim) have emerged. Yet, most of these solvers tend either to solve a relaxed formulation of the original contact problems (at the price of physical inconsistencies) or to scale poorly with the problem dimension or its numerical conditioning (e.g., a robotic hand manipulating a paper sheet). In this paper, we introduce a unified and efficient approach to solving NCPs in the context of contact simulation. It relies on a sound combination of the Alternating Direction Method of Multipliers (ADMM) and proximal algorithms to account for both compliant and rigid contact interfaces in a unified way. To handle ill-conditioned problems and accelerate the convergence rate, we also propose an efficient update strategy to adapt the ADMM hyperparameters automatically. By leveraging proximal methods, we also propose new algorithmic solutions to efficiently evaluate the inverse dynamics involving rigid and compliant contact interactions, extending the approach developed in MuJoCo. We validate the efficiency and robustness of our contact solver against several alternative contact methods of the literature and benchmark them on various robotics and granular mechanics scenarios. Overall, the proposed approach is shown to be competitive against classic methods for simple contact problems and outperforms existing solutions on more complex scenarios, involving tens of contacts and poor conditioning. Our code is made open-source at https://github.com/Simple-Robotics/Simple.
I Introduction
Contact interactions are the substrate of movement. When performing locomotion or manipulation tasks, one naturally makes and breaks contact with the environment. Providing robots with such abilities would require them to precisely apprehend the physics of contact. In this respect, simulators have been key components for various robotics applications. Reinforcement Learning (RL) policies for manipulation [11, 23] or locomotion [34] often rely on years of simulated trajectories during training. Alternatively, Model Predictive Control (MPC) techniques call the simulator and its derivatives at high frequency at runtime to achieve a reactive behavior [31, 29].
Simulating rigid contact interactions with friction requires solving a Nonlinear Complementarity Problem (NCP) which has been recognized as a hard problem both from an optimization and numerical perspective [2]. For this reason, simulators proceed to tradeoffs between physical realism, robustness, and efficiency. In this respect, each physics engine embeds its own contact model and contact solver, which come with their underpinning physical hypotheses and numerical capabilities.
Earlier simulators like ODE [48] and Bullet [13] have been developed for graphical purposes. Graphical applications mainly require the physics engine to provide visually consistent trajectories. Nowadays, the requirements in robotics applications are different: the physics of contacts should be as close as possible to reality, and the contact solver should be numerically efficient. Indeed, this would reduce training time and enhance the transferability of RL while making MPC more reactive.
More recently, robotics-oriented engines were developed with a focus on efficiency. MuJoCo [53] introduces a physical relaxation that induces a convex contact model with distant and compliant interactions. This choice enables the use of powerful algorithms from the optimization literature and leads to improved stability of the simulation. Its robustness has made MuJoCo the standard testbed for RL algorithms, but its reality gap still limits its use in robotics. Drake [52] leverages a similar contact model [10] and proposes a procedure to set the compliance parameter more realistically, according to the physical parameters of the simulated objects. This results in an improved sim-to-real transfer capability on manipulation tasks [28]. RaiSim [26] fixes MuJoCo’s artifacts due to distant and artificially compliant contact forces but at the expense of less robust numerics [32]. The resulting contact model drove success in quadrupedal locomotion in challenging setups [27, 34, 38].
Recent works [24, 32] compare the different existing approaches and their impact. Although they highlight the significant improvements in contact simulation driven by the efforts of the robotics community, they also reveal current simulators are all subject to trade-offs and, thus, some barriers are still to be removed. Some experimental works [19, 3] also evaluate contact models from current simulators against real-world data. They show that they still fail to capture accurately the physical phenomena involved in contact interactions. Inspired by these studies, we aim to improve the physicality of current simulators while preserving their efficiency.
We make the following contributions:
-
•
we introduce a new formulation and related algorithm for solving forward dynamics problems with contacts that can efficiently deal with compliant and rigid contacts without making any physical relaxation,
-
•
we propose a formulation and an algorithmic solution for computing inverse dynamics with rigid and compliant contacts, the reciprocal of forward dynamics, extending the seminal work of Todorov [53],
-
•
we provide an open-source and efficient C++ implementation leveraging the Eigen library [22], and that can be easily plugged into existing simulation frameworks,
-
•
we extensively evaluate the proposed solutions on both mechanics and robotics scenarios of the literature.
The paper is organized as follows. We first provide the necessary background on constrained dynamics, how it connects to proximal optimization, the NCP appearing during the simulation of frictional contacts and the existing solver from the robotics community (Sec. II). Second, we propose an ADMM-based algorithm for contact solving (Sec. III). Third, we introduce a similar algorithm for the problem of inverse dynamics in the presence of contacts (Sec. IV). Finally, we evaluate our approach against standard problems of the robotics and mechanics communities (Sec. V) before discussing how it compares to previous works (Sec. VI).
II Background
In this section, we recall the principles underlying the simulation of systems under rigid and compliant constraints. To prepare the ground for the proposed algorithm, we introduce proximal operators and how they can be interpreted from a mechanics point of view. Finally, we introduce the NCP underlying the simulation of physical systems in the presence of frictional contacts and the existing approaches to solve it.
II-A Equality-constrained dynamics
The free motion of a poly-articulated system is governed by the so-called Lagrangian dynamics of the form
(1) |
where we denote by , and , the joint configuration vector, the joint velocity vector and the joint torque vector. is the joint-space inertia matrix and includes terms related to the gravity, Coriolis, and centrifugal effects. Considering constraints such as kinematic loop closures or anchor points can be done by adding implicit equations of the form
(2) |
where is the constraint function of dimension . Such constraints act on the system via the constraint forces that are spanned by the transpose of its Jacobian, denoted by . Thus, the constrained equation of motion reads
(3) |
In order to simulate the constrained system, given a torque input and knowing , , one needs to solve (3) and (2) for the unknowns and . To do so, it is more convenient to first rewrite (2) as a function of by proceeding to index reduction. Thus, differentiating (2) twice with respect to time yields
(4) |
For index reduction to be valid, it implicitly assumes that the preceding once differentiated and the original systems (2) are verified at the current time-step. Due to numerical reasons, such a condition is never exactly met, and a corrective term is added to to avoid numerical drift as done in the Baumgarte stabilization techniques. Combining the equations (3) and (4) leads to
(5) |
At this stage, it is worth noting that (5) is often not invertible as cases where is rank deficient i.e. hyper-static systems, are common in robotics.
II-B Compliant constraints
In the previously described constrained dynamics, rigid constraints were modeled via (2) which enforces to be null. When modeling compliant constraints, the function is allowed to be non-null but now defines a potential energy where for any and any positive semi-definite. Here, denotes compliance, which is a physical property of the system. Such potential energy leads to a constraint torque
(6) |
which induces (2) to be replaced by a linear map** between constraint forces and constraint violation
(7) |
As previously described, proceeding to index reduction with (7) and adding (3), simulation of compliant constraints leads to the following system
(8) |
whose only difference with (5) lies in the null lower right block being replaced by the compliance matrix . Numerically, this difference appears to be critical. Indeed, being positive definite makes the system (8) always well-defined and invertible [54].
II-C Constrained dynamics from an optimization perspective
All the previous equations could be alternatively derived from an optimization standpoint. Indeed, one could observe that (5) coincides with the Karush–Kuhn–Tucker (KKT) conditions of the following Quadratic Programming (QP):
(9) | ||||
where is the so-called joint free acceleration of the unconstrained system (1). Problem (9) corresponds to the formulation of the least constraint principle [7] and is equivalent to the following saddle-point problem:
(10) |
with being the Lagrangian associated to (9).
II-D Proximal algorithms
Proximal algorithms [44] are a general class of optimization techniques, ubiquitous in convex optimization, and are essentially rooted around the notion of proximal operators. The proximal operator [39] of a convex function is defined by
(12) |
where is homogeneous to the inverse of a step size. A specific property of this operator is that its fixed points coincide with the set of minimizers of . The proximal point algorithm [44] uses the corresponding fixed point iteration
(13) |
and is known to converge towards a minimizer of .
Considering the particular case of QP, one seeks to solve
(14) | ||||
where is a symmetric positive semi-definite matrix, , and . The Lagrangian of this problem writes,
(15) |
where is the dual variable. The solution to the problem verifies
(16) |
and the KKT conditions
(17) |
When applied to the dual part of the QP (14), the proximal point algorithm can be written as
(18) |
where the minus sign in front of the term comes from the maximization over the dual variable . The solution of the proximal iteration defined in Eq. (18) is obtained by solving the following system of equations
(19) |
This approach is widely used in optimization and at the core of several optimization solvers [50, 4] and algorithms [9] for solving constrained dynamical problems.
Link between proximal and compliance terms. Going back to the simulation of constrained systems, one should notice the similarity between (8) and (19). Interestingly, drawing parallels between these two systems, the proximal regularization acts as a numerical compliance, thus making the iterates (19) always well-defined. However, due to different regularization terms in (11) and (18), the effect of the proximal operator differs from a standard mechanical compliance. This translates into a shift in the right-hand side of (19) through the term . As further discussed in [44], this term cancels out over the iterations, making the proximal solution converge towards the solution of the original rigid problem (17). Therefore, proximal regularization can be seen as a numerical technique akin to vanishing compliance.
II-E Modeling frictional unilateral contacts: the Nonlinear Complementary Problem
Previously described equations govern the motion of systems under equality constraints, i.e., bilateral constraints.
However, punctual contact interactions are subject to three main modeling hypotheses: the unilateral contact constraint, the Coulomb friction law, and the Maximum Dissipation Principle (MDP).
Next, we detail these principles and the problem they induce.
Unilateral contact hypothesis. Through the unilateral hypothesis, one assumes that objects cannot interpenetrate, yielding the following inequality:
(20) |
where denotes the separation vector [18] defined as the minimum norm translation vector putting two shapes at a null distance, being the number of contact points, and the subscripts and refer to the normal and tangential indices, respectively.
By duality, such a constraint induces contact forces that can only act in a repulsive fashion and when objects are in contact, i.e., when . These constraints are summarized by the Signorini condition [47]
(21) |
where for vectors and means .
We use an impulse-based formulation to deal with rigid dynamics and impacts. Applying the Euler symplectic scheme to discretize (3) leads to
(22) |
where now denotes an impulse, , being the time step, and is the Jacobian of . Here, the choice of integration scheme only requires one costly evaluation of , , at the current time step , . More involved implicit integrators could be envisaged to improve stability, but they would also need to evaluate these operators multiple times. In particular, they would require multiple calls to the contact-detection and rigid-body algorithms, which can be computationally expensive. As was done earlier, the Signorini condition can be written in impulse via index reduction [40]
(23) |
where is equal to plus previously mentioned corrective terms. We note the contact point velocities.
Similarly to the bilateral case from Sec. II, any contact constraint can be made compliant by modifying the Signorini condition
(24) |
The additional compliance term of (24) introduces possible interpenetration which can be used to model soft bodies.
More involved deformation models based on the finite element method [45] are also used in robotics [35] but requires an extra computational cost.
Coulomb’s law of friction is generally employed to model dry friction via
(25) |
where is the second-order friction cone, is the vector of coefficient of frictions
and the superscript refers to the indices associated with the i-th contact point.
By noting the Cartesian product , (25) can be aggregated into .
Maximum dissipation principle. In addition, the maximum dissipation principle (MDP) [40] states that, whenever a contact point is sliding, friction impulses should maximize the dissipated power
(26) |
where is the contact point velocity previously defined.
Nonlinear complementary problem. Finally, combining equations (22), (24), (25), (26), verifies the following NCP
(27) |
where is the dual friction cone of , is the so-called Delassus matrix [15], is the free contact point velocities plus corrective terms, and we use the shorthand notation . We use the notation with denoting the De Saxcé correction [14].
II-F Existing solvers
Due to the non-convexity and nonsmoothness of the complementarity constraint, the nonlinearity of the DeSaxcé correction, and the ill-conditioning of the Delassus matrix , the NCP (27) is known to be a numerically hard problem to solve in general [2].
A first class of simulators, e.g., ODE [48], PhysX [36], DART[33], proceed by linearizing the friction cones to get an approximate but more tractable Linear Complementarity Problem (LCP). LCPs are well-studied [12] and can be solved with the Projected Gauss-Seidel (PGS) algorithm. As a first-order algorithm, PGS is not affected by null eigenvalues due to hyperstaticity but is sensitive to the conditioning of the Delassus matrix , thus hindering robustness. In its recent versions, Bullet [13] implements a similar PGS algorithm working on the original second-order friction cone.
An alternative approach, notably adopted in MuJoCo[53] and Drake[52], consists in ignoring the DeSaxcé correction in (27), which has the effect of relaxing the Signorini condition [32]. This leads to a Cone Complementarity Problem (CCP) which is equivalent to a convex Second-Order Cone Programming (SOCP) problem [6]. Such a problem can be solved via robust off-the-shelf optimization algorithms benefiting from strong guarantees. In this respect, a previous work [51] uses an ADMM algorithm to solve this problem. In MuJoCo [53] and Drake [52] simulators, the hyperstatic cases are handled by systematically adding compliance to the problem, thus making it impossible to simulate purely rigid systems.
Raisim [26] uses a specific contact model enforcing the Signorini condition, which combines favorably with its custom per-contact bisection algorithm. Despite its computational efficiency, this approach inherits the sensitivity to conditioning from Gauss-Seidel-like techniques.
Dojo [25] avoids any physical relaxation and uses an Interior Point (IP) algorithm to solve (27). As a second-order algorithm, Dojo’s solver can handle ill-conditioned problems. However, just like IP algorithms, the resulting algorithm is difficult to warm-start and requires an expensive Choleksy computation at each iteration, which makes the approach time-consuming, and thus limits its range of applications, notably in the context of real-time control scenarios.
III Efficient solving of frictional contact dynamics
In this section, we detail the central contribution of this paper, namely a novel algorithm to solve NCPs of the form of problem (27). At the core of our approach, is the development of a new ADMM method and update strategy, enabling us to solve complex contact problems, that might be poorly conditioned, as it might occur in contact mechanics problems.
III-A NCP as a cascade of optimization problems
The NCP formulated in (27) corresponds to the solving of the interweaving problems of the form
(28a) | ||||
and | ||||
(28b) |
Indeed, if we denote by the dual variable associated to the primal variable , the Lagrangian of Problem (28) reads
(29) |
The optimality conditions are thus given by canceling the gradient of w.r.t. at the optimal solution
(30) |
and the fact that
(31) |
Injecting (30) into (31), leads to the nonlinear complementarity conditions of (27).
Interestingly, (28a) appears to be a convex problem which proves to be useful as it allows sub-steps of our ADMM approach to be performed efficiently. However, the interweaving of (28a) and (28b) through the non-smooth de Saxcé function makes the problem (27) difficult to solve in general by standard optimization techniques [2]. As discussed in [2], one approach consists in incorporating estimates of (which is equal to ), via the updates of a variable named , inside the solving of (28a). More specifically, this results in a cascade of optimization problems and is done by setting, at the iterate, (Alg. 1, line 1), corresponding to the nonlinear term in (28) considered as constant, then solving (28a) (Alg. 1, line 1) and updating (Alg. 1, line 1) from the new optimal force vector . The solving of this cascade of problems continues until the primal and dual optimal convergence criteria have been met up to a certain numerical tolerance .
This cascaded strategy has shown to be effective in practice [2]. A critical part involves solving the inner optimization problem (28a) efficiently, which is nothing more than a Quadratically Constrained Quadratic Program (QCQP). One approach could thus consist in leveraging existing SOCP solvers, such as ECOS [17] or SCS [42]. Yet, because of their high level of versatility, such off-the-shelf solvers tend to be less efficient when considering a specific class of problems such as the ones occurring in contact mechanics [2]. Following this line of thought, we instead propose to develop a dedicated and efficient ADMM approach for solving 28a, particularly suited for solving ill-conditioned problems.
III-B Proximal ADMM formulation
By introducing a slack variable such that , the QCQP (28a) is made separable as follows
(32a) | ||||
(32b) |
where is a smooth convex function with being the current estimate of , and is the nonsmooth indicator function associated with the convex cone defined as
By naming the dual variable associated with (32b), the augmented Lagrangian of problem (32) reads
(33) |
where is the augmented Lagrangian penalty term [5]. defined in (33) can be equivalently rewritten as
(34) |
Finally, to make the sub-problem associated to strongly convex w.r.t. , we propose to add the proximal term to , leading to
(35) |
where is the previous estimate of . This proximal term affects the conditioning of the problem by adding a regularization to the Hessian of , which will play an essential role in the update strategy further detailed in Sec. III-F. is fixed and typically set to a value of .
III-C Pseudocode of the ADMM updates
Solving the saddle-point point problem associated with the proximal augmented Lagrangian defined in (35) can be efficiently done via ADMM iterates. The updates are defined by the following successive optimization steps:
(36a) | ||||
(36b) | ||||
(36c) |
We now detail the content of each sub-computations of (36). The first equation (36a) corresponds to the solving of an unconstrained quadratic problem, whose solution is given by:
(37) |
It is worth noting that (37) is always well-defined because of the proximal regularization. This allows our ADMM approach to handle both compliant and purely rigid contacts as illustrated in Fig.2. The second equation (36b) can be explicitly written as:
(38) |
It corresponds to the orthogonal projection of the vector on the Cartesian product of friction cones , also denoted by:
(39) |
The third equation (36c) corresponds to the classic augmented Lagrangian multiplier updates [5].
III-D Primal and dual convergence criteria
As classically done in the ADMM settings, the primal and dual residuals associated with the augmented Lagrangian function at the iterates are respectively given by
(40) |
and
(41) |
In addition, we use a contact complementarity residual defined as
(42) |
The iterations of ADMM have converged to precision when:
(43) |
A typical set of values for which offers a good compromise between realistic simulation (compared to alternative resolution methods) and computation times to solve the NCP problems is . We choose to use the infinity norm () as it is independent of the problem dimensions. As explained in [44], it is also possible to introduce relative convergence criterion, as done in many optimization solvers of the literature [42, 4], to account for the potential stagnation of the optimization variables due to the numerics.
III-E Exploiting problem sparsity
Over the three main steps of the ADMM recursion (36), the last two (36b) and (36c) are cheap operations of linear complexity w.r.t. the problem dimensions. The most complex operation lies in the resolution of the linear system in Eq. (36a), and detailed in Eq. 37. It notably requires the inversion of the augmented Delassus matrix, denoted by in the sequel. Interestingly, the additional terms only modify the diagonal of the Delassus matrix with positive elements. In other words, both the sparsity of the Delassus matrix and the positivity are preserved, allowing to directly call sparse or dense Cholesky methods to decompose the augmented Delassus .
Additionally, in the case of kinematics algorithms composed of multiple joints including loop-closure, one can leverage branch-inducing sparsity algorithms [21] to efficiently evaluate the Delassus matrix [20, 46] or directly obtain its Cholesky decomposition at a reduced cost [9]. Finally, it is worth noticing that, as soon as the ADMM penalty term is updated, it requires the full Cholesky refactorization of . In other words, to lower the computational footprint induced by the successive Cholesky factorizations, it is essential to lower the number of updates of . This motivates the introduction of the spectral update rule for , detailed in the next subsection, which is experimentally validated in Sec. V.
III-F ADMM parameters update strategies
The convergence rate of the ADMM methods is directly related to the value of the augmented penalty term [44].
Choosing this value directly depends on input problem values ( and ).
There is no automatic procedure to choose the best which will lower the number of iterations to reach the desired primal/dual accuracy .
Linear update rule. From a given initial value , a well-known strategy is to linearly update according to the ratio between primal and dual residuals , following the update rule at the iterate
(44) |
where and are increment/decrement factors, and is the ratio parameter between primal and dual residuals.
The overall idea is to maintain the trajectory of primal and dual residual norms within a tube of diameter .
Yet, if the problem is poorly conditioned, this linear update rule will often trigger many updates of , thus requiring each time to recompute the Cholesky factorization associated with the nonsingular augmented Delassus matrix .
Spectral update rule. To overcome the inherent limitations of the standard linear ADMM update rule, we introduce a new update strategy that accounts for the spectral properties of the augmented Delassus matrix . More precisely, our approach is inspired by the work of Nishihara et al. [41] which provides a convergence analysis of a generic class of ADMM formulations, including ours depicted by (32). Their analysis assumes that the ADMM penalty parameter is of the form
(45) |
where is the strong convexity parameter of and is the Lipschitz constant associated with , where refers to the smooth convex function in the ADMM formulation (32). As is a quadratic function, and respectively correspond to the lowest and largest eigenvalues of . The ratio is the condition number of and . is the free exponent parameter directly balancing the contribution of the condition number in the choice of .
While in [41] is assumed to be constant, we suggest adjusting its value according to the ratio between primal and dual residuals , in the spirit of the linear update rule recalled previously. More precisely, we propose this selection strategy
(46) | ||||
where and are increments on the exponent parameter . A typical value is . As for the linear update rule, is the ratio parameter between primal and dual residuals, forcing the primal and dual residual norms to lie within a tube of diameter . To the best of the author’s knowledge, this spectral update strategy is novel and directly scales the ADMM update parameter according to the smoothness of the NCP problem.
It is worth noticing that the proposed solution only considers the lowest and largest eigenvalues of , which can be easily estimated from the power iteration algorithm for instance, which, in practice, converges in very few iterations compared to the problem dimensions. Finally, thanks to the presence of the proximal term added in (35), we have , which guarantees the well-posedness of the strategy.
III-G Pseudocode
Algorithm 1 summarizes our ADMM-based approach for solving the NCP (27) problem of frictional contacts simulation. It takes as inputs the contact problem parameters, such as the Delassus matrix , the free contact point velocities , the Cartesian product of the friction cones , the compliance matrix , as well as a desired precision . The outputs of Alg. 1 at line 1 correspond to both the optimal contact forces and the contact point velocities . This last quantity can be directly obtained from the dual variable of the NCP problem, which corresponds to the sum of the contact point velocity and the DeSaxé corrective term .
IV Inverse Dynamics
We now consider the inverse dynamics problem, corresponding to the search of the torque and the contact impulses that induce a given joint velocity . Starting from the NCP (27) formulation and enforcing the contact point velocities to be equal to yields the inverse dynamics problem,
(47) | ||||
We recall that, from KKT conditions, a solution of (47) should minimize
(48) |
where . This corresponds to the projection on under the metric induced by , so we note the minimizer of (48). We observe the formulation of (48) becomes ill-defined for the purely rigid case . Indeed, as described previously (Sec. II), the rigidity often makes the problem of contact impulses under-determined and thus non-invertible. As was done in the case of forward dynamics (Sec. III), we aim to preserve the rigid contact hypothesis by leveraging proximal optimization. For the case of inverse dynamics, iterating the proximal operator associated to (48) (Alg. 2, line 2) allows to find a verifying (47) even in the rigid case.
At this stage, it is worth noting that (47) has the same structure as (27) and is also an NCP. Therefore, one could have used the algorithm introduced in Sec. III (Alg. 1) to solve it. However, one notable difference between (27) and (47) is the absence of the Delassus matrix in the latter, which has been replaced by the compliance matrix . Most often, the compliance matrix has a diagonal structure of the form which can be exploited to design a more efficient algorithm. Indeed, in the case of a diagonal matrix , the operator can be computed analytically via the equality with which makes the iterations of Alg. 2 computationally cheap.
Once contact forces are determined, the torque can be retrieved with a call to the Recursive Newton-Euler Algorithm (RNEA) (Alg. 2, line 2). It is worth noticing that the contribution of the contact torque can be accounted for through the backward sweep of the RNEA algorithm, thus efficiently exploiting the sparsity induced by the kinematic tree, in the spirit of rigid-body dynamics algorithms [21].
V Evaluations and Benchmarks
In this section, we evaluate our approach to challenging robotics scenarios and benchmarks from the computational mechanics community. In particular, we measure performances in terms of the number of iterations required to converge and computational timings. We also validate our inverse dynamics algorithm by performing a control task involving contact interactions with a UR5 arm. Additional results are available in the companion video (https://youtu.be/i_qg9cTx0NY?si=NGtx1tiYrIGtHXSK).
V-A Simulation benchmarks: standard robotics systems
Our solver has been implemented in C++, leveraging the Eigen library [22] for efficient linear algebra, the Pinocchio framework [8] for efficient rigid body algorithms, which comes with the HPP-FCL library [43] for fast collision detection.
As baselines, we have also implemented the over-relaxed PGS algorithm [30] and used the state-of-the-art SCS solver [42] to solve the CCP relaxation.
Our experiments are done on a MacBook Pro with a M1 Max CPU.
For all the benchmarks, we use a time step of ms and set the proximal value to .
Convergence analysis. We evaluate the convergence speed of our algorithm by monitoring the evolution of the convergence criteria and across iterations ( being null due to projection steps). The study is done on two different contact problems. The first one is a stack of rigid boxes of different masses (between and kg) hit by a ball of kg (Fig. 1). Stacking objects of high mass ratios induces bad conditioning of the Delassus matrix (these ratios are of for the Talos scene and for the wall of cubes). Therefore, this scene allows us to evaluate the numerical stability of the solvers. For the second one, we study a problem obtained with the humanoid robot Talos [49] falling on the ground, whose 36-dof kinematic chain induces a complex inertial coupling between all the contact points through the Delassus matrix. Additionally, we use these two scenarios to perform an ablation study on the benefits of using the spectral update rule for the ADMM parameter adaption.
As shown by Fig. 4, the per-contact strategy of PGS loops; this undesired behavior is explained by the fact that these problems involve high mass ratios and strongly coupled contact forces.
On the opposite, our ADMM-based algorithm exploits the Cholesky decomposition of which allows it to be insensitive to the conditioning and to capture the coupling between individual contact problems.
Figure 4 also demonstrates how leveraging the spectral information to update adapt leads to an improved convergence rate w.r.t. the linear update rule.
Therefore, our approach can efficiently solve the two problems at a high precision threshold .
Timings. When evaluating timings, we cover a range of contact types that aim to be wide enough to represent robotics applications. To do so, we consider four distinct scenarios (Fig. 1,3). For the first two, we reuse the setups of the convergence analysis: a stack of boxes of different masses (around 60 contact points) and a humanoid falling on the ground (around 10 contact points). As a third experiment, we simulate an Allegro hand holding a stack of cubes, a classical setup in manipulation applications. Finally, we evaluate our approach on a more dynamic task: a quadruped moving on a steep terrain (Fig. 3). The walking motion is generated via an MPC controller, which produces reactive behavior and results in a wide range of contact types, notably many breaking and sliding contacts.
For each benchmark, we average on a trajectory the computational time necessary to reach a fixed precision () or to hit the maximum number of iterations ().
Figure 5 summarizes our evaluation of timings on robotics systems.
We observe that our algorithm performs consistently well in different scenarios compared to the PGS and SCS algorithms, even when they operate on the relaxed CCP problem.
In particular, we note a significant performance gap between ADMM and PGS when the complexity of the structure of the inertia grows, as is the case for the hand, the humanoid, and the stack of cubes.
As shown during the convergence analysis, the complex coupling induced by can slow down per-contact approaches and even hinder convergence.
It is also worth noting that our algorithm does not require any hyperparameter tuning across the various considered scenarios.
Indeed, is automatically scaled via the spectral rule, which induces a reduced computational overhead when combined with rigid body algorithms for the Cholesky updates.
V-B Comparison against state-of-the-art physics engines
We implemented our algorithm in a C++ simulation loop, building on top of Pinocchio [8] for rigid body dynamics and HPP-FCL [43] for collision detection. We evaluate our approach against the state-of-the-art simulators MuJoCo, Drake, and Bullet on three scenarios of increasing complexity in terms of degrees of freedom: a UR5 robotic arm, a Cassie robot, and the MuJoCo humanoid. For each benchmark, we simulate the robot for 2 seconds without any actuation or dam** in the joints. For all simulators, joint limits are not taken into account. The MuJoCo Humanoid is made of capsules for collisions, while the UR5 and Cassie robots use convex meshes (which can reach up to 1000 vertices per body). All possible pairs of bodies are considered for collision detection, except the successive pairs in the kinematic chain. Performance is measured by the number of time steps per second on a single thread of an Apple M1 Max CPU. The results are reported in Fig. 6. The ADMM solver embedded in a simulation loop depicts competitive results compared to alternative solutions in the robotics literature.
It is worth noticing that this is a preliminary benchmark. A more detailed one (involving additional metrics, more physical systems, and comparisons to real data) would benefit the robotics community but would require a dedicated study. We leave this benchmarking study as future work.
V-C Complex scenarios of computational mechanics
In this section, we leverage the collection of complex contact problems provided in the FCLIB benchmark collections [1].
These problems correspond to a set of various mechanical problems identified as challenging by the computational mechanics community.
We focus on three main categories of problems: BoxesStack, Chain, and Capsules.
The size and properties of this dataset are reported in Tab. II.
Timings. We benchmark our solver against PGS on the three aforementioned scenarios. We request a precision of on both solvers. Due to the size of the problems, we exploit the sparse Cholesky solver coming with Eigen to account for the sparsity of the contact problems.
We use performance-profile distributions [16], a metric to fairly compare optimization solvers in the optimization community.
More precisely, for a given solver, it measures the ratio of problems solved which are times slower than the best solver for a given problem.
We refer to [16] for further details.
The performance-profile distributions are reported in Fig. 7 for the three categories.
These plots show that our solver significantly outperforms PGS on the complex contact problems collected in the FCLIB dataset.
On problems from BoxesStack and Capsules, we observe that PGS is not able to converge to the desired accuracy and reach the maximum number of iterations set to .
Category | # Problems | Dofs | friction | |
---|---|---|---|---|
BoxesStack | 255 | [0:200] | [6:300] | 0.7 |
Chain | 242 | [8:28] | [48:60] | 0.3 |
Capsules | 249 | [0:200] | [6:300] | 0.7 |
Empirical evaluation of the spectral and linear ADMM updates. In section III-F, we have motivated the introduction of the spectral update strategy as a way to reduce the number of Cholesky factorization updates involved in the ADMM algorithm 1, which is the most computationally demanding part of the ADMM solver. We evaluate this assertion by evaluating the mean number and the standard deviation of Cholesky updates over the BoxesStack, Chain, and Capsules problem categories. We notably vary the linear parameter and spectral parameter .
The results are reported in Tab. III. Depending on the problem category, the linear update strategy tends to produce higher standard deviations and mean numbers of Cholesky updates than the spectral rule. The spectral rule provides the most consistent statistics with lower updates and is less sensitive to the class of problems to solve, with an average of a dozen Cholesky updates. We also report in Tab. IV the mean timings to solve a given problem. These timings are consistent with the mean number of Cholesky updates reported in Tab. III, in the sense that best timings are obtained for the spectral update rule depicting the lowest number of Cholesky updates.
Linear | Spectral | ||||||
---|---|---|---|---|---|---|---|
Category | p = 0.01 | p = 0.05 | p = 0.08 | ||||
BoxesStack | |||||||
Chain | |||||||
Capsules |
Linear | Spectral | ||||||
---|---|---|---|---|---|---|---|
Category | p = 0.01 | p = 0.05 | p = 0.08 | ||||
BoxesStack | |||||||
Chain | |||||||
Capsules |
V-D Inverse dynamics
We evaluate our approach for inverse dynamics on the control problem consisting of finding the torque to slide the end effector of a robotic arm (UR5) on a wall where the contact is assumed to be purely rigid (Fig. 8). The reference joint velocity is such that the contact point velocity is is tangent to the wall, i.e., .
When setting to , the approach proposed in Sec. IV requires only one iteration to find a solution with a precision of . One should note that the proximal regularization is necessary due to the rigid contact hypothesis. On Fig. 8, we evaluate the benefits of incorporating the iterative De Saxcé correction (Alg. 2, line 2) and work directly with the NCP (27) model. When setting this corrective term to which exactly corresponds to the CCP contact model, we observe that the contact torque and thus the actuation torque diverge. Indeed, for a sliding motion, which causes the desired motion to be infeasible in the sense of CCP.
VI Discussion
The closest to our work are the approaches of MuJoCo [53] and Drake [10], which tend to solve linear systems coupling all the contact points together, by inverting a matrix containing the Delassus contact matrix. Unlike per-contact approaches [48, 13], we exploit the Cholesky decomposition of the Delassus matrix to jointly update contact forces, which improves robustness. As explained in Sec. III, such Cholesky decomposition can be efficiently computed by leveraging the sparsity pattern induced by the kinematic trees of robots composing the scene. Moreover, we alleviate the computational footprint by reducing the number of these Cholesky re-factorizations via a spectral adaptation of the ADMM parameter. In terms of physical realism, our improvement is twofold: we do not relax the Signorini condition, i.e., our simulator does not exhibit forces at distance, and we make it possible to simulate purely rigid systems (). Optionally, by setting De Saxcé corrective term to zero, we robustly solve the convex formulation of these simulators and retrieve convergence guarantees. For future work, further experiments should be conducted to evaluate to what extent avoiding simulation artifacts related to relaxations [24, 32] tightens the reality gap of simulators [19, 35] and how a reduced gap impacts downstream tasks.
Our approach also influences the formulation of the inverse dynamics. As shown by Todorov [53], in the case of MuJoCo and Drake, the convex relaxation and the compliance make the dynamics invertible. Because we also encompass rigid contacts, we cannot assume the map** between joint velocities and contact forces to be uniquely defined (e.g., hyperstatic scenarios). Using a proximal algorithm allows handling ill-defined cases (i.e., nonunique solution) and converges toward one possible solution of the inverse problem on forces. Moreover, via the iterative DeSaxcé correction, we preserve the NCP formulation. This enables us to invert the dynamics of motions that were previously infeasible in the sense of the CCP, for instance for sliding motions.
In a parallel line of research, a recent growing effort has been made to port classical simulators such as MuJoCo and PhysX to hardware accelerators e.g. GPUs and TPUs, which resulted in MuJoCo XLA and Isaac Gym [37]. These architectures provide high parallelization capabilities, but they impose hard constraints on the design of contact models and algorithms. The approach introduced in this paper focuses on exploiting the versatility and efficiency of modern CPUs to achieve physically accurate simulation at competitive rates. However, it seems promising for future work to adapt it in order to leverage hardware accelerators.
VII Conclusion
In this paper, we have introduced an ADMM-based algorithm to solve the NCP associated with the simulation of dynamics involving rigid frictional contacts. We have evaluated our approach to challenging benchmarks from both the robotics and computational mechanics communities. Our rich set of experiments demonstrates that we can robustly simulate a wide range of scenarios while kee** a limited computational burden and avoiding physical relaxation.
The current approach could still be improved by gaining timings on the collision detection routine corresponding to the bottleneck. Similarly, rigid-body dynamics algorithms for constrained dynamical systems still represent an active area of research whose improvements would directly affect contact solvers and, thus, physics simulation in robotics. Although we did not observe cases causing our algorithm to diverge, working towards theoretical convergence guarantees could be an interesting research direction. Moreover, our algorithm for inverse dynamics could be generalized to account for the underactuation and the unfeasible reference accelerations it can induce.
Finally, we believe these promising results are a further step towards more computationally efficient and physically consistent simulators which, due to their centrality, could positively impact the overall robotics community and related fields where efficient and reliable simulation matters.
Acknowledgements
This work was supported in part by the French government under the management of Agence Nationale de la Recherche (ANR) as part of the ”Investissements d’avenir” program, references ANR-19-P3IA-0001 (PRAIRIE 3IA Institute) and ANR-22-CE33-0007 (INEXACT), the European project AGIMUS (Grant 101070165), the Louis Vuitton ENS Chair on Artificial Intelligence and the Casino ENS Chair on Algorithmic and Machine Learning. Any opinions, findings, conclusions, or recommendations expressed in this material are those of the authors and do not necessarily reflect the views of the funding agencies.
References
- Acary et al. [2014] Vincent Acary, Maurice Brémond, Tomasz Koziara, and Franck Pérignon. FCLIB: a collection of discrete 3D Frictional Contact problems. Technical Report RT-0444, INRIA, February 2014. URL https://inria.hal.science/hal-00945820.
- Acary et al. [2017] Vincent Acary, Maurice Brémond, and Olivier Huber. On solving contact problems with Coulomb friction: formulations and numerical comparisons. Research Report RR-9118, INRIA, November 2017. URL https://hal.inria.fr/hal-01630836.
- Acosta et al. [2022] Brian Acosta, William Yang, and Michael Posa. Validating robotics simulators on real-world impacts. IEEE Robotics and Automation Letters, 7(3):6471–6478, 2022.
- Bambade et al. [2022] Antoine Bambade, Sarah El-Kazdadi, Adrien Taylor, and Justin Carpentier. Prox-qp: Yet another quadratic programming solver for robotics and beyond. In RSS 2022-Robotics: Science and Systems, 2022.
- Bertsekas [2014] Dimitri P Bertsekas. Constrained optimization and Lagrange multiplier methods. Academic press, 2014.
- Boyd and Vandenberghe [2004] Stephen P Boyd and Lieven Vandenberghe. Convex optimization. Cambridge university press, 2004.
- Bruyninckx and Khatib [2000] Herman Bruyninckx and Oussama Khatib. Gauss’ principle and the dynamics of redundant and constrained manipulators. In Proceedings 2000 ICRA. Millennium Conference. IEEE International Conference on Robotics and Automation. Symposia Proceedings (Cat. No. 00CH37065), volume 3, pages 2563–2568. IEEE, 2000.
- Carpentier et al. [2015–2021] Justin Carpentier, Florian Valenza, Nicolas Mansard, et al. Pinocchio: fast forward and inverse dynamics for poly-articulated systems. https://stack-of-tasks.github.io/pinocchio, 2015–2021.
- Carpentier et al. [2021] Justin Carpentier, Rohan Budhiraja, and Nicolas Mansard. Proximal and sparse resolution of constrained dynamic equations. In Proc. Robot., Sci. Syst., 2021.
- Castro et al. [2022] Alejandro M Castro, Frank N Permenter, and Xuchen Han. An unconstrained convex formulation of compliant contact. IEEE Transactions on Robotics, 39(2):1301–1320, 2022.
- Chen et al. [2022] Tao Chen, Jie Xu, and Pulkit Agrawal. A system for general in-hand object re-orientation. In Conference on Robot Learning, pages 297–307. PMLR, 2022.
- Cottle et al. [2009] Richard W. Cottle, Jong-Shi Pang, and Richard E. Stone. The Linear Complementarity Problem. Society for Industrial and Applied Mathematics, 2009. doi: 10.1137/1.9780898719000. URL https://epubs.siam.org/doi/abs/10.1137/1.9780898719000.
- Coumans and Bai [2016–2021] Erwin Coumans and Yunfei Bai. Pybullet, a python module for physics simulation for games, robotics and machine learning. http://pybullet.org, 2016–2021.
- de Saxcé and Feng [1998] Géry de Saxcé and Z.-Q. Feng. The bipotential method: A constructive approach to design the complete contact law with friction and improved numerical algorithms. Mathematical and Computer Modelling, 28(4-8):225–245, August 1998. doi: 10.1016/S0895-7177(98)00119-8. URL https://hal.archives-ouvertes.fr/hal-03883288.
- Delassus [1917] Étienne Delassus. Mémoire sur la théorie des liaisons finies unilatérales. In Annales scientifiques de l’École normale supérieure, volume 34, pages 95–179, 1917.
- Dolan and Moré [2002] Elizabeth D Dolan and Jorge J Moré. Benchmarking optimization software with performance profiles. Mathematical programming, 91:201–213, 2002.
- Domahidi et al. [2013] Alexander Domahidi, Eric Chu, and Stephen Boyd. Ecos: An socp solver for embedded systems. In 2013 European control conference (ECC), pages 3071–3076. IEEE, 2013.
- Ericson [2004] Christer Ericson. Real-time collision detection. Crc Press, 2004.
- Fazeli et al. [2017] Nima Fazeli, Elliott Donlon, Evan Drumwright, and Alberto Rodriguez. Empirical evaluation of common contact models for planar impact. In 2017 IEEE international conference on robotics and automation (ICRA), pages 3418–3425. IEEE, 2017.
- Featherstone [2010] Roy Featherstone. Exploiting sparsity in operational-space dynamics. The International Journal of Robotics Research, 29(10):1353–1368, 2010.
- Featherstone [2014] Roy Featherstone. Rigid body dynamics algorithms. Springer, 2014.
- Guennebaud et al. [2010] Gaël Guennebaud, Benoît Jacob, et al. Eigen v3. http://eigen.tuxfamily.org, 2010.
- Handa et al. [2023] Ankur Handa, Arthur Allshire, Viktor Makoviychuk, Aleksei Petrenko, Ritvik Singh, **gzhou Liu, Denys Makoviichuk, Karl Van Wyk, Alexander Zhurkevich, Balakumar Sundaralingam, et al. Dextreme: Transfer of agile in-hand manipulation from simulation to reality. In 2023 IEEE International Conference on Robotics and Automation (ICRA), pages 5977–5984. IEEE, 2023.
- Horak and Trinkle [2019] Peter C Horak and Jeff C Trinkle. On the similarities and differences among contact models in robot simulation. IEEE Robotics and Automation Letters, 4(2):493–499, 2019.
- Howell et al. [2022] Taylor A Howell, Simon Le Cleac’h, J Zico Kolter, Mac Schwager, and Zachary Manchester. Dojo: A differentiable simulator for robotics. arXiv preprint arXiv:2203.00806, 9, 2022.
- Hwangbo et al. [2018] Jemin Hwangbo, Joonho Lee, and Marco Hutter. Per-contact iteration method for solving contact dynamics. IEEE Robotics and Automation Letters, 3(2):895–902, 2018. URL www.raisim.com.
- Hwangbo et al. [2019] Jemin Hwangbo, Joonho Lee, Alexey Dosovitskiy, Dario Bellicoso, Vassilios Tsounis, Vladlen Koltun, and Marco Hutter. Learning agile and dynamic motor skills for legged robots. Science Robotics, 4(26):eaau5872, 2019.
- [28] Toyota Research Institute. Scoo** (sim vs reality side-by-side). https://youtu.be/5aVDWjWd0EU?si=R511Doxi7Xh-V07W.
- Jenelten et al. [2024] Fabian Jenelten, Junzhe He, Farbod Farshidian, and Marco Hutter. DTC: Deep Tracking Control. Science Robotics, 9(86):eadh5401, 2024.
- Jourdan et al. [1998] Franck Jourdan, Pierre Alart, and Michel Jean. A Gauss-Seidel like algorithm to solve frictional contact problems. Computer methods in applied mechanics and engineering, 155(1-2):31–47, 1998.
- Koenemann et al. [2015] Jonas Koenemann, Andrea Del Prete, Yuval Tassa, Emanuel Todorov, Olivier Stasse, Maren Bennewitz, and Nicolas Mansard. Whole-body model-predictive control applied to the hrp-2 humanoid. In 2015 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), pages 3346–3351. IEEE, 2015.
- [32] Quentin Le Lidec, Wilson Jallet, Louis Montaut, Ivan Laptev, Cordelia Schmid, and Justin Carpentier. Contact models in robotics: a comparative analysis.
- Lee et al. [2018] Jeongseok Lee, Michael X. Grey, Sehoon Ha, Tobias Kunz, Sumit Jain, Yuting Ye, Siddhartha S. Srinivasa, Mike Stilman, and C. Karen Liu. DART: Dynamic animation and robotics toolkit. The Journal of Open Source Software, 3(22):500, Feb 2018. doi: 10.21105/joss.00500. URL https://doi.org/10.21105/joss.00500.
- Lee et al. [2020] Joonho Lee, Jemin Hwangbo, Lorenz Wellhausen, Vladlen Koltun, and Marco Hutter. Learning quadrupedal locomotion over challenging terrain. Science robotics, 5(47):eabc5986, 2020.
- Ma et al. [2019] Daolin Ma, Elliott Donlon, Siyuan Dong, and Alberto Rodriguez. Dense tactile force estimation using gelslim and inverse fem. In 2019 International Conference on Robotics and Automation (ICRA), pages 5418–5424. IEEE, 2019.
- Macklin et al. [2019] Miles Macklin, Kier Storey, Michelle Lu, Pierre Terdiman, Nuttapong Chentanez, Stefan Jeschke, and Matthias Müller. Small steps in physics simulation. In Proceedings of the 18th Annual ACM SIGGRAPH/Eurographics Symposium on Computer Animation, SCA ’19, New York, NY, USA, 2019. Association for Computing Machinery. ISBN 9781450366779. doi: 10.1145/3309486.3340247. URL https://doi.org/10.1145/3309486.3340247.
- Makoviychuk et al. [2021] Viktor Makoviychuk, Lukasz Wawrzyniak, Yunrong Guo, Michelle Lu, Kier Storey, Miles Macklin, David Hoeller, Nikita Rudin, Arthur Allshire, Ankur Handa, et al. Isaac gym: High performance gpu-based physics simulation for robot learning. arXiv preprint arXiv:2108.10470, 2021.
- Miki et al. [2022] Takahiro Miki, Joonho Lee, Jemin Hwangbo, Lorenz Wellhausen, Vladlen Koltun, and Marco Hutter. Learning robust perceptive locomotion for quadrupedal robots in the wild. Science Robotics, 7(62):eabk2822, 2022.
- Moreau [1962] Jean Jacques Moreau. Fonctions convexes duales et points proximaux dans un espace hilbertien. Comptes rendus hebdomadaires des séances de l’Académie des sciences, 255:2897–2899, 1962.
- Moreau [1988] Jean Jacques Moreau. Unilateral Contact and Dry Friction in Finite Freedom Dynamics. In Moreau J.J. and Panagiotopoulos P.D., editors, Nonsmooth Mechanics and Applications, volume 302 of International Centre for Mechanical Sciences (Courses and Lectures), pages 1–82. Springer, 1988. doi: 10.1007/978-3-7091-2624-0“˙1. URL https://hal.archives-ouvertes.fr/hal-01713847.
- Nishihara et al. [2015] Robert Nishihara, Laurent Lessard, Ben Recht, Andrew Packard, and Michael Jordan. A general analysis of the convergence of ADMM. In International conference on machine learning, pages 343–352. PMLR, 2015.
- O’Donoghue [2021] Brendan O’Donoghue. Operator splitting for a homogeneous embedding of the linear complementarity problem. SIAM Journal on Optimization, 31:1999–2023, August 2021.
- Pan et al. [2015–2022] Jia Pan, Sachin Chitta, Dinesh Manocha, Florent Lamiraux, Joseph Mirabel, Justin Carpentier, et al. HPP-FCL: an extension of the Flexible Collision Library. https://github.com/humanoid-path-planner/hpp-fcl, 2015–2022.
- Parikh et al. [2014] Neal Parikh, Stephen Boyd, et al. Proximal algorithms. Foundations and trends® in Optimization, 1(3):127–239, 2014.
- Reddy [1993] Junuthula Narasimha Reddy. An introduction to the finite element method. New York, 27:14, 1993.
- Sathya et al. [2023] Ajay Suresha Sathya, Wilm Decre, and Jan Swevers. Pv-osimr: A lowest order complexity algorithm for computing the delassus matrix, 2023.
- Signorini [1959] Antonio Signorini. Questioni di elasticità non linearizzata e semilinearizzata. Rendiconti di Matematica e delle sue applicazioni, 18(5):95–139, 1959.
- Smith [2008] Russell Smith. Open dynamics engine, 2008. URL http://www.ode.org/. http://www.ode.org/.
- Stasse et al. [2017] Olivier Stasse, Thomas Flayols, Rohan Budhiraja, Kevin Giraud-Esclasse, Justin Carpentier, Joseph Mirabel, Andrea Del Prete, Philippe Souères, Nicolas Mansard, Florent Lamiraux, et al. Talos: A new humanoid research platform targeted for industrial applications. In 2017 IEEE-RAS 17th International Conference on Humanoid Robotics (Humanoids), pages 689–695. IEEE, 2017.
- Stellato et al. [2020] Bartolomeo Stellato, Goran Banjac, Paul Goulart, Alberto Bemporad, and Stephen Boyd. Osqp: An operator splitting solver for quadratic programs. Mathematical Programming Computation, 12(4):637–672, 2020.
- Tasora et al. [2021] Alessandro Tasora, Dario Mangoni, Simone Benatti, and Rinaldo Garziera. Solving variational inequalities and cone complementarity problems in nonsmooth dynamics using the alternating direction method of multipliers. International Journal for Numerical Methods in Engineering, 122(16):4093–4113, 2021.
- Tedrake and the Drake Development Team [2019] Russ Tedrake and the Drake Development Team. Drake: Model-based design and verification for robotics, 2019. URL https://drake.mit.edu.
- Todorov et al. [2012] Emanuel Todorov, Tom Erez, and Yuval Tassa. Mujoco: A physics engine for model-based control. In 2012 IEEE/RSJ international conference on intelligent robots and systems, pages 5026–5033. IEEE, 2012.
- Tournier et al. [2015] Maxime Tournier, Matthieu Nesme, Benjamin Gilles, and François Faure. Stable constrained dynamics. ACM Transactions on Graphics (TOG), 34(4):1–10, 2015.