0% found this document useful (0 votes)
35 views65 pages

1 AdvancedRobotics Motion Planning 2

Copyright
© © All Rights Reserved
We take content rights seriously. If you suspect this is your content, claim it here.
Available Formats
Download as PDF, TXT or read online on Scribd
0% found this document useful (0 votes)
35 views65 pages

1 AdvancedRobotics Motion Planning 2

Copyright
© © All Rights Reserved
We take content rights seriously. If you suspect this is your content, claim it here.
Available Formats
Download as PDF, TXT or read online on Scribd
You are on page 1/ 65

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
[email protected]

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
[email protected]

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
[email protected]

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
[email protected]

ecam.fr 21

You might also like