Real-time Performance
Time and tide wait for no one.
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).
2025
- arXiv
Revisiting Replanning from Scratch: Real-Time Incremental Planning with Fast Almost-Surely Asymptotically Optimal PlannersMitchell E. C. Sabbadini, Andrew H. Liu, Joseph Ruan, Tyler S. Wilson, Zachary Kingston, and Jonathan D. GammellUnder ReviewRobots operating in changing environments either predict obstacle changes and/or plan quickly enough to react to them. Predictive approaches require a strong prior about the position and motion of obstacles. Reactive approaches require no assumptions about their environment but must replan quickly and find high-quality paths to navigate effectively. Reactive approaches often reuse information between queries to reduce planning cost. These techniques are conceptually sound but updating dense planning graphs when information changes can be computationally prohibitive. It can also require significant effort to detect the changes in some applications. This paper revisits the long-held assumption that reactive replanning requires updating existing plans. It shows that the incremental planning problem can alternatively be solved more efficiently as a series of independent problems using fast almost-surely asymptotically optimal (ASAO) planning algorithms. These ASAO algorithms quickly find an initial solution and converge towards an optimal solution which allows them to find consistent global plans in the presence of changing obstacles without requiring explicit plan reuse. This is demonstrated with simulated experiments where Effort Informed Trees (EIT*) finds shorter median solution paths than the tested reactive planning algorithms and is further validated using Asymptotically Optimal RRT-Connect (AORRTC) on a real-world planning problem on a robot arm.
@misc{sabbadini2025replan, title = {Revisiting Replanning from Scratch: Real-Time Incremental Planning with Fast Almost-Surely Asymptotically Optimal Planners}, author = {Sabbadini, Mitchell E. C. and Liu, Andrew H. and Ruan, Joseph and Wilson, Tyler S. and Kingston, Zachary and Gammell, Jonathan D.}, year = {2025}, eprint = {2510.21074}, archiveprefix = {arXiv}, primaryclass = {cs.RO}, note = {Under Review}, } - arXiv
Differentiable Particle Optimization for Fast Sequential ManipulationUnder ReviewSequential robot manipulation tasks require finding collision-free trajectories that satisfy geometric constraints across multiple object interactions in potentially high-dimensional configuration spaces. Solving these problems in real-time and at large scales has remained out of reach due to computational requirements. Recently, GPU-based acceleration has shown promising results, but prior methods achieve limited performance due to CPU-GPU data transfer overhead and complex logic that prevents full hardware utilization. To this end, we present SPaSM (Sampling Particle optimization for Sequential Manipulation), a fully GPU-parallelized framework that compiles constraint evaluation, sampling, and gradient-based optimization into optimized CUDA kernels for end-to-end trajectory optimization without CPU coordination. The method consists of a two-stage particle optimization strategy: first solving placement constraints through massively parallel sampling, then lifting solutions to full trajectory optimization in joint space. Unlike hierarchical approaches, SPaSM jointly optimizes object placements and robot trajectories to handle scenarios where motion feasibility constrains placement options. Experimental evaluation on challenging benchmarks demonstrates solution times in the realm of milliseconds with a 100% success rate; a 4000x speedup compared to existing approaches.
@misc{chen2025spasm, title = {Differentiable Particle Optimization for Fast Sequential Manipulation}, author = {Chen, Lucas and Iyer, Shrutheesh R. and Kingston, Zachary}, eprint = {2510.07674}, archiveprefix = {arXiv}, primaryclass = {cs.RO}, year = {2025}, note = {Under Review}, } - arXiv
HJCD-IK: GPU-Accelerated Inverse Kinematics through Batched Hybrid Jacobian Coordinate DescentUnder ReviewInverse Kinematics (IK) is a core problem in robotics, in which joint configurations are found to achieve a desired end-effector pose. Although analytical solvers are fast and efficient, they are limited to systems with low degrees-of-freedom and specific topological structures. Numerical optimization-based approaches are more general, but suffer from high computational costs and frequent convergence to spurious local minima. Recent efforts have explored the use of GPUs to combine sampling and optimization to enhance both the accuracy and speed of IK solvers. We build on this recent literature and introduce HJCD-IK, a GPU-accelerated, sampling-based hybrid solver that combines an orientation-aware greedy coordinate descent initialization scheme with a Jacobian-based polishing routine. This design enables our solver to improve both convergence speed and overall accuracy as compared to the state-of-the-art, consistently finding solutions along the accuracy-latency Pareto frontier and often achieving order-of-magnitude gains. In addition, our method produces a broad distribution of high-quality samples, yielding the lowest maximum mean discrepancy. We release our code open-source for the benefit of the community.
@misc{yasutake2025hjcdik, title = {{HJCD-IK}: {GPU}-Accelerated Inverse Kinematics through Batched Hybrid Jacobian Coordinate Descent}, author = {Yasutake, Cael and Kingston, Zachary and Plancher, Brian}, eprint = {2510.07514}, archiveprefix = {arXiv}, primaryclass = {cs.RO}, year = {2025}, note = {Under Review}, } - arXiv
Look as You Leap: Planning Simultaneous Motion and Perception for High-DoF RobotsQingxi Meng, Emiliano Flores, Carlos Quintero-Peña, Peizhu Qian, Zachary Kingston, Shannan K. Hamlin, Vaibhav Unhelkar, and Lydia E. KavrakiUnder ReviewIn this work, we address the problem of planning robot motions for a high-degree-of-freedom (DoF) robot that effectively achieves a given perception task while the robot and the perception target move in a dynamic environment. Achieving navigation and perception tasks simultaneously is challenging, as these objectives often impose conflicting requirements. Existing methods that compute motion under perception constraints fail to account for obstacles, are designed for low-DoF robots, or rely on simplified models of perception. Furthermore, in dynamic real-world environments, robots must replan and react quickly to changes and directly evaluating the quality of perception (e.g., object detection confidence) is often expensive or infeasible at runtime. This problem is especially important in human-centered environments such as homes and hospitals, where effective perception is essential for safe and reliable operation. To address these challenges, we propose a GPU-parallelized perception-score-guided probabilistic roadmap planner with a neural surrogate model (PS-PRM). The planner explicitly incorporates the estimated quality of a perception task into motion planning for high-DoF robots. Our method uses a learned model to approximate perception scores and leverages GPU parallelism to enable efficient online replanning in dynamic settings. We demonstrate that our planner, evaluated on high-DoF robots, outperforms baseline methods in both static and dynamic environments in both simulation and real-robot experiments.
@misc{meng2025look, title = {Look as You Leap: Planning Simultaneous Motion and Perception for High-{DoF} Robots}, author = {Meng, Qingxi and Flores, Emiliano and Quintero-Peña, Carlos and Qian, Peizhu and Kingston, Zachary and Hamlin, Shannan K. and Unhelkar, Vaibhav and Kavraki, Lydia E.}, eprint = {2509.19610}, archiveprefix = {arXiv}, primaryclass = {cs.RO}, year = {2025}, note = {Under Review}, } - AORRTC: Almost-Surely Asymptotically Optimal Planning with RRT-ConnectIEEE Robotics and Automation Letters
Finding high-quality solutions quickly is an important objective in motion planning. This is especially true for high-degree-of-freedom robots. Satisficing planners have traditionally found feasible solutions quickly but provide no guarantees on their optimality, while almost-surely asymptotically optimal (a.s.a.o.) planners have probabilistic guarantees on their convergence towards an optimal solution but are more computationally expensive. This paper uses the AO-x meta-algorithm to extend the satisficing RRT-Connect planner to optimal planning. The resulting Asymptotically Optimal RRT-Connect (AORRTC) finds initial solutions in similar times as RRT-Connect and uses any additional planning time to converge towards the optimal solution in an anytime manner. It is proven to be probabilistically complete and a.s.a.o. AORRTC was tested with the Panda (7 DoF) and Fetch (8 DoF) robotic arms on the MotionBenchMaker dataset. These experiments show that AORRTC finds initial solutions as fast as RRT-Connect and faster than the tested state-of-the-art a.s.a.o. algorithms while converging to better solutions faster. AORRTC finds solutions to difficult high-DoF planning problems in milliseconds where the other a.s.a.o. planners could not consistently find solutions in seconds. This performance was demonstrated both with and without single instruction/multiple data (SIMD) acceleration.
@article{wilson2025aorrtc, title = {{AORRTC}: Almost-Surely Asymptotically Optimal Planning with {RRT}-Connect}, author = {Wilson, Tyler S. and Thomason, Wil and Kingston, Zachary and Gammell, Jonathan D.}, journal = {IEEE Robotics and Automation Letters}, year = {2025}, doi = {10.1109/LRA.2025.3615522}, } - arXiv
pRRTC: GPU-Parallel RRT-Connect for Fast, Consistent, and Low-Cost Motion PlanningUnder ReviewSampling-based motion planning algorithms, like the Rapidly-Exploring Random Tree (RRT) and its widely used variant, RRT-Connect, provide efficient solutions for high-dimensional planning problems faced by real-world robots. However, these methods remain computationally intensive, particularly in complex environments that require many collision checks. As such, to improve performance, recent efforts have explored parallelizing specific components of RRT, such as collision checking or running multiple planners independently, but no prior work has integrated parallelism at multiple levels of the algorithm for robotic manipulation. In this work, we present pRRTC, a GPU-accelerated implementation of RRT-Connect that achieves parallelism across the entire algorithm through multithreaded expansion and connection, SIMT-optimized collision checking, and hierarchical parallelism optimization, improving efficiency, consistency, and initial solution cost. We evaluate the effectiveness of pRRTC on the MotionBenchMaker dataset using robots with 7, 8, and 14 degrees-of-freedom, demonstrating up to 6x average speedup on constrained reaching tasks at high collision checking resolution compared to state-of-the-art. pRRTC also demonstrates a 5x reduction in solution time variance and 1.5x improvement in initial path costs compared to state-of-the-art motion planners in complex environments across all robots.
@misc{huangjadhav2025prrtc, title = {{pRRTC}: {GPU}-Parallel {RRT}-Connect for Fast, Consistent, and Low-Cost Motion Planning}, author = {Huang, Chih H. and Jadhav, Pranav and Plancher, Brian and Kingston, Zachary}, eprint = {2503.06757}, archiveprefix = {arXiv}, primaryclass = {cs.RO}, year = {2025}, note = {Under Review}, } - Nearest-Neighbourless Asymptotically Optimal Motion Planning with Fully Connected Informed Trees (FCIT*)In IEEE International Conference on Robotics and Automation
Improving the performance of motion planning algorithms for high-degree-of-freedom robots usually requires reducing the cost or frequency of computationally expensive operations. Traditionally, and especially for asymptotically optimal sampling-based motion planners, the most expensive operations are local motion validation and querying the nearest neighbours of a configuration. Recent advances have significantly reduced the cost of motion validation by using single instruction/multiple data (SIMD) parallelism to improve solution times for satisficing motion planning problems. These advances have not yet been applied to asymptotically optimal motion planning. This paper presents Fully Connected Informed Trees (FCIT*), the first fully connected, informed, anytime almost-surely asymptotically optimal (ASAO) algorithm. FCIT* exploits the radically reduced cost of edge evaluation via SIMD parallelism to build and search fully connected graphs. This removes the need for nearest-neighbours structures, which are a dominant cost for many sampling-based motion planners, and allows it to find initial solutions faster than state-of-the-art ASAO (VAMP, OMPL) and satisficing (OMPL) algorithms on the MotionBenchMaker dataset while converging towards optimal plans in an anytime manner.
@inproceedings{wilson2025fcit, title = {Nearest-Neighbourless Asymptotically Optimal Motion Planning with Fully Connected Informed Trees ({FCIT}*)}, author = {Wilson, Tyler S. and Thomason, Wil and Kingston, Zachary and Kavraki, Lydia E. and Gammell, Jonathan D.}, booktitle = {IEEE International Conference on Robotics and Automation}, pages = {14140--14146}, year = {2025}, doi = {10.1109/ICRA55743.2025.11127785}, } - Workshop
Faster Behavior Cloning with Hardware-Accelerated Motion PlanningIn IEEE ICRA 2025 Workshop—RoboARCH: Robotics Acceleration with Computing Hardware and Systems@inproceedings{buynitsky2025wksp, title = {Faster Behavior Cloning with Hardware-Accelerated Motion Planning}, author = {Buynitsky, Alexiy and Kingston, Zachary}, booktitle = {IEEE ICRA 2025 Workshop---RoboARCH: Robotics Acceleration with Computing Hardware and Systems}, year = {2025}, }
2024
- Scaling Long-Horizon Online POMDP Planning via Rapid State Space SamplingYuanchu Liang*, Edward Kim*, Wil Thomason*, Zachary Kingston*, Hanna Kurniawati, and Lydia E. KavrakiIn International Symposium of Robotics Research
Partially Observable Markov Decision Processes (POMDPs) are a general and principled framework for motion planning under uncertainty. Despite tremendous improvement in the scalability of POMDP solvers, long-horizon POMDPs (e.g., ≥15 steps) remain difficult to solve. This paper proposes a new approximate online POMDP solver, called Reference-Based Online POMDP Planning via Rapid State Space Sampling (ROP-RaS3). ROP-RaS3 uses novel extremely fast sampling-based motion planning techniques to sample the state space and generate a diverse set of macro actions online which are then used to bias belief-space sampling and infer high-quality policies without requiring exhaustive enumeration of the action space—a fundamental constraint for modern online POMDP solvers. ROP-RaS3 is evaluated on various long-horizon POMDPs, including on a problem with a planning horizon of more than 100 steps and a problem with a 15-dimensional state space that requires more than 20 look ahead steps. In all of these problems, ROP-RaS3 substantially outperforms other state-of-the-art methods by up to multiple folds.
@inproceedings{liang2024ropras, title = {Scaling Long-Horizon Online {POMDP} Planning via Rapid State Space Sampling}, author = {Liang, Yuanchu and Kim, Edward and Thomason, Wil and Kingston, Zachary and Kurniawati, Hanna and Kavraki, Lydia E.}, booktitle = {International Symposium of Robotics Research}, year = {2024}, } - Collision-Affording Point Trees: SIMD-Amenable Nearest Neighbors for Fast Collision CheckingIn Robotics: Science and Systems
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, title = {Collision-Affording Point Trees: SIMD-Amenable Nearest Neighbors for Fast Collision Checking}, author = {Ramsey, Clayton W. and Kingston, Zachary and Thomason, Wil and Kavraki, Lydia E.}, booktitle = {Robotics: Science and Systems}, year = {2024}, doi = {10.15607/RSS.2024.XX.038}, } - Motions in Microseconds via Vectorized Sampling-Based PlanningIn IEEE International Conference on Robotics and Automation
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, title = {Motions in Microseconds via Vectorized Sampling-Based Planning}, author = {Thomason, Wil and Kingston, Zachary and Kavraki, Lydia E.}, booktitle = {IEEE International Conference on Robotics and Automation}, pages = {8749--8756}, year = {2024}, doi = {10.1109/ICRA57147.2024.10611190}, }
2021
- HyperPlan: A Framework for Motion Planning Algorithm Selection and Parameter OptimizationIn IEEE/RSJ International Conference on Intelligent Robots and Systems
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, title = {HyperPlan: A Framework for Motion Planning Algorithm Selection and Parameter Optimization}, author = {Moll, Mark and Chamzas, Constantinos and Kingston, Zachary and Kavraki, Lydia E.}, booktitle = {IEEE/RSJ International Conference on Intelligent Robots and Systems}, pages = {2511--2518}, year = {2021}, doi = {10.1109/IROS51168.2021.9636651}, }
