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