Introduction to Motion Planning Phillip Coleman Algorithms & Applications Group Parasol Lab, Dept. of Computer Science, Texas A&M University

Outline Motion Planning Problem Sampling Approaches Alternative Approaches Differential Constraints Extensions and Variations

Motion Planning Problem start goal obstacles (Basic) Motion Planning: Given a movable object and a description of the environment, find a sequence of valid configurations that moves it from the start to the goal. Example: The Alpha Puzzle Objective: Separate the two tubes, one is the ‘robot’ the other is an ‘obstacle’ Have movies for both cases - you can play them as you speak.

Motion Planning Problem The Alpha Puzzle The Piano Movers Problem start goal obstacles (Basic) Motion Planning: Given a movable object and a description of the environment, find a sequence of valid configurations that moves it from the start to the goal. Have movies for both cases - you can play them as you speak.

Configuration Space (C-Space) “robot” maps to a point in higher dimensional space parameter for each degree of freedom (dof) of robot C-space = set of all robot placements C-obstacle = infeasible robot placements C-obst 3D C-space (a,b,g) a b g 3D C-space (x,y,z) 6D C-space (x,y,z,pitch,roll,yaw) 2n-D C-space (f1, y1, f2, y2, . . . , f n, y n)

The Complexity of Motion Planning Most motion planning problems of interest are PSPACE-hard The best deterministic algorithm known has running time that is exponential in the dimension of the robot’s C-space [Canny 86] C-space has high dimension - 6D for rigid body in 3-space simple obstacles have complex C-obstacles impractical to compute explicit representation of freespace for more than 4 or 5 dof

Outline Motion Planning Problem Sampling Approaches Alternative Approaches Differential Constraints Extensions and Variations

Sampling Methods Development of fast Collision Detection Sample Points in C-space Form Roadmap Weaker form of Completeness PRMs: Sample configurations and connect using a simple local planner to build a roadmap. start goal obstacles

Sampling Methods Development of fast Collision Detection Sample Points in C-space Form Roadmap Weaker form of Completeness Tree-based: Explore the space from some start configuration to reach goal configuration start goal obstacles

Probabilistic Roadmap Methods (PRMs) Roadmap represents connectivity of the workspace Computed only once, multiple queries Must maintain: Accessibility Connectivity

Probabilistic Roadmap Methods (PRMs) C-space Roadmap Construction (Pre-processing) Query processing start goal 1. Randomly generate robot configurations (nodes) - discard nodes that are invalid 1. Connect start and goal to roadmap C-obst 2. Connect pairs of nodes to form roadmap - simple, deterministic local planner (e.g., straightline) - discard paths that are invalid 2. Find path in roadmap between start and goal - regenerate plans for edges in roadmap C-obst C-obst C-obst C-obst

PRMs: Pros & Cons 1. PRMs are probabilistically complete PRMs: The Good News 1. PRMs are probabilistically complete 2. PRMs apply easily to high-dimensional C-space 3. PRMs support fast queries w/ enough preprocessing Many success stories where PRMs solve previously unsolved problems C-obst start goal PRMs: The Bad News 1. PRMs don’t work as well for some problems: unlikely to sample nodes in narrow passages hard to sample/connect nodes on constraint surfaces start goal C-obst

Motion Planning Given: an environment (descriptions of moveable object and obstacles), and start and goal positions Find: a valid path (continuous sequence of configurations) from start to goal (e.g., which avoids collision with obstacles)

Basic RRT x init RRT_EXPAND(T, K, t) for k = 1 to K do xrand = random configuration xnear = nearest neighbor in T to xrand xnew = extend xnear toward xrand for step length if (edge can be created between xnew & xnear) T.add_vertex(xnew) T.add_edge(xnear, xnew) x init

Basic RRT x rand x init RRT_EXPAND(T, K, t) for k = 1 to K do xrand = random configuration xnear = nearest neighbor in T to xrand xnew = extend xnear toward xrand for step length if (edge can be created between xnew & xnear) T.add_vertex(xnew) T.add_edge(xnear, xnew) x rand x init

Basic RRT x rand  x new x init RRT_EXPAND(T, K, t) for k = 1 to K do xrand = random configuration xnear = nearest neighbor in T to xrand xnew = extend xnear toward xrand for step length if (edge can be created between xnew & xnear) T.add_vertex(xnew) T.add_edge(xnear, xnew) x rand  x new x init

Basic RRT x init RRT_EXPAND(T, K, t) for k = 1 to K do xrand = random configuration xnear = nearest neighbor in T to xrand xnew = extend xnear toward xrand for step length if (edge can be created between xnew & xnear) T.add_vertex(xnew) T.add_edge(xnear, xnew) x init

