Advanced Robotics

Motion planning

Author : Guillaume Gibert

Cursus : EENG4
UE : Advanced Robotics
Version : 1.0
Date : 28/11/2022

ecam.fr
 Advanced Robotics – Motion planning

Motion planning

 Motion planning 𝒒𝟐
• Computing a path that gets the robot from point A 𝒒𝟑
(start) to point B (destination)
• Also called path planning 𝒒𝟏
A
• Cartesian control is an example of motion planning

ecam.fr
 Advanced Robotics – Motion planning

Motion planning

 Motion planning 𝒒𝟐
• Computing a path that gets the robot from point A 𝒒𝟑
(start) to point B (destination)
• Also called path planning 𝒒𝟏
A
• Cartesian control is an example of motion planning
 Obstacle(s)
• Straight line may not be possible B
• Robot needs to go around the obstacle
• Plan a path to avoid collision
• Same issue with singularities

ecam.fr
 Advanced Robotics – Motion planning

Task vs Configuration spaces

 Task space
• Also called Cartesian space
• End-effector position 𝒒𝟐 𝒒𝟐
• 𝑋 = 𝑥, 𝑦 𝑇

 Configuration space 𝒚
• Also called Joint space
• Vector of joint values
𝑇 𝒒𝟏
• 𝑞 = 𝑞1 , 𝑞2
𝒙
𝒒𝟏

ecam.fr
 Advanced Robotics – Motion planning

Obstacle

 Hypothesis
• Infinitesimally small
 Question 𝒒𝟐 𝒒𝟐
• Given an obstacle in Cartesian
space 𝒚
• What are the robot configurations
that generate a collision?
𝒒𝟏
𝒙
𝒒𝟏

ecam.fr
 Advanced Robotics – Motion planning

Obstacle

 Hypothesis
• Infinitesimally small
 Question 𝒒𝟐
• Given an obstacle in Cartesian
space 𝒚
• What are the robot configurations
that generate a collision? 𝒒𝟐
𝒒𝟏
𝒙
𝒒𝟏

ecam.fr
 Advanced Robotics – Motion planning

Obstacle

 Hypothesis
• Infinitesimally small
 Question 𝒒𝟐
• Given an obstacle in Cartesian 𝒒𝟐
space 𝒚
• What are the robot configurations
that generate a collision?
𝒒𝟏
𝒙
𝒒𝟏

ecam.fr
 Advanced Robotics – Motion planning

Obstacle

 Hypothesis
• Infinitesimally small
 Question 𝒒𝟐
• Given an obstacle in Cartesian
space 𝒚
• What are the robot configurations
that generate a collision?

𝒒𝟏 𝒙
𝒒𝟐 𝒒𝟏

ecam.fr
 Advanced Robotics – Motion planning

Obstacle

 Hypothesis
• Infinitesimally small
 Question 𝒒𝟐
• Given an obstacle in Cartesian
space 𝒚
• What are the robot configurations
that generate a collision?
 Curve of illegal configurations
𝒒𝟏 𝒙
𝒒𝟐 𝒒𝟏

ecam.fr
 Advanced Robotics – Motion planning

Obstacle

 Hypothesis
• Circle with a non-null radius
 Question 𝒒𝟐
• Given an obstacle in Cartesian
space 𝒚
• What are the robot configurations
that generate a collision?

𝒙
𝒒𝟏

ecam.fr
 Advanced Robotics – Motion planning

Obstacle

 Hypothesis
• Circle with a non-null radius
 Question 𝒒𝟐
• Given an obstacle in Cartesian
space 𝒚
• What are the robot configurations
that generate a collision?
𝒒𝟏
𝒙
𝒒𝟏

ecam.fr
 Advanced Robotics – Motion planning

Obstacle

 Hypothesis
• Circle with a non-null radius
 Question 𝒒𝟐
• Given an obstacle in Cartesian
space 𝒚
• What are the robot configurations
that generate a collision? 𝒒𝟐
𝒒𝟏
𝒙
𝒒𝟏

ecam.fr
 Advanced Robotics – Motion planning

Obstacle

 Hypothesis
• Circle with a non-null radius
 Question 𝒒𝟐
• Given an obstacle in Cartesian
space 𝒚
• What are the robot configurations
that generate a collision? 𝒒𝟐
𝒒𝟏
𝒙
𝒒𝟏

ecam.fr
 Advanced Robotics – Motion planning

Obstacle

 Hypothesis
• Circle with a non-null radius
 Question 𝒒𝟐
• Given an obstacle in Cartesian
space 𝒚
• What are the robot configurations
that generate a collision? 𝒒𝟐
 Any configuration 𝑞1 , 𝑞2 that 𝒒𝟏
