Complementarity-based Dynamic Simulation for Kinodynamic Motion - PowerPoint PPT Presentation

Complementarity-based Dynamic Simulation for Kinodynamic Motion Planning Nilanjan Chakraborty Robotics Institute, CMU Srinivas Akella Computer Science, UNC Charlotte Jeff Trinkle Computer Science, RPI

Kinodynamic Motion Planning • Planning in state space with – Collision avoidance constraints (algebraic) – Kinematics constraints (algebraic/differential) – Dynamics constraints (differential) • Resulting plan consists of – Time-varying sequence of actuator inputs – Time-varying sequence of states that satisfy the constraints (feasible state trajectory)

Related Work • Decoupled approach : Bobrow, Dubowsky, Gibson 1985; Shin and McKay 1985; Shiller and Dubowsky 1989 • Potential field methods : Khatib 1986; Rimon and Koditschek 1992 • Sampling-based approaches : LaValle and Kuffner 2001; Hsu et al. 2002 • Approximation algorithms for kinodynamic planning: Donald et al. 1993; Donald and Xavier 1995 • Compliant motion planning: Lozano-Perez, Mason, Taylor 1984; Erdmann 1984

Sampling-based Motion Planners • Graph representation of free space (eg. Rapidly-exploring Random Trees, LaValle and Kuffner 2001; Expansive space algorithm Hsu et al. 2002) : – Nodes for expansion selected by sampling – Edges and new nodes generated by local planner • Dynamic simulation methods generate a path segment by integrating differential constraints first • Collision checking along the path segment is performed as a follow-on step

Sampling-based Kinodynamic Motion Planning Dynamic Collision Simulator Detector • Focus has been on heuristics to select nodes to expand, and number of trees to expand

Sampling-based Kinodynamic Motion Planning: This Talk • Consider dynamics constraints and collision constraints simultaneously when generating path segments for local planning • Focus is on node expansion (local planning) step; can be used with any node selection method Dynamic Collision Simulator Detector 6

Complementarity-based Dynamic Simulation Algorithm • Use a complementarity based model for dynamic simulation • Contact force and (safe) distance to obstacle are complementary variables • Add (virtual) contact forces transformed to input space to the applied input to obtain input that avoids collision Modification is applicable to all variations of sampling based algorithms!

Complementarity Problem

Complementarity-based Dynamic Simulation Algorithm • Use a complementarity based model for dynamic simulation • Contact force and (safe) distance to obstacle are complementary variables • Add (virtual) contact forces transformed to input space to the applied input to obtain input that avoids collision Modification is applicable to all variations of sampling based algorithms!

Continuous Time Dynamics Model Equations of Motion: Contact forces Mass Matrix Coriolis force Applied force Contact constraints: is the state trajectory. It is feasible, if it satisfies the equations of motion and collision/contact constraints. 10

Discrete Time Dynamics Model Discrete Time Model: = (Stewart and Trinkle 1996, subproblem at each time step is a Mixed LCP) Since we do not have friction (because our contacts are virtual contacts), an unique solution can be found to the MLCP in polynomial time. 11

Compliant Motion • Input action set includes goal directed force • Set of executed actions can be a superset of the set of (search) input actions due to compliance • Use Safety distance ε to ensure no contact with obstacles

Advantages of Approach • Robust to the choice of • Robust to the discretization of input action set • Easy to find feasible inputs in cluttered environments Search Input Set = {(1,0), (-1,0), (0,1), (0,-1), F} F is a force directed towards goal.

Example: 2D Point Robot Feasible path found with single attractive potential at goal (single input towards goal) S = (0.4, 0.9, 0, 0), G = (5, 0.5, 0, 0)

Example: 2R Manipulator

Example: 2R Manipulator

Example: 2R Manipulator (contd.) Search with 5 inputs

n

n

Input Torque: Joint 1

Input Torque: Joint 2

Conclusion • Developed contact dynamics based algorithm for kinodynamic motion planning with collision avoidance. • Algorithm is more likely to find inputs for traversing narrow passages, a non-trivial problem for sampling-based randomized planners. • Algorithm relatively robust to simulation duration and choice of input set.

Acknowledgments • NSF CCF-0729161