Contact-Implicit Inverse Dynamics

Etienne Ménager, Antoine Bambade, Wilson Jallet, Alberto de Marchi, Justin
Carpentier

► To cite this version:

Etienne Ménager, Antoine Bambade, Wilson Jallet, Alberto de Marchi, Justin Carpentier. Contact-Implicit Inverse Dynamics. 2025. hal-05201780v2

HAL Id: hal-05201780

https://hal.science/hal-05201780v2

Preprint submitted on 21 Sep 2025

**HAL** is a multi-disciplinary open access
archive for the deposit and dissemination of sci-
entific research documents, whether they are pub-
lished or not. The documents may come from
teaching and research institutions in France or
abroad, or from public or private research centers.

L'archive ouverte pluridisciplinaire HAL, est
destinée au dépôt et à la diffusion de documents
scientifiques de niveau recherche, publiés ou non,
émanant des établissements d'enseignement et de
recherche français ou étrangers, des laboratoires
publics ou privés.

<figure><img src="image_1.png" /></figure>

Distributed under a Creative Commons Attribution 4.0 International License
Contact-Implicit Inverse Dynamics

Etienne Ménager<fnref n="1" />, Antoine Bambade<fnref n="2" />, Wilson Jallet<fnref n="1" />, Alberto De Marchi<fnref n="3" /> and Justin Carpentier<fnref n="1" />

<figure><img src="image_2.png" /><figcaption>Fig. 3: **The proposed method generically handles inverse dynamics with frictional contact for both rigid and soft robots.**
**Standing humanoid (left).** The humanoid tracks an unstable reference configuration (standing on its left leg). The only cost terms are the reference tracking cost and regulating the angular momentum around zero. **Deformable robot (right).** The robot controls a ball (in blue) by deforming its body. The task is formulated as minimizing the distance between a reference position and the ball's center, and solved using the robot-ball coupling via frictional contacts.</figcaption></figure>

**Abstract**—Task-space inverse dynamics, also known as opera-
tional space control, is a popular control paradigm for controlling
robots in real-time. It enables the control or stabilization of
robot dynamics around reference trajectories while accounting
for under-actuation, actuator limits, and contact interactions.
Over the past few decades, this versatile control paradigm has
been successfully deployed in numerous robotics settings, ranging
from quadrupeds and humanoid robots to deformable robots, in
scenarios involving rich physical contact interactions between a
robot and its environment. In practice, contact-aware inverse
dynamics controllers assume that contact sequences are known
in advance, typically provided by a higher-level contact planner,
which inherently limits their ability to select among breaking,
sliding, or sticking contacts automatically. In this work, we extend
the control formalism of task-space inverse dynamics, which is
classically formulated as a quadratic program, to a more general
quadratic program with complementarity constraints (QPCC).
This formulation fully accounts for actuator limits and frictional
contacts, modeled as nonlinear complementary constraints. To
solve these QPCC problems, we draw inspiration from the
alternating direction method of multipliers to devise an iterative
optimization approach that alternates between minimizing a
smooth convex function that accounts for task objectives and
system dynamics, and projecting over convex and non-convex
sets that capture actuator and complementary frictional con-
tact constraints. By notably handling complementary frictional
contact constraints through projection, our approach enables
us to implicitly and automatically reason about the optimal
contact modes that fulfill the task objectives and constraints.
We have implemented our QPCC solver in C++ for efficiency,

and demonstrate its usability and versatility on rigid and soft
robots across various control scenarios, ranging from the control
of actuated box sliding on the grounds, to control balance of
legged robots that automatically break and create contacts (e.g.,
jumping tasks, balancing tasks) or control of deformable robots
interacting with their environment.

***Index Terms***—Numerical optimization, Robot control, Contact dynamics, Soft robots

I. INTRODUCTION

Contact modeling plays a crucial role for simulation and
control of robotic systems that create and break contacts
with the environment in order to move or manipulate objects.
Whether in the context of rigid-body robotics or soft robotics,
previous works have emphasized the importance of accurately
capturing contact interactions, particularly in scenarios involv-
ing friction and sliding [1], [2]. These interactions are crucial
not only for realistic simulations but also for enabling robust
control in tasks such as manipulation and locomotion.

Yet, achieving effective control in contact-rich environments
remains a challenge, due to the inherently nonlinear and non-
convex nature of realistic contact models. Both rigid-body
models and deformable-body models – such as those based
on the finite element method (FEM) – are subject to this
complexity, often necessitating advanced numerical methods
to ensure stability and convergence.

Interestingly, despite their structural and physical differ-
ences, rigid and soft robotic systems share an underlying
structure in the mathematical expression of contact constraints.
In both domains, contact dynamics can be formulated as a

<footnote n="1">¹Inria and Département d’Informatique de l’École Normale Supérieure,
PSL Research University in Paris, 75013 Paris, France.</footnote>

<footnote n="2">²EDF Lab, 7 boulevard Gaspard Monge, 91120 Palaiseau, France.</footnote>

<footnote n="3">³University of the Bundeswehr Munich, 85577 Neubiberg, Germany.
e-mails: etienne.menager@inria.fr, wilson.jallet@inria.fr, bam-
bade.antoine@gmail.com, alberto.demarchi@unibw.de, justin.carpentier@inria.fr</footnote>
linear complementarity problem (LCP) or a nonlinear complementarity problem (NCP), in which the Delassus matrix couples contact point velocities to contact forces. This observation enables the development of a unified mathematical framework for both domains.

Leveraging this insight, we show in this work that inverse dynamics problems in both rigid- and soft-body robotics can be formulated as quadratic programs with complementarity constraints (QPCC), which belong to the general class of mathematical programs with complementarity constraints (MPCC) of the form

$$ 
\begin{array}{ll}
\underset{z}{\operatorname{minimize}} & f(z), \\
\text { subject to } & g(z)=0, \quad h(z) \geq 0, \\
& 0 \leq G(z) \perp H(z) \geq 0,
\end{array}
\qquad (1) 
$$

with decision variable $z \in \mathbb{R}^n$. The complementarity condition $0 \le G(z) \perp H(z) \ge 0$ is a more involved constraint than the standard nonlinear inequality $h(z) \ge 0$. It means that both $G(z)$ and $H(z)$ should be nonnegative (component-wise), but they cannot be nonzero at the same time, meaning that $G(z)^\top H(z) = 0$ every time, which is, by essence, a non-convex constraint.

Such complementarity constraint can be further generalized to cone complementarity constraints of the form $\mathcal{K} \ni G(z) \perp H(z) \in \mathcal{K}^*$, where the zero complementarity should be understood in the sense of the duality relationship between the quantities $G(z)$ and $H(z)$ that respectively belong to the (more) generic cone $\mathcal{K}$ and its dual $\mathcal{K}^*$. This formulation naturally accommodates frictional and sliding contact phenomena [3] within a unified optimization framework applicable to both rigid [2] and deformable [4] robotic systems. In the context of frictional contacts, $\mathcal{K}$ can represent the so-called Coulomb friction cone.

From a purely mathematical optimization point of view, MPCCs are particularly difficult to solve due to the nonconvexity of the feasible set, which fails standard constraint qualifications at every feasible point [5]. A widespread strategy is to relax the complementarity constraint and solve a sequence of nonlinear programs (NLPs) that better approximate the original MPCC. Such homotopy (or continuation) can take the form of a penalty term in the objective [6], [7] or an enlarged feasible set [8], [9]. Even special instances of MPCC such as linear and (convex) quadratic programs with complementarity constraints (LPCC and QPCC) pose numerical challenges. Although mixed-integer linear and quadratic solvers can address these problems and even find global solutions, they do not scale well with the number of integer variables. Forgoing the guarantee of global optimality, the penalty homotopy of [10] follows a sequential convex programming approach and exploits the structure of QPCC, reusing matrix factorizations. More details and references on theory and algorithms for MPCC can be found in the survey [11].

**Contributions.** In this work, we introduce a unified contact-implicit inverse dynamics formulation that applies to both rigid and soft robots by explicitly handling complementary

frictional constraints for operational-space control. More precisely, we show that this formulation can be cast as a QPCC problem, where the objective function is a quadratic cost encapsulating the task, while frictional contacts and robot dynamics are expressed as complementarity constraints. Particularly, we introduce a tailored ADMM-based algorithm to efficiently and accurately solve this problem, and we demonstrate that our method supports accurate control in the presence of complex contact interactions. We also discuss the convergence properties of the proposed algorithm and outline future research to strengthen the overall theoretical guarantees of the proposed approach.

**Paper organization.** As the paper tends to unify task-space inverse dynamics with frictional contacts for both rigid and soft robotics, we begin by introducing the problem separately in each domain. Section II-C and Section II-D describe the inverse dynamics formulation in rigid and soft robotics, respectively. These sections reflect the conventions and modeling approaches specific to each community, and can be read independently depending on the reader's background. In Section III, we propose a unified modeling formulation for contact-implicit inverse-dynamics formulated as QPCC instance with harmonized notations and introduce the optimization procedure for solving the underlying QPCC problems. The performance and versatility of the proposed approach are assessed in Section IV, followed by a discussion of its scope and limitations in Section V.

## LIST OF SYMBOLS

Throughout this article, we use various symbols to represent physical and optimization quantities which are summarized hereafter. We use bold symbols to represent vectors (e.g., $\mathbf{x}$) and capital for matrices (e.g., $\mathbf{X}$).

### General symbols

$[x_T, x_N]$ Tangential and normal components of the vector $\mathbf{x}$.

$\mu$ friction coefficient.

$\mathcal{K}_\mu \triangleq \{x \in \mathbb{R}^3 \mid \|x_T\|_2 \le \mu x_N\}$ Second-order Coulomb friction cone.

$\mathcal{K}_\mu^*$ Dual of the Coulomb friction cone.

$\Gamma_\mu(\mathbf{x}) \triangleq [0, 0, \mu \|x_T\|_2]^T$ De Saxcé correction.

$h$ Integration time step.

### Rigid-body dynamics

$\mathbf{q}$ Robot configuration.

$\mathbf{v}$ Robot generalized velocities.

$\mathcal{Q}$ Robot configuration space.

$\tau$ Motor torques.

$\mathcal{M}(\mathbf{q})$ Joint-space inertia matrix (JSIM).

$\dot{\mathbf{v}}$ Robot generalized accelerations.

$\mathbf{f}_c$ Contact force.

$\mathcal{J}_a(\mathbf{q}), \mathcal{J}_c(\mathbf{q})$ Actuation and contact Jacobian matrices.

$\mathbf{b}(\mathbf{q}, \mathbf{v})$ Non-linear terms (gravity, Coriolis, centrifugal forces).

$\lambda_c$ Contact impulse.

$\mathbf{v}_{\text{fr}}$ Contact- and actuation-free generalized velocities.

### Soft-body dynamics
$\lambda_e, \lambda_a$ Contact or actuation forces.

$\sigma_c$ Contact point velocity.

$A$ Impedance matrix defined as $A = M - hD - h^2K$ a combination of the mass matrix $M$, the damping matrix $D$, and the stiffness matrix $K$.

$H_i$ ($i \in [e, a, c]$) Jacobian matrix of the effectors/actuators/contacts.

$W_{ij} = H_i A^{-1} H_j^T$ Admittance matrix in the effector/actuator/contact space.

$\delta_e^{\text{free}}$ Free position of the effectors.

$\dot{\mathbf{X}}_{e, \text{goal}}$ Target positions for the effectors.

$v_{\text{free}}$ Free velocity of the contact points.

$[\lambda_{a, \text{min}}, \lambda_{a, \text{max}}]$ Actuation bounds.

## II. BACKGROUND AND PROBLEM STATEMENT

Contact problems are typically formulated as linear complementarity problems or nonlinear complementarity problems, depending on the nature of the contact model. Such formulations are central to both rigid-body and soft-body simulations and have led to a wide variety of solution methods [2], [12]. However, the inverse dynamics paradigm introduces additional constraints and modeling variations, resulting in more complex optimization problems. In this section, we first review how contact is typically handled in simulation before detailing its classical treatment in rigid and soft robotics inverse dynamics.

### A. Frictional contact modeling

Frictional contact is usually modeled through three core principles:

(i) The Signorini condition provides a complementarity constraint $0 \le \lambda_N \perp \sigma_N \ge 0$, ensuring the normal force is repulsive, bodies do not interpenetrate, and no simultaneous separation motion and contact force exist. Signorini condition alone can be handled as an LCP;

(ii) The Coulomb friction law, characterized by the so-called friction coefficient $\mu$, which states that the tangential force component is bounded by the normal force value $\|\lambda_T\|_2 \le \mu\lambda_N$;

(iii) The maximum dissipation principle states that the tangential force maximizes the power dissipated by the contact, namely $\lambda_T \in \arg\max_{y: \|y\|_2 \le \mu\lambda_N} -y^\top \sigma_T$.

These three principles are equivalent to the following NCP formula:

$$K_{\mu} \ni \lambda \perp \sigma + \Gamma_{\mu}(\sigma) \in K_{\mu}^{*}, \quad \sigma = G\lambda + g, \quad (2)$$

where $G$ is the so-called Delassus matrix [13] that gives the system inertia projected on the contacts, linking contact forces to contact point velocities, $K_\mu$ is the friction cone and $K_\mu^*$ its dual cone, and $\Gamma_\mu(\cdot)$ the De Saxcé correction [14]. Physically speaking, the De Saxcé correction enforces the Signorini condition. Without the De Saxcé term, the complementarity condition becomes a conic complementarity problem (CCP), which has the physical effect of enabling normal motions in the presence of sliding contact points, which is physically incorrect, prohibited by the Signorini condition. Using a pyramidal approximation of the cones turns this NCP into an LCP; both are special cases of the MPCC stated in (1).

<figure><img src="image_3.png" /><figcaption>Fig. 4: Geometric interpretation of the three frictional contact modes. Each plot shows the friction cone $K_\mu$ (green), its dual $K_\mu^*$ (cyan), and the contact plane (grey). (a) Sticking: the relative velocity is zero ($\sigma = 0$) and the force $\lambda$ remains strictly inside $K_\mu$. (b) Breaking: the contact force vanishes ($\lambda = 0$) while the corrected relative velocity $\sigma + \Gamma_\mu(\sigma)$ lies strictly inside $K_\mu^*$. (c) Sliding: the normal velocity is zero ($\sigma_N = 0$); the tangential velocity $\sigma_T \ne 0$ selects, through the maximum-dissipation principle, a boundary force $\lambda_T = -\mu\lambda_N\sigma_T/\|\sigma_T\|_2$. The De Saxcé correction $\Gamma_\mu(\sigma)$ (violet) guarantees complementarity between the two cones in every case.</figcaption></figure>

