1. CS B659: Principles of Intelligent Robot Motion Configuration Space

2. Readings Ch 3.1-3.4 A Simple Motion-Planning Algorithm for General Robot Manipulators, T. Lozano-Perez, 1987. * Spatial Planning: a Configuration Space Approach, T. Lozano-Perez, 1980.

3. Agenda Introduce configuration spaces (C-spaces)  Topology of C-spaces  Mapping obstacles into C-space Discuss semester project ideas Reading schedule

4. Motion Planning Framework Continuous representation (configuration space and related spaces + constraints) Discretization (random sampling, criticality-based decomposition) Graph searching (blind, best-first, A*)

5. Robots have different shapes and kinematics! What is a path?

6. Idea: Reduce robot to a point  Configuration Space Feasible space Forbidden space

7. Configuration Space q=(q 1,…,q n ) q1q1q1q1 q2q2q2q2 q3q3q3q3 qnqnqnqn  A robot configuration is a specification of the positions of all robot points relative to a fixed coordinate system  Usually a configuration is expressed as a “vector” of parameters

8. reference point Rigid Robot 3-parameter representation: q = (x,y,  ) In a 3-D workspace q would be of the form (x,y,z,  ) x y  robot reference direction workspace

9. Articulated Robot q1q1q1q1 q2q2q2q2 q = (q 1,q 2,…,q 10 )

10. Protein

11. Configuration Space  Space of all its possible configurations  But the topology of this space is usually not that of a Cartesian space y  robot x q q’ 3-D cylinder embedded in 4-D space x y  22 S1S1 R2S1R2S1

12. Configuration Space  Space of all its possible configurations  But the topology of this space is usually not that of a Cartesian space C = S 1 x S 1

13. Configuration Space  Space of all its possible configurations  But the topology of this space is usually not that of a Cartesian space C = S 1 xS 1

14. Configuration Space  Space of all its possible configurations  But the topology of this space is usually not that of a Cartesian space C = S 1 xS 1

15. What is its topology? q1q1q1q1 q2q2q2q2 (S1) 7 xI 3 (I: Interval of reals)

16. Structure of Configuration Space  It is a manifold, i.e., for each point q, there is a 1-to-1 map between a neighborhood of q and a Cartesian space R n, where n is the dimensionality of C  This map is a local coordinate system called a chart.  C can always be covered by a finite number of charts. Such a set is called an atlas

17. Example: A sphere

18. reference point Rigid Robot in 2-D Workspace 3-parameter representation: q = (x,y,  ) with   [0,2  ). Two charts are needed Other representation:  = (x,y,cos ,sin  )  c-space is a 3-D cylinder R 2 x S 1 embedded in a 4-D space x y  robot reference direction workspace

19. Rigid Robot in 3-D Workspace q = (x,y,z,  ) Other representation: q = (x,y,z,r 11,r 12,…,r 33 ) where r 11, r 12, …, r 33 are the elements of rotation matrix R: r 11 r 12 r 13 r 21 r 22 r 23 r 31 r 32 r 33 with:  r i1 2 +r i2 2 +r i3 2 = 1  r i1 r j1 + r i2 r 2j + r i3 r j3 = 0  det(R) = +1

20. Rigid Robot in 3-D Workspace q = (x,y,z,  ) Other representation: q = (x,y,z,r 11,r 12,…,r 33 ) where r 11, r 12, …, r 33 are the elements of rotation matrix R: r 11 r 12 r 13 r 21 r 22 r 23 r 31 r 32 r 33 with:  r i1 2 +r i2 2 +r i3 2 = 1  r i1 r j1 + r i2 r 2j + r i3 r j3 = 0  det(R) = +1 The c-space is a 6-D space (manifold) embedded in a 12-D Cartesian space. It is denoted by R 3 xSO(3)

