the University of North Carolina at CHAPEL HILL

A Simple Path Non-Existence Algorithm using C-obstacle Query

http://gamma.cs.unc.edu/nopath

Liang-Jun Zhang University of North Carolina - Chapel Hill

Young J. Kim EWHA Womans University, Korea

Dinesh Manocha University of North Carolina - Chapel Hill

the University of North Carolina at CHAPEL HILL

Motion Planning

Initial

Goal

Obstacle

To find a path

Robot

72 DOFCourtesy of P. Isto and M. Saha, 2006

Goal

Initial

Obstacle

To report no path

Robot

the University of North Carolina at CHAPEL HILL

Path Non-existence Problem

ObstacleObstacle

GoalInitial

Robot

the University of North Carolina at CHAPEL HILL

Previous Work

• Exact Motion Planning♦ Exact cell decomposition [Schwartz et

al. 83]♦ Roadmap [Canny 88]♦ Criticality based method [Latombe 99]

♦ Implementation challenges♦ Special and simple objects

• Ladders, sphere, convex shapes

the University of North Carolina at CHAPEL HILL

Previous Work

• Approximation Cell Decomposition♦ [Lozano-Pérez 83], [Zhu et al. 91],

[Latombe 91]♦ Relatively easy to implement

♦ Combinatorial complexity of cell decomposition

♦ Computational issue for labelling the cells during cell decomposition

the University of North Carolina at CHAPEL HILL

Previous Work

• Probabilistic Sampling Based Approach♦ [Kavarki et al. 96] [LaValle et al. 98],

[Choset et al. 05], [LaValle 06]♦ Simple and widely used

♦ May not be terminated when non-path exists

♦ Difficult for narrow passage

the University of North Carolina at CHAPEL HILL

Previous Work

• Path non-existence for special cases♦ Planar section, [Basch et al. 01], [Bretl et al.

04]

the University of North Carolina at CHAPEL HILL

Main Results

• Efficient cell labelling algorithm♦ Workspace-based♦ C-obstacle query using generalized

penetration depth

• Improved cell decomposition algorithm ♦ Simple ♦ Efficient for path non-existence

the University of North Carolina at CHAPEL HILL

Path Non-existence Problem

qgoal

• More difficult than finding a path♦ To check all possible

paths

• Identify a region in C-obstacle ♦ separating qinit and qgoal

qinit

Configuration space

the University of North Carolina at CHAPEL HILL

C-obstacle Query

• Whether a primitive lies entirely in C-obstacle?♦ Usually a cell

• Useful for path non-existence

qgoal

qinit

the University of North Carolina at CHAPEL HILL

full mixed

empty

• [Lozano-Pérez 83]• [Zhu et al. 91]• [Latombe 91]

Cell Decomposition for Path Non-existence

the University of North Carolina at CHAPEL HILL

Cell Decomposition for Path Non-existence

Connectivity Graph Guiding Path

the University of North Carolina at CHAPEL HILL

Cell Decomposition for Path Non-existence

Connectivity graph is not connected

No path!

the University of North Carolina at CHAPEL HILL

Previous Work on C-obstacle Query

• Explicit free space computation♦ Exponential complexity [Sacks 99, Sharir 97]♦ Hard in practice: degeneracy

• Check against every C-surface♦ [Latombe 91, Zhu et al. 91]♦ C-surface enumeration♦ To deal with non-linear C-surfaces

• Workspace distance computation ♦ [Paden 89]♦ Overly conservative

the University of North Carolina at CHAPEL HILL

C-obstacle QueryA Collision Detection Problem

•Does the cell lie inside C-obstacle?

• Do robot and obstacle intersect at all configurations?

Obstacle

WorkspaceConfiguration space

?Robot

the University of North Carolina at CHAPEL HILL

Clearance VS ‘Forbiddance’

• Separation distance

• Clearance

• Penetration Depth

• ‘Forbiddance’

PDd

the University of North Carolina at CHAPEL HILL

q

C-obstacle Query Algorithm

• Penetration Depth♦ Extent of interpenetration

between robot and obstacle

• Motion Bound♦ Extent of the motion that robot

can make.

• Is Penetration Depth > Motion Bound?

Robot A(q)

PD

Cell

Obstacle

the University of North Carolina at CHAPEL HILL

Translational Penetration Depth: PDt

• Minimum translation to separate A, B ♦ [Dobkin 93, Agarwal 00,

Bergen 01, Kim 02]

• PDt: not applicable♦ The robot is allowed to both

translate and rotate. ♦ Undergoing rotation, A may

‘escape’ from B more easily

B

A

A’

A

B

the University of North Carolina at CHAPEL HILL

Generalized Penetration Depth:

PDg

• Take into account translational and rotational motion ♦ [L. Zhang, Y. Kim, G. Varadhan, D. Manocha,

ACM Solid and Physical Modeling 06]♦ Trajectory length ♦ Distance metric Dg

♦ Min/Max operations

Trajectory length A(q0)

A(q1)

the University of North Carolina at CHAPEL HILL

PDg Computation

• Difficult for non-convex objects