A comprehensive review of frictional contact modeling is available in [2], [3], [12].

The optimality conditions that arise from the contact complementarity problem (2) naturally partition the solution space into three mutually exclusive contact regimes. Each regime, illustrated in Fig. 4, represents an “optimal” way for the system to satisfy both non-penetration and Coulomb friction:

* **Breaking:** $\lambda = 0$ and $\sigma \in \text{int} K_\mu^*$. In this regime, the two bodies lose contact entirely. Physically, there is no normal reaction and therefore no tangential friction can develop.

* **Sticking:** $\sigma = 0$ and $\lambda \in \text{int} K_\mu$. Here, the contact point remains locked, preventing even the slightest microscopic slip. Consequently, the relative velocity vanishes while the reaction force stays strictly inside Coulomb's cone.

* **Sliding:** $\sigma_N = 0$ and $\lambda_T = -\mu\lambda_N \frac{\sigma_T}{\|\sigma_T\|_2}$. In this scenario, the contact still resists penetration, but the tangential friction reaches its limit. The tangential reaction is opposite to the direction of motion.

### B. Simulation of frictional contacts

Contact is a key component of physics-based simulation engines for both rigid and soft robotics. Contact problems are commonly formulated either as LCPs or NCPs. These problems are solved using different numerical methods.

The projected Gauss-Seidel (PGS) algorithm is widely used in contact simulation for both LCP and NCP formulations. PGS solves complementarity conditions via successive projections. This strategy is adopted in several physics engines such as SOFA [15], [16], Bullet [17], XDE [18], and PhysX [19], where PGS serves as a general-purpose and computationally efficient method. However, PGS suffers from several known
limitations. Notably, it may produce non-physical internal forces, and its convergence is highly sensitive to the conditioning of the problem.

More advanced strategies, such as the Incremental Potential Contact (IPC) model [20], [21], have been developed to enhance robustness and prevent interpenetration, especially in soft-body simulations. While this approach improves stability, it relies on a simplified version of contact using smooth potentials and regularized friction, which may not fully capture the exact physical behavior in complex interactions.

Other numerical techniques have been proposed, including nonsmooth Newton methods [22], interior-point methods (IPM) [23], and alternating direction method of multipliers (ADMM) [4], [24]. The Drake framework [25] and MuJoCo exploits dedicated Newton solver for handling multibody contact with friction modeled as convex complementarity constraints, whereas Dojo solves the frictional contact problem with interior-point techniques.

These methods form the backbone of forward simulation for contact-rich scenarios. However, when shifting to inverse dynamics, new modeling and algorithmic challenges arise. In the remainder of this section, we review these differences in the context of rigid and soft robotics.

As a preamble to the following two subsections that formalize the problem of frictional inverse dynamics for rigid and soft systems, it is essential to recall that the rigid and soft robotics communities do not share the same language, problem formulation, and notation for depicting similar control paradigms. To facilitate the reading and dissemination of this work in both communities, we have endeavored to adhere to the convention of each community as closely as possible.

### C. Contact-aware inverse dynamics for poly-articulated rigid robots

In the rigid-body dynamics case, the robot's state is described by the pair $(q, v)$ where $q \in \mathcal{Q}$ is the robot configuration (and $\mathcal{Q}$ the robot configuration space<fnref n="1" />) and $v \in \mathcal{Q} \triangleq \mathcal{T}_e \mathcal{Q} \cong \mathbb{R}^{n_v}$ are<fnref n="2" /> the generalized velocities. The robot control inputs are the motor torques, denoted $\tau \in \mathbb{R}^{n_u}$.

Prior work on simulation [24], [26] has investigated the inverse dynamics problem (in joint space) under frictional contact via impulse formulations, with efficient algorithmic derivations to solve the resulting feasibility problem (finding the joint torque $\tau$ and frictional impulses $\lambda_c$ which produce given feasible contact point velocities).

Here, we aim to extend this approach to task-space inverse dynamics, also known as operational-space control, in the presence of complementary frictional contact constraints. This accounts for many problems in the robotics literature, such as driving the robot system's end-effector to an arbitrary task-space velocity or performing position-level tracking, while subject to dynamical constraints that include friction and underactuation. While state-of-the-art contact-aware inverse

dynamics frameworks for instantaneous control assume pre-defined contact sequences (e.g., those given by a contact planner) [27]–[29], the approach developed in this work provides a contact-implicit formulation, enabling automatic reasoning about the contact state. However, recent works on model-predictive control (MPC) have focused on just this aspect: contact-implicit formulations (somewhat paradoxically, as task-space inverse dynamics can be seen as MPC with a horizon of just one step). These works handle the problem with either gradient-based or splitting methods.

→ Gradient-based methods. Prior work by Todorov [30] defined a notion of goal-directed dynamics, which can be understood as a “forward” version of task-space inverse dynamics, and that the author applies to trajectory optimization (TO). The idea is to compute generalized accelerations and joint torques by minimizing a cost function, with torques given by MuJoCo's smooth, differentiable approximation of inverse dynamics $\tau = \mathrm{ID}(q, v, \dot{v})$ [26]. In a similar vein, [31], [32], [33] also rely on differentiating through smooth approximations of dynamics<fnref n="3" />. Although these methods are fully contact-implicit and require neither pre-planned contact sequences nor knowing the contact modes in advance, they rely on sets of approximations and relaxations of the frictional contact dynamics which might lead to non-physical effects and a substantial reality gap [2].

→ Splitting methods. Our algorithm (detailed in the next section) is part of splitting methods (albeit for instantaneous control), a class of methods which decomposes complex optimization problems into simpler subproblems. In the MPC literature, a closely related method is consensus complementarity control (C3) [34], which applies to MPC problems with LCP dynamical constraints and linearized friction cones. The splitting in C3 includes a QP step to account for LCP constraints (resembling the problem we solve in this paper), for which the authors propose multiple approximate resolution techniques (one of them akin to relaxing complementarity). In contrast, our method solves the problem with NCP constraints, accounting for the full frictional dynamics without any approximation.

**Continuous-time multicontact dynamics.** The positions of contact points will be denoted $c(q)$ where $p_c: \mathcal{Q} \to \mathbb{R}^{3n_c}$, with $n_c$ the number of contact points.

In the context of systems composed of multiple rigid bodies coupled by joints, Lagrangian dynamics is the standard approach for describing their equations of motion, expressed in their generalized coordinates. The Lagrangian dynamics reads

$$M(q)\dot{v} + b(q, v) = J_a^\top \tau + J_c^\top f_c, \quad (3)$$

where the $f_c$ are instantaneous contact forces, $M(q)$ is the joint-space inertia matrix (JSIM), $J_c = \partial c/\partial q$ is the contact Jacobian, and $J_a \in \mathbb{R}^{n_u \times n_v}$ is the actuation Jacobian.

Accounting for unilateral contact, the Signorini condition is

$$0 \le (f_c)_N \perp c_N(q) \ge 0, \quad (4)$$

<footnote n="1">¹For a floating-base system, (e.g., a legged robot), the configuration space is typically of the form $\mathcal{Q} = \mathrm{SE}(3) \times \mathbb{R}^{n_f}$ with $n_f$ the number of joints.</footnote>

<footnote n="2">²Here $e$ denotes the neutral element of the group $\mathcal{Q}$.</footnote>

<footnote n="3">³The former uses a linearized friction constraint in contrast to the conic (CCP) formulations of Todorov.</footnote>
which leads to an LCP condition in the dynamics. Or, after index reduction (differentiating $c_N$ twice with respect to time)

$$0 \le (\mathbf{f}_c)_N \perp a_c(\mathbf{q}, \mathbf{v}, \dot{\mathbf{v}})_N \ge 0, \quad (5)$$

where

$$\mathbf{a}_c(\mathbf{q}, \mathbf{v}, \dot{\mathbf{v}}) = \ddot{c} = J_c \dot{\mathbf{v}} + \underbrace{\dot{J}_c \mathbf{v}}_{\gamma_c(\mathbf{q}, \mathbf{v})}, \quad (6)$$

is the contact point acceleration. In theory, this index reduction assumes that $\mathbf{c} = \mathbf{0}$ and $\dot{\mathbf{c}} = \mathbf{0}$, which is rarely the case in practice due to, for instance, numerical integration errors. In the robotics literature [2], [3], [24], this is accounted for by augmenting $\gamma_c$ with additional terms, such as Baumgarte stabilization [35].

**Impulse-based dynamics.** Let $h > 0$ be a discrete time step. Following [24], the discretized system dynamics can be written using an *impulse* formulation [36], replacing joint acceleration with joint velocities and contact forces $\mathbf{f}_c$ with contact impulses $\boldsymbol{\lambda}_c = h\mathbf{f}_c$ (e.g., their sum over the integration time step).

Using a semi-implicit Euler discretization, the velocity time-stepping scheme is given by

$$\mathbf{v}^{+} = \mathbf{v} + hM^{-1}(J_{a}^{\top}\boldsymbol{\tau} + J_{c}^{\top}\mathbf{f}_{c} - \mathbf{b}(\mathbf{q}, \mathbf{v})). \quad (7)$$

The robot configuration is then updated using $\mathbf{v}^+$. Introducing the “free” (as in contact- and actuation-free) generalized velocity

$$\mathbf{v}_{\text{fr}} = \mathbf{v} - hM^{-1}\mathbf{b}(\mathbf{q}, \mathbf{v}),$$

the discrete dynamics can be rewritten again as the affine map

$$\mathbf{v}^{+} = \mathcal{F}(\boldsymbol{\tau}, \boldsymbol{\lambda}_{c}) \triangleq \mathbf{v}_{\text{fr}} + M^{-1}(hJ_{a}^{\top}\boldsymbol{\tau} + J_{c}^{\top}\boldsymbol{\lambda}_{c}). \quad (8)$$

Under the sole unilateral contact hypothesis, the dynamics are also subject to an LCP condition in the normal direction

$$0 \le (\boldsymbol{\lambda}_c)_N \perp (J_c \mathbf{v}^+ + \gamma_c)_N \ge 0, \quad (9)$$

where $\boldsymbol{\sigma}_c = J_c \mathbf{v}^+ + \gamma_c$ is the contact-point velocity, and $\gamma_c$ is equal to $c(\mathbf{q})$ plus the corrective terms mentioned in the previous paragraph. With these terms (see [2, Sec. II])

$$\gamma_c = c(\mathbf{q}) - K_p \left[ \frac{c(\mathbf{q})}{h} \right]_{-} - K_d J_c \mathbf{v}. \quad (10)$$

Accounting for frictional effects, the dynamics instead follow an NCP condition

$$\mathcal{K}_{\mu} \ni \boldsymbol{\lambda}_{c} \perp \boldsymbol{\sigma}_{c} + \Gamma(\boldsymbol{\sigma}_{c}) \in \mathcal{K}_{\mu}^{*}, \quad (11)$$

where $\Gamma(\cdot)$ is the De Saxcé corrective term.

**Problem statement.** Let $p_{ee}: \mathcal{Q} \to \mathcal{E}$ be the mapping from robot configuration $\mathbf{q}$ to desired end-effector positions or poses (e.g., a Cartesian product of $\mathbb{R}^3$ or $\text{SE}(3)$).

For convenience (to incorporate contact forces), we propose an impulse formulation for task-space inverse dynamics, leading to the following MPCC

$$\underset{\boldsymbol{\tau}, \boldsymbol{\lambda}_c, \mathbf{v}^+}{\text{minimize}} \quad \frac{1}{2} \|J_{ee}\mathbf{v}^+ + \mathbf{b}_{ee}\|_2^2, \quad (12a)$$

$$\text{subject to} \quad \mathbf{v}^+ = \mathcal{F}(\boldsymbol{\tau}, \boldsymbol{\lambda}_c), \quad (12b)$$

$$\mathcal{K}_{\mu} \ni \boldsymbol{\lambda}_{c} \perp \boldsymbol{\sigma}_{c} + \Gamma(\boldsymbol{\sigma}_{c}) \in \mathcal{K}_{\mu}^{*}, \quad (12c)$$

$$\boldsymbol{\sigma}_{c} = J_{c} \mathbf{v}^{+}, \quad (12d)$$

$$\boldsymbol{\tau} \in \mathbb{B}, \quad (12e)$$

with motor torque limits $\mathbb{B} = [\tau_{\min}, \tau_{\max}]$, and drift term $\mathbf{b}_{ee}$. In the classic framework of task-space inverse dynamics [28], [29], [37], as contact modes are assumed to be known in advance, it is worth mentioning that Eq. (12c) reduces to the two following simpler constraints: $\boldsymbol{\lambda}_c \in \mathcal{K}_\mu$ and $\boldsymbol{\sigma}_c = \mathbf{0}$. In other words, the contact forces should lie in the Coulomb friction cone while contact points should not move.

In the following paragraph, we show that problem (12) is analogous to generic problem (21) and falls within the general QPCC framework introduced in this work.

**Standard form of the problem.** We can express the problem in terms of the contact impulses $\boldsymbol{\lambda}_c$ and control input $\boldsymbol{\tau}$ only. The next velocity $\mathbf{v}^+$ is an affine function of $(\boldsymbol{\tau}, \boldsymbol{\lambda}_c)$. Following (8), the contact-point velocity is given by

$$\boldsymbol{\sigma}_{c} = F\boldsymbol{\tau} + G\boldsymbol{\lambda}_{c} + \mathbf{g}, \quad (13)$$

where $G = J_c M^{-1} J_c^\top$ is<fnref n="4" /> the Delassus or *operational-space inertia matrix* (OSIM), $F = h J_c M^{-1} J_a^\top$ is the Jacobian of the contact-point velocity with respect to control torque input, and

$$\mathbf{g} = J_c \mathbf{v}_{\text{fr}} + \gamma_c$$

is the contact-point “free” velocity. The task-space velocity is similarly rewritten

$$J_{ee} \mathbf{v}^{+} = F_{ee} \boldsymbol{\tau} + G_{ee} \boldsymbol{\lambda}_{c} + \mathbf{g}_{ee}, \quad (14)$$

where the matrices have the same expressions as above, with the leftmost $J_c$ matrix replaced by $J_{ee}$

$$F_{ee} = h J_{ee} M^{-1} J_{a}^{\top}, \quad G_{ee} = J_{ee} M^{-1} J_{c}^{\top},$$

and $g_{ee} = J_{ee} \mathbf{v}_{\text{fr}}$. The difference between the expressions used in our approach and the ones from [24] is that we make the dependence on the input torque $\boldsymbol{\tau}$ explicit, since $\boldsymbol{\tau}$ is a decision variable of the task-space inverse dynamics problem (whereas it is not a decision variable in the context of direct simulation). In this case, “free” velocities refer to the contact-and actuation-free velocities.

