Configuration Space Obstacles

Section 4.2 defined , the manifold of robot transformations in the absence of any collision constraints. The current section removes from the configurations that either cause the robot to collide with obstacles or cause some specified links of the robot to collide with each other. The removed part of is referred to as the obstacle region. The leftover space is precisely what a solution path must traverse. A motion planning algorithm must find a path in the leftover space from an initial configuration to a goal configuration. Finally, after the models of Chapter 3 and the previous sections of this chapter, the motion planning problem can be precisely described.

C

C

C

• • 1. Definition of the Basic Motion Planning Problem

Obstacle region for a rigid body Suppose that the world, = R2 or = R3, contains an obstacle region, . Assume here that a rigid robot, , is defined; the case of multiple links will be handled shortly. Assume that both

O ⊂ W A ⊂ W

W W

and are expressed as semi-algebraic models (which includes polygonal and polyhedral models) from Section 3.1. Let q denote the configuration of , in which q = (xt, yt, θ) for = R2 and q = (xt, yt, zt, h) for = R3 (h represents the unit quaternion).

W W

∈ C A

A O

The obstacle region, Cobs ⊆ C, is defined as

Cobs = {q ∈ C | A(q) ∩ O /= ∅}, (4.34)

which is the set of all configurations, q, at which (q), the transformed robot, intersects the obstacle region, . Since and (q) are closed sets in , the obstacle region is a closed set in .

C

O O A W

A

The leftover configurations are called the free space, which is defined and de- noted as free = obs. Since is a topological space and obs is closed, free must be an open set. This implies that the robot can come arbitrarily close to the

C C \ C C C C

obstacles while remaining in Cfree. If A “touches” O,

int(O) ∩ int(A(q)) = ∅ and O ∩ A(q) /= ∅, (4.35)

then q obs (recall that int means the interior). The condition above indicates that only their boundaries intersect.

∈ C

The idea of getting arbitrarily close may be nonsense in practical robotics, but it makes a clean formulation of the motion planning problem. Since free is open, it becomes impossible to formulate some optimization problems, such as finding the shortest path. In this case, the closure, cl( free), should instead be used, as described in Section 7.7.

C

C

Obstacle region for multiple bodies If the robot consists of multiple bodies, the situation is more complicated. The definition in (4.34) only implies that the robot does not collide with the obstacles; however, if the robot consists of multiple bodies, then it might also be appropriate to avoid collisions between different links of the robot. Let the robot be modeled as a collection, 1, 2, . . . , m , of m links, which may or may not be attached together by joints. A single configuration

{A A A }

vector q is given for the entire collection of links. We will write Ai(q) for each link,

i, even though some of the parameters of q may be irrelevant for moving link i. For example, in a kinematic chain, the configuration of the second body does not depend on the angle between the ninth and tenth bodies.

A

Let P denote the set of collision pairs, in which each collision pair, (i, j) ∈ P, represents a pair of link indices i, j ∈ {1, 2, . . . , m}, such that i /= j. If (i, j)

appears in P, it means that Ai and Aj are not allowed to be in a configuration, q, for which i(q) j (q) = . Usually, P does not represent all pairs because consecutive links are in contact all of the time due to the joint that connects them. One common definition for P is that each link must avoid collisions with any links to which it is not attached by a joint. For m bodies, P is generally of size O(m2); however, in practice it is often possible to eliminate many pairs by some geometric analysis of the linkage. Collisions between some pairs of links may be impossible over all of , in which case they do not need to appear in P.

C

A ∩ A / ∅

Using P, the consideration of robot self-collisions is added to the definition of

Cobs to obtain

（ m ＼ （ ＼

I

I

Cobs =

{q ∈ C | Ai(q) ∩ O /= ∅}

i=1

[i,_I_j]∈P

{q ∈ C | Ai(q) ∩ Aj (q)

∅} .

(4.36)

Figure 4.11: The basic motion planning problem is conceptually very simple using C-space ideas. The task is to find a path from qI to qG in Cfree. The entire blob represents C = Cfree ∪ Cobs.

