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.
翻译:穿透深度(Penetration depth,PD)是机器人学中的重要问题,由于其在动态仿真、运动规划、触觉渲染等方面的广泛应用。目前被广泛接受的解决方案是扩展多面体算法(Expanding Polytope Algorithm,EPA),该算法通过扩展隐式集合的内部多面体逼近来估算穿透深度。本文提出了一种新的基于优化的算法,用于增量估算最小穿透深度及其方向。我们算法的一项主要优势在于它可以通过利用空间和时间的相干性来进行热启动,这在许多机器人应用中自然出现(例如相邻仿真时间点之间的时间相干性)。因此,我们的算法获得了显著的加速,我们证明它在几个基准测试中比EPA快5-30倍。此外,我们的方法基于与EPA相同的隐式几何表示,从而使其易于整合和部署到现有的软件堆栈中。我们还提供了一个开源实现,以进行进一步的评估和实验。