falls in between the two green 𝒙
curves is illegal 𝒒𝟏

ecam.fr
 Advanced Robotics – Motion planning

Simulator

 Developed by Prof. Alterovitz (UNC)

 Tool to experiment the different robot
spaces with obstacles
• Cartesian vs Joint
• Taskspace vs Configuration (C-space)
 In C-space, joint values can be > 2𝜋
 Link
• https://robotics.cs.unc.edu/C-space/

ecam.fr 15
 Thanks for your attention
guillaume.gibert@ecam.fr

ecam.fr 16
 Advanced Robotics
Motion planning

Author : Guillaume Gibert

Cursus : EENG4
UE : Advanced Robotics
Version : 1.0
Date : 28/11/2022

ecam.fr
 Advanced Robotics – Motion planning

Map representation
Polygonal Grid
y

S
S
x
Obstacle 1: Obstacle 2:
𝑎1 , 𝑏1 𝑎1 , 𝑏1 11111111 …
𝑎2 , 𝑏2 𝑎2 , 𝑏2 10000001 10001001
… … 10000001 10001001
𝑎𝑛 , 𝑏𝑛 𝑎𝑚 , 𝑏𝑚 10011001 10000001
… 11111111
ecam.fr 2
 Advanced Robotics – Motion planning

Pros & Cons

 Polygonal representation
• Low in memory
• High computation online
• Need to estimate the extent of the obstacle from vertices
 Grid representation
• High in memory
• 10x10m with 1cm accuracy means 1e6 cells
• Low online computation
• Is cell free or not?

ecam.fr 3
 Advanced Robotics – Motion planning

Robot arms
Joint3 Link3

End of Arm
 High dimensional C-space
(configuration space)
Discretizing map into grid is not tractable Link2
Polygons may be very hard to compute in Link1
C-space
 Localization on the map is an easy Joint2
task
Joint1

Link0
Base

ecam.fr 4
 Advanced Robotics – Motion planning

Motion planning
Joint3 Link3

End of Arm
 How can we plan a path with
these constraints?
Link2
Link1

Joint2

Joint1

Link0
Base

ecam.fr 5
 Advanced Robotics – Motion planning

Motion planning
Joint3 Link3

End of Arm
 How can we plan a path with these
constraints?
Point queries! Link2
 For a specific point in C-space (i.e. joint values), Link1
determine if the robot is in collision or not
Algorithms called “sampling-based motion Joint2
planning”
Joint1

Link0
Base

ecam.fr 6
 Advanced Robotics – Motion planning

Motion planning
Joint3 Link3

End of Arm
 How can we sample the C-space
efficiently?
Random exploration Link2
 Counterintuitive Link1
 Powerful tool for high dimension problems
 Unstuck in dead ends Joint2
Algorithms referred as stochastic motion planning
Joint1

Link0
Base

ecam.fr 7
 Advanced Robotics – Motion planning

References

 https://robotacademy.net.au/
 https://courses.edx.org/courses/course-
v1:ColumbiaX+CSMM.103x+3T2020/

ecam.fr 8
 Thanks for your attention
guillaume.gibert@ecam.fr

ecam.fr 9
 Advanced Robotics
Motion planning

Author : Guillaume Gibert

Cursus : EENG4
UE : Advanced Robotics
Version : 1.0
Date : 28/11/2022

ecam.fr
 Advanced Robotics – Motion planning

Rapidly-exploring Random Trees (RRT)

Insert Start point 𝑞2
in tree
G

S
𝑞1

ecam.fr 2
 Advanced Robotics – Motion planning

Rapidly-exploring Random Trees (RRT)

Insert Start point 𝑞2
in tree
Can tree connect to Goal? G

S
𝑞1

ecam.fr 3
 Advanced Robotics – Motion planning

Rapidly-exploring Random Trees (RRT)

Insert Start point 𝑞2
in tree
G

Sample random
point r in C-space
r

S
𝑞1

ecam.fr 4
 Advanced Robotics – Motion planning

Rapidly-exploring Random Trees (RRT)

Insert Start point 𝑞2
in tree
G

Sample random
point r in C-space
r

p
Find point p in tree
S
closest to r
𝑞1

ecam.fr 5
 Advanced Robotics – Motion planning

Rapidly-exploring Random Trees (RRT)

Insert Start point 𝑞2
in tree
G

Sample random
point r in C-space

Find point p in tree

S
closest to r
𝑞1
Discard or shorten Add branch of
branch if it predefined length
intersects obstacle from p to r

ecam.fr 6
 Advanced Robotics – Motion planning

Rapidly-exploring Random Trees (RRT)

