[SOLVED] CS arm algorithm CS 4610/5335

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 Dijkstras 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 cant reach q_new:
just go as far as you can and create a new vertex there

RRT versus a naive random tree
RRT
Naive random tree
Growing the naive 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 thats 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*
Dont just
connect to
Attempt to connect to every vertex within a radius r

Is there a version of RRT that is optimal?
Dont 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
Dont 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 thats faster

RRT* Algorithm
Dont 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 thats faster

RRT* Algorithm
Dont 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 thats faster

RRT* Algorithm
Dont 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 thats faster

RRT* Algorithm
Dont 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 thats faster

RRT* Algorithm
Dont 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 thats faster

RRT* Algorithm
Dont 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 thats 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 its 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