21. Parameterization of SO(3)  Euler angles: (   Unit quaternion: (cos  /2, n 1 sin  /2, n 2 sin  /2, n 3 sin  /2) x yz x yz x y z  x yz 1  2  3  4

22. Parameterization of SO(3)  Euler angles: (   Unit quaternion: (cos  /2, n 1 sin  /2, n 2 sin  /2, n 3 sin  /2) x yz x yz x y z  x yz 1  2  3  4 More on this next week …

23. Notion of a Path  A path in C is a piece of continuous curve connecting two configurations q and q’:  : s  [0,1]   (s)  C q 1 q 3 q 0 q n q 4 q 2  (s)

24. Notion of Trajectory vs. Path  A trajectory is a path parameterized by time:  : t  [0,T]   (t)  C q 1 q 3 q 0 q n q 4 q 2  (s)

25. x y  reference point robot reference direction workspace  x y 22 configuration space What is the placement of the robot in the workspace at configuration (0,0,0)? Translating & Rotating Rigid Robot in 2-D Workspace

26. x y  reference point robot reference direction workspace  x y 22 configuration space What is the placement of the robot in the workspace at configuration (0,0,0)? Translating & Rotating Rigid Robot in 2-D Workspace

27. x y  reference point robot reference direction workspace  x y 22 configuration space What is this path in the workspace? What would be the path in configuration space corresponding to a full rotation of the robot about point P? P Translating & Rotating Rigid Robot in 2-D Workspace

28. Every robot maps to a point in its configuration space... q 1 q 3 q 0 q n q 4 12 D ~65-120 D 6 D 15 D ~40 D

29. ... and every robot path is a curve in configuration space q 1 q 3 q 0 q n q 4

30. But how do obstacles (and other constraints) map in configuration space? q 1 q 3 q 0 q n q 4 12 D ~65-120 D 6 D 15 D ~40 D

31. Obstacles in C-Space  A configuration q is collision-free, or free, if the robot placed at q has null intersection with the obstacles in the workspace  The free space F is the set of free configurations  A C-obstacle is the set of configurations where the robot collides with a given workspace obstacle  A configuration is semi-free if the robot at this configuration touches obstacles without overlap

32. Disc Robot in 2-D Workspace Workspace W Configuration space C x y configuration = coordinates (x,y) of robot’s center configuration space C = {(x,y)} free space F = subset of collision-free configurations path

33. Translating Polygon in 2-D Workspace reference point

34. CB = B A = {b-a | a  A, b  B} a1 b1 b1-a1

35. Linear-Time Computation (convex polygons) O(n+m)

36. Translating & Rotating Polygon in 2- D Workspace

37. Articulated Robot

38. Some constraints can’t be modeled as C-Space obstacles  Differential constraints: smoothness, finite curvature, drift, etc… Global constraints: finite length, power consumption, etc…

39. Remark on Free-Space Topology  The robot and the obstacles are modeled as closed subsets, meaning that they contain their boundaries  One can show that the C-obstacles are closed subsets of the configuration space C as well  Consequently, the free space F is an open subset of C. Hence, each free configuration is the center of a ball of non-zero radius entirely contained in F

40. Homotopic Paths  Two paths with the same endpoints are homotopic if one can be continuously deformed into the other  RxS 1 example:  t 1 and t 2 are homotopic  t 1 and t 3 are not homotopic  In this example, infinity of homotopy classes q q’ t1t1 t2t2 t3t3

41. Connectedness of C-Space  C is connected if every two configurations can be connected by a path  C is simply-connected if any two paths connecting the same endpoints are homotopic Examples: R 2 or R 3  Otherwise C is multiply-connected Examples: S 1 and SO(3) are multiply-connected: - In S 1, infinity of homotopy classes - In SO(3), only two homotopy classes

42. Recap Configuration space:  Tool to map a robot in a 2-D or 3-D workspace into a point in n-D space Mapping obstacles into C-space

43. Schedule 1/21: Probabilistic roadmap planning 1/26: Rigid transforms & collision detection 1/28: Forward & inverse kinematics  2 slots 2/2, 2/4: project discussions 2/9, 2/11: Nonholonomic systems  3 slots

44. Project Proposal Due 1/28 for each group 1-2 paragraphs on proposed topic

45. Notions of Continuity Path continuity (geometric, or C 0 )  For all s, d(  (s),  (s’)) -> 0 as s’->s Trajectory continuity (geometric, or C 0 )  For all t, d(  (t),  (t’)) -> 0 as t’->t Higher order continuity  C 1 : For all t,  ’(t) is well defined  C 2 : For all t,  ’’(t) is well defined  …

46. Metric in Configuration Space A metric or distance function d in C is a map d: (q 1,q 2 )  C 2  d(q 1,q 2 ) > 0 such that:  d(q 1,q 2 ) = 0 if and only if q 1 = q 2  d(q 1,q 2 ) = d (q 2,q 1 )  d(q 1,q 2 ) < d(q 1,q 3 ) + d(q 3,q 2 )

47. Metric in Configuration Space Example: Robot A and point x of A x(q): location of x in the workspace when A is at configuration q A distance d in C is defined by: d(q,q’) = max x  A ||x(q)-x(q’)|| where ||a - b|| denotes the Euclidean distance between points a and b in the workspace

48. Specific Examples in R 2 x S 1  q = (x,y,  ), q ’ = (x ’,y ’,  ’ ) with   ’  [0,2  )   = min{|  ’ |, 2  |  ’ |}  d(q,q ’ ) = sqrt[(x-x ’ ) 2 + (y-y ’ ) 2 +  2 ]  d(q,q ’ ) = sqrt[(x-x ’ ) 2 + (y-y ’ ) 2 + (  ) 2 ] where  is the maximal distance between the reference point and a robot point ’’’’  