Collision Detection with Analytical Derivatives of Contact Kinematics

Technical Summary: iDCOL – Implicit Differentiable Collision Detection with Analytical Derivatives

Problem Statement
Gradient-based methods in robotics (planning, control, learning) require fast and accurate computation of derivatives of governing equations with respect to robot states and control inputs. A critical prerequisite for differentiable contact physics is a well-defined, differentiable contact kinematics map that relates robot states to contact distance, location, and normal.

Existing approaches face two primary limitations:

1. Non-smoothness in Degenerate Configurations: Standard collision detection methods (e.g., GJK/EPA) rely on discrete feature selection. When contact features switch (e.g., face-to-face, edge-to-edge), the resulting contact quantities become piecewise-defined and non-differentiable.
2. Geometric Degeneracy: Differentiability is fundamentally limited by the geometry of contacting bodies. Non-strictly convex geometries (e.g., polytopes, cylinders) possess zero or undefined curvature in certain directions, leading to non-unique contact locations or normals. In contrast, strictly convex geometries (e.g., spheres, ellipsoids) admit unique contact points and normals, yielding differentiable kinematics.

Methodology
The paper introduces iDCOL (implicit differentiable collision detection), a framework that addresses these limitations by regularizing non-strictly convex geometries into strictly convex implicit representations.

1. Geometric Regularization via Implicit Surfaces:
   Instead of using exact polyhedral or conic representations, iDCOL models colliding bodies using strictly convex implicit surfaces defined by scalar functions $\phi: \mathbb{R}^3 \to \mathbb{R}$.

   • Smooth Polytopes: Approximated using the LogSumExp (smooth-max) operator to blend half-space constraints.
   • Superquadrics and Superelliptic Cylinders: Used to approximate boxes, cones, and cylinders with tunable shape exponents ($n$) and sharpness parameters ($\beta$).
   • These representations ensure positive finite curvature everywhere, guaranteeing unique contact points and normals.

2. Scaling-Based Optimization Formulation:
   Building on the DCOL framework, iDCOL formulates collision detection as finding the minimum uniform scaling factor $\alpha$ such that two scaled bodies share a contact point. The problem is posed as:
   $$\min_{x, \alpha} \alpha \quad \text{s.t.} \quad \phi_1(x/\alpha) \le 0, \quad \phi_2(R^T(x-r)/\alpha) \le 0$$
   The Karush–Kuhn–Tucker (KKT) conditions of this problem reduce to a fixed-size system of six nonlinear equations with six unknowns ($x \in \mathbb{R}^3, \alpha, \lambda_1, \lambda_2$).

3. Numerical Solution Strategy:

   • Reparameterization: The scaling factor is reparameterized as $\alpha = e^s$ to enforce positivity and improve conditioning.
   • Surrogate Formulation: To handle extreme configurations (near-coincident or widely separated bodies), a surrogate problem rescales the relative translation to ensure non-penetrating initial conditions.
   • Solver: A custom safeguarded Newton-type solver with backtracking line search is employed. It utilizes analytically computed Jacobians and supports warm-starting from previous time steps.

4. Analytical Derivatives:
   By applying the Implicit Function Theorem (IFT) to the KKT residual system, the framework derives analytical derivatives of the contact solution ($z^*$) with respect to the configuration $q$. This allows for the exact computation of Jacobians for contact kinematic quantities (gap, normal, witness points) without numerical differentiation or automatic differentiation overhead.

Key Contributions

• iDCOL Framework: An implicit differentiable collision detection and contact kinematics framework that restores differentiability in degenerate contact configurations through geometric regularization.
• Fixed-Size Nonlinear System: Unlike previous optimization-based approaches that may vary in dimension, iDCOL reduces contact detection to a deterministic $6 \times 6$ system, enabling efficient solution and differentiation.
• Analytical Derivatives: The derivation of exact analytical derivatives for contact kinematics using the IFT, avoiding the computational cost and approximation errors of numerical or automatic differentiation.
• Open-Source Implementation: A fast, microsecond-scale C++ solver is provided, supporting a library of smooth implicit primitives (polytopes, truncated cones, superellipsoids, superelliptic cylinders).

Results and Evaluation
The approach was evaluated through extensive simulations and benchmarking:

• Robustness and Runtime: The solver converges reliably across diverse shape pairs. Without warm-starting, average runtimes range from ~2 µs (simple shapes) to ~19 µs (complex superellipsoids). With warm-starting, runtimes consistently drop to ~2 µs.
• Comparison with DCOL:
  • For polytope-polytope contacts, iDCOL is initially slower than DCOL (which uses exact polyhedral geometry) but becomes comparable with warm-starting.
  • For smooth shape pairs (e.g., ellipsoid-ellipsoid), iDCOL is faster than DCOL.
  • Crucially, iDCOL provides smoother configuration dependence, which is advantageous for gradient-based optimization, whereas DCOL exhibits piecewise-smooth behavior due to active set changes.
• Applications:
  • Quadrotor Path Planning: Gradient-based optimization of a collision-free trajectory reduced computation time by nearly two orders of magnitude compared to methods without analytical gradients.
  • Multibody Rigid Collisions: A simulation of 10 interacting rigid bodies completed in ~1.94s; without analytical derivatives, the simulation took ~6.5 times longer and exhibited reduced robustness.
  • Soft-Rigid Interaction: A soft manipulator interacting with rigid obstacles was simulated successfully. Without analytical derivatives, the solver stalled, leading to runtimes up to two orders of magnitude longer.

Significance and Claims
The paper posits that iDCOL addresses a fundamental tension between geometric exactness and analytical differentiability in contact-rich robotics. By explicitly trading exact geometric fidelity for strict convexity (via tunable smoothing parameters), the framework mitigates geometric degeneracies that undermine differentiability.

The authors claim that iDCOL enables efficient, microsecond-scale collision queries with full analytical derivatives, making it suitable for integration into gradient-based simulation, control, and trajectory optimization frameworks. The framework is designed to be modular, allowing new convex primitives to be introduced simply by defining their implicit functions and derivatives, while the core algorithm remains shape-independent. The work aims to facilitate the development of scalable, contact-aware algorithms that tightly couple geometry, dynamics, and optimization.