Efficient Incremental Penetration Depth Estimation between Convex Geometries

TL;DR

A novel optimization-based algorithm that incrementally estimates minimum penetration depth and its direction and is built upon the same implicit geometry representation as EPA, which enables easy integration into existing software stacks.

Abstract

Penetration depth (PD) is essential for robotics due to its extensive applications in dynamic simulation, motion planning, haptic rendering, etc. The Expanding Polytope Algorithm (EPA) is the de facto standard for this problem, which estimates PD by expanding an inner polyhedral approximation of an implicit set. In this paper, we propose a novel optimization-based algorithm that incrementally estimates minimum penetration depth and its direction. One major advantage of our method is that it can be warm-started by exploiting the spatial and temporal coherence, which emerges naturally in many robotic applications (e.g., the temporal coherence between adjacent simulation time knots). As a result, our algorithm achieves substantial speedup -- we demonstrate it is 5-30x faster than EPA on several benchmarks. Moreover, our approach is built upon the same implicit geometry representation as EPA, which enables easy integration and deployment into existing software stacks. We also provide an open-source implementation on: https://github.com/weigao95/mind-fcl

Figures (7)

• Figure 1: An illustration of penetration depth and Minkowski Difference. (a) For two colliding shapes, their Minkowski Difference contains the origin $O$. The penetration depth PD$(A_1, A_2)$ is also the minimum distance from the origin $O$ to the boundary of their Minkowski Difference. (b) For two non-overlapping shapes, the origin $O$ is outside their Minkowski Difference and PD is undefined.
• Figure 2: An illustration of a modified MPR algorithm to compute the intersection point (the $v_{\text{intersect}}$ in (a)) of an origin ray with the boundary of the Minkowski Difference. The algorithm takes the ray direction $d_{\text{ray}}$ as the input. In each iteration, the algorithm maintains and updates a "portal", which is a segment for 2D (or a triangle for 3D) that is inscribed to the Minkowski Difference. The illustrations from (a) to (c) show one MPR iteration, as the portal $(v_1, v_2)$ in (a) is updated to a new portal $(v_2, v_{\text{candidate}})$ in (c).
• Figure 3: Approximation scheme used in Sec. \ref{['subsec:alg_2']} and Algorithm \ref{['alg:sqp2']}. (a) shows the configuration of the portal and supporting hyperplane upon the convergence (with tolerance $\Delta$) of MPR, when it is used as a subroutine in Algorithm \ref{['alg:sqp2']}. Intuitively, the approximation in Sec. \ref{['subsec:alg_2']} corresponds to removing all points "outside" the final portal, as shown in (b). A detailed explanation is in Sec. \ref{['subsec:alg_2']}.
• Figure 4: An illustration of the shortcut mechanism in Sec. \ref{['subsec:alg_3']}. The penetration depth along $d_v$ is lower bounded by $|p|$, while the penetration depth along $d_{\text{portal}}(d_z)$ is upper bounded by $z$. We proposed to switch to $d_z$ as the new search direction when $|p| \geq |z|$, as explained in Sec. \ref{['subsec:alg_3']}.
• Figure 5: Performance comparison between our method and EPA van2001epa under different number of vertices in convex polyhedron. Results are the averaged time in microseconds computed from 10000 independent runs. Our method is evaluated with different angular deviations (of initial penetration direction) at $5^{\circ}$ and $45^{\circ}$. The proposed method achieves 6-10x speedup compared to the EPA baseline.