Discrete and
“2D” Planning
Lydia Tapia
[email protected]
Discrete Planning
•  Each state can be represented as a discrete state
•  World can be transformed by the application of actions
•  Given a start and a goal state
•  Find sequence of actions to take from start to goal
Labyrinth search
•  <=4 possible moves from
each state
Rubik’s Cube
•  12 possible actions for
each state
•  Tree of possible actions
•  We want to build as little of
the tree as possible
Possible Search Methods
•  General Forward Search
Other Search Methods
•  Depth-First Search
•  Priority queue becomes Last In First Out
•  Dijkstra’s Algorithm
•  Need costs on edges
•  Priority queue is sorted by cost-to-come
•  Cost-to-come becomes optimal as all edges are searched
•  A*
•  Attempt to reduces number of states explored by Dijkstra’s
•  Priority queue is sorted by sum of cost-to-come and cost-to-go
•  Cost-to-go estimate must be done well
Other Search Methods II
•  Best First
•  Priority queue sorted by cost-to-go
•  Always selecting the “next best step”
•  Very Greedy approach
•  Iterative deepening
•  Use depth-first search and find all states that are some distance, d,
from current state
•  If goal is not found, then find all states d+1 from current state
•  Works well with large branching factors
Other Search Methods III
•  Backwards Search
•  Start search from goal
•  Depends on branching factor
•  Bidirectional Search
•  Search from both start and goal at same time
•  Need to merge the search trees together at some point
Logic Formulations
•  Explicit representations of state space can be huge!
•  Implicit representations would be preferred
•  Logic-based representations were popular (1950s-1980s)
•  Outputs clear plan to user without any intervention
•  Extremely hard to generalize
STRIPS-like Planning
•  STRIPS: Stanford Research Institute Problem Solver
STRIPS-like Flashlight Problem
1. Set of Instances, I
I={Battery1, Battery2, Cap, Flashlight}
2. Set of Predicates, P
On and In
For example, On(Cap, Flashlight) In(Battery1, Flashlight)
3. Set of Operators, O
4. Initial State
S= {On(Cap, Flashlight)}
5. Goal State
G={On(Cap, Flashlight), In(Battery1, Flashlight), In(Battery2, Flashlight)}
Logic Formulations
Blocks world
1. Set of Instances, I
2. Set of Predicates, P
3. Set of Operators, O
4. Initial State
5. Goal State
What is Motion Planning
(Basic) Motion Planning
(in a nutshell):
The Alpha Puzzle:
A MP Benchmark
Given a movable object, find a
sequence of valid configurations
that moves the object from the
start to the goal.
Objective: Separate the two
tubes, one is the robot the
other is an obstacle .
start
goal
obstacles
[Movie from the Parasol Lab, Texas A&M Univ.]
Configuration Space (C-Space)
C-Space
•  robot maps to a point in higher
C-obst
C-obst
C-obst
C-obst
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
(x,y,z)
6D C-space
(x,y,z,pitch,roll,yaw)
γ
β
α
3D C-space
(α,β,γ)
2n-D C-space
(φ1, ψ1, φ2, ψ2, . . . , φ n, ψ n)
Motion Planning in C-space
Simple workspace obstacle transformed
Into complicated C-obstacle!!
Workspace
C-space
C-obst
C-obst
obst
obst
C-obst
C-obst
obst
obst
robot
Path is swept volume
robot
Path is 1D curve
x
y
Algorithms
•  Visibility Graph
Goal reduce the N-dimensional
configuration space to a set of
one-D paths to search. •  Cell Decomposition
Goal account for all of the free space •  Potential Fields
Goal Create local control strategies that
will be more flexible than those above •  State of the Art: Sampling-based Algorithms
Goal reduce the computational cost
•  Basic PRM
of configuration space, and only
•  OBPRM
have a set of 1-D paths to search. •  Current Open Problems:
•  Feature-Sensitive Motion Planning
•  Super high-dimensional problems
Algorithms
•  Visibility Graph
Goal reduce the N-dimensional
configuration space to a set of
one-D paths to search. •  Cell Decomposition
Goal account for all of the free space •  Potential Fields
Goal Create local control strategies that
will be more flexible than those above •  State of the Art: Sampling-based Algorithms
Goal reduce the computational cost
•  Basic PRM
of configuration space, and only
•  OBPRM
have a set of 1-D paths to search. •  Current Open Problems:
•  Feature-Sensitive Motion Planning
•  Super high-dimensional problems
The Visibility Graph in Action (Part 1)
•  First, draw lines of sight from the start and goal to all visible
vertices and corners of the world.
goal
start
The Visibility Graph in Action (Part 2)
•  Second, draw lines of sight from every vertex of every
obstacle like before. Remember lines along edges are also
lines of sight.
goal
start
The Visibility Graph in Action (Part 3)
•  Second, draw lines of sight from every vertex of every
obstacle like before. Remember lines along edges are also
lines of sight.
goal
start
The Visibility Graph in Action (Part 4)
•  Second, draw lines of sight from every vertex of every
obstacle like before. Remember lines along edges are also
lines of sight.
goal
start
The Visibility Graph (Done)
•  Repeat until you re done.
goal
start
Since the map was in C-space, each line potentially represents part of a path
from the start to the goal.
Visibility graph drawbacks
Visibility graphs do not preserve their
optimality in higher dimensions:
shortest path
shortest path within the visibility graph
In addition, the paths they find are semi-free, i.e. in contact with obstacles.
No clearance
Algorithms
•  Visibility Graph
•  Voronoi Cells
Goal reduce the N-dimensional
configuration space to a set of
one-D paths to search. Goal optimize clearance
•  Cell Decomposition
Goal account for all of the free space •  Potential Fields
Goal Create local control strategies that
will be more flexible than those above •  State of the Art: Sampling-based Algorithms
Goal reduce the computational cost
•  Basic PRM
of configuration space, and only
•  OBPRM
have a set of 1-D paths to search. •  Current Open Problems:
•  Feature-Sensitive Motion Planning
•  Super high-dimensional problems
Roadmap: Voronoi diagrams
official Voronoi diagram
(line segments make up the
Voronoi diagram isolates a
set of points)
Property: maximizing the
clearance between the points
and obstacles.
Generalized Voronoi Graph (GVG):
locus of points equidistant from the closest two
or more obstacle boundaries, including the
workspace boundary.
Roadmap: Voronoi diagrams
•  GVG is formed
by paths
equidistant from
the two closest
objects
•  maximizing the
clearance
between the
obstacles.
•  This generates a very safe roadmap which avoids
obstacles as much as possible
Algorithms
•  Visibility Graph
Goal reduce the N-dimensional
configuration space to a set of
one-D paths to search. •  Cell Decomposition
Goal account for all of the free space •  Potential Fields
Goal Create local control strategies that
will be more flexible than those above •  State of the Art: Sampling-based Algorithms
Goal reduce the computational cost
•  Basic PRM
of configuration space, and only
•  OBPRM
have a set of 1-D paths to search. •  Current Open Problems:
•  Feature-Sensitive Motion Planning
•  Super high-dimensional problems
Exact Cell Decomposition
Trapezoidal Decomposition:
Decomposition of the free space
into trapezoidal & triangular cells
Connectivity graph representing the
adjacency relation between the cells
(Sweepline algorithm)
29
Exact Cell Decomposition
Trapezoidal Decomposition:
Search the graph for a path
(sequence of consecutive cells)
Exact Cell Decomposition
Trapezoidal Decomposition:
Transform the sequence of cells into a
free path (e.g., connecting the midpoints of the intersection of two
consecutive cells)
Optimality
Trapezoidal Decomposition:
15 cells
Trapezoidal decomposition is exact
and complete, but not optimal 9 cells
Obtaining the minimum number of convex
cells is NP-complete.
Approximate Cell Decomposition
Quadtree Decomposition:
Quadtree:
recursively subdivides each mixed obstacle
/free (sub)region into four quarters...
34
further decomposing...
Quadtree Decomposition:
Quadtree:
recursively subdivides each mixed obstacle
/free (sub)region into four quarters...
Quadtree Decomposition:
• The rectangle cell is recursively
decomposed into smaller rectangles
• At a certain level of resolution, only the
cells whose interiors lie entirely in the free
space are used
• A search in this graph yields a collision
free path
Again, use a graph-search algorithm to
find a path from the start to goal
Quadtree
is this a complete path-planning algorithm?
i.e., does it find a path when one exists ?
Algorithms
•  Visibility Graph
Goal reduce the N-dimensional
configuration space to a set of
one-D paths to search. •  Cell Decomposition
Goal account for all of the free space •  Potential Fields
Goal Create local control strategies that
will be more flexible than those above •  State of the Art: Sampling-based Algorithms
Goal reduce the computational cost
•  Basic PRM
of configuration space, and only
•  OBPRM
have a set of 1-D paths to search. •  Current Open Problems:
•  Feature-Sensitive Motion Planning
•  Super high-dimensional problems
Potential Field Method
Potential Field (Working Principle)
– The goal location generates an attractive potential – pulling the robot
towards the goal
– The obstacles generate a repulsive potential – pushing the robot far away
from the obstacles
– The negative gradient of the total potential is treated as an artificial
force applied to the robot
-- Let the sum of the forces control the robot
C-obstacles
Potential Field Method
•  Compute an attractive force toward the goal
C-obstacles
Attractive potential
Potential Field Method
•  Compute a repulsive force away from obstacles
Repulsive Potential
  Create a potential barrier around the C-obstacle
region that cannot be traversed by the robot s
configuration
  It is usually desirable that the repulsive potential
does not affect the motion of the robot when it is
sufficiently far away from C-obstacles
Potential Field Method
•  Compute a repulsive force away from obstacles
•  Repulsive Potential
Potential Field Method
•  Sum of Potential
Attractive potential
C-obstacle
Sum of potentials
Repulsive potential
Potential Field Method
•  After get total potential, generate force field
(negative gradient)
•  Let the sum of the forces control the robot
Equipotential contours
Negative
gradient
Total potential
To a large extent, this is computable from sensor readings
Potential Field Method
Pros:
•  Spatial paths are not preplanned and can be
generated in real time
•  Planning and control are merged into one
function
•  Smooth paths are generated
•  Planning can be coupled directly to a control
algorithm
Cons:
•  Trapped in local minima in the potential field
•  Because of this limitation, commonly used for
local path planning
•  Use random walk, backtracking, etc to escape
the local minima
random walks are not perfect...