http://parasol.tamu.edu
Probabilistic Roadmap Methods(PRMs)
Nancy M. AmatoParasol Lab,Texas A&M University
Motion Planning
start
goal obstacles
(Basic) Motion Planning:
Given a movable object and a description of the environment, find a sequence of valid configurations that moves it from the start to the goal.
The Alpha Puzzle
The Piano Movers Problem
Applications of PRM-based Motion Planning (Amato et al)
Outline
• C-space, Planning in C-space (basic definitions)
• Probabilistic Roadmap Methods (PRMs)• PRM variants (OBPRM, MAPRM) • User-Input: Haptically Enhanced PRMs
• PRMs for more `complex’ robots• deformable objects • group behaviors for multiple robots• protein folding
Configuration Space (C-Space)
C-obst
C-obst
C-obst
C-obst
C-obst
C-Space
6D C-space(x,y,z,pitch,roll,yaw)
3D C-space(x,y,z) 3D C-space
()
• “robot” maps to a point in higher dimensional space • parameter for each degree of freedom (dof) of robot• C-space = set of all robot placements • C-obstacle = infeasible robot placements
2n-D C-space(1, 1, 2, 2, . . . , n, n)
• workspace obstacles transformed to C-obstaclesfree obstaclesc c c
Figure from Latombe’91
C-obstacles
simple 2D workspace obstacle => complicated 3D C-obstacle
Figure from Latombe’91
C-obstacles
robot
obst
obst
obst
obst
xy
C-obst
C-obstC-obst
C-obst
robot Path is swept volume
Motion Planning in C-space
Path is 1D curve
Workspace
C-spaceSimple workspace obstacle transformed Into complicated C-obstacle!!
Most motion planning problems of interest are PSPACE-hard [Reif 79, Hopcroft et al. 84 & 86]
The best deterministic algorithm known has running time that is exponential in the dimension of the robot’s C-space [Canny 86]
• C-space has high dimension - 6D for rigid body in 3-space• simple obstacles have complex C-obstacles impractical to compute explicit representation of freespace for more than 4 or 5 dof
So … attention has turned to randomized algorithms which • trade full completeness of the planner• for probabilistic completeness and a major gain in efficiency
The Complexity of Motion Planning
1. Connect start and goal to roadmap
Query processingstart
goal
Probabilistic Roadmap Methods (PRMs)[Kavraki, Svestka, Latombe,Overmars 1996]
C-obst
C-obst
C-obst
C-obst
Roadmap Construction (Pre-processing)
2. Connect pairs of nodes to form roadmap - simple, deterministic local planner (e.g., straightline) - discard paths that are invalid
1. Randomly generate robot configurations (nodes) - discard nodes that are invalid
C-obst
C-space
2. Find path in roadmap between start and goal - regenerate plans for edges in roadmap
Primitives Required:1. Method for Sampling points in C-Space2. Method for `validating’ points in C-Space
PRMs: Pros & Cons
PRMs: The Good News1. PRMs are probabilistically complete2. PRMs apply easily to high-dimensional C-space3. PRMs support fast queries w/ enough preprocessing
Many success stories where PRMs solve previously unsolved problems
C-obst
C-obst
C-obst
C-obst
C-obst
start
goal
PRMs: The Bad News
1. PRMs don’t work as well for some problems:– unlikely to sample nodes in narrow passages– hard to sample/connect nodes on constraint surfaces
Our work concentrates on improving PRM performance for such problems.
start
goal
C-obst
C-obst
C-obst
C-obst
Related Work (selected)
•Probabilistic Roadmap Methods• Uniform Sampling (original) [Kavraki, Latombe, Overmars, Svestka, 92, 94, 96]
•Obstacle-based PRM (OBPRM) [Amato et al, 98]
• PRM Roadmaps in Dilated Free space [Hsu et al, 98]
• Gaussian Sampling PRMs [Boor/Overmars/van der Steppen 99]
• PRM for Closed Chain Systems [Lavalle/Yakey/Kavraki 99, Han/Amato 00]
• PRM for Flexible/Deformable Objects [Kavraki et al 98, Bayazit/Lien/Amato 01]
• Visibility Roadmaps [Laumond et al 99]
• Using Medial Axis [Kavraki et al 99, Lien/Thomas/Wilmarth/Amato/Stiller 99, 03, Lin et al 00]
• Generating Contact Configurations [Xiao et al 99]
•Single Shot [Vallejo/Remmler/Amato 01]
•Bio-Applications: Protein Folding [Song/Thomas/Amato 01,02,03, Apaydin et al 01,02]
• Lazy Evaluation Methods: [Nielsen/Kavraki 00 Bohlin/Kavraki 00, Song/Miller/Amato 01, 03]
• Related Methods•Ariadnes Clew Algorithm [Ahuactzin et al, 92]
• RRT (Rapidly Exploring Random Trees) [Lavalle/Kuffner 99]
Other Sampling Strategies
Obstacle Sensitive Sampling Strategies
Goal: Sample nodes in Narrow Passages
• OBPRM: Obstacle-Based PRM• sampling around obstacles
• MAPRM: Medial-Axis PRM• sampling on the medial axis
• Repairing/Improving approximate paths• transforming invalid samples into valid samples
OBPRM: An Obstacle-Based PRM[IEEE ICRA’96, IEEE ICRA’98, WAFR’98]
start
goal
C-obst
C-obst
C-obst
C-obst
To Navigate Narrow Passages we must sample in them• most PRM nodes are where planning is easy (not needed)
PRM Roadmap
start
goal
C-obst
C-obst
C-obst
C-obst
Idea: Can we sample nodes near C-obstacle surfaces?• we cannot explicitly construct the C-obstacles...• we do have models of the (workspace) obstacles...
OBPRM Roadmap
1
3
2
45
OBPRM: Finding Points on C-obstacles
Basic Idea (for workspace obstacle S)
1. Find a point in S’s C-obstacle (robot placement colliding with S)2. Select a random direction in C-space3. Find a free point in that direction4. Find boundary point between them using binary search (collision checks)
Note: we can use more sophisticated heuristics to try to cover C-obstacle
C-obst
PRM vs OBPRM Roadmaps
PRM• 328 nodes• 4 major CCs
OBPRM• 161 nodes• 2 major CCs
OBPRM for Paper Folding
Box: 12 => 5 dof Periscope: 11 dof
Roadmap Statistics• 218 nodes, 3 sec
Roadmap Statistics• 450 nodes, 6 sec
MAPRM: Medial-Axis PRMSteve Wilmarth, Jyh-Ming Lien, Shawna Thomas [IEEE ICRA’99, ACM SoCG’99, IEEE ICRA’03]
• Key Observation: We can efficiently retract almost any configuration, free or not, onto the medial axis of the free space without computing the medial axis explicitly.
C-obstacle
Medial axis
Repairing Approximate Paths
1. Create initial roadmap2. Extract approximate path P 3. Repair P (push to C-free)
– Focus search around P– Use OBPRM-like techniques
Problem: Even with the best sampling methods, roadmaps may not contain valid solution paths• may lack critical cfgs in narrow passages • may contain approximate paths that are ‘nearly’ valid
Repairing/Improving Approximate Paths
C-obstacle
C-obstacle
approximatepath
repairedpath
PHANToM 1. User collects approximate path using haptic device • User insight identifies critical cfgs• User feels when robot touches
obstacles and adjusts trajectory
2. Approximate path passed to planner and it fixes it
Current Applications• Intelligent CAD Applications• Molecule Docking in drug design • Animation w/ Deformable Models
Hybrid Human/Planner System[IEEE ICRA’01, IEEE ICRA’00, PUG’99]
0
500
1000
1500
2000
2500
3000
3500
time (sec)
OBPRMhaptic pushiterative push
Hybrid Human/Planner SystemHaptic Hints Results
Flange Problem
0.85 0.95 1
0
500
1000
1500
2000
2500
3000
3500
time (sec)
OBPRMhaptic pushiterative push
Hybrid Human/Planner SystemHaptic Hints Results
Flange Problem
0.85 0.95 1
Applications
Same Method: Diverse range of problems
• Deformable Objects
• Simulating Group Behaviors• homing, covering, shepherding
• Modeling Molecular Motions•Protein folding, RNA folding, protein/ligand binding
Deformable Objects[IEEE ICRA’02]
Our Approach:1. Build roadmap relaxing collision
free requirement (allow penetration)
2. Extract approximate path for a rigid version of robot (select path with smallest collision/penetration)
3. Follow path and deform the robot to avoid the collisions
Problem: Find a path for a deformable object that can deform to avoid collision with obstacles• deformable objects have infinite dof!
Deformable ObjectsRepairing Path using Bounding Box Deformation
Bounding Box Deformationbuild a 3D voxel bounding box.
—Convert it to ChainMail bounding box (3D grid of springs)—Deform ChainMail Bounding box.—Deform objects using Free Form Deformation based on deformation of the bounding box.
Deformed ChainMail Bounding BoxObstacleChainMail Box Apply FFD
Approximate Solution
Query
The approximate path returned is not the shortest path but the energetically most feasible path (less deformation required)