Insert Start point 𝑞2
in tree
G

Sample random
point r in C-space

Find point p in tree

S
closest to r
𝑞1
Discard or shorten Add branch of
branch if it predefined length
intersects obstacle from p to r

Can tree connect to Goal?

ecam.fr 7
 Advanced Robotics – Motion planning

Rapidly-exploring Random Trees (RRT)

Insert Start point 𝑞2
in tree
G

Sample random
point r in C-space

Find point p in tree

S
closest to r
𝑞1
Discard or shorten Add branch of
branch if it predefined length
intersects obstacle from p to r

ecam.fr 8
 Advanced Robotics – Motion planning

Rapidly-exploring Random Trees (RRT)

Insert Start point 𝑞2
in tree
G

Sample random
point r in C-space

Find point p in tree

S
closest to r
𝑞1
Discard or shorten Add branch of
branch if it predefined length
intersects obstacle from p to r

ecam.fr 9
 Advanced Robotics – Motion planning

Rapidly-exploring Random Trees (RRT)

Insert Start point 𝑞2
in tree
G

Sample random
point r in C-space

Find point p in tree

S
closest to r
𝑞1
Discard or shorten Add branch of
branch if it predefined length
intersects obstacle from p to r

Can tree connect to Goal?

ecam.fr 10
 Advanced Robotics – Motion planning

Rapidly-exploring Random Trees (RRT)

Insert Start point 𝑞2
in tree
G

Sample random
point r in C-space

Find point p in tree

S
closest to r
𝑞1
Discard or shorten Add branch of
branch if it predefined length
intersects obstacle from p to r

ecam.fr 11
 Advanced Robotics – Motion planning

Rapidly-exploring Random Trees (RRT)

Insert Start point 𝑞2
in tree
G

Sample random
point r in C-space

Find point p in tree

S
closest to r
𝑞1
Discard or shorten Add branch of
branch if it predefined length
intersects obstacle from p to r

ecam.fr 12
 Advanced Robotics – Motion planning

Rapidly-exploring Random Trees (RRT)

Insert Start point 𝑞2
in tree
G

Sample random
point r in C-space

Find point p in tree

S
closest to r
𝑞1
Discard or shorten Add branch of
branch if it predefined length
intersects obstacle from p to r

Can tree connect to Goal?

ecam.fr 13
 Advanced Robotics – Motion planning

Rapidly-exploring Random Trees (RRT)

Insert Start point 𝑞2
in tree
G

Sample random
point r in C-space

Find point p in tree

S
closest to r
𝑞1
Discard or shorten Add branch of
branch if it predefined length
intersects obstacle from p to r
Add direct lines for
Compute path
any two points if
from start to goal
obstacle free
ecam.fr 14
 Advanced Robotics – Motion planning

Rapidly-exploring Random Trees (RRT)

Insert Start point 𝑞2
in tree
G

Sample random
point r in C-space

Find point p in tree

S
closest to r
𝑞1
Discard or shorten Add branch of
branch if it predefined length
intersects obstacle from p to r
Add direct lines for
Compute path
any two points if
from start to goal
obstacle free
ecam.fr 15
 Advanced Robotics – Motion planning

Rapidly-exploring Random Trees (RRT)

Insert Start point 𝑞2
in tree
G

Sample random
point r in C-space

Find point p in tree

S
closest to r
𝑞1
Discard or shorten Add branch of
branch if it predefined length
intersects obstacle from p to r
Add direct lines for
Compute path
any two points if
from start to goal
obstacle free
ecam.fr 16
 Advanced Robotics – Motion planning

RRT Features

 Tree grows in random direction

• If trapped in a corner, there will be a way around the obstacle
 Production-level implementation
• Create one tree from S and one tree from G
• Check if the 2 trees can connect
 Faster computation
 External functions
• Random number generator
• Intersect function
• Only have access to point queries
 Sampling, i.e. discretizing the branch, resolution ?

ecam.fr 17
 Advanced Robotics – Motion planning

References

 https://robotacademy.net.au/
 https://courses.edx.org/courses/course-
v1:ColumbiaX+CSMM.103x+3T2020/

ecam.fr 18
 Thanks for your attention
guillaume.gibert@ecam.fr

ecam.fr 19
 Advanced Robotics
Motion planning

Author : Guillaume Gibert

Cursus : EENG4
UE : Advanced Robotics
Version : 1.0
Date : 28/11/2022

ecam.fr
 Advanced Robotics – Motion planning

Probabilistic Road Maps (PRM)

 Probabilistic Road Maps 𝑞2

• Map construction
• Path planning using the map

𝑞1

ecam.fr 2
 Advanced Robotics – Motion planning

