This work opens new doors in almost-surely asymptotically optimal (ASAO) planning and shows with SIMD-accelerated edge validation and no nearest neighbors datastructure we can achieve real-time ASAO planning for high-dimensional manipulators!
This work presents an LLM-based natural language-to-specification framework that goes beyond goal reachability and can specify constraints such as action ordering, blocking, and more! We show using constraints is more effective than direct PDDL translation or task decomposition.
This work presents a new method for finding valid configurations of multi-robot teams under complex non-linear constraints (e.g., as in collaborative transport) - handling systems where Newton-Raphson fails!
Geoff, Zak, and Akshaya in front of Akshaya's poster and Akshaya giving her talk.
This work presents AORRTC, an almost-surely asymptotically-optimal variant of RRT-Connect building on the AO-X meta-planner. AORRTC reaches the empirical optimum solution in milliseconds and finds initial solutions as fast as RRT-Connect.
This work presents some initial results on using VAMP to accelerate imitation learning methods, demonstrating significantly faster data generation and training than prior.
This work presents pRRTC, a GPU-parallel implementation of RRT-Connect that achieves the best performance on the hardest problems compared to other CPU/GPU planners.
“Underwater Multi-Robot Simulation and Motion Planning in Angler” with Akshaya Agrawal, Evan Palmer, Zachary Kingston, and Geoffrey A. Hollinger.
This work presents an extension to the underwater simulation framework Angler for multi-robot simulation and motion planning.
Tyler, Alexiy, Pranav, and Chih in front of their posters at the workshop.
Sampling-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.
@inproceedings{huangjadhav2026prrtc,author={Huang, Chih H. and Jadhav, Pranav and Plancher, Brian and Kingston, Zachary},booktitle={IEEE International Conference on Robotics and Automation},note={To Appear},title={{pRRTC}: {GPU}-Parallel {RRT}-Connect for Fast, Consistent, and Low-Cost Motion Planning},year={2026}}
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,author={Wilson, Tyler S. and Thomason, Wil and Kingston, Zachary and Gammell, Jonathan D.},doi={10.1109/LRA.2025.3615522},journal={IEEE Robotics and Automation Letters},title={{AORRTC}: Almost-Surely Asymptotically Optimal Planning with {RRT}-Connect},year={2025}}
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,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},doi={10.1109/ICRA55743.2025.11127785},pages={14140--14146},title={Nearest-Neighbourless Asymptotically Optimal Motion Planning with Fully Connected Informed Trees ({FCIT}*)},year={2025}}
Cooperative manipulation tasks impose various structure-, task-, and robot-specific constraints on mobile manipulators. However, current methods struggle to model and solve these myriad constraints simultaneously. We propose a twofold solution: first, we model constraints as a family of manifolds amenable to simultaneous solving. Second, we introduce the constrained nonlinear Kaczmarz (cNKZ) projection technique to produce constraint-satisfying solutions. Experiments show that cNKZ dramatically outperforms baseline approaches, which cannot find solutions at all. We integrate cNKZ with a sampling-based motion planning algorithm to generate complex, coordinated motions for 3 to 6 mobile manipulators (18–36 DoF), with cNKZ solving up to 80 nonlinear constraints simultaneously and achieving up to a 92% success rate in cluttered environments. We also demonstrate our approach on hardware using three Turtlebot3 Waffle Pi robots with OpenMANIPULATOR-X arms.
@inproceedings{agrawal2025cnkz,author={Agrawal, Akshaya and Mayer, Parker and Kingston, Zachary and Hollinger, Geoffrey A.},booktitle={IEEE International Conference on Robotics and Automation},doi={10.1109/ICRA55743.2025.11127991},pages={7726--7732},title={Constrained Nonlinear {Kaczmarz} Projection on Intersections of Manifolds for Coordinated Multi-Robot Mobile Manipulation},year={2025}}
Large Language Models (LLMs) have demonstrated remarkable ability in long-horizon Task and Motion Planning (TAMP) by translating clear and straightforward natural language problems into formal specifications such as the Planning Domain Definition Language (PDDL). However, real-world problems are often ambiguous and involve many complex constraints. In this paper, we introduce Constraints as Specifications through LLMs (CaStL), a framework that identifies constraints such as goal conditions, action ordering, and action blocking from natural language in multiple stages. CaStL translates these constraints into PDDL and Python scripts, which are solved using an custom PDDL solver. Tested across three PDDL domains, CaStL significantly improves constraint handling and planning success rates from natural language specification in complex scenarios.
@inproceedings{guo2025castl,author={Guo, Weihang and Kingston, Zachary and Kavraki, Lydia E.},booktitle={IEEE International Conference on Robotics and Automation},doi={10.1109/ICRA55743.2025.11127555},pages={11957--11964},title={{CaStL}: Constraints as Specifications through {LLM} Translation for Long-Horizon Task and Motion Planning},year={2025}}
Workshop
Faster Behavior Cloning with Hardware-Accelerated Motion Planning
@misc{buynitsky2025wksp,author={Buynitsky, Alexiy and Kingston, Zachary},booktitle={IEEE ICRA 2025 Workshop---RoboARCH: Robotics Acceleration with Computing Hardware and Systems},title={Faster Behavior Cloning with Hardware-Accelerated Motion Planning},year={2025}}
