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$