CS B659: Principles of Intelligent Robot Motion

Collision Detection

Probabilistic Roadmaps

How to test forcollision?

Collision Detection Methods• Many different methods• In particular:

• Grid method: good for many simple moving objects of about the same size (e.g., many moving discs with similar radii)

• Closest-feature tracking: good for moving polyhedral objects• Bounding Volume Hierarchy (BVH) method: good for few moving

objects with complex and diverse geometry

Grid Method

Subdivide space into a regular grid cubic of square bins

Index each object in a bin

d

Grid Method

d

Running time is proportional tonumber of moving objects

Useful also to compute pairs of objects within some distance (vision,sound, …)

Closest-Feature Tracking(M. Lin and J. Canny. A Fast Algorithm for Incremental Distance Calculation. Proc. IEEE Int. Conf. on Robotics and Automation, 1991)

The closest pair of features (vertex, edge, face) between two polyhedral objects are computed at the start configurations of the objects

During motion, at each small increment of the motion, they are updated

Efficiency derives from two observations: The pair of closest features

changes relatively infrequently When it changes the new closest features will

usually be on a boundary of the previous closest features

Closest-Feature Test for Vertex-Vertex

VertexVertex

Application: Detecting Self-Collision in Humanoid Robots(J. Kuffner et al. Self-Collision and Prevention for Humanoid Robots. Proc. IEEE Int. Conf. on Robotics and Automation, 2002)

BVH with spheres:S. Quinlan. Efficient Distance Computation Between Non-Convex Objects. Proc. IEEE Int. Conf. on Robotics and Automation, 1994.

BVH with Oriented Bounding Boxes:S. Gottschalk, M. Lin, and D. Manocha. OBB-Tree: A Hierarchical Structure for Rapid Interference Detection. Proc. ACM SIGGRAPH '96, 1996.

Combination of BVH and feature-tracking:S.A. Ehmann and M.C. Lin. Accurate and Fast Proximity Queries Between Polyhedra Using Convex Surface Decomposition. Proc. 2001 Eurographics, Vol. 20, No. 3, pp. 500-510, 2001.

Adaptive bisection in dynamic collision checking:F. Schwarzer, M. Saha, J.C. Latombe. Adaptive Dynamic Collision Checking for Single and Multiple Articulated Robots in Complex Environments, manuscript, 2003.

Bounding Volume Hierarchy Method

Enclose objects into bounding volumes (spheres or boxes) Check the bounding volumes first Decompose an object into two

Bounding Volume Hierarchy Method

Enclose objects into bounding volumes (spheres or boxes) Check the bounding volumes first Decompose an object into two Proceed hierarchically

Bounding Volume Hierarchy Method

Enclose objects into bounding volumes (spheres or boxes) Check the bounding volumes first Decompose an object into two Proceed hierarchically

Bounding Volume Hierarchy Method

• BVH is pre-computed for each object

Bounding Volume Hierarchy Method

BVH in 3D

Collision Detection

Two objects described by their precomputed BVHs

A

B C

D E F G

A

B C

D E F G

Collision Detection

AASearch tree

AA

pruning

Collision Detection

AA

CCCBBCBB

Search tree

AA

A

B C

D E F G

Collision Detection

CCCBBCBB

AASearch tree

pruning

A

B C

D E F G

If two leaves of the BVH’s overlap(here, G and D) check their contentfor collision

Collision Detection

CCCBBCBB

AASearch tree

GEGDFEFD

A

B C

D E F G

G D

Variant

AA

CCCBBCBB

Search tree

AA

A

B C

D E F GAA

CABA

Collision Detection• Pruning discards subsets of the two objects that are separated

by the BVs

• Each path is followed until pruning or until two leaves overlap

• When two leaves overlap, their contents are tested for overlap

Search Strategy and Heuristics

If there is no collision, all paths must eventually be followed down to pruning or a leaf node

But if there is collision, it is desirable to detect it as quickly as possible

Greedy best-first search strategy with f(N) = d/(rX+rY)

[Expand the node XY with largest relative overlap (most likely to contain a collision)]

rX

rYd

X

Y

Recursive (Depth-First) Collision Detection Algorithm

Test(A,B)1. If A and B do not overlap, then return 12. If A and B are both leaves, then return 0 if their contents overlap

and 1 otherwise3. Switch A and B if A is a leaf, or if B is bigger and not a leaf4. Set A1 and A2 to be A’s children5. If Test(A1,B) = 1 then return Test(A2,B) else return 0

Performance• Several thousand collision checks per second for 2 three-

dimensional objects each described by 500,000 triangles, on a 1-GHz PC

Distance Computation

M

> M, prune

Greedy Distance Computation

Greedy-Distance(A,B,M)1. If min-dist(A,B) > M, then return M2. If A and B are both leaves, then return distance between their

contents 3. Switch A and B if A is a leaf, or if B is bigger and not a leaf4. Set A1 and A2 to be A’s children5. M min(max-dist(A1,B), max-dist(A2,B), M)6. d1 Greedy-Distance(A1,B,M) 7. d2 Greedy-Distance(A2,B,M) 8. Return Min(d1,d2)

M (upper bound on distance) is initialized to infinity

Approximate Distance

