In the real world, people and objects will move around the environment, the world will change, and a robot cannot stay stationary and wait, scratching its head to think of what to do. It is essential for critical robotics algorithms that guarantee safety and liveness (i.e., will be able to reach the goal) to be able to react immediately to changing circumstances. Autonomous cars must make decisions that determine the safety of their passengers and the world around them, mobile manipulators working in a factory or a warehouse may need to dodge unseen people and obstacles, and surgical robots must maintain patient safety at all times.
Algorithms that provide real-time performance can come through theoretical properties (guaranteeing a solution in a fixed amount of computation) or software/hardware engineering (using implementation or platform-specific features to accelerate algorithms).
Motion planning against sensor data is often a critical bottleneck in real-time robot control. For sampling-based motion planners, which are effective for high-dimensional systems such as manipulators, the most time-intensive component is collision checking. We present a novel spatial data structure, the collision-affording point tree (CAPT): an exact representation of point clouds that accelerates collision-checking queries between robots and point clouds by an order of magnitude, with an average query time of less than 10 nanoseconds on 3D scenes comprising thousands of points. With the CAPT, sampling-based planners can generate valid, high-quality paths in under a millisecond, with total end-to-end computation time faster than 60 FPS, on a single thread of a consumer-grade CPU. We also present a point cloud filtering algorithm, based on space-filling curves, which reduces the number of points in a point cloud while preserving structure. Our approach enables robots to plan at real-time speeds in sensed environments, opening up potential uses of planning for high-dimensional systems in dynamic, changing, and unmodeled environments.
@inproceedings{ramsey2024,author={Ramsey, Clayton W. and Kingston, Zachary and Thomason, Wil and Kavraki, Lydia E.},title={Collision-Affording Point Trees: SIMD-Amenable Nearest Neighbors for Fast Collision Checking},booktitle={Robotics: Science and Systems},year={2024},doi={10.15607/RSS.2024.XX.038},}
Modern sampling-based motion planning algorithms typically take between hundreds of milliseconds to dozens of seconds to find collision-free motions for high degree-of-freedom problems. This paper presents performance improvements of more than 500x over the state-of-the-art, bringing planning times into the range of microseconds and solution rates into the range of kilohertz, without specialized hardware. Our key insight is how to exploit fine-grained parallelism within sampling-based planners, providing generality-preserving algorithmic improvements to any such planner and significantly accelerating critical subroutines, such as forward kinematics and collision checking. We demonstrate our approach over a diverse set of challenging, realistic problems for complex robots ranging from 7 to 14 degrees-of-freedom. Moreover, we show that our approach does not require high-power hardware by also evaluating on a low-power single-board computer. The planning speeds demonstrated are fast enough to reside in the range of control frequencies and open up new avenues of motion planning research.
@inproceedings{thomason2024vamp,author={Thomason, Wil and Kingston, Zachary and Kavraki, Lydia E.},title={Motions in Microseconds via Vectorized Sampling-Based Planning},year={2024},booktitle={IEEE International Conference on Robotics and Automation},pages={8749--8756},doi={10.1109/ICRA57147.2024.10611190},}
Workshop
Dynamic Motion Planning from Perception via Accelerated Point Cloud Collision Checking
@inproceedings{Ramsey2024wksp,author={Ramsey, Clayton W. and Kingston, Zachary and Thomason, Wil and Kavraki, Lydia E.},title={Dynamic Motion Planning from Perception via Accelerated Point Cloud Collision Checking},booktitle={IEEE ICRA 2024 Workshop---Agile Robotics: From Perception to Dynamic Action},year={2024},}
Over the years, many motion planning algorithms have been proposed. It is often unclear which algorithm might be best suited for a particular class of problems. The problem is compounded by the fact that algorithm performance can be highly dependent on parameter settings. This paper shows that hyperparameter optimization is an effective tool in both algorithm selection and parameter tuning over a given set of motion planning problems. We present different loss functions for optimization that capture different notions of optimality. The approach is evaluated on a broad range of scenes using two different manipulators, a Fetch and a Baxter. We show that optimized planning algorithm performance significantly improves upon baseline performance and generalizes broadly in the sense that performance improvements carry over to problems that are very different from the ones considered during optimization.
@inproceedings{moll2021hyper,author={Moll, Mark and Chamzas, Constantinos and Kingston, Zachary and Kavraki, Lydia E.},title={HyperPlan: A Framework for Motion Planning Algorithm Selection and Parameter Optimization},booktitle={IEEE/RSJ International Conference on Intelligent Robots and Systems},year={2021},pages={2511-2518},doi={10.1109/IROS51168.2021.9636651},}