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

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

3. 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.

4. 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.

5. 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)

6. 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

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

8. 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

9. 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

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

11. 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

12. 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

13. 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)

14. 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

15. 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

16. 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

17. 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

18. 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

19. 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

20. 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

21. 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)

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

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

24. 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

25. 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

26. Vertical Cell Decomposition

27. 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

28. 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

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

30. 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

31. 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

32. 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

33. 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)

34. Discretization of Constraints
Reachability Tree

35. 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

36. 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

37. Kinodynamic Planning

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

39. 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.

40. 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.

41. 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

42. 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

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

44. Conclusion
Questions?