CS B659: Principles of Intelligent Robot Motion

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

2. Probabilistic Roadmaps How to test forcollision?

3. q2 q1 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)

4. Rigid Transformation in 2D q = (tx,ty,q) with q [0,2p) Robot R0R2 given in reference frame T0 What’s the new robot Rq? workspace robot reference direction q ty reference point tx

5. Rigid Transformation in 2D q = (tx,ty,q) with q [0,2p) Robot R0R2 given in reference frame T0 What’s the new robot Rq?{Tq(x,y) | (x,y)  R0} Define rigid transformation Tq(x,y) : R2 R2 cos θ -sin θ sin θ cos θ x y tx ty Tq(x,y) = + 2D rotation matrix Affine translation

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

7. 3-D Rigid Rotations • det(A) = +1 • Orthogonal rows and columns: ATA=I, AAT=I • ||Ax|| = ||x|| for any x, L2 norm ||.|| • Product of two rotations is a rotation r11 r12 r13 r21 r22 r23 r31 r32 r33 (0,1,0) (r11,r21,r31) (Right-handed coordinate system) (r13,r23,r33) (1,0,0) (1,0,0) (r12,r22,r32) (0,0,1) (0,0,1)

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

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

10. Parameterization of SO(3) Euler angles: (f,q,y) z z z z y f y q y y y x x x x 1 2  3 4

11. Euler Angles Which axes to pick, and what order? Convention A,B,C R(f,q,y) = RA(f)RB(q)RC(y) E.g., ZYZ, ZYX (roll-pitch-yaw), etc

12. Euler Angles Which axes to pick, and what order? Convention A,B,C R(f,q,y) = RA(f)RB(q)RC(y) E.g., ZYZ, ZYX (roll-pitch-yaw), etc Disadvantages Must constrain to range of values Singularities, e.g. ZYZ when q=0 or p Interpolation?

13. Axis-Angle Representation • Axis v (||v||=1), angle θ • Rodrigues’ formula: rotate x about v -> x’ x’ = x cos θ + (v x x) sin θ + v (vT x) (1 - cos θ) Or in matrix form… R(θ,v) = cos θ I+ sin θ [v] + (1 - cos θ) vvT Cross product matrix 0 -vz vy vz 0 -vx -vy vx 0

14. Recovering Axis and Angle from the Rotation Matrix • θ = cos-1((r11+r22+r33-1)/2 • v = 1/(2 sin θ) [r32-r23, r13-r31, r21-r12]

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

16. Quaternion representation • Generalization of complex numbers • Complex z=z0+i z1, with |z|=1 can represent a 2D rotation. What’s the 3D analogue? q = q0+q1i + q2j +q3k, where i2 = j2 = k2 = -1i = jk = -kjj = ki = -ikk = ij = -ji Quaternions were a forerunner of vectors and were once mandatory of all students of physics and math!

17. Unit quaternion representation • q = (q0,q1,q2,q3), ||q||=1 • Related to axis angle: • q = (cos q/2,vx sin q/2, vy sin q/2, vz sin q/2) 2(q02+q12)-1 2(q1q2-q0q3) 2(q1q3+q0q2) 2(q1q2+q0q3) 2(q02+q22)-1 2(q2q3-q0q1) 2(q1q3-q0q2) 2(q2q3+q0q1) 2(q02+q32)-1 R(q) =

18. 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)

19. 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

20. Grid Method Subdivide space into a regular grid cubic of square bins Index each object in a bin d

21. Grid Method d Running time is proportional tonumber of moving objects Useful also to compute pairs of objects within some distance (vision,sound, …)

22. 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

23. Closest-Feature Test for Vertex-Vertex Vertex Vertex

24. 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)

25. Bounding Volume Hierarchy Method 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.

26. Bounding Volume Hierarchy Method • Enclose objects into bounding volumes (spheres or boxes) • Check the bounding volumes first • Decompose an object into two

27. Bounding Volume Hierarchy Method • Enclose objects into bounding volumes (spheres or boxes) • Check the bounding volumes first • Decompose an object into two • Proceed hierarchically

28. Bounding Volume Hierarchy Method • Enclose objects into bounding volumes (spheres or boxes) • Check the bounding volumes first • Decompose an object into two • Proceed hierarchically

29. Bounding Volume Hierarchy Method • BVH is pre-computed for each object

30. BVH in 3D

31. Collision Detection A A C C B B E F E F D D G G Two objects described by their precomputed BVHs

32. Collision Detection pruning Search tree AA A A

33. Collision Detection A C B E F D G BB BC CB CC Search tree AA A A

34. Collision Detection A C B E F D G pruning Search tree AA BB BC CB CC

35. Collision Detection A C B E F D G FD FE GD GE If two leaves of the BVH’s overlap(here, G and D) check their content for collision Search tree AA BB BC CB CC G D

36. Variant A C B AA E F D G BB BC CB CC AA BA CA Search tree A A

37. 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

38. 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 searchstrategy with f(N) = d/(rX+rY)[Expand the nodeXYwith largest relative overlap (most likely to contain a collision)] rY d rX Y X

39. Recursive (Depth-First) Collision Detection Algorithm Test(A,B) If A and B do not overlap, then return 1 If A and B are both leaves, then return 0 if their contents overlap and 1 otherwise Switch A and B if A is a leaf, or if B is bigger and not a leaf Set A1 and A2 to be A’s children If Test(A1,B) = 1 then return Test(A2,B) else return 0

40. Performance Several thousand collision checks per second for 2 three-dimensional objects each described by 500,000 triangles, on a 1-GHz PC

41. > M, prune M Distance Computation

42. Greedy Distance Computation Greedy-Distance(A,B,M) If min-dist(A,B) > M, then return M If A and B are both leaves, then return distance between their contents Switch A and B if A is a leaf, or if B is bigger and not a leaf Set A1 and A2 to be A’s children M  min(max-dist(A1,B), max-dist(A2,B), M) d1Greedy-Distance(A1,B,M) d2Greedy-Distance(A2,B,M) Return Min(d1,d2) M (upper bound on distance) is initialized to infinity

43. Approximate Distance Approx-Greedy-Distance(A,B,M,a) If (1+a)min-dist(A,B) > M, then return M If A and B are both leaves, then return distance between their contents Switch A and B if A is a leaf, or if B is bigger and not a leaf Set A1 and A2 to be A’s children M  min(max-dist(A1,B), max-dist(A2,B), M) d1Approx-Greedy-Distance(A1,B,M,a) d2Approx-Greedy-Distance(A2,B,M,a) Return Min(d1,d2) M (upper bound on distance) is initialized to infinity

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

45. Desirable Properties of BVs and BVHs BVs: Tightness Efficient testing Invariance • BVH: • Separation • Balanced tree

46. Spheres Invariant Efficient to test But tight?

47. Axis-Aligned Bounding Box (AABB)

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

49. Oriented Bounding Box (OBB)

50. Oriented Bounding Box (OBB) • Invariant • Less efficient to test • Tight