Cell Decomposition Methods

Prof. Ashish Dutta

Professor
Department of Mechanical engineering
IIT Kanpur
Kanpur 208016, India
The Bushfire Algorithm for finding retract
Obstacles and boundaries are numbered 1
Increment each adjoining cell by 1
 Configuration space – finding a path
Shape of configuration space for mobile robot and serial arm is
different.
Torus
Silhouette Methods: Ellipsoid structure

Let S be the ellipsoid

with a through hole.
Pc is a hyperplane
of codimension 1
( x = c ) which will be
swept through S in
the X direction.
At each point
the slice travels
along X we’ll
find the extrema
in SPc in the Y
direction. If we
trace these out we
get silhouette
curves.
 Critical points
We’ll connect a
critical point to the rest
of the silhouette curve
with a path that lies
within SPc. This can
be done by running the
algorithm recursively.
Each time, we increase
the codimension of the
hyperplane by 1.
Torus
Types of Cell Decompositions
• Polygonal Cell Decomposition
• Trapezoidal cell Decomposition
• Morse Cell Decomposition
– Boustrophedon decomposition
– Morse decomposition definition
– Sensor-based coverage
 Polygonal Cell decomposition
A convex polygonal decomposition is a finite collection of
convex polygons, called cells, such that the interior of any
two cells do not intersect and the union is equal to free
space.

Connect convex vertices (to reduce paths going inside concave vertices).
 Polygonal Cell decomposition
Mid points of polygon edges to set up network of paths.

Connect start point and goal point to network

Connect convex vertices (to reduce paths going inside concave vertices).
Basic idea of Trapezoidal Cell decomposition method

The free workspace is divided into 2D

cells that look like trapezoids.

Input to the algorithm:

1. Vertices of workspace (x,y) coordinates.
2. Vertices of obstacles - coordinates.
 Adjacency Graph
– Node correspond to a cell
– Edge connects nodes of adjacent cells

c4 c7
c5
c2 c8
c15
c11
c1 c
c13
c3 10
c9
c6
c12
 Path Planning

• Path Planning in two steps:

– Planner determines cells that contain the start

and goal

– Planner searches for a path within adjacency

graph
 Trapezoidal Decomposition
• At each vertex draw two line segments one going up and the other going down.
• Lines do not go through obstacles.
Trapezoidal Decomposition

RI 16-735 Howie Choset

Trapezoidal Decomposition

RI 16-735 Howie Choset

Trapezoidal Decomposition
Trapezoidal Decomposition
Identify center of line segments and connect them to find path.
Trapezoidal Decomposition Path
 Sweep line method: what is the ultimate
goal?

Find the mid point of vertical line segments.

• Sweep a line through the free space stopping at
vertices.

• Maintain a list L of all the current edges the slice intersects

• Update the main list depending on the way the upper and
lower lines intersect.

• Depending on whether the vertical lines are above, below

or through an obstacle, decide the mid point of the
segments.
Example

Keep both
Example
Example
Example
Find mid points of vertical line segments.
Examine the adjacency graph to
get possible paths
Path planning is just one application:
Other requirements are for covering or
exploring the full free workspace:

1. Exploration

2. Demining

3. Cleaning
Basic idea of Coverage
Planner determines an exhaustive walk
through the adjacency graph

Planner computes explicit robot motions

within each cell

EXACT information is required

Moorse Decomposition
Disadvantage of trapezoidal decomposition: many small cells can be
formed

Moorse decomposition: Only Vertical lines that can be extended up

and down at vertices.

Such vertices are called critical vertices.

Complete Coverage of free
space
 Morse Decomposition in Terms of Critical Points
h

m

• Slice function: h(x,y)= x

• At a critical point x of h |M ,h(x) m(x) where M = {x|m(x)=0}

1-connected
• Connectivity of the slice in the free space
changes at the critical points
• Each cell can be covered by back and
forth motions
• Reeb graph represents the topology of
the cellular decomposition
 Incremental construction
•While covering the space, look for critical points

Stage 1
Incremental construction (cont’d)

Stage 1 Stage 2 Stage 3

Incremental construction (cont’d)

Stage 1 Stage 2 Stage 3 Stage 4

Detect Critical Points
Morse Decomposition
h(x,y) = x2 + y2
Morse Decomposition
h(x,y) = |x| + |y|
Morse Decomposition
h(x,y) = tan(y/x)
Brushfire Decomposition
Brushfire Decomposition
h(x,y) = D(x,y)

RI 16-735 Howie Choset

Brushfire Decomposition
h(x,y) = D(x,y)

RI 16-735 Howie Choset

Brushfire Decomposition
h(x,y) = D(x,y)
Sample based motion
planning
 Other possibilities to simplify
the problem
• Exhaustive search covering all possibilities not required.

• Search in lower-dimensional space

• Limit the number of possibilities (add constraints,

reduce “volume” of free space)

• Sacrifice optimality, completeness

 Basics of Sampling based
methods

• Randomly explore a smaller subset of possibilities while