Approx-Greedy-Distance(A,B,M,a)1. If (1+a)min-dist(A,B) > M, then return M2. If A and B are both leaves, then return distance between their

contents 3. Switch A and B if A is a leaf, or if B is bigger and not a leaf4. Set A1 and A2 to be A’s children5. M min(max-dist(A1,B), max-dist(A2,B), M)6. d1 Approx-Greedy-Distance(A1,B,M,a) 7. d2 Approx-Greedy-Distance(A2,B,M,a) 8. Return Min(d1,d2)

M (upper bound on distance) is initialized to infinity

Desirable Properties of BVs and BVHsBVs:• Tightness• Efficient testing• Invariance

BVH: Separation Balanced tree

?

Spheres• Invariant• Efficient to test• But tight?

Axis-Aligned Bounding Box (AABB)

Axis-Aligned Bounding Box (AABB) Not invariant Efficient to test Not tight

Oriented Bounding Box (OBB)

Invariant Less efficient to test Tight

Oriented Bounding Box (OBB)

Comparison of BVs

Sphere AABB OBB

Tightness - -- +

Testing + + o

Invariance yes no yes

No type of BV is optimal for all situations

Desirable Properties of BVs and BVHsBVs:• Tightness• Efficient testing• Invariance

BVH: Separation Balanced tree ?

Desirable Properties of BVs and BVHsBVs:• Tightness• Efficient testing• Invariance

BVH: Separation Balanced tree

Construction of a BVH • Top-down construction • At each step, create the two children of a BV• Example:

For OBB, split longest side at midpoint

Computation of an OBB[Gottschalk, Lin, and Manocha, 96]

N points ai = (xi, yi, zi)T, i = 1,…, N

SVD of A = (a1 a2 ... aN) A = UDVT where

D = diag(s1,s2,s3) such that s1 s2 s3 0

U is a 3x3 rotation matrix that defines the principal axes of variance of the ai’s OBB’s directions

The OBB is defined by max and min coordinates of the ai’s along these directions

Possible improvements: use vertices of convex hull of the ai’s or dense uniform sampling of convex hull

x

y

X

Yrotation described bymatrix U

Static vs. Dynamic Collision Detection

Static checks Dynamic checks

Usual Approach to Dynamic Checking (in PRM Planning)1) Discretize path at some fine resolution e2) Test statically each intermediate configuration

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

1

2

32

3

3

3

Testing Path Segmentvs. Finding First Collision

PRM planning Detect collision as quickly as possible Bisection strategy

Physical simulation, haptic interactionFind first collision Sequential strategy

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

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

Other Approaches to Dynamic Collision Detection

Bounding-volume (BV) hierarchies Discretization issueFeature-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 Sequential strategy (first collision) that is not efficient for PRM path segmentsSwept-volume intersection

[Cameron, 85][Foisy, Hayward, 93]

Swept-volumes are expensive to compute. Too much data. No pre-computed BV hierarchiesAlgebraic trajectory parameterization

[Canny, 86][Schweikard, 91] [Redon, Kheddar, Coquillard, 00]

High-degree polynomials, expensive Floating-point arithmetics difficulties Sequential strategyCombination

[Redon, Kheddar, Coquillard, 00] BVH + algebraic parameterization [Ehmann, Lin, 01] BVH + feature tracking Sequential strategy

Exact Collision Detection with Adaptive Bisection

Idea: Cover line segment with collision free C-space neighborhoods

Use distance computation instead of collision checking

How do you show a C-space neighborhood is collision free?Relate changes in C-space to changes in workspace

Distance R

When moving from (x,y,q) to (x’,y’,q’), no point traces out more than distance|x-x’| + |y-y’| + R|q-q’|

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

l(q,q’) = 3|dq1|+2|dq2|+|dq3|

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

q1

q2

q3

How do you show a C-space neighborhood is collision free?Relate changes in C-space to changes in workspace

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

d(q)

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

q1

q2

q3

How do you show a C-space neighborhood is collision free?Relate changes in C-space to changes in workspace

q

q’

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

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

l(q,q’) < d(q) + d(q’)

q

q’

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

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

l(q,q’) < d(q) + d(q’)l(q,q’) = l(q,qint) + l(qint,q’) < d(q) + d(q’)

qint

q

q’

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

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

Bisectionl(q,q’) > d(q) + d(q’)

Generalization• A bound based on point that moves the most may be too

restrictive• Some links move much less than others• Some links may be closer to obstacles than others• There might be several interacting robots

• Instead look at each link individually

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’) = 2|dq1|+|dq2| l3(q,q’) = 3|dq1|+2|dq2|+|dq3|

q1

q2

q3

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’) < dij(q) + dij(q’)then Ai and Aj do not collide between q and q’

Generalized Bisection Method

I. Until Q is not empty do:1. [qa,qb]ij remove-first(Q)2. If li(qa,qb) + lj(qa,qb) dij(qa) + dij(qb) then

a. qmid (qa+qb)/2b. If dij(qmid) = 0 then return collisionc. 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 Q

Goal: Discover collision quicker if there is one.

Sort Q by decreasing values of: [li(qa,qb) + lj(qa,qb)] – [dij(qa) + dij(qb)]

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