🔢 mathematics

Robust Rigid Body Assembly via Contact-Implicit Optimal Control with Exact Second-Order Derivatives

This paper presents a sample-efficient, robust optimal control framework for rigid body assembly that leverages a differentiable physics simulation with exact second-order analytic derivatives and multi-scenario trajectory optimization to achieve over 99% success rates in real-world experiments.

Original authors: Christian Dietz, Sebastian Albrecht, Gianluca Frison, Moritz Diehl, Armin Nurkanović

Published 2026-02-02

📖 4 min read🧠 Deep dive

CC BY 4.0

Original authors: Christian Dietz, Sebastian Albrecht, Gianluca Frison, Moritz Diehl, Armin Nurkanović

Original paper licensed under CC BY 4.0 (http://creativecommons.org/licenses/by/4.0/). ✨ This is an AI-generated explanation of the paper below. It is not written or endorsed by the authors. For technical accuracy, refer to the original paper. Read full disclaimer

Imagine you are trying to teach a robot arm to perform a delicate task, like sliding a peg into a tiny hole. In the real world, this is tricky because the peg might hit the edge of the hole, bounce off, or get stuck. For a robot to do this successfully, it needs a "brain" that can predict exactly how the peg will behave when it touches the hole, and then plan a perfect path to get it in.

For a long time, robots have learned this kind of task by "trial and error," similar to how a baby learns to walk. They try millions of times in a computer simulation, hoping that one of those random attempts works. This is slow and inefficient.

This paper proposes a smarter, faster way. Instead of guessing, the authors give the robot a "mathematical crystal ball" that can see exactly how the physics of the situation will change if it moves just a tiny bit. Here is how they did it, broken down into simple concepts:

1. The "Smooth" Problem

The main headache in robot physics is contact. When two objects touch, the math suddenly becomes "jagged" or "bumpy."

The Analogy: Imagine trying to roll a ball down a hill. If the hill is smooth, you can easily predict the path. But if the hill is covered in jagged rocks (like a peg hitting a hole), the ball might bounce unpredictably. Standard math tools struggle to find the best path over these jagged rocks because the "slope" changes instantly and violently.
The Solution: The authors created a "smoothed" version of these jagged rocks. Think of it like putting a soft, stretchy rubber sheet over the jagged rocks. The robot can now roll over the rubber sheet easily, calculating the perfect path. Once the path is found, they slowly tighten the rubber sheet until it fits the real, jagged rocks perfectly. This allows the robot to find the solution quickly without getting stuck on the "bumps."

2. The "Super-Gradient" (Second-Order Derivatives)

Most robots that use math to plan paths only look at the "slope" of the hill (first-order derivatives). They know which way is "down," but they don't know how fast the slope is changing.

The Analogy: Imagine driving a car. A driver who only knows the slope is like someone who knows the road goes down but doesn't know if it's a gentle hill or a steep cliff. A driver with "second-order" information knows exactly how the curve changes, allowing them to take the turn at the perfect speed without crashing.
The Innovation: This paper provides the robot with these "super-gradients." They built a system that calculates not just the direction, but the curvature of the physics. This makes the planning process incredibly efficient, requiring far fewer "guesses" (simulations) to find the right move.

3. The "Safety Net" (Robustness)

Even with a perfect plan, real robots aren't perfect. Sensors might be slightly off, or the table might be slightly tilted.

The Analogy: Imagine you are teaching a student to walk a tightrope. If you only teach them to walk the exact center line, a tiny breeze will knock them off. Instead, the authors teach the robot to walk the tightrope while imagining it is being pushed by the wind from different directions at the same time.
The Method: They run the simulation many times at once, each time pretending the robot is slightly off-center or the hole is slightly shifted. The robot then finds a single path that works for all these slightly different scenarios. This ensures that when the robot actually tries the task in the real world, it succeeds even if things aren't perfect.

4. The Results

The authors tested this method on a real robot (a UR10e arm) trying to do "peg-in-hole" tasks.

The Outcome: The robot succeeded in over 99% of the real-world attempts without needing any extra tuning or fine-tuning after the plan was made.
The Comparison: They also showed that using their "exact math" (exact Hessians) was much faster and more reliable than using the common "approximate math" (like Gauss-Newton) that other methods use.

Summary

In short, this paper gives robots a new way to learn assembly tasks. Instead of blindly guessing millions of times, the robot uses a "smoothed" mathematical model to see the future clearly, calculates the perfect curve to follow, and plans a path that is robust enough to handle real-world imperfections. The result is a robot that can learn complex assembly tasks quickly and execute them with near-perfect reliability.

Technical Summary: Robust Rigid Body Assembly via Contact-Implicit Optimal Control with Exact Second-Order Derivatives

Problem Statement

Efficient planning of assembly motions remains a significant challenge in robotics. Traditional approaches rely heavily on derivative-free reinforcement learning (RL) and sampling-based methods, which require extensive physics simulations and often lack sample efficiency. While derivative-based methods have shown promise in other domains (e.g., locomotion), they struggle with assembly tasks due to the inherent non-smoothness of contact dynamics.

The core difficulty lies in the fact that contact-rich dynamical systems are typically formulated as Nonlinear Complementarity Problems (NCPs) or Linear Complementarity Problems (LCPs). The solutions to these problems are non-differentiable with respect to state and control inputs because infinitesimal changes can cause discrete switches in active contact sets (e.g., contact points shifting between faces, edges, or vertices, or friction modes changing). This non-smoothness prevents the direct application of efficient second-order optimization methods, which rely on exact derivatives to achieve rapid convergence. Furthermore, existing differentiable physics simulations often neglect the non-differentiability introduced by collision detection or rely on approximations (finite differences, randomized smoothing) that do not match the nominal evaluation, hindering optimization convergence.

Methodology

The paper proposes a sample-efficient, robust optimal control approach that utilizes a differentiable physics simulation capable of providing exact second-order analytic derivatives. The methodology integrates three main components:

1. Smooth Differentiable Collision Detection

The authors formulate collision detection as a Linear Program (LP) using growth distances (a variant of signed distance functions).

Formulation: Instead of scaling polytopes in a form-stable manner, they apply uniform scaling in all directions to ensure well-scaled optimization problems.
Smoothing: To handle non-differentiability, they employ interior-point methods. By solving the KKT system of the LP with a barrier parameter $\tau > 0$ , they obtain a smooth approximation of the signed distance function (SDF), $\Phi_\tau(q)$ .
Derivatives: Using the Implicit Function Theorem (IFT), they derive exact first- and second-order derivatives of the SDF and the contact normal vector.
Gradient Approximation: A specific approximation for the contact normal vector is proposed, derived from the primal-dual variables of the LP. This avoids the need for third-order derivatives of the SDF, which would be required if the exact gradient were used directly.
Robustness: A heuristic scaling strategy is introduced to handle cases where the solver fails to converge due to ill-conditioned KKT matrices, ensuring reliable contact information generation.

2. Differentiable Multi-Contact Physics

The contact resolution problem is modeled using a frictionless contact dynamics formulation based on Moreau's time-stepping scheme, resulting in a contact LCP.

Smoothing/Relaxation: The complementarity conditions are smoothed using Scholtes' smoothing (replacing $a \perp b$ with $ab = \sigma$ ) or Scholtes' relaxation ( $ab \leq \sigma$ ). This transforms the non-smooth LCP into a set of smooth equality and inequality constraints.
Integration: The smooth contact dynamics are directly transcribed into the optimal control problem (OCP). The solver simultaneously optimizes for control inputs and system dynamics.
Homotopy Procedure: To solve the stiff contact dynamics (where $\tau, \sigma \to 0$ ), a homotopy procedure is used. The optimization starts with large smoothing parameters (relaxed physics) and sequentially tightens them, using previous solutions to warm-start the solver. This allows the algorithm to converge from trivial initial guesses to solutions that closely approximate the original non-differentiable formulations.

3. Robust Multi-Scenario Optimal Control

To ensure successful execution on real hardware despite model-reality mismatches, the authors formulate a multi-scenario-based OCP.

Impedance Tracking: A reference trajectory is optimized, which is tracked by multiple "compliant" trajectories via a Cartesian impedance controller.
Perturbation: Each scenario corresponds to a compliant trajectory with a randomized positional offset relative to the reference. The optimizer must find a reference trajectory that successfully completes the assembly task for all scenarios simultaneously.
Objective: This approach effectively optimizes for robustness, ensuring that the planned motion remains feasible even if the real robot deviates slightly from the model.

Key Contributions

Modified Growth Distance Formulation: A new LP-based formulation for signed distance functions that ensures well-scaled derivatives and closely approximates Euclidean distance, including a gradient approximation suitable for contact normals.
Efficient Derivative Implementation: An implementation using the high-performance interior-point solver HPIPM and linear algebra library BLASFEO to compute exact first- and second-order derivatives of collision detection and contact resolution, interfaced with the optimal control library CasADi.
Robust Multi-Scenario Planning: A formulation that combines contact-implicit optimal control with multi-scenario robustness, ensuring executable motions on real robots with Cartesian impedance control.
Empirical Validation: Real-world experiments demonstrating high success rates (>99%) on peg-in-hole tasks with near-zero clearance, and simulation results showing the benefits of exact Hessians over approximations (Gauss-Newton, L-BFGS) and the impact of smoothing parameters.

Results

Real-World Performance: Experiments on a UR10e robot performing peg-in-hole assembly tasks achieved over 99% successful executions. The method successfully handled substantial model-reality mismatches without further tuning.
Computational Efficiency: The proposed SDF implementation computes derivatives efficiently. While slightly slower than state-of-the-art GJK algorithms for nominal evaluation, the ability to compute exact second-order derivatives makes the overall optimization process significantly more sample-efficient.
Convergence: Numerical results indicate that using exact Hessians significantly improves convergence speed compared to Hessian approximations like Gauss-Newton or L-BFGS.
Smoothing Impact: The study investigates the trade-off between smoothing parameters and success rates, showing that the homotopy procedure effectively bridges the gap between smooth, easy-to-solve approximations and the stiff, accurate contact dynamics required for real-world execution.

Significance

The paper claims to bridge the gap between derivative-based optimization and complex assembly tasks. By providing a framework that utilizes exact second-order derivatives for both collision detection and contact resolution, the method enables the use of efficient Newton-based solvers for contact-rich problems. This leads to:

Improved Sample Efficiency: Significantly fewer physics simulation steps are required during planning compared to derivative-free methods.
Robustness: The multi-scenario formulation ensures that the generated trajectories are robust to uncertainties, making them directly executable on real hardware.
Generality: The approach is applicable to any rigid body assembly problem where shapes are represented as unions of convex polytopes.

The work demonstrates that with careful smoothing and exact derivative computation, optimal control can effectively solve challenging assembly problems that have traditionally been the domain of reinforcement learning and sampling-based methods.