Framework and Software for Real-TimeMulti-Contact Model Predictive Control
Alp Aydinoglu, Michael Posa

Abstract—Our recent work on consensus complementairy
control (C3) proposes a model predictive control algorithm
for hybrid systems that make and break contact with theirenvironment [1]. C3 is based on the alternating direction methodof multipliers (ADMM), that is capable of high-speed reasoning
over potential contact events. Via a consensus formulation,
the approach enables parallelization of the contact schedulingproblem and is able to run at real-time rates. In this abstract,
we build upon this with software improvements that increase
the run-time by approximately 2 times over our previous
implementation (achieving 25 Hz rate for a problem with 19states, 12 complementarity variables and 0.5 seconds prediction
horizon). Additionally we integrated our software with Drake [4].
The software parses models in URDF format into a non-smoothlinear complementarity system and generates a controller thatsolves the optimal control problem. In addition to our previous
work, we also validated our approach on a 3D manipulation
example.

events, can be parallelized. This enables our algorithm to befast, robust to disturbances and also minimizes the effect ofcontrol horizon on the run-time of the algorithm. Since theoriginal publication, we have rewritten the software library
in C++ and improved the run-time (∼ 2 x faster), now
achieving 25 Hz rate for an example with 19 states and
12 complementarity variables. Additionally, we integrated
our software with Drake and only require URDF models torun the algorithm. We also demonstrate the effectiveness of
our method on (including new 3D manipulation) simulated
examples as well as on a physical example. To the best of ourknowledge, these are the first real-time control results on anunderactuated system as complex and dynamic as the hybrid
cart-pole.

I. I NTRODUCTION

A standard approach to modeling robotic systems is throughthe framework of rigid-body systems with contacts. The
continuous time dynamics can be modeled by manipulator
equations

For many important tasks such as manipulation andlocomotion, robots need to make and break contact withthe environment. Even though such multi-contact systems
are common, they are notoriously hard to control. The main
challenge is finding policies and/or trajectories that explicitly
consider the interaction of the robot with its environment inorder to enable stable, robust motion. For a wide range ofproblems, it is computationally challenging to discover control
policies and/or trajectories and the methods are not suitable forrunning in real-time speeds for complex problems.
Model predictive control (MPC) is one of the most powerfultools for automatic control. Predominant in robotics are MPCbased methods utilizing linearization, leading to quadratic
programs which can be solved efficiently. However, for multicontact systems, the algorithm must also decide when toinitiate or break contact. As a result, linearizations are nolonger appropriate and a hybrid formulation is required.
When linearizations are not viable, the resulting MPC
algorithm includes the hybrid elements that result from making
and breaking of contact. Because of this, developing MPC
based control techniques that can reason about contact eventsand that do not require domain knowledge is a hard task.
Recently, contact-implicit control with a primal-dual interiorpoint method has been explored with impressive results on
simulation [2].
The primary contribution of our work is an algorithm,
consensus complementarity control (C3), for solving thehybrid MPC problem approximately for multi-contact systems.
We exploit the distributed nature of ADMM and demonstratethat the hard part of the problem, reasoning about contact

II. M ETHODS

M (q)v̇ + C(q, v) = Bu + J(q)T λ,(1)where q, v, λ, u represent the generalized coordinates,generalized velocities, the contact forces and the input
respectively. Linear complementarity systems provide nonsmooth, local approximations to (1):xk+1 = Axk + Buk + Dλk + d,
0 ≤ λk ⊥ Exk + F λk + Huk + c ≥ 0,(2)

where the orthogonality constraint embeds the hybridness.
A. Problem Formulation
In this work, we want to solve the following mathematicaloptimization problem:minxk ,λk ,uk

N−1
X(xTk Qk xk + uTk Rk uk ) + xTN QN xN

k=0

s.t. xk+1 = Axk + Buk + Dλk + d,

(3)

