In each iteration, the tangent plane computation is computed at some as follows. The differential configuration vector lies in the tangent space of a constraint if

(7.22) |

This leads to the following homogeneous system for all of the polynomials in that define the closure constraints

If the rank of the matrix is , then configuration displacements can be chosen independently, and the remaining 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 . Let , , , denote these -dimensional basis vectors. The components of the motion direction are obtained from . First, construct the inner products, , , , . Using these, the projection of in the tangent plane is the -dimensional vector given by

(7.24) |

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

Since the dimension of is less than , the procedure just described can only produce numerical approximations to paths in . This problem also arises in implicit curve tracing in graphics and geometric modeling [454]. Therefore, each constraint is actually slightly weakened to for some fixed tolerance . This essentially ``thickens'' so that its dimension is . 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].

Steven M LaValle 2020-08-14