Sampling-Based Motion Planning on Sequenced Manifolds

We address the problem of planning robot motions in constrained configuration spaces where the constraints change throughout the motion. The problem is formulated as a fixed sequence of intersecting manifolds, which the robot needs to traverse in order to solve the task. We specify a class of sequential motion planning problems that fulfill a particular property of the change in the free configuration space when transitioning between manifolds. For this problem class, the algorithm Planning on Sequenced Manifolds (PSM∗) is developed which searches for optimal intersection points between manifolds by using RRT∗ in an inner loop with a novel steering strategy. We provide a theoretical analysis regarding PSM∗s probabilistic completeness and asymptotic optimality. Further, we evaluate its planning performance on multi-robot object transportation tasks. Video: https://www.youtube.com/watch?v=Q8kbILTRxfU Code: https://github.com/etpr/sequential-manifold-planning

[1]  Swarat Chaudhuri,et al.  Incremental Task and Motion Planning: A Constraint-Based Approach , 2016, Robotics: Science and Systems.

[2]  Alessandro Rinaldo,et al.  Estimating the reach of a manifold , 2017, Electronic Journal of Statistics.

[3]  E. Stein,et al.  Real Analysis: Measure Theory, Integration, and Hilbert Spaces , 2005 .

[4]  Lydia E. Kavraki,et al.  Sampling-Based Methods for Motion Planning with Constraints , 2018, Annu. Rev. Control. Robotics Auton. Syst..

[5]  Thierry Siméon,et al.  Visibility-based probabilistic roadmaps for motion planning , 2000, Adv. Robotics.

[6]  Nathan D. Ratliff.  Multivariate Calculus II: The geometry of smooth maps , 2015 .

[7]  Nicolas Mansard,et al.  HPP: A new software for constrained motion planning , 2016, 2016 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS).

[8]  Emilio Frazzoli,et al.  Sampling-based algorithms for optimal motion planning , 2011, Int. J. Robotics Res..

[9]  Wolfram Burgard,et al.  Modeling and Planning Manipulation in Dynamic Environments , 2019, 2019 International Conference on Robotics and Automation (ICRA).

[10]  Kostas E. Bekris,et al.  Probabilistic Completeness of RRT for Geometric and Kinodynamic Planning With Forward Propagation , 2018, IEEE Robotics and Automation Letters.

[11]  Léonard Jaillet,et al.  Path Planning Under Kinematic Constraints by Rapidly Exploring Manifolds , 2013, IEEE Transactions on Robotics.

[12]  Léonard Jaillet,et al.  Asymptotically-optimal Path Planning on Manifolds , 2012, Robotics: Science and Systems.

[13]  Siddhartha S. Srinivasa,et al.  Manipulation planning on constraint manifolds , 2009, 2009 IEEE International Conference on Robotics and Automation.

[14]  Mike Stilman,et al.  Global Manipulation Planning in Robot Joint Space With Task Constraints , 2010, IEEE Transactions on Robotics.

[15]  Nicholas Roy,et al.  Asymptotically Optimal Planning under Piecewise-Analytic Constraints , 2016, WAFR.

[16]  Lydia E. Kavraki,et al.  Randomized preprocessing of configuration for fast path planning , 1994, Proceedings of the 1994 IEEE International Conference on Robotics and Automation.

[17]  Marc Toussaint.  A Tutorial on Newton Methods for Constrained Trajectory Optimization and Relations to SLAM, Gaussian Process Smoothing, Optimal Control, and Probabilistic Inference , 2017, Geometric and Numerical Foundations of Movements.

[18]  Rachid Alami,et al.  A Hybrid Approach to Intricate Motion, Manipulation and Task Planning , 2009, Int. J. Robotics Res..

[19]  W. Boothby.  An introduction to differentiable manifolds and Riemannian geometry , 1975 .

[20]  Siddhartha S. Srinivasa,et al.  Constellation - An algorithm for finding robot configurations that satisfy multiple constraints , 2012, 2012 IEEE International Conference on Robotics and Automation.

[21]  Frank Chongwoo Park,et al.  Randomized path planning on foliated configuration spaces , 2014, 2014 11th International Conference on Ubiquitous Robots and Ambient Intelligence (URAI).

[22]  Riccardo Bonalli,et al.  Trajectory Optimization on Manifolds: A Theoretically-Guaranteed Embedded Sequential Convex Programming Approach , 2019, Robotics: Science and Systems.