The resulting MPCC has a form analogous to (21) and reads

$$\underset{\boldsymbol{\tau}, \boldsymbol{\lambda}_c}{\text{minimize}} \quad \frac{1}{2} \|F_{ee}\boldsymbol{\tau} + G_{ee}\boldsymbol{\lambda}_c + \mathbf{g}_{ee} + \mathbf{b}_{ee}\|_2^2, \quad (15a)$$

$$\text{subject to} \quad \mathcal{K}_{\mu} \ni \boldsymbol{\lambda}_{c} \perp \boldsymbol{\sigma}_{c} + \Gamma(\boldsymbol{\sigma}_{c}) \in \mathcal{K}_{\mu}^{*}, \quad (15b)$$

$$\boldsymbol{\sigma}_{c} = F\boldsymbol{\tau} + G\boldsymbol{\lambda}_{c} + \mathbf{g}, \quad (15c)$$

$$\boldsymbol{\tau} \in \mathbb{B}, \quad (15d)$$

<footnote n="4">⁴Scaled by the time step $h$.</footnote>
which fits the general form of optimization problems introduced and solved in this work.

### D. Contact-aware inverse dynamics for soft-body systems

In soft robotics, while frictional contact models have been extensively studied in direct simulation, relatively few works incorporate accurate contact models directly into inverse modelling. In [38], [39], the authors employ penalty-based contact models. Other simplified models, such as variations of Piecewise Constant Curvature (PCC) [40], enable the control of the dynamics of soft robots interacting with their environment under strong assumptions regarding geometry and contact modeling.

Unlike prior work on devising optimization-based inverse dynamics for soft robots, as proposed, for instance, in [41], which only enforces the Signorini constraint while ignoring frictional effects, we aim to formulate an inverse-dynamics problem that also natively accounts for Coulomb friction and maximum dissipation effects. In the remainder of this section, we begin by revisiting the standard FEM dynamics, then review how inverse dynamics is traditionally treated in the soft-robot FEM literature, and finally extend this framework by incorporating friction cones into the complementarity system.

### FEM modelling of soft robots.
The behavior of soft robots can be described using the mechanical equations of continuous media, which can be discretized using the finite element method (FEM) [15], [42], [43] or the material point method (MPM) [44], [45]. In this article, we focus on FEM-based approaches where the system state is given by the pair [(q, ḋ) ∈ ℝ$^{n_q}$ × ℝ$^{n_q}$, n_q = 3n_n], in which q stacks the Cartesian positions of the n_n nodes composing the mesh, and ḋ their velocities. FEM simulation software is used to obtain an approximate solution of these equations. In general, these equations can be written as

$$M\ddot{q}_t = f_{\text{int}}(q_t, \dot{q}_t) + f_{\text{ext}} + H_a^\top \lambda_a + H_c^\top \lambda_c, \quad (16)$$

where M is the mass matrix, $f_{\text{int}}$ the internal forces, $f_{\text{ext}}$ the external forces, $\ddot{q}_t$, $\dot{q}_t$, $q_t$ are respectively the acceleration, the velocity and position of the nodes of the FEM mesh, $H_a$ can be seen as the Jacobian matrix of the actuation and depends on the type of the actuator, $H_c$ is the Jacobian matrix of the contacts, $\lambda_a$ and $\lambda_c$ are the Lagrange multipliers which represent the actuation and contact forces.

The dynamics equation is discretized in time using an integration scheme. In soft robotics, an implicit Euler integration scheme is generally used because of its unconditional stability and its capacity to adapt to the "time-stepping" used in non-regular mechanics to deal with speed jumps on contact. In addition, internal forces are linearized using a first-order approximation. Noting h the time step, $q_k = q_{kh}$ and $\dot{q}_k = \dot{q}_{kh}$ the discretized position and velocity, and

$d\dot{q} \triangleq h\ddot{q} = \dot{q}_{k+1} - \dot{q}_k$, the discretized dynamics is then written

$$A = M - hD - h^2 K, \quad (17a)$$

$$\mathbf{b}(\mathbf{q}_k, \dot{\mathbf{q}}_k) = h\mathbf{f}_{\text{int}}(\mathbf{q}_k, \dot{\mathbf{q}}_k) + h^2 K \dot{\mathbf{q}}_k + h \mathbf{f}_{\text{ext}}, \quad (17b)$$

$$Ad\dot{\mathbf{q}} = \mathbf{b}(\mathbf{q}_k, \dot{\mathbf{q}}_k) + hH_a^T \lambda_a + hH_c^T \lambda_c, \quad (17c)$$

where K and D are respectively the compliance and the damping matrices.

To simplify the handling of actuation, contact, and taskspace objectives, it is common in the FEM literature to project the full space dynamics (17) onto lower-dimensional constraint subspaces [46]. To each actuation (a), contact (c), or end-effector task (e), we can associate a Jacobian $H_j$. This projection leads to a set of linear relations between the displacements $\delta_j$ and the forces $\lambda_k$ of the form

$$\delta_j = \sum_{k \in \{a,e,c\}} W_{jk} \lambda_k + \delta_j^{\text{free}}, \quad (18)$$

where $\delta_j^{\text{free}}$ denotes the displacement that would occur in the absence of applied forces, and $W_{jk} = H_j A^{-1} H_k^\top$ is a coupling matrix that captures how forces on k influence motion on j. These matrices naturally arise in the FEM formulation and will serve as the fundamental building blocks for the inverse-dynamics problem developed in the next section.

### Inverse dynamics with linear complementarity constraints.

The inverse dynamics problem in FEM modeling is formulated as a quadratic optimization problem with (linear) complementarity constraints [41]. When controlling the position of a robot's end-effector (i.e, minimizing the distance between an effector and a goal), the problem can be written as

$$\underset{\boldsymbol{\lambda}_a, \boldsymbol{\lambda}_c}{\text{minimize}} \quad \frac{1}{2} \left\| W_{ea} \boldsymbol{\lambda}_a + W_{ec} \boldsymbol{\lambda}_c + \boldsymbol{\delta}_e^{\text{free}} - \mathbf{X}_{e, \text{goal}} \right\|_2^2, \quad (19a)$$

$$\text{subject to} \quad 0 \le \boldsymbol{\lambda}_c \perp \boldsymbol{\sigma}_c \ge 0, \quad (19b)$$

$$\boldsymbol{\sigma}_c = W_{ca}\boldsymbol{\lambda}_a + W_{cc}\boldsymbol{\lambda}_c + \boldsymbol{v}_{\text{fr}}, \quad (19c)$$

$$\boldsymbol{\lambda}_a \in [\boldsymbol{\lambda}_{a, \text{min}}, \boldsymbol{\lambda}_{a, \text{max}}], \quad (19d)$$

with $X_{e,goal}$ the goal position, $\sigma_c$ the velocity of the contact points, $v_{free}$ the free velocity of the contact points and $[\lambda_{a,min}, \lambda_{a,max}]$ the actuation bounds.

In this problem, Eq. (19b) accounts for the Signorini law. This complementarity and positivity constraint forces either $\lambda_{c,i}$ or $\sigma_{c,i}$, the i-th component of the corresponding vectors, to be zero.

In [41], problem (19) is solved using an active-set strategy. More precisely, let the sets $I = \{i : \lambda_{c,i} = 0\}$ and $\bar{I} = \{i : \lambda_{c,i} \neq 0\}$, and let $S_I$ and $S_{\bar{I}}$ be the corresponding selection matrices. The complementarity constraint can then be rewritten as

$$S_I \boldsymbol{\lambda}_c = 0, \quad S_{\bar{I}} \boldsymbol{\sigma}_c = 0, \quad S_{\bar{I}} \boldsymbol{\lambda}_c \ge 0, \quad S_I \boldsymbol{\sigma}_c \ge 0, \quad (20)$$

thus transforming the LCP problem into a QP problem, as leveraged in former work for unilateral contact simulation [47]. The active set is then updated whenever one of the inequality constraints reaches its bound. However, this heuristic does not guarantee convergence in the non-convex
case, as cycling between active sets is possible, and contact modes are discovered through active set modifications that occur externally to the optimization process. Coevoet et al. [48] extends the active-set strategy to account for Coulomb friction, but only in the sticking regime (excluding sliding). Similarly to the previous work, friction limitations are handled in the outer loop and not considered in the internal QP problems. Consequently, frictional contact modes (sliding, breaking, sticking) remain difficult to discover within the algorithm iterations automatically.

**Inverse dynamics with nonlinear complementarity constraints.** In the following, we propose a formulation that unifies both linear and nonlinear complementarity conditions into a single optimization-based inverse dynamics framework. Rather than relying on heuristic mode detection or active-set strategies, contact interactions are directly embedded into the optimization via nonlinear complementarity constraints. In particular, we express Signorini law, Coulomb friction, and the maximum dissipation principle using NCP formulation, as formulated in (2). This leads to a general formulation of the form

$$ \underset{\boldsymbol{\lambda}_a, \boldsymbol{\lambda}_c, \boldsymbol{\sigma}_c}{\text{minimize}} \quad \frac{1}{2} \left\| W_{ea} \boldsymbol{\lambda}_a + W_{ec} \boldsymbol{\lambda}_c + \boldsymbol{\delta}_e^{\text{free}} - \boldsymbol{X}_{e, \text{goal}} \right\|_2^2, \qquad (21a) $$

$$ \text{subject to} \quad \mathcal{K}_{\mu} \ni \boldsymbol{\lambda}_c \perp \boldsymbol{\sigma}_c + \Gamma(\boldsymbol{\sigma}_c) \in \mathcal{K}_{\mu}^*, \qquad (21b) $$

$$ \boldsymbol{\sigma}_c = W_{ca} \boldsymbol{\lambda}_a + W_{cc} \boldsymbol{\lambda}_c + \boldsymbol{v}_{\text{fr}}, \qquad (21c) $$

$$ \boldsymbol{\lambda}_a \in [\boldsymbol{\lambda}_{a, \text{min}}, \boldsymbol{\lambda}_{a, \text{max}}], \qquad (21d) $$

where $\mathcal{K}_{\mu}$ is the friction cone with friction parameter $\mu$, $\mathcal{K}_{\mu}^*$ is the dual cone, and $\Gamma(\boldsymbol{\sigma}_c)$ is the De Saixé correction term. The resulting QPCC has a form analogous to (15) for rigid robotics, and fits the general form of optimization problems introduced and solved in this work.

### III. CONTACT-IMPLICIT TASK-SPACE INVERSE DYNAMICS

This section presents the core contribution of this work. We first formalize the generic problem for contact-implicit task-space inverse dynamics which accounts for frictional contact models encapsulated as NCP constraints. We then introduce an ADMM-based scheme to solve the proposed formulation. In particular, we derive the projection operator to adequately account for the LCP/NCP constraints within the ADMM-based formulation, and establish the underlying stationary conditions of the QPCC which are then used as stopping criteria. The last part summarizes the overall algorithm and analyzes its convergence properties alongside the required pre- and post-processing stages.

#### A. Unified problem formulation

In the context of task-space inverse dynamics control, the task objective function can be generically written as

$$ f(\boldsymbol{\lambda}_a, \boldsymbol{\lambda}_c) \triangleq \frac{1}{2} \|A\boldsymbol{\lambda}_a + B\boldsymbol{\lambda}_c + \boldsymbol{b}\|_2^2, $$

where $\boldsymbol{\lambda}_a \in \mathbb{R}^{n_a}$, $\boldsymbol{\lambda}_c \in \mathbb{R}^{n_c}$, $A \in \mathbb{R}^{n_e \times n_a}$, $B \in \mathbb{R}^{n_e \times n_c}$, $\boldsymbol{b} \in \mathbb{R}^{n_e}$, where $n_a, n_c$, and $n_e$ denote respectively the number

<table><caption>TABLE I: Definition of coefficient matrices in the soft- and rigid-body settings.</caption><thead><tr><th>Variable/Parameter</th><th>Soft-body</th><th>Rigid-body</th></tr></thead><tbody><tr><td>λ<sub>a</sub></td><td>λ<sub>a</sub></td><td>τ</td></tr><tr><td>λ<sub>c</sub></td><td>λ<sub>c</sub></td><td>λ<sub>c</sub></td></tr><tr><td>A</td><td>W<sub>ea</sub></td><td>F<sub>ee</sub> = hJ<sub>ee</sub>M<sup>-1</sup>J<sub>a</sub><sup>T</sup></td></tr><tr><td>B</td><td>W<sub>ec</sub></td><td>G<sub>ee</sub> = J<sub>ee</sub>M<sup>-1</sup>J<sub>c</sub><sup>T</sup></td></tr><tr><td>b</td><td>δ<sup>free</sup><sub>e</sub> - X<sub>e,goal</sub></td><td>g<sub>ee</sub> + b = J<sub>ee</sub>v<sub>fr</sub> + b</td></tr><tr><td>C</td><td>W<sub>ca</sub></td><td>F = hJ<sub>c</sub>M<sup>-1</sup>J<sub>a</sub><sup>T</sup></td></tr><tr><td>D</td><td>W<sub>cc</sub></td><td>G = J<sub>c</sub>M<sup>-1</sup>J<sub>c</sub><sup>T</sup></td></tr><tr><td>g</td><td>v<sub>fr</sub></td><td>g = J<sub>c</sub>v<sub>fr</sub> + γ<sub>c</sub></td></tr></tbody></table>

of actuators, contact and end-effector dimensions. The A and B matrices respectively relate the input actuation forces $\boldsymbol{\lambda}_a$ and contact forces $\boldsymbol{\lambda}_c$ to the task residual, given by

$$ r(\boldsymbol{\lambda}_a, \boldsymbol{\lambda}_c) \triangleq A\boldsymbol{\lambda}_a + B\boldsymbol{\lambda}_c + \boldsymbol{b}. $$

Let F be the objective f augmented with the equality constraint $\boldsymbol{\sigma}_c = C\boldsymbol{\lambda}_a + D\boldsymbol{\lambda}_c + \boldsymbol{g}$, i.e.,

$$ F(\boldsymbol{\lambda}_a, \boldsymbol{\lambda}_c, \boldsymbol{\sigma}_c) \triangleq f(\boldsymbol{\lambda}_a, \boldsymbol{\lambda}_c) + \mathcal{I}_{\{\boldsymbol{\sigma}_c = C\boldsymbol{\lambda}_a + D\boldsymbol{\lambda}_c + \boldsymbol{g}\}}(\boldsymbol{\lambda}_a, \boldsymbol{\lambda}_c, \boldsymbol{\sigma}_c), $$

