Exact Collision Checking of Robot Paths

1
Exact Collision Checking of Robot Paths

• Fabian Schwarzer Mitul SahaJean-Claude Latombe
• Computer Science Department
• Stanford University

2
Motivation (1)
One tenet of PRM planning is that sampled
configurations and connections can be
efficiently tested for collision.
3
Motivation (2)
Static collision tests (for sampled
configurations) are done efficiently using
pre-computed bounding volumes (BV) hierarchies
4
Motivation (3)
But dynamic collision tests (for connections)
using BV hierarchies are usually approximate.

• e too large ? collisions are missed
• e too small ? slow test of local paths

5
Motivation (3)
But dynamic collision tests (for connections)
using BV hierarchies are usually approximate.
6
Previous Approaches to 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.

7
Our Approach

• ProblemGiven two configurations, test if the
  straight path between them is collision-free, or
  not.
• Ideas
• Relate configuration changes to path lengths in
  workspace
• Use distance computation rather than pure
  collision checking

8

• Ideas
• Relate configuration changes to path lengths in
  workspace
• Use distance computation rather than pure
  collision checking

q (q1,q2,q3) q (q1,q2,q3) dqi qi-qi
For any q and q no robot point traces a path
longer than l(q,q) 3dq12dq2dq3
9

• Ideas
• Relate configuration changes to path lengths in
  workspace
• Use distance computation rather than pure
  collision checking

h(q) Euclidean distance between
robot and obstacles (or lower bound)
h(q)
If l(q,q) lt h(q) h(q) then the straight path
betweenq and q is collision-free
10

• Ideas
• Relate configuration changes to path lengths in
  workspace
• Use distance computation rather than pure
  collision checking

l(q,q) lt h(q) h(q)
11

• Ideas
• Relate configuration changes to path lengths in
  workspace
• Use distance computation rather than pure
  collision checking

l(q,q) gt h(q) h(q)
Bisection
q
q
q l(q,q) lt h(q)
q l(q,q) lt h(q)
12
Greedy Distance Computation

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

• 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

13
Generalization

• Robot(s) and static obstacles treated as
  collection of rigid bodies A1, , An.
• li(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

l1(q,q) dq1 l2(q,q) 2dq1dq2
l3(q,q) 3dq12dq2dq3
14
Generalization

• Robot(s) and static obstacles treated as
  collection of rigid bodies A1, , An.
• li(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 li(q,q) lj(q,q) lt hij(q) hij(q)then
  Ai and Aj do not collide between q and q

15
Generalized Bisection Method

• Each pair of bodies is checked independently of
  the others ? priority queue Q of elements
  qa,qbij
• Initially, Q consists of q,qij for all pairs
  of bodies Ai and Aj that need to be tested.

• Until Q is not empty do
• qa,qbij ? remove-first(Q)
• If li(qa,qb) lj(qa,qb) ? hij(qa) hij(qb) then
• qmid ? (qaqb)/2
• If hij(qmid) 0 then return collision
• Else insert qa,qmidij and qmid,qbij into Q
• Return no collision

16
Heuristic Ordering Q

• Goal Discover collision quicker if there is one.
  
• Sort Q by decreasing values of
  li(qa,qb) lj(qa,qb) hij(qa) hij(qb)
• Possible extension to multi-segment paths(very
  useful with lazy collision-checking PRM)

17
Segment Covering Strategies
Allows caching of forward kinematic results
18
Two Arms and Three Rings
Two 20-dof linkages, with 320 triangles
eachThree rings with 6,300 triangles each
19
Robot in a Cage
Robot 2,991 trianglesCage 432 triangles
20
Spot Welding
Robot 2,991 triangles Obstacles 74,681 triangles
21
Comparative Experiment

• SBL PRM planner (single-query,
  bi-directional, lazy in cc) with
  fixed-discretization collision checker
• A-SBL Same planner, with new collision
  checkerExperiment
• Run SBL 10 times on same planning problem with
  some resolution e
• If a collision has been missed, reduce e and
  repeat
• If no collision has been missed, return average
  planning time
• Run A-SBL 10 times and return average planning
  time

Robot 2,502 triangles Obstacles 432
Triangles SBL ? 17 sec A-SBL ? 4.8 sec
22
Some Results
Robot 2,502 triangles Obstacles 432
Triangles SBL ? 17 sec A-SBL ? 4.8 sec
Robot 2,991 triangles Obstacles 432
Triangles SBL ? 83 sec A-SBL ? 44 sec
Robot 2,991 trianglesObstacles 74,681
triangles SBL ? 1.20 sec A-SBL ? 0.81 sec
Robot 2,502 trianglesObstacles 34,171
triangles SBL ? 3.2 sec A-SBL ? 2.1 sec
Robots 6 x 2,991 trianglesObstacles 19,668
triangles SBL ? 85 sec A-SBL ? 52 sec
23
Conclusion

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

24
Acknowledgements

• This research was partially funded by grants from
  General Motors and ABB
• Additional geometric models were provided by PSA
  (Peugeot-Citroen)

25
Greedy Computation of hij(q)
Idea BV hierarchy same recursion as for pure
collision checking, but compute distance
between boxes (? BVs are RSSs)

• Algorithm GREEDY-DIST(Ba,Bb)
• h ? distance(Ba,Bb)
• If Ba and Bb are both triangles then return h
• If h gt 0 then return h
• If Ba is bigger than Bb then switch Ba and Bb
• a ? GREEDY-DIST(Ba,Bb1)
• If a gt 0 then
• b ? GREEDY-DIST(Ba,Bb2)
• If b gt 0 then return mina,b
• Return 0

Ba
Bb
hij(q)

• 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