An Efficient Motion An Efficient Motion PlannerPlanner
Based on Random Based on Random SamplingSampling
Jean-Claude LatombeJean-Claude Latombe
Computer Science DepartmentComputer Science DepartmentStanford UniversityStanford University
Main CollaboratorsMain Collaborators
Lydia Kavraki (Rice U.)Lydia Kavraki (Rice U.)
David Hsu (U. of North Carolina, Chapel Hill)David Hsu (U. of North Carolina, Chapel Hill)
Gildardo Sanchez (ITESM, Mexico)Gildardo Sanchez (ITESM, Mexico)
James Kuffner (U. of Tokyo)James Kuffner (U. of Tokyo)
Rajeev Motwani (Stanford U.)Rajeev Motwani (Stanford U.)
Goal of Motion PlanningGoal of Motion Planning
Answer queries about the Answer queries about the connectivityconnectivity of a space of a space
Possible ConstraintsPossible Constraints Collision-freeCollision-free
Kino-dynamicKino-dynamic
StabilityStability
VisibilityVisibility
The Beginning …The Beginning …
Shakey (Nilsson, 1969): Visibility graphShakey (Nilsson, 1969): Visibility graph
Configuration SpaceConfiguration Space
Represent the robot as a point in a parameter spaceRepresent the robot as a point in a parameter space
Why Sampling-Based Planning?Why Sampling-Based Planning?
Computing an explicit representation of the Computing an explicit representation of the collision-free space is extremely time consuming collision-free space is extremely time consuming and impracticaland impractical
There exist fast collision-checking algorithms to There exist fast collision-checking algorithms to test whether any given configuration or short path test whether any given configuration or short path is collision-free, or not (0.001 sec or less)is collision-free, or not (0.001 sec or less)
OutOutlineline General ApproachGeneral Approach
Specific PlannerSpecific Planner
Experimental ResultsExperimental Results
Other ApplicationsOther Applications
Probabilistic Roadmap (PRM)Probabilistic Roadmap (PRM)
admissible space
mmbb
mmgg
milestone
[Kavraki, Svetska, Latombe,Overmars, 95][Kavraki, Svetska, Latombe,Overmars, 95]
Relation to Art-Gallery ProblemsRelation to Art-Gallery Problems
[Kavraki, Latombe, Motwani, Raghavan, 95]
Narrow Passage IssueNarrow Passage Issue
EasyEasyDifficultDifficult
Probabilistic CompletenessProbabilistic Completeness
Under generally satisfied assumptions, Under generally satisfied assumptions, if a solution path exists, the probability that a if a solution path exists, the probability that a PRM planner fails to find one goes to 0 PRM planner fails to find one goes to 0 exponentially in the number of milestones.exponentially in the number of milestones.
Full completenessFull completeness Too costlyToo costly
HeuristicHeuristic Too unreliableToo unreliable
Probabilistic completenessProbabilistic completeness Fast and reliableFast and reliable
Key TechniquesKey Techniques
Collision checking / Distance computationCollision checking / Distance computation
Sampling strategiesSampling strategies
Key TechniquesKey Techniques
Collision checking / Distance computationCollision checking / Distance computation
Hierarchical approachHierarchical approach Feature-based approachFeature-based approach
Sampling strategiesSampling strategies
Hierarchical Collision CheckingHierarchical Collision Checking
Three-Dimensional CaseThree-Dimensional Case
Collision CheckingCollision Checking
Collision CheckingCollision Checking
PerformancePerformance
Collision checking takes between 0.0001 and .002 Collision checking takes between 0.0001 and .002 seconds for 2 objects of 500,000 triangles each on seconds for 2 objects of 500,000 triangles each on a 1-GHz Pentium IIIa 1-GHz Pentium III
Collision checking is faster when objects collide Collision checking is faster when objects collide or are far apart, and gets slower when they get or are far apart, and gets slower when they get closer without collidingcloser without colliding
Overall collision checking time grows roughly as Overall collision checking time grows roughly as the the loglog of the number of triangles of the number of triangles
Key TechniquesKey Techniques Collision checking / Distance computationCollision checking / Distance computation
Sampling strategiesSampling strategies Multi-stage strategiesMulti-stage strategies Obstacle-sensitive strategies Obstacle-sensitive strategies Multiple vs. single query strategiesMultiple vs. single query strategies Configuration vs. control samplingConfiguration vs. control sampling Single vs. bi-directional samplingSingle vs. bi-directional sampling Lazy collision checkingLazy collision checking Probabilistic biases (e.g., medial axis transform)Probabilistic biases (e.g., medial axis transform)
OutlineOutline
General ApproachGeneral Approach
Specific PlannerSpecific Planner
Experimental ResultsExperimental Results
Other ApplicationsOther Applications
SBL PlannerSBL Planner
SSingle-queryingle-query
Does not pre-compute a roadmap Does not pre-compute a roadmap [Hsu, Latombe, Motwani, 1997][Hsu, Latombe, Motwani, 1997]
BBi-directional samplingi-directional sampling
Constructs a roadmap by growing two trees of milestones Constructs a roadmap by growing two trees of milestones rooted at the input query configuration rooted at the input query configuration [Hsu, 2000][Hsu, 2000]
LLazy collision checkingazy collision checking
Postpone collision-checking operations until absolutely Postpone collision-checking operations until absolutely needed needed [Bohlin and Kavraki, 2000][Bohlin and Kavraki, 2000]
SBL PlannerSBL Planner
SBL PlannerSBL Planner
mm
mm is picked at random among the milestones is picked at random among the milestoneswith a probabilistic distribution inverse to thewith a probabilistic distribution inverse to thelocal density of samplinglocal density of sampling
SBL PlannerSBL Planner
SBL PlannerSBL Planner
SBL PlannerSBL Planner
SBL PlannerSBL Planner
XX
SBL PlannerSBL Planner
The collision-checking workThe collision-checking workis memorizedis memorized
Why Postponing Collision Checking?Why Postponing Collision Checking? The a priori probability that a short edge be The a priori probability that a short edge be
collision-free is rather large collision-free is rather large
0.00
0.20
0.40
0.60
0.80
1.00
1.20
0.02
0.06 0.
1
0.14
0.18
0.22
0.26 0.
3
0.34
0.38
0.42
0.46 0.
5
0.54
0.58
0.62
0.66 0.
7
Length of the segm ent
Rat
io o
f re
ject
ion
s / t
ota
l
Why Postponing Collision Checking?Why Postponing Collision Checking? The a priori probability that a short edge be The a priori probability that a short edge be
collision-free is rather large collision-free is rather large
The test of an edge is most expensive when it is The test of an edge is most expensive when it is actually collision-freeactually collision-free
Most edges of a roadmap do not end up in a Most edges of a roadmap do not end up in a solution pathsolution path
Path OptimizationPath Optimization
ProblemsProblems
– too few vertices: get stucktoo few vertices: get stuck
– too many vertices: slowtoo many vertices: slow
RemedyRemedy
– remove as many vertices remove as many vertices as possibleas possible
– add vertices as neededadd vertices as needed
OutlineOutline
General ApproachGeneral Approach
Specific PlannerSpecific Planner
Experimental ResultsExperimental Results
Other ApplicationsOther Applications
Single-Robot ExamplesSingle-Robot Examples
nrob = 5,000 and nobs = 21,000 nrob = 5,000; nobs = 83,000 nrob = 3,000 and nobs = 50,000
nrob = 3,000 and nobs = 100 nrob = 3,000; nobs = 50
VideosVideos
nrobot =5,000; nobst = 21,000
Tav = 0.6 s
VideosVideos
nrobot =5,000; nobst = 83,000
Tav = 4.42 s
nrobot =3,000; nobst = 50,000
Tav = 0.17 s
VideosVideos
nrobot =3,000; nobst = 50,000
Tav = 4.45 s
nrobot =3,000; nobst = 100
Tav = 6.99 s
Experimental Data on One ExampleExperimental Data on One Example
(1 GHz Pentium III processor)(1 GHz Pentium III processor)
Running Milestones in Milestones Total Nr of Collision Checks Sampled Comput. Time forTime(secs) Roadmap in Path Collision Checks on the Path Milestones Coll-Check (secs)
0.36 112 9 934 247 174 0.361.19 216 21 3170 602 334 1.170.4 95 9 884 234 148 0.40.64 167 18 1701 461 265 0.641.09 200 10 2625 272 311 1.060.78 178 20 2038 520 260 0.760.51 150 14 1307 411 239 0.50.46 67 15 1112 377 100 0.450.46 104 16 1213 420 156 0.460.63 194 13 1499 322 329 0.62
nrob = 5,000
nobs = 21,000
Average PerformanceAverage Performance
1a1a 1b1b 1c1c 1d1d 1e1e
(1GHz Pentium III processor)(1GHz Pentium III processor)
Averages over 100 runsAverages over 100 runsExample Running Milestones in Milestones Total Nr of Collision Checks Sampled Comput. Time for Std. Deviation
Time(secs) Roadmap in Path Collision Checks on the Path Milestones Coll-Check (secs) for running time1a 0.60 159 13 1483 342 245 0.58 0.381b 4.45 1609 39 11211 411 7832 4.21 2.481c 4.42 1405 24 7267 277 3769 4.17 1.861d 0.17 33 10 406 124 47 0.17 0.071e 6.99 4160 44 12228 447 6990 6.30 3.55
Convergence of SBLConvergence of SBL
Weld
0
50
100
150
200
250
0 500 1000 1500
Metal Sheet
0
50
100
150
200
250
0 500 1000 1500
Manufacture Cell
0
50
100
150
200
250
0 100 200 300 400
Impact of Lazy Collision CheckingImpact of Lazy Collision Checking
Example Running Milestones in Milestones Total Nr of Collision Checks Sampled Comput. Time for Std. DeviationTime(secs) Roadmap in Path Collision Checks on the Path Milestones Coll-Check (secs) for running time
1a 0.60 159 13 1483 342 245 0.58 0.381b 4.45 1609 39 11211 411 7832 4.21 2.481c 4.42 1405 24 7267 277 3769 4.17 1.861d 0.17 33 10 406 124 47 0.17 0.071e 6.99 4160 44 12228 447 6990 6.30 3.55
Example Running Milestones in Milestones Total Nr of Collision Checks Sampled Comput. Time for Std. DeviationTime(secs) Roadmap in Path Collision Checks on the Path Milestones Coll-Check (secs) for running time
1a 2.82 22 5 7425 173 83 2.81 3.01 1b 106.20 3388 32 300060 421 9504 105.56 59.30 1c 18.46 771 16 38975 219 3793 18.35 15.34 1d 1.03 29 9 2440 123 46 1.02 0.70 1e 293.77 6737 24 666084 300 11971 292.40 122.75
Average performance with lazy collision checkingAverage performance with lazy collision checking
Average performance without lazy collision checkingAverage performance without lazy collision checking
Multi-Robot Spot WeldingMulti-Robot Spot Welding
Typical ProblemTypical Problem
VideoVideo
Average Running TimesAverage Running Times
(1 GHz processor)(1 GHz processor)
Problem Running time Milestones in Milestones Total Nr of Collision Checks Sampled Comput. Time for Std. Deviation(secs) Roadmap in Path Collision Checks on the Path Milestones Coll-Check (secs) for running time
PI- 2 Robots 0.26 11 4 242 58 18 0.26 0.52PII- 2 Robots 0.25 11 5 248 76 13 0.25 0.17PIII-2 Robots 2.44 191 17 2356 243 718 2.41 1.57
PI-4 Robots 3.97 62 7 1015 106 193 3.96 5.67PII-4 Robots 3.94 56 10 968 166 112 3.93 2.40PIII-4 Robots 30.82 841 32 8895 542 2945 30.57 15.55
PI-6 Robots 28.91 322 14 3599 212 1083 28.82 28.91PII-6 Robots 59.65 882 30 6891 533 1981 59.41 31.08PIII-6 Robots 442.85 5648 91 47384 1525 24511 439.39 170.46
Centralized vs. Decoupled PlanningCentralized vs. Decoupled Planning
Planner
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
6 Robots
PROBLEM III
2 Robots 4 Robots 6 Robots
PROBLEM II
2 Robots 4 Robots2 Robots 4 Robots 6 Robots
PROBLEM I
Averages over 20 runsAverages over 20 runs
OutlineOutline
General ApproachGeneral Approach
Specific PlannerSpecific Planner
Experimental ResultsExperimental Results
Other ApplicationsOther Applications
Design for Manufacturing/ServicingDesign for Manufacturing/Servicing
General ElectricGeneral Electric
General MotorsGeneral MotorsGeneral MotorsGeneral Motors
[Hsu, 2000][Hsu, 2000]
Radio-Surgical PlanningRadio-Surgical Planning
Cyberknife System (Accuray, Inc.) Cyberknife System (Accuray, Inc.) CARABEAMER Planner CARABEAMER Planner
[Tombropoulos, Adler, and Latombe, 1997][Tombropoulos, Adler, and Latombe, 1997] Visibility constraintsVisibility constraints
Radio-Surgical PlanningRadio-Surgical Planning
• 2000 < Tumor < 22002000 < B2 + B4 < 22002000 < B4 < 22002000 < B3 + B4 < 22002000 < B3 < 22002000 < B1 + B3 + B4 < 22002000 < B1 + B4 < 22002000 < B1 + B2 + B4 < 22002000 < B1 < 22002000 < B1 + B2 < 2200
• 0 < Critical < 5000 < B2 < 500
T
C
B1
B2
B3B4
T
Radio-Surgical PlanningRadio-Surgical Planning
50% Isodose Surface
80% Isodose Surface
Conventional system’s plan CARABEAMER’s plan
Contact Stanford Report
News Service
/Press Releases
Stanford Report, July 25, 2001
Patients gather to praise minimallyinvasive 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
the membranes that cover the brain), and acoustic neuromas. Since January 1999, more than 335 patients have been treated at Stanford with the CyberKnife.
Cyberknife SystemsCyberknife Systems
Modular Reconfigurable RobotsModular Reconfigurable Robots
Xerox, ParcXerox, Parc
Casal and Yim, 1999
Humanoid RobotHumanoid Robot[Kuffner and Inoue, 2000] (U. Tokyo)[Kuffner and Inoue, 2000] (U. Tokyo)
Stability constraintsStability constraints
Space RoboticsSpace Robotics
air bearingair bearing
gas tankgas tank
air thrustersair thrustersobstacles
robotrobot
[Kindel, Hsu, Latombe, and Rock, 2000][Kindel, Hsu, Latombe, and Rock, 2000]Dynamic constraintsDynamic constraints
Total duration : 40 secTotal duration : 40 sec
Autonomous HelicopterAutonomous Helicopter
[Feron, 2000] (AA Dept., MIT)[Feron, 2000] (AA Dept., MIT)
Interacting Nonholonomic RobotsInteracting Nonholonomic Robots
yy11
xx22
d
xx11
yy22
(Grasp Lab - U. Penn)(Grasp Lab - U. Penn)
Map BuildingMap Building
[Gonzalez, 2000][Gonzalez, 2000]
Next-Best View ComputationNext-Best View Computation
Map BuildingMap Building
[Gonzalez, 2000][Gonzalez, 2000]
Map BuildingMap Building
[Gonzalez, 2000][Gonzalez, 2000]
Graphic Animation of Digital ActorsGraphic Animation of Digital Actors
[Koga, Kondo, Kuffner, and Latombe, 1994][Koga, Kondo, Kuffner, and Latombe, 1994]
The MotionThe MotionFactoryFactory
Prediction of Molecular MotionsPrediction of Molecular Motions
[Singh, Latombe, and Brutlag, 1999][Singh, Latombe, and Brutlag, 1999]
Ligand-protein bindingLigand-protein binding
OutlineOutline
General ApproachGeneral Approach
Specific PlannerSpecific Planner
Experimental ResultsExperimental Results
Other ApplicationsOther Applications
ConclusionConclusion
ConclusionConclusion
Probabilistic Roadmaps provide an efficient and Probabilistic Roadmaps provide an efficient and reliable computational approach to motion reliable computational approach to motion planningplanning
PRM planners are rather easy to implementPRM planners are rather easy to implement
They have been experimented on very different They have been experimented on very different problemsproblems
Remaining IssuesRemaining Issues
Relatively large standard deviation of Relatively large standard deviation of planning timeplanning time
No rigorous termination criterion when No rigorous termination criterion when no solution is foundno solution is found
New challenging applicationsNew challenging applications ……
Optimal Touring of Multiple GoalsOptimal Touring of Multiple Goals
Surgical Planning with Soft TissueSurgical Planning with Soft Tissue
Planning Nice-Looking MotionsPlanning Nice-Looking Motions
A Bug’s Life (Pixar/Disney) Toy Story (Pixar/Disney)
Tomb Raider 3 (Eidos Interactive) Final Fantasy VIII (SquareOne)The Legend of Zelda (Nintendo)
Antz (Dreamworks)
1,000s of Degrees of Freedom1,000s of Degrees of Freedom
Protein foldingProtein folding