• Theorem: for convex objects, PDg = PDt

• Convex/Convex♦ Known efficient PDt algorithms directly applicable♦ [Dobkin 93, Agarwal 00, Bergen 01, Kim 02]

• Non-Convex / Non-Convex♦ A lower bound on PDg based on convex decomposition

the University of North Carolina at CHAPEL HILL

C-obstacle Query

Is Penetration Depth > Motion Bound?

the University of North Carolina at CHAPEL HILL

Motion Bound

• ♦ [Schwarzer, Saha, Latombe

04]

• ♦ Achieved by any diagonal

line segment, e.g. qa,c

bqaq,( , )a bq qMB A

Cell

,( , ) max ( , )a bq qMB A C MB A

bq

cq

Configuration space

qa

the University of North Carolina at CHAPEL HILL

Free Cell Query

• Separation distance describes the clearance• If Separation Distance ≥ Motion Bound

the robot can not intersect with the obstacle♦ The cell lies inside free space

d

the University of North Carolina at CHAPEL HILL

Experimental Results C-obstacle Query Computation

Faces # of A 28 8,452 304

Faces # of B 1,692 336 304

Per C-obstacle

query

1.901 (ms) 6.127 (ms) 4.112 (ms)

the University of North Carolina at CHAPEL HILL

Experimental Results Path Non-existence

• 2D rigid robots with 3-DOF♦ 2 translational DOF and 1 rotational DOF

B1B2

B3

B4

A

A'

the University of North Carolina at CHAPEL HILL

Two-gear Example

no path!

Cells in C-obstacle

Initial

Goal

Roadmap in F

Video 3.356s

the University of North Carolina at CHAPEL HILL

Performance of Two-gear Example

# of C-obstacle queries

30K

# of free cell queries

32K

# of iterations 41

Total timing 3.356s

Free cell queries 0.858s

C-obstacle queries 0.827s

Graph searching 0.466s

Subdivision 1.205s

# of total cells 28K

# of total free cells 2K

# of total c-obstacle cells

12K

# of total mixed cells 14K

the University of North Carolina at CHAPEL HILL

Five-gear Example

Cells in C-obstacle

Initial

Goal

Roadmap in F

6.317sNo path!

the University of North Carolina at CHAPEL HILL

Narrow PassageModified Five-gear Example

Total timing 85s

# of C-obstacle queries

176K

# of total cells 168K

Video

Initial

Goal

roadmap in free space

the University of North Carolina at CHAPEL HILL

2D Puzzle

No path!

7.9s

Narrow passage

15.8s

B1B2

B3

B4

Initial

GoalB1B2

B4

A

A'

VideoRemoved

the University of North Carolina at CHAPEL HILL

Conclusion

• C-obstacle query is essential for deciding path non-existence

• Efficient C-obstacle and free cell queries♦ Workspace-based♦ Using generalized penetration depth and

separation distance computation

• Improved cell decomposition algorithm♦ Simple♦ Efficient for path non-existence

the University of North Carolina at CHAPEL HILL

Limitations

• C-obstacle & free cell queries are conservative

• Can not deal with compliant motion planning

• Current implementation of cell decomposition is limited to 3-DOF robots

the University of North Carolina at CHAPEL HILL

Future Work

• Higher DOF motion planning♦ 6 DOF rigid robot♦ C-obstacle & free cell queries are

applicable♦ Combinatorial complexity of cell

decomposition

• Hybrid planner♦ To combine with sampling based

approach

the University of North Carolina at CHAPEL HILL

Acknowledgements

• Army Research Office, DARPA/REDCOM, NSF, ONR, Intel Corporation

• KRF, STAR program of MOST, Ewha SMBA consortium, ITRC program, Korea

• Mink2D, Tel Aviv University

• GAMMA Group, UNC Chapel Hill

the University of North Carolina at CHAPEL HILL

Thank you!

Any Questions?

http://gamma.cs.unc.edu/nopath

the University of North Carolina at CHAPEL HILL

Min over every path connecting q0 and q1

Max trajectory length for distinct points

Dg(q0, q1) =

Dg Metric in C-space

X

Yθ

q1q0

l1

l2

Motion Paths in C-Space Trajectory length

A(q0)

A(q1)

Dg(q0,q1)

the University of North Carolina at CHAPEL HILL

min({ ( , ) | int( ( )) })ggPD D A B 0q q q

PDg definition

The minimum Dg distance over all possible collision-free configurationsA

B

PDg

the University of North Carolina at CHAPEL HILL

Lower Bound on PDg

1. Convex decomposition 2. Eliminate non-overlapping pairs

3. PDt for overlapping pairs

4. LB(PDg) = Max over all PDts

PDt

PDt

the University of North Carolina at CHAPEL HILL

Performance of Five-gear Example

Total timing 6.317s

# of total cells 39K

# of C-obstacle queries

41K

the University of North Carolina at CHAPEL HILL

Compared with Star-shaped roadmap

• Pros♦ Simpler than the star-shaped test♦ Need not capture the intra-connectivity♦ More likely to be extended for higher DOF

• Cons♦ More conservative