CS 326A: Motion Planning Configuration Space

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

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

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

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

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

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

Protein

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   S1S1 R2S1R2S1

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

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

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

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

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

Example: A sphere

reference point Rigid Robot in 2-D Workspace 3-parameter representation: q = (x,y,  ) with   [0,2  ). Two charts are needed Other representation: q = (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

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

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)

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

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 )

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

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 ’’’’  

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  s’  s  d(  (s),  (s’))  0 q 1 q 3 q 0 q n q 4 q 2  (s)

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

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

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

Other Possible Constraints on Path  Finite length, smoothness, curvature, etc…  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)

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

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

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

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

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

Translating Polygon in 2-D Workspace reference point

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

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

Translating & Rotating Polygon in 2-D Workspace

Articulated Robot

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

Homotopic Paths  Two paths with the same endpoints are homotopic if one can be continuously deformed into the other  RxS 1 example:   1 and  2 are homotopic   1 and  3 are not homotopic  In this example, infinity of homotopy classes q q’   

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