where $C \in \mathbb{R}^{n_c \times n_a}$, $D \in \mathbb{R}^{n_c \times n_c}$, $\boldsymbol{g} \in \mathbb{R}^{n_c}$ and $\mathcal{I}_{\{\boldsymbol{\sigma}_c = C\boldsymbol{\lambda}_a + D\boldsymbol{\lambda}_c + \boldsymbol{g}\}}$ denotes the indicator function<fnref n="5" /> for the corresponding equality constraint.

The optimization problem for inverse dynamics (either (15), (19) or (21)) can then be more generally re-formulated as

$$ \underset{\boldsymbol{\lambda}_a, \boldsymbol{\lambda}_c, \boldsymbol{\sigma}_c}{\text{minimize}} \quad F(\boldsymbol{\lambda}_a, \boldsymbol{\lambda}_c, \boldsymbol{\sigma}_c) + \mathcal{I}_{\mathbb{S}}(\boldsymbol{\lambda}_a, \boldsymbol{\lambda}_c, \boldsymbol{\sigma}_c), \qquad (22) $$

where $\mathbb{S} \triangleq \mathbb{B} \times \mathbb{S}_c$, with $\mathbb{B} = [\boldsymbol{\lambda}_{a, \text{min}}, \boldsymbol{\lambda}_{a, \text{max}}]$ the actuation limits and $\mathbb{S}_c$ some nonconvex constraint set. For example, in the case of an LCP, the nonconvex constraint set is

$$ \mathbb{S}_c \triangleq \{(\boldsymbol{\lambda}_c, \boldsymbol{\sigma}_c) | 0 \leq \boldsymbol{\lambda}_c \perp \boldsymbol{\sigma}_c \geq 0\}, $$

and for an NCP, it reads

$$ \mathbb{S}_c \triangleq \{(\boldsymbol{\lambda}_c, \boldsymbol{\sigma}_c) | \mathcal{K}_\mu \ni \boldsymbol{\lambda}_c \perp \boldsymbol{\sigma}_c + \Gamma(\boldsymbol{\sigma}_c) \in \mathcal{K}_\mu^*\}. $$

Finally, the optimization problem (22) can be re-formulated, in splitting form, as follows

$$ \underset{\boldsymbol{\lambda}_a, \boldsymbol{\lambda}_c, \boldsymbol{\sigma}_c, \boldsymbol{\lambda}_a, \boldsymbol{\lambda}_c, \boldsymbol{\sigma}_c}{\text{minimize}} \quad F(\boldsymbol{\lambda}_a, \boldsymbol{\lambda}_c, \boldsymbol{\sigma}_c) + \mathcal{I}_{\mathbb{S}}(\hat{\boldsymbol{\lambda}}_a, \hat{\boldsymbol{\lambda}}_c, \hat{\boldsymbol{\sigma}}_c), \qquad (23) $$

$$ \text{subject to} \quad \boldsymbol{\lambda}_a = \hat{\boldsymbol{\lambda}}_a, \quad \boldsymbol{\lambda}_c = \hat{\boldsymbol{\lambda}}_c, \quad \boldsymbol{\sigma}_c = \hat{\boldsymbol{\sigma}}_c. $$

Interestingly, the standard problem (22) encapsulates task-space inverse dynamics problems with frictional contacts for both rigid and deformable systems. Table I summarizes the notations for both soft- and rigid-body problems using the ones exposed in Section II. The (equivalent) splitting formulation (23) also offers the possibility of leveraging operator-splitting methods (such as ADMM) as the core of our approach, as detailed next.

<footnote n="5">⁵For a nonempty set $S \in \mathbb{R}^n$, the associated indicator $\mathcal{I}_S: \mathbb{R}^n \to \mathbb{R} \cup \{\infty\}$ is defined by $\mathcal{I}_S(x) = 0$ if $x \in S$, and $\mathcal{I}_S(x) = \infty$ otherwise.</footnote>
## B. Reminder on ADMM

To solve the generic problem (23), we leverage ADMM, a first-order splitting method known for its modularity and scalability [49]. ADMM is particularly suited for problems with separable structure, as it decouples objective minimization from constraint enforcement through iterative updates. ADMM can address problems of the form

$$ \underset{\mathbf{x},\mathbf{y}}{\text{minimize}} \quad f(\mathbf{x}) + g(\mathbf{y}) \quad \text{subject to} \quad \mathbf{x} = \mathbf{y}, \qquad (24) $$

where $f$ and $g$ are functions (possibly nonsmooth), and the constraint $\mathbf{x} = \mathbf{y}$ is enforced via a dual variable. By naming $\mathbf{z}$ the dual variable associated with the equality constraint $\mathbf{x} = \mathbf{y}$, the augmented Lagrangian of the optimization problem (24) reads

$$ \mathcal{L}_{\rho}(\mathbf{x}, \mathbf{y}, \mathbf{z}) \triangleq f(\mathbf{x}) + g(\mathbf{y}) + \mathbf{z}^{\top}(\mathbf{x} - \mathbf{y}) + \frac{\rho}{2} \|\mathbf{x} - \mathbf{y}\|_{2}^{2}, $$

where $\rho$ is the augmented Lagrangian penalty term. Solving the saddle-point problem associated with the augmented Lagrangian $\mathcal{L}_{\rho}$ can be efficiently done via ADMM iterates. Let $k$ denote the current iteration index. ADMM loops over three main steps until some optimality convergence criteria are met:

(i) $\mathbf{x}_k = \arg\min_{\mathbf{x}} \mathcal{L}_{\rho}(\mathbf{x}, \mathbf{y}_{k-1}, \mathbf{z}_{k-1})$: minimizing with respect to $\mathbf{x}$,

(ii) $\mathbf{y}_k = \arg\min_{\mathbf{y}} \mathcal{L}_{\rho}(\mathbf{x}_k, \mathbf{y}, \mathbf{z}_{k-1})$: minimizing with respect to $\mathbf{y}$,

(iii) $\mathbf{z}_k = \mathbf{z}_{k-1} + \rho(\mathbf{x}_k - \mathbf{y}_k)$: updating the dual variable estimate (Lagrange multiplier) to drive $\mathbf{x}$ and $\mathbf{y}$ closer.

## C. ADMM with complementarity constraints

In the particular case of contact-implicit task-space inverse dynamics, we propose to choose $f$ to encode the task objective and linear constraints (such as dynamics or consistency relations), and $g$ to capture both actuator limits and contact complementarity constraints. In what follows, we show how to apply ADMM to optimization problems with complementarity constraints, and more specifically for the formulation in (23).

Let $\mathbf{z}_a$, $\mathbf{z}_c$ and $\mathbf{w}_c$ be the three dual variables respectively associated with the consensus constraint $\lambda_a = \hat{\lambda}_a$, $\lambda_c = \hat{\lambda}_c$ and $\sigma_c = \hat{\sigma}_c$ in (23). Then, the augmented Lagrangian for (23) is defined by

$$ \begin{aligned} & \mathcal{L}_{\theta}(\lambda_a, \lambda_c, \sigma_c, \hat{\lambda}_a, \hat{\lambda}_c, \hat{\sigma}_c, z_a, z_c, w_c) \\ & \triangleq F(\lambda_a, \lambda_c, \sigma_c) + \mathcal{I}_{\mathbb{S}}(\hat{\lambda}_a, \hat{\lambda}_c, \hat{\sigma}_c) + \begin{bmatrix} z_a \\ z_c \\ w_c \end{bmatrix}^{\top} \begin{bmatrix} \lambda_a - \hat{\lambda}_a \\ \lambda_c - \hat{\lambda}_c \\ \sigma_c - \hat{\sigma}_c \end{bmatrix} \\ & \quad + \frac{\xi}{2} \|\lambda_a - \hat{\lambda}_a\|_2^2 + \frac{\eta}{2} \|\lambda_c - \hat{\lambda}_c\|_2^2 + \frac{\rho}{2} \|\sigma_c - \hat{\sigma}_c\|_2^2, \quad (25) \end{aligned} $$

where $\theta \triangleq (\xi, \eta, \rho)$ collects the strictly positive penalty parameters associated with each splitting constraint. Importantly, as depicted in the experimental section, choosing distinct values for these penalty parameters yields better performance than using a single common penalty parameter.

Applying ADMM to (23) results in the following iterations:

$$ \begin{bmatrix} \lambda_a^{k+1} \\ \lambda_c^{k+1} \\ \sigma_c^{k+1} \end{bmatrix} \leftarrow \operatorname*{argmin}_{\lambda_a, \lambda_c, \sigma_c} \mathcal{L}_{\theta}(\cdot, \cdot, \cdot, \hat{\lambda}_a^k, \hat{\lambda}_c^k, \hat{\sigma}_c^k, z_a^k, z_c^k, w_c^k) \quad (26) \\ \begin{bmatrix} \hat{\lambda}_a^{k+1} \\ \hat{\lambda}_c^{k+1} \\ \hat{\sigma}_c^{k+1} \end{bmatrix} \leftarrow \operatorname*{argmin}_{\lambda_a, \lambda_c, \sigma_c} \mathcal{L}_{\theta}(\lambda_a^{k+1}, \lambda_c^{k+1}, \sigma_c^{k+1}, \cdot, \cdot, \cdot, z_a^k, z_c^k, w_c^k) \quad (27) $$

$$ \begin{bmatrix} z_a^{k+1} \\ z_c^{k+1} \\ w_c^{k+1} \end{bmatrix} \leftarrow \begin{bmatrix} z_a^k \\ z_c^k \\ w_c^k \end{bmatrix} + \operatorname{diag}(\theta) \begin{bmatrix} \lambda_a^{k+1} - \hat{\lambda}_a^{k+1} \\ \lambda_c^{k+1} - \hat{\lambda}_c^{k+1} \\ \sigma_c^{k+1} - \hat{\sigma}_c^{k+1} \end{bmatrix}. \quad (28) $$

In two following sections, we now provide the explicit forms of the two minimization steps: the minimization of the smooth term exposed in Section III-D and the projection onto the complementarity constraints set derived in Section III-E.

## D. ADMM step 1: minimizing the smooth term (26)

More explicitly, the optimization step (26) is equivalent to

$$ \begin{bmatrix} \lambda_c^{k+1} \\ \lambda_a^{k+1} \\ \sigma_c^{k+1} \end{bmatrix} \in \operatorname*{argmin}_{\lambda_a, \lambda_c, \sigma_c} f(\lambda_a, \lambda_c) + \frac{\xi}{2} \left\| \lambda_a - \hat{\lambda}_a^k + \frac{z_a^k}{\xi} \right\|_2^2 \\ + \frac{\eta}{2} \left\| \lambda_c - \hat{\lambda}_c^k + \frac{z_c^k}{\eta} \right\|_2^2 + \frac{\rho}{2} \left\| \sigma_c - \hat{\sigma}_c^k + \frac{z_c^k}{\rho} \right\|_2^2 \\ \text{subject to} \quad \sigma_c = C\lambda_a + D\lambda_c + g, \quad (29) $$

which is a strictly convex QP in $(\lambda_a, \lambda_c, \sigma_c)$. In particular, it always admits a unique solution. The optimality conditions for (29) result in the linear system

$$ \begin{bmatrix} A^{\top} A + \xi I & A^{\top} B & C^{\top} \\ B^{\top} A & B^{\top} B + \eta I & D^{\top} \\ C & D & -\frac{1}{\rho} I \end{bmatrix} \begin{bmatrix} \lambda_a^{k+1} \\ \lambda_c^{k+1} \\ z^* \end{bmatrix} = \begin{bmatrix} -A^{\top} b + \xi \hat{\lambda}_a^k - z_a^k \\ -B^{\top} b + \eta \hat{\lambda}_c^k - z_c^k \\ -g + \hat{\sigma}_c^k - \frac{1}{\rho} w_c^k \end{bmatrix}, \quad (30) $$

with $z^*$ a dummy variable, and the optimal $\sigma_c^{k+1}$ can be recovered as

$$ \sigma_c^{k+1} = C\lambda_a^{k+1} + D\lambda_c^{k+1} + g. \quad (31) $$

Note that the linear system involves a symmetric quasi-definite matrix, which is strongly factorizable, hence it can be efficiently solved via a LDL$^{\top}$ routine [50, Thm 2].

## E. ADMM step 2: projection onto complementarity constraints (27)

The second optimization step (27) boils down to a projection of

$$ \begin{bmatrix} \tilde{\lambda}_c^k \\ \hat{\sigma}_c^k \end{bmatrix} = \alpha \begin{bmatrix} \lambda_c^{k+1} \\ \sigma_c^{k+1} \end{bmatrix} + (1-\alpha) \begin{bmatrix} \lambda_c^k \\ \sigma_c^k \end{bmatrix} + \begin{bmatrix} z_c^k \\ \frac{\eta}{\rho} w_c^k \end{bmatrix}, \quad (32) $$

over the complementarity constraint $\mathbb{S}_c$ and to a projection of $\alpha\lambda_a^{k+1} + (1-\alpha)\lambda_a^k + \frac{z_a^k}{\xi}$ over the box $\mathbb{B}$.
Complementarity constraints are particularly challenging, as they define nonconvex nonsmooth feasible sets with no direct analytical treatment. However, a key strength of ADMM lies in its ability to handle such constraints through projections. Although in general nontrivial, with formulation (23) the second step of the algorithm requires only applying a projection onto the constraint set, which can be explicitly derived and efficiently computed for the contact complementarity set. In our case, this step is central: we embed the contact complementarity conditions directly into ADMM via closed-form projection operators, enabling implicit contact constraint handling. Next, we recall the standard notion of projection operators and derive the closed-form solution in the presence of LCP and NCP constraints.

**Orthogonal projection onto (non-)convex sets.** Given a convex set $S \subseteq \mathbb{R}^n$ and $x \in \mathbb{R}^n$, the orthogonal projection of $x$ onto $S$ is defined as the minimizer of the Euclidean distance from $x$ to the set

$$x^* = \operatorname{proj}_S(x) \triangleq \operatorname*{argmin}_{z \in S} \|x - z\|_2^2. \quad (33)$$

This minimizer is unique by virtue of $\mathbb{S}$ being convex (and thus the distance function being strictly convex).

When the set $S$ is instead non-convex, the minimizer may be nonunique, resulting in a set-valued projection, equal to the set of all distance minimizers. These two different cases are illustrated in Fig. 5.

<figure><img src="image_4.png" /><figcaption>Fig. 5: Geometric interpretation of the orthogonal projection. Left: projection on a convex set. Right: projection on a non-convex set, with two distinct distance minimizers.</figcaption></figure>

**Orthogonal projection onto LCP constraints.** In the LCP setting, the complementarity set $S_c$ (see Sec. III) admits a simple coordinate-wise projection. For a pair $(\lambda, \sigma) \in \mathbb{R}^2$, the projection is:

