A Smooth Analytical Formulation of Collision Detection and Rigid Body Dynamics With Contact

TL;DR

This work tackles the challenge of learning and controlling in contact-rich settings by introducing a smooth, differentiable alternative to traditional non-smooth contact models. It jointly develops a soft signed distance function (SSDF) for continuous collision detection and a soft-minimum contact model (SCM) for distributed, analytic contact forces, enabling forward and inverse dynamics that are fully analytical and twice differentiable ($C^2$). The method supports arbitrary geometries without convex decomposition and achieves runtime that is effectively independent of the number of contacts, facilitating gradient-based planning and inference via automatic differentiation. Experiments on planar pushing demonstrate plausible, inference-friendly behavior with clear advantages for optimization, while highlighting trade-offs in physical fidelity and memory usage, pointing to future work in trajectory optimization and system identification.

Abstract

Generating intelligent robot behavior in contact-rich settings is a research problem where zeroth-order methods currently prevail. A major contributor to the success of such methods is their robustness in the face of non-smooth and discontinuous optimization landscapes that are characteristic of contact interactions, yet zeroth-order methods remain computationally inefficient. It is therefore desirable to develop methods for perception, planning and control in contact-rich settings that can achieve further efficiency by making use of first and second order information (i.e., gradients and Hessians). To facilitate this, we present a joint formulation of collision detection and contact modelling which, compared to existing differentiable simulation approaches, provides the following benefits: i) it results in forward and inverse dynamics that are entirely analytical (i.e. do not require solving optimization or root-finding problems with iterative methods) and smooth (i.e. twice differentiable), ii) it supports arbitrary collision geometries without needing a convex decomposition, and iii) its runtime is independent of the number of contacts. Through simulation experiments, we demonstrate the validity of the proposed formulation as a "physics for inference" that can facilitate future development of efficient methods to generate intelligent contact-rich behavior.

Figures (7)

• Figure 1: We propose a joint formulation for collision detection and contact modelling where both capacities take the form of smooth analytical functions. On the collision detection side (above left), we propose a 3D surface representation called "soft signed distance function" (SSDF) that can tightly approximate any closed 3D surface, and continuously interpolate it towards an ellipsoid by means of a smoothing parameter $\epsilon$. SSDF's allow us to formulate a "soft-minimum contact model" (SCM) that represents the contact interaction between two collision geometries as a smooth force field distributed over their entire volume of intersection. Right side of the figure visualizes this smoothing effect with an example showing how the contact force between two spheres changes as one is moved continuously along the y axis while the other is fixed, for SCM and contact models from other well-established simulators. We demonstrate the plausibility of the proposed formulation through simulation experiments on a planar pushing system involving a T-shaped collision geometry.
• Figure 2: Computing the SSDF $\hat{\phi}_\mathcal{S}(\bm{p})$ of a point $\mathbf{p}$ with respect to an AOPC $\mathcal{S}$ involves three steps: i) computing all distances $\{d(\mathbf{p})_i\}_{\textcolor{ourred}{i=1}}^{\textcolor{ourred}{4}}$ between $\mathbf{p}$ and points $\{\mathbf{p}_i\}_{\textcolor{ourred}{i=1}}^{\textcolor{ourred}{4}}$ on the AOPC, ii) computing a softminimum $\bm \sigma(-\mathbf{d}(\bm{p}))$ of all distances, resulting in a distribution where the entry $i^*\textcolor{ourred}{=4}$ corresponding to the smallest distance has the largest probability mass, iii) using the distribution $\bm \sigma(-\mathbf{d}(\bm{p}))$ for soft selection of the corresponding signed distance via a weighted average $\hat{\phi}_\mathcal{S}(\bm{p}) = \sum_{i=1}^I \sigma(-\mathbf{d}(\bm{p}))_i \ \bm{n}_{i}^{\top}(\bm{p} - \bm{p}_{i}) \textcolor{ourred}{\approx \bm{n}_{4}^{\top}(\bm{p} - \bm{p}_{4})}$.
• Figure 3: Steps involved in performing collision detection between two AOPCs are as follows: i) computing SSDFs of all points on one AOPC with respect to the other AOPC; and vice versa; which gives $\{\hat{\phi}_{\mathcal{S}_1}(\bm{p}'_{j})\}_{\textcolor{ourred}{j=1:3}}$ and $\{\hat{\phi}_{\mathcal{S}_2}(\bm{p}_{i})\}_{\textcolor{ourred}{i=1:4}}$, ii) stacking them into a single vector $\bm{\Phi}_{{12}} \textcolor{ourred}{\in \mathbb{R}^{7}}$ to form the separation field, iii) computing a soft-argminimum $\bm\sigma(-\bm{\Phi}_{12})\textcolor{ourred}{\in[0, 1]^7}$ over the separation field to obtain a separation distribution, which acts as a soft selection operator whose entries concentrate probability mass on AOPC points that have the largest penetration depth (i.e., the smallest SSDF value).
• Figure 4: SSDF of a 2D box for different softmax temperatures $\epsilon$, points $\bm p_i$, and normals $\bm n_i$. The outline of the box, as well as points and normals, are marked in white while the zero isosurface is blue.
• Figure 5: Comparison of contact forces obtained from SCM (top row) and MJX's contact model (bottom row).