Tight Motion Planning by Riemannian Optimization for Sliding and Rolling with Finite Number of Contact Points

We address a challenging problem in motion planning where robots must navigate through narrow passages in their configuration space. Our novel approach leverages optimization techniques to facilitate sliding and rolling movements across critical regions, which represent semi-free configurations, where the robot and the obstacles are in contact. Our algorithm seamlessly traverses widely free regions, follows semi-free paths in narrow passages, and smoothly transitions between the two types. We specifically focus on scenarios resembling 3D puzzles, intentionally designed to be complex for humans by requiring intricate simultaneous translations and rotations. Remarkably, these complexities also present computational challenges. Our contributions are threefold: First, we solve previously unsolved problems; second, we outperform state-of-the-art algorithms on certain problem types; and third, we present a rigorous analysis supporting the consistency of the algorithm. In the Supplementary Material we provide theoretical foundations for our approach. The Supplementary Material and our open source software are available at https://github.com/TAU-CGL/tr-rrt-public. This research sheds light on effective approaches to address motion planning difficulties in intricate 3D puzzle-like scenarios.