Thus, a configuration q is in obs if at least one link collides with or a pair of links indicated by P collide with each other.

∈ C C O

Definition of basic motion planning Finally, enough tools have been intro- duced to precisely define the motion planning problem. The problem is concep- tually illustrated in Figure 4.11. The main difficulty is that it is neither straight- forward nor efficient to construct an explicit boundary or solid representation of

either Cfree or Cobs. The components are as follows:

Formulation 4.1 (The Piano Mover’s Problem)

• • • 1. A world W in which either W = R2 or W = R3.
      2. A semi-algebraic obstacle region O ⊂ W in the world.
      3. A semi-algebraic robot is defined in W. It may be a rigid robot A or a collection of m links, A1, A2, . . . , Am.
      4. The configuration space C determined by specifying the set of all possible transformations that may be applied to the robot. From this, obs and free are derived.

C C

• • • 1. A configuration, qI ∈ Cfree designated as the initial configuration.
      2. A configuration qG free designated as the goal configuration. The initial and goal configurations together are often called a query pair (or query) and designated as (qI , qG).

∈ C

• • • 1. A complete algorithm must compute a (continuous) path, τ : [0, 1] → Cfree, such that τ (0) = qI and τ (1) = qG, or correctly report that such a path does not exist.

It was shown by Reif [817] that this problem is PSPACE-hard, which implies NP-hard. The main problem is that the dimension of C is unbounded.

• • 1. Explicitly Modeling Cobs: The Translational Case

It is important to understand how to construct a representation of obs. In some algorithms, especially the combinatorial methods of Chapter 6, this represents an important first step to solving the problem. In other algorithms, especially the sampling-based planning algorithms of Chapter 5, it helps to understand why such constructions are avoided due to their complexity.

C

The simplest case for characterizing obs is when = Rn for n = 1, 2, and 3, and the robot is a rigid body that is restricted to translation only. Under

C C

these conditions, Cobs can be expressed as a type of convolution. For any two sets

X, Y ⊂ Rn, let their Minkowski difference10 be defined as

X ⊖ Y = {x − y ∈ R | x ∈ X and y ∈ Y }, (4.37) in which x−y is just vector subtraction on Rn. The Minkowski difference between

n

X and Y can also be considered as the Minkowski sum of X and −Y . The

Minkowski sum ⊕ is obtained by simply adding elements of X and Y in (4.37), as opposed to subtracting them. The set −Y is obtained by replacing each y ∈ Y by

−y.

In terms of the Minkowski difference, obs = (0). To see this, it is helpful to consider a one-dimensional example.

C O ⊖ A

Example 4.13 (One-Dimensional C-Space Obstacle) In Figure 4.12, both the robot A = [−1, 2] and obstacle region O = [0, 4] are intervals in a one- dimensional world, W = R. The negation, −A, of the robot is shown as the interval [−2, 1]. Finally, by applying the Minkowski sum to O and −A, the C- space obstacle, Cobs = [−2, 5], is obtained. ．

The Minkowski difference is often considered as a convolution. It can even be defined to appear the same as studied in differential equations and system theory.

10In some contexts, which include mathematics and image processing, the Minkowski difference or Minkowski subtraction is defined differently (instead, it is a kind of “erosion”). For this

reason, some authors prefer to define all operations in terms of the Minkowski sum, ⊕, which is

consistently defined in all contexts. Following this convention, we would define X ⊕ (−Y ), which is equivalent to X ⊖ Y .

-4 -3 -2 -1 0

1 2 3

4 5 6

A

O

−A

Cobs

Figure 4.12: A one-dimensional C-space obstacle.

O

Figure 4.13: A triangular robot and a rectangular obstacle.

For a one-dimensional example, let f : R → {0, 1} be a function such that f(x) = 1 if and only if x ∈ O. Similarly, let g : R → {0, 1} be a function such that g(x) = 1 if and only if x ∈ A. The convolution

h(x) = ∞ f(τ )g(x − τ )dτ, (4.38)

r

−∞

yields a function h, for which h(x) > 0 if x ∈ int(Cobs), and h(x) = 0 otherwise.