keeping track of progress
• Facilities “probing” deeper in a search tree much earlier than
any exhaustive algorithm can
• Sacrifice completeness and optimality
• Tradeoff between solution quality and runtime performace
Positive and Negative aspects of
such methods
Path-Planning in High Dimensional Spaces

Needs an exhaustive search to cover all of C space

Complexity increases exponentially with high DOF systems

Building C –space for high DOF systems is complex

Humanoids
Snake robots
 Good news, but bad news too
goal
Sample-based: The Good News
C-obst 1. probabilistically complete
C-obst
2. Do not construct the C-space
C-obst 3. apply easily to high-dimensional C-space
C-obst 4. support fast queries w/ enough preprocessing

C-obst Many success stories where PRMs solve previously

start unsolved problems

goal
Sample-Based: The Bad News
C-obst C-obst
1. don’t work as well for some problems:
– unlikely to sample nodes in narrow passages
– hard to sample/connect nodes on constraint surfaces
C-obst C-obst 2. No optimality or completeness

start
 High-Dimensional Planning as of 1999

Single-Query: EXAMPLE: Potential-Field

Barraquand, Latombe ’89; Mazer, Talbi,

Ahuactzin, Bessiere ’92; Hsu, Latombe,
Motwani ’97; Vallejo, Jones, Amato ’99;

Multiple-Query:
EXAMPLE: PRM

Kavraki, Svestka, Latombe, Overmars ’95;

Amato, Wu ’96; Simeon, Laumound,
Nissoux ’99; Boor, Overmars, van der
Stappen ’99;
 Probabalistic Roadmaps
(ref: Robot Motion Planning, by J
C Latombe)

• Generation of sample points

• Learning Phase
Generation of a connected graph of paths

• Query Phase

Connecting the start and goal point to the graph of paths.

 The Learning Phase

• Construct a probabilistic roadmap by generating random free configurations of

the robot and connecting them using a simple, but very fast motion planner,
also know as a local planner

• Store as a graph whose nodes are the configurations and whose edges are the
paths computed by the local planner
 Learning Phase (Construction Step)

• Initially, the graph G = (V, E) is empty

• Then, repeatedly, a random free configuration is generated and added to V
• For every new node c, select a number of nodes from V and try to connect c
to each of them using the local planner.
• If a path is found between c and the selected node v, the edge (c,v) is added
to E. The path itself is not memorized (usually).
 How do we determine a random
free configuration?

• We want the nodes of V to be a rather uniform sampling of Qfree

– Draw each of its coordinates from the interval of values of the
corresponding degrees of freedom. (Use the uniform probability
distribution over the interval)
– Check for collision both with robot itself and with obstacles
– If collision free, add to V, otherwise discard
– What about rotations? Sample Euler angles gives samples near poles,
what about quartenions?

A random variable X taking values in the interval

[a, b] is uniformly distributed if in any two equal
subintervals of [a, b] X occurs with the same
probability. fX(x) = 1 b − a , a ≤ x ≤ b, and zero
outside the interval.
Serial arms – how to sample
 Local planners

• Need to make sure start and goal configurations can connect to graph,
which requires a somewhat dense roadmap
• Can reuse local planner at query time to connect start and goal
configurations
• Don’t need to memorize local paths
RI 16-735, Howie Choset with slides from Nancy Amato, Sujay Bhattacharjee, G.D. Hager, S. LaValle, and a lot from James Kuffner
RI 16-735, Howie Choset with slides from Nancy Amato, Sujay Bhattacharjee, G.D. Hager, S. LaValle, and a lot from James Kuffner
End of Construction Step

RI 16-735, Howie Choset with slides from Nancy Amato, Sujay Bhattacharjee, G.D. Hager, S. LaValle, and a lot from James Kuffner
 Distance Functions
• Really, D should reflect the likelihood that the planner will fail to
find a path
– close points, likely to succeed
– far away, less likely

• Typical approaches
– Euclidean distance on some embedding of c-space
• Embedding is often based on control points

– Alternative is to create a weighted combination of translation and

rotational “distances”

– Workspace volume
 2 DOF arm
– Check intersection by
• checking endpoints and line-ellipse intersection for each segment
• do this for each link
– Approximate distance by vector norm on angles

(x,y)


L2

y
L1

x
 Selecting Closest Neighbors

• kd-tree
– Given: a set S of n points in d-dimensional space
– Recursively
• choose a plane P that splits S about evenly (usually in a coordinate
dimension)
• store P at node
• apply to children Sl and Sr

• cell-based method
– when each point is generated, hash to a cell location
 Unconnected graph

• To expand a node c, we compute a short random-bounce walk starting from c.

This means
– Repeatedly pick at random a direction of motion in C-space and move in this direction
until an obstacle is hit.
– When a collision occurs, choose a new random direction.
– The final configuration n and the edge (c,n) are inserted into R and the path is
memorized.
– Try to connect n to the other connected components like in the construction step.
– Weights are only computed once at the beginning and not modified as nodes are added
to G.
Expansion Step

RI 16-735, Howie Choset with slides from Nancy Amato, Sujay Bhattacharjee, G.D. Hager, S. LaValle, and a lot from James Kuffner
Connection in Expansion Step

