CS B659: Principles of Intelligent Robot Motion Rigid Transformations and Collision Detection

Probabilistic Roadmaps How to test for collision?

Articulated Robot Robot: usually a rigid articulated structure Geometric CAD models, relative to reference frames A configuration specifies the placement of those frames (next lecture on kinematics) q1q1q1q1 q2q2q2q2

reference point Rigid Transformation in 2D q = (t x,t y,  ) with   [0,2  ) Robot R 0  R 2 given in reference frame T 0 What’s the new robot R q ? txtx tyty  robot reference direction workspace

Rigid Transformation in 2D q = (t x,t y,  ) with   [0,2  ) Robot R 0  R 2 given in reference frame T 0 What’s the new robot R q ? {T q (x,y) | (x,y)  R 0 } Define rigid transformation T q (x,y) : R 2  R 2 T q (x,y) = cos θ -sin θ sin θ cos θ xyxy txtytxty + 2D rotation matrixAffine translation

2D Rigid Rotations A rotation matrix A has:  det(A) = +1  Orthogonal rows and columns: A T A=I, AA T =I  ||Ax|| = ||x|| for any x, L 2 norm ||.|| Product of two rotations is a rotation cos θ -sin θ sin θ cos θ θ (cos θ, sin θ) (-sin θ, cos θ) (1,0) (0,1)

3-D Rigid Rotations r 11 r 12 r 13 r 21 r 22 r 23 r 31 r 32 r 33 (1,0,0) (0,1,0) (0,0,1) (Right-handed coordinate system) det(A) = +1 Orthogonal rows and columns: A T A=I, AA T =I ||Ax|| = ||x|| for any x, L 2 norm ||.|| Product of two rotations is a rotation (1,0,0) (r 11,r 21,r 31 ) (0,0,1) (r 13,r 23,r 33 ) (r 12,r 22,r 32 )

3 representations Euler angles Axis angle Quaternion All representations are “equivalent” but may have certain mathematical or computational advantages

Axis-aligned rotations cos θ -sin θ 0 sin θ cos θ Rotate about: Z axis Y axis X axis cos θ 0 sin θ sin θ 0 cos θ cos θ -sin θ 0 sin θ cos θ

