Stepping along ${{\cal C}_{clo}}$

The RDT algorithm given Figure 5.21 can be applied directly; however, the STOPPING-CONFIGURATION function in line 4 must be changed to account for both obstacles and the constraints that define ${{\cal C}_{clo}}$. Figure 7.22 shows one general approach that is based on numerical continuation [18]. An alternative is to use inverse kinematics, which is part of the approach described in Section 7.4.2. The nearest RDT vertex, $q \in {\cal C}$, to the sample ${\alpha }(i)$ is first computed. Let $v = {\alpha}(i) - q$, which indicates the direction in which an edge would be made from $q$ if there were no constraints. A local motion is then computed by projecting $v$ into the tangent plane^7.3 of ${{\cal C}_{clo}}$ at the point $q$. Since ${{\cal C}_{clo}}$ is generally nonlinear, the local motion produces a point that is not precisely on ${{\cal C}_{clo}}$. Some numerical tolerance is generally accepted, and a small enough step is taken to ensure that the tolerance is maintained. The process iterates by computing $v$ with respect to the new point and moving in the direction of $v$ projected into the new tangent plane. If the error threshold is surpassed, then motions must be executed in the normal direction to return to ${{\cal C}_{clo}}$. This process of executing tangent and normal motions terminates when progress can no longer be made, due either to the alignment of the tangent plane (nearly perpendicular to $v$) or to an obstacle. This finally yields $q_s$, the stopping configuration. The new path followed in ${{\cal C}_{fea}}$ is no longer a ``straight line'' as was possible for some problems in Section 5.5; therefore, the approximate methods in Section 5.5.2 should be used to create intermediate vertices along the path.

In each iteration, the tangent plane computation is computed at some $q \in {{\cal C}_{clo}}$ as follows. The differential configuration vector $dq$ lies in the tangent space of a constraint $f_i(q) = 0$ if

$$\frac{\partial f_i(q)}{\partial q_1} dq_1 + \frac{\partial f_i(q)}{\partial q_2} dq_2 + \cdots + \frac{\partial f_i(q)}{\partial q_n} dq_n = 0 .$$ (7.22)

This leads to the following homogeneous system for all of the $k$ polynomials in ${\cal P}$ that define the closure constraints

$$\begin{pmatrix}\displaystyle\strut \frac{\partial f_1(q)}{\partia... ...atrix} \begin{pmatrix}dq_1 dq_2 \vdots dq_n \end{pmatrix} = {\bf0}.$$ (7.23)

If the rank of the matrix is $m \leq n$, then $m$ configuration displacements can be chosen independently, and the remaining $n-m$ parameters must satisfy (7.23). This can be solved using linear algebra techniques, such as singular value decomposition (SVD) [399,961], to compute an orthonormal basis for the tangent space at $q$. Let $e_1$, $\ldots$, $e_m$, denote these $n$-dimensional basis vectors. The components of the motion direction are obtained from $v = {\alpha}(i) - q_n$. First, construct the inner products, $a_1 = v \cdot e_1$, $a_2 = v \cdot e_2$, $\ldots$, $a_m = v \cdot e_m$. Using these, the projection of $v$ in the tangent plane is the $n$-dimensional vector $w$ given by

$$w = \sum_i^m a_i e_i,$$ (7.24)

which is used as the direction of motion. The magnitude must be appropriately scaled to take sufficiently small steps. Since ${{\cal C}_{clo}}$ is generally curved, a linear motion leaves ${{\cal C}_{clo}}$. A motion in the inward normal direction is then required to move back onto ${{\cal C}_{clo}}$.

Since the dimension $m$ of ${{\cal C}_{clo}}$ is less than $n$, the procedure just described can only produce numerical approximations to paths in ${{\cal C}_{clo}}$. This problem also arises in implicit curve tracing in graphics and geometric modeling [454]. Therefore, each constraint $f_i(q_1,\ldots,q_n) = 0$ is actually slightly weakened to $\vert f_i(q_1,\ldots,q_n)\vert < \epsilon$ for some fixed tolerance $\epsilon > 0$. This essentially ``thickens'' ${{\cal C}_{clo}}$ so that its dimension is $n$. As an alternative to computing the tangent plane, motion directions can be sampled directly inside of this thickened region without computing tangent planes. This results in an easier implementation, but it is less efficient [979].