$$ (\lambda^*, \sigma^*) = \begin{cases} (\lambda, 0) & \text{if } \lambda \ge 0, \sigma < \lambda, \\ (0, \sigma) & \text{if } \sigma \ge 0, \lambda < \sigma, \\ \{(\lambda, 0), (0, \sigma)\} & \text{if } \lambda = \sigma > 0, \\ (0, 0) & \text{otherwise.} \end{cases} \quad (34) $$

This is illustrated in Fig. 6, where the feasible set corresponds to the union of the two positive coordinate axes. When $\lambda = \sigma > 0$, the projection becomes set-valued. In practice, we resolve this ambiguity by selecting the element in the set that preserves the contact mode (active or inactive) from the previous iteration, ensuring consistency across updates.

<figure><img src="image_5.png" /><figcaption>Fig. 6: Orthogonal projection onto the complementarity set $\mathbb{S} \triangleq \{(\lambda, \sigma) \in \mathbb{R}^2 | 0 \le \lambda \perp \sigma \ge 0\}$. Different colors indicate the projection outcome depending on the initial values of $(\lambda, \sigma)$. Transparent points represent the current values, thick dots the projected values, and dashed arrows the projection paths.</figcaption></figure>

**Orthogonal projection onto frictional contact NCP constraints.** In the NCP setting, the situation becomes more involved. Indeed, for frictional contacts, the complementarity set now involves projection of nonlinear quantities on second-order cones. We generalize the same approach for the LCP case, by projecting onto these nonlinear cones, which remain convex. These projections are still efficient and fully compatible with the ADMM loop. To address the nonlinearity introduced by regularized friction (De Saxcé correction), we follow [24], [51] and treat the nonlinear term as constant within a single ADMM iteration. This leads to a two-level algorithm with an inner ADMM loop solving a QPCC sub-problem with “frozen” nonlinear terms, and an outer fixed-point loop updating these terms after the three steps of the ADMM procedure. More details are provided in Appendix A.

## F. Stationary conditions

In the frame of mathematical programs with complementarity conditions, several optimality criteria might be used to assess convergence toward an optimal solution. In this work, we adopt the Lagrangian formalism to derive the strong stationarity conditions of our conic complementarity constrained problem (21) in the compact form (22).

**The NCP case.** We denote by $\mathcal{L}$ the Lagrangian of the constrained problem (22) with $\mathbb{S}_{DS}$ (the De Saxcé correction being added afterward and considered constant for the iteration). Introducing the multipliers $y_{\lambda_c}$ and $y_{\sigma_c}$ for the complementarity conditions, $y_{eq}$ for the equality constraint on $\sigma_c$, and $y_{\lambda_a}$ for the box constraints on $\lambda_a$ (merged into one via sign separation), the Lagrangian can be written as

$$ \mathcal{L}(\lambda_a, \lambda_c, \sigma_c, y.) \triangleq f(\lambda_a, \lambda_c) - y_{\lambda_c}^{\top} \lambda_c - y_{\sigma_c}^{\top} \sigma_c + y_{\lambda_a}^{\top} \phi(\lambda_a) + y_{\sigma_c}^{\top} (C \lambda_a + D \lambda_c + g - \sigma_c), \quad (35) $$

where $\phi = \mathcal{I}_{\mathbb{B}}$ encodes the box constraints, and $f(\cdot)$ is the quadratic cost function.
A primal-dual candidate $(\lambda_a^*, \lambda_c^*, \sigma_c^*, y_c^*)$ satisfies the (strong) stationarity conditions if:

* **Primal feasibility:**

$$ \sigma_c^* = C\lambda_a^* + D\lambda_c^* + g, \quad (36a) $$

$$ \lambda_c^* \in K_\mu, \quad \sigma_c^* + \Gamma(\sigma_c^*) \in K_\mu^*, \quad (36b) $$

$$ \lambda_a^* \in \mathbb{B} = [\lambda_{a, \min}, \lambda_{a, \max}]. \quad (36c) $$

* **Dual feasibility:**

$$ A^\top r(\lambda_a^*, \lambda_c^*) - C^\top y_{\sigma_c}^* + y_{\lambda_a}^* = 0, \quad (37) $$

$$ B^\top r(\lambda_a^*, \lambda_c^*) - D^\top y_{\sigma_c}^* - y_{\lambda_c}^* = 0. \quad (38) $$

* **Complementarity and conic stationarity:** These conditions depend on the location of $\lambda_c$ and $\sigma_c$ with respect to the cone $K_\mu$ and its dual $K_\mu^*$. Let $\tilde{x} = (x_1, -x_T)$ be the reflection used in the second-order cone (SOC) formulation. For each frictional contact constraints, we have:

- if $\sigma_c + \Gamma(\sigma_c) \in \text{int}(K_\mu^*)$, then $\lambda_c = 0$ and $y_{\sigma_c} = 0$;

- if $\lambda_c \in \text{int}(K_\mu)$, then $\sigma_c = 0$ and $y_{\lambda_c} = 0$;

- if both are on the boundary and $\lambda_c \propto \sigma_c$, then $y_{\lambda_c} \in \mathbb{R}_+ \tilde{\lambda}_c$, $y_{\sigma_c} \in \mathbb{R}_+(\tilde{\sigma}_c + \Gamma(\tilde{\sigma}_c))$;

- if $\lambda_c = 0$ and $\sigma_c + \Gamma(\sigma_c) \in \partial K_\mu^* \setminus \{0\}$, then $y_{\lambda_c} \in \text{int}(\mathbb{R}_- \tilde{\sigma}_c)$ and $y_{\sigma_c} \in \mathbb{R}_+(\tilde{\sigma}_c + \Gamma(\tilde{\sigma}_c))$;

- if $\sigma_c = 0$ and $\lambda_c \in \partial K_\mu^* \setminus \{0\}$, then $y_{\lambda_c} \in \mathbb{R}_+ \tilde{\lambda}_c$ and $y_{\sigma_c} \in \text{int}(\mathbb{R}_- \tilde{\lambda}_c)$;

- if both $\lambda_c$ and $\sigma_c$ are zero, then $y_{\lambda_c} \in K_\mu^*$ and $y_{\sigma_c} \in K_\mu$.

For the box constraints on $\lambda_a$, using sign-based slack splitting:

$$ [y_{\lambda_a}^*]_{-}^\top (\lambda_a^* - \lambda_{a, \min}) = 0, \quad (39a) $$

$$ [y_{\lambda_a}^*]_{+}^\top (\lambda_a^* - \lambda_{a, \max}) = 0. \quad (39b) $$

Primal feasibility, dual feasibility and complementary conditions ensure that all active inequalities (contact and box) are enforced, with their associated multipliers satisfying the standard complementarity and dual feasibility properties.

**The LCP case.** The LCP arises as a special case of the NCP formulation when the contact model considers only normal forces (frictionless contact). In this specific setting, the friction cone $K_\mu$ and its dual $K_\mu^*$ reduce to the non-negative orthant $\mathbb{R}_+^m$. Consequently, the conic constraints simplify to the element-wise complementarity conditions

$$ \begin{array}{l} 0 \le \lambda_c \perp \sigma_c \ge 0 \\ \Leftrightarrow \forall i: \lambda_{c,i} \ge 0, \sigma_{c,i} \ge 0, \lambda_{c,i} \sigma_{c,i} = 0. \quad (40) \end{array} $$

In this case, the dual variables $y_{\lambda_c}$ and $y_{\sigma_c}$ are also constrained component-wise.

### G. Pseudo-code summarizing the approach

Algorithm 1 provides the pseudo-code associated with our solver. Our ADMM-based solver applies equally to both the LCP and NCP formulations presented in Section III, with the appropriate choice of projection operator $\text{proj}_S$ and the optional De Saxcé correction for NCPs. Unlike standard ADMM

**Algorithm 1:** ADMM for goal-oriented contact-implicit inverse dynamics.

**Initialize:** $\hat{\lambda}_c^0, \hat{\lambda}_a^0, \hat{\sigma}_c^0, z^0, z_a^0, z_c^0$, tolerances $\epsilon_{\text{abs}}$ and $\epsilon_{\text{rel}}$, maximum iter $K$;

Optional pre-processing: Apply Ruiz equilibration to scale the matrix $M$;

for $k = 0, 1, \dots, K$ do

**Step 0:** Fix the De Saxcé correction to define $S_{\text{DS}}$ for this $k$;

**Step 1:** $(\lambda_c^{k+1}, \lambda_a^{k+1}, \cdot) \leftarrow \text{solve linear system (30)}$

$\sigma_c^{k+1} \leftarrow (31)$

**Step 2:** $(\lambda_c^{k+1}, \hat{\sigma}_c^{k+1}) \leftarrow \text{project } (\tilde{\lambda}_c^{k+1}, \tilde{\sigma}_c^{k+1}) \text{ onto } S_{\text{DS}}$

$\hat{\lambda}_a^{k+1} \leftarrow \text{project } \tilde{\lambda}_a^{k+1} \text{ onto } \mathbb{B}$

**Step 3:** Update dual variables via (28)

**Step 4:** Compute residuals $r_{\text{prim}}^k, r_{\text{dual}}^k, r_{\text{comp}}^k$ from stationarity conditions.

if $\|r^k\|_{\infty} \le \epsilon_{\text{abs}}$ and $\|r^k - r^{k-1}\|_{\infty} \le \epsilon_{\text{rel}}$ then
end

end

Optional post-processing: Apply the polishing step using current active sets.

**Return:** $(\hat{\lambda}_a^{k+1}, \hat{\lambda}_c^{k+1}, \hat{\sigma}_c^{k+1})$

methods that track primal and dual residuals between variables and their splitting counterparts, the termination condition in Algorithm 1 is based on the stationarity conditions detailed in Section III-F. This provides a physically meaningful stopping criterion and remains valid for both LCP and NCP problems.

In Algorithm 1, we define three quantities for measuring the convergence:

- The primal residual $r_{\text{prim}}$, quantifying primal feasibility.

- The dual residual $r_{\text{dual}}$, measuring the violation of the first-order optimality.

- The complementarity residual $r_{\text{comp}}$, assessing the violation of the conic (or orthant) complementarity condition.

The algorithm stops when all residuals $r_i$ satisfy both an absolute and a relative criterion.

**Numerical equilibration.** To improve numerical performance, we apply diagonal Ruiz equilibration to the system solved during the proximal step. This ensures a better conditioning of the linear system and stability across scales.

**Polishing.** Once the stopping criterion is satisfied, we optionally perform a polishing step, to ensure full satisfaction of the optimality conditions, especially for the dual variables. This step solves a reduced KKT system using the active sets detected during the ADMM iterations and recovers a refined primal-dual pair consistent with the contact modes.

These two operations are further detailed in Appendix B.

### H. Possible variants of ADMM

The presented ADMM-based solver belongs to the class of first-order methods, which are appealing for their simplicity
and scalability. We have explored several potential enhance-
ments, including (i) *over-relaxation*, (ii) *Anderson accelera-
tion*, and (iii) *changing the order of steps* (26) and (27).

**Over-relaxation.** A popular variant of ADMM includes an over-relaxation factor $\alpha \in (0, 2)$ and replaces $(\lambda_a^{k+1}, \lambda_c^{k+1}, \sigma_c^{k+1})$ in (26)–(28) with $\alpha(\lambda_a^{k+1}, \lambda_c^{k+1}, \sigma_c^{k+1}) + (1-\alpha)(\hat{\lambda}_a^k, \hat{\lambda}_c^k, \hat{\sigma}_c^k)$ [49]. The scheme above is recovered with $\alpha = 1$.

**Anderson acceleration.** Another ADMM variant is Anderson acceleration, which treats the iteration as a fixed-point process $s \mapsto T(s)$ and extrapolates using a short history [52], [53]. Instead of taking the raw update $s_{k+1} = T(s_k)$, it forms a weighted combination of recent mapped points $T(s_{k-j})$, with weights obtained from a small least-squares fit that reduces the fixed-point residuals $r_i = T(s_i) - s_i$. Intuitively, it exploits a local linear relation between the last few steps to extrapolate an estimate of the primal-dual solution, emulating a kind of quasi-Newton steps. In practice, it often leads to early convergence when used with small memory.

We benchmarked all these three ADMM variants for our experiments. However, we found all these variations did not lead to significant improvements in convergence speed or final accuracy. In practice, over-relaxation around $\alpha \approx 1.6$ gave consistent gains; switching the step order helped only when the linear solve came first; Anderson acceleration did not help and is omitted. For the penalty parameters, we ran a grid search to select the best values that work well across as many problems as possible. Equilibration was found to be key, especially in the context of soft robotics where problems may exhibit large variations in terms of numerical conditioning. In addition, polishing helps reduce the dual residual when early stopping occurs, for example due to relative tolerance criteria. We therefore keep the basic scheme, with $\alpha \approx 1.6$, linear solve first, and grid-searched penalty parameters.

I. Convergence guarantees

ADMM was originally developed for convex problems but it
can also perform well when applied to certain nonconvex set-
tings, preserving some convergence guarantees [54]. Although
a convergence analysis of our ADMM-like scheme is beyond
the scope of this work, we point out some key observations
in this respect.

When strict complementarity holds<fnref n="6" />, QPCC is locally a convex problem, around solutions. Therefore, one can expect standard convergence guarantees to be valid *when starting close enough* to a solution. In contrast with the convex setting, however, the penalty parameters cannot be chosen arbitrarily, because of the projection onto a nonconvex set. In our experiments, we address this issue through a coarse grid search to select penalty parameters that yield stable convergence across all scenarios.

Finally, projections onto the set with fixed De Saxcé correc-
tion could be treated as inexact, or approximate, projections.
If the iterates converge, then the projection error with respect

to the original set vanishes, asymptotically recovering (21b). It
is conceivable that, possibly with additional checks, ADMM
can be robust to inexact proximal evaluations, as it is based
on the augmented Lagrangian framework; see the analysis in
[55, §4] for a proximal-gradient scheme with inexact proximal
oracles.

IV. EXPERIMENTAL VALIDATIONS

To assess the versatility and performance of our ADMM-
based contact-implicit inverse dynamics solver, we conduct
several experiments on a diverse set of robotic systems,
both rigid and soft. We begin our evaluation with rigid-body
scenarios, including 3D and 6D pose tracking, angular velocity
control, and manipulation tasks involving frictional contacts.
We then extend our evaluation to soft robotics, comparing
our method against active-set and generic solvers on LCP
and NCP benchmarks, and demonstrate contact-rich control
in soft systems, such as sliding and object positioning through
frictional interactions.

