Fast Anytime Motion Planning in Point Clouds by Interleaving Sampling and Interior Point Optimization

Robotic manipulators operating in unstructured environments such as homes and offices need to plan their motions quickly while relying on real-world sensors, which typically produce point clouds. To enable intuitive, interactive, and reactive user interfaces, the motion plan computation should provide high-quality solutions quickly and in an anytime manner, meaning the algorithm progressively improves its solution and can be interrupted at any time and return a valid solution. To address these challenges, we combine two paradigms: (1) asymptotically-optimal sampling-based motion planning, which is effective at providing anytime solutions but can struggle to quickly converge to high quality solutions in high dimensional configuration spaces, and (2) optimization, which locally refines paths quickly. We propose the use of interior point optimization for its ability to perform in an anytime manner that guarantees obstacle avoidance in each iteration, and we provide a novel lazy formulation that efficiently operates directly on point cloud data. Our method iteratively alternates between anytime sampling-based motion planning and anytime, lazy interior point optimization to compute high quality motion plans quickly, converging to a globally optimal solution.