Probabilistic Motion Planning: Algorithms and …jmlien/seminar/papers/overview-pmp.pdfProbabilistic Motion Planning: Algorithms and Applications Jyh-Ming Lien Department of Computer

Probabilistic Motion Planning:Algorithms and Applications

Jyh-Ming Lien

Department of Computer ScienceGeorge Mason University

2007-09-06 Grand Seminar

The Alpha Puzzle

Motion Planningin continuous spaces

start

goal obstacles

(Basic) Motion Planning(in a nutshell):

Given a movable object, find asequence of valid configurationsthat moves the object from thestart to the goal.

Swapping Cubes Puzzle


Polyhedron: 25 dof

Hard Motion Planning ProblemsHighly Articulated (Constrained) Systems

Paper Folding Articulated robot

Line: 30 dof


Hard Motion Planning ProblemsHighly Articulated (Constrained) Systems

Digital Actors

Reaching and grasping

Closed Chain System


Hard Motion Planning ProblemsFlocking: Covering, Homing, Shepherding

Motion for coordinated entities Control the motion of coordinated entities


Hard Motion Planning ProblemsDeformable Objects

• Find a path for a deformable object that candeform 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 ProblemsIntelligent 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 moreextensive testing, and is useful for training (VR or e-manuals)

Maintainability Problems:Mechanical Designs from GE

flange Airplane engine


Hard Motion Planning Problemscomputational 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

normal - misfold

prion protein


• 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•…

Outline


Workspace

•Fixed Base•Two Joints•Fixed length links

Robot

Package

Pickup the package


Workspace


Workspace


Workspace


Workspace


Workspace


Workspace


Workspace

α

β

Degree of freedom (DOF)


Configuration SpaceC-Space

β=125

α

β

0

180

18055

125

C-Space

α=55


C-Space

β=100

α

β

0

180

18075

100

C-Space

α=75


C-Space

α=85

α

β

0

180

18085

80

C-Space

β=80


C-Space

α=90

α

β

0

180

18090

55

C-Space

β=55


C-Space

α=110

α

β

0

180

180110

30

C-Space

β=30


C-Space

α=135

α

β

0

180

18055

15

C-Space

C-Space

β=15


C-Spaceset 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

α

β

0

180

180

C-Space


Workspace

•Moving in X-Y•Rotating

Robot


Workspace

•Moving in X-Y•Rotatingx

y

Θ


The

ta

Workspace C-Space

Workspace vs. C-Space


The

ta

Workspace C-Space

Workspace vs. C-Space


Workspace

(4,5,45)

Workspace

obstacle

Workspace

(x,y)

theta

Configuration (x,y,theta)

Workspace Obstacle


Th

et a

C-Space Obstacle

C-Obstacle


Initial

Goal

Finding a PathFind a path inworkspace fora robot

The

ta

Find a path inC-space for a point


robot

obst

obst

obst

obst

xy

!

C-obst

C-obstC-obst

C-obst

robotPath is swept volume

Motion Planning in C-space

Path is 1D curve

Workspace

C-spaceSimple workspace obstacle transformed Into complicated C-obstacle!!


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)


General motion planning problem isPSPACE-hard [Reif 79, Hopcroft et al. 84 & 86]

PSPACE-complete [Canny 87]

The best deterministic algorithm known has runningtime that is exponential in the dimension of the robot’sC-space [Canny 86]

• C-space has high dimension - 6D for rigid body in 3-space• simple obstacles have complex C-obstacles impractical to computeexplicit 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 ofMotion Planning PSPACE

NP

P


Roadmap Construction(Pre-processing)

Th

eta

Probabilistic Roadmap Method[Kavraki, Svestka, Latombe,Overmars 1996]

unknown


1. Randomly generate robotconfigurations (nodes) - discard nodes that are invalid

Probabilistic Roadmap MethodRoadmap Construction(Pre-processing)

Th

eta



2. Connect pairs of nodes to form roadmap - simple, deterministic local planner - e.g., straight-line - discard connections that are invalid


Th

eta


2. Connect pairs of nodes to form roadmap - simple, deterministic local planner (e.g.,straightline) - discard paths that are invalid



Th

eta

1. Connect start and goal to roadmap

Query processing

2. Find path in roadmap between start and goal - regenerate plans for edges in roadmap


1. Connect start and goal to roadmap

Query processingstart

goal

Probabilistic Roadmap Method

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


PRMs: Pros & ConsPRMs: The Good News

1. PRMs are probabilistically complete2. PRMs apply easily to high-dimensional C-space3. PRMs support fast queries w/ enoughpreprocessing

Many success stories where PRMs solve previouslyunsolved 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

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]


An Obstacle-Based PRM

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

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 sophisticatedheuristics to try to cover C-obstacle

C-obst


1

2

Gaussian Sampling PRM

1. Find a point in S’s C-obstacle (robot placement colliding with S)

2. Find another point that is withindistance d to the first point, where dis a random variable in a Gaussiandistribution

3. Keep the second point if it iscollision free

C-obstd

Note• 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 inthe narrow passage will increase!


Medial Axis PRM (MAPRM)

Intuitively, points on the medialaxis are points that are farthestaway 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

3 closest points

Property: Points on the Medial axis has morethan one closest point to the boundary

1 closest point



Given a point that is not on the Medial axis, wecan always retract the point to the medial axis

1. Push the point awayfrom the closestboundary point

2. until the point hasmore than one closestboundary points


Medial Axis PRM (MAPRM)Sample a Configuration, p

p is in collision

q = NearestContactCfg_Penetration(p)

V = q - p

q = NearestContactCfg_Clearance(p)

V = p - q

p is collision-free

Retract p to the Medial Axis ofthe free C-space in direction V

samples < N

Connect sampled configurations



PRM MAPRM

1000 samples 1000 samples



• In-collision configurations are retracted to free C-space

• The volume of the narrow passage is increased

Vol(S )+Vol(B’ )

Vol(C )Pro( Sampling in S ) =

Sampling is increased in the narrow passage


Machine Learning forFeature-Sensitive MP

Basic PRM – Kavraki,Sveska,Latombe,Overmars ‘96Fuzzy PRM – Nielsen, Kavaki ‘00Lazy PRM – Bohlin, Kavraki ‘00RNG – Yang, LaValle ‘00OBPRM – Amato, Wu ‘96GaussPRM – Boor, Overmars, van der Stappen ‘99Visibility Rdmp – Laumond, Simeon ‘00MAPRM – Wilmarth, Amato, Stiller ‘99Ariadne’s Clew – Bessiere, Ahuactzin, Talbi, Mazer ‘93RRT – LaValle, Kuffner ‘99Dilated Spaces – Hsu,Kavraki,Latombe,Motwani,Sorkin’98ClosestVE (surfaces) – Dale, ‘00User Input – Bayazit, Song, Amato ‘00

XXXX

X

X

XXXXXXXXX

X

XXXXXXXX

X

X

XXXX

open clutter Algorithm less more most

free

narrow passage

blocked passage

isolated

cluttered

free

free

narrow passage

blocked passage

isolated

cluttered

free

Randomized methods• Many available• Strengths and

weaknesses inproblems withdifferent features


Machine Learning forFeature-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 partialroadmap

• Integrate solutions

Free nodes: blackUnfeasible nodes: yellow

Successful connections: blackUnsuccessful connections: yellow

Decision Tree classes


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– …

Documents

Probabilistic Motion Planning: Algorithms and …jmlien/seminar/papers/overview-pmp.pdfProbabilistic Motion Planning: Algorithms and Applications Jyh-Ming Lien Department of Computer