Logo
ScienceStack
Table of Contents
Fetching ...
Sign In

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

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

TL;DR

This work tackles robust, sample-efficient planning of contact-rich rigid-body assembly by introducing a differentiable physics framework that delivers up to second-order derivatives to the solver. It combines an optimization-based, interior-point smoothed collision-detection approach (via a growth-distance SDF) with a smooth, differentiable multi-contact physics model and a robust, multi-scenario contact-implicit optimal control formulation using Cartesian impedance tracking. The main contributions include the modified SDF as a growth distance with an efficient derivative pipeline, a multi-scenario OCP that generalizes to sim-to-real mismatches, and extensive real-world and simulation results showing high success rates and the advantage of exact Hessians over common approximations. The approach achieves reliable assembly on real hardware and offers a flexible framework for incorporating additional constraints or frictional effects, advancing the practical use of derivative-based optimization for assembly tasks.

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.

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

TL;DR

This work tackles robust, sample-efficient planning of contact-rich rigid-body assembly by introducing a differentiable physics framework that delivers up to second-order derivatives to the solver. It combines an optimization-based, interior-point smoothed collision-detection approach (via a growth-distance SDF) with a smooth, differentiable multi-contact physics model and a robust, multi-scenario contact-implicit optimal control formulation using Cartesian impedance tracking. The main contributions include the modified SDF as a growth distance with an efficient derivative pipeline, a multi-scenario OCP that generalizes to sim-to-real mismatches, and extensive real-world and simulation results showing high success rates and the advantage of exact Hessians over common approximations. The approach achieves reliable assembly on real hardware and offers a flexible framework for incorporating additional constraints or frictional effects, advancing the practical use of derivative-based optimization for assembly tasks.

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.
Paper Structure (31 sections, 1 theorem, 62 equations, 12 figures, 4 tables)

This paper contains 31 sections, 1 theorem, 62 equations, 12 figures, 4 tables.

Table of Contents

  1. Introduction
  2. Contribution
  3. Notation
  4. Outline
  5. Related Work
  6. Differentiable Collision Detection
  7. Differentiable Physics Simulation
  8. Planning and Control for Contact-Rich Manipulation
  9. Smooth Differentiable Collision Detection
  10. Optimization-based Collision Detection
  11. SDF Gradient Approximation
  12. First- and Second-Order Derivatives of Collision Detection
  13. Reliable solutions for the distance problems
  14. Differentiable Multi-Contact Physics
  15. Continuous-Time Contact Dynamics
  16. ...and 16 more sections

Key Result

Proposition 1

Let Assumption 1 hold. Then the SDF approximation $\Phi_{\tau}(q)$ for $\tau > 0$ is a well-defined infinitely differentiable function. For $\tau \rightarrow 0$, the smooth SDF $\Phi_{\tau}(q)$ converges to the nonsmooth SDF $\Phi_{0}(q)$. It holds $\Phi_{\tau}(q) \geq \Phi_{0}(q)$.

Figures (12)

  • Figure 1: Illustration of a considered assembly problem and this paper's methodology. The optimization of assembly trajectories is facilitated by initially relaxing object shapes and force-distance complementarities followed by sequentially tightening the relaxations. Video highlighting the methodology: https://youtu.be/g4E83bjs7lg
  • Figure 2: Illustration of the type of assembly problem addressed in this paper. For this specific example, we have $n_{\mathrm{act}} = 3$ and $n_{\mathrm{env}} = 2$.
  • Figure 3: The {1,0,-0.4}-level lines for different optimization-based distance formulations for the distance between a point and the gray cuboid. Previous work refers to Ong1996Tracy2023. The 0-level lines coincide for all distance formulations.
  • Figure 4: Exact gradients $\nabla_{\rho} \Phi_\tau(\rho)$ and the proposed gradient approximation $n_\tau(\rho)$ for a point-polytope distance in two dimensions. Four different values of $\tau$ are considered, the behavior that for $\tau \rightarrow 0$ the approximation converges to the exact gradient can be observed.
  • Figure 5: Illustration of impedance tracking in contact, without and with an offset pose.
  • ...and 7 more figures

Theorems & Definitions (3)

  • Example 1
  • Proposition 1
  • Remark 1