Interacting with the world imposes constraints on your motion: you open doors about their hinge, pull drawers along their rails, keep glasses of liquid upright so they do not spill, and more. Respecting these constraints while still generating feasible or optimal motion is complex and requires careful consideration in the design of planning algorithms and how the constraints are specified and satisfied. There are also many exciting considerations when there are many constraints that need to be satisfied simultaneously (e.g., a humanoid robot that must open a door, keep a glass upright, and maintain balance, or as shown in the video, a parallel mechanism with many interacting arms) or in sequence (e.g., as shown in the video, Robonaut 2 traversing multiple handrails, opening the hatch, and so on).
2024
arXiv
Constrained Nonlinear Kaczmarz Projection on Intersections of Manifolds for Coordinated Multi-Robot Mobile Manipulation
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},}
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},}
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},}
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},}
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},}
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},}
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},}
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 Operations
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},}
2015
Kinematically Constrained Workspace Control via Linear Optimization
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},}