0 ≤ λk ⊥ Exk + F λk + Huk + c ≥ 0,
(xk , λk , uk ) ∈ C,
where N is the planning horizon, Qk , QN are positive
semidefinite matrices, Rk are positive definite matrices andC is a convex set.
Given x0 , one solves the optimization (3) and applies u0
to the plant and repeats the process in every time step in areceding horizon manner.

TABLE I
RUN - TIME R ATE / N UMBER OF ADMM I TERATIONS / S TATE D IMENSION/ C OMPLEMENTARITY VARIABLE D IMENSION / I NPUT D IMENSION /H ORIZON
ExampleFinger Gaiting
Pivoting
3D Manipulation

Fig. 1.

Freq.
20 Hz (10 Hz before)
27 Hz (16 Hz before)
25 HzIter.
1053x61019

λ61012u443N
10 (1 sec)
5 (0.5 sec)
5 (0.5 sec)

Experimental setup for cart-pole with soft walls.

B. Consensus Complementarity Control
Our approach consists of solving quadratic programs(QP) followed by low-dimensional projections onto thecomplementarity constraints (MIQP or heuristics), details
can be found in [1]. The QP can be solved quickly via
off-the-shelf solvers and is analogous to solving the MPC
problem for a linear system without contact. Projection steprequires projecting onto the complementarity constraints andis the most challenging part (contact scheduling). Regardless,
ADMM approach allows us to split contact scheduling into N
independent sub-problems and leads to dramatically increased
performance by minimizing the effect of control horizon on
the run-time.
III. S OFTWARE D ETAILS1

The code is written in C++ with a Python interface.
Software consists of three main components: LCS generation,
main solver operations (including QP) and projection.
Using a URDF model and contact pairs, the software cangenerate an LCS model (2) for any given state. The LCS
model is based on a implicit time-stepping scheme [3] that
replaces the friction cone with a polyhedral approximation.
The software allows changing the number edges of thepolyhedral approximation that introduces a trade-off between
model accuracy and model complexity.
Given an LCS object and costs, the C3 (solver) object canbe defined. We use OSQP for solving the QP’s and users caneasily add constraints after creating an object of the C3 class.
We solve the projection problem via MIQP projection usingGurobi.
IV. R ESULTS /D ISCUSSION
Previously [1], we validated our results on three numericalexamples, including two frictional contact problems, andphysical experimentation on an underactuated multi-contact
system as in Figure 1. We provide the run-time details in TableI.
We have done initial experiments on manipulating a rigid
sphere with a single finger (red) as shown in Figure 2. The
1 https://github.com/DAIRLab/dairlib/blob/admm/systems/controllers/c3
controller.hFig. 2. Manipulating a rigid object (sphere) with a finger (red) in Drakeenvironment.

solver is able to find strategies such as pushing from side or
rolling the sphere for manipulating it to the desired location.
Approximately 70 percent of the solve time is spent on
the projection step and almost the majority of the remaining
solve time is spent on the QP. We are implementing warmstart procedures to speed up the QP part, and also exploringheuristics for the projection step. Our goal is to achievefast, dynamic rolling motions that follow a path, including
verification on a experimental setup.
Integration with learned models is in the scope of future
work with few successful preliminary trials on the cart-pole
experimental setup.
R EFERENCES[1] Alp Aydinoglu and Michael Posa. Real-time multicontact model predictive control via admm. In 2022 IEEEInternational Conference on Robotics and Automation(ICRA). IEEE, 2022.
[2] Simon Le Cleac’h, Taylor Howell, Mac Schwager, andZachary Manchester.
Linear contact-implicit modelpredictive control. arXiv preprint arXiv:2107.05616, 2021.
[3] David Stewart and Jeffrey C Trinkle.
An implicit
time-stepping scheme for rigid body dynamics withcoulomb friction. In Proceedings 2000 ICRA. Millennium
Conference. IEEE International Conference on Roboticsand Automation. Symposia Proceedings (Cat. No.
00CH37065), volume 1, pages 162–169. IEEE, 2000.
[4] Russ Tedrake and the Drake Development Team. Drake:
Model-based design and verification for robotics, 2019.
URL https://drake.mit.edu.