RI 16-735, Howie Choset with slides from Nancy Amato, Sujay Bhattacharjee, G.D. Hager, S. LaValle, and a lot from James Kuffner
 The Query Phase

• Find a path from the start and goal configurations to two nodes of the
roadmap

• Search the graph to find a sequence of edges connecting those nodes in the
roadmap

• Concatenating the successive segments gives a feasible path for the robot
Select start and goal

Goal
Start

RI 16-735, Howie Choset with slides from Nancy Amato, Sujay Bhattacharjee, G.D. Hager, S. LaValle, and a lot from James Kuffner
Connect Start and Goal to Roadmap

Goal
Start

RI 16-735, Howie Choset with slides from Nancy Amato, Sujay Bhattacharjee, G.D. Hager, S. LaValle, and a lot from James Kuffner
Find the Path from Start to Goal

Goal
Start

RI 16-735, Howie Choset with slides from Nancy Amato, Sujay Bhattacharjee, G.D. Hager, S. LaValle, and a lot from James Kuffner
 What if we fail?

• Maybe the roadmap was not adequate.

• Could spend more time in the Learning Phase
• Could do another Learning Phase and reuse R constructed in the first Learning
Phase. In fact, Learning and Query Phases don’t have to be executed sequentially.
 Sampling Strategies

Uniform is good because it is easy to implement but is bad

because it needs to cover all the workspace and needs time

• Learning Phase
• Construction Step
• Uniform sampling
• New sampling
• Expansion Step
• Uniform around neighbor (local
repair)
• New sampling
• Query Phase
Path-Planning in High Dimensional Spaces

Needs an exhaustive search to cover all of C space

Complexity increases exponentially with high DOF systems

Building C –space for high DOF systems is complex

Humanoids
Snake robots
 Sampling Strategies

• Near obstacles
• Narrow passages
• Visibility-based
• Manipulatibility-based
• Quasirandom
• Grid-based
Sample Near Obstacles
 Sampling in narrow passages
To Navigate Narrow Passages we must sample in them
• most PRM nodes are where planning is easy (not needed)

PRM Roadmap OBPRM Roadmap

goal goal

C-obst C-obst C-obst

C-obst

C-obst C-obst C-obst C-obst

Sampling inside the Narrow
Passageways
• Bridge Planner
– q1 and q2 are randomly sampled
– If they are both in collision, their midpoint is considered

• Dilate

• GVD of Cspace
– Somehow retract samples onto it without construction

• GVD of Workspace
– Use knot points or handle points
 Manipulability Sampling
Manipulability : ease of motion in a particular direction.
 RRT

Prof. Ashish Dutta

Professor
Department of Mechanical engineering
IIT Kanpur
Kanpur 208016, India
 Sampling Strategies

• Near obstacles
• Narrow passages
• Visibility-based
• Manipulatibility-based
• Quasirandom
• Grid-based
 The Simplified Probabilistic
Roadmap Planner (s-PRM)
The parameters of model are:

• Free Space Qfree: An arbitrary open subset of the unit square W=[0,1]d

• The Robot: A point free to move in Qfree

• The Local Connector: It takes the robot from point a to point b along a
straight line and succeeds if the straight line segment ab is contained in Qfree

• The collection of Random Configurations:

Collection of N independent points uniform in Qfree
Rapidly-Exploring Random Trees
(RRTs)
[Ref: Principles of Robot Motion, by Howie Choset, et al.]

The Basic RRT

single tree
bidirectional
multiple trees (forests)

RRTs with Differential Constraints

nonholonomic
kinodynamic systems
closed chains
Rapidly-Exploring Random Tree
 Path Planning with RRTs

EXTEND(T, qrand) qnew

qnear qrand
qinit
 Biases
• Bias toward larger spaces
• Bias toward goal
Grow two RRTs towards each other

qnew
qtarget [ Kuffner, LaValle ICRA ‘00]

qgoal
qnear
qinit
A single RRT-Connect iteration...

qgoal

qinit
1) One tree grown using random target

qgoal

qinit
2) New node becomes target for
other tree

qtarget
qgoal

qinit
3) Calculate node “nearest” to target

qtarget
qgoal
qnear
qinit
4) Try to add new collision-free branch
qnew
qtarget
qgoal
qnear
qinit
5) If successful, keep extending branch
qnew
qtarget
qgoal
qnear
qinit
5) If successful, keep extending branch
qnew
qtarget
qgoal
qnear
qinit
5) If successful, keep extending branch
qnew
qtarget
qgoal
qnear
qinit
6) Path found if branch reaches target

qgoal
qnear
qinit
7) Return path connecting start and goal

qgoal

qinit
Articulated Robot

RI 16-735, Howie
Choset with slides from
Nancy Amato, Sujay
Bhattacharjee, G.D.
Highly Articulated Robot
Hovercraft with 2 Thusters
Out of This World Demo
Programme for RRT –
Assignment -3
RRT
examples
Videos
Dynamic RRT
RRT Biped
RRT robot arm