Kinodynamic Planning on Constraint Manifolds

This paper presents a motion planner for systems subject to kinematic and dynamic constraints. The former appear when kinematic loops are present in the system, such as in parallel manipulators, in robots that cooperate to achieve a given task, or in situations involving contacts with the environment. The latter are necessary to obtain realistic trajectories, taking into account the forces acting on the system. The kinematic constraints make the state space become an implicitly-defined manifold, which complicates the application of common motion planning techniques. To address this issue, the planner constructs an atlas of the state space manifold incrementally, and uses this atlas both to generate random states and to dynamically simulate the steering of the system towards such states. The resulting tools are then exploited to construct a rapidly-exploring random tree (RRT) over the state space. To the best of our knowledge, this is the first randomized kinodynamic planner for implicitly-defined state spaces. The test cases presented in this paper validate the approach in significantly-complex systems.

[1]  Josep M. Porta,et al.  Planning Singularity-Free Paths on Closed-Chain Manipulators , 2013, IEEE Transactions on Robotics.

[2]  J. Marsden,et al.  Discrete mechanics and variational integrators , 2001, Acta Numerica.

[3]  Lydia E. Kavraki,et al.  Discrete Search Leading Continuous Exploration for Kinodynamic Motion Planning , 2007, Robotics: Science and Systems.

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

[5]  John F. Canny,et al.  Some algebraic and geometric computations in PSPACE , 1988, STOC '88.

[6]  Pieter Abbeel,et al.  EG-RRT: Environment-guided random trees for kinodynamic motion planning with uncertainty and obstacles , 2011, 2011 IEEE/RSJ International Conference on Intelligent Robots and Systems.

[7]  B. Faverjon,et al.  Probabilistic Roadmaps for Path Planning in High-Dimensional Con(cid:12)guration Spaces , 1996 .

[8]  Yoshihiko Nakamura,et al.  Admissible velocity propagation: Beyond quasi-static path planning for high-dimensional robots , 2017, Int. J. Robotics Res..

[9]  Lydia E. Kavraki,et al.  Path planning using lazy PRM , 2000, Proceedings 2000 ICRA. Millennium Conference. IEEE International Conference on Robotics and Automation. Symposia Proceedings (Cat. No.00CH37065).

[10]  Kevin M. Lynch,et al.  Stable Pushing: Mechanics, Controllability, and Planning , 1995, Int. J. Robotics Res..

[11]  Scott Kuindersma,et al.  Optimization and stabilization of trajectories for constrained dynamical systems , 2016, 2016 IEEE International Conference on Robotics and Automation (ICRA).

[12]  W. Blajer Elimination of Constraint Violation and Accuracy Aspects in Numerical Simulation of Multibody Systems , 2002 .

[13]  Jean-Claude Latombe,et al.  Nonholonomic multibody mobile robots: Controllability and motion planning in the presence of obstacles , 2005, Algorithmica.

[14]  Steven M. LaValle,et al.  Resolution complete rapidly-exploring random trees , 2002, Proceedings 2002 IEEE International Conference on Robotics and Automation (Cat. No.02CH37292).

[15]  John H. Reif,et al.  Complexity of the mover's problem and generalizations , 1979, 20th Annual Symposium on Foundations of Computer Science (sfcs 1979).

[16]  Lydia E. Kavraki,et al.  Fast Tree-Based Exploration of State Space for Robots with Dynamics , 2004, WAFR.

[17]  D. J. Braun,et al.  Eliminating Constraint Drift in the Numerical Simulation of Constrained Dynamical Systems , 2009 .

[18]  Kris Hauser,et al.  Asymptotically Optimal Planning by Feasible Kinodynamic Planning in a State–Cost Space , 2015, IEEE Transactions on Robotics.

[19]  G. Swaminathan Robot Motion Planning , 2006 .

[20]  F. Potra,et al.  Implicit Numerical Integration for Euler-Lagrange Equations via Tangent Space Parametrization∗ , 1991 .

[21]  Linda R. Petzold,et al.  Numerical solution of differential-algebraic equations in mechanical systems simulation , 1992 .

[22]  J. Baumgarte Stabilization of constraints and integrals of motion in dynamical systems , 1972 .

[23]  O. Bauchau,et al.  Review of Classical Approaches for Constraint Enforcement in Multibody Systems , 2008 .

