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 for further evaluations and experiments.

翻译：穿透深度（PD）对于机器人技术至关重要，因其在动态仿真、运动规划、力触觉渲染等领域应用广泛。扩展多面体算法（EPA）是该问题的实际标准方法，通过对隐式集合的内接多面体近似进行扩展来估计PD。本文提出一种新颖的基于优化的算法，可增量式估计最小穿透深度及其方向。本方法的主要优势在于能够利用机器人应用中自然出现的时空连贯性（例如相邻仿真时间节点间的时序连贯性）进行热启动。因此，该算法实现了显著加速——在多个基准测试中，其速度比EPA快5-30倍。此外，本方法基于与EPA相同的隐式几何表示构建，便于集成部署至现有软件栈。我们还提供了开源实现以供进一步评估与实验。