Exact Collision Checking Exact Collision Checking of Robot Pathsof Robot Paths

Fabian Schwarzer Mitul Saha

Jean-Claude Latombe

Computer Science DepartmentStanford University

Motivation (1)Motivation (1)One tenet of PRM planning is that sampled configurations and connections can be efficiently tested for collision.

[Quinlan, 94; Gottschalk, Lin, Manocha, 96]

Static collision tests (for sampled configurations) are done efficiently using pre-computed bounding volumes (BV) hierarchies

Motivation (2)Motivation (2)

But dynamic collision tests (for connections) using BV hierarchies are usually approximate.

Motivation (3)Motivation (3)

too large collisions are missed too small slow test of local paths

Motivation (3)Motivation (3)

But dynamic collision tests (for connections) using BV hierarchies are usually approximate.

Previous Approaches toPrevious Approaches to Dynamic Collision Testing Dynamic Collision Testing

Bounding-volume (BV) hierarchies Discretization issue

Feature-tracking methods[Lin, Canny, 91][Mirtich, 98] V-Clip[Cohen, Lin, Manocha, Ponamgi, 95] I-Collide[Basch, Guibas, Hershberger, 97] KDS

Geometric complexity issue with highly non-convex objects

Swept-volume intersection[Cameron, 85][Foisy, Hayward, 93]

Swept-volumes are expensive to compute. Too much data.

Our Approach Our Approach

Problem:Given two configurations, test if the straight path between them is collision-free, or not.

Ideas: a) Relate configuration changes to path

lengths in workspaceb) Use distance computation rather than

pure collision checking

For any q and q’ no robot point traces a path longer than:

(q,q’) = 3|q1|+2|q2|+|q3|

q = (q1,q2,q3)q’ = (q’1,q’2,q’3)qi = q’i-qi

q1

q2

q3

Ideas: a) Relate configuration changes to path lengths in

workspaceb) Use distance computation rather than pure collision

checking

If (q,q’) < (q) + (q’)then the straight path betweenq and q’ is collision-free

(q)

(q) = Euclidean distance between robot and obstacles (or lower bound)

q1

q2

q3

Ideas: a) Relate configuration changes to path lengths in

workspaceb) Use distance computation rather than pure collision

checking

q

q’

{q” | (q,q”) < (q)}

{q” | (q’,q”) < (q’)}

(q,q’) < (q) + (q’)

Ideas: a) Relate configuration changes to path lengths in

workspaceb) Use distance computation rather than pure collision

checking

q

q’

{q” | (q,q”) < (q)}

{q” | (q’,q”) < (q’)}

Bisection(q,q’) > (q) + (q’)

Ideas: a) Relate configuration changes to path lengths in

workspaceb) Use distance computation rather than pure collision

checking

Use BV hierarchy + same recursion as for pure collision checking But compute distance between BVs instead of testing BV overlap BVs are RSSs

Greedy Distance ComputationGreedy Distance Computation

returns lower bounds on distance that are often much larger than ½ actual distances small factor slower than a pure collision checking much faster than BV-based exact or approximate distance computation

GeneralizationGeneralizationRobot(s) and static obstacles treated as collection of rigid bodies A1, …, An.

i(q,q’): upper bound on length of curve segment traced by any point on Ai when robot system is linearly interpolated between q and q’

1(q,q’) = |q1|2(q,q’) = 2|q1|+|q2| 3(q,q’) = 3|q1|+2|q2|+|q3|

q1

q2

q3

GeneralizationGeneralizationRobot(s) and static obstacles treated as collection of rigid bodies A1, …, An.

i(q,q’): upper bound on length of curve segment traced by any point on Ai when robot system is linearly interpolated between q and q’

If i(q,q’) + j(q,q’) < ij(q) + ij(q’)then Ai and Aj do not collide between q and q’

Generalized Bisection MethodGeneralized Bisection Method

I. Until Q is not empty do:1. [qa,qb]ij remove-first(Q)

2. If i(qa,qb) + j(qa,qb) ij(qa) + ij(qb) then

a. qmid (qa+qb)/2

b. If ij(qmid) = 0 then return collision

c. Else insert [qa,qmid]ij and [qmid,qb]ij into Q

II. Return no collision

Each pair of bodies is checked independently of the others priority queue Q of elements [qa,qb]ij

Initially, Q consists of [q,q’]ij for all pairs of bodies Ai and Aj that need to be tested.

Heuristic Ordering QHeuristic Ordering Q

Goal: Discover collision quicker if there is one.

Sort Q by decreasing values of: [i(qa,qb) + j(qa,qb)] – [ij(qa) + ij(qb)]

Possible extension to multi-segment paths(very useful with lazy collision-checking PRM)

Segment Covering StrategiesSegment Covering Strategies

Allows caching of forward kinematic results

Two Arms and Three Rings Two Arms and Three Rings Two 20-dof linkages, with 320 triangles eachThree rings with 6,300 triangles each

Robot in a CageRobot in a CageRobot: 2,991 trianglesCage: 432 triangles

Spot WeldingSpot Welding

Robot: 2,991 trianglesObstacles: 74,681 triangles

Comparative ExperimentComparative Experiment

Robot: 2,502 trianglesObstacles: 432 Triangles

SBL 17 secA-SBL 4.8 sec

SBL: PRM planner (single-query, bi-directional, lazy in cc) with fixed-discretization collision checkerA-SBL: Same planner, with new collision checker

Experiment: • Run SBL 10 times on same planning problem with some resolution • If a collision has been missed, reduce and repeat• If no collision has been missed, return average planning time• Run A-SBL 10 times and return average planning time

Some ResultsSome ResultsRobot: 2,502 trianglesObstacles: 432 Triangles

SBL 17 secA-SBL 4.8 sec

Robot: 2,991 trianglesObstacles: 74,681 triangles

SBL 1.20 secA-SBL 0.81 sec

Robot: 2,991 trianglesObstacles: 432 Triangles

SBL 83 secA-SBL 44 sec

Robot: 2,502 trianglesObstacles: 34,171 triangles

SBL 3.2 secA-SBL 2.1 sec

Robots: 6 x 2,991 trianglesObstacles: 19,668 triangles

SBL 85 secA-SBL 52 sec

ConclusionConclusion

New collision checker suited for PRM planners: Faster than fixed-resolution checkers Fully reliable

Future work: Automatic computation of tight upper

bounds on path lengths in w-space from robot kinematics

Better treatment of pairs of moving bodies (e.g., bodies moving along parallel paths)

AcknowledgementsAcknowledgements

This research was partially funded by grants from General Motors and ABB

Additional geometric models were provided by PSA (Peugeot-Citroen)

Greedy Computation of Greedy Computation of ijij(q)(q)

Algorithm GREEDY-DIST(Ba,Bb)1. h distance(Ba,Bb)2. If Ba and Bb are both triangles then return h3. If h > 0 then return h4. If Ba is “bigger” than Bb then switch Ba and Bb5. GREEDY-DIST(Ba,Bb1)6. If > 0 then

1. GREEDY-DIST(Ba,Bb2)2. If > 0 then return min{,}

7. Return 0

Ba

Bb

GREEDY-DIST is small factor slower than a pure collision checker is much faster than BV-based exact or approximate distance computation returns lower-bounds that are often much larger than ½ actual distances

Idea: BV hierarchy + same recursion as for pure collision checking, but compute distance between boxes ( BVs are RSSs)

ij(q)