A polygonal C-space obstacle A simple algorithm for computing Cobs exists in the case of a 2D world that contains a convex polygonal obstacle O and a convex polygonal robot A [657]. This is often called the star algorithm. For this

problem, obs is also a convex polygon. Recall that nonconvex obstacles and robots can be modeled as the union of convex parts. The concepts discussed below can also be applied in the nonconvex case by considering obs as the union of convex components, each of which corresponds to a convex component of colliding with a convex component of .

C

O

A

C

The method is based on sorting normals to the edges of the polygons on the basis of angles. The key observation is that every edge of Cobs is a translated edge from either A or O. In fact, every edge from O and A is used exactly once in the construction of Cobs. The only problem is to determine the ordering of these edges of Cobs. Let α1, α2, . . ., αn denote the angles of the inward edge normals

in counterclockwise order around . Let β1, β2, . . ., βn denote the outward edge normals to . After sorting both sets of angles in circular order around S1, obs can be constructed incrementally by using the edges that correspond to the sorted

O C

A

normals, in the order in which they are encountered.

Example 4.14 (A Triangular Robot and Rectangular Obstacle) To gain an understanding of the method, consider the case of a triangular robot and a rect-

angular obstacle, as shown in Figure 4.13. The black dot on A denotes the origin

1. (b)

Figure 4.14: (a) Slide the robot around the obstacle while keeping them both in contact. (b) The edges traced out by the origin of A form Cobs.

α

α_2 β_3

_β_2

_β_2

_α_3

_α_2

β

1

β_3 β_1

β_4 β4α_1

• 1. (b)

Figure 4.15: (a) Take the inward edge normals of A and the outward edge normals of O. (b) Sort the edge normals around S1. This gives the order of edges in Cobs.

of its body frame. Consider sliding the robot around the obstacle in such a way

that they are always in contact, as shown in Figure 4.14a. This corresponds to the traversal of all of the configurations in ∂Cobs (the boundary of Cobs). The origin of

traces out the edges of obs, as shown in Figure 4.14b. There are seven edges, and each edge corresponds to either an edge of or an edge of . The directions

A O

A C

of the normals are defined as shown in Figure 4.15a. When sorted as shown in Figure 4.15b, the edges of Cobs can be incrementally constructed. ．

The running time of the algorithm is O(n + m), in which n is the number of edges defining , and m is the number of edges defining . Note that the angles can be sorted in linear time because they already appear in counterclockwise order

A O

around A and O; they only need to be merged. If two edges are collinear, then they can be placed end-to-end as a single edge of Cobs.

Computing the boundary of Cobs So far, the method quickly identifies each edge that contributes to obs. It can also construct a solid representation of obs in terms of half-planes. This requires defining n + m linear equations (assuming there are no collinear edges).

C C

Type EV Type VE

Figure 4.16: Two different types of contact, each of which generates a different kind of Cobs edge [280, 657].

There are two different ways in which an edge of obs is generated, as shown in Figure 4.16 [282, 657]. Type EV contact refers to the case in which an edge

C

of A is in contact with a vertex of O. Type EV contacts contribute to n edges of Cobs, once for each edge of A. Type VE contact refers to the case in which a

vertex of is in contact with an edge of . This contributes to m edges of obs. The relationships between the edge normals are also shown in Figure 4.16. For Type EV, the inward edge normal points between the outward edge normals of the obstacle edges that share the contact vertex. Likewise for Type VE, the outward edge normal of points between the inward edge normals of .

O A

A O C

Using the ordering shown in Figure 4.15b, Type EV contacts occur precisely when an edge normal of A is encountered, and Type VE contacts occur when an

Figure 4.17: Contact occurs when n and v are perpendicular.

edge normal of is encountered. The task is to determine the line equation for each occurrence. Consider the case of a Type EV contact; the Type VE contact can be handled in a similar manner. In addition to the constraint on the directions of the edge normals, the contact vertex of must lie on the contact edge of . Recall that convex obstacles were constructed by the intersection of half-planes.

O

O A

