An Efficient Motion Planner
Based on Random Sampling
Jean-Claude Latombe
Computer Science Department
Stanford University
Main Collaborators

Lydia Kavraki (Rice U.)

David Hsu (U. of North Carolina, Chapel Hill)

Gildardo Sanchez (ITESM, Mexico)

James Kuffner (U. of Tokyo)

Rajeev Motwani (Stanford U.)
Goal of Motion Planning
Answer queries about the connectivity of a space
Possible Constraints

Collision-free

Stability

Kino-dynamic

Visibility
The Beginning …
Shakey (Nilsson, 1969): Visibility graph
Configuration Space
Represent the robot as a point in a parameter space
Why Sampling-Based Planning?

Computing an explicit representation of the
collision-free space is extremely time consuming
and impractical

There exist fast collision-checking algorithms to
test whether any given configuration or short path
is collision-free, or not (0.001 sec or less)
Outline

General Approach

Specific Planner

Experimental Results

Other Applications
Probabilistic Roadmap (PRM)
admissible space
milestone
mg
mb
[Kavraki, Svetska, Latombe,Overmars, 95]
Relation to Art-Gallery Problems
[Kavraki, Latombe, Motwani, Raghavan, 95]
Narrow Passage Issue
Difficult
Easy
Probabilistic Completeness
Under generally satisfied assumptions,
if a solution path exists, the probability that a
PRM planner fails to find one goes to 0
exponentially in the number of milestones.
Full completeness
 Too costly
Heuristic
 Too unreliable
Probabilistic completeness
 Fast and reliable
Key Techniques

Collision checking / Distance computation

Sampling strategies
Key Techniques

Collision checking / Distance computation
 Hierarchical approach
 Feature-based approach

Sampling strategies
Hierarchical Collision Checking
Three-Dimensional Case
Collision Checking
Collision Checking
Performance

Collision checking takes between 0.0001 and .002
seconds for 2 objects of 500,000 triangles each on
a 1-GHz Pentium III

Collision checking is faster when objects collide
or are far apart, and gets slower when they get
closer without colliding

Overall collision checking time grows roughly as
the log of the number of triangles
Key Techniques

Collision checking / Distance computation

Sampling strategies







Multi-stage strategies
Obstacle-sensitive strategies
Multiple vs. single query strategies
Configuration vs. control sampling
Single vs. bi-directional sampling
Lazy collision checking
Probabilistic biases (e.g., medial axis transform)
Outline

General Approach

Specific Planner

Experimental Results

Other Applications
SBL Planner

Single-query
Does not pre-compute a roadmap [Hsu, Latombe, Motwani, 1997]

Bi-directional sampling
Constructs a roadmap by growing two trees of milestones
rooted at the input query configuration [Hsu, 2000]

Lazy collision checking
Postpone collision-checking operations until absolutely
needed [Bohlin and Kavraki, 2000]
SBL Planner
SBL Planner
m
m is picked at random among the milestones
with a probabilistic distribution inverse to the
local density of sampling
SBL Planner
SBL Planner
SBL Planner
SBL Planner
X
SBL Planner
The collision-checking work
is memorized
Why Postponing Collision Checking?
The a priori probability that a short edge be
collision-free is rather large
1.20
1.00
0.80
0.60
0.40
0.20
Length of the segm ent
0.7
0.66
0.62
0.58
0.5
0.54
0.46
0.42
0.38
0.3
0.34
0.26
0.22
0.18
0.14
0.1
0.06
0.00
0.02
Ratio of rejections / total

Why Postponing Collision Checking?

The a priori probability that a short edge be
collision-free is rather large

The test of an edge is most expensive when it is
actually collision-free

Most edges of a roadmap do not end up in a
solution path
Path Optimization

Problems

Remedy
– too few vertices: get stuck
– remove as many vertices
as possible
– too many vertices: slow
– add vertices as needed
Outline

General Approach

Specific Planner

Experimental Results

