7.4 Planning for Closed Kinematic Chains
This section continues where Section 4.4 left off. The subspace of that results from maintaining kinematic closure was defined and illustrated through some ex-
C
amples. Planning in this context requires that paths remain on a lower dimensional variety for which a parameterization is not available. Many important applications require motion planning while maintaining these constraints. For example, con- sider a manipulation problem that involves multiple manipulators grasping the same object, which forms a closed loop as shown in Figure 7.19. A loop exists be- cause both manipulators are attached to the ground, which may itself be considered as a link. The development of virtual actors for movies and video games also in- volves related manipulation problems. Loops also arise in this context when more than one human limb is touching a fixed surface (e.g., two feet on the ground). A class of robots called parallel manipulators are intentionally designed with internal closed loops [693]. For example, consider the Stewart-Gough platform [407, 914] illustrated in Figure 7.20. The lengths of each of the six arms, 1, . . ., 6, can be independently varied while they remain attached via spherical joints to the ground and to the platform, which is 7. Each arm can actually be imagined as two links that are connected by a prismatic joint. Due to the total of 6 degrees of freedom introduced by the variable lengths, the platform actually achieves the full 6 degrees of freedom (hence, some six-dimensional region in SE(3) is obtained for 7). Planning the motion of the Stewart-Gough platform, or robots that are based on the platform (the robot shown in Figure 7.27 uses a stack of several of these mechanisms), requires handling many closure constraints that must be main- tained simultaneously. Another application is computational biology, in which the C-space of molecules is searched, many of which are derived from molecules that have closed, flexible chains of bonds [245].
A
A
A A
7.4.1 Adaptation of Motion Planning Algorithms
All of the components from the general motion planning problem of Formulation
4.1 are included: , , 1, . . ., m, , qI , and qG. It is assumed that the robot is a collection of r links that are possibly attached in loops.
W O A A C
It is assumed in this section that = Rn. If this is not satisfactory, there are two ways to overcome the assumption. The first is to represent SO(2) and SO(3) as S1 and S3, respectively, and include the circle or sphere equation as part of the constraints considered here. This avoids the topology problems. The other option is to abandon the restriction of using Rn and instead use a parameterization of that is of the appropriate dimension. To perform calculus on such manifolds, a
C
C
smooth structure is required, which is introduced in Section 8.3.2. In the presenta- tion here, however, vector calculus on Rn is sufficient, which intentionally avoids these extra technicalities.
Closure constraints The closure constraints introduced in Section 4.4 can be summarized as follows. There is a set, , of polynomials f1, . . ., fk that belong
P
to Q[q1, . . . , qn] and express the constraints for particular points on the links of
the robot. The determination of these is detailed in Section 4.4.3. As mentioned
previously, polynomials that force points to lie on a circle or sphere in the case of
Figure 7.19: Two or more manipulators manipulating the same object causes closed kinematic chains. Each black disc corresponds to a revolute joint.
rotations may also be included in P.
Let n denote the dimension of C. The closure space is defined as
Cclo = {q ∈ C | ∀fi ∈ P, fi(q1, . . . , qn) = 0}, (7.21)
which is an m-dimensional subspace of that corresponds to all configurations that satisfy the closure constants. The obstacle set must also be taken into account.
C
Once again, Cobs and Cfree are defined using (4.34). The feasible space is defined
as fea = clo free, which are the configurations that satisfy closure constraints and avoid collisions.
C C ∩ C
The motion planning problem is to find a path τ : [0, 1] → Cfea such that τ (0) =
qI and τ (1) = qG. The new challenge is that there is no explicit parameterization
of Cfea, which is further complicated by the fact that m < n (recall that m is the dimension of Cclo).
Combinatorial methods Since the constraints are expressed with polynomials, it may not be surprising that the computational algebraic geometry methods of Section 6.4 can solve the general motion planning problem with closed kinematic chains. Either cylindrical algebraic decomposition or Canny’s roadmap algorithm may be applied. As mentioned in Section 6.5.3, an adaptation of the roadmap algorithm that is optimized for problems in which m < n is given in [76].
Figure 7.20: An illustration of the Stewart-Gough platform (adapted from a figure made by Frank Sottile).
Sampling-based methods Most of the methods of Chapter 5 are not easy to adapt because they require sampling in fea, for which a parameterization does not
C
exist. If points in a bounded region of Rn are chosen at random, the probability
is zero that a point on fea will be obtained. Some incremental sampling and
C
searching methods can, however, be adapted by the construction of a local planning method (LPM) that is suited for problems with closure constraints. The sampling- based roadmap methods require many samples to be generated directly on fea. Section 7.4.2 presents some techniques that can be used to generate such samples for certain classes of problems, enabling the development of efficient sampling- based planners and also improving the efficiency of incremental search planners. Before covering these techniques, we first present a method that leads to a more general sampling-based planner and is easier to implement. However, if designed well, planners based on the techniques of Section 7.4.2 are more efficient.
C
Now consider adapting the RDT of Section 5.5 to work for problems with closure constraints. Similar adaptations may be possible for other incremental sampling and searching methods covered in Section 5.4, such as the randomized potential field planner. A dense sampling sequence, α, is generated over a bounded
n-dimensional subset of Rn, such as a rectangle or sphere, as shown in Figure 7.21.
Figure 7.21: For the RDT, the samples can be drawn from a region in Rn, the space in which Cclo is embedded.
The samples are not actually required to lie on clo because they do not necessarily become part of the topological graph, . They mainly serve to pull the search tree in different directions. One concern in choosing the bounding region is that it must include clo (at least the connected component that includes qI ) but it must not be unnecessarily large. Such bounds are obtained by analyzing the motion limits for a particular linkage.
G
C
C
Stepping along 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 clo. Figure
C
C
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 , to the sample α(i) is first com- puted. Let v = α(i) q, which indicates the direction in which an edge would
−
∈ C
be made from q if there were no constraints. A local motion is then computed by projecting v into the tangent plane3 of Cclo at the point q. Since Cclo is generally
nonlinear, the local motion produces a point that is not precisely on 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 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 qs, the stopping configuration. The new path followed in 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
C
C
C
3Tangent planes are defined rigorously in Section 8.3.
Figure 7.22: For each sample α(i) the nearest point, qn , is found, and then the local planner generates a motion that lies in the local tangent plane. The motion is the project of the vector from qn to α(i) onto the tangent plane.
∈ C
vertices along the path.
In each iteration, the tangent plane computation is computed at some q clo as follows. The differential configuration vector dq lies in the tangent space of a constraint fi(q) = 0 if
∈ C
∂fi(q) dq
1
∂q1
∂fi(q)
- dq2
∂q2
∂fi(q)
- · · · + ∂q dqn
n
= 0. (7.22)
This leads to the following homogeneous system for all of the k polynomials in that define the closure constraints
P
∂f1(q) ∂f1(q) ∂f1(q)
· · ·
∂q ∂q ∂q
1 2 n dq
1
..
∂f2(q)
∂q1
..
.
∂f2(q)
∂q2
..
· · ·
∂f2(q)
∂qn
..
dq2
. = 0. (7.23)
∂f (q)
.
k
∂q1
.
∂f (q)
k
∂q2
∂f (q)
dqn
If the rank of the matrix is m 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 decom- position (SVD) [399, 961], to compute an orthonormal basis for the tangent space at q. Let e1, . . ., em, denote these n-dimensional basis vectors. The components
· · ·
k
∂qn
−
≤
of the motion direction are obtained from v = α(i) −qn. First, construct the inner
products, a1 = v e1, a2 = v e2, . . ., am = v em. Using these, the projection of v
· · ·
in the tangent plane is the n-dimensional vector w given by
m
-
w = ai_e_i, (7.24)
i
which is used as the direction of motion. The magnitude must be appropriately scaled to take sufficiently small steps. Since Cclo is generally curved, a linear motion leaves Cclo. A motion in the inward normal direction is then required to move back onto Cclo.
Since the dimension m of Cclo is less than n, the procedure just described can
only produce numerical approximations to paths in clo. This problem also arises in implicit curve tracing in graphics and geometric modeling [454]. Therefore, each
C
constraint fi(q1, . . . , qn) = 0 is actually slightly weakened to |fi(q1, . . . , qn)| < ǫ for
some fixed tolerance ǫ > 0. This essentially “thickens” 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].
C
7.4.2 Active-Passive Link Decompositions
An alternative sampling-based approach is to perform an active-passive decom- position, which is used to generate samples in clo by directly sampling active variables, and computing the closure values for passive variables using inverse kinematics methods. This method was introduced in [432] and subsequently im- proved through the development of the random loop generator in [244, 246]. The method serves as a general framework that can adapt virtually any of the meth- ods of Chapter 5 to handle closed kinematic chains, and experimental evidence suggests that the performance is better than the method of Section 7.4.1. One drawback is that the method requires some careful analysis of the linkage to de- termine the best decomposition and also bounds on its mobility. Such analysis exists for very general classes of linkages [244].
C
Active and passive variables In this section, let denote the C-space ob- tained from all joint variables, instead of requiring = Rn, as in Section 7.4.1. This means that includes only polynomials that encode closure constraints, as
P
C
C
opposed to allowing constraints that represent rotations. Using the tree repre- sentation from Section 4.4.3, this means that is of dimension n, arising from assigning one variable for each revolute joint of the linkage in the absence of any constraints. Let q denote this vector of configuration variables. The active- passive decomposition partitions the variables of q to form two vectors, qa, called the active variables and qp, called the passive variables. The values of passive variables are always determined from the active variables by enforcing the closure
C
∈ C
constraints and using inverse kinematics techniques. If m is the dimension of Cclo, then there are always m active variables and n − m passive variables.
Figure 7.23: A chain of links in the plane. There are seven links and seven joints, which are constrained to form a loop. The dimension of C is seven, but the dimension of Cclo is four.
Temporarily, suppose that the linkage forms a single loop as shown in Figure
7.23. One possible decomposition into active qa and passive qp variables is given in Figure 7.24. If constrained to form a loop, the linkage has four degrees of freedom, assuming the bottom link is rigidly attached to the ground. This means that values can be chosen for four active joint angles qa and the remaining three qp can be derived from solving the inverse kinematics. To determine qp, there are three equations and three unknowns. Unfortunately, these equations are nonlinear and fairly complicated. Nevertheless, efficient solutions exist for this case, and the 3D generalization [675]. For a 3D loop formed of revolute joints, there are six passive
variables. The number, 3, of passive links in R2 and the number 6 for R3 arise from the dimensions of SE(2) and SE(3), respectively. This is the freedom that
is stripped away from the system by enforcing the closure constraints. Methods for efficiently computing inverse kinematics in two and three dimensions are given in [30]. These can also be applied to the RDT stepping method in Section 7.4.1, instead of using continuation.
If the maximal number of passive variables is used, there is at most a finite number of solutions to the inverse kinematics problem; this implies that there are often several choices for the passive variable values. It could be the case that for some assignments of active variables, there are no solutions to the inverse kinematics. An example is depicted in Figure 7.25. Suppose that we want to
generate samples in clo by selecting random values for qa and then using inverse kinematics for qp. What is the probability that a solution to the inverse kinematics exists? For the example shown, it appears that solutions would not exist in most
C
Figure 7.24: Three of the joint variables can be determined automatically by inverse kinematics. Therefore, four of the joints be designated as active, and the remaining three will be passive.
trials.
Loop generator The random loop generator greatly improves the chance of obtaining closure by iteratively restricting the range on each of the active variables. The method requires that the active variables appear sequentially along the chain (i.e., there is no interleaving of active and passive variables). The m coordinates of qa are obtained sequentially as follows. First, compute an interval, I1, of allowable values for qa. The interval serves as a loose bound in the sense that, for any value qa I1, it is known for certain that closure cannot be obtained. This is ensured by performing a careful geometric analysis of the linkage, which will be explained shortly. The next step is to generate a sample in qa I1, which is accomplished in [244] by picking a random point in I1. Using the value qa, a bounding interval
1
1
1
∈
1
/∈
I2 is computed for allowable values of qa. The value qa is obtained by sampling
2 2
in I2. This process continues iteratively until Im and qa
m
are obtained, unless it
terminates early because some Ii = for i < m. If successful termination occurs, then the active variables qa are used to find values qp for the passive variables. This step still might fail, but the probability of success is now much higher. The
∅
method can also be applied to linkages in which there are multiple, common loops, as in the Stewart-Gough platform, by breaking the linkage into a tree and closing loops one at a time using the loop generator. The performance depends on how the linkage is decomposed [244].
Computing bounds on joint angles The main requirement for successful application of the method is the ability to compute bounds on how far a chain of
links can travel in W over some range of joint variables. For example, for a planar
Figure 7.25: In this case, the active variables are chosen in a way that makes it impossible to assign passive variables that close the loop.
chain that has revolute joints with no limits, the chain can sweep out a circle as shown in Figure 7.26a. Suppose it is known that the angle between links must remain between π/6 and π/6. A tighter bounding region can be obtained, as shown in Figure 7.26b. Three-dimensional versions of these bounds, along with many necessary details, are included in [244]. These bounds are then used to compute Ii in each iteration of the sampling algorithm.
−
Now that there is an efficient method that generates samples directly in clo, it is straightforward to adapt any of the sampling-based planning methods of Chapter 5. In [244] many impressive results are obtained for challenging problems that have the dimension of up to 97 and the dimension of clo up to 25; see Figure 7.27. These methods are based on applying the new sampling techniques to the RDTs of Section 5.5 and the visibility sampling-based roadmap of Section
C
C C
- For these algorithms, the local planning method is applied to the active variables, and inverse kinematics algorithms are used for the passive variables in the path validation step. This means that inverse kinematics and collision checking are performed together, instead of only collision checking, as described in Section 5.3.4.
One important issue that has been neglected in this section is the existence of kinematic singularities, which cause the dimension of clo to drop in the vicinity of certain points. The methods presented here have assumed that solving the motion planning problem does not require passing through a singularity. This assump- tion is reasonable for robot systems that have many extra degrees of freedom, but it is important to understand that completeness is lost in general because the sampling-based methods do not explicitly handle these degeneracies. In a
C
- (b)
Figure 7.26: (a) If any joint angle is possible, then the links sweep out a circle in the limit. (b) If there are limits on the joint angles, then a tighter bound can be obtained for the reachability of the linkage.
sense, they occur below the level of sampling resolution. For more information on kinematic singularities and related issues, see [693].