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

• Focus has been on heuristics to select nodes to

expand, and number of trees to expand

Dynamic Simulator Collision Detector

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

6

Dynamic Simulator Collision Detector

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!

10

Continuous Time Dynamics Model

Equations of Motion: Contact constraints: Mass Matrix Contact forces Applied force Coriolis force

is the state trajectory. It is feasible, if it satisfies the equations of motion and collision/contact constraints.

11

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.

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
• bstacles

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

S = (0.4, 0.9, 0, 0), G = (5, 0.5, 0, 0) (single input towards goal)

Feasible path found with single attractive potential at goal

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