Basic RRT x rand x init RRT_EXPAND(T, K, t) for k = 1 to K do xrand = random configuration xnear = nearest neighbor in T to xrand xnew = extend xnear toward xrand for step length if (edge can be created between xnew & xnear) T.add_vertex(xnew) T.add_edge(xnear, xnew) x init

Basic RRT x rand x near x init RRT_EXPAND(T, K, t) for k = 1 to K do xrand = random configuration xnear = nearest neighbor in T to xrand xnew = extend xnear toward xrand for step length if (edge can be created between xnew & xnear) T.add_vertex(xnew) T.add_edge(xnear, xnew) x near x init

Basic RRT x rand  x new x near x init RRT_EXPAND(T, K, t) for k = 1 to K do xrand = random configuration xnear = nearest neighbor in T to xrand xnew = extend xnear toward xrand for step length if (edge can be created between xnew & xnear) T.add_vertex(xnew) T.add_edge(xnear, xnew)  x new x near x init

Basic RRT RRT_EXPAND(T, K, t) for k = 1 to K do xrand = random configuration xnear = nearest neighbor in T to xrand xnew = extend xnear toward xrand for step length if (edge can be created between xnew & xnear) T.add_vertex(xnew) T.add_edge(xnear, xnew)

Outline Motion Planning Problem Sampling Approaches Alternative Approaches Differential Constraints Extensions and Variations

Alternative Approaches Non sampling approaches to the problem Restricted to C-Space dimensionality R or R2 Polygonal Obstacles Combinational Roadmaps

Maximum Clearance Roadmap Stay away from obstacles C-Space – R2 3 Different Types of Curves Compute all curves for all pairs Intersections become nodes O(n2) Complexity

Vertical Cell Decomposition Break the environment into Trapezoids or Triangles Planning within polygon is simple Node in each polygon and on every boundary Sweeping Method Sweep along x-axis Create a vertical boundary when vertices detected. O(n log n) Complexity

Vertical Cell Decomposition

Shortest Path Roadmap A roadmap containing the shortest paths around obstacles Assumes movement in close proximity to obstacles Formed by connecting the reflex vertices of the Cobs Edge must be a bitangent between vertices O(n2 + m) complexity

Potential Field Approaches Requires a Potential Function Rn -> R Deals with two forces: Attraction (goals) Repulsion (obstacles) Function forms a gradient that points towards the goal Gradient Descent used to find path Cons: Able to get stuck in local minimum

Outline Motion Planning Problem Sampling Approaches Alternative Approaches Differential Constraints Extensions and Variations

Differential Constraints Robot can deal with two types of constraints: Global Constraints (Obstacles, Joint Limits) Local Constraints (Differential Constraints) acceleration velocity turning radius Functional Representation x = f(q,u) Forms an X-Space

Differential Constraints X-Space x is in Xobs if q is in Cobs Region of inevitable collision (momentum) Types of Differential Constraint Planning Nonholonomic planning Kinodynamic Planning Trajectory Planning8

Discretization of Constraints Restricted to X = R or X = R2 Discretize C, T, or U (Space, Time, or Action) Discrete Time Model Time is broken into steps Only a subset of the Action Space U is considered Action u(t) is constant during the time step

Discretization of Constraints Reachability Tree From an initial state x apply all discretized actions Trajectory determined by integration of f(q,u) Dubins Car Incremental Simulator (steps through potential actions)

Discretization of Constraints Reachability Tree

Decoupled Approach Split the planning into two steps 1st plan path (simple motion planning problem) 2nd plan timing function Search space spanned by (s, s`) Path parameter and first derivative

Kinodynamic Planning Due to complexity many techniques are sampling based Explore reachability tree Trees are dense and hard to explore exhaustively Force it to behave like a lattice Regular cell decomposition over X Only one node allowed per cell Prunes the tree

Kinodynamic Planning

Outline Motion Planning Problem Sampling Approaches Alternative Approaches Differential Constraints Extensions and Variations

Extensions and Variations Closed Kinematic Chains Robot consists of loops formed by links A robot holding an object with two arms Loops are broken into chains Constrained to configuration where loops are maintained Deals with a subset of C-Space PRMs and RRTs can be adapted.

Extensions and Variations Manipulation Planning A robot must interact with an obstacle Moving a box from one location to another. Split into discrete and continuous modes Move towards object Transport object Switch is triggered by a defined set of requirements.

Extensions and Variations Time Varying Problem The obstacles or environment changes over time Adds another dimension to C-space Requires a directed Roadmap All paths must move forward in time

Extensions and Variations Multiple Robots Collisions with obstacles and other robots C-space must encompass all degrees of freedom C = C1, C2, … , Cn Centralized Planning Optimize set of all paths Decentralized Planning Each robot plans path Other robots considered obstacles

Conclusion Motion Planning is a complex problem Numerous Approaches Applications Many Challenges left to solve

Conclusion Questions?