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!
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}}
