Publications
2024
- Scaling Long-Horizon Online POMDP Planning via Rapid State Space SamplingIn International Symposium of Robotics ResearchTo Be Presented
Partially Observable Markov Decision Processes (POMDPs) are a general and principled framework for motion planning under uncertainty. Despite tremendous improvement in the scalability of POMDP solvers, long-horizon POMDPs (e.g., ≥15 steps) remain difficult to solve. This paper proposes a new approximate online POMDP solver, called Reference-Based Online POMDP Planning via Rapid State Space Sampling (ROP-RaS3). ROP-RaS3 uses novel extremely fast sampling-based motion planning techniques to sample the state space and generate a diverse set of macro actions online which are then used to bias belief-space sampling and infer high-quality policies without requiring exhaustive enumeration of the action space—a fundamental constraint for modern online POMDP solvers. ROP-RaS3 is evaluated on various long-horizon POMDPs, including on a problem with a planning horizon of more than 100 steps and a problem with a 15-dimensional state space that requires more than 20 look ahead steps. In all of these problems, ROP-RaS3 substantially outperforms other state-of-the-art methods by up to multiple folds.
@inproceedings{liang2024ropras, title = {Scaling Long-Horizon Online {POMDP} Planning via Rapid State Space Sampling}, author = {Liang, Yuanchu and Kim, Edward and Thomason, Wil and Kingston, Zachary and Kurniawati, Hanna and Kavraki, Lydia E.}, booktitle = {International Symposium of Robotics Research}, year = {2024}, note = {To Be Presented}, } - arXivConstrained Nonlinear Kaczmarz Projection on Intersections of Manifolds for Coordinated Multi-Robot Mobile ManipulationUnder Review
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.
@misc{agrawal2024cnkz, title = {Constrained Nonlinear {Kaczmarz} Projection on Intersections of Manifolds for Coordinated Multi-Robot Mobile Manipulation}, author = {Agrawal, Akshaya and Mayer, Parker and Kingston, Zachary and Hollinger, Geoffrey A.}, year = {2024}, eprint = {2410.21630}, archiveprefix = {arXiv}, primaryclass = {cs.RO}, note = {Under Review}, } - arXivCaStL: Constraints as Specifications through LLM Translation for Long-Horizon Task and Motion PlanningUnder Review
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.
@misc{guo2024castl, title = {{CaStL}: Constraints as Specifications through {LLM} Translation for Long-Horizon Task and Motion Planning}, author = {Guo, Weihang and Kingston, Zachary and Kavraki, Lydia E.}, year = {2024}, eprint = {2410.22225}, archiveprefix = {arXiv}, primaryclass = {cs.RO}, note = {Under Review}, } - AbstractPerception-aware Planning for Robotics: Challenges and OpportunitiesIn 40th Anniversary of the IEEE Conference on Robotics and Automation (ICRA@40)
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.
@inproceedings{meng2024icra40, author = {Meng, Qingxi and Quintero-Peña, Carlos and Kingston, Zachary and Unhelkar, Vaibhav and Kavraki, Lydia E.}, title = {Perception-aware Planning for Robotics: Challenges and Opportunities}, booktitle = {40th Anniversary of the IEEE Conference on Robotics and Automation (ICRA@40)}, year = {2024}, } - Collision-Affording Point Trees: SIMD-Amenable Nearest Neighbors for Fast Collision CheckingIn Robotics: Science and Systems
Motion planning against sensor data is often a critical bottleneck in real-time robot control. For sampling-based motion planners, which are effective for high-dimensional systems such as manipulators, the most time-intensive component is collision checking. We present a novel spatial data structure, the collision-affording point tree (CAPT): an exact representation of point clouds that accelerates collision-checking queries between robots and point clouds by an order of magnitude, with an average query time of less than 10 nanoseconds on 3D scenes comprising thousands of points. With the CAPT, sampling-based planners can generate valid, high-quality paths in under a millisecond, with total end-to-end computation time faster than 60 FPS, on a single thread of a consumer-grade CPU. We also present a point cloud filtering algorithm, based on space-filling curves, which reduces the number of points in a point cloud while preserving structure. Our approach enables robots to plan at real-time speeds in sensed environments, opening up potential uses of planning for high-dimensional systems in dynamic, changing, and unmodeled environments.
@inproceedings{ramsey2024, author = {Ramsey, Clayton W. and Kingston, Zachary and Thomason, Wil and Kavraki, Lydia E.}, title = {Collision-Affording Point Trees: SIMD-Amenable Nearest Neighbors for Fast Collision Checking}, booktitle = {Robotics: Science and Systems}, year = {2024}, doi = {10.15607/RSS.2024.XX.038}, } - Motions in Microseconds via Vectorized Sampling-Based PlanningIn IEEE International Conference on Robotics and Automation
Modern sampling-based motion planning algorithms typically take between hundreds of milliseconds to dozens of seconds to find collision-free motions for high degree-of-freedom problems. This paper presents performance improvements of more than 500x over the state-of-the-art, bringing planning times into the range of microseconds and solution rates into the range of kilohertz, without specialized hardware. Our key insight is how to exploit fine-grained parallelism within sampling-based planners, providing generality-preserving algorithmic improvements to any such planner and significantly accelerating critical subroutines, such as forward kinematics and collision checking. We demonstrate our approach over a diverse set of challenging, realistic problems for complex robots ranging from 7 to 14 degrees-of-freedom. Moreover, we show that our approach does not require high-power hardware by also evaluating on a low-power single-board computer. The planning speeds demonstrated are fast enough to reside in the range of control frequencies and open up new avenues of motion planning research.
@inproceedings{thomason2024vamp, author = {Thomason, Wil and Kingston, Zachary and Kavraki, Lydia E.}, title = {Motions in Microseconds via Vectorized Sampling-Based Planning}, year = {2024}, booktitle = {IEEE International Conference on Robotics and Automation}, pages = {8749--8756}, doi = {10.1109/ICRA57147.2024.10611190}, } - Stochastic Implicit Neural Signed Distance Functions for Safe Motion Planning under Sensing UncertaintyCarlos Quintero-Peña , Wil Thomason , Zachary Kingston , Anastasios Kyrillidis , and Lydia E. KavrakiIn IEEE International Conference on Robotics and Automation
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.
@inproceedings{quintero2024impdist, author = {Quintero-Peña, Carlos and Thomason, Wil and Kingston, Zachary and Kyrillidis, Anastasios and Kavraki, Lydia E.}, title = {Stochastic Implicit Neural Signed Distance Functions for Safe Motion Planning under Sensing Uncertainty}, year = {2024}, booktitle = {IEEE International Conference on Robotics and Automation}, pages = {2360--2367}, doi = {10.1109/ICRA57147.2024.10610773}, } - Accelerating Long-Horizon Planning with Affordance-Directed Dynamic Grounding of Abstract StrategiesIn IEEE International Conference on Robotics and Automation
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.
@inproceedings{elimelech2024skills, author = {Elimelech, Khen and Kingston, Zachary and Thomason, Wil and Vardi, Moshe Y. and Kavraki, Lydia E.}, title = {Accelerating Long-Horizon Planning with Affordance-Directed Dynamic Grounding of Abstract Strategies}, year = {2024}, booktitle = {IEEE International Conference on Robotics and Automation}, pages = {12688--12695}, doi = {10.1109/ICRA57147.2024.10610486}, } - Software Engineering for Robotics: Future Research Directions; Report from the 2023 Workshop on Software Engineering for RoboticsClaire Le Goues , Sebastian Elbaum , David Anthony , Z. Berkay Celik , Mauricio Castillo-Effen , and 5 more authorsInvited Contributor
@misc{Goues2024, title = {Software Engineering for Robotics: Future Research Directions; Report from the 2023 Workshop on Software Engineering for Robotics}, 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}, year = {2024}, note = {Invited Contributor}, } - WorkshopStochastic Implicit Neural Signed Distance Functions for Safe Motion Planning under Sensing UncertaintyCarlos Quintero-Peña , Wil Thomason , Zachary Kingston , Anastasios Kyrillidis , and Lydia E. KavrakiIn IEEE ICRA 2024 Workshop—Back to the Future: Robot Learning Going Probabilistic
@inproceedings{Quintero2024wksp, author = {Quintero-Peña, Carlos and Thomason, Wil and Kingston, Zachary and Kyrillidis, Anastasios and Kavraki, Lydia E.}, title = {Stochastic Implicit Neural Signed Distance Functions for Safe Motion Planning under Sensing Uncertainty}, booktitle = {IEEE ICRA 2024 Workshop---Back to the Future: Robot Learning Going Probabilistic}, year = {2024}, url = {https://arxiv.org/pdf/2309.16862.pdf}, } - WorkshopDynamic Motion Planning from Perception via Accelerated Point Cloud Collision CheckingIn IEEE ICRA 2024 Workshop—Agile Robotics: From Perception to Dynamic Action
@inproceedings{Ramsey2024wksp, author = {Ramsey, Clayton W. and Kingston, Zachary and Thomason, Wil and Kavraki, Lydia E.}, title = {Dynamic Motion Planning from Perception via Accelerated Point Cloud Collision Checking}, booktitle = {IEEE ICRA 2024 Workshop---Agile Robotics: From Perception to Dynamic Action}, year = {2024}, } - WorkshopMonitoring Constraints for Robotic Tutors in Nurse Education: A Motion Planning PerspectiveQingxi Meng* , Carlos Quintero-Peña* , Zachary Kingston , Nicole M. Fontenot , Shannan K. Hamlin , and 2 more authorsIn IEEE ICRA 2024 Workshop—Workshop on Nursing Robotics
@inproceedings{Meng2024wksp, author = {Meng, Qingxi and Quintero-Peña, Carlos and Kingston, Zachary and Fontenot, Nicole M. and Hamlin, Shannan K. and Unhelkar, Vaibhav and Kavraki, Lydia E.}, title = {Monitoring Constraints for Robotic Tutors in Nurse Education: A Motion Planning Perspective}, booktitle = {IEEE ICRA 2024 Workshop---Workshop on Nursing Robotics}, year = {2024}, }
2023
- Robots as AI Double Agents: Privacy in Motion PlanningIn IEEE/RSJ International Conference on Intelligent Robots and Systems
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.
@inproceedings{shome2023privacy, author = {Shome, Rahul and Kingston, Zachary and Kavraki, Lydia E.}, title = {Robots as AI Double Agents: Privacy in Motion Planning}, booktitle = {IEEE/RSJ International Conference on Intelligent Robots and Systems}, year = {2023}, volume = {}, number = {}, pages = {2861-2868}, doi = {10.1109/IROS55552.2023.10341460}, } - Solving Rearrangement Puzzles using Path Defragmentation in Factored State SpacesIEEE Robotics and Automation Letters
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.
@article{bayraktar2023, author = {Bayraktar, S. Bora and Orthey, Andreas and Kingston, Zachary and Toussaint, Marc and Kavraki, Lydia E.}, journal = {IEEE Robotics and Automation Letters}, title = {Solving Rearrangement Puzzles using Path Defragmentation in Factored State Spaces}, year = {2023}, volume = {8}, number = {8}, pages = {4529--4536}, doi = {10.1109/LRA.2023.3282788}, } - Object Reconfiguration with Simulation-Derived Feasible ActionsIn IEEE International Conference on Robotics and Automation
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.
@inproceedings{lee2023physics, title = {Object Reconfiguration with Simulation-Derived Feasible Actions}, author = {Lee, Yiyuan and Thomason, Wil and Kingston, Zachary and Kavraki, Lydia E.}, booktitle = {IEEE International Conference on Robotics and Automation}, year = {2023}, volume = {}, number = {}, pages = {8104-8111}, doi = {10.1109/ICRA48891.2023.10160377}, } - Optimal Grasps and Placements for Task and Motion Planning in ClutterCarlos Quintero-Peña , Zachary Kingston , Tianyang Pan , Rahul Shome , Anastasios Kyrillidis , and 1 more authorIn IEEE International Conference on Robotics and Automation
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.
@inproceedings{quintero2023optimal, title = {Optimal Grasps and Placements for Task and Motion Planning in Clutter}, author = {Quintero-Peña, Carlos and Kingston, Zachary and Pan, Tianyang and Shome, Rahul and Kyrillidis, Anastasios and Kavraki, Lydia E.}, booktitle = {IEEE International Conference on Robotics and Automation}, year = {2023}, volume = {}, number = {}, pages = {3707-3713}, doi = {10.1109/ICRA48891.2023.10161455}, } - Scaling Multimodal Planning: Using Experience and Informing Discrete SearchIEEE Transactions on Robotics
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.
@article{kingston2023tro, author = {Kingston, Zachary and Kavraki, Lydia E.}, journal = {IEEE Transactions on Robotics}, title = {Scaling Multimodal Planning: Using Experience and Informing Discrete Search}, year = {2023}, volume = {39}, number = {1}, pages = {128--146}, doi = {10.1109/TRO.2022.3197080}, }
2022
- Robowflex: Robot Motion Planning with MoveIt Made EasyIn IEEE/RSJ International Conference on Intelligent Robots and Systems
Nominated for Best Paper for Industrial Robotics Research for Practicality.
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.
@inproceedings{kingston2022robowflex, author = {Kingston, Zachary and Kavraki, Lydia E.}, title = {Robowflex: Robot Motion Planning with MoveIt Made Easy}, booktitle = {IEEE/RSJ International Conference on Intelligent Robots and Systems}, year = {2022}, pages = {3108--3114}, doi = {10.1109/IROS47612.2022.9981698}, }
2021
- MotionBenchMaker: A Tool to Generate and Benchmark Motion Planning DatasetsConstantinos Chamzas , Carlos Quintero-Peña , Zachary Kingston , Andreas Orthey , Daniel Rakita , and 3 more authorsIEEE Robotics and Automation Letters
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.
@article{chamzas2021mbm, author = {Chamzas, Constantinos and Quintero-Peña, Carlos and Kingston, Zachary and Orthey, Andreas and Rakita, Daniel and Gleicher, Michael and Toussaint, Marc and Kavraki, Lydia E.}, title = {MotionBenchMaker: A Tool to Generate and Benchmark Motion Planning Datasets}, journal = {IEEE Robotics and Automation Letters}, year = {2021}, volume = {7}, number = {2}, pages = {882--889}, doi = {10.1109/LRA.2021.3133603}, } - Using Experience to Improve Constrained Planning on Foliations for Multi-Modal ProblemsIn IEEE/RSJ International Conference on Intelligent Robots and Systems
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.
@inproceedings{kingston2021, author = {Kingston, Zachary and Chamzas, Constantinos and Kavraki, Lydia E.}, title = {Using Experience to Improve Constrained Planning on Foliations for Multi-Modal Problems}, booktitle = {IEEE/RSJ International Conference on Intelligent Robots and Systems}, year = {2021}, pages = {6922--6927}, doi = {10.1109/IROS51168.2021.9636236}, } - HyperPlan: A Framework for Motion Planning Algorithm Selection and Parameter OptimizationIn IEEE/RSJ International Conference on Intelligent Robots and Systems
Over the years, many motion planning algorithms have been proposed. It is often unclear which algorithm might be best suited for a particular class of problems. The problem is compounded by the fact that algorithm performance can be highly dependent on parameter settings. This paper shows that hyperparameter optimization is an effective tool in both algorithm selection and parameter tuning over a given set of motion planning problems. We present different loss functions for optimization that capture different notions of optimality. The approach is evaluated on a broad range of scenes using two different manipulators, a Fetch and a Baxter. We show that optimized planning algorithm performance significantly improves upon baseline performance and generalizes broadly in the sense that performance improvements carry over to problems that are very different from the ones considered during optimization.
@inproceedings{moll2021hyper, author = {Moll, Mark and Chamzas, Constantinos and Kingston, Zachary and Kavraki, Lydia E.}, title = {HyperPlan: A Framework for Motion Planning Algorithm Selection and Parameter Optimization}, booktitle = {IEEE/RSJ International Conference on Intelligent Robots and Systems}, year = {2021}, pages = {2511-2518}, doi = {10.1109/IROS51168.2021.9636651}, } - Learning Sampling Distributions Using Local 3D Workspace Decompositions for Motion Planning in High DimensionsConstantinos Chamzas , Zachary Kingston , Carlos Quintero-Peña , Anshumali Shrivastava , and Lydia E. KavrakiIn IEEE International Conference on Robotics and Automation
Nominated for Best Paper in Cognitive Robotics.
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.
@inproceedings{chamzas2021flame, author = {Chamzas, Constantinos and Kingston, Zachary and Quintero-Peña, Carlos and Shrivastava, Anshumali and Kavraki, Lydia E.}, title = {Learning Sampling Distributions Using Local 3D Workspace Decompositions for Motion Planning in High Dimensions}, booktitle = {IEEE International Conference on Robotics and Automation}, year = {2021}, doi = {10.1109/ICRA48506.2021.9561104}, pages = {1283--1289}, } - Finite Horizon Synthesis for Probabilistic Manipulation DomainsIn IEEE International Conference on Robotics and Automation
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.
@inproceedings{wells2021icra, author = {Wells, Andrew M. and Kingston, Zachary and Lahijanian, Morteza and Kavraki, Lydia E. and Vardi, Moshe Y.}, title = {Finite Horizon Synthesis for Probabilistic Manipulation Domains}, booktitle = {IEEE International Conference on Robotics and Automation}, year = {2021}, doi = {10.1109/ICRA48506.2021.9561297}, pages = {6336--6342}, }
2020
- Planning Under Manifold Constraints, Encyclopedia of Robotics
@inbook{kingston2020book, chapter = {Planning Under Manifold Constraints}, pages = {1--9}, title = {Encyclopedia of Robotics}, publisher = {Springer Berlin Heidelberg}, year = {2020}, author = {Kingston, Zachary}, editor = {Ang, Marcelo H. and Khatib, Oussama and Siciliano, Bruno}, address = {Berlin, Heidelberg}, isbn = {978-3-642-41610-1}, doi = {10.1007/978-3-642-41610-1_174-1}, } - Informing Multi-Modal Planning with Synergistic Discrete LeadsIn IEEE International Conference on Robotics and Automation
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.
@inproceedings{kingston2020leads, author = {Kingston, Zachary and Wells, Andrew M. and Moll, Mark and Kavraki, Lydia E.}, title = {Informing Multi-Modal Planning with Synergistic Discrete Leads}, booktitle = {IEEE International Conference on Robotics and Automation}, year = {2020}, pages = {3199--3205}, doi = {10.1109/ICRA40945.2020.9197545}, } - Decoupling Constraints from Sampling-Based PlannersIn Robotics Research
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.
@incollection{kingston2020isrr, author = {Kingston, Zachary and Moll, Mark and Kavraki, Lydia E.}, title = {Decoupling Constraints from Sampling-Based Planners}, booktitle = {Robotics Research}, publisher = {Springer International Publishing}, year = {2020}, editor = {Amato, N. M. and Hager, G. and Thomas, S. and Torres-Torriti, M.}, pages = {913--928}, address = {Cham}, isbn = {978-3-030-28619-4}, doi = {10.1007/978-3-030-28619-4_62}, }
2019
- Exploring Implicit Spaces for Constrained Sampling-Based PlanningThe International Journal of Robotics Research
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.
@article{kingston2019imacs, author = {Kingston, Zachary and Moll, Mark and Kavraki, Lydia E.}, title = {Exploring Implicit Spaces for Constrained Sampling-Based Planning}, journal = {The International Journal of Robotics Research}, year = {2019}, volume = {38}, number = {10--11}, pages = {1151--1178}, month = sep, doi = {10.1177/0278364919868530}, }
2018
- Distributed Object Characterization with Local Sensing by a Multi-Robot SystemGolnaz Habibi , Sándor P. Fekete , Zachary Kingston , and James McLurkinIn Distributed Autonomous Robotic Systems
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.
@incollection{habibi2018dars, author = {Habibi, Golnaz and Fekete, S{\'a}ndor P. and Kingston, Zachary and McLurkin, James}, title = {Distributed Object Characterization with Local Sensing by a Multi-Robot System}, booktitle = {Distributed Autonomous Robotic Systems}, publisher = {Springer Proceedings in Advanced Robotics}, year = {2018}, editor = {Gro{\ss}, Roderich and Kolling, Andreas and Berman, Spring and Frazzoli, Emilio and Martinoli, Alcherio and Matsuno, Fumitoshi and Gauci, Melvin}, volume = {6}, pages = {205--218}, doi = {10.1007/978-3-319-73008-0_15}, } - An Incremental Constraint-Based Framework for Task and Motion PlanningThe International Journal of Robotics Research, Special Issue on the 2016 Robotics: Science and Systems Conference
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.
@article{dantam2018tmp, author = {Dantam, Neil T. and Kingston, Zachary and Chaudhuri, Swarat and Kavraki, Lydia E.}, title = {An Incremental Constraint-Based Framework for Task and Motion Planning}, journal = {The International Journal of Robotics Research, Special Issue on the 2016 Robotics: Science and Systems Conference}, year = {2018}, volume = {37}, number = {10}, pages = {1134--1151}, doi = {10.1177/0278364918761570}, } - Sampling-Based Methods for Motion Planning with ConstraintsAnnual Review of Control, Robotics, and Autonomous Systems
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.
@article{kingston2018ar, author = {Kingston, Zachary and Moll, Mark and Kavraki, Lydia E.}, title = {Sampling-Based Methods for Motion Planning with Constraints}, journal = {Annual Review of Control, Robotics, and Autonomous Systems}, year = {2018}, volume = {1}, number = {1}, pages = {159--185}, doi = {10.1146/annurev-control-060117-105226}, }
2017
- Robonaut 2 and You: Specifying and Executing Complex OperationsIn IEEE Workshop on Advanced Robotics and its Social Impacts
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.
@inproceedings{baker2017r2, author = {Baker, William and Kingston, Zachary and Moll, Mark and Badger, Julia and Kavraki, Lydia E.}, title = {Robonaut 2 and You: Specifying and Executing Complex Operations}, booktitle = {IEEE Workshop on Advanced Robotics and its Social Impacts}, year = {2017}, pages = {1--8}, address = {Austin, TX}, month = mar, doi = {10.1109/ARSO.2017.8025204}, }
2016
- Incremental Task and Motion Planning: A Constraint-Based ApproachIn Robotics: Science and Systems
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.
@inproceedings{dantam2016tmp, author = {Dantam, Neil T. and Kingston, Zachary and Chaudhuri, Swarat and Kavraki, Lydia E.}, title = {Incremental Task and Motion Planning: A Constraint-Based Approach}, booktitle = {Robotics: Science and Systems}, year = {2016}, address = {Ann Arbor, MI}, month = jun, doi = {10.15607/RSS.2016.XII.002}, }
2015
- Kinematically Constrained Workspace Control via Linear OptimizationIn IEEE-RAS International Conference on Humanoid Robots
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.
@inproceedings{kingston2015lc3, author = {Kingston, Zachary and Dantam, Neil T. and Kavraki, Lydia E.}, title = {Kinematically Constrained Workspace Control via Linear Optimization}, booktitle = {IEEE-RAS International Conference on Humanoid Robots}, year = {2015}, pages = {758--764}, month = nov, doi = {10.1109/HUMANOIDS.2015.7363455}, } - Pipelined Consensus for Global State Estimation in Multi-Agent SystemsGolnaz Habibi , Zachary Kingston , Zijian Wang , Mac Schwager , and James McLurkinIn Proceedings of the 2015 International Conference on Autonomous Agents and Multiagent Systems
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.
@incollection{habibi2015aamas, author = {Habibi, Golnaz and Kingston, Zachary and Wang, Zijian and Schwager, Mac and McLurkin, James}, title = {Pipelined Consensus for Global State Estimation in Multi-Agent Systems}, booktitle = {Proceedings of the 2015 International Conference on Autonomous Agents and Multiagent Systems}, publisher = {International Foundation for Autonomous Agents and Multiagent Systems}, year = {2015}, pages = {1315--1323}, isbn = {9781450334136}, doi = {10.5555/2772879.2773320}, } - Distributed Centroid Estimation and Motion Controllers for Collective Transport by Multi-Robot SystemsGolnaz Habibi , Zachary Kingston , William Xie , Mathew Jellins , and James McLurkinIn IEEE International Conference on Robotics and Automation
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.
@inproceedings{habibi2015icra, author = {Habibi, Golnaz and Kingston, Zachary and Xie, William and Jellins, Mathew and McLurkin, James}, title = {Distributed Centroid Estimation and Motion Controllers for Collective Transport by Multi-Robot Systems}, booktitle = {IEEE International Conference on Robotics and Automation}, year = {2015}, pages = {1282--1288}, doi = {10.1109/ICRA.2015.7139356}, }