All the examples have the same underlying formulation,
highlighting the generality of our approach across domains.
This collection of experiments illustrates the ability of our
approach to handle a wide variety of actuation modes, contact
conditions, and task objectives within a unified and modular
framework. Our solver is implemented in C++ and uses the
Eigen library [56] for efficient linear algebra, and it will be
released as open-source after the review process.

<figure><img src="image_6.png" /><figcaption>Fig. 7: **Tracking the rigid cube's CoM** in the *x*-direction. Realized (in simulation) vs. desired position (top) and velocity (middle). Residuals and errors (bottom).</figcaption></figure>

A. *Experimental validation on poly-articulated rigid robots*

In this section, we investigate several experimental setups to
demonstrate the contact-implicit inverse dynamics framework

<footnote n="6">⁶Meaning that a solution $(\lambda_a, \lambda_c, \sigma_c)$ satisfies the complementarity constraint nontrivially, that is, for all $i$ either $\lambda_{c,i}$ or $\sigma_{c,i}$ is nonzero.</footnote>
in the context of poly-articulated rigid robots, following the formulation detailed in Section II-C. The robot dynamics and tasks are modeled using the Pinocchio [57], [58] rigid-body dynamics library, which provides implementations for all the required quantities (e.g., the inverse of the joint-space inertia matrix, free velocity, frame Jacobians). We leverage the Coal [59] collision detection to compute the contact placement and associated normal of the contact point candidates.

**Task formulation.** In our experiments, we use classical task functions from the literature [60], but rewritten at the velocity level to fit our impulse formulation from Section II-C. For further details, please refer to Appendix C, where we provide the complete derivations for these few relatively standard task functions expressed in impulse.

**Experiment 1 – sliding rigid cube.** The actuation vector $\tau$ is composed of 6D forces applied directly to the cube’s center of mass with the z-component (in world-space) disabled (thus $\tau \in \mathbb{R}^5$).

↔ *Position tracking.* The desired position for the cube is a sinusoidal signal in the x-direction (after a time of rest). The physical parameters are as follows: the friction coefficient is $\mu = 0.75$, the cube has a side of 40 cm, its mass is 1 kg. Figure 7 shows tracking results. Despite the presence of frictional constraints, the controller can accurately track the desired position at millimeter precision. Here, for this experiment and for all the others, we do not plot the complementarity because it remains very low (below $10^{-8}$).

↔ *Angular velocity tracking.* In this setting, the tracking signal is a desired angular velocity $\bar{\omega}_z$ around the world z-axis. This means the target twist in (48) is $\bar{\nu} = (0, 0, 0, 0, 0, \bar{\omega}_z)^\top$. The difficulty in this task is that of overcoming rotational friction. Figure 8 shows results in terms of tracking the angular velocity with two different friction coefficients.

**Experiment 2 – MuJoCo humanoid.** We use the 27-DoF

humanoid from the MuJoCo repository$^7$, which is a complex, high-dimensional system.

↔ 1. *Stand on one leg.* The task is to regulate the robot’s pose around a given $q_0$, corresponding to standing on its left leg. We regularize the centroidal momentum $h_g$ around the reference $h_g^{\text{ref}} = 0$. The corresponding weight matrix is $W_h = \text{diag}(0.1, 0.1, 10^{-5}, 1.0, 1.0, 1.0)$ (we heavily penalize creating angular momentum and moving the CoM in the xy-plane). The friction coefficient is $\mu = 0.75$ (metal-to-metal contact). The joint torques $\tau$ are constrained to have values below 100 N m. Furthermore, to test the controller’s robustness, we apply an *unmodelled* external force $f_{\text{ext}} \in \mathbb{R}^2$, given by a random process, to the robot’s head in the xy-plane. The tracking error and solver convergence measures, along with the external forces, are shown Fig. 9.

<figure><img src="image_7.png" /><figcaption>Fig. 9: **Humanoid standing.** Top: Robot configuration tracking error and solver primal-dual feasibility measures. Bottom: external force $f_{\text{ext}}$ applied to the robot’s head.</figcaption></figure>

<figure><img src="image_8.png" /><figcaption>Fig. 8: **Cube angular velocity tracking.** Realized angular velocity vs. target angular velocity. Left. On ice (friction coefficient $\mu = 0.1$). Right. On concrete ($\mu = 1.0$).</figcaption></figure>

<footnote>$^7$https://github.com/google-deepmind/mujoco/blob/main/model/humanoid</footnote>
<figure><img src="image_9.png" /><figcaption>Fig. 10: **Humanoid: head altitude tracking.** Visualization. The controller breaks contact by having the humanoid pushing on its ankles in order to reach the desired head altitude $\bar{z}_{\text{head}}$, performing something like a “calf jump”. The time horizon in this screenshot is over 1 s (corresponding to the last two jumps in Fig. 11).</figcaption></figure>

→ 2. *Calf jumps.* The task here is to track a reference altitude $\bar{z}_{\text{head}}(t)$ for the humanoid's head, with the reference reaching higher than the robot's actual height – the desired effect is that the contact-aware controller has the robot break contact with the ground and perform ankle jumps. As in the previous task, we regularize the system's centroidal momentum and have torque limits, and also have a regularization around a reference pose $q_0$. The friction coefficient is the same, 0.75. Note that the relatively high torque limits might not be realistic, and that actual motors would not be able to deliver the high torques required to perform the calf jump. Fig. 10 depicts a screenshot of the jumping motion. Fig. 11 shows the reference desired head altitude and its realization in simulation, and the primal/dual residuals associated with our non-convex ADMM optimization solver.

<figure><img src="image_10.png" /><figcaption>Fig. 11: **Humanoid: head altitude tracking.** Realized head altitude vs. target altitude. The desired head altitude $\bar{z}_{\text{head}}$ is a square wave.</figcaption></figure>

## B. *Experimental results on soft robots*

In this section, we demonstrate the versatility and efficiency of the proposed ADMM-based approach for solving contact-

implicit inverse dynamics problems in soft robotics with frictional contacts. The results are structured in four parts: simulation setup and robot models, evaluation in the LCP setting, extension to the NCP formulation, and control with frictional and sliding contacts. In both the LCP and NCP cases, we compare our approach to established baselines: an active-set method is used as a reference for the LCP formulation, similarly to the approach proposed in [41], while two Ipopt-based methods [61] are considered in the context of control problems with NCP constraints.

<figure><img src="image_11.png" /><figcaption>Fig. 12: Illustration of the robotic systems used in this study: (1) Tower Robot, (2) Trunk Robot, (3) Quadrupod Robot, and (4) Sliding Cube. For the three robotic platforms, we show representative examples of contact configurations along with the distribution of the normal contact problem dimensions.</figcaption></figure>
<figure><img src="image_12.png" /><figcaption>Fig. 13: Comparison of loss (left panel) and timing (right panel) across robots for the LCP problem, considering reference method (active set) and our method (ADMM-based approach).</figcaption></figure>

Four different soft robot systems are considered:

*   **Tower robot.** A pneumatically actuated structure with four internal cavities, introduced in [41]. Its behavior is highly nonlinear due to the presence of self-contact.

*   **Trunk robot.** A silicone robot actuated by eight cables, as presented in [41], interacting with a rigid cylinder. It exhibits rich contact dynamics.

*   **Quadrupod robot.** A deformable parallel mechanism actuated by servo motors, developed as part of this work to manipulate a ball through contact.

*   **Sliding cube.** A deformable cube undergoing planar motion with controlled sliding, used to validate frictional control strategies.

These systems are selected to cover diverse actuation types (pneumatic, cable-driven, motorized), geometries (parallel, elongated), and behaviors (sliding, self-contact). We consider both single-step control (trajectory tracking at each time step) and multi-step goal tracking using an exponentially decaying error signal to reduce overshooting. The units are millimeters for Trunk, Tower, and Quadrupod, and centimeters for the sliding cube. We use absolute tolerances of $10^{-3}$ and relative tolerances of $10^{-2}$, corresponding to sub-millimetric precision in all cases. The systems are represented in Fig. 12.

**Remark.** Note that, in soft robotics, strict satisfaction of all constraints is often neither achievable nor necessary, given uncertainties in sensing, actuation, and modeling. Rather than enforcing exact optimality, we target solutions with low residuals—sufficiently accurate to ensure physically consistent behavior, yet fast enough for online control. This compromise reflects the realities of soft robot deployment, where control within a few millimeters is typically adequate. Our approach embraces this trade-off while preserving key physical properties, including realistic transitions between sticking, sliding, and breaking in contact-rich interactions.

**Simulation framework and soft-robotics models.** We build our experiments on a differentiable version [4] of the SOFA framework [15], which allows accurate FEM-based simulation of soft robots, including nonlinear frictional contact as introduced in [24].

**LCP case and comparison against active set methods.** We begin by evaluating our solver in the LCP setting, where the

contact model is defined as a linear complementarity problem with Signorini constraints (i.e., non-penetration without friction).

We benchmark our ADMM-based solver against the active set method from Coevoet et al. [41] on three systems: Trunk, Tower, and Quadrupod. For each robot, we construct a dataset of 177, 166, and 179 different contact configurations, respectively. These configurations are sampled along actuation trajectories that explore the robot's contact space through extremal actuation sequences. Visual examples of representative contact configurations, along with the distribution of contact dimensions, are shown in Fig. 12. A grid search of the solver parameters is then performed over all the examples to identify the best parameter values across all robots and examples.

Quantitatively, we compare the two methods in terms of computation time, loss value *f*, and residuals (primal, dual, complementarity). The ADMM-based approach outperforms the active-set technique in terms of loss (always within millimeter or sub-millimeter range) and computational efficiency, as shown in Fig. 13.

<table><caption>TABLE II: Mean residuals computed across all contact configurations for each robot in the LCP setting. Comparison between the active set method (reference) and the proposed ADMM-based solver.</caption><thead><tr><th>Trunk Robot</th><th>Reference</th><th>Our method</th></tr></thead><tbody><tr><td>r<sub>prim</sub></td><td>1.52 &middot; 10<sup>-5</sup></td><td>9.51 &middot; 10<sup>-1</sup></td></tr><tr><td>r<sub>dual</sub></td><td>3.32 &middot; 10<sup>-1</sup></td><td>1.98 &middot; 10<sup>-1</sup></td></tr><tr><td>r<sub>comp</sub></td><td>8.44 &middot; 10<sup>-2</sup></td><td>4.41 &middot; 10<sup>-2</sup></td></tr></tbody><thead><tr><th>Tower Robot</th><th>Reference</th><th>Our method</th></tr></thead><tbody><tr><td>r<sub>prim</sub></td><td>2.64 &middot; 10<sup>-5</sup></td><td>1.47 &middot; 10<sup>-1</sup></td></tr><tr><td>r<sub>dual</sub></td><td>1.44</td><td>3.13 &middot; 10<sup>-1</sup></td></tr><tr><td>r<sub>comp</sub></td><td>1.89 &middot; 10<sup>-3</sup></td><td>3.38 &middot; 10<sup>-2</sup></td></tr></tbody><thead><tr><th>Quadrupod Robot</th><th>Reference</th><th>Our method</th></tr></thead><tbody><tr><td>r<sub>prim</sub></td><td>1.26 &middot; 10<sup>-4</sup></td><td>1.56</td></tr><tr><td>r<sub>dual</sub></td><td>1.80</td><td>2.98 &middot; 10<sup>-1</sup></td></tr><tr><td>r<sub>comp</sub></td><td>3.26</td><td>3.77 &middot; 10<sup>-2</sup></td></tr></tbody></table>

As expected, the active-set method yields lower primal residuals, since it explicitly satisfies the primal constraints by construction. However, our method exhibits better dual and complementarity residuals, leading to more balanced solutions,
<figure><img src="image_13.png" /><figcaption>Fig. 14: Comparison of loss (left panel) and timing (right panel) across robots for the NCP problem, considering a nonlinear programming method (Ipopt), a regularized scheme with homotopy, and our method (ADMM-based approach).</figcaption></figure>

as summarized in Table II. This level of error is acceptable for our target applications, where position or force accuracy within a few millimeters is sufficient for effective control. In this context, reducing dual or complementarity residuals further would provide limited practical benefit and is not strictly required.

Residuals can be further reduced by lowering the convergence thresholds or by using scheduling strategies on the ADMM steps. However, this comes at the cost of increased computation time, as changing the parameters requires matrix refactorization in (26).

**NCP case and comparison against IP methods.** We now extend our evaluation to the full nonlinear frictional contact formulation using the NCP model. The same trajectories as in the LCP case are reused, with contact dimensions now tripled due to 3D contact force components. As for the LCP case, a grid search is performed to find the best set of parameters across all robots and contact configurations. In addition, we compare our approach to two Ipopt-based baselines. In both cases, the conic constraints are represented as nonlinear inequalities, and the complementarity conditions are enforced via equality constraints of the form $\lambda_{c,i}^T(\sigma_{c,i} + \Gamma(\sigma_{c,i})) = 0$ for each contact point $i$. Analytical gradients are provided, while the Hessian of the Lagrangian — being indefinite — is approximated via BFGS.

Despite the increased complexity, our method maintains sub-millimeter precision, with residuals comparable to the LCP case. Computation times increase slightly due to the size of the systems, with average runtimes around 0.1 seconds per solve. The quality of the solution remains stable, with minimal loss degradation, as shown in Fig. 14. The average values of all residuals remain strictly below one across the benchmark. While Ipopt can achieve low objective losses and fast local convergence thanks to its Newton-like steps, we observe that it occasionally returns solutions with poor stationarity residuals. Moreover, the Python overhead in our implementation remains negligible (less than 5% of the total time), confirming that most of the runtime is spent inside the solver. Our method offers more consistent residuals and faster, more predictable

solve times, making it better suited to soft robotics settings where reliability is crucial.

Contact detection and filtering play a critical role here: to limit the size of the contact problem, we have to filter candidate contact points tetrahedron-wise, keeping only the most relevant ones for each interaction. Further improvements in runtime and conditioning could be obtained by leveraging more sophisticated contact culling techniques.

**Control with frictional contacts.** We next evaluate the solver in a control setting, using the inverse dynamics formulation to compute actuation inputs that guide the robot toward desired goals in the presence of friction and contact.

<figure><img src="image_14.png" /><figcaption>Fig. 15: Illustration of the two tracking scenarios considered in the cube example. Left: trajectory tracking. Right: target reaching with exponential decay. In both cases, (1) initial configuration and (2) final configuration. The end-effector is shown in orange, and the goal(s) are shown in green.</figcaption></figure>