Other Applications
Single-Robot Examples
nrob = 5,000 and nobs = 21,000
nrob = 3,000 and nobs = 50,000
nrob = 3,000 and nobs = 100
nrob = 3,000; nobs = 50
nrob = 5,000; nobs = 83,000
Videos
nrobot =5,000; nobst = 21,000
Tav = 0.6 s
Videos
nrobot =5,000; nobst = 83,000
Tav = 4.42 s
nrobot =3,000; nobst = 50,000
Tav = 0.17 s
Videos
nrobot =3,000; nobst = 100
Tav = 6.99 s
nrobot =3,000; nobst = 50,000
Tav = 4.45 s
Experimental Data on One Example
nrob = 5,000
nobs = 21,000
Running
Time(secs)
Milestones in
Roadmap
Milestones
in Path
0.36
1.19
0.4
0.64
1.09
0.78
0.51
0.46
0.46
0.63
112
216
95
167
200
178
150
67
104
194
9
21
9
18
10
20
14
15
16
13
Total Nr of
Collision Checks
Collision Checks
on the Path
(1 GHz Pentium III processor)
934
3170
884
1701
2625
2038
1307
1112
1213
1499
247
602
234
461
272
520
411
377
420
322
Sampled
Milestones
Comput. Time for
Coll-Check (secs)
174
334
148
265
311
260
239
100
156
329
0.36
1.17
0.4
0.64
1.06
0.76
0.5
0.45
0.46
0.62
Average Performance
1d
1c
1b
1a
1e
Averages over 100 runs
Example
1a
1b
1c
1d
1e
Running
Time(secs)
0.60
4.45
4.42
0.17
6.99
Milestones in
Roadmap
159
1609
1405
33
4160
Milestones
in Path
13
39
24
10
44
(1GHz Pentium III processor)
Total Nr of
Collision Checks
1483
11211
7267
406
12228
Collision Checks
on the Path
342
411
277
124
447
Sampled
Milestones
245
7832
3769
47
6990
Comput. Time for
Coll-Check (secs)
0.58
4.21
4.17
0.17
6.30
Std. Deviation
for running time
0.38
2.48
1.86
0.07
3.55
Convergence of SBL
Manufacture Cell
Metal Sheet
Weld
250
250
200
200
200
150
150
150
100
100
100
50
50
50
250
0
0
0
0
500
1000
1500
0
500
1000
1500
0
100
200
300
400
Impact of Lazy Collision Checking
Average performance with lazy collision checking
Example
1a
1b
1c
1d
1e
Running
Time(secs)
0.60
4.45
4.42
0.17
6.99
Milestones in
Roadmap
159
1609
1405
33
4160
Milestones
in Path
13
39
24
10
44
Total Nr of
Collision Checks
1483
11211
7267
406
12228
Collision Checks
on the Path
342
411
277
124
447
Sampled
Milestones
Comput. Time for
Coll-Check (secs)
245
7832
3769
47
6990
0.58
4.21
4.17
0.17
6.30
Std. Deviation
for running time
0.38
2.48
1.86
0.07
3.55
Average performance without lazy collision checking
Example
1a
1b
1c
1d
1e
Running
Milestones in
Time(secs)
Roadmap
2.82
106.20
18.46
1.03
293.77
22
3388
771
29
6737
Milestones
in Path
5
32
16
9
24
Total Nr of
Collision Checks
7425
300060
38975
2440
666084
Collision Checks
on the Path
173
421
219
123
300
Sampled
Milestones
83
9504
3793
46
11971
Comput. Time for
Std. Deviation
Coll-Check (secs) for running time
2.81
105.56
18.35
1.02
292.40
3.01
59.30
15.34
0.70
122.75
Multi-Robot Spot Welding
Typical Problem
Video
Average Running Times
Problem
PI- 2 Robots
PII- 2 Robots
PIII-2 Robots
Running time
(secs)
0.26
0.25
2.44
PI-4 Robots
PII-4 Robots
PIII-4 Robots
PI-6 Robots
PII-6 Robots
PIII-6 Robots
Milestones in
Roadmap
Milestones
in Path
Total Nr of
Collision Checks
Collision Checks
on the Path
Sampled
Milestones
Comput. Time for
Coll-Check (secs)
Std. Deviation
for running time
11
11
191
4
5
17
242
248
2356
58
76
243
18
13
718
0.26
0.25
2.41
0.52
0.17
1.57
3.97
3.94
30.82
62
56
841
7
10
32
1015
968
8895
106
166
542
193
112
2945
3.96
3.93
30.57
5.67
2.40
15.55
28.91
59.65
442.85
322
882
5648
14
30
91
3599
6891
47384
212
533
1525
1083
1981
24511
28.82
59.41
439.39
28.91
31.08
170.46
(1 GHz processor)
Centralized vs. Decoupled Planning
Averages over 20 runs
PROBLEM I
Planner
PROBLEM II
PROBLEM III
2 Robots
4 Robots
6 Robots
2 Robots
4 Robots
6 Robots
2 Robots
4 Robots
6 Robots
Time(s) Failures Time(s) Failures Time(s) Failures Time(s) Failures Time(s) Failures Time(s) Failures Time(s) Failures Time(s) Failures Time(s) Failures
Centralized
0.26
0
3.97
0
28.91
0
0.25
0
3.94
0
59.65
0
2.44
0
30.81
0
442.85
0
Dec. Global
0.22
1
2.74
3
29.53
7
0.37
2
6.59
4
65.45
6
4.32
5
16.23
6
267.81
13
Dec. Pairwise
0.30
3
4.85
5
19.23
9
0.42
3
5.63
7
28.92
6
3.42
9
25.35
13
182.63
17
Outline

General Approach

Specific Planner

Experimental Results

