4.3.3 Explicitly Modeling ${\cal C}_{obs}$ : The General Case

Unfortunately, the cases in which ${\cal C}_{obs}$ is polygonal or polyhedral are quite limited. Most problems yield extremely complicated C-space obstacles. One good point is that ${\cal C}_{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].

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 ${\cal A}$ ; thus, ${\cal C}= {\mathbb{R}}^2 \times {\mathbb{S}}^1$ and $q = (x_t,y_t,\theta)$ . The task is to define a set of algebraic primitives that can be combined to define ${\cal C}_{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.

Figure 4.21: An illustration to help in constructing ${\cal C}_{obs}$ when rotation is allowed.

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 $\theta$ . This implies that the applicability of a Type EV contact depends on $\theta$ , the robot orientation. Recall the constraint that the inward normal of ${\cal A}$ must point between the outward normals of the edges of ${\cal O}$ 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 $v_1$ and $v_2$ . The statement regarding the directions of the normals can equivalently be formulated as the statement that the angle between $n$ and $v_1$ , and between $n$ and $v_2$ , must each be less than $\pi/2$ . Using inner products, this implies that $n \cdot v_1 \geq 0$ and $n \cdot v_2 \geq 0$ . As in the translation case, the condition $n \cdot v = 0$ is required for contact. Observe that $n$ now depends on $\theta$ . For any $q \in {\cal C}$ , if $n(\theta) \cdot v_1 \geq 0$ , $n(\theta) \cdot v_2 \geq 0$ , and $n(\theta) \cdot v(q) > 0$ , then $q \in {\cal C}_{free}$ . Let $H_f$ denote the set of configurations that satisfy these conditions. These conditions imply that a point is in ${\cal C}_{free}$ . Furthermore, any other Type EV and Type VE contacts could imply that more points are in ${\cal C}_{free}$ . Ordinarily, $H_f \subset {\cal C}_{free}$ , which implies that the complement, ${\cal C}\setminus H_f$ , is a superset of ${\cal C}_{obs}$ (thus, ${\cal C}_{obs} \subset {\cal C}\setminus H_f$ ). Let $H_A = {\cal C}\setminus H_f$ . Using the primitives

$$H_1 = \{ q \in {\cal C}\; \vert \; n(\theta) \cdot v_1 \leq 0 \},$$ (4.39)

$$H_2 = \{ q \in {\cal C}\; \vert \; n(\theta) \cdot v_2 \leq 0 \},$$ (4.40)

and

$$H_3 = \{ q \in {\cal C}\; \vert \; n(\theta) \cdot v(q) \leq 0 \},$$ (4.41)

let $H_A = H_1 \cup H_2 \cup H_3$ .

It is known that ${\cal C}_{obs}\subseteq H_A$ , but $H_A$ may contain points in ${\cal C}_{free}$ . The situation 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 $H_A$ must be in ${\cal C}_{free}$ . If $H_A$ is intersected with all other corresponding sets for each possible Type EV and Type VE contact, then the result is ${\cal C}_{obs}$ . Each contact has the opportunity to remove a portion of ${\cal C}_{free}$ from consideration. Eventually, enough pieces of ${\cal C}_{free}$ are removed so that the only configurations remaining must lie in ${\cal C}_{obs}$ . For any Type EV contact, $(H_1 \cup H_2) \setminus H_3 \subseteq {\cal C}_{free}$ . A similar statement can be made for Type VE contacts. A logical predicate, similar to that defined in Section 3.1.1, can be constructed to determine whether $q \in {\cal C}_{obs}$ in time that is linear in the number of ${\cal C}_{obs}$ primitives.

One important issue remains. The expression $n(\theta)$ is not a polynomial because of the $\cos \theta$ and $\sin \theta$ 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 represented as (4.18), and the $3 \times 3$ homogeneous transformation matrix becomes

$$T(a,b,x_t,y_t) = \begin{pmatrix}a & -b & x_t b & a & y_t 0 & 0 & 1 \end{pmatrix} .$$ (4.42)

Using this matrix to transform a point $[x \; y \; 1]$ results in the point coordinates $(ax -by + x_t, bx + ay + y_t)$ . Thus, any transformed point on ${\cal A}$ is a linear function of $a$ , $b$ , $x_t$ , and $y_t$ .

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 $\theta$ . This appears to increase the dimension of ${\cal C}$ from 3 to 4, and ${\cal C}= {\mathbb{R}}^4$ . However, an algebraic primitive must be added that constrains $a$ and $b$ to lie on the unit circle.

By using complex numbers, primitives in ${\mathbb{R}}^4$ are obtained for each Type EV and Type VE contact. By defining ${\cal C}= {\mathbb{R}}^4$ , the following algebraic primitives are obtained for a Type EV contact:

$$H_1 = \{ (x_t,y_t,a,b) \in {\cal C}\; \vert \; n(x_t,y_t,a,b) \cdot v_1 \leq 0 \},$$ (4.43)

$$H_2 = \{ (x_t,y_t,a,b) \in {\cal C}\; \vert \; n(x_t,y_t,a,b) \cdot v_2 \leq 0 \},$$ (4.44)

and

$$H_3 = \{ (x_t,y_t,a,b) \in {\cal C}\; \vert \; n(x_t,y_t,a,b) \cdot v(x_t,y_t,a,b) \leq 0 \}.$$ (4.45)

This yields $H_A = H_1 \cup H_2 \cup H_3$ . To preserve the correct ${\mathbb{R}}^2 \times {\mathbb{S}}^1$ topology of ${\cal C}$ , the set

$$H_s = \{ (x_t,y_t,a,b) \in {\cal C}\; \vert \; a^2 + b^2 - 1 = 0 \}$$ (4.46)

is intersected with $H_A$ . The set $H_s$ 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 ${\cal C}_{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 contacts and $12$ Type VE contacts. Each contact produces $3$ algebraic primitives. With the inclusion of $H_s$ , 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 $a_3$ and $b_4$ -$b_1$ . The outward edge normal $n$ remains fixed at $n = [1,0]$ . The vectors $v_1$ and $v_2$ are derived from the edges adjacent to $a_3$ , which are $a_3$ -$a_2$ and $a_3$ -$a_1$ . Note that each of $a_1$ , $a_2$ , and $a_3$ depend on the configuration. Using the 2D homogeneous transformation (3.35), $a_1$ at configuration $(x_t,y_t,\theta)$ is $(\cos \theta + x_t, \sin \theta + y_t)$ . Using $a + bi$ to represent rotation, the expression of $a_1$ becomes $(a + x_t, b + y_t)$ . The expressions of $a_2$ and $a_3$ are $(-b + x_t,a + y_t)$ and $(-a + b + x_t,-b - a + y_t)$ , respectively. It follows that $v_1 = a_2 - a_3 = [a - 2b, 2a + b]$ and $v_2 = a_1 - a_3 = [2a - b, a + 2b]$ . Note that $v_1$ and $v_2$ depend only on the orientation of ${\cal A}$ , as expected. Assume that $v$ is drawn from $b_4$ to $a_3$ . This yields $v = a_3 - b_4 = [-a + b + x_t- 1, -a - b + y_t+ 1]$ . The inner products $v_1 \cdot n$ , $v_2 \cdot n$ , and $v \cdot n$ can easily be computed to form $H_1$ , $H_2$ , and $H_3$ as algebraic primitives.

One interesting observation can be made here. The only nonlinear primitive is $a^2 + b^2 = 1$ . Therefore, ${\cal C}_{obs}$ can be considered as a linear polytope (like a polyhedron, but one dimension higher) in ${\mathbb{R}}^4$ that is intersected with a cylinder. $\blacksquare$