The first task involves pushing a deformable cube across a planar surface. Two control schemes are tested: (i) trajectory tracking, where each goal is reachable in one time step, corresponding to Eq. (23) with the matrices and vectors as outlined in Table I; and (ii) exponentially decaying tracking, where a distant goal is approached progressively over time. These two cases are represented in Fig. 15. For the second case, let suppose that we want to minimize the norm of the error $e(t) = W_{ea}\lambda_a + W_{ec}\lambda_c + \delta_e^{\text{free}} - X_{e,\text{goal}}$ with an exponentially decaying tracking. The cost function can be written:

$$f(\lambda_a, \lambda_c) = \frac{1}{2}\|\dot{e}(t) + \alpha e(t)\|_2^2 \\ \approx \frac{1}{2}\|(1 + \alpha h)e(t) - e(t - h)\|_2^2, \quad (41)$$
with an explicit scheme to approximate a derivative, α is a parameter to control the decaying velocity (and h > 0 is once again an integration time step). This new loss function can be integrated in (23) by considering:

$$A = (1 + \alpha h) W_{ea}, \quad (42)$$

$$B = (1 + \alpha h) W_{ec},$$

$$b = (1 + \alpha h)(\delta_e^{\text{free}} - \mathbf{X}_{e,\text{goal}}) - e(t - h).$$

In both cases, at each time step, we solve the inverse problem to compute actuation forces, which are then applied in a physics-based simulation using the ADMM contact solver from [24]. This means that the computed control is validated in a realistic simulation, where contact dynamics come from an external solver.

<figure><img src="image_15.png" /><figcaption>Fig. 16: Results obtained for the control of a sliding cube on a surface, in the two scenarios considered in this study. Top: results for trajectory tracking. Bottom: results for exponential error decay control. In blue, the position of the end-effector along the y-axis. In red, the target position. In black, the positioning error relative to the target. In orange, the primal residual. In green, the dual residual.</figcaption></figure>

Results show sub-millimeter tracking error for both control schemes, with residuals consistently below the unit, as illustrated in Fig. 16. The exponentially decaying approach avoids overshooting and results in trajectories that closely follow an exponential decay in error. These results demonstrate that the solver produces physically meaningful and stable control strategies even in the presence of sliding contacts.

<figure><img src="image_16.png" /><figcaption>Fig. 17: Illustration of the two tracking scenarios considered in the Quadrupod example. Left: trajectory tracking. Right: target reaching with exponential decay. In both cases, (1) initial configuration and (2) final configuration. The end-effector is shown in orange, and the goal(s) are shown in green.</figcaption></figure>

Similar scenarios are considered for the Quadrupod robot, as illustrated in Fig. 17. In these control tasks, the Quadrupod robot must reposition a ball through contact by coordinating its legs. As illustrated in Fig. 18, the same conclusions can be drawn as in the sliding cube example. The solver successfully computes actuation inputs that account for contact, friction, and compliance, yielding stable and precise trajectories.

<figure><img src="image_17.png" /><figcaption>Fig. 18: Results obtained for the control of the Quadrupod robot, in the two scenarios considered in this study. Top: results for trajectory tracking. Bottom: results for exponential error decay control. In blue, the position of the end-effector along the y-axis. In red, the target position. In black, the positioning error relative to the target. In orange, the primal residual. In green, the dual residual.</figcaption></figure>
# V. CONCLUSION

In this article, we introduced a unified task-space contact-implicit inverse dynamics framework for both rigid and soft robots. The approach extends the standard instantaneous inverse dynamics control scheme with contact-implicit reasoning capabilities. In particular, we cast the inverse dynamics problem as a QPCC, allowing contact and friction to be handled implicitly without any predefined contact sequence. To solve this QPCC formulation, we introduced a modular ADMM-based solver that achieves accurate and robust control across diverse systems, from humanoid robots to deformable structures, by automatically selecting appropriate contact modes. Extensive experiments demonstrate sub-millimeter accuracy and improved robustness over active-set or second-order solvers. While convergence is consistently observed in practice, future work will focus on strengthening theoretical guarantees through adaptive penalty updates and second-order extensions.

# ACKNOWLEDGMENTS

This work has received support from the French government, managed by the National Research Agency, through the INEXACT project (ANR-22-CE33-0007-01) and NIMBLE project (ANR-22-CE33-0008) and under the France 2030 program with the references Organic Robotics Program (PEPR O2R), the PIQ program under the management of Agence de Programme du Numérique, and "PR[AI]RIE-PSAI" (ANR-23-IACL-0008). The European Union also supported this work through the AGIMUS project (GA no.101070165). Views and opinions expressed are those of the author(s) only and do not necessarily reflect those of the funding agencies.

# REFERENCES

[1] M. Teschner, S. Kimmerle, B. Heidelberger, G. Zachmann, L. Raghupathi, A. Fuhrmann, M.-P. Cani, F. Faure, N. Magnenat-Thalmann, W. Strasser, and P. Volino, "Collision detection for deformable objects," *Computer Graphics Forum*, vol. 24, no. 1, pp. 61–81, 2005.

[2] Q. Le Lidec, W. Jallet, L. Montaut, I. Laptev, C. Schmid, and J. Carpentier, "Contact models in robotics: A comparative analysis," *IEEE Transactions on Robotics*, vol. 40, pp. 3716–3733, July 2024.

[3] V. Acary, M. Brémond, and O. Huber, "On solving contact problems with Coulomb friction: formulations and numerical comparisons," *Research Report RR-9118*, INRIA, Nov. 2017.

[4] E. Ménager, L. Montaut, Q. Le Lidec, and J. Carpentier, "Differentiable simulation of soft robots with frictional contacts," in *2025 IEEE 8th International Conference on Soft Robotics (RoboSoft)*, pp. 1–8, IEEE, 2025.

[5] J. J. Ye, D. L. Zhu, and Q. J. Zhu, "Exact penalization and necessary optimality conditions for generalized bilevel programming problems," *SIAM Journal on Optimization*, vol. 7, no. 2, pp. 481–507, 1997.

[6] M. Anitescu, "On using the elastic mode in nonlinear programming approaches to mathematical programs with complementarity constraints," *SIAM Journal on Optimization*, vol. 15, no. 4, pp. 1203–1236, 2005.

[7] K. Lin and T. Ohtsuka, "A gap penalty method for optimal control of linear complementarity systems," in *2024 IEEE 63rd Conference on Decision and Control (CDC)*, pp. 880–887, 2024.

[8] C. Kanzow and A. Schwartz, "A new regularization method for mathematical programs with complementarity constraints with strong convergence properties," *SIAM Journal on Optimization*, vol. 23, no. 2, pp. 770–798, 2013.

[9] K. Lin and T. Ohtsuka, "A non-interior-point continuation method for the optimal control problem with equilibrium constraints," *Automatica*, vol. 171, p. 111940, 2025.

[10] J. Hall, A. Nurkanović, F. Messerer, and M. Diehl, "LCQPow: a solver for linear complementarity quadratic programs," *Mathematical Programming Computation*, vol. 17, no. 1, pp. 81–109, 2025.

[11] Y. Kim, S. Leyffer, and T. Munson, "MPEC methods for bilevel optimization problems," in *Bilevel Optimization: Advances and Next Challenges* (S. Dempe and A. Zemkoho, eds.), pp. 335–360, Springer, 2020.

[12] P. C. Horak and J. C. Trinkle, "On the similarities and differences among contact models in robot simulation," *IEEE Robotics and Automation Letters*, vol. 4, no. 2, pp. 493–499, 2019.

[13] É. Delassus, "Mémoire sur la théorie des liaisons finies unilatérales," *Ann. Sci. Ec. Norm. Super.* (4), vol. 34, pp. 95–179, 1917.

[14] G. 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*, vol. 28, no. 4, pp. 225–245, 1998. Recent Advances in Contact Mechanics.

[15] F. Faure, C. Duriez, H. Delingette, J. Allard, B. Gilles, S. Marchesseau, H. Talbot, H. Courtecuisse, G. Bousquet, I. Peterlik, and S. Cotin, "SOFA: A multi-model framework for interactive physical simulation," in *Soft Tissue Biomechanical Modeling for Computer Assisted Surgery* (Y. Payan, ed.), vol. 11 of *Studies in Mechanobiology*, *Tissue Engineering and Biomaterials*, pp. 283–321, Springer, June 2012. [Online].

[16] F. Jourdan, P. Alart, and M. Jean, "A Gauss-Seidel like algorithm to solve frictional contact problems," *Computer Methods in Applied Mechanics and Engineering*, vol. 155, no. 1, pp. 31–47, 1998.

[17] E. Coumans and Y. Bai, "PyBullet, a python module for physics simulation for games, robotics and machine learning." http://pybullet.org, 2016.

[18] CEA LIST, "XDE physics simulator." https://www-list.cea.fr/, 2020. Simulation engine for multibody dynamics with contact and friction, developed by CEA.

[19] M. Macklin, K. Storey, M. Lu, P. Terdiman, N. Chentanez, S. Jeschke, and M. Müller, "Small steps in physics simulation," in *Proceedings of the 18th Annual ACM SIGGRAPH/Eurographics Symposium on Computer Animation*, pp. 1–7, 2019.

[20] I. Huang, Y. Narang, C. Eppner, B. Sundaralingam, M. Macklin, T. Hermans, and D. Fox, "DefGraspSim: Simulation-based grasping of 3D deformable objects," *arXiv preprint*, 2021.

[21] C. Yu, W. Du, Z. Zong, A. Castro, C. Jiang, and X. Han, "A convex formulation of material points and rigid bodies with GPU-accelerated async-coupling for interactive simulation," 2025.

[22] M. Macklin, K. Erleben, M. Müller, N. Chentanez, S. Jeschke, and V. Makovychuk, "Non-smooth Newton methods for deformable multi-body dynamics," *ACM Transactions on Graphics (TOG)*, vol. 38, no. 5, pp. 1–20, 2019.

[23] T. Howell, S. Cleac'h, J. Kolter, M. Schwager, and Z. Manchester, "Dojo: A differentiable physics engine for robotics," *arXiv preprint*, 2022.

[24] J. Carpentier, L. Montaut, and Q. L. Lidec, "From compliant to rigid contact simulation: a unified and efficient approach," *Robotics: Science and Systems*, 2024.

[25] R. Tedrake and the Drake Development Team, "Drake: Model-based design and verification for robotics," 2019.

[26] E. Todorov, "Convex and analytically-invertible dynamics with contacts and constraints: Theory and implementation in MuJoCo," in *2014 IEEE International Conference on Robotics and Automation (ICRA)*, pp. 6054–6061, IEEE, 2014.

[27] R. Budhiraja, J. Carpentier, C. Mastalli, and N. Mansard, "Differential dynamic programming for multi-phase rigid contact dynamics," in *2018 IEEE-RAS 18th International Conference on Humanoid Robots (Humanoids)*, pp. 1–9, IEEE, 2018.

[28] J. Carpentier and P.-B. Wieber, "Recent progress in legged robots locomotion control," *Current Robotics Reports*, vol. 2, no. 3, pp. 231–238, 2021.

[29] P. M. Wensing, M. Posa, Y. Hu, A. Escande, N. Mansard, and A. Del Prete, "Optimization-based control for dynamic legged robots," *IEEE Transactions on Robotics*, vol. 40, pp. 43–63, 2023.

[30] E. Todorov, "Goal directed dynamics," in *2018 IEEE International Conference on Robotics and Automation (ICRA)*, pp. 2994–3000, May 2018.

[31] S. Le Cleac'h, T. A. Howell, S. Yang, C.-Y. Lee, J. Zhang, A. Bishop, M. Schwager, and Z. Manchester, "Fast contact-implicit model predictive control," *IEEE Transactions on Robotics*, vol. 40, pp. 1617–1629, 2024.

[32] G. Kim, D. Kang, J.-H. Kim, S. Hong, and H.-W. Park, "Contact-implicit model predictive control: Controlling diverse quadruped motions without pre-planned contact modes or trajectories," *The International Journal of Robotics Research*, p. 02783649241273645, Oct. 2024.
[33] V. Kurtz, A. Castro, A. O. Onol, and H. Lin, "Inverse dynamics trajectory optimization for contact-implicit model predictive control," *The International Journal of Robotics Research*, p. 02783649251344635, May 2025.

[34] A. Aydinoglu, A. Wei, W.-C. Huang, and M. Posa, "Consensus complementarity control for multicontact mpc," *IEEE Transactions on Robotics*, vol. 40, pp. 3879-3896, 2024.

[35] J. Baumgarte, "Stabilization of constraints and integrals of motion in dynamical systems," *Computer Methods in Applied Mechanics and Engineering*, vol. 1, pp. 1-16, June 1972.

[36] B. Mirtich and J. Canny, "Impulse-based simulation of rigid bodies," in *Proceedings of the 1995 Symposium on Interactive 3D Graphics*, 13D '95, (New York, NY, USA), pp. 181-ff., Association for Computing Machinery, Apr. 1995.

[37] L. Righetti, J. Buchli, M. Mistry, and S. Schaal, "Inverse dynamics control of floating-base robots with external constraints: A unified view," in *2011 IEEE international conference on robotics and automation*, pp. 1085-1090, IEEE, 2011.

[38] J. Kim and N. S. Pollard, "Direct control of simulated non-human characters," *IEEE Computer Graphics and Applications*, vol. 31, pp. 56-65, July-August 2011.

[39] J. Kim and N. S. Pollard, "Fast simulation of skeleton-driven deformable body characters," *ACM Transactions on Graphics (TOG)*, vol. 30, no. 5, pp. 1-19, 2011.

[40] C. D. Santina, R. K. Katzschmann, A. Bicchi, and D. Rus, "Dynamic control of soft robots interacting with the environment," *IEEE International Conference on Soft Robotics (RoboSoft)*, pp. 46-53, 2018.

[41] E. Coevoet, A. Escande, and C. Duriez, "Optimization-based inverse model of soft robots with contact handling," *IEEE Robotics and Automation Letters*, vol. 2, pp. 1413-1419, 2017.

[42] C. Duriez, "Control of elastic soft robots based on real-time finite element method," in *2013 IEEE international conference on robotics and automation*, pp. 3982-3987, IEEE, 2013.

[43] B. Caasenbrood, A. Pogromsky, and H. Nijmeijer, "Sorotoki: A MAT-LAB toolkit for design, modeling, and control of soft robots," *IEEE Access*, vol. 12, pp. 17604-17638, 2024.

[44] Y. Hu, L. Anderson, T.-M. Li, Q. Sun, N. Carr, J. Ragan-Kelley, and F. Durand, "DiffTaichi: Differentiable programming for physical simulation," in *International Conference on Learning Representations*, 2020.

