topic 1: discrete and "2d" planning

Discrete and “2D” Planning Lydia Tapia tapia@cs.unm.edu

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

The Alpha Puzzle: A MP Benchmark

Objective: Separate the two tubes, one is the ‘robot’ the other is an ‘obstacle’.

start

goal obstacles

(Basic) Motion Planning (in a nutshell):

Given a movable object, find a sequence of valid configurations that moves the object from the start to the goal.

[Movie from the Parasol Lab, Texas A&M Univ.]

Configuration Space (C-Space)

C-obst

C-obst

C-obst

C-obst

C-obst

C-Space

6D C-space (x,y,z,pitch,roll,yaw)

3D C-space (x,y,z) 3D 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

2n-D C-space (φ1, ψ1, φ2, ψ2, . . . , φ n, ψ n)

robot

obst

obst

obst

obst

x y

C-obst

C-obst C-obst

C-obst

robot Path is swept volume

Motion Planning in C-space

Path is 1D curve

Workspace

C-space Simple workspace obstacle transformed Into complicated C-obstacle!!

Algorithms • Visibility Graph • Cell Decomposition • Potential Fields

• State of the Art: Sampling-based Algorithms •  Basic PRM •  OBPRM

• Current Open Problems: •  Feature-Sensitive Motion Planning •  Super high-dimensional problems

Goal reduce the N-dimensional configuration space to a set of one-D paths to search. Goal account for all of the free space Goal Create local control strategies that will be more flexible than those above

Goal reduce the computational cost of configuration space, and only have a set of 1-D paths to search.

Algorithms • Visibility Graph • Cell Decomposition • Potential Fields

• State of the Art: Sampling-based Algorithms •  Basic PRM •  OBPRM

• Current Open Problems: •  Feature-Sensitive Motion Planning •  Super high-dimensional problems

Goal reduce the N-dimensional configuration space to a set of one-D paths to search. Goal account for all of the free space Goal Create local control strategies that will be more flexible than those above

Goal reduce the computational cost of configuration space, and only have a set of 1-D paths to search.

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.

start

goal

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.

start

goal

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.

start

goal

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.

start

goal

The Visibility Graph (Done) • Repeat until you’re done.

start

goal

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:

In addition, the paths they find are “semi-free,” i.e. in contact with obstacles.

shortest path

shortest path within the visibility graph

No clearance

Algorithms • Visibility Graph • Voronoi Cells • Cell Decomposition • Potential Fields

• State of the Art: Sampling-based Algorithms •  Basic PRM •  OBPRM

• Current Open Problems: •  Feature-Sensitive Motion Planning •  Super high-dimensional problems

Goal reduce the N-dimensional configuration space to a set of one-D paths to search.

Goal account for all of the free space Goal Create local control strategies that will be more flexible than those above

Goal reduce the computational cost of configuration space, and only have a set of 1-D paths to search.

Goal optimize clearance

“official” Voronoi diagram

(line segments make up the Voronoi diagram isolates a set of points)

Roadmap: Voronoi diagrams

Generalized Voronoi Graph (GVG): locus of points equidistant from the closest two or more obstacle boundaries, including the workspace boundary.

Property: maximizing the clearance between the points and obstacles.

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 • Cell Decomposition • Potential Fields

• State of the Art: Sampling-based Algorithms •  Basic PRM •  OBPRM

• Current Open Problems: •  Feature-Sensitive Motion Planning •  Super high-dimensional problems

Goal reduce the N-dimensional configuration space to a set of one-D paths to search. Goal account for all of the free space Goal Create local control strategies that will be more flexible than those above

Goal reduce the computational cost of configuration space, and only have a set of 1-D paths to search.

29

Exact Cell Decomposition

Decomposition of the free space into trapezoidal & triangular cells

Connectivity graph representing the adjacency relation between the cells

(Sweepline algorithm)

Trapezoidal Decomposition:

Exact Cell Decomposition

Search the graph for a path (sequence of consecutive cells)

Trapezoidal Decomposition:

Exact Cell Decomposition

Transform the sequence of cells into a free path (e.g., connecting the mid-points of the intersection of two consecutive cells)

Trapezoidal Decomposition:

Obtaining the minimum number of convex cells is NP-complete.

Optimality

15 cells 9 cells

Trapezoidal decomposition is exact and complete, but not optimal

Trapezoidal Decomposition:

Quadtree Decomposition:

Approximate Cell Decomposition

recursively subdivides each mixed obstacle/free (sub)region into four quarters... Quadtree:

34

further decomposing...

recursively subdivides each mixed obstacle/free (sub)region into four quarters... Quadtree:

Quadtree Decomposition:

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 ?

• 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

Quadtree Decomposition:

Algorithms • Visibility Graph • Cell Decomposition • Potential Fields

• State of the Art: Sampling-based Algorithms •  Basic PRM •  OBPRM

• Current Open Problems: •  Feature-Sensitive Motion Planning •  Super high-dimensional problems

Goal reduce the N-dimensional configuration space to a set of one-D paths to search. Goal account for all of the free space Goal Create local control strategies that will be more flexible than those above

Goal reduce the computational cost of configuration space, and only have a set of 1-D paths to search.

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

•  Compute an attractive force toward the goal

Potential Field Method

C-obstacles

Attractive potential

Potential Field Method

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

•  Compute a repulsive force away from obstacles

•  Compute a repulsive force away from obstacles

Potential Field Method

•  Repulsive Potential

•  Sum of Potential

Potential Field Method

C-obstacle

Attractive potential Repulsive potential

Sum of potentials

•  After get total potential, generate force field (negative gradient)

•  Let the sum of the forces control the robot

To a large extent, this is computable from sensor readings

Equipotential contours

Negative gradient

Total potential

Potential Field Method

random walks are not perfect...

Potential Field Method •  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

Pros:

•  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

Cons: