Probabilistic Motion Planning: Algorithms and Applications

Probabilistic Motion Planning: Algorithms and Applications Jyh-Ming Lien Department of Computer Science George Mason University

Motion Planning in continuous spaces (Basic) Motion Planning (in a nutshell): Given a movable object, find a sequence of valid configurations that moves the object from the start to the goal. start The Alpha Puzzle Swapping Cubes Puzzle goal obstacles

Hard Motion Planning Problems Highly Articulated (Constrained) Systems Paper Folding Articulated robot Polyhedron: 25 dof Line: 30 dof

Hard Motion Planning Problems Highly Articulated (Constrained) Systems Digital Actors Closed Chain System Reaching and grasping

Hard Motion Planning Problems Flocking: Covering, Homing, Shepherding Motion for coordinated entities Control the motion of coordinated entities

Hard Motion Planning Problems Deformable Objects Find a path for a deformable object that can deform to avoid collision with obstacles move a mattress in a house, elastic or air-filled objects, metal sheets or long flexible tubes virtual surgery applications computer animation and games Issue: difficult to find natural deformation efficiently

Hard Motion Planning Problems Intelligent CAD Applications Using Motion Planning to Test Design Requirements: Accessibility for servicing/assembly tested on physical mock ups Digital testing saves time and money, is more accurate, enables more extensive testing, and is useful for training (VR or e-manuals) Maintainability Problems: Mechanical Designs from GE flange Airplane engine

Hard Motion Planning Problems computational biology & chemistry Motion of molecules help understand important interactions - protein structure/function prediction diseases such as Alzheimer s and Mad Cow are related to misfolded proteins prion protein normal - misfold

Outline The basic ideas of motion planning Configuration space (C-Space) C-Space obstacles (C-obstacle) Motion planning in C-Space Probabilistic Roadmap Methods (PRM) Tradition PRM and the Narrow passage problem Obstacle-based PRM Gaussian PRM Medial axis PRM Feature-based PRM

Workspace Pickup the package Robot Fixed Base Two Joints Fixed length links Package

Workspace

Workspace Degree of freedom (DOF) α β

Configuration Space C-Space β=125 180 125 β α=55 0 55 α C-Space 180

C-Space β=100 180 β 10 0 α=75 0 75 α C-Space 180

C-Space β=80 180 β 80 α=85 0 85 α C-Space 180

C-Space β=55 180 β α=90 55 0 90 α C-Space 180

C-Space β=30 180 β α=110 30 0 α 110 C-Space 180

C-Space β=15 180 C-Space β α=135 15 0 55 α C-Space 180

C-Space set of all robot placements Robot maps to a point (in usually higher dimensional space) Parameter for each degree of freedom (dof) of robot Each point in C-Space corresponds the robot s position and orientation in workspace 180 C-Space β 0 α 180

Workspace Robot Moving in -Y Rotating

Workspace x Θ Moving in -Y Rotating y

Theta Workspace vs. C-Space Workspace C-Space

Workspace Obstacle Workspace Workspace theta (x,y) obstacle Configuration (x,y,theta) (4,5,45)

Thet a C-Space Obstacle C-Obstacle

Theta Finding a Path Find a path in workspace for a robot Find a path in C-space for a point Goal Initial

Motion Planning in C-space Simple workspace obstacle transformed Into complicated C-obstacle!! Workspace C-obst C-space C-obst! obst obst obst obst robot Path is swept volume C-obst C-obst x robot Path is 1D curve y

Configuration Space (C-Space) C-Space C-obst C-obst C-obst C-obst C-obst 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 3D C-space (x,y,z) 6D C-space (x,y,z,pitch,roll,yaw) β α γ 3D C-space (α,β,γ) 2n-D C-space (φ 1, ψ 1, φ 2, ψ 2,..., φ n, ψ n)

The Complexity of Motion Planning General motion planning problem is PSPACE-hard [Reif 79, Hopcroft et al. 84 & 86] PSPACE-complete [Canny 87] 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 PSPACE So attention has turned to randomized algorithms which trade full completeness of the planner for probabilistic completeness and a major gain in efficiency NP P

Theta Probabilistic Roadmap Method [Kavraki, Svestka, Latombe,Overmars 1996] unknown Roadmap Construction (Pre-processing)

Theta Probabilistic Roadmap Method Roadmap Construction (Pre-processing) 1. Randomly generate robot configurations (nodes) - discard nodes that are invalid

Probabilistic Roadmap Method Roadmap Construction (Pre-processing) Theta 1. Randomly generate robot configurations (nodes) - discard nodes that are invalid 2. Connect pairs of nodes to form roadmap - simple, deterministic local planner (e.g., straightline) - discard paths that are invalid Query processing 1. Connect start and goal to roadmap 2. Find path in roadmap between start and goal - regenerate plans for edges in roadmap

Probabilistic Roadmap Method C-space goal C-obst C-obst C-obst C-obst Roadmap Construction (Pre-processing) 1. Randomly generate robot configurations (nodes) - discard nodes that are invalid 2. Connect pairs of nodes to form roadmap - simple, deterministic local planner (e.g., straightline) - discard paths that are invalid start C-obst Query processing 1. Connect start and goal to roadmap 2. Find path in roadmap between start and goal - regenerate plans for edges in roadmap

PRMs: Pros & Cons goal C-obst C-obst C-obst C-obst PRMs: The Good News 1. PRMs are probabilistically complete 2. PRMs apply easily to high-dimensional C-space 3. PRMs support fast queries w/ enough preprocessing start C-obst Many success stories where PRMs solve previously unsolved problems goal PRMs: The Bad News C-obst C-obst C-obst C-obst 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 start

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 [iao 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]

An Obstacle-Based PRM To Navigate Narrow Passages we must sample in them most PRM nodes are where planning is easy (not needed) PRM Roadmap OBPRM Roadmap goal goal C-obst C-obst C-obst C-obst C-obst C-obst C-obst C-obst start start Idea: Can we sample nodes near C-obstacle surfaces? we cannot explicitly construct the C-obstacles... we do have models of the (workspace) obstacles...

Finding Points on C-obstacles 2 1 C-obst 3 5 4 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-space 3. Find a free point in that direction 4. Find boundary point between them using binary search (collision checks) Note: we can use more sophisticated heuristics to try to cover C-obstacle

Gaussian Sampling PRM 1. Find a point in S s C-obstacle (robot placement colliding with S) d 1 C-obst 2. Find another point that is within distance d to the first point, where d is a random variable in a Gaussian distribution Note 2 3. Keep the second point if it is collision free Two paradigms: (1) OBPRM: Fix the samples (2) Gaussian PRM: Filter the samples None of these methods can (be proved to) provide guarantee that the samples in the narrow passage will increase!

Medial Axis PRM (MAPRM) Intuitively, points on the medial axis are points that are farthest away from the boundary Line segments and parabolic curves 3D medial axis is hard to compute

Medial Axis PRM (MAPRM) It is easier to sample on the Medial axis Property: Points on the Medial axis has more than one closest point to the boundary 3 closest points 1 closest point

Medial Axis PRM (MAPRM) Given a point that is not on the Medial axis, we can always retract the point to the medial axis 1. Push the point away from the closest boundary point 2. until the point has more than one closest boundary points

Medial Axis PRM (MAPRM) Sample a Configuration, p p is collision-free p is in collision q = NearestContactCfg_Clearance(p) V = p - q q = NearestContactCfg_Penetration(p) V = q - p Retract p to the Medial Axis of the free C-space in direction V samples < N Connect sampled configurations

Medial Axis PRM (MAPRM) 1000 samples 1000 samples PRM MAPRM

Medial Axis PRM (MAPRM) Sampling is increased in the narrow passage In-collision configurations are retracted to free C- space The volume of the narrow passage is increased Pro( Sampling in S ) = Vol(S )+Vol(B ) Vol(C )

Machine Learning for Feature-Sensitive MP Randomized methods Many available Strengths and weaknesses in problems with different features open clutter Algorithm less more most Basic PRM Kavraki,Sveska,Latombe,Overmars 96 Fuzzy PRM Nielsen, Kavaki 00 Lazy PRM Bohlin, Kavraki 00 RNG Yang, LaValle 00 OBPRM Amato, Wu 96 GaussPRM Boor, Overmars, van der Stappen 99 Visibility Rdmp Laumond, Simeon 00 MAPRM Wilmarth, Amato, Stiller 99 Ariadne s Clew Bessiere, Ahuactzin, Talbi, Mazer 93 RRT LaValle, Kuffner 99 Dilated Spaces Hsu,Kavraki,Latombe,Motwani,Sorkin 98 ClosestVE (surfaces) Dale, 00 User Input Bayazit, Song, Amato 00 narrow passage cluttered free isolated blocked passage free

Machine Learning for Feature-Sensitive MP Overview of the algorithm Subdivide C-space into regions Classify from features into free, cluttered, or narrow passage In each subdivision create a partial roadmap Integrate solutions Decision Tree classes Free nodes: black Unfeasible nodes: yellow Successful connections: black Unsuccessful connections: yellow

Conclusion Motion planning has many applications Motion planning is difficult (intractable) Probabilistic Motion Planners PRM and the Narrow passage problem OBPRM Gaussian PRM MAPRM Feature-based PRM There are still many problems to be answered Differential constrains Uncertainty Deterministic vs. Probabilistic Dynamic environments