C3: Consensus Complementarity Control

01

Start here: what is MPC?

Model Predictive Control (MPC) is a strategy for controlling a system by repeatedly solving a short-horizon optimization problem. At every timestep, the controller asks: given where I am now, what sequence of actions minimizes my cost over the next N steps?

It executes the first action, the world advances one step, and then it replans from scratch. This receding-horizon loop is what gives MPC its power. Errors from the previous solve get corrected automatically in the next one.

Receding horizon: replanning at every step

now

k+1

k+2

k+3

k+4

···

execute

plan

···

Execute the first action, step forward, replan. The horizon slides forward each time.

The MPC objective is a quadratic cost over states and inputs, subject to dynamics constraints. When the dynamics are linear and the cost is quadratic with no other constraints, this reduces to a convex Quadratic Program (QP), which is fast to solve even at high frequency.

02

The contact problem

Now imagine the robot must manipulate an object through physical contact, like pushing, sliding, or lifting. The dynamics must now account for contact forces λ between surfaces:

x_k+1 = A x_k + B u_k + Dλ_k + d
↑ state evolves according to applied forces AND contact forces

Contact forces obey a physical law called complementarity. Let φ be the gap between two surfaces. Then:

0 ≤ λ ⊥ φ ≥ 0
⇒ λ · φ = 0 always

The feasible set is the two rays: either the force is zero, or the gap is zero, never both positive at once.

Every contact mode, whether sticking, sliding, or breaking contact, is captured by this constraint. Fully general, but the shape is the source of all the computational difficulty.

So what exactly makes this constraint hard to optimize over? Three things.

Non-convexity in one picture. Both A and B are valid (λ·φ = 0). Their midpoint is not. It sits deep in the infeasible interior. Any solver that moves in a straight line between feasible states will violate the constraint.

Three reasons this breaks standard optimization

1

Non-convex feasible set. Convex QP and LP solvers require a convex feasible set. The L-shape fails this. As shown above, the straight line between two feasible points passes through infeasible territory. Standard solvers simply cannot handle this geometry.

2

Hidden binary decisions. Every contact at every timestep makes a yes/no choice: touching (φ=0) or not (λ=0). And a square friction pyramid needs four force variables per contact, each carrying its own binary, so 7 contacts over 5 timesteps is 140 simultaneous binary choices for the solver to implicitly reason over.

3

The constraint can't be softened. Relaxing to λ·φ ≤ ε just gives the optimizer room to set both to √ε, which is physically meaningless. Tightening ε back to zero recovers the original hard problem.

03

The obvious approach doesn't scale

The standard way to handle complementarity is to introduce binary variables. Each complementarity row gets a binary that selects which side of the pair is driven to zero, either the force or the gap. The full problem becomes a Mixed-Integer Quadratic Program (MIQP).

The scaling problem: with 7 contact points, four friction-pyramid rays each, and 5 planning timesteps, you need 140 binary variables, or 2¹⁴⁰ possible mode assignments. The MIQP scales exponentially with that count, which is exactly why solving the full problem directly can't keep up with a robot that needs a new plan every 17–33 ms.

You could pre-specify contact modes (motion primitives), but then the robot can only use the modes you thought of in advance. You could use reference trajectories, but then the controller needs to be told what to do rather than discovering it. Neither approach is satisfying for a general-purpose manipulation system.

04

The C3 insight: separate and reconcile

C3 (Consensus Complementarity Control, Aydinoglu, Wei & Posa, 2023) makes a simple but powerful observation: the two jobs, optimizing cost and satisfying complementarity, can be done separately. You then use a mathematical technique called ADMM to force them to agree.

Part 1 · QP

Solve the full MPC problem (cost, dynamics, input limits) as a convex QP. Ignore complementarity entirely. Add a penalty term that pulls toward last round's feasible solution.

Result: cost-optimal, possibly infeasible trajectory.

⇄

Part 2 · MIQP

For each timestep independently, project the QP solution onto the complementarity feasible set. A small per-knot MIQP with 28 binaries, four per contact.

Result: feasible contact modes, with no knowledge of task cost.

ADMM (Alternating Direction Method of Multipliers) drives the two halves toward agreement, though its convergence proof assumes convexity, which complementarity does not have.

The gap between the two solutions is computed and folded back as a penalty into the QP's cost function. Next iteration, the QP is pulled toward the feasible region. The MIQP updates its projection. They converge.

The key to scalability: the per-timestep MIQPs are fully independent and can be solved in parallel. Instead of one exponential MIQP over the entire horizon, you solve N tiny MIQPs, one per knot point. In practice, two ADMM iterations is what the tray experiments ship with.

The algorithm terminates after the QP step (not the MIQP), because the QP solution has better trajectory quality even if it doesn't exactly satisfy complementarity. In a closed-loop MPC setting, the controller replans every cycle anyway, so approximate feasibility is enough.

05

See it converge

Here is C3 on a scalar problem: minimize (φ−4)² + (λ−2)² subject to λ·φ = 0, λ≥0, φ≥0. The unconstrained minimum is at (φ=4, λ=2), which is infeasible because both are positive. The nearest feasible point is (4, 0). Watch QP and MIQP converge to that same point.

iter 0

QP ·

MIQP ·

gap ·

QP solution

MIQP projection

Gap (ADMM dual)

Feasible set

Unconstrained opt.

Watch the QP solution (cyan) and MIQP projection (amber) approach each other iteration by iteration. The dashed line is the gap, proportional to the ADMM penalty added to the next QP solve.

06

Does it actually work?

Yang and Posa (RSS 2024) applied C3 to a dynamic tray-balancing task: a Franka robot must retrieve a tray from external supports, lift it, and return it, all through sliding and sticking contacts, with no grasp. The system uses 7 contact points, N=5 knots, a 0.3 second planning horizon, and runs at 30–60 Hz.

Experiment	Result	Note
C3 with force tracking	8/10 (80%)	Baseline method
Without force tracking	3/10 (30%)	Force tracking matters
Reliability (no resets)	6 consecutive cycles	Task repeats to initial position
Unmodeled mug (319 g)	100%, no re-tuning	~30% of tray mass, unmodeled
Unmodeled sugar box (515 g)	100%, no re-tuning	~50% of tray mass, unmodeled
Sim-to-real transfer	No re-tuning needed	Parameters tuned in Drake simulator

A notable finding: the friction coefficients the controller was given are measurably wrong. The model used μ = 0.6 and 0.1, against 0.5 and 0.18 measured on the bench, and the task still transferred to hardware with no re-tuning. The authors hypothesize that the closed-loop stick-slip gait that emerges from C3 has inherent robustness to friction mismatch: the robot modulates normal force to open a margin around the stick-slip boundary rather than riding it.

The task completes in approximately 5 seconds. To the authors' knowledge, this is the first application of contact-implicit MPC to a dynamic manipulation task in three dimensions.