papers.bib

---
---
@string{rss = {Robotics: Science and Systems}}
@string{icra = {IEEE International Conference on Robotics and Automation}}
@string{iros = {IEEE/RSJ International Conference on Intelligent Robots and Systems}}
@string{ral = {IEEE Robotics and Automation Letters}}
@string{ram = {IEEE Robotics and Automation Magazine}}
@string{tro = {IEEE Transactions on Robotics}}
@string{ijrr = {The International Journal of Robotics Research}}

@inproceedings{iyer2026mcvamp,
  abbr      = {IROS},
  abstract  = {Many robot planning tasks require satisfaction of one or more constraints throughout the entire trajectory. For geometric constraints, manifold-constrained motion planning algorithms are capable of planning collision-free path between start and goal configurations on the constraint submanifolds specified by task. Current state-of-the-art methods can take tens of seconds to solve these tasks for complex systems such as humanoid robots, making real-world use impractical, especially in dynamic settings. Inspired by recent advances in hardware accelerated motion planning, we present a CPU SIMD-accelerated manifold-constrained motion planner that revisits projection-based constraint satisfaction through the lens of parallelization. By transforming relevant components into parallelizable structures, we use SIMD parallelism to plan constraint satisfying solutions. Our approach achieves up to 100-1000x speed-ups over the state-of-the-art, making real-time constrained motion planning feasible for the first time. We demonstrate our planner on a real humanoid robot and show real-time whole-body quasi-static plan generation.},
  author    = {Shrutheesh R. Iyer and I-Chia Chang and Andrew Z. Liu and Yan Gu and Zachary Kingston},
  booktitle = iros,
  code      = {https://github.com/CoMMALab/vamp/tree/constrained_planner?tab=readme-ov-file#manifold-constrained-planner},
  note      = {To Appear},
  pdf       = {https://arxiv.org/pdf/2604.13323},
  preview   = {mcvamp_maze.webm},
  projects  = {constraints,realtime},
  title     = {Vectorizing Projection in Manifold-Constrained Motion Planning for Real-Time Whole-Body Control},
  website   = {../papers/mcvamp},
  year      = 2026
}
@inproceedings{yasutake2026hjcdik,
  abbr      = {IROS},
  abstract  = {Inverse 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.},
  author    = {Cael Yasutake and Andrew H. Liu and Zachary Kingston and Brian Plancher},
  booktitle = iros,
  code      = {https://github.com/a2r-lab/HJCD-IK},
  note      = {To Appear},
  pdf       = {https://arxiv.org/abs/2510.07514},
  preview   = {hjcdik.jpg},
  projects  = {realtime,software},
  title     = {{HJCD-IK}: {GPU}-Accelerated Inverse Kinematics through Batched Hybrid Jacobian Coordinate Descent},
  website   = {https://a2r-lab.org/HJCD-IK/},
  year      = 2026
}
@inproceedings{chen2026nehmo,
  abbr      = {IROS},
  abstract  = {Safe multi-arm motion planning is a challenging problem in robotics due to its high dimensionality, coupled configuration space, and complex collision constraints. Centralized planners are capable of coordinating all arms but often face scalability limitations, restricting applicability in real-time settings. On the other hand, decentralized methods are scalable and recent deep learning-based approaches have shown promising results. However, these depend on accurate behavior prediction or coordination protocols and may fail when other arms act unpredictably. To address these challenges, we introduce a neural Hamilton-Jacobi Reachability (HJR) learning-based approach to approximate a safety value function that captures worst-case inter-arm safety constraints. We further develop a decentralized trajectory optimization framework that uses the learned HJR representation for real-time planning. The proposed method is scalable and data-efficient, generalizes across multi-manipulator systems, and outperforms state-of-the-art baselines on challenging multi-arm motion planning tasks. },
  author    = {Qingyi Chen and Zachary Kingston and Ahmed H. Qureshi},
  booktitle = iros,
  code      = {https://github.com/qingyichen/nehmo-dev},
  note      = {To Appear},
  pdf       = {https://arxiv.org/abs/2607.00326},
  preview   = {nehmo.webm},
  projects  = {multi,implicit},
  title     = {{NeHMO}: Neural {Hamilton-Jacobi} Reachability Learning for Decentralized Safe Multi-Agent Motion Planning},
  video     = {https://www.youtube.com/watch?v=IZiePX0p1Mc},
  year      = 2026
}
@inproceedings{gao2026pakr,
  abbr      = {IROS},
  abstract  = {Sampling-based motion planners have been shown to be effective for systems with complex kinodynamic constraints and high dimensionality. However, these algorithms struggle to achieve real-time performance, leading to recent efforts to parallelize planning. While GPU-accelerated planners have achieved significant speedups, existing approaches require specialized CUDA programming that limits accessibility and portability. We present Parallel Asymptotically Optimal Kinodynamic RRT (PAKR), a massively parallel kinodynamic planner leveraging JAX and the XLA compiler to achieve GPU acceleration through standard Python tooling. By combining our parallel planner with the AO-x meta-algorithm, we achieve asymptotic optimality through fast iterative replanning. We provide a theoretical analysis of probabilistic completeness, analyze the effects of batch size and branching factor on convergence, and demonstrate scalability to complex dynamics using the MuJoCo-XLA simulator. Experiments show competitive runtimes with state-of-the-art GPU planners and superior solution quality.},
  author    = {Yitian Gao and Andrew Lu and Zachary Kingston},
  booktitle = iros,
  code      = {https://github.com/CoMMALab/pakr},
  note      = {To Appear},
  pdf       = {https://arxiv.org/abs/2607.03987},
  preview   = {pakr.webm},
  projects  = {realtime},
  title     = {Fast Asymptotically Optimal Kinodynamic Planning via Vectorization},
  video     = {https://youtu.be/EjxSVqkC3sk},
  website   = {../papers/pakr},
  year      = 2026
}
@inproceedings{mao2026cde,
  abbr      = {IROS},
  abstract  = {Intelligent exploration remains a critical challenge in reinforcement learning (RL), especially in visual control tasks. Unlike low-dimensional state-based RL, visual RL must extract task-relevant structure from raw pixels, making exploration inefficient. We propose Concept-Driven Exploration (CDE), which leverages a pre-trained vision-language model (VLM) to generate object-centric visual concepts from textual task descriptions as weak, potentially noisy supervisory signals. Rather than directly conditioning on these noisy signals, CDE trains a policy to reconstruct the concepts via an auxiliary objective, using reconstruction accuracy as an intrinsic reward to guide exploration toward task-relevant objects. Because the policy internalizes these concepts, VLM queries are only needed during training, reducing dependence on external models during deployment. Across five challenging simulated visual manipulation tasks, CDE achieves efficient, targeted exploration and remains robust to noisy VLM predictions. Finally, we demonstrate real-world transfer by deploying CDE on a Franka Research 3 arm, attaining an 80\% success rate in a real-world manipulation task.},
  author    = {Le Mao and Andrew H. Liu and Renos Zabounidis and Yanan Niu and Zachary Kingston and Joseph Campbell},
  booktitle = iros,
  code      = {https://github.com/LeMaoLeMao/Concept-Learning},
  note      = {To Appear},
  pdf       = {https://arxiv.org/abs/2510.08851},
  preview   = {cde.png},
  projects  = {implicit},
  title     = {{CDE}: Concept-Driven Exploration for Reinforcement Learning},
  website   = {https://sites.google.com/view/concept-learn/home},
  year      = 2026
}
@inproceedings{huangjadhav2026prrtc,
  abbr      = {ICRA},
  abstract  = {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.},
  author    = {Chih H. Huang* and Pranav Jadhav* and Brian Plancher and Zachary Kingston},
  booktitle = icra,
  code      = {https://github.com/CoMMALab/pRRTC},
  note      = {To Appear},
  pdf       = {https://arxiv.org/abs/2503.06757},
  preview   = {prrtc.webm},
  projects  = {realtime},
  title     = {{pRRTC}: {GPU}-Parallel {RRT}-Connect for Fast, Consistent, and Low-Cost Motion Planning},
  video     = {https://www.youtube.com/watch?v=okpqftLB6P8},
  website   = {../papers/pRRTC},
  year      = 2026
}
@inproceedings{yang2026pachs,
  abbr      = {ICRA},
  abstract  = {Actor-Critic models are a class of model-free deep reinforcement learning (RL) algorithms that have demonstrated effectiveness across various robot learning tasks. While considerable research has focused on improving training stability and data sampling efficiency, most deployment strategies have remained relatively simplistic, typically relying on direct actor policy rollouts. In contrast, we propose PACHS (Parallel Actor-Critic Heuristic Search), an efficient parallel best-first search algorithm for inference that leverages both components of the actor-critic architecture: the actor network generates actions, while the critic network provides cost-to-go estimates to guide the search. Two levels of parallelism are employed within the search -- actions and cost-to-go estimates are generated in batches by the actor and critic networks respectively, and graph expansion is distributed across multiple threads. We demonstrate the effectiveness of our approach in robotic manipulation tasks, including collision-free motion planning and contact-rich interactions such as non-prehensile pushing.},
  author    = {Hanlan Yang and Itamar Mishani and Luca Pivetti and Zachary Kingston and Maxim Likhachev},
  booktitle = icra,
  note      = {To Appear},
  pdf       = {https://arxiv.org/abs/2509.25402},
  preview   = {pachs.webm},
  projects  = {implicit},
  title     = {Parallel Heuristic Search as Inference for Actor-Critic Reinforcement Learning Models},
  year      = 2026
}
@inproceedings{chen2026spasm,
  abbr      = {ICRA},
  abstract  = {Sequential 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.},
  author    = {Lucas Chen and Shrutheesh R. Iyer and Zachary Kingston},
  booktitle = icra,
  code      = {https://github.com/CoMMALab/SPaSM},
  note      = {To Appear},
  pdf       = {https://arxiv.org/abs/2510.07674},
  preview   = {spasm.webm},
  projects  = {realtime,long-horizon,constraints},
  title     = {Differentiable Particle Optimization for Fast Sequential Manipulation},
  video     = {https://www.youtube.com/watch?v=VK8PYsdXNBk},
  website   = {../papers/spasm},
  year      = 2026
}
@inproceedings{sabbadini2026replan,
  abbr      = {ICRA},
  abstract  = {Robots 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.},
  author    = {Mitchell E. C. Sabbadini and Andrew H. Liu and Joseph Ruan and Tyler S. Wilson and Zachary Kingston and Jonathan D. Gammell},
  booktitle = icra,
  note      = {To Appear},
  pdf       = {https://arxiv.org/abs/2510.21074},
  preview   = {from_scratch.png},
  projects  = {realtime},
  title     = {Revisiting Replanning from Scratch: Real-Time Incremental Planning with Fast Almost-Surely Asymptotically Optimal Planners},
  video     = {https://www.youtube.com/watch?v=XaZrFy8wGZs},
  year      = 2026
}
@inproceedings{patino2026oneshot,
  abbr      = {ICRA},
  abstract  = {Robotic inspection tasks often require constructing high-quality 3D models of objects from a minimal number of views. Traditional next-best view planning (NBVP) approaches incrementally select view poses but fail to account for global optimality of the inspection trajectory, thus leading to inefficient inspection paths. Recent one-shot view planning (OSVP) methods address this challenge by predicting informative view poses from an initial observation. While subsequent improvements on the pioneering OSVP approach attempt to improve prediction accuracy, they can still fail when faced with out of distribution(OoD) examples. With recent advances in generative modeling, OSVP methods can infer a plausible object shape from one observation and then derive the corresponding solution set of view poses. However, because the predicted shape may deviate from the true geometry, these methods can still generate infeasible views. To overcome these limitations, we propose a novel OSVP framework that leverages RGB-D data to generate geometric priors and incorporates online video-based reconstruction. Our method formulates viewpoint selection and path optimization, so that both the calculated poses and the connecting trajectories satisfy visibility constraints, maintain smoothness, and can be locally replanned to compensate for discrepancies between predicted and real object geometries. We validate our OSVP approach through simulation benchmarks against state-of-the-art OSVP techniques and demonstrate its effectiveness on a real Franka Emika manipulator.},
  author    = {Jos\'e J. Pati\~no and Zachary Kingston and Victor Romero-Cano and Yu-Kun Lai and Juan David Hern\'andez},
  booktitle = icra,
  note      = {To Appear},
  pdf       = {https://orca.cardiff.ac.uk/id/eprint/184709/},
  preview   = {oneshot.webm},
  projects  = {implicit},
  title     = {One-shot View Planning and Online Optimization-based Replanning for Unknown Object Reconstruction},
  year      = 2026
}
@article{liang2026ropras3,
  abbr     = {IJRR},
  abstract = {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 remain difficult to solve. To alleviate the difficulty, 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 converges to a near-optimal reference-based solution at a rate that depends on the number of sampled actions, rather than the size of the action space. ROP-RAS3 is evaluated on various long-horizon POMDPs with up to 3000 lookahead steps and 35-dimensional state spaces, where the state, action and observation spaces can be continuous, discrete, or a hybrid of discrete and continuous. Although the reference-based optimal solution may not be the same as the optimal POMDP solution, empirical results indicate that in all of these problems, in terms of success rate, ROP-RAS3 outperforms other state-of-the-art methods by up to multiple folds. We also demonstrate the capability of our approach on a physical robot demonstration. This work extends the theory and empirical results of our ISRR24 paper.},
  author   = {Yuanchu Liang and Edward Kim and J. Arden Knoll and Wil Thomason and Zachary Kingston and Lydia E. Kavraki and Hanna Kurniawati},
  code     = {https://github.com/RDLLab/ROPRAS3},
  doi      = {10.1177/02783649261453546},
  journal  = ijrr,
  pdf      = {https://arxiv.org/abs/2606.04355},
  preview  = {ropras_shelf.webm},
  projects = {realtime},
  title    = {Think Fast and Far: Long-Horizon Online {POMDP} Planning via Rapid State Sampling},
  year     = 2026
}
@article{fuentes2025sensing,
  abbr     = {IJRR},
  abstract = {Passive deformation due to compliance is a commonly used benefit of soft robots, providing opportunities to achieve robust actuation with few active degrees of freedom. Soft growing robots in particular have shown promise in navigation of unstructured environments due to their passive deformation. If their collisions and subsequent deformations can be better understood, soft robots could be used to understand the structure of the environment from direct tactile measurements. In this work, we propose the use of soft growing robots as mapping and exploration tools. We do this by first characterizing collision behavior during discrete turns, then leveraging this model to develop a geometry-based simulator that models robot trajectories in 2D environments. Finally, we demonstrate the model and simulator validity by mapping unknown environments using Monte Carlo sampling to estimate the optimal next deployment given current knowledge. Over both uniform and non-uniform environments, this selection method rapidly approaches ideal actions, showing the potential for soft growing robots in unstructured environment exploration and mapping.},
  author   = {Francesco Fuentes and Serigne Diagne and Zachary Kingston and Laura H. Blumenschein},
  doi      = {10.1177/02783649261448230},
  journal  = ijrr,
  pdf      = {https://arxiv.org/abs/2507.10694},
  preview  = {extero.jpg},
  projects = {softrob},
  title    = {Linking Exteroception and Proprioception through Improved Contact Modeling for Soft Growing Robots},
  year     = 2026
}
@article{gaochen2025actsim,
  abbr     = {Soft Robotics},
  abstract = {Soft growing robots, commonly referred to as vine robots, have demonstrated remarkable ability to interact safely and robustly with unstructured and dynamic environments. It is therefore natural to exploit contact with the environment for planning and design optimization tasks. Previous research has focused on planning under contact for passively deforming robots with pre-formed bends. However, adding active steering to these soft growing robots is necessary for successful navigation in more complex environments. To this end, we develop a unified modeling framework that integrates vine robot growth, bending, actuation, and obstacle contact. We extend the beam moment model to include the effects of actuation on kinematics under growth and then use these models to develop a fast parallel simulation framework. We validate our model and simulator with real robot experiments. To showcase the capabilities of our framework, we apply our model in a design optimization task to find designs for vine robots navigating through cluttered environments, identifying designs that minimize the number of required actuators by exploiting environmental contacts. We show the robustness of the designs to environmental and manufacturing uncertainties. Finally, we fabricate an optimized design and successfully deploy it in an obstacle-rich environment.},
  author   = {Yitian Gao* and Lucas Chen* and Priyanka Bhovad and Sicheng Wang and Zachary Kingston and Laura H. Blumenschein},
  code     = {https://github.com/CoMMALab/ActVineSimPy},
  doi      = {10.1177/21695172261425906},
  journal  = {Soft Robotics},
  pdf      = {https://arxiv.org/abs/2509.15180},
  preview  = {actsim.webm},
  projects = {implicit,softrob},
  title    = {Parallel Simulation of Contact and Actuation for Soft Growing Robots},
  year     = 2026
}
@article{guo2026stac,
  abbr     = {RAL},
  abstract = {Multi-robot motion planning for high degree-of-freedom manipulators in shared, constrained, and narrow spaces is a complex problem and essential for many scenarios such as construction, surgery, and more. Traditional coupled and decoupled methods either scale poorly or lack completeness, and hybrid methods that compose paths from individual robots together require the enumeration of many paths before they can find valid composite solutions. This paper introduces Scheduling to Avoid Collisions (StAC), a hybrid approach that more effectively composes paths from individual robots by scheduling (adding random stops and coordination motion along each path) and generates paths that are more likely to be feasible by using bidirectional feedback between the scheduler and motion planner for informed sampling. StAC uses 10 to 100 times fewer paths from the low-level planner than state-of-the-art baselines on challenging problems in manipulator cases.},
  author   = {Weihang Guo and Zachary Kingston and Kaiyu Hang and Lydia E. Kavraki},
  doi      = {10.1109/LRA.2026.3662639},
  journal  = ral,
  pdf      = {https://arxiv.org/abs/2412.00366},
  preview  = {stac.jpg},
  projects = {constraints,multi},
  title    = {Efficient Multi-Robot Motion Planning for Manifold-Constrained Manipulators by Randomized Scheduling and Informed Path Generation},
  year     = 2026
}
@article{guo2026ompl,
  abbr     = {RAM},
  abstract = {The Open Motion Planning Library (OMPL), first released in 2008, has become a cornerstone of the motion planning community, providing implementations of a wide range of state-of-the-art sampling-based algorithms. Over almost two decades of continuous development, we have steadily expanded the library with new planners, state spaces, and problem formulations. These additions range from asymptotically optimal and lazy planners to constrained motion planning and planning with temporal-logic goals. Building on this foundation, we introduce OMPL 2.0, a major evolution of the library that targets real-time motion planning through hardware acceleration and integrates seamlessly with modern AI research workflows. We also reflect on how OMPL and the field of motion planning have grown together over the years, and discuss the library's broader impact on the research community.},
  author   = {Weihang Guo and Theodoros Tyrovouzis and Emiliano Flores and Clayton W. Ramsey and Zachary Kingston and Ioan A. Şucan and Mark Moll and Lydia E. Kavraki},
  code     = {https://github.com/ompl/ompl},
  journal  = ram,
  note     = {To Appear},
  pdf      = {https://arxiv.org/abs/2605.29301},
  preview  = {ompl.jpg},
  projects = {software},
  title    = {The Open Motion Planning Library 2.0},
  website  = {https://ompl.kavrakilab.org/},
  year     = 2026
}
@article{duong2026akinopdf,
  abbr     = {TRO},
  abstract      = {Motion planning under dynamics constraints, i.e., kinodynamic planning, enables safe robot operation by generating dynamically feasible trajectories that the robot can accurately track. For high-DoF robots such as manipulators, sampling-based motion planners are commonly used, especially for complex tasks in cluttered environments. However, enforcing constraints on robot dynamics in such planners requires solving either challenging two-point boundary value problems (BVPs) or propagating robot dynamics over time, both of which are computational bottlenecks that drastically increase planning times. Meanwhile, recent efforts have shown that sampling-based motion planners can generate plans in microseconds using parallelization, but are limited to geometric paths. This paper develops AkinoPDF, a fast parallelized sampling-based kinodynamic motion planning technique for a broad class of differentially flat robot systems, including manipulators, ground and aerial vehicles, and more. Differential flatness allows us to transform the motion planning problem from the original state space to a flat output space, where an analytical time-parameterized solution of the BVP and dynamics integration can be obtained. A trajectory in the flat output space is then converted back to a closed-form dynamically feasible trajectory in the original state space, enabling fast validation via ``single instruction, multiple data" parallelism. Our method is fast, exact, and compatible with any sampling-based motion planner. We extensively verify the effectiveness of our approach in both simulated benchmarks and real experiments with cluttered and dynamic environments, requiring mere microseconds to milliseconds of planning time.},
  author        = {Thai Duong and Clayton W. Ramsey and Zachary Kingston and Wil Thomason and Lydia E. Kavraki},
  journal  = tro,
  note          = {To Appear},
  pdf           = {https://arxiv.org/abs/2603.16059},
  preview       = {diffflat.webm},
  projects      = {realtime},
  title         = {Ultrafast Sampling-based Kinodynamic Planning via Differential Flatness},
  year          = 2026
}
@article{meng2026look,
  abbr     = {TRO},
  abstract = {In 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.},
  author   = {Qingxi Meng and Emiliano Flores and Carlos Quintero-Peña and Peizhu Qian and Zachary Kingston and Shannan K. Hamlin and Vaibhav Unhelkar and Lydia E. Kavraki},
  journal  = tro,
  note     = {To Appear},
  pdf      = {https://arxiv.org/abs/2509.19610},
  preview  = {lookasyouleap.jpg},
  projects = {implicit,realtime,hri,constraints},
  title    = {Look as You Leap: Planning Simultaneous Motion and Perception for High-{DoF} Robots},
  year     = 2026
}
@misc{bayraktar2026peel,
  abbr          = {ARXIV},
  abstract      = {Long-horizon multi-part object disassembly requires robots to compute feasible sequences of collision-free removal motions, even in the presence of tight, narrow escape corridors. To efficiently solve such disassembly problems, we propose Parallel Extraction for Long-Horizon Disassembly (PEEL), an algorithm which efficiently computes disassembly motions for object assemblies and feeds them to a robot manipulator for execution. PEEL uses sampling-based motion planning to compute single-object motions through the use of a scale-invariant sampling scheme, where the object scale is estimated in a burn-in phase and a subsequent directional sampler exploits the scale. This sampling scheme is integrated into a multi-arm bandit rapidly-exploring random tree (MAB-RRT) planner, which switches between different samplers depending on the reward signal received. Using MAB-RRT, the PEEL algorithm runs a batch of planners in parallel to obtain an ordered graph specifying the sequence in which object parts have to be removed. We show that MAB-RRT can efficiently solve single-part disassemblies with 100 percent success rate on 76 assemblies, and that it is robust to its parameters. By integrating MAB-RRT into PEEL, we solve four long-horizon disassembly problems using the Fetch manipulator robot involving 10 to 17 individual object parts.},
  archiveprefix = {arXiv},
  author        = {Servet B. Bayraktar and Andreas Orthey and Zachary Kingston and Marc Toussaint},
  code          = {https://peel-disassembly.surge.sh/#code},
  eprint        = {2608.08773},
  note          = {Under Review},
  pdf           = {https://arxiv.org/abs/2608.08773},
  preview       = {peel.webm},
  primaryclass  = {cs.RO},
  projects      = {long-horizon},
  title         = {{PEEL}: Parallel Extraction for Long-Horizon Disassembly Planning via Scale-Invariant Sampling},
  video         = {https://peel-disassembly.surge.sh/#videos},
  website       = {https://peel-disassembly.surge.sh/},
  projects      = {long-horizon},
  year          = 2026
}
@misc{bukhari2026meanflow,
  abbr          = {ARXIV},
  abstract      = {Grasp synthesis is a core task in robotic manipulation, for which the solution typically forms a multimodal distribution rather than a point estimate. Generative robotic grasping aims to learn this distribution with deep generative models such as diffusion and flow-based approaches. The iterative nature of such generative models makes them flexible and generalizable; however, multi-step sampling impedes the time-critical operation required in robotics. We devise an approach to fast generative grasping based on MeanFlow on the product Lie group SO(3) x R^3. The training objective couples a purely algebraic semigroup consistency condition with Riemannian Conditional Flow Matching on the product Lie group that anchors the average velocity to the data distribution. The resulting Lie Group-constrained MeanFlow formulation samples reliable grasps in at most 5 network evaluations, matching the grasp generation performance of state-of-the-art diffusion and flow-based models on the ACRONYM dataset at millisecond-scale inference latency (up to 39 times speed-up). We further demonstrate that the approach directly translates to real-world robotic grasping without additional training or domain adaptation, exhibiting robust grasp synthesis under observation noise.},
  archiveprefix = {arXiv},
  author        = {S. Talha Bukhari and Yi Wei and Ruiqi Ni and Zachary Kingston and Aniket Bera},
  eprint        = {2608.26076},
  note          = {Under Review},
  pdf           = {https://arxiv.org/abs/2608.26076},
  preview       = {rmf.png},
  primaryclass  = {cs.RO},
  projects      = {implicit,realtime},
  title         = {Fast Generative Grasping via {L}ie Group-Constrained {MeanFlow}},
  year          = 2026
}
@misc{coumar2026anytime,
  abbr          = {ARXIV},
  abstract      = {Global Tensor Motion Planning (GTMP) solves motion planning with batched tensor operations over a layered multipartite graph. We generalize GTMP so that adjacent-layer edges are realized by any black-box local planner (e.g., linear interpolation, splines, sampling-based planning, trajectory optimization, or generative sampling). We provide two anytime policies on top of this generalization: Anytime GTMP with random restarts at a fixed budget, which covers every homotopy class almost surely, and AO-GTMP with informed expansion with growing budgets, which converges to the optimal cost. We prove that a single sampled graph covers every endpoint-fixed homotopy class admitting a delta-clear representative of bounded length. We also prove that additional samples per layer reduce the per-layer miss probability exponentially, whereas stronger local planners reduce the required layer count only sublinearly. On manipulation benchmarks the method matches state-of-the-art performance, and on 2D navigation it returns batches of topologically diverse solutions, while the informed baselines concentrate on one or two classes.},
  archiveprefix = {arXiv},
  author        = {Sai Coumar and An T. Le and Zachary Kingston},
  eprint        = {2608.25830},
  note          = {Under Review},
  pdf           = {https://arxiv.org/abs/2608.25830},
  preview       = {agtmp.png},
  primaryclass  = {cs.RO},
  projects      = {realtime},
  title         = {Anytime Global Tensor Motion Planning},
  year          = 2026
}
@misc{guo2026planar,
  abbr          = {ARXIV},
  abstract      = {Recent advances in vision-language models (VLMs) have enabled increasing progress in real-world robot manipulation. However, long-horizon manipulation in unstructured environments requires VLMs to reason about changing scene states, action constraints, and execution outcomes, which remains difficult with natural language reasoning alone. We present PLanAR, a planning-language-grounded robot agent framework for open-vocabulary, long-horizon manipulation. PLanAR uses a planning-language interface to define the VLM reasoning space: object predicates represent scene states, action schemas specify robot skills with preconditions and effects, and symbolic plans provide executable intermediate representations. This interface enables stepwise verification: after each action, PLanAR uses onboard observations to check whether the expected symbolic effects have been achieved, allowing the VLM-based agent to update task states, detect failures, and replan when execution deviates from expectation. Across robot embodiments, VLM backends, and tasks including stacking, crossword solving, and long-horizon kitchen workflows, PLanAR demonstrates strong real-world capability while revealing key limitations of current VLMs in embodied reasoning.},
  archiveprefix = {arXiv},
  author        = {Pengyuan Guo and Zhonghao Mai and Zhengtong Xu and Kaidi Zhang and Heng Zhang and Zichen Miao and Arash Ajoudani and Zachary Kingston and Qiang Qiu and Yu She},
  eprint        = {2602.01662},
  note          = {Under Review},
  pdf           = {https://arxiv.org/abs/2602.01662},
  preview       = {agenticlab.webm},
  primaryclass  = {cs.RO},
  projects      = {implicit,long-horizon,hri},
  title         = {{PLanAR}: Planning-Language-Grounded Agentic Reasoning for Robot Manipulation},
  video         = {https://www.youtube.com/watch?v=8nfeQnN27jw},
  website       = {https://agentic1ab.github.io/},
  year          = 2026
}
@article{bukhari2025graspdiff,
  abbr     = {RAL},
  abstract = {Grasp synthesis is a fundamental task in robotic manipulation which usually has multiple feasible solutions. Multimodal grasp synthesis seeks to generate diverse sets of stable grasps conditioned on object geometry, making the robust learning of geometric features crucial for success. To address this challenge, we propose a framework for learning multimodal grasp distributions that leverages variational shape inference to enhance robustness against shape noise and measurement sparsity. Our approach first trains a variational autoencoder for shape inference using implicit neural representations, and then uses these learned geometric features to guide a diffusion model for grasp synthesis on the SE(3) manifold. Additionally, we introduce a test-time grasp optimization technique that can be integrated as a plugin to further enhance grasping performance. Experimental results demonstrate that our shape inference for grasp synthesis formulation outperforms state-of-the-art multimodal grasp synthesis methods on the ACRONYM dataset by 6.3\%, while demonstrating robustness to deterioration in point cloud density compared to other approaches. Furthermore, our trained model achieves zero-shot transfer to real-world manipulation of household objects, generating 34\% more successful grasps than baselines despite measurement noise and point cloud calibration errors.},
  author   = {S. Talha Bukhari and Kaivalya Agrawal and Zachary Kingston and Aniket Bera},
  code     = {https://github.com/stalhabukhari/vsigd},
  doi      = {10.1109/LRA.2025.3645521},
  journal  = ral,
  pdf      = {https://arxiv.org/abs/2508.17482},
  preview  = {graspdiff.webm},
  projects = {implicit},
  title    = {Variational Shape Inference for Grasp Diffusion on SE(3)},
  video    = {https://www.youtube.com/watch?v=YyJPZQgEErM},
  year     = 2025
}
@article{wilson2025aorrtc,
  abbr     = {RAL},
  abstract = {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.},
  author   = {Tyler S. Wilson and Wil Thomason and Zachary Kingston and Jonathan D. Gammell},
  code     = {https://robotic-esp.com/code/aorrtc/},
  doi      = {10.1109/LRA.2025.3615522},
  journal  = ral,
  pdf      = {https://arxiv.org/abs/2505.10542},
  preview  = {aorrtc.jpg},
  projects = {realtime},
  title    = {{AORRTC}: Almost-Surely Asymptotically Optimal Planning with {RRT}-Connect},
  video    = {https://www.youtube.com/watch?v=j1itxP3KuiM},
  year     = 2025
}
@inproceedings{agrawal2025mrangler,
  abbr      = {OCEANS},
  abstract  = {Deploying multi-robot systems in underwater environments is expensive and lengthy; testing algorithms and software in simulation improves development by decoupling software and hardware. However, this requires a simulation framework that closely resembles the real-world. Angler is an open-source framework that simulates low-level communication protocols for an onboard autopilot, such as ArduSub, providing a framework that is close to reality, but unfortunately lacking support for simulating multiple robots. We present an extension to Angler that supports multi-robot simulation and motion planning. Our extension has a modular architecture that creates non-conflicting communication channels between Gazebo, ArduSub Software-in-the-Loop (SITL), and MAVROS to operate multiple robots simultaneously in the same environment. Our multi-robot motion planning module interfaces with cascaded controllers via a JointTrajectory controller in ROS~2. We also provide an integration with the Open Motion Planning Library (OMPL), a collision avoidance module, and tools for procedural environment generation. Our work enables the development and benchmarking of underwater multi-robot motion planning in dynamic environments.},
  address   = {Brest, France},
  author    = {Akshaya Agrawal and Evan Palmer and Zachary Kingston and Geoffrey A. Hollinger},
  booktitle = {IEEE/MTS OCEANS Conference},
  doi       = {10.1109/OCEANS58557.2025.11104649},
  pages     = {1--6},
  pdf       = {https://arxiv.org/abs/2506.06612},
  preview   = {mrangler.jpg},
  projects  = {software,multi},
  title     = {Underwater Multi-Robot Simulation and Motion Planning in Angler},
  year      = 2025
}
@inproceedings{chengao2025diffsim,
  abbr      = {RoboSoft},
  abstract  = {Soft-growing robots (i.e., vine robots) are a promising class of soft robots that allow for navigation and growth in tightly confined environments. However, these robots remain challenging to model and control due to the complex interplay of the inflated structure and inextensible materials, which leads to obstacles for autonomous operation and design optimization. Although there exist simulators for these systems that have achieved qualitative and quantitative success in matching high-level behavior, they still often fail to capture realistic vine robot shapes using simplified parameter models and have difficulties in high-throughput simulation necessary for planning and parameter optimization. We propose a differentiable simulator for these systems, enabling the use of the simulator "in-the-loop" of gradient-based optimization approaches to address the issues listed above. With the more complex parameter fitting made possible by this approach, we experimentally validate and integrate a closed-form nonlinear stiffness model for thin-walled inflated tubes based on a first-principles approach to local material wrinkling. Our simulator also takes advantage of data-parallel operations by leveraging existing differentiable computation frameworks, allowing multiple simultaneous rollouts. We demonstrate the feasibility of using a physics-grounded nonlinear stiffness model within our simulator, and how it can be an effective tool in sim-to-real transfer. We provide our implementation open source.},
  author    = {Lucas Chen* and Yitian Gao* and Sicheng Wang and Francesco Fuentes and Laura H. Blumenschein and Zachary Kingston},
  booktitle = {IEEE-RAS International Conference on Soft Robotics},
  code      = {https://github.com/CoMMALab/DiffVineSimPy},
  doi       = {10.1109/RoboSoft63089.2025.11020809},
  pages     = {1--8},
  pdf       = {https://arxiv.org/abs/2501.17963},
  preview   = {robosoft25.jpg},
  projects  = {software,softrob},
  title     = {Physics-Grounded Differentiable Simulation for Soft Growing Robots},
  year      = 2025
}
@inproceedings{wilson2025fcit,
  abbr      = {ICRA},
  abstract  = {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.},
  author    = {Tyler S. Wilson and Wil Thomason and Zachary Kingston and Lydia E. Kavraki and Jonathan D. Gammell},
  booktitle = icra,
  code      = {https://robotic-esp.com/code/fcitstar/},
  doi       = {10.1109/ICRA55743.2025.11127785},
  pages     = {14140--14146},
  pdf       = {https://arxiv.org/abs/2411.17902},
  preview   = {fcit.webm},
  projects  = {realtime},
  title     = {Nearest-Neighbourless Asymptotically Optimal Motion Planning with Fully Connected Informed Trees ({FCIT}*)},
  video     = {https://www.youtube.com/watch?v=Lb_5Znpcleg},
  year      = 2025
}
@inproceedings{agrawal2025cnkz,
  abbr      = {ICRA},
  abstract  = {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.},
  author    = {Akshaya Agrawal and Parker Mayer and Zachary Kingston and Geoffrey A. Hollinger},
  booktitle = icra,
  code      = {https://github.com/JBVAkshaya/PlanningOnManifoldIntersection},
  doi       = {10.1109/ICRA55743.2025.11127991},
  pages     = {7726--7732},
  pdf       = {https://arxiv.org/abs/2410.21630},
  preview   = {cnkz.jpg},
  projects  = {constraints,multi},
  title     = {Constrained Nonlinear {Kaczmarz} Projection on Intersections of Manifolds for Coordinated Multi-Robot Mobile Manipulation},
  year      = 2025
}
@inproceedings{guo2025castl,
  abbr      = {ICRA},
  abstract  = {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.},
  author    = {Weihang Guo and Zachary Kingston and Lydia E. Kavraki},
  booktitle = icra,
  doi       = {10.1109/ICRA55743.2025.11127555},
  pages     = {11957--11964},
  pdf       = {https://arxiv.org/abs/2410.22225},
  preview   = {castl.webm},
  projects  = {long-horizon,hri},
  title     = {{CaStL}: Constraints as Specifications through {LLM} Translation for Long-Horizon Task and Motion Planning},
  year      = 2025
}
@misc{yan2025vizcoast,
  abbr          = {ARXIV},
  abstract      = {In task and motion planning, high-level task planning is done over an abstraction of the world to enable efficient search in long-horizon robotics problems. However, the feasibility of these task-level plans relies on the downward refinability of the abstraction into continuous motion. When a domain's refinability is poor, task-level plans that appear valid may ultimately fail during motion planning, requiring replanning and resulting in slower overall performance. Prior works mitigate this by encoding refinement issues as constraints to prune infeasible task plans. However, these approaches only add constraints upon refinement failure, expending significant search effort on infeasible branches. We propose VIZ-COAST, a method of leveraging the common-sense spatial reasoning of large pretrained Vision-Language Models to identify issues with downward refinement a priori, bypassing the need to fix these failures during planning. Experiments on two challenging TAMP domains show that our approach is able to extract plausible constraints from images and domain descriptions, drastically reducing planning times and, in some cases, eliminating downward refinement failures altogether, generalizing to a diverse range of instances from the broader domain.},
  archiveprefix = {arXiv},
  author        = {Muyang Yan* and Miras Mengdibayev* and Ardon Floros and Weihang Guo and Lydia E. Kavraki and Zachary Kingston},
  eprint        = {2510.25548},
  note          = {Under Review},
  pdf           = {https://arxiv.org/abs/2510.25548},
  preview       = {viz_coast.webm},
  primaryclass  = {cs.RO},
  projects      = {long-horizon,implicit},
  title         = {Using {VLM} Reasoning to Constrain Task and Motion Planning},
  year          = 2025
}
@misc{coumar2025foam,
  abbr          = {ARXIV},
  abstract      = {Many applications in robotics require primitive spherical geometry, especially in cases where efficient distance queries are necessary. Manual creation of spherical models is time-consuming and prone to errors. This paper presents Foam, a tool to generate spherical approximations of robot geometry from an input Universal Robot Description Format (URDF) file. Foam provides a robust preprocessing pipeline to handle mesh defects and a number of configuration parameters to control the level and approximation of the spherization, and generates an output URDF with collision geometry specified only by spheres. We demonstrate Foam on a number of standard robot models on common tasks, and demonstrate improved collision checking and distance query performance with only a minor loss in fidelity compared to the true collision geometry. We release our tool as an open source Python library and containerized command-line application to facilitate adoption across the robotics community.},
  archiveprefix = {arXiv},
  author        = {Sai Coumar and Gilbert Chang and Nihar Kodkani and Zachary Kingston},
  code          = {https://github.com/CoMMALab/foam},
  eprint        = {2503.13704},
  pdf           = {https://arxiv.org/abs/2503.13704},
  preview       = {foam.jpg},
  primaryclass  = {cs.RO},
  projects      = {software},
  title         = {Foam: A Tool for Spherical Approximation of Robot Geometry},
  year          = 2025
}
@misc{buynitsky2025wksp,
  abbr      = {WKSP},
  author    = {Alexiy Buynitsky and Zachary Kingston},
  booktitle = {IEEE ICRA 2025 Workshop---RoboARCH: Robotics Acceleration with Computing Hardware and Systems},
  pdf       = {https://drive.google.com/file/d/1NBt92zhFCtcCSH7jrKMFAdLFEjc5qxV1/view},
  preview   = {fast_bc.jpg},
  projects  = {realtime,implicit},
  title     = {Faster Behavior Cloning with Hardware-Accelerated Motion Planning},
  year      = 2025
}
@inproceedings{liang2024ropras,
  abbr      = {ISRR},
  abstract  = {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., $\geq$ 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.},
  author    = {Yuanchu Liang* and Edward Kim* and Wil Thomason* and Zachary Kingston* and Hanna Kurniawati and Lydia E. Kavraki},
  booktitle = {International Symposium of Robotics Research},
  pdf       = {https://arxiv.org/abs/2411.07032},
  preview   = {ropras_1.jpg},
  projects  = {realtime},
  title     = {Scaling Long-Horizon Online {POMDP} Planning via Rapid State Space Sampling},
  year      = 2024
}
@misc{meng2024icra40,
  abbr      = {ABS},
  abstract  = {In this work, we argue that new methods are needed to generate robot motion for navigation or manipulation while effectively achieving perception goals. We support our argument by conducting experiments with a simulated robot that must accomplish a primary task, such as manipulation or navigation, while concurrently monitoring an object in the environment. Our preliminary study demonstrates that a decoupled approach fails to achieve high success in either action-focused motion generation or perception goals, motivating further developments of approaches that holistically consider both goals.},
  author    = {Qingxi Meng and Carlos Quintero-Peña and Zachary Kingston and Vaibhav Unhelkar and Lydia E. Kavraki},
  booktitle = {40th Anniversary of the IEEE Conference on Robotics and Automation (ICRA@40)},
  pdf       = {https://kavrakilab.org/publications/meng2024-review.pdf},
  projects  = {implicit,hri},
  title     = {Perception-aware Planning for Robotics: Challenges and Opportunities},
  year      = 2024
}
@inproceedings{ramsey2024,
  abbr      = {RSS},
  abstract  = {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.},
  author    = {Clayton W. Ramsey and Zachary Kingston* and Wil Thomason* and Lydia E. Kavraki},
  blog      = {https://claytonwramsey.com/blog/captree},
  booktitle = rss,
  code      = {https://github.com/kavrakilab/vamp},
  doi       = {10.15607/RSS.2024.XX.038},
  pdf       = {https://arxiv.org/abs/2406.02807},
  preview   = {capt.webm},
  projects  = {realtime,software},
  title     = {Collision-Affording Point Trees: SIMD-Amenable Nearest Neighbors for Fast Collision Checking},
  video     = {https://www.youtube.com/watch?v=BzDKdrU1VpM},
  year      = 2024
}
@inproceedings{thomason2024vamp,
  abbr      = {ICRA},
  abstract  = {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.},
  author    = {Wil Thomason* and Zachary Kingston* and Lydia E. Kavraki},
  booktitle = icra,
  code      = {https://github.com/kavrakilab/vamp},
  doi       = {10.1109/ICRA57147.2024.10611190},
  pages     = {8749--8756},
  pdf       = {https://arxiv.org/abs/2309.14545},
  preview   = {vamp.jpg},
  projects  = {realtime,software},
  title     = {Motions in Microseconds via Vectorized Sampling-Based Planning},
  website   = {https://zkingston.com/vamp-web},
  year      = 2024
}
@inproceedings{quintero2024impdist,
  abbr      = {ICRA},
  abstract  = {Motion planning under sensing uncertainty is critical for robots in unstructured environments to guarantee safety for both the robot and any nearby humans. Most work on planning under uncertainty does not scale to high-dimensional robots such as manipulators, assumes simplified geometry of the robot or environment, or requires per-object knowledge of noise. Instead, we propose a method that directly models sensor-specific aleatoric uncertainty to find safe motions for high-dimensional systems in complex environments, without exact knowledge of environment geometry. We combine a novel implicit neural model of stochastic signed distance functions with a hierarchical optimization-based motion planner to plan low-risk motions without sacrificing path quality. Our method also explicitly bounds the risk of the path, offering trustworthiness. We empirically validate that our method produces safe motions and accurate risk bounds and is safer than baseline approaches.},
  author    = {Carlos Quintero-Peña and Wil Thomason and Zachary Kingston and Anastasios Kyrillidis and Lydia E. Kavraki},
  booktitle = icra,
  doi       = {10.1109/ICRA57147.2024.10610773},
  pages     = {2360--2367},
  pdf       = {https://arxiv.org/pdf/2309.16862.pdf},
  preview   = {impdist.jpg},
  projects  = {implicit},
  title     = {Stochastic Implicit Neural Signed Distance Functions for Safe Motion Planning under Sensing Uncertainty},
  year      = 2024
}
@inproceedings{elimelech2024skills,
  abbr      = {ICRA},
  abstract  = {Long-horizon task planning is important for robot autonomy, especially as a subroutine for frameworks such as Integrated Task and Motion Planning. However, task planning is computationally challenging and struggles to scale to realistic problem settings. We propose to accelerate task planning over an agent's lifetime by integrating learned abstract strategies: a generalizable planning experience encoding introduced in earlier work. In this work, we contribute a practical approach to planning with strategies by introducing a novel formalism of planning in a skill-augmented domain. We also introduce and formulate the notion of a skill's affordance, which indicates its predicted benefit to the solution, and use it to guide the planning and skill grounding processes. Together, our observations yield an affordance-directed, lazy-search planning algorithm, which can seamlessly compose strategies and actions to solve long-horizon planning problems. We evaluate our planner in an object rearrangement domain, where we demonstrate performance benefits relative to a state-of-the-art task planner.},
  author    = {Khen Elimelech and Zachary Kingston and Wil Thomason and Moshe Y. Vardi and Lydia E. Kavraki},
  booktitle = icra,
  doi       = {10.1109/ICRA57147.2024.10610486},
  pages     = {12688--12695},
  pdf       = {http://khen.io/icra24appendix.pdf},
  preview   = {khen_skills.jpg},
  projects  = {long-horizon},
  title     = {Accelerating Long-Horizon Planning with Affordance-Directed Dynamic Grounding of Abstract Strategies},
  year      = 2024
}
@misc{Goues2024,
  author   = {Goues, Claire Le and Elbaum, Sebastian and Anthony, David and Celik, Z. Berkay and Castillo-Effen, Mauricio and Correll, Nikolaus and Jamshidi, Pooyan and Quigley, Morgan and Tabor, Trenton and Zhu, Qi},
  journal  = {arXiv preprint arXiv:2401.12317},
  note     = {Invited Contributor},
  pdf      = {https://arxiv.org/pdf/2401.12317.pdf},
  projects = {software},
  title    = {Software Engineering for Robotics: Future Research Directions; Report from the 2023 Workshop on Software Engineering for Robotics},
  year     = 2024
}
@inproceedings{shome2023privacy,
  abbr      = {IROS},
  abstract  = {Robotics and automation are poised to change the landscape of home and work in the near future. Robots are adept at deliberately moving, sensing, and interacting with their environments. The pervasive use of this technology promises societal and economic payoffs due to its capabilities - conversely, the capabilities of robots to move within and sense the world around them is susceptible to abuse. Robots, unlike typical sensors, are inherently autonomous, active, and deliberate. Such automated agents can become AI double agents liable to violate the privacy of coworkers, privileged spaces, and other stakeholders. In this work we highlight the understudied and inevitable threats to privacy that can be posed by the autonomous, deliberate motions and sensing of robots. We frame the problem within broader sociotechnological questions alongside a comprehensive review. The privacy-aware motion planning problem is formulated in terms of cost functions that can be modified to induce privacy-aware behavior - preserving, agnostic, or violating. Simulated case studies in manipulation and navigation, with altered cost functions, are used to demonstrate how privacy-violating threats can be easily injected, sometimes with only small changes in performance (solution path lengths). Such functionality is already widely available. This preliminary work is meant to lay the foundations for near-future, holistic, interdisciplinary investigations that can address questions surrounding privacy in intelligent robotic behaviors determined by planning algorithms.},
  author    = {Rahul Shome and Zachary Kingston and Lydia E. Kavraki},
  booktitle = iros,
  doi       = {10.1109/IROS55552.2023.10341460},
  number    = {},
  pages     = {2861--2868},
  pdf       = {https://arxiv.org/pdf/2308.03385.pdf},
  preview   = {privacy.jpg},
  projects  = {hri},
  title     = {Robots as AI Double Agents: Privacy in Motion Planning},
  volume    = {},
  year      = 2023
}
@article{bayraktar2023,
  abbr     = {RAL},
  abstract = {Rearrangement puzzles are variations of rearrangement problems in which the elements of a problem are potentially logically linked together. To efficiently solve such puzzles, we develop a motion planning approach based on a new state space that is logically factored, integrating the capabilities of the robot through factors of simultaneously manipulatable joints of an object. Based on this factored state space, we propose less-actions RRT (LA-RRT), a planner which optimizes for a low number of actions to solve a puzzle. At the core of our approach lies a new path defragmentation method, which rearranges and optimizes consecutive edges to minimize action cost. We solve six rearrangement scenarios with a Fetch robot, involving planar table puzzles and an escape room scenario. LA-RRT significantly outperforms the next best asymptotically-optimal planner by 4.01 to 6.58 times improvement in final action cost.},
  author   = {Servet B. Bayraktar and Andreas Orthey and Zachary Kingston and Marc Toussaint and Lydia E. Kavraki},
  doi      = {10.1109/LRA.2023.3282788},
  journal  = ral,
  number   = 8,
  pages    = {4529--4536},
  pdf      = {https://arxiv.org/pdf/2212.02955.pdf},
  preview  = {larrt.jpg},
  projects = {long-horizon},
  title    = {Solving Rearrangement Puzzles using Path Defragmentation in Factored State Spaces},
  volume   = 8,
  year     = 2023
}
@inproceedings{lee2023physics,
  abbr      = {ICRA},
  abstract  = {3D object reconfiguration encompasses common robot manipulation tasks in which a set of objects must be moved through a series of physically feasible state changes into a desired final configuration. Object reconfiguration is challenging to solve in general, as it requires efficient reasoning about environment physics that determine action validity. This information is typically manually encoded in an explicit transition system. Constructing these explicit encodings is tedious and error-prone, and is often a bottleneck for planner use. In this work, we explore embedding a physics simulator within a motion planner to implicitly discover and specify the valid actions from any state, removing the need for manual specification of action semantics. Our experiments demonstrate that the resulting simulation-based planner can effectively produce physically valid rearrangement trajectories for a range of 3D object reconfiguration problems without requiring more than an environment description and start and goal arrangements.},
  author    = {Yiyuan Lee and Wil Thomason and Zachary Kingston and Lydia E. Kavraki},
  booktitle = icra,
  doi       = {10.1109/ICRA48891.2023.10160377},
  number    = {},
  pages     = {8104--8111},
  pdf       = {https://kavrakilab.org/publications/lee2023-simulation-actions.pdf},
  preview   = {physfeas.jpg},
  projects  = {long-horizon,implicit},
  title     = {Object Reconfiguration with Simulation-Derived Feasible Actions},
  volume    = {},
  year      = 2023
}
@inproceedings{quintero2023optimal,
  abbr      = {ICRA},
  abstract  = {Many methods that solve robot planning problems, such as task and motion planners, employ discrete symbolic search to find sequences of valid symbolic actions that are grounded with motion planning. Much of the efficacy of these planners lies in this grounding—bad placement and grasp choices can lead to inefficient planning when a problem has many geometric constraints. Moreover, grounding methods such as naı̈ve sampling often fail to find appropriate values for these choices in the presence of clutter. Towards efficient task and motion planning, we present a novel optimization-based approach for grounding to solve cluttered problems that have many constraints that arise from geometry. Our approach finds an optimal grounding and can provide feedback to discrete search for more effective planning. We demonstrate our method against baseline methods in complex simulated environments.},
  author    = {Carlos Quintero-Peña and Zachary Kingston and Tianyang Pan and Rahul Shome and Anastasios Kyrillidis and Lydia E. Kavraki},
  booktitle = icra,
  doi       = {10.1109/ICRA48891.2023.10161455},
  number    = {},
  pages     = {3707--3713},
  pdf       = {https://kavrakilab.org/publications/quintero2023-optimal-tmp.pdf},
  preview   = {optgrasp.webm},
  projects  = {long-horizon},
  title     = {Optimal Grasps and Placements for Task and Motion Planning in Clutter},
  volume    = {},
  year      = 2023
}
@article{kingston2023tro,
  abbr     = {TRO},
  abstract = {Robotic manipulation is inherently continuous, but typically has an underlying discrete structure, such as if an object is grasped. Many problems like these are multi-modal, such as pick-and-place tasks where every object grasp and placement is a mode. Multi-modal problems require finding a sequence of transitions between modes - for example, a particular sequence of object picks and placements. However, many multi-modal planners fail to scale when motion planning is difficult (e.g., in clutter) or the task has a long horizon (e.g., rearrangement). This work presents solutions for multi-modal scalability in both these areas. For motion planning, we present an experience-based planning framework ALEF which reuses experience from similar modes both online and from training data. For task satisfaction, we present a layered planning approach that uses a discrete lead to bias search towards useful mode transitions, informed by weights over mode transitions. Together, these contributions enable multi-modal planners to tackle complex manipulation tasks that were previously infeasible or inefficient, and provide significant improvements in scenes with high-dimensional robots.},
  author   = {Zachary Kingston and Lydia E. Kavraki},
  doi      = {10.1109/TRO.2022.3197080},
  journal  = tro,
  number   = 1,
  pages    = {128--146},
  pdf      = {http://kavrakilab.org/publications/kingston2022-scaling-mmp.pdf},
  preview  = {r2_walking.jpg},
  projects = {long-horizon,constraints},
  title    = {Scaling Multimodal Planning: Using Experience and Informing Discrete Search},
  video    = {https://player.vimeo.com/video/743110686?loop=1&color=ffffff&byline=0&portrait=0},
  volume   = 39,
  year     = 2023
}
@article{chamzas2021mbm,
  abbr     = {RAL},
  abstract = {Recently, there has been a wealth of development in motion planning for robotic manipulationnew motion planners are continuously proposed, each with its own unique set of strengths and weaknesses. However, evaluating these new planners is challenging, and researchers often create their own ad-hoc problems for benchmarking, which is time-consuming, prone to bias, and does not directly compare against other state-of-the-art planners. We present MotionBenchMaker, an open-source tool to generate benchmarking datasets for realistic robot manipulation problems. MotionBenchMaker is designed to be an extensible, easy-to-use tool that allows users to both generate datasets and benchmark them by comparing motion planning algorithms. Empirically, we show the benefit of using MotionBenchMaker as a tool to procedurally generate datasets which helps in the fair evaluation of planners. We also present a suite of over 40 prefabricated datasets, with 5 different commonly used robots in 8 environments, to serve as a common ground for future motion planning research.},
  author   = {Constantinos Chamzas and Carlos Quintero-Peña and Zachary Kingston and Andreas Orthey and Daniel Rakita and Michael Gleicher and Marc Toussaint and Lydia E. Kavraki},
  code     = {https://github.com/KavrakiLab/motion_bench_maker},
  doi      = {10.1109/LRA.2021.3133603},
  journal  = ral,
  number   = 2,
  pages    = {882--889},
  pdf      = {http://kavrakilab.org/publications/chamzas2022-motion-bench-maker.pdf},
  preview  = {mbm.jpg},
  projects = {software},
  talk     = {https://www.youtube.com/watch?v=dCxXZWGQIlQ},
  title    = {MotionBenchMaker: A Tool to Generate and Benchmark Motion Planning Datasets},
  video    = {https://www.youtube.com/watch?v=t96Py0QX0NI},
  volume   = 7,
  year     = 2021
}
@inproceedings{kingston2022robowflex,
  abbr      = {IROS},
  abstract  = {Robowflex is a software library for robot motion planning in industrial and research applications, leveraging the popular MoveIt library and Robot Operating System (ROS) middleware. Robowflex takes advantage of the ease of motion planning with MoveIt while providing an augmented API to craft and manipulate motion planning queries within a single program. Robowflex's high-level API simplifies many common use-cases while still providing access to the underlying MoveIt library. Robowflex is particularly useful for 1) developing new motion planners, 2) evaluation of motion planners, and 3) complex problems that use motion planning (e.g., task and motion planning). Robowflex also provides visualization capabilities, integrations to other robotics libraries (e.g., DART and Tesseract), and is complimentary to many other robotics packages. With our library, the user does not need to be an expert at ROS or MoveIt in order to set up motion planning queries, extract information from results, and directly interface with a variety of software components. We provide a few example use-cases that demonstrate its efficacy.},
  author    = {Zachary Kingston and Lydia E. Kavraki},
  booktitle = iros,
  code      = {https://github.com/KavrakiLab/robowflex},
  doi       = {10.1109/IROS47612.2022.9981698},
  nominated = {Nominated for Best Paper for Industrial Robotics Research for Practicality.},
  pages     = {3108--3114},
  pdf       = {https://kavrakilab.org/publications/kingston2022-robowflex.pdf},
  preview   = {robowflex.webm},
  projects  = {software},
  talk      = {https://www.youtube.com/watch?v=mPDE3QSkLJ0},
  title     = {Robowflex: Robot Motion Planning with MoveIt Made Easy},
  video     = {https://player.vimeo.com/video/760062092?h=68a0b835cf&badge=0&autopause=0&player_id=0&app_id=58479},
  year      = 2022
}
@inproceedings{kingston2021,
  abbr      = {IROS},
  abstract  = {Many robotic manipulation problems are multi-modal—they consist of a discrete set of mode families (e.g., whether an object is grasped or placed) each with a continuum of parameters (e.g., where exactly an object is grasped). Core to these problems is solving single-mode motion plans, i.e., given a mode from a mode family (e.g., a specific grasp), find a feasible motion to transition to the next desired mode. Many planners for such problems have been proposed, but complex manipulation plans may require prohibitively long computation times due to the difficulty of solving these underlying single-mode problems. It has been shown that using experience from similar planning queries can significantly improve the efficiency of motion planning. However, even though modes from the same family are similar, they impose different constraints on the planning problem, and thus experience gained in one mode cannot be directly applied to another. We present a new experience-based framework, ALEF , for such multi-modal planning problems. ALEF learns using paths from single-mode problems from a mode family, and applies this experience to novel modes from the same family. We evaluate ALEF on a variety of challenging problems and show a significant improvement in the efficiency of sampling-based planners both in isolation and within a multi-modal manipulation planner.},
  author    = {Zachary Kingston and Constantinos Chamzas and Lydia E. Kavraki},
  booktitle = iros,
  doi       = {10.1109/IROS51168.2021.9636236},
  pages     = {6922--6927},
  pdf       = {http://www.kavrakilab.org/publications/kingston2021experience-foliations.pdf},
  preview   = {alef.jpg},
  projects  = {long-horizon,constraints},
  title     = {Using Experience to Improve Constrained Planning on Foliations for Multi-Modal Problems},
  year      = 2021
}
@inproceedings{moll2021hyper,
  abbr      = {IROS},
  abstract  = {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.},
  author    = {Mark Moll and Constantinos Chamzas and Zachary Kingston and Lydia E. Kavraki},
  booktitle = iros,
  code      = {https://github.com/KavrakiLab/hyperplan},
  doi       = {10.1109/IROS51168.2021.9636651},
  pages     = {2511--2518},
  pdf       = {http://kavrakilab.org/publications/moll2021hyperplan.pdf},
  preview   = {hyperplan.jpg},
  projects  = {realtime,software},
  title     = {HyperPlan: A Framework for Motion Planning Algorithm Selection and Parameter Optimization},
  year      = 2021
}
@inproceedings{chamzas2021flame,
  abbr      = {ICRA},
  abstract  = {Earlier work has shown that reusing experience from prior motion planning problems can improve the efficiency of similar, future motion planning queries. However, for robots with many degrees-of-freedom, these methods exhibit poor generalization across different environments and often require large datasets that are impractical to gather. We present SPARK and FLAME, two experience-based frameworks for sampling- based planning applicable to complex manipulators in 3D environments. Both combine samplers associated with features from a workspace decomposition into a global biased sampling distribution. SPARK decomposes the environment based on exact geometry while FLAME is more general, and uses an octree-based decomposition obtained from sensor data. We demonstrate the effectiveness of SPARK and FLAME on a real and simulated Fetch robot tasked with challenging pick-and-place manipulation problems. Our approaches can be trained incrementally and significantly improve performance with only a handful of examples, generalizing better over diverse tasks and environments as compared to prior approaches.},
  author    = {Constantinos Chamzas and Zachary Kingston and Carlos Quintero-Peña and Anshumali Shrivastava and Lydia E. Kavraki},
  booktitle = icra,
  code      = {https://github.com/KavrakiLab/pyre},
  doi       = {10.1109/ICRA48506.2021.9561104},
  nominated = {Nominated for Best Paper in Cognitive Robotics.},
  pages     = {1283--1289},
  pdf       = {http://www.kavrakilab.org/publications/chamzas2021-learn-sampling.pdf},
  preview   = {flame.jpg},
  projects  = {implicit},
  title     = {Learning Sampling Distributions Using Local 3D Workspace Decompositions for Motion Planning in High Dimensions},
  video     = {https://www.youtube.com/watch?v=cH4_lIjjs58},
  year      = 2021
}
@inproceedings{wells2021icra,
  abbr      = {ICRA},
  abstract  = {Robots have begun operating and collaborating with humans in industrial and social settings. This collaboration introduces challenges: the robot must plan while taking the human’s actions into account. In prior work, the problem was posed as a 2-player deterministic game, with a limited number of human moves. The limit on human moves is unintuitive, and in many settings determinism is undesirable. In this paper, we present a novel planning method for collaborative human-robot manipulation tasks via probabilistic synthesis. We introduce a probabilistic manipulation domain that captures the interaction by allowing for both robot and human actions with states that represent the configurations of the objects in the workspace. The task is specified using Linear Temporal Logic over finite traces (LTLf). We then transform our manipulation domain into a Markov Decision Process (MDP) and synthesize an optimal policy to satisfy the specification on this MDP. We present two novel contributions: a formalization of probabilistic manipulation domains allowing us to apply existing techniques and a comparison of different encodings of these domains. Our framework is validated on a physical UR5 robot.},
  author    = {Andrew M. Wells and Zachary Kingston and Morteza Lahijanian and Lydia E. Kavraki and Moshe Y. Vardi},
  booktitle = icra,
  doi       = {10.1109/ICRA48506.2021.9561297},
  pages     = {6336--6342},
  pdf       = {http://www.kavrakilab.org/publications/wells2021-finite-horizon-synthesis.pdf},
  preview   = {finite_synth.png},
  projects  = {long-horizon,hri},
  title     = {Finite Horizon Synthesis for Probabilistic Manipulation Domains},
  year      = 2021
}
@inbook{kingston2020book,
  address   = {Berlin, Heidelberg},
  author    = {Zachary Kingston},
  chapter   = {Planning Under Manifold Constraints},
  doi       = {10.1007/978-3-642-41610-1_174-1},
  editor    = {Marcelo H. Ang and Oussama Khatib and Bruno Siciliano},
  isbn      = {978-3-642-41610-1},
  pages     = {1--9},
  preview   = {constraint.jpg},
  projects  = {constraints},
  publisher = {Springer Berlin Heidelberg},
  title     = {Encyclopedia of Robotics},
  year      = 2020
}
@inproceedings{kingston2020leads,
  abbr      = {ICRA},
  abstract  = {Robotic manipulation problems are inherently continuous, but typically have underlying discrete structure, e.g., whether or not an object is grasped. This means many problems are multi-modal and in particular have a continuous infinity of modes. For example, in a pick-and-place manipulation domain, every grasp and placement of an object is a mode. Usually manipulation problems require the robot to transition into different modes, e.g., going from a mode with an object placed to another mode with the object grasped. To successfully find a manipulation plan, a planner must find a sequence of valid single-mode motions as well as valid transitions between these modes. Many manipulation planners have been proposed to solve tasks with multi-modal structure. However, these methods require mode-specific planners and fail to scale to very cluttered environments or to tasks that require long sequences of transitions. This paper presents a general layered planning approach to multi-modal planning that uses a discrete "lead" to bias search towards useful mode transitions. The difficulty of achieving specific mode transitions is captured online and used to bias search towards more promising sequences of modes. We demonstrate our planner on complex scenes and show that significant performance improvements are tied to both our discrete "lead" and our continuous representation.},
  author    = {Zachary Kingston and Andrew M. Wells and Mark Moll and Lydia E. Kavraki},
  booktitle = icra,
  doi       = {10.1109/ICRA40945.2020.9197545},
  pages     = {3199--3205},
  pdf       = {http://kavrakilab.org/publications/kingston2020weighting-multi-modal-leads.pdf},
  preview   = {mmp.jpg},
  projects  = {long-horizon,constraints},
  title     = {Informing Multi-Modal Planning with Synergistic Discrete Leads},
  video     = {https://player.vimeo.com/video/393316293?loop=1&color=ffffff&byline=0&portrait=0},
  year      = 2020
}
@incollection{kingston2020isrr,
  abbr      = {ISRR},
  abstract  = {We present a general unifying framework for sampling-based motion planning under kinematic task constraints which enables a broad class of planners to compute plans that satisfy a given constraint function that encodes, e.g., loop closure, balance, and end-effector constraints. The framework decouples a planner’s method for exploration from constraint satisfaction by representing the implicit configuration space defined by a constraint function. We emulate three constraint satisfaction methodologies from the literature, and demonstrate the framework with a range of planners utilizing these constraint methodologies. Our results show that the appropriate choice of constrained satisfaction methodology depends on many factors, e.g., the dimension of the configuration space and implicit constraint manifold, and number of obstacles. Furthermore, we show that novel combinations of planners and constraint satisfaction methodologies can be more effective than previous approaches. The framework is also easily extended for novel planners and constraint spaces.},
  address   = {Cham},
  author    = {Zachary Kingston and Mark Moll and Lydia E. Kavraki},
  booktitle = {Robotics Research},
  code      = {https://ompl.kavrakilab.org/constrainedPlanning.html},
  doi       = {10.1007/978-3-030-28619-4_62},
  editor    = {Amato, N. M. and Hager, G. and Thomas, S. and Torres-Torriti, M.},
  isbn      = {978-3-030-28619-4},
  pages     = {913--928},
  pdf       = {http://kavrakilab.org/publications/kingston2017decoupling-constraints.pdf},
  preview   = {isrr_constraint.webm},
  projects  = {constraints},
  publisher = {Springer International Publishing},
  title     = {Decoupling Constraints from Sampling-Based Planners},
  video     = {https://player.vimeo.com/video/261052837?loop=1&color=ffffff&byline=0&portrait=0},
  year      = 2020
}
@article{kingston2019imacs,
  abbr     = {IJRR},
  abstract = {We present a review and reformulation of manifold constrained sampling-based motion planning within a unifying framework, IMACS (implicit manifold configuration space). IMACS enables a broad class of motion planners to plan in the presence of manifold constraints, decoupling the choice of motion planning algorithm and method for constraint adherence into orthogonal choices. We show that implicit configuration spaces defined by constraints can be presented to sampling-based planners by addressing two key fundamental primitives, sampling and local planning, and that IMACS preserves theoretical properties of probabilistic completeness and asymptotic optimality through these primitives. Within IMACS, we implement projection- and continuation-based methods for constraint adherence, and demonstrate the framework on a range of planners with both methods in simulated and realistic scenarios. Our results show that the choice of method for constraint adherence depends on many factors and that novel combinations of planners and methods of constraint adherence can be more effective than previous approaches. Our implementation of IMACS is open source within the Open Motion Planning Library and is easily extended for novel planners and constraint spaces.},
  author   = {Zachary Kingston and Mark Moll and Lydia E. Kavraki},
  code     = {https://ompl.kavrakilab.org/constrainedPlanning.html},
  doi      = {10.1177/0278364919868530},
  journal  = ijrr,
  month    = 9,
  number   = {10--11},
  pages    = {1151--1178},
  pdf      = {http://kavrakilab.org/publications/kingston2019exploring-implicit-spaces-for-constrained.pdf},
  preview  = {parallel.jpg},
  projects = {constraints},
  title    = {Exploring Implicit Spaces for Constrained Sampling-Based Planning},
  volume   = 38,
  year     = 2019
}
@incollection{habibi2018dars,
  abstract  = {This paper presents two distributed algorithms for enabling a swarm of robots with local sensing and local coordinates to estimate the dimensions and orientation of an unknown complex polygonal object, ie, its minimum and maximum width and its main axis. Our first approach is based on a robust heuristic of distributed Principal Component Analysis (DPCA), while the second is based on turning the idea of Rotating Calipers into a distributed algorithm (DRC). We simulate DRC and DPCA methods and test DPCA on real robots. The result show our algorithms successfully estimate the dimension and orientation of convex or concave objects with a reasonable error in the presence of noisy data.},
  author    = {Golnaz Habibi and S{\'a}ndor P. Fekete and Zachary Kingston and James McLurkin},
  booktitle = {Distributed Autonomous Robotic Systems},
  doi       = {10.1007/978-3-319-73008-0_15},
  editor    = {Roderich Gro{\ss} and Andreas Kolling and Spring Berman and Emilio Frazzoli and Alcherio Martinoli and Fumitoshi Matsuno and Melvin Gauci},
  pages     = {205--218},
  pdf       = {https://s3.amazonaws.com/zk-bucket/rsc/Habibi2018.pdf},
  preview   = {swarmchar.webm},
  projects  = {multi},
  publisher = {Springer Proceedings in Advanced Robotics},
  title     = {Distributed Object Characterization with Local Sensing by a Multi-Robot System},
  video     = {https://player.vimeo.com/video/287250201?loop=1&color=ffffff&byline=0&portrait=0},
  volume    = 6,
  year      = 2018
}
@article{dantam2018tmp,
  abbr     = {IJRR},
  abstract = {We present a new algorithm for task and motion planning (TMP) and discuss the requirements and abstrations necessary to obtain robust solutions for TMP in general. Our Iteratively Deepened Task and Motion Planning (IDTMP) method is probabilistically-complete and offers improved performance and generality compared to a similar, state-of-the-art, probabilistically-complete planner. The key idea of IDTMP is to leverage incremental constraint solving to efficiently add and remove constraints on motion feasibility at the task level. We validate IDTMP on a physical manipulator and evaluate scalability on scenarios with many objects and long plans, showing order-of-magnitude gains compared to the benchmark planner and a four-times self-comparison speedup from our extensions. Finally, in addition to describing a new method for TMP and its implementation on a physical robot, we also put forward requirements and abstractions for the development of similar planners in the future.},
  author   = {Neil T. Dantam and Zachary Kingston and Swarat Chaudhuri and Lydia E. Kavraki},
  code     = {http://tmkit.kavrakilab.org/},
  doi      = {10.1177/0278364918761570},
  journal  = {The International Journal of Robotics Research, Special Issue on the 2016 Robotics: Science and Systems Conference},
  number   = 10,
  pages    = {1134--1151},
  pdf      = {http://kavrakilab.org/publications/dantam2018incremental-tmp.pdf},
  preview  = {idtmp_deep.webm},
  projects = {long-horizon},
  title    = {An Incremental Constraint-Based Framework for Task and Motion Planning},
  volume   = 37,
  year     = 2018
}
@article{kingston2018ar,
  abbr     = {AR},
  abstract = {Robots with many degrees of freedom (e.g., humanoid robots and mobile manipulators) have increasingly been employed to accomplish realistic tasks in domains such as disaster relief, spacecraft logistics, and home caretaking. Finding feasible motions for these robots autonomously is essential for their operation. Sampling-based motion planning algorithms have been shown to be effective for these high-dimensional systems. However, incorporating task constraints (e.g., keeping a cup level, writing on a board) into the planning process introduces significant challenges. is survey describes the families of methods for sampling-based planning with constraints and places them on a spectrum delineated by their complexity. Constrained sampling-based methods are based upon two core primitive operations: (1) sampling constraint-satisfying configurations and (2) generating constraint-satisfying continuous motion. Although the basics of sampling-based planning are presented for contextual background, the survey focuses on the representation of constraints and sampling-based planners that incorporate constraints.},
  author   = {Zachary Kingston and Mark Moll and Lydia E. Kavraki},
  doi      = {10.1146/annurev-control-060117-105226},
  journal  = {Annual Review of Control, Robotics, and Autonomous Systems},
  number   = 1,
  pages    = {159--185},
  pdf      = {http://kavrakilab.org/publications/kingston2018sampling-based-methods-for-motion-planning.pdf},
  preview  = {r2_tasks.jpg},
  projects = {constraints},
  title    = {Sampling-Based Methods for Motion Planning with Constraints},
  volume   = 1,
  year     = 2018
}
@inproceedings{baker2017r2,
  abstract  = {Crew time is a precious resource due to the expense of trained human operators in space. Efficient caretaker robots could lessen the manual labor load required by frequent vehicular and life support maintenance tasks, freeing astronaut time for scientific mission objectives. Humanoid robots can fluidly exist alongside human counterparts due to their form, but they are complex and high-dimensional platforms. This paper describes a system that human operators can use to maneuver Robonaut 2 (R2), a dexterous humanoid robot developed by NASA to research co-robotic applications. The system includes a specification of constraints used to describe operations, and the supporting planning framework that solves constrained problems on R2 at interactive speeds. The paper is developed in reference to an illustrative, typical example of an operation R2 performs to highlight the challenges inherent to the problems R2 must face. Finally, the interface and planner is validated through a case-study using the guiding example on the physical robot in a simulated microgravity environment. This work reveals the complexity of employing humanoid caretaker robots and suggest solutions that are broadly applicable.},
  address   = {Austin, TX},
  author    = {William Baker and Zachary Kingston and Mark Moll and Julia Badger and Lydia E. Kavraki},
  booktitle = {IEEE Workshop on Advanced Robotics and its Social Impacts},
  doi       = {10.1109/ARSO.2017.8025204},
  month     = {March},
  pages     = {1--8},
  pdf       = {http://kavrakilab.org/publications/baker2017robonaut-2-and-you.pdf},
  preview   = {r2you.jpg},
  projects  = {constraints,software},
  title     = {Robonaut 2 and You: Specifying and Executing Complex Operations},
  video     = {https://player.vimeo.com/video/287250170?loop=1&color=ffffff&byline=0&portrait=0},
  year      = 2017
}
@inproceedings{dantam2016tmp,
  abbr      = {RSS},
  abstract  = {We present a new algorithm for task and motion planning (TMP) and discuss the requirements and abstractions necessary to obtain robust solutions for TMP in general. Our Iteratively Deepened Task and Motion Planning (IDTMP) method is probabilistically-complete and offers improved performance and generality compared to a similar, state-of-the-art, probabilistically-complete planner. The key idea of IDTMP is to leverage incremental constraint solving to efficiently add and remove constraints on motion feasibility at the task level. We validate IDTMP on a physical manipulator and evaluate scalability on scenarios with many objects and long plans, showing order-of-magnitude gains compared to the benchmark planner and a four-times self-comparison speedup from our extensions. Finally, in addition to describing a new method for TMP and its implementation on a physical robot, we also put forward requirements and abstractions for the development of similar planners in the future.},
  address   = {Ann Arbor, MI},
  author    = {Neil T. Dantam and Zachary Kingston and Swarat Chaudhuri and Lydia E. Kavraki},
  booktitle = rss,
  code      = {http://tmkit.kavrakilab.org/},
  doi       = {10.15607/RSS.2016.XII.002},
  month     = {June},
  pdf       = {http://kavrakilab.org/publications/dantam2016tmp.pdf},
  preview   = {idtmp.jpg},
  projects  = {long-horizon},
  talk      = {https://youtu.be/9EcOJ8mF5JE?si=9uhdZpqSHZh25oIS},
  title     = {Incremental Task and Motion Planning: A Constraint-Based Approach},
  video     = {https://youtu.be/QHCuD0tOdfY?si=dCfjMJHFuLzxMNom},
  year      = 2016
}
@inproceedings{kingston2015lc3,
  abbr      = {HUMANOIDS},
  abstract  = {We present a method for Cartesian workspace control of a robot manipulator that enforces joint-level acceleration, velocity, and position constraints using linear optimization. This method is robust to kinematic singularities. On redundant manipulators, we avoid poor configurations near joint limits by including a maximum permissible velocity term to center each joint within its limits. Compared to the baseline Jacobian damped least-squares method of workspace control, this new approach honors kinematic limits, ensuring physically realizable control inputs and providing smoother motion of the robot. We demonstrate our method on simulated redundant and non-redundant manipulators and implement it on the physical 7-degree-of-freedom Baxter manipulator. We provide our control software under a permissive license.},
  author    = {Zachary Kingston and Neil T. Dantam and Lydia E. Kavraki},
  booktitle = {IEEE-RAS International Conference on Humanoid Robots},
  code      = {http://amino.dyalab.org},
  doi       = {10.1109/HUMANOIDS.2015.7363455},
  month     = {Nov},
  pages     = {758--764},
  pdf       = {http://kavrakilab.org/publications/kingston2015lc3.pdf},
  preview   = {lc3.jpg},
  projects  = {constraints},
  title     = {Kinematically Constrained Workspace Control via Linear Optimization},
  video     = {https://youtu.be/Jl6AmQLjT8w?si=h7PrdHmonGGxw_We},
  year      = 2015
}
@incollection{habibi2015aamas,
  abbr      = {AAMAS},
  abstract  = {This paper presents pipelined consensus, an extension of pair-wise gossip-based consensus, for multi-agent systems using mesh networks. Each agent starts a new consensus in each round of gossiping, and stores the intermediate results for the previous k consensus in a pipeline message. After k rounds of gossiping, the results of the first consensus are ready. The pipeline keeps each consensus independent, so any errors only persist for k rounds. This makes pipelined consensus robust to many real-world problems that other algorithms cannot handle, including message loss, changes in network topology, sensor variance, and changes in agent population. The algorithm is fully distributed and self-stabilizing, and uses a communication message of fixed size. We demonstrate the efficiency of pipelined consensus in two scenarios: computing mean sensor values in a distributed sensor network, and computing a centroid estimate in a multi-robot system. We provide extensive simulation results, and real-world experiments with up to 24 agents. The algorithm produces accurate results, and handles all of the disturbances mentioned above.},
  author    = {Golnaz Habibi and Zachary Kingston and Zijian Wang and Mac Schwager and James McLurkin},
  booktitle = {Proceedings of the 2015 International Conference on Autonomous Agents and Multiagent Systems},
  doi       = {10.5555/2772879.2773320},
  isbn      = 9781450334136,
  pages     = {1315--1323},
  pdf       = {https://zkingston.com/papers/habibi2015aamas.pdf},
  preview   = {pipeline.png},
  projects  = {multi},
  publisher = {International Foundation for Autonomous Agents and Multiagent Systems},
  title     = {Pipelined Consensus for Global State Estimation in Multi-Agent Systems},
  year      = 2015
}
@inproceedings{habibi2015icra,
  abbr      = {ICRA},
  abstract  = {This paper presents four distributed motion controllers to enable a group of robots to collectively transport an object towards a guide robot. These controllers include: rotation around a pivot robot, rotation in-place around an estimated centroid of the object, translation, and a combined motion of rotation and translation in which each manipulating robot follows a trochoid path. Three of these controllers require an estimate of the centroid of the object, to use as the axis of rotation. Assuming the object is surrounded by manipulator robots, we approximate the centroid of the object by measuring the centroid of the manipulating robots. Our algorithms and controllers are fully distributed and robust to changes in network topology, robot population, and sensor error. We tested all of the algorithms in real-world environments with 9 robots, and show that the error of the centroid estimation is low, and that all four controllers produce reliable motion of the object.},
  author    = {Golnaz Habibi and Zachary Kingston and William Xie and Mathew Jellins and James McLurkin},
  booktitle = icra,
  doi       = {10.1109/ICRA.2015.7139356},
  pages     = {1282--1288},
  pdf       = {https://zkingston.com/papers/habibi2015icra.pdf},
  preview   = {swarmtrans.webm},
  projects  = {multi},
  title     = {Distributed Centroid Estimation and Motion Controllers for Collective Transport by Multi-Robot Systems},
  video     = {https://player.vimeo.com/video/287250199?loop=1&color=ffffff&byline=0&portrait=0},
  year      = 2015
}