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

2026-01-30

AI Summary

This work addresses the challenges of low planning efficiency and suboptimal success rates in robotic assembly tasks, which stem from contact uncertainty and the sim-to-real gap. To overcome these issues, the authors propose a sample-efficient trajectory optimization method grounded in implicit optimal control with contact. By constructing a high-fidelity differentiable physics simulator and incorporating a smoothing strategy inspired by interior-point methods, the approach enables highly accurate and differentiable collision detection and contact resolution, while efficiently computing exact Hessian matrices. Integrated within a multi-scenario trajectory optimization framework, this method significantly enhances sim-to-real robustness. Real-world experiments on various peg-in-hole assembly tasks demonstrate a success rate exceeding 99%, underscoring the substantial advantage of using exact second-order derivatives over approximate alternatives.

Technology Category

• Search and Optimization: Sampling/Simulation-based Search
• Intelligent Robots: Learning & Optimization for ROB
• Planning, Routing, and Scheduling: Mixed Discrete/Continuous Planning

Application Category

• Responsible Web: Machine-in-the-loop, human agency and autonomy
• Search and Retrieval-Augmented AI: Agentic search
• User Modeling, Personalization and Recommendation: User modeling and simulation for interactive and conversational systems

Abstract

Efficient planning of assembly motions is a long standing challenge in the field of robotics that has been primarily tackled with reinforcement learning and sampling-based methods by using extensive physics simulations. This paper proposes a sample-efficient robust optimal control approach for the determination of assembly motions, which requires significantly less physics simulation steps during planning through the efficient use of derivative information. To this end, a differentiable physics simulation is constructed that provides second-order analytic derivatives to the numerical solver and allows one to traverse seamlessly from informative derivatives to accurate contact simulation. The solution of the physics simulation problem is made differentiable by using smoothing inspired by interior-point methods applied to both the collision detection as well as the contact resolution problem. We propose a modified variant of an optimization-based formulation of collision detection formulated as a linear program and present an efficient implementation for the nominal evaluation and corresponding first- and second-order derivatives. Moreover, a multi-scenario-based trajectory optimization problem that ensures robustness with respect to sim-to-real mismatches is derived. The capability of the considered formulation is illustrated by results where over 99\% successful executions are achieved in real-world experiments. Thereby, we carefully investigate the effect of smooth approximations of the contact dynamics and robust modeling on the success rates. Furthermore, the method's capability is tested on different peg-in-hole problems in simulation to show the benefit of using exact Hessians over commonly used Hessian approximations.

Problem

• rigid body assembly
• robust motion planning
• sim-to-real mismatch
• contact dynamics
• peg-in-hole

Innovation

• differentiable physics simulation
• second-order derivatives
• contact-implicit optimal control
• robust trajectory optimization
• smoothed collision detection