Analytic Conditions for Differentiable Collision Detection in Trajectory Optimization

TL;DR

The paper addresses the computational burden of enforcing non-penetration in trajectory optimization by introducing differentiable Minimum-Offset-To-Touch (MOTT) conditions that embed a signed-distance-like metric directly into a single-level optimization. It derives smooth, analytic constraints for touching between convex, smooth bodies and extends to non-smooth polytopes through smooth semi-algebraic approximations based on superquadratics. The approach reduces reliance on non-differentiable complementarity and demonstrates improved efficiency in various planning scenarios, especially as problem size grows, while providing controllable approximation accuracy via the parameter $\rho$. The work enables robust collision-free planning in cluttered and contact-rich environments and offers a path toward handling higher-dimensional polytopes and more complex contact dynamics in future work.

Abstract

Optimization-based methods are widely used for computing fast, diverse solutions for complex tasks such as collision-free movement or planning in the presence of contacts. However, most of these methods require enforcing non-penetration constraints between objects, resulting in a non-trivial and computationally expensive problem. This makes the use of optimization-based methods for planning and control challenging. In this paper, we present a method to efficiently enforce non-penetration of sets while performing optimization over their configuration, which is directly applicable to problems like collision-aware trajectory optimization. We introduce novel differentiable conditions with analytic expressions to achieve this. To enforce non-collision between non-smooth bodies using these conditions, we introduce a method to approximate polytopes as smooth semi-algebraic sets. We present several numerical experiments to demonstrate the performance of the proposed method and compare the performance with other baseline methods recently proposed in the literature.

Figures (8)

• Figure 1: Optimization-based methods for collision-aware trajectory optimization are traditionally bi-level (shown above in red), where a high level optimization problem calls a lower-level problem to calculate constrained values, like the signed distance between objects. In contrast, we propose Minimum-Offset-To-Touch (MOTT) conditions, which allow us to embed collision avoidance as smooth constraints with analytic form in the higher level problem, resulting in a single level optimization.
• Figure 2: Superquadratic approximation of a unit-hypercube in $\mathbb{R}^3$. As $\rho$ increases, the approximation increases in accuracy.
• Figure 3: The 'Minimum-Offset-To-Touch' distance is positive in the case of non-penetration, negative in the case of penetration, and 0 in the case that bodies are touching. Additionally shown are the points ($x_i, x_j$) that would be touching when the bodies are offset to touch.
• Figure 4: The 'Minimum-Offset-To-Touch' conditions enforce that the surface normals ($\mathbf{\nabla_x g}_i(x_i)$, $\mathbf{\nabla_x g}_j(x_j)$) between two convex bodies are in opposite directions, and that the direction of offset ($\mathbf{a}$) is along this surface normal.
• Figure 5: This is a sub-optimal solution for Minimum-Offset-To-Touch, even though it satisfies the conditions for local optimality.

Theorems & Definitions (1)

• proof