[24]  John M. Lee Introduction to Smooth Manifolds , 2002 .

[25]  Moëz Cherif Kinodynamic motion planning for all-terrain wheeled vehicles , 1999, Proceedings 1999 IEEE International Conference on Robotics and Automation (Cat. No.99CH36288C).

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

[27]  Steven M. LaValle,et al.  Reducing metric sensitivity in randomized trajectory design , 2001, Proceedings 2001 IEEE/RSJ International Conference on Intelligent Robots and Systems. Expanding the Societal Role of Robotics in the the Next Millennium (Cat. No.01CH37180).

[28]  Pieter Abbeel,et al.  Motion planning with sequential convex optimization and convex collision checking , 2014, Int. J. Robotics Res..

[29]  Steven Dubowsky,et al.  Optimal Dynamic Trajectories for Robotic Manipulators , 1985 .

[30]  Siddhartha S. Srinivasa,et al.  CHOMP: Covariant Hamiltonian optimization for motion planning , 2013, Int. J. Robotics Res..

[31]  Kostas E. Bekris,et al.  Asymptotically optimal sampling-based kinodynamic planning , 2014, Int. J. Robotics Res..

[32]  Michael E. Henderson,et al.  Multiple Parameter Continuation: Computing Implicitly Defined k-Manifolds , 2002, Int. J. Bifurc. Chaos.

[33]  Ahmad A. Masoud,et al.  Kinodynamic Motion Planning , 2010, IEEE Robotics & Automation Magazine.

[34]  O. Bauchau,et al.  Review of Contemporary Approaches for Constraint Enforcement in Multibody Systems , 2008 .

[35]  Stefan Schaal,et al.  STOMP: Stochastic trajectory optimization for motion planning , 2011, 2011 IEEE International Conference on Robotics and Automation.

[36]  Todd D. Murphey,et al.  Scalable Variational Integrators for Constrained Mechanical Systems in Generalized Coordinates , 2009, IEEE Transactions on Robotics.

[37]  Steven M. LaValle,et al.  Randomized Kinodynamic Planning , 1999, Proceedings 1999 IEEE International Conference on Robotics and Automation (Cat. No.99CH36288C).

[38]  Simon Parsons,et al.  Principles of Robot Motion: Theory, Algorithms and Implementations by Howie Choset, Kevin M. Lynch, Seth Hutchinson, George Kantor, Wolfram Burgard, Lydia E. Kavraki and Sebastian Thrun, 603 pp., $60.00, ISBN 0-262-033275 , 2007, The Knowledge Engineering Review.

[39]  Matthew R. Walter,et al.  Reachability-guided sampling for planning under differential constraints , 2009, 2009 IEEE International Conference on Robotics and Automation.

[40]  Siddhartha S. Srinivasa,et al.  DART: Dynamic Animation and Robotics Toolkit , 2018, J. Open Source Softw..

[41]  John T. Betts,et al.  Practical Methods for Optimal Control and Estimation Using Nonlinear Programming , 2009 .

[42]  Léonard Jaillet,et al.  The CUIK Suite: Analyzing the Motion Closed-Chain Multibody Systems , 2014, IEEE Robotics & Automation Magazine.

[43]  Ilian A. Bonev,et al.  Minimum-Time Trajectory Planning and Control of a Pick-and-Place Five-Bar Parallel Robot , 2015, IEEE/ASME Transactions on Mechatronics.

[44]  E. Hairer,et al.  Geometric Numerical Integration: Structure Preserving Algorithms for Ordinary Differential Equations , 2004 .

[45]  M. Kalisiak,et al.  Toward More Efficient Motion Planning with Differential Constraints , 2008 .

[46]  Lydia E. Kavraki,et al.  A General Algorithm for Time-Optimal Trajectory Generation Subject to Minimum and Maximum Constraints , 2016, WAFR.

[47]  Lydia E. Kavraki,et al.  A Sampling-Based Tree Planner for Systems With Complex Dynamics , 2012, IEEE Transactions on Robotics.

[48]  S. LaValle,et al.  Sampling-Based Motion Planning With Differential Constraints , 2005 .

[49]  Léonard Jaillet,et al.  Randomized path planning on manifolds based on higher-dimensional continuation , 2012, Int. J. Robotics Res..

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

[51]  Ernst Hairer,et al.  Geometric Integration of Ordinary Differential Equations on Manifolds , 2001 .