Efficient Incremental Penetration Depth Estimation between Convex Geometries

Penetration depth (PD) is essential for robotics due to its extensive applications in dynamic simulation, motion plan- ning, 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 im- plicit 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 the capability to be warm-started by leveraging the spatial and temporal coherence. This coherence 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 ap- proach is built upon the same implicit geometry representation as EPA, which enables easy integration into existing software stacks. The code and supplemental document are available on: https://github.com/weigao95/mind-fcl.