Each edge of Cobs can be defined in terms of a supporting half-plane; hence, it is only necessary to determine whether the vertex of O lies on the line through the contact edge of A. This condition occurs precisely as n and v are perpendicular, as shown in Figure 4.17, and yields the constraint n · v = 0.

Note that the normal vector n does not depend on the configuration of because the robot cannot rotate. The vector v, however, depends on the translation q = (xt, yt) of the point p. Therefore, it is more appropriate to write the condition

A

as n · v(xt, yt) = 0. The transformation equations are linear for translation; hence,

n · v(xt, yt) = 0 is the equation of a line in C. For example, if the coordinates of p are (1, 2) for A(0, 0), then the expression for p at configuration (xt, yt) is (1 + xt, 2 + yt). Let f(xt, yt) = n · v(xt, yt). Let H = {(xt, yt) ∈ C | f(xt, yt) ≤ 0}. Observe that any configurations not in H must lie in Cfree. The half-plane H

is used to define one edge of obs. The obstacle region obs can be completely characterized by intersecting the resulting half-planes for each of the Type EV and Type VE contacts. This yields a convex polygon in that has n + m sides, as expected.

C

C C

_b_2

(−1, 1)

_b_1

(1, 1)

(−1, −1) (1, −1)

_b_3 _b_4

Figure 4.18: Consider constructing the obstacle region for this example.

Figure 4.19: The various contact conditions are shown in the order as the edge normals appear around S1 (using inward normals for A and outward normals for O).

Example 4.15 (The Boundary of Cobs) Consider building a geometric model of obs for the robot and obstacle shown in Figure 4.18. Suppose that the orien- tation of is fixed as shown, and = R2. In this case, obs will be a convex polygon with seven sides. The contact conditions that occur are shown in Figure

A C C

C

4.19. The ordering as the normals appear around S1 (using inward edge normals

A O C

for and outward edge normals for ). The obs edges and their corresponding contact types are shown in Figure 4.19. ．

A polyhedral C-space obstacle Most of the previous ideas generalize nicely

for the case of a polyhedral robot that is capable of translation only in a 3D world that contains polyhedral obstacles. If A and O are convex polyhedra, the resulting Cobs is a convex polyhedron.

Type FV Type VF Type EE

Figure 4.20: Three different types of contact, each of which generates a different kind of Cobs face.

There are three different kinds of contacts that each lead to half-spaces in C:

1. Type FV: A face of A and a vertex of O

Figure 4.21: An illustration to help in constructing Cobs when rotation is allowed.

1. Type VF: A vertex of A and a face of O
2. Type EE: An edge of A and an edge of O .

These are shown in Figure 4.20. Each half-space defines a face of the polyhedron, Cobs. The representation of Cobs can be constructed in O(n + m + k) time, in which n is the number of faces of A, m is the number of faces of O, and k is the number of faces of Cobs, which is at most nm [411].

• • 1. Explicitly Modeling Cobs: The General Case

Unfortunately, the cases in which obs is polygonal or polyhedral are quite lim- ited. Most problems yield extremely complicated C-space obstacles. One good point is that obs can be expressed using semi-algebraic models, for any robots and obstacles defined using semi-algebraic models, even after applying any of the transformations from Sections 3.2 to 3.4. It might not be true, however, for other kinds of transformations, such as warping a flexible material [32, 577].

C

C

Consider the case of a convex polygonal robot and a convex polygonal obstacle in a 2D world. Assume that any transformation in SE(2) may be applied to A; thus, C = R2 × S1 and q = (xt, yt, θ). The task is to define a set of algebraic

primitives that can be combined to define obs. Once again, it is important to distinguish between Type EV and Type VE contacts. Consider how to construct the algebraic primitives for the Type EV contacts; Type VE can be handled in a similar manner.

C

For the translation-only case, we were able to determine all of the Type EV contacts by sorting the edge normals. With rotation, the ordering of edge normals depends on θ. This implies that the applicability of a Type EV contact depends on θ, the robot orientation. Recall the constraint that the inward normal of must point between the outward normals of the edges of that contain the vertex of contact, as shown in Figure 4.21. This constraint can be expressed in terms of inner products using the vectors v1 and v2. The statement regarding the directions of the normals can equivalently be formulated as the statement that the angle between n and v1, and between n and v2, must each be less than π/2. Using inner products,

