Collision Detection with Analytical Derivatives of Contact Kinematics

TL;DR

This work tackles the intrinsic non-differentiability of contact kinematics in degenerate geometries by regularizing non-strictly convex shapes into strictly convex implicit surfaces, enabling analytical derivatives via the Implicit Function Theorem. Building on DCOL's scaling-based detection, iDCOL replaces conic constraints with a small, fixed-size nonlinear system (six equations in six unknowns) derived from a geometric scaling formulation and solves it with a fast, safeguarded Newton method; a LogSumExp-based surrogate yields robust conditioning across penetrating and separating configurations. The framework supports multiple implicit primitives (Smooth Polytope, Smooth Truncated Cone, Superellipsoid, and Superelliptic Cylinder), with exact derivatives available for the transformed surface representations. Analytical derivatives of the contact solution with respect to configuration enable gradient-based planning and differentiable contact physics, demonstrated on quadrotor path planning, multibody collisions, and soft manipulator interactions. The results show microsecond-scale collision queries and favorable differentiability properties, at the cost of a tunable trade-off between geometric fidelity and smoothness, with open-source C++ implementation for integration into planners and physics engines.

Abstract

Differentiable contact kinematics are essential for gradient-based methods in robotics, yet the mapping from robot state to contact distance, location, and normal becomes non-smooth in degenerate configurations of shapes with zero or undefined curvature. We address this inherent limitation by selectively regularizing such geometries into strictly convex implicit representations, restoring uniqueness and smoothness of the contact map. Leveraging this geometric regularization, we develop iDCOL, an implicit differentiable collision detection and contact kinematics framework. iDCOL represents colliding bodies using strictly convex implicit surfaces and computes collision detection and contact kinematics by solving a fixed-size nonlinear system derived from a geometric scaling-based convex optimization formulation. By applying the Implicit Function Theorem to the resulting system residual, we derive analytical derivatives of the contact kinematic quantities. We develop a fast Newton-based solver for iDCOL and provide an open-source C++ implementation of the framework. The robustness of the approach is evaluated through extensive collision simulations and benchmarking, and applicability is demonstrated in gradient-based kinematic path planning and differentiable contact physics, including multi-body rigid collisions and a soft-robot interaction example.

Figures (9)

• Figure 1: (a) The scaling-based formulation computes the minimum geometric scaling factor $\alpha^*$ for which the scaled convex bodies share a contact point $\bm{x}^*$. $\alpha^*<1$ indicates penetration, $\alpha^*=1$ contact, and $\alpha^*>1$ separation. (b) In non-strictly convex geometries (left), degenerate configurations lead to non-unique contact locations or normals. In contrast, strictly convex geometries (right) admit an isolated contact point and a unique contact normal, yielding well-defined, differentiable contact kinematics.
• Figure 2: Example of an implicit surface illustrated using a 2D ellipse, $\phi(\bm{x}) = (x_1/a)^2+(x_2/b)^2-1=0$, where $a$ and $b$ are semi-principal axis lengths: (a) Level sets of the implicit function $\phi(\bm{x})$, with the zero level set $\phi(\bm{x})=0$ indicated by a dashed curve. Surface normals, given by $\hat{\bm{n}}(\bm{x})$, are shown at different level sets. (b) Effects of uniform scaling and rigid-body transformations on implicit surfaces.
• Figure 3: From left to right, increasing $\beta$ or $n$ drives the strictly convex implicit surfaces (orange) to converge to the corresponding exact geometries (black wireframes): (a) smooth polytope, (b) smooth truncated cone, (c) superellipsoid, and (d) superelliptic cylinder.
• Figure 4: Geometrical bounds and surrogate formulation: (a) A contact body pair with inner and outer bounding spheres centered at the body frame origin. (b) A surrogate problem is introduced to handle configurations that are either near-coincident or widely separated. The problem rescales the relative translation such that the bounding spheres are non-penetrating.
• Figure 5: Schematic of the iDCOL numerical solver pipeline. Numerical robustness is improved through geometric scaling, surrogate scaling (Sec. \ref{['sec:surrogate']}), and reparameterization of the scaling variable $\alpha$ (Sec. \ref{['sec:re_para_bounds']}). Geometric scaling uniformly rescales the contact geometry by a fixed length scale (e.g., $\max(r_{1,\mathrm{out}},, r_{2,\mathrm{out}})$). The Newton Solver block corresponds to Algorithm \ref{['alg:idcol_solver']}. The continuation strategy is triggered if the Newton solver fails to converge or if continuation parameters (when present) have not yet reached their target values.