A Framework for Manipulation Planning and Execution under Uncertainty in Partially-Known Environments

There is a pressing need to make today’s robots capable, robust, and efficient during real-world operation. This proposal focuses on near-future scenarios that require complex long-horizon reasoning with non-trivial constraints on the robot’s motion. Examples include a robot operating in a warehouse with a partly automated storage and retrieval system or a robot running experiments in an automated laboratory. Several methods in robotics address the challenges of the above scenarios with explicit and carefully crafted planning domains that model how the robot interacts with the environment. Planning domains provide an abstraction over the world that is essential for Task and Motion Planning (TAMP) methods that plan over long horizons, that is, compute executable complex plans that require many steps or have non-monotonic properties, such as rearranging objects on shelves or fetching reactants for an experiment. This proposal will develop interpretable TAMP methods with the capability to deal with increasing uncertainty in the environment, while not sacrificing their strengths and providing a structured framework that allows for a meaningful connection with emerging model-free approaches. The project makes fundamental contributions to the core robotics problem of effective and efficient long-horizon planning under uncertainty by bringing together ideas from motion planning and control under uncertainty, optimization theory, probability and statistics, inverse reinforcement learning, imitation learning, high-dimensional search, factor graphs, probabilistic inference, classical AI planning, and formal methods.

This work has been supported by grant NSF CCF 2336612.

Related Publications

1. Y. Liang, E. Kim, W. Thomason, Z. Kingston, H. Kurniawati, and L. E. Kavraki, “Scaling Long-Horizon Online POMDP Planning via Rapid State Space Sampling,” in Robotics Research, 2025.
   
   pdf publisher bibtex abstract details

   BibTeX

   ×

   @inproceedings{liang2024-scaling,
     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 = {Robotics Research},
     year = {2025},
     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., 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.}
   }
   

   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., 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.
   Details
2. T. Pan, C. K. Verginis, and L. E. Kavraki, “Robust and Safe Task-Driven Planning and Navigation for Heterogeneous Multi-Robot Teams with Uncertain Dynamics,” in IEEE/RSJ International Conference on Intelligent Robots and Systems, 2024.
   
   pdf publisher bibtex abstract details

   BibTeX

   ×

   @inproceedings{pan2024-iros,
     author = {Pan, Tianyang and Verginis, Christos K. and Kavraki, Lydia E.},
     title = {Robust and Safe Task-Driven Planning and Navigation for Heterogeneous Multi-Robot Teams
         with Uncertain Dynamics},
     booktitle = {IEEE/RSJ International Conference on Intelligent Robots and Systems},
     year = {2024},
     abstract = {Task and motion planning (TAMP) can enhance intelligent multi-robot coordination.
         TAMP becomes signifi- cantly more complicated in obstacle-cluttered environments and in the
         presence of robot dynamic uncertainties. We propose a control framework that solves the
         motion-planning problem for multi-robot teams with uncertain dynamics, addressing a key
         component of the TAMP pipeline. The principal part of the proposed algorithm constitutes a
         decentralized feedback control policy for tracking of reference paths taken by the robots
         while avoiding collision and adapting in real time to the underlying dynamic uncertainties.
         The proposed framework further leverages sampling-based motion planners to free the robots
         from local-minimum configurations. Extensive exper- imental results in complex, realistic
         environments illustrate the superior efficiency of the proposed approach, in terms of
         planning time and number of encountered local minima, with respect to state-of-the-art
         baseline methods.}
   }
   

   Abstract

   ×

   Task and motion planning (TAMP) can enhance intelligent multi-robot coordination. TAMP becomes signifi- cantly more complicated in obstacle-cluttered environments and in the presence of robot dynamic uncertainties. We propose a control framework that solves the motion-planning problem for multi-robot teams with uncertain dynamics, addressing a key component of the TAMP pipeline. The principal part of the proposed algorithm constitutes a decentralized feedback control policy for tracking of reference paths taken by the robots while avoiding collision and adapting in real time to the underlying dynamic uncertainties. The proposed framework further leverages sampling-based motion planners to free the robots from local-minimum configurations. Extensive exper- imental results in complex, realistic environments illustrate the superior efficiency of the proposed approach, in terms of planning time and number of encountered local minima, with respect to state-of-the-art baseline methods.
   Details
3. C. W. Ramsey, Z. Kingston, W. Thomason, and L. E. Kavraki, “Collision-Affording Point Trees: SIMD-Amenable Nearest Neighbors for Fast Collision Checking,” in Robotics: Science and Systems, 2024.
   
   pdf publisher bibtex abstract details

   BibTeX

   ×

   @inproceedings{ramsey2024-capt,
     author = {Ramsey, Clayton W. and Kingston, Zachary and Thomason, Wil and Kavraki, Lydia E.},
     booktitle = {Robotics: Science and Systems},
     title = {Collision-Affording Point Trees: SIMD-Amenable Nearest Neighbors for Fast Collision
         Checking},
     year = {2024},
     doi = {10.15607/RSS.2024.XX.038},
     url = {https://roboticsconference.org/2024/program/papers/38/},
     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.}
   }
   

   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.
   Details
4. T. Pan, R. Shome, and L. E. Kavraki, “Task and Motion Planning for Execution in the Real,” IEEE Transactions on Robotics, pp. 1–16, 2024.
   
   pdf publisher bibtex abstract details

   BibTeX

   ×

   @article{pan2024-tamper,
     author = {Pan, Tianyang and Shome, Rahul and Kavraki, Lydia E.},
     journal = {IEEE Transactions on Robotics},
     title = {Task and Motion Planning for Execution in the Real},
     year = {2024},
     volume = {},
     number = {},
     pages = {1-16},
     doi = {10.1109/TRO.2024.3418550},
     abstract = {Task and motion planning represents a powerful set of hybrid planning methods that
         combine reasoning over discrete task domains and continuous motion generation. Traditional
         reasoning necessitates task domain models and enough information to ground actions to motion
         planning queries. Gaps in this knowledge often arise from sources like occlusion or
         imprecise modeling. This work generates task and motion plans that include actions cannot be
         fully grounded at planning time. During execution, such an action is handled by a provided
         human-designed or learned closed-loop behavior. Execution combines offline planned motions
         and online behaviors till reaching the task goal. Failures of behaviors are fed back as
         constraints to find new plans. Forty real-robot trials and motivating demonstrations are
         performed to evaluate the proposed framework and compare against state-of-the-art. Results
         show faster execution time, less number of actions, and more success in problems where
         diverse gaps arise. The experiment data is shared for researchers to simulate these
         settings. The work shows promise in expanding the applicable class of realistic partially
         grounded problems that robots can address.}
   }
   

   Abstract

   ×

   Task and motion planning represents a powerful set of hybrid planning methods that combine reasoning over discrete task domains and continuous motion generation. Traditional reasoning necessitates task domain models and enough information to ground actions to motion planning queries. Gaps in this knowledge often arise from sources like occlusion or imprecise modeling. This work generates task and motion plans that include actions cannot be fully grounded at planning time. During execution, such an action is handled by a provided human-designed or learned closed-loop behavior. Execution combines offline planned motions and online behaviors till reaching the task goal. Failures of behaviors are fed back as constraints to find new plans. Forty real-robot trials and motivating demonstrations are performed to evaluate the proposed framework and compare against state-of-the-art. Results show faster execution time, less number of actions, and more success in problems where diverse gaps arise. The experiment data is shared for researchers to simulate these settings. The work shows promise in expanding the applicable class of realistic partially grounded problems that robots can address.
   Details