Map construction

𝑞2
Sample random
While # points < Threshold

point in C-space

Connect new point

to all other points 𝑞1
(free of obstacles)

ecam.fr 3
 Advanced Robotics – Motion planning

Map construction

𝑞2
Sample random
While # points < Threshold

point in C-space

Connect new point

to all other points 𝑞1
(free of obstacles)

ecam.fr 4
 Advanced Robotics – Motion planning

Map construction

𝑞2
Sample random
While # points < Threshold

point in C-space

Connect new point

to all other points 𝑞1
(free of obstacles)

ecam.fr 5
 Advanced Robotics – Motion planning

Map construction

𝑞2
Sample random
While # points < Threshold

point in C-space

Connect new point

to all other points 𝑞1
(free of obstacles)

ecam.fr 6
 Advanced Robotics – Motion planning

Map construction

𝑞2
Sample random
While # points < Threshold

point in C-space

Connect new point

to all other points 𝑞1
(free of obstacles)

ecam.fr 7
 Advanced Robotics – Motion planning

Map construction

𝑞2
Sample random
While # points < Threshold

point in C-space

Connect new point

to all other points 𝑞1
(free of obstacles)

ecam.fr 8
 Advanced Robotics – Motion planning

Map construction

𝑞2
Sample random
While # points < Threshold

point in C-space

Connect new point

to all other points 𝑞1
(free of obstacles)

ecam.fr 9
 Advanced Robotics – Motion planning

Map construction

𝑞2
Sample random
While # points < Threshold

point in C-space

Connect new point

to all other points 𝑞1
(free of obstacles)

ecam.fr 10
 Advanced Robotics – Motion planning

Map construction

𝑞2
Sample random
While # points < Threshold

point in C-space

Connect new point

to all other points 𝑞1
(free of obstacles)

ecam.fr 11
 Advanced Robotics – Motion planning

Path planning

𝑞2
Place Goal point
Place Start point S
G

Connect S to the Connect G to the

nearest roadmap nearest roadmap
point point

Find a path 𝑞1
between S and G
on the roadmap

ecam.fr 12
 Advanced Robotics – Motion planning

Path planning

𝑞2
Place Goal point
Place Start point S
G

Connect S to the Connect G to the

nearest roadmap nearest roadmap
point point S

Find a path 𝑞1
between S and G
on the roadmap

ecam.fr 13
 Advanced Robotics – Motion planning

Path planning

𝑞2
Place Goal point
Place Start point S
G

Connect S to the Connect G to the

nearest roadmap nearest roadmap
point point
S

Find a path 𝑞1
between S and G
on the roadmap

ecam.fr 14
 Advanced Robotics – Motion planning

Path planning

𝑞2
Place Goal point G
Place Start point S
G

Connect S to the Connect G to the

nearest roadmap nearest roadmap
point point
S

Find a path 𝑞1
between S and G
on the roadmap

ecam.fr 15
 Advanced Robotics – Motion planning

Path planning

𝑞2
Place Goal point G
Place Start point S
G

Connect S to the Connect G to the

nearest roadmap nearest roadmap
point point
S

Find a path 𝑞1
between S and G
on the roadmap

ecam.fr 16
 Advanced Robotics – Motion planning

Path planning

𝑞2
Place Goal point G
Place Start point S
G

Connect S to the Connect G to the

nearest roadmap nearest roadmap
point point
S

Find a path 𝑞1
between S and G
on the roadmap

ecam.fr 17
 Advanced Robotics – Motion planning

PRM Features

 Random exploration
• Space well covered with a minimal number of points
 Potential issues
• Roadmap has 2 separated components
• Start point cannot hop onto the roadmap
 Add random points (and branches) to your map
 External functions
• Random number generator
• Intersect function
• Only have access to point queries
 Sampling, i.e. discretizing the branch, resolution ?
• Find a path on the roadmap

ecam.fr 18
 Advanced Robotics – Motion planning

Summary

 Most problems in 6-7 DoF robot arms can be solved with PRM or
RRT in a few seconds
• Requires the ability to check if a point in C-space is legal or not
• Many algorithms are probabilistically complete
• If a path exists, it will be found in a finite amount of time
• But there is no guarantee that it may not be longer than exhaustive search
• There is no guarantee about the quality of the solution
• Shortest path possible
• Post processing are necessary to eliminate zig zag

ecam.fr 19
 Advanced Robotics – Motion planning

References

 https://robotacademy.net.au/
 https://courses.edx.org/courses/course-
v1:ColumbiaX+CSMM.103x+3T2020/

ecam.fr 20
 Thanks for your attention
guillaume.gibert@ecam.fr

ecam.fr 21