[23]  Gaurav S. Sukhatme,et al.  Multi-step planning for robotic manipulation , 2015, 2015 IEEE International Conference on Robotics and Automation (ICRA).

[24]  Josep M. Porta,et al.  Randomized Kinodynamic Planning for Constrained Systems , 2018, 2018 IEEE International Conference on Robotics and Automation (ICRA).

[25]  S. LaValle.  Rapidly-exploring random trees : a new tool for path planning , 1998 .

[26]  Leslie Pack Kaelbling,et al.  From Skills to Symbols: Learning Symbolic Representations for Abstract High-Level Planning , 2018, J. Artif. Intell. Res..

[27]  Daniel Liberzon,et al.  Calculus of Variations and Optimal Control Theory: A Concise Introduction , 2012 .

[28]  Leslie Pack Kaelbling,et al.  Integrated task and motion planning in belief space , 2013, Int. J. Robotics Res..

[29]  Pieter Abbeel,et al.  Finding Locally Optimal, Collision-Free Trajectories with Sequential Convex Optimization , 2013, Robotics: Science and Systems.

[30]  Lydia E. Kavraki,et al.  Exploring implicit spaces for constrained sampling-based planning , 2019, Int. J. Robotics Res..

[31]  Nancy M. Amato,et al.  A randomized roadmap method for path and manipulation planning , 1996, Proceedings of IEEE International Conference on Robotics and Automation.

[32]  Swarat Chaudhuri,et al.  The Task-Motion Kit: An Open Source, General-Purpose Task and Motion-Planning Framework , 2018, IEEE Robotics & Automation Magazine.

[33]  Mike Stilman,et al.  Task constrained motion planning in robot joint space , 2007, 2007 IEEE/RSJ International Conference on Intelligent Robots and Systems.

[34]  Mark H. Overmars,et al.  A random approach to motion planning , 1992 .

[35]  Lydia E. Kavraki,et al.  Informing Multi-Modal Planning with Synergistic Discrete Leads , 2020, 2020 IEEE International Conference on Robotics and Automation (ICRA).

[36]  Leslie Pack Kaelbling,et al.  A hierarchical approach to manipulation with diverse actions , 2013, 2013 IEEE International Conference on Robotics and Automation.

[37]  Frank Chongwoo Park,et al.  Tangent bundle RRT: A randomized algorithm for constrained motion planning , 2014, Robotica.

[38]  Gaurav S. Sukhatme,et al.  Learning Equality Constraints for Motion Planning on Manifolds , 2020, CoRL.

[39]  Marc Toussaint,et al.  Differentiable Physics and Stable Modes for Tool-Use and Manipulation Planning , 2018, Robotics: Science and Systems.

[40]  Oskar von Stryk,et al.  Direct and indirect methods for trajectory optimization , 1992, Ann. Oper. Res..

[41]  Sachin Chitta,et al.  Motion planning with constraints using configuration space approximations , 2012, 2012 IEEE/RSJ International Conference on Intelligent Robots and Systems.

[42]  Steven M. LaValle,et al.  Planning algorithms , 2006 .

[43]  Thierry Siméon,et al.  Manipulation Planning with Probabilistic Roadmaps , 2004, Int. J. Robotics Res..

[44]  Frank Chongwoo Park,et al.  Tangent space RRT: A randomized planning algorithm on constraint manifolds , 2011, 2011 IEEE International Conference on Robotics and Automation.

[45]  Steven M. LaValle,et al.  RRT-connect: An efficient approach to single-query path planning , 2000, Proceedings 2000 ICRA. Millennium Conference. IEEE International Conference on Robotics and Automation. Symposia Proceedings (Cat. No.00CH37065).

[46]  John M. Lee.  Riemannian Manifolds: An Introduction to Curvature , 1997 .

[47]  Thierry Siméon,et al.  Sampling-Based Motion Planning under Kinematic Loop-Closure Constraints , 2004, WAFR.

[48]  Victor Ng-Thow-Hing,et al.  Randomized multi-modal motion planning for a humanoid robot manipulation task , 2011, Int. J. Robotics Res..

[49]  Marc Toussaint,et al.  Understanding the geometry of workspace obstacles in Motion Optimization , 2015, 2015 IEEE International Conference on Robotics and Automation (ICRA).