Other Applications
Design for Manufacturing/Servicing
General Motors
General Motors
General Electric
[Hsu, 2000]
Radio-Surgical Planning
Cyberknife System (Accuray, Inc.)
CARABEAMER Planner
[Tombropoulos, Adler, and Latombe, 1997]
Visibility constraints
Radio-Surgical Planning
• 2000 < Tumor < 2200
T
B1
B2
2000 < B2 + B4 < 2200
2000 < B4 < 2200
2000 < B3 + B4 < 2200
2000 < B3 < 2200
2000 < B1 + B3 + B4 < 2200
2000 < B1 + B4 < 2200
2000 < B1 + B2 + B4 < 2200
2000 < B1 < 2200
2000 < B1 + B2 < 2200
C
B4
B3
• 0 < Critical < 500
0 < B2 < 500
Radio-Surgical Planning
50% Isodose Surface
80% Isodose Surface
Conventional system’s plan
CARABEAMER’s plan
Cyberknife Systems
Stanford Report, July 25, 2001
Contact
Stanford Report
News
Servic
e
/Press
Releas
es
Patients gather to praise minimally
invasive technique used in treating tumors
By MICHELLE BRANDT
When Jeanie Schmidt, a critical care nurse from Foster City, lost hearing in her left ear and experienced
numbing in her face, she prayed that her first instincts were off. “I said to the doctor, `I think I have an
acoustic neuroma (a brain tumor), but I'm hoping I'm wrong. Tell me it's wax, tell me it's anything,'” Schmidt
recalled.
It wasn't wax, however, and Schmidt – who wound up in the Stanford Hospital emergency room when her
symptoms worsened – was quickly forced to make a decision regarding treatment for her tumor.
On July 13, Schmidt found herself back at Stanford – but this time with a group of patients who were treated
with the same minimally invasive treatment that Schmidt ultimately chose: the CyberKnife. She was one of 40
former patients who met with Stanford faculty and staff to discuss their experiences with the CyberKnife – a
radiosurgery system designed at Stanford by John Adler Jr., MD, in 1994 for performing neurosurgeries without
incisions.
“I wanted the chance to thank everyone again and to share experiences with other patients,” said Schmidt,
who had the procedure on June 20 and will have an MRI in six months to determine its effectiveness. “I feel
really lucky that I came along when this technology was around.”
The CyberKnife is the newest member of the radiosurgery family. Like its ancestor, the 33-year-old Gamma
Knife, the CyberKnife uses 3-D computer targeting to deliver a single, large dose of radiation to the tumor in an
outpatient setting. But unlike the Gamma Knife – which requires patients to wear an external frame to keep
their head completely immobile during the procedure – the CyberKnife can make real-time adjustments to
body movements so that patients aren't required to wear the bulky, uncomfortable head gear.
The procedure provides patients an alternative to both difficult, risky surgery and conventional radiation
therapy, in which small doses of radiation are delivered each day to a large area. The procedure is used to treat
a variety of conditions – including several that can't be treated by any other procedure – but is most commonly
used for metastases (the most common type of brain tumor in adults), meningomas (tumors that develop from
Since January 1999, more than
335 patients have been treated at Stanford with the CyberKnife.
the membranes that cover the brain), and acoustic neuromas.
Modular Reconfigurable Robots
Casal and Yim, 1999
Xerox, Parc
Humanoid Robot
[Kuffner and Inoue, 2000] (U. Tokyo)
Stability constraints
Space Robotics
robot
obstacles
air thrusters
gas tank
air bearing
[Kindel, Hsu, Latombe, and Rock, 2000]
Dynamic constraints
Total duration : 40 sec
Autonomous Helicopter
[Feron, 2000] (AA Dept., MIT)
Interacting Nonholonomic Robots
q2
y2
q1
d
y1
x1
(Grasp Lab - U. Penn)
x2
Map Building
[Gonzalez, 2000]
Next-Best View Computation
Map Building
[Gonzalez, 2000]
Map Building
[Gonzalez, 2000]
Graphic Animation of Digital Actors
The Motion
Factory
[Koga, Kondo, Kuffner, and Latombe, 1994]
Prediction of Molecular Motions
Ligand-protein binding
[Singh, Latombe, and Brutlag, 1999]
Outline

General Approach

Specific Planner

Experimental Results

Other Applications

Conclusion
Conclusion

Probabilistic Roadmaps provide an efficient and
reliable computational approach to motion
planning

PRM planners are rather easy to implement

They have been experimented on very different
problems
Remaining Issues

Relatively large standard deviation of
planning time

No rigorous termination criterion when
no solution is found

New challenging applications …
Optimal Touring of Multiple Goals
Surgical Planning with Soft Tissue
Planning Nice-Looking Motions
A Bug’s Life (Pixar/Disney)
Tomb Raider 3 (Eidos Interactive)
Toy Story (Pixar/Disney)
The Legend of Zelda (Nintendo)
Antz (Dreamworks)
Final Fantasy VIII (SquareOne)
1,000s of Degrees of Freedom
Protein folding