O

A

this implies that n · v1 ≥ 0 and n · v2 ≥ 0. As in the translation case, the condition

n · v = 0 is required for contact. Observe that n now depends on θ. For any q ∈ C,

if n(θ) v1 0, n(θ) v2 0, and n(θ) v(q) > 0, then q free. Let Hf denote the set of configurations that satisfy these conditions. These conditions imply that a

· ≥ · ≥ · ∈ C

point is in Cfree. Furthermore, any other Type EV and Type VE contacts could imply that more points are in Cfree. Ordinarily, Hf ⊂ Cfree, which implies that the

complement, Hf , is a superset of obs (thus, obs Hf ). Let HA = Hf . Using the primitives

C \ C C ⊂ C \ C \

and

let HA = H1 ∪ H2 ∪ H3.

H1 = {q ∈ C | n(θ) · v1 ≤ 0}, (4.39)

H2 = {q ∈ C | n(θ) · v2 ≤ 0}, (4.40)

H3 = {q ∈ C | n(θ) · v(q) ≤ 0}, (4.41)

It is known that obs HA, but HA may contain points in free. The situ- ation is similar to what was explained in Section 3.1.1 for building a model of a convex polygon from half-planes. In the current setting, it is only known that any configuration outside of HA must be in free. If HA is intersected with all other cor- responding sets for each possible Type EV and Type VE contact, then the result is

C

C ⊆ C

Cobs. Each contact has the opportunity to remove a portion of Cfree from considera- tion. Eventually, enough pieces of Cfree are removed so that the only configurations

remaining must lie in obs. For any Type EV contact, (H1 H2) H3 free. A similar statement can be made for Type VE contacts. A logical predicate, similar

C ∪ \ ⊆ C

to that defined in Section 3.1.1, can be constructed to determine whether q ∈ Cobs

in time that is linear in the number of obs primitives.

C

One important issue remains. The expression n(θ) is not a polynomial because of the cos θ and sin θ terms in the rotation matrix of SO(2). If polynomials could be substituted for these expressions, then everything would be fixed because the expression of the normal vector (not a unit normal) and the inner product are both linear functions, thereby transforming polynomials into polynomials. Such a substitution can be made using stereographic projection (see [588]); however, a simpler approach is to use complex numbers to represent rotation. Recall that

when a + bi is used to represent rotation, each rotation matrix in SO(2) is repre- sented as (4.18), and the 3 × 3 homogeneous transformation matrix becomes

a −b xt

T (a, b, x , y ) =

b a y

. (4.42)



t t 

t

0 0 1

Using this matrix to transform a point [x y 1] results in the point coordinates (ax − by + xt, bx + ay + yt). Thus, any transformed point on A is a linear function

of a, b, xt, and yt.

This was a simple trick to make a nice, linear function, but what was the cost? The dependency is now on a and b instead of θ. This appears to increase the

dimension of from 3 to 4, and = R4. However, an algebraic primitive must be added that constrains a and b to lie on the unit circle.

C C

By using complex numbers, primitives in R4 are obtained for each Type EV and Type VE contact. By defining = R4, the following algebraic primitives are obtained for a Type EV contact:

C

H1 = {(xt, yt, a, b) ∈ C | n(xt, yt, a, b) · v1 ≤ 0}, (4.43)

H2 = {(xt, yt, a, b) ∈ C | n(xt, yt, a, b) · v2 ≤ 0}, (4.44)

and

H3 = {(xt, yt, a, b) ∈ C | n(xt, yt, a, b) · v(xt, yt, a, b) ≤ 0}. (4.45)

This yields HA = H1 H2 H3. To preserve the correct R2 S1 topology of , the set

∪ ∪ × C

Hs = {(xt, yt, a, b) ∈ C | a + b − 1 = 0} (4.46)

2 2