Parameterization of SO(3)  Euler angles: (  x yz x yz x y z  x yz 1  2  3  4

Euler Angles  Which axes to pick, and what order?  Convention A,B,C  R(  ) = R A (  )R B (  )R C (  )  E.g., ZYZ, ZYX (roll-pitch-yaw), etc

Euler Angles  Which axes to pick, and what order?  Convention A,B,C  R(  ) = R A (  )R B (  )R C (  )  E.g., ZYZ, ZYX (roll-pitch-yaw), etc  Disadvantages  Must constrain to range of values  Singularities, e.g. ZYZ when  =0 or   Interpolation?

Axis-Angle Representation Axis v (||v||=1), angle θ Rodrigues’ formula: rotate x about v -> x’ x’ = x cos θ + (v x x) sin θ + v (v T x) (1 - cos θ) R(θ,v) = cos θ I + sin θ [v] + (1 - cos θ) v v T Cross product matrix Or in matrix form… 0 -v z v y v z 0 -v x -v y v x 0

Recovering Axis and Angle from the Rotation Matrix θ = cos -1 ((r 11 +r 22 +r 33 -1)/2 v = 1/(2 sin θ) [r 32 -r 23, r 13 -r 31, r 21 -r 12 ]

Properties of Axis-Angle Problems: Non unique: R(θ,v)=R(-θ,-v) Encode vector w = θv  θ = ||w||, v = w/||w|| Advantages  θ(R 1 T R 2 ) is a good distance metric between rotations R 1, R 2  Interpolate by scaling θ  For parameterized rotation trajectory R( t w ), w is the angular velocity (exponential map)

Quaternion representation Generalization of complex numbers  Complex z=z 0 +i z 1, with |z|=1 can represent a 2D rotation. What’s the 3D analogue? Quaternions were a forerunner of vectors and were once mandatory of all students of physics and math! q = q 0 +q 1 i + q 2 j +q 3 k, where i 2 = j 2 = k 2 = -1 i = jk = -kj j = ki = -ik k = ij = -ji

Unit quaternion representation q = (q 0,q 1,q 2,q 3 ), ||q||=1 Related to axis angle:  q = (cos  /2,v x sin  /2, v y sin  /2, v z sin  /2) R(q) = 2(q 0 2 +q 1 2 )-12(q 1 q 2 -q 0 q 3 )2(q 1 q 3 +q 0 q 2 ) 2(q 1 q 2 +q 0 q 3 ) 2(q 0 2 +q 2 2 )-1 2(q 2 q 3 -q 0 q 1 ) 2(q 1 q 3 -q 0 q 2 ) 2(q 2 q 3 +q 0 q 1 ) 2(q 0 2 +q 3 2 )-1

Properties of Unit Quaternions 4-sphere: 3D manifold in 4D space Double cover of SO(3) Advantages:  Quaternion multiplication = rotation composition (slightly faster than matrix *)  Curve interpolation formulas (Shoemake, ‘85)

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 d  Subdivide space into a regular grid cubic of square bins  Index each object in a bin

Grid Method d Running time is proportional to number 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 Vertex

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, 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, 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 Eurographics, Vol. 20, No. 3, pp , 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, 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 EF G A B C D EF G

Collision Detection A Search tree A A pruning

Collision Detection A CCBCBBCBCB Search tree A A A B C D EF G

Collision Detection CCBCBBCBCB A Search tree pruning A B C D EF G

If two leaves of the BVH’s overlap (here, G and D) check their content for collision Collision Detection CCBCBBCBCB A Search tree GEGEGDGDFEFEFDFD A B C D EF G G D

Variant A CCBCBBCBCB Search tree A A A B C D EF G A CACABABA

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/(r X +r Y ) [Expand the node XY with largest relative overlap (most likely to contain a collision)] rXrX rYrY d X Y

Recursive (Depth-First) Collision Detection Algorithm Test(A,B) 1. If A and B do not overlap, then return 1 2. If A and B are both leaves, then return 0 if their contents overlap and 1 otherwise 3. Switch A and B if A is a leaf, or if B is bigger and not a leaf 4. Set A 1 and A 2 to be A’s children 5. If Test(A 1,B) = 1 then return Test(A 2,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 M 2. 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 leaf 4. Set A 1 and A 2 to be A’s children 5. M  min(max-dist(A 1,B), max-dist(A 2,B), M) 6. d 1  Greedy-Distance(A 1,B,M) 7. d 2  Greedy-Distance(A 2,B,M) 8. Return Min(d 1,d 2 ) M (upper bound on distance) is initialized to infinity

Approximate Distance Approx-Greedy-Distance(A,B,M,  ) 1. If (1+  )min-dist(A,B) > M, then return M 2. 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 leaf 4. Set A 1 and A 2 to be A’s children 5. M  min(max-dist(A 1,B), max-dist(A 2,B), M) 6. d 1  Approx-Greedy-Distance(A 1,B,M,  ) 7. d 2  Approx-Greedy-Distance(A 2,B,M,  ) 8. Return Min(d 1,d 2 ) M (upper bound on distance) is initialized to infinity

Desirable Properties of BVs and BVHs BVs:  Tightness  Efficient testing  Invariance BVH:  Separation  Balanced tree ?

Desirable Properties of BVs and BVHs BVs:  Tightness  Efficient testing  Invariance BVH:  Separation  Balanced tree

Spheres  Invariant  Efficient to test  But tight?

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 SphereAABBOBB Tightness---+ Testing++o Invarianceyesnoyes No type of BV is optimal for all situations

Desirable Properties of BVs and BVHs BVs:  Tightness  Efficient testing  Invariance BVH:  Separation  Balanced tree ?

Desirable Properties of BVs and BVHs BVs:  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 a i = (x i, y i, z i ) T, i = 1,…, N  SVD of A = (a 1 a 2... a N )  A = UDV T where  D = diag(s 1,s 2,s 3 ) such that s 1  s 2  s 3  0  U is a 3x3 rotation matrix that defines the principal axes of variance of the a i ’s  OBB’s directions  The OBB is defined by max and min coordinates of the a i ’s along these directions  Possible improvements: use vertices of convex hull of the a i ’s or dense uniform sampling of convex hull x y X Y rotation described by matrix U

Static vs. Dynamic Collision Detection Static checks Dynamic checks

Usual Approach to Dynamic Checking (in PRM Planning) <    too large  collisions are missed   too small  slow test of local paths ) Discretize path at some fine resolution e 2) Test statically each intermediate configuration

Testing Path Segment vs. Finding First Collision  PRM planning Detect collision as quickly as possible  Bisection strategy  Physical simulation, haptic interaction Find first collision  Sequential strategy

  too large  collisions are missed   too small  slow test of local paths

  too large  collisions are missed   too small  slow test of local paths

Other Approaches to Dynamic Collision Detection 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  Sequential strategy (first collision) that is not efficient for PRM path segments Swept-volume intersection [Cameron, 85] [Foisy, Hayward, 93]  Swept-volumes are expensive to compute. Too much data.  No pre-computed BV hierarchies Algebraic trajectory parameterization [Canny, 86] [Schweikard, 91] [Redon, Kheddar, Coquillard, 00]  High-degree polynomials, expensive  Floating-point arithmetics difficulties  Sequential strategy Combination [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,  ) to (x’,y’,  ’), no point traces out more than distance |x-x’| + |y-y’| + R|  -  ’|

For any q and q’ no robot point traces a path longer than: (q,q’) = 3|  q 1 |+2|  q 2 |+|  q 3 | q = (q 1,q 2,q 3 ) q’ = (q’ 1,q’ 2,q’ 3 )  q i = q’ i -q i q1q1 q2q2 q3q3 How do you show a C-space neighborhood is collision free? Relate changes in C-space to changes in workspace

If (q,q’) <  (q) +  (q’) then the straight path between q and q’ is collision-free  (q)  (q) = Euclidean distance between robot and obstacles (or lower bound) q1q1 q2q2 q3q3 How do you show a C-space neighborhood is collision free? Relate changes in C-space to changes in workspace

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

q q’ {q” | (q,q”) <  (q)} {q” | (q’,q”) <  (q’)} (q,q’) <  (q) +  (q’) (q,q’) = (q,q int ) + (q int,q’) <  (q) +  (q’) q int

q q’ {q” | (q,q”) <  (q)} {q” | (q’,q”) <  (q’)} Bisection (q,q’) >  (q) +  (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. 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’) = |  q 1 | 2 (q,q’) = 2|  q 1 |+|  q 2 | 3 (q,q’) = 3|  q 1 |+2|  q 2 |+|  q 3 | q1q1 q2q2 q3q3

Generalization Robot(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 Method I. Until Q is not empty do: 1. [q a,q b ] ij  remove-first(Q) 2. If i (q a,q b ) + j (q a,q b )   ij (q a ) +  ij (q b ) then a. q mid  (q a +q b )/2 b. If h ij (q mid ) = 0 then return collision c. Else insert [q a,q mid ] ij and [q mid,q b ] ij into Q II. Return no collision Each pair of bodies is checked independently of the others  priority queue Q of elements [q a,q b ] 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: [ i (q a,q b ) + j (q a,q b )] – [  ij (q a ) +  ij (q b )]  Possible extension to multi-segment paths (very useful with lazy collision-checking PRM)