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

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

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


$\displaystyle 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

$\displaystyle 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:

$\displaystyle 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)

$\displaystyle 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)


$\displaystyle 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

$\displaystyle 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$

Steven M LaValle 2012-04-20