Collision detection for 3D rigid body motion planning with narrow passages

Abstract

In sampling-based 3D rigid body motion planning one of the major subroutines is collision detection. Especially for problems with narrow passages many samples have to be checked by a collision detection algorithm. In this application, the runtime of the motion planning algorithm is dominated by collision detection and the samples have the very specific characteristic that many of them are in collision and have small penetration volumes. In our work, we introduce a data structure and an algorithm that makes use of this characteristic by combining well-known data structures like a distance field and an octree with the swap algorithm by Llanas et al. For 3D rigid body motion planning with narrow passages, our approach achieves a speedup of up to 5.0 compared to well-established collision detection libraries like the Proximity Query Package (PQP) and the Flexible Collision Library (FCL).