Page 307 - Sensing, Intelligence, Motion : How Robots and Humans Move in an Unstructured World

282 MOTION PLANNING FOR THREE-DIMENSIONAL ARM MANIPULATORS

l 3
d g

O 1
O 3
e f

o l
c 2
O 2

a b
l 1
Figure 6.5 C-space and C-obstacles that correspond to W-space in Figures 6.2 and
6.4. Thicker dotted and solid lines show intersections between obstacles. Shown also are
projections of the three obstacles on the plane l 1 ,l 2 .

Local Directions. Similar to other algorithms in previous chapters, a common
operation in the algorithm here will be the choice of a local direction for the
next turn (say, left or right). This will be needed when, while moving along a
curve, the C-point encounters a sort of T-intersection with another curve (which
is here the horizontal part of “T”). Let us define the vector of current motion p
and consider all possible cases.

1. The C-point moves along the M-line or along an intersection curve between
the M-plane and an obstacle and is about to leave M-plane at the cross-
point. Define the normal vector m of the M-plane [97]. Then the local
direction b is upward if b · m > 0 and downward if b · m ≤ 0.
2. The C-point moves along the M-line or along an intersection curve between
the V-plane and an obstacle, and it is about to leave V-plane at the cross-
point. Let l 3 be the vector of l 3 axis. Then, local direction b is left if

b · (p × l 3 )> 0and right if b · (p × l 3 ) ≤ 0.