[45] Y. Hu, J. Liu, A. E. Spielberg, J. B. Tenenbaum, W. T. Freeman, J. Wu, D. Rus, and W. Matusik, "Chainqueen: A real-time differentiable physical simulator for soft robotics," *2019 International Conference on Robotics and Automation (ICRA)*, pp. 6265-6271, 2018.

[46] T. Navez, E. Ménager, P. Chaillou, O. Goury, A. Kruszewski, and C. Duriez, "Modeling, embedded control, and design of soft robots using a learned condensed FEM model," *IEEE Transactions on Robotics*, vol. 41, pp. 2441-2459, 2025.

[47] S. Redon, A. Kheddar, and S. Coquillart, "Gauss' least constraints principle and rigid body simulations," in *Proceedings 2002 IEEE international conference on robotics and automation (Cat. No. 02CH37292)*, vol. 1, pp. 517-522, IEEE, 2002.

[48] E. Coevoet, A. Escande, and C. Duriez, "Soft robots locomotion and manipulation control using FEM simulation and quadratic programming," in *2019 2nd IEEE International Conference on Soft Robotics (RoboSoft)*, pp. 739-745, 2019.

[49] S. Boyd, N. Parikh, E. Chu, B. Peleato, and J. Eckstein, "Distributed optimization and statistical learning via the alternating direction method of multipliers," *Foundations and Trends in Machine Learning*, vol. 3, no. 1, pp. 1-122, 2011.

[50] R. J. Vanderbei, "Symmetric quasidefinite matrices," *SIAM Journal on Optimization*, vol. 5, no. 1, pp. 100-113, 1995.

[51] V. Acary, F. Cadoux, C. Lemaréchal, and J. Malick, "A formulation of the linear discrete coulomb friction problem via convex optimization," *ZAMM-Journal of Applied Mathematics and Mechanics/Zeitschrift für Angewandte Mathematik und Mechanik*, vol. 91, no. 2, pp. 155-175, 2011.

[52] J. Zhang, Y. Peng, W. Ouyang, and B. Deng, "Accelerating admm for efficient simulation and optimization," *ACM Transactions on Graphics (TOG)*, vol. 38, pp. 1-21, 2019.

[53] D. G. Anderson, "Iterative procedures for nonlinear integral equations," *J. ACM*, vol. 12, pp. 547-560, Oct. 1965.

[54] A. Themelis and P. Patrinos, "Douglas-Rachford splitting and ADMM for nonconvex optimization: Tight convergence results," *SIAM Journal on Optimization*, vol. 30, no. 1, pp. 149-181, 2020.

[55] A. De Marchi and A. Themelis, "Proximal gradient algorithms under local Lipschitz gradient continuity," *Journal of Optimization Theory and Applications*, vol. 194, no. 3, pp. 771-794, 2022.

[56] G. Guennebaud, B. Jacob, et al., "Eigen v3." http://eigen.tuxfamily.org, 2010.

[57] J. Carpentier, F. Valenza, N. Mansard, et al., "Pinocchio: fast forward and inverse dynamics for poly-articulated systems." https://stack-of-tasks.github.io/pinocchio, 2015-2021.

[58] 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," in *IEEE International Symposium on System Integrations (SII)*, 2019.

[59] J. Pan, S. Chitta, D. Manocha, J. Mirabel, J. Carpentier, and L. Montaut, "Coal - An extension of the Flexible Collision Library," Feb. 2025.

[60] S. Kuindersma, R. Deits, M. Fallon, A. Valenzuela, H. Dai, F. Permenter, T. Koolen, P. Marion, and R. Tedrake, "Optimization-based locomotion planning, estimation, and control design for the atlas humanoid robot," *Autonomous robots*, vol. 40, no. 3, pp. 429-455, 2016.

[61] A. Wächter and L. T. Biegler, "On the implementation of an interior-point filter line-search algorithm for large-scale nonlinear programming," *Mathematical Programming*, vol. 106, no. 1, pp. 25-57, 2006.

[62] M. Garstka, M. Cannon, and P. Goulart, "COSMO: A conic operator splitting method for convex conic problems," *J. Optim. Theory Appl.*, vol. 190, pp. 779-810, Sept. 2021.

[63] D. E. Orin, A. Goswami, and S.-H. Lee, "Centroidal dynamics of a humanoid robot," *Autonomous Robots*, vol. 35, pp. 161-176, Oct. 2013.

# APPENDIX

## A. Details on the NCP constraint projection

As proposed in [24], the De Saxcé correction can be treated as a constant term during one iteration of the algorithm and updated only after completing the three ADMM steps. Without loss of generality, we therefore consider the projection set

$$ \mathbb{S}_{\mathrm{DS}} \triangleq \{ (\lambda_c, \sigma_c) \mid \mathcal{K}_{\mu} \ni \lambda_c \perp \sigma_c \in \mathcal{K}_{\mu}^* \}. $$

To handle the conic frictional constraints, we must rewrite the projection step to consider both the cones and the complementarity. Focusing on the contact forces and velocities (not the actuation forces) and dropping the index for readability, we have

$$ \operatorname{proj}_{\mathbb{S}_{\mathrm{DS}}} (\lambda_{\mathrm{ref}}, \sigma_{\mathrm{ref}}) = \underbrace{\operatorname{argmin}}_{(\lambda, \sigma) \in \mathbb{S}_{\mathrm{DS}}} \frac{p(\lambda, \sigma)}{a \|\lambda - \lambda_{\mathrm{ref}}\|_2^2 + b \|\sigma - \sigma_{\mathrm{ref}}\|_2^2}. \quad (43) $$

where $a$ and $b$ are coefficients depending on $\theta = (\xi, \eta, \rho)$, the set of penalty parameters associated with the ADMM formulation as introduced in Section III-C.

Let $p(\lambda, \sigma)$ be the function to minimize. To compute the values of $\lambda_c$ and $\sigma_c$, we use the contact modes illustrated in Fig. 4:

* **Sticking:** $\lambda \in \text{int } \mathcal{K}$ and $\sigma = 0$. The value of the cost function is: $p(\operatorname{proj}_{\mathcal{K}}(\lambda), 0)$.

* **Breaking:** $\sigma \in \text{int } \mathcal{K}^*$ and $\lambda = 0$. The value of the cost function is: $p(0, \operatorname{proj}_{\mathcal{K}^*}(\sigma))$.

* **Sliding:** $\sigma_N = 0$ and $\lambda_T = -\mu\lambda_N \frac{\sigma_T}{\|\sigma_T\|_2}$. The value of the cost function is the minimum value of

$$ \begin{aligned} \underset{\sigma_T, \lambda_N}{\text{minimize}} \quad & a \left\| \left[ -\mu \frac{\sigma_T}{\|\sigma_T\|_2} \lambda_N, \lambda_N \right] - \lambda_{\text{ref}} \right\|_2^2 \\ & + b (\|\sigma_T - \sigma_{T, \text{ref}}\|_2^2 + \sigma_{N, \text{ref}}^2). \end{aligned} \quad (44) $$
A local minimum is found using a dedicated alternating solver. At each iteration, we first optimize the intensities of the contact force and velocity with a fixed direction, then update the direction by solving a least-squares problem and projecting onto the unit sphere. Although the subproblem is nonconvex, the method is fast in practice and usually converges to a meaningful local minimum. Note that by definition, in this mode $\sigma_T \neq 0$.

The idea is then to compare the three values of the cost function. The effective contact mode (and so the value of $\lambda_c$ and $\sigma_c$) is given by the mode with the lowest cost $p$.

## B. Pre- and post-processing strategies

**Preconditioning via Ruiz equilibration.** To improve numerical stability and reduce the condition number of the linear system solved at each ADMM iteration, we perform a diagonal preconditioning step inspired by the Ruiz equilibration scheme. We aim to solve systems of the form

$$ \Delta(M + \hat{\theta}I)\Delta(\Delta^{-1}X) = -\Delta \begin{bmatrix} -A^{\top}b + \xi\lambda_a^k - z_a^k \\ -B^{\top}b + \eta\lambda_c^k - z_c^k \\ -g + \sigma_c^k - \frac{1}{\rho}w_c^k \end{bmatrix}, \quad (45) $$

where $M$ is the system matrix given by:

$$ M = \begin{bmatrix} A^{\top}A & A^{\top}B & C^{\top} \\ B^{\top}A & B^{\top}B & D^{\top} \\ C & D & 0 \end{bmatrix}, \quad (46) $$

and $\hat{\theta} = \text{diag}(\xi, \eta, \rho^{-1})$ is the diagonal matrix containing proximal steps, and $\Delta$ is a diagonal scaling matrix applied for preconditioning.

The Ruiz scaling consists of finding $\Delta = \text{diag}(\delta)$ such that the scaled system $\Delta(M + \hat{\theta}I)\Delta$ is better conditioned. It is performed by iteratively updating

$$ \delta_i = \frac{1}{\sqrt{\|M_i\|_{\infty} + \varepsilon}}, \quad (47) $$

where $M_i$ is the $i$-th column of $M$ and $\varepsilon$ is a (small) regularization constant to avoid division by zero. Similarly to the modified Ruiz equilibration scheme applied to SOCPs [62], we average the $\delta_i$ corresponding to each second-order cone to ensure a valid scaling between the original and scaled cones.

This scaling is applied only for the resolution of the linear system and projection. All variables involved in the optimization (primal and dual) are scaled accordingly during the computation and unscaled afterward before evaluating optimality or stationary conditions.

**Post-processing: primal-dual refinement (aka polishing).**

We assume that the ADMM provides a primal-dual candidate that satisfies primal feasibility and complementarity conditions. However, dual optimality may not be achieved due to inexact convergence or projection errors. To refine the solution, we extract the active sets from the current multipliers and variables. By freezing the corresponding variables and enforcing the complementarity at active contacts, we solve the reduced KKT system to recover a fully stationary solution. This step ensures a better dual feasibility without modifying the contact mode or actuation. This refinement is applied as a

post-solve heuristic, triggered only when the primal residual falls below a stricter threshold (typically ten times tighter than the main stopping criterion).

## C. Some task functions in the impulse-based formulation of inverse-dynamics control for rigid robots

In the following, we derive the mathematical formulas for inverse-dynamics task functions used to design an inverse-dynamics controller used in the experiments depicted in Section IV-A. In contrast to classical acceleration-level formulations, where position-based tasks have to be differentiated twice (with extra spring-damper stabilization terms added), here we use an impulse-based formulation. In this context, the position tasks need only be differentiated once, and a suitable exponential stabilization is easily formulated. Similarly, velocity-level tasks do not require differentiation, and acceleration tasks only necessitate an implicit integration step to determine the next-timestep joint velocity $v^+$. Despite every task being written at the velocity level, the impulse formulation accounts for full physical (inertia and contact) effects. All these tasks can be easily implemented within the classic rigid-body dynamics library (e.g., Pinocchio [57], [58]).

**6D world-frame velocity tracking.** In this case, the target 6D velocity $\bar{\nu}$ is the world-frame velocity for the end-effector, and the task Jacobian $J_{ee}$ is computed in the world-frame. Mathematically,

$$ e(t) \triangleq J_{ee}(q)v - \bar{\nu}. \quad (48) $$

**3D position tracking.** The aim is to track a reference end-effector trajectory $t \mapsto \bar{p}(t) \in \mathbb{R}^3$. The corresponding task error function we seek to minimize is:

$$ e(t) \triangleq p_{ee}(q(t)) - \bar{p}(t). \quad (49) $$

In the impulse formulation, the decision variables appear at the level of the error time derivative $\dot{e}$,

$$ \dot{e}(t) = J_{ee}(q)v - \bar{v}(t) $$

where $\bar{v}(t) = \dot{\bar{p}}(t)$. To stabilize the error, we will instead minimize

$$ \dot{e}(t) + \alpha e(t) = 0 $$

in the least-squares sense. Here, the task Jacobian $J_{ee}$ is computed in the body-frame with world-aligned axes. We introduce the discretization times $\{t_k\}_k$ and values $e_k = e(t_k)$, $q_k = q(t_k)$, $v_k = v(t_k)$. By semi-implicit integration:

$$ e_{k+1} = e_k + h\dot{e}_{k+1}, \quad \dot{e}_{k+1} \approx J_{ee}(q_k)v_{k+1} - \bar{v}_{k+1}, $$

where the task Jacobian is evaluated at configuration $q_k$, and not at the (unknown) $q_{k+1}$ (as it would be for fully-implicit integration). The discrete-time residual we will aim to minimize is thus

$$ \dot{e}_{k+1} + \alpha(e_k + h\dot{e}_{k+1}). $$

Since $v_{k+1}$ depends on the actual decision variables $(\tau, \lambda_c)$ through the map $\mathcal{F}$, the residual is

$$ J_{ee}(q_k)v_{k+1} - \bar{v}_{k+1} + \alpha_h e_k = 0, $$
with $\alpha_h = \frac{\alpha}{1+\alpha h}$. To solve this in the least-squares sense, we introduce the cost function

$$f(v^+) = \|J_{ee}(q_k)v^+ - \bar{v}_{k+1} + \alpha_h e_k\|_2^2. \quad (50)$$

**Robot configuration tracking.** Assume we want to track a given reference configuration $\bar{q}(t) \in \mathcal{Q}$, which has corresponding velocity $\bar{v}(t)$. The task error in the configuration Lie group is just

$$e(t) = \bar{q}(t)^{-1}q(t) \in \mathcal{Q}.$$

Here, we take special care of defining the error on the Lie group as to ensure correctness for floating-base systems (if the floating base is excluded from the reference, everything simplifies down to Euclidean-space errors). The corresponding Lie algebra error is $\psi \triangleq \log e$; its time derivative is

$$\dot{\psi} = J_1 \bar{v} + J_2 v,$$

where $J_2 = \partial\psi/\partial q = J_{\log}(e)$ is the trivialized Jacobian of the log map at the group error $e$ and $J_1 = \partial\psi/\partial \bar{q} = -J_{\log}(e) Ad_{e^{-1}}$. To stabilize the error, we introduce an ODE on the Lie algebra, $\dot{\psi} + \alpha\psi = 0$.

**Stabilizing centroidal momentum.** The goal here is to impose that the centroidal momentum $h_g$ does not vary, i.e., $\dot{h}_g = 0$. In impulse formulation, what we can do is track a desired value $h_g^{\text{ref}}$ of the momentum (which can be the previous timestep's momentum). Mathematically, the discrete-time task reads (following [63] for the expression of the centroidal momentum)

$$e(t) \triangleq h_g^+ - h_g^{\text{ref}} = A_g(q)v^+ - h_g^{\text{ref}}, \quad (51)$$

leading to the least-squares cost term

$$f(v^+) = \|A_g(q)v^+ - h_g^{\text{ref}}\|_{W_h}^2.$$