CS 4610/5335
Path and Motion Planning
Robert Platt Northeastern University 1/22/20
Material adapted from:
1. Lawson Wong, CS 4610/5335
2. Peter Corke, Robotics, Vision and Control
3. Marc Toussaint, U. Stuttgart Robotics Course 4. Oussama Khatib, Stanford CS 223A
5. Howie Choset
6. Kai Arras
Where we are so far
– Rotations and transformations – Forward kinematics
f (q) x
– Inverse kinematics
Input: x*
Output: q*
1. repeat until dx is small:
2. 3. 4. 5. 6. 7. 8.
init q to random joint configuration repeat K times:
x = FK(q)
dx = x*-x
dq = stepsize * J# dx q = q + dq
return q* = q
Problem we want to solve
Given:
– description of the robot
– description of the obstacle environment
Find:
– path from start to goal that does not result in a collision
Starting configuration
Goal configuration
Problem we want to solve
Given:
– a point-robot (robot is a point in space) – a start and goal configuration
Find:
– path from start to goal that does not result in a collision
Assumptions:
– the position of the robot can always be measured perfectly – the motion of the robot can always be controlled perfectly – the robot can move in any direction instantaneously
Heuristic Solutions
Bug algorithms:
– assume only local knowledge of the environment is available – simple behaviors: follow a wall; follow straight line toward goal
Wavefront planner (distance transform) Potential functions
First attempt: BUG 0
assume a left- turning robot
The turning direction might be decided beforehand…
BUG 0:
1. head toward goal
2. if hit a wall, turn left
3. follow wall until a line toward goal will move you away from wall.
(assume we only have local sensing – we cannot sense position of walls we are not touching)
Question
goal
start
What does BUG0 do here?
Second attempt: BUG 1
BUG 1:
1. move on straight line toward goal
2. if obstacle encountered, circumnavigate entire obstacle and remember how
close bug got to goal
3. return to closest point and continue on a straight line toward goal
Second attempt: BUG 1
BUG 1:
1. move on straight line toward goal
2. if obstacle encountered, circumnavigate entire obstacle and remember how
close bug got to goal
3. return to closest point and continue on a straight line toward goal
Question
goal
start
What does BUG1 do here?
m-line
Another bug: BUG 2
1. head toward goal on m-line
m-line
Another bug: BUG 2
1. head toward goal on m-line
2. if you encounter obstacle, follow it until you encounter m-line again at a point closer to goal
m-line
Another bug: BUG 2
1. head toward goal on m-line
2. if you encounter obstacle, follow it until you encounter m-line again at a point closer to goal
3. leave line and head toward goal again
Question
goal
start
What does BUG2 do here?
Wavefront planner (distance transform)
– intensity of a point denotes its (obstacle-respecting) distance from the goal
Wavefront planner (distance transform) Algorithm:
1. 2. 3. 4. 5. 6.
L: d:
L={goal state}, d(goal state) = 2, d(obstacle states) = 1, d(rest of states) = 0 while L != null
pop item i from L
for all neighbors j of i such that d(j)==0
d(j) = d(i)+1 push j onto L
list of nodes in wave front; initially just the goal state
distance function over nodes; initially zero everywhere except goal state
Wavefront planner (distance transform) Algorithm:
1. 2. 3. 4. 5. 6.
L={goal state}, d(goal state) = 2, d(obstacle states) = 1, d(rest of states) = 0 while L != null
pop item i from L
for all neighbors j of i such that d(j)==0
d(j) = d(i)+1 push j onto L
Wavefront planner (distance transform) Algorithm:
1. 2. 3. 4. 5. 6.
L={goal state}, d(goal state) = 2, d(obstacle states) = 1, d(rest of states) = 0 while L != null
pop item i from L
for all neighbors j of i such that d(j)==0
d(j) = d(i)+1 push j onto L
Wavefront planner (distance transform) Algorithm:
1. 2. 3. 4. 5. 6.
L={goal state}, d(goal state) = 2, d(obstacle states) = 1, d(rest of states) = 0 while L != null
pop item i from L
for all neighbors j of i such that d(j)==0
d(j) = d(i)+1 push j onto L
Wavefront planner (distance transform) Algorithm:
1. 2. 3. 4. 5. 6.
L={goal state}, d(goal state) = 2, d(obstacle states) = 1, d(rest of states) = 0 while L != null
pop item i from L
for all neighbors j of i such that d(j)==0
d(j) = d(i)+1 push j onto L
Wavefront planner (distance transform) Algorithm:
1. 2. 3. 4. 5. 6.
L={goal state}, d(goal state) = 2, d(obstacle states) = 1, d(rest of states) = 0 while L != null
pop item i from L
for all neighbors j of i such that d(j)==0
d(j) = d(i)+1 push j onto L
Wavefront planner (distance transform) Algorithm:
1. 2. 3. 4. 5. 6.
L={goal state}, d(goal state) = 2, d(obstacle states) = 1, d(rest of states) = 0 while L != null
pop item i from L
for all neighbors j of i such that d(j)==0
d(j) = d(i)+1 push j onto L
Wavefront planner (distance transform) Algorithm:
1. 2. 3. 4. 5. 6.
L={goal state}, d(goal state) = 2, d(obstacle states) = 1, d(rest of states) = 0 while L != null
pop item i from L
for all neighbors j of i such that d(j)==0
d(j) = d(i)+1 push j onto L
Wavefront planner (distance transform) Algorithm:
1. 2. 3. 4. 5. 6.
L={goal state}, d(goal state) = 2, d(obstacle states) = 1, d(rest of states) = 0 while L != null
pop item i from L
for all neighbors j of i such that d(j)==0
d(j) = d(i)+1 push j onto L
c
Wavefront planner (distance transform) Algorithm:
1. 2. 3. 4. 5. 6.
L={goal state}, d(goal state) = 2, d(obstacle states) = 1, d(rest of states) = 0 while L != null
pop item i from L
for all neighbors j of i such that d(j)==0
d(j) = d(i)+1 push j onto L
For node j, how many steps to goal state in terms of d(j)?
c
Complete? Optimal?
Questions
Guaranteed to find a solution if one exists? Does it find shorted path solutions?
c
Potential Functions
Potential Functions
Potential Functions
Potential Functions
After computing U, follow the negative gradient:
Potential Function Limitations
Bug trap
Heuristic Solutions
Bug algorithms:
– assume only local knowledge of the environment is available – simple behaviors: follow a wall; follow straight line toward goal
Wavefront planner (distance transform)
Potential functions
So far, these only work for 2-D path planning
or are not optimal / principled approximations
Problem we want to solve
Given:
– description of the robot
– description of the obstacle environment
Find:
– path from start to goal that does not result in a collision
Starting configuration
Goal configuration
Problem we want to solve
This problem statement is actually very general – manipulators
Problem we want to solve
This problem statement is actually very general – manipulators
– mobile robots
Approach: plan in “configuration space”
Convert the original planning problem into a planning problem for a single point.
workspace
Convert the original planning problem into a planning problem for a single point.
configuration space
Approach: plan in “configuration space”
workspace
Convert the original planning problem into a planning problem for a single point.
configuration space
Approach: plan in “configuration space”
Approach: plan in “configuration space” Conversion to c-space: Minkowski Sum
workspace
Convert the original planning problem into a planning problem for a single point.
configuration space
Approach: plan in “configuration space”
Approach: plan in “configuration space”
Convert the original planning problem into a planning problem for a single point.
workspace
configuration space
Original problem Equivalent problem:
– plan path for robot arm – plan path for a point
Approach: plan in “configuration space”
workspace
configuration space
Notice the axes!
Cartesian space! Joint angles!
Approach: plan in “configuration space”
workspace
configuration space
Every point here corresponds to a single robot configuration here
Approach: plan in “configuration space”
workspace
configuration space
Every point that intersects an obstacle here corresponds to an arm configuration that intersects an obstacle
Approach: plan in “configuration space”
workspace
configuration space
C-obstacles Free space
workspace
configuration space
Question
Which object is responsible for this c-obstacle?
Configuration space
The dimension of a configuration space is the minimum number of parameters needed to specify the configuration of the robot completely. – also called the number of “degrees of freedom” (DOFs)
Dimension = 3
Question
The dimension of a configuration space is the minimum number of parameters needed to specify the configuration of the robot completely. – also called the number of “degrees of freedom” (DOFs)
Dimension = ?
Formalization of the motion planning problem
Configuration space: Obstacle space: Free space:
Path: where must be continuous Collision-free path:
Formalization of the motion planning problem
Given:
– configuration space – free space
– start state
– goal region
Find:
– a collision-free path , such that and
Configuration-space Motion Planning
Visibility graphs
Voronoi diagrams
Exact cell decomposition Approximate cell decomposition
Method #1: Visibility Graphs
n = num of obstacle vertices
Method #2: Generalized Voronoi Diagram
Method #2: Generalized Voronoi Diagram
Method #2: Generalized Voronoi Diagram
Method #3: Exact Cell Decomposition
Method #3.1: Trapezoidal Decomposition
Method #3.2: Uniform Approximate Cell Decomposition
c
Uniform cell shape: e.g. wavefront planner
Method #3.3: Quadtrees
Non-Uniform cell shape: e.g. quadtree decomposition
2. 3. 4. 5. 6. 7. 8.
create coarse grid
collision-check G
for all occupied cells c in G:
delete c from G
subdivide c into four cells (sub) add sub into G
collision-check sub
Method #3.3: Quadtrees
define G = Decompose(G,resolution):
1. if G null:
Collision-check: check whether each cell is completely free or not
define FindPath(maxresolution):
1. for resolution = coarse to maxresolution:
2. 3. 4.
G = Decompose(G,resolution) if Check-for-path(G) == True:
Success!
Why do you think this method is called “quadtree”?
Method #3.3: Quadtrees
How deep is the decomposition shown here?
Method #3.4: Octomaps
Same as quadtrees, but in three dimensions…
Examples of solutions found using octomaps
Exact vs approximate cell decomposition
Key challenge: high dimensions and complex geometry of free space
None of the methods we have looked at so far scale well to manipulator path planning
– e.g. a 6-DOF UR5 arm
Methods studied so far: – visibility graphs
– Voronoi diagrams
– cell decomposition
– potential functions
Only work for small num of obstacles Only work for small dimensional spaces
Not complete
Approach: Plan in “configuration space”
Convert the original planning problem into a planning problem for a single point.
workspace
configuration space
Original problem Equivalent problem:
– plan path for robot arm – plan path for a point
Formalization of the motion planning problem
Configuration space: Obstacle space: Free space:
Path: where must be continuous Collision-free path:
Naive approach for constructing C-space obstacles
workspace configuration space
– Densely sample the space of all robot configurations
– For each config q, use known environment and robot geometric models
to construct the workspace shapes
– Apply standard collision checking routines to check if in collision
– If in collision, add to Cobs ; else add to Cfree
Naive approach for constructing C-space obstacles
Checking collision between two convex polyhedra is easy
All non-convex polyhedra can be decomposed into a union of convex polyhedra Approximate all other shapes as polyhedra / unions of spheres
Sampling-based Motion Planning
Two methods:
Probabilistic roadmaps (PRM) Rapidly-exploring random trees (RRT)
Probabilistic Roadmap (PRM)
75
This produces a roadmap
Shortest paths between roadmap nodes can be found using Dijkstra’s algorithm
To answer a motion planning query, connect init and target to closest node
76
77
Collision checking: Static configuration
Checking collision between two convex polyhedra is easy
All non-convex polyhedra can be decomposed into a union of convex polyhedra Approximate all other shapes as polyhedra / unions of spheres
Collision checking: Local path
Check for collisions between small straight-line segments in C-space
Collision checking: Local path
Form the swept volume of the polyhedra by taking the convex hull of two configurations
Collision checking: Local path
Form the swept volume of the polyhedra by taking the convex hull of two configurations
Note: This could be an approximation – depends on collision check resolution
Alternatives: – Connect all neighbors within some distance (called sPRM) – Connect to all possible points! (inefficient but exhaustive)
82
Does this algorithm work? Is it complete?
83
Is it optimal?
Does this algorithm work?
Is it complete? – depends on n Is it optimal?
84
Does this algorithm work?
Is it complete? – depends on n
Is it optimal? – depends on the graph, and the search algorithm
85
Probabilistic completeness of PRM
What guarantees do we have?
Probability of not finding a solution to a robustly feasible problem decreases exponentially with the number of vertices
86
PRM sampling strategies Problem: it may take a lot of samples to reach a fully connected graph
PRM sampling strategies
89
Gaussian sampler
So far, we have only discussed uniform sampling…
Problem: uniform sampling is not a great way to find paths through narrow passageways.
PRM Roadmap
C-obst C-obst
goal
C-obst
C-obst
start
90
Gaussian sampler
Gaussian sampler:
1. Sample points uniformly at random (as before)
2. For each sampled point, sample a second point from a Gaussian distribution centered at the first sampled point
3. Discard both samples if both samples are either free or in collision
4. Keep the free sample if the two samples are NOT both free or both in collision (that is, keep the sample if the free/collision status of the second sample is different from the first).
91
PRM sampling strategies
Rapidly Exploring Random Trees (RRTs)
Problems with PRM:
– two steps: graph construction, then graph search
– hard to apply to problems where edges are directed, i.e. kinodynamic problems
RRTs solve both of these problems:
– create a tree instead of graph: no graph search needed! – tree rooted at start or goal – edges can be directed
RRT Algorithm
RRT Algorithm
RRT Algorithm
RRT Algorithm
Strategies for finding q_near: – take closest vertex in tree
– check intermediate points at regular intervals and split edge
RRT Algorithm
Strategies for reaching q_new: – straight line in c-space
– some kind of local planner
If can’t reach q_new:
– just go as far as you can and create a new vertex there
RRT versus a naïve random tree
RRT
Naïve random tree
Growing the naïve random tree: 1. pick a node at random
2. sample a new node near it
3. grow tree from random node to new node
RRTs and
Bias toward large Voronoi regions
http://msl.cs.uiuc.edu/rrt/gallery.html
Biases
Bias toward larger spaces Bias toward goal
When generating a random sample, with some probability pick the goal instead of a random node when expanding
This introduces another parameter 5-10% is probably the right choice
● ●
RRT Algorithm
Kinodynamic planning with RRTs
So far, we have assumed that the system has no dynamics
– the system can instantaneously move in any direction in c-space – but what if that’s not true?
Consider the Dubins car:
– c-space: x-y position and velocity, angle
– control forward velocity and steering angle – plan a path through c-space with the corresponding control signals
where:
x_t – state (x/y position and velocity, steering angle) u_t – control signal (forward velocity, steering angle)
RRT Algorithm
Replace highlighted line with a feasible trajectory
– e.g., try a small set of feasible actions/controls starting from qnear ,
add trajectory that results in configuration closest to qtarget
Left-turn only forward car
Hovercraft with 2 Thusters
RRT probabilistic completeness
Notice that this is exactly the same bound as for PRM.
112
RRT does not find optimal paths
Does this algorithm work?
Is it complete? – depends on n
Is it optimal? – depends on the graph, and the search algorithm
114
Is there a version of RRT that is optimal?
Yes: RRG and RRT*
Don’t just
connect to
Attempt to connect to every vertex within a radius r
Is there a version of RRT that is optimal?
Don’t just
connect to
Attempt to connect to every vertex within a radius r
RRG Properties
RRG is probabilistically complete and asympototically optimal.
But, why might RRT still be preferable to RRG?
RRT* Algorithm
Don’t just
connect to
Attempt to connect to every vertex within a radius r
Get position and cost of min-cost vertex in
Rewire parents of nodes in to go through if that’s faster
RRT* Algorithm
Don’t just
connect to
Attempt to connect to every vertex within a radius r
Get position and cost of min-cost vertex in
Rewire parents of nodes in to go through if that’s faster
RRT* Algorithm
Don’t just
connect to
Attempt to connect to every vertex within a radius r
Get position and cost of min-cost vertex in
Rewire parents of nodes in to go through if that’s faster
RRT* Algorithm
Don’t just
connect to
Attempt to connect to every vertex within a radius r
Get position and cost of min-cost vertex in
Rewire parents of nodes in to go through if that’s faster
RRT* Algorithm
Don’t just
connect to
Attempt to connect to every vertex within a radius r
Get position and cost of min-cost vertex in
Rewire parents of nodes in to go through if that’s faster
RRT* Algorithm
Don’t just
connect to
Attempt to connect to every vertex within a radius r
Get position and cost of min-cost vertex in
Rewire parents of nodes in to go through if that’s faster
RRT* Algorithm
Don’t just
connect to
Attempt to connect to every vertex within a radius r
Get position and cost of min-cost vertex in
Rewire parents of nodes in to go through if that’s faster
RRT* Algorithm
Path Smoothing
Paths produced by sample based planners are generally not smooth
– RRT* and PRM* converge to optimal paths in the limit, but it’s generally not possible to run these algorithms long enough to converge.
Smoothing the path
127
Smoothing the path
128
Smoothing the path
129
Smoothing the path
130
Smoothing the path
131
Smoothing the path
132
Smoothing the path
133
Smoothing the path
134
Smoothing the path
135
Smoothing the path
136
Smoothing the path
137
Smoothing the path
138
139
140