is intersected with HA. The set Hs remains fixed over all Type EV and Type VE contacts; therefore, it only needs to be considered once.

Example 4.16 (A Nonlinear Boundary for obs) Consider adding rotation to the model described in Example 4.15. In this case, all possible contacts between pairs of edges must be considered. For this example, there are 12 Type EV con- tacts and 12 Type VE contacts. Each contact produces 3 algebraic primitives. With the inclusion of Hs, this simple example produces 73 primitives! Rather than construct all of these, we derive the primitives for a single contact. Consider the Type VE contact between a3 and b4-b1. The outward edge normal n remains fixed at n = [1, 0]. The vectors v1 and v2 are derived from the edges adjacent to a3, which are a3-a2 and a3-a1. Note that each of a1, a2, and a3 depend on the con- figuration. Using the 2D homogeneous transformation (3.35), a1 at configuration

C

(xt, yt, θ) is (cos θ + xt, sin θ + yt). Using a+ bi to represent rotation, the expression of a1 becomes (a+ xt, b + yt). The expressions of a2 and a3 are (−b + xt, a+ yt) and

(−a+ b + xt, −b −a+ yt), respectively. It follows that v1 = a2 −a3 = [a− 2b, 2a+ b] and v2 = a1 − a3 = [2a − b, a + 2b]. Note that v1 and v2 depend only on the orientation of A, as expected. Assume that v is drawn from b4 to a3. This yields v = a3 − b4 = [−a + b + xt − 1, −a − b + yt + 1]. The inner products v1 · n, v2 · n,

and v n can easily be computed to form H1, H2, and H3 as algebraic primitives.

·

One interesting observation can be made here. The only nonlinear primitive is

C

a2 + b2 = 1. Therefore, obs can be considered as a linear polytope (like a polyhe- dron, but one dimension higher) in R4 that is intersected with a cylinder. ．

3D rigid bodies For the case of a 3D rigid body to which any transformation in SE(3) may be applied, the same general principles apply. The quaternion parameterization once again becomes the right way to represent SO(3) because using (4.20) avoids all trigonometric functions in the same way that (4.18) avoided them for SO(2). Unfortunately, (4.20) is not linear in the configuration variables,

as it was for (4.18), but it is at least polynomial. This enables semi-algebraic models to be formed for obs. Type FV, VF, and EE contacts arise for the SE(3) case. From all of the contact conditions, polynomials that correspond to each

C

patch of Cobs can be made. These patches are polynomials in seven variables: xt,

yt, zt, a, b, c, and d. Once again, a special primitive must be intersected with all others; here, it enforces the constraint that unit quaternions are used. This reduces the dimension from 7 back down to 6. Also, constraints should be added to throw away half of S3, which is redundant because of the identification of antipodal

points on S3.

Chains and trees of bodies For chains and trees of bodies, the ideas are con- ceptually the same, but the algebra becomes more cumbersome. Recall that the transformation for each link is obtained by a product of homogeneous transforma- tion matrices, as given in (3.53) and (3.57) for the 2D and 3D cases, respectively. If the rotation part is parameterized using complex numbers for SO(2) or quater- nions for SO(3), then each matrix consists of polynomial entries. After the matrix product is formed, polynomial expressions in terms of the configuration variables are obtained. Therefore, a semi-algebraic model can be constructed. For each link, all of the contact types need to be considered. Extrapolating from Exam- ples 4.15 and 4.16, you can imagine that no human would ever want to do all of that by hand, but it can at least be automated. The ability to construct this representation automatically is also very important for the existence of theoretical algorithms that solve the motion planning problem combinatorially; see Section 6.4.

If the kinematic chains were formulated for = R3 using the DH parameter- ization, it may be inconvenient to convert to the quaternion representation. One way to avoid this is to use complex numbers to represent each of the θi and αi vari- ables that appear as configuration variables. This can be accomplished because only cos and sin functions appear in the transformation matrices. They can be replaced by the real and imaginary parts, respectively, of a complex number. The

W

dimension will be increased, but this will be appropriately reduced after imposing the constraints that all complex numbers must have unit magnitude.