Motion planning of closed-chain manipulators is quite challenging because their C-spaces might be complex submanifolds due to nonlinear loop-closure constraints. Traditional approaches (exact or sampling-based) are either very difficult to implement, or fail to sample critical regions in C-free for properly handling the challengs of C-space bifurcations and narrow passages.
Although C-space of close chains can be embeded into a high-dimensional euclidean space, samples in the embient space has to project (through an optimization problem) onto the C-space submanifold, which is not straightforward, and sometimes has poor speed of convergence. Covering C-space using local coordinate chart with minimal number of parameters (equal to the dimesion of the manifold itself) always requires multiple charts. Bifurcation of C-space parameterization is unavoidable. Moreover, narrow passages among C-obstacles could form which makes the problem even hard to solve.
In earlier 2000s, Trinkle and Milgram [1][2] initiated a program that investigates the topological structure of C-space of closed chains, and use the obtained structural information for developing power motion planning algorithm for closed chains without obstacles. In the following paper [3] we develop a framework to employ the structure information for developing explicitly exact cell decomposition and roadmap algorithm, as well as powerful sampling-based roadmap algorithm. The main result of [3] is a new random loop generator which not only samples C-free, but also its boundary variety; and another sampling algorithm for sampling regions near each C-obstacle. The animations of various examples are given as follows (intermediate configurations on the boundary variety B(1) are shown as megenta color).
1. A 5-bar closed chain with link lenghth vector [1,1.3,4,4,5], two point obstacles [2,2], [5.5,1], and the start and goal configurations on the elbow-up and elbow-down tori, respectively. See this figure.
The C-space of this closed chain can be embedded into a minimal coordinate atlas with just 2 coordinate charts. Both charts are parametrized by (phi_3,phi_4). Given phi_3 and phi_4, (phi_1,phi_2) has 0, 1, or 2 solutions based upon the inverse kinematics (IK) of the 2-bar chain (l_1,l_2) in order for closing the loop. In fact, those regions on both torii which can not be reached by the (l_1,l_2) chain got 0 IK solutions, and the remaining reachable region mostly got 2 IK solutions, which converge to 1 solution at the boundary of the reachable set. This boundary is referred to as the boundary varieties (C-boundary) in the paper[3] where the C-space bifurcates. Recall that a 2-D torus can be drawn in the [-2pi,2pi] rectangle. So the C-space patches, their boundaries, and the C-obstacles can be illustrated in 2 copies of the [-2pi, 2pi] rectangles, see Fig. 1.1 (non-canonical, parametrized by phi_3, phi_4), 1.2, 1.3 (canonical parametrization by phi_1, phi_2). Note that a classical result in [1][2] states that for closed chains with 3 long links (Example 1 falls in this category with 3 long links), its C-space has 2 components, both of which are products of circles corresponding the joint angles of short links. Therefore, joint angles of short links are referred to as canonical parameters. Of course, we still can use the joint angles of long links, or the combination of both. There are called non-canonical parameters.
Points on the boundary varieties play an important role in bridging the samples in different tori, and planning a smooth motion between them. Consider the two configurations, (phi_{3,1}, phi_{4,1}) on the elbow-up torus, and (phi_{3,2}, phi_{4,2}) on the elbow-down torus. They are very close in the sense that (phi_{3,2}-phi_{3,1})^2 + (phi_{4,2}-phi_{4,1})^2 is very small. Theoretically, the robot can jump directly from the first configuration to the sencond, or vice versa. But considering the motion control problem of the (l_1,l_2) chain using IK, this direct jump can cause instantaneous infinite speed of phi_2. This should certainly be avoided in practice.
If no samples get sampled in the boundary variety, any motion joining two samples in different tori is usually unacceptable unless both of them are very close to the boundary variety. Uniform random sampling on both tori in general takes long time to find samples very close to the boundary. Therefore direct sampling of the C-boundary becomes essentail for these problems. See this video. The closed chain, point obstacles in workspace, and the start and goal configurations are drawn in this figure. Fig. 1.4, Fig. 1.5 show the C-space patches on the elbow-up and elbow-down tori.
2. A 5-bar closed chain with link lenghth vector [1,1.3,4,4,5], two tightly arranged point obstacles [1,1], [1,1.4], and start and goal configurations fall into a narrow band of C-free. Fig. 2.1 shows the chain along with point obstacles.
Because the two point obstacles are very close in the workspace, there will be narrow regions in C-free, as shown in Fig. 2.2, and 2.3 (non-canonical parameterization), and 2.4 and 2.5 (canonical parameterization). If no points near C-obst got sampled, it would be hard to generate a path connecting the start and goal configation in the narrow band. Purely random sampling takes long time (>40s) and more sample (>2000), and still fails to give the right answer. Our algorithm only requires several hundred samples and <5s computation time.
Our algorithm samples the C-space of a new closed chain, whose link j (j=1,..,m-1, m=DoF) intersects a dilated point obstacle (original point osbtacle, or convex obstacle, or even non-convex obstacle can be approximated by a set of point obstacles on the boundary of the dilated set of the original obstacle). This C-space turns out to be a fibration, with the base manifold the C-space of a closed chain (l_1,...,l_j, l_b1) with l_j touchs a point obstacle, and the fiber the C-space of another closed chain (l_{j+1},... l_{m-1}, l_b2). Finally, we obtain many samples which cover the narrow region in Fig. 2.2, 2.3, 2.4,2.5, and quickly find a path between the start and goal. The motion of this example is shown in three videos with different number of near c-obst samples 2.6 (722 samples), 2.7(728 samples), and 2.8(1228 samples). The same algorithm applies to the narrow passage problem of a 12-bar closed chain. The result is shown in the videos 2.9, 3.0. For this problem, the traditional PRM algorithm fails to give a solution even after more than 40,000 samples, and more than 5-hour of computation, but our algorithm just needs around 6000 samples, and reports the correct path.
3. The following examples are for general high-dimensional closed chains without narrow passages. Both our algorithm and the traditional algorithm can yield correct answer, with no significant difference in computation time. But in general our algorithm yields better roadmap if we consider the predicted number of components of C-free, and the overall succesful rate with respect to many start-goal pairs. See the discussions in the expriement section of our paper [3].
(1) a 6-bar closed chain moving among 2 point obstacles (using exact cell decomposition algorithm)
Example 1
(2) a 6-bar closed chain moving among 2 point obstacls (using sampling-based roadmap algorithm )
Example 1
Example 2
Example 3
Example 4
Example 5
Example 6
(3) a 6-bar closed chain moving among 3 point obtacles (using sampling-based roadmap algorithm)
Example 1
Example 2
Example 3
Example 4
(4) a 6-bar closed chain moving among 4 point obtacles (using sampling-based roadmap algorithm)
Example 1
Example 2
Example 3
Example 4
(5) a 6-bar closed chain moving among 5 point obtacles (using sampling-based roadmap algorithm)
Example 1
Example 2
(6) a 6-bar closed chain moving among 6 point obtacles (using sampling-based roadmap algorithm)
Example 1
Example 2
Example 3
(7) a 6-bar closed chain moving among 7 point obtacles (using sampling-based roadmap algorithm)
Example 1
Example 2
Example 3
(8) a 8-bar closed chain moving among 6 point obtacles (using sampling-based roadmap algorithm)
Example 1
Example 2
Example 3
(9) a 8-bar closed chain moving among 7 point obtacles (using sampling-based roadmap algorithm)
Example 1
(10) a 10-bar closed chain moving among 6 point obstacles (using sampling-based roadmap algorithm,
video plays with slow speed, but can see the motion crosses the boundary variety B(1) in
megenta color very clearly. This shows the importance of B(1) in global connectivity of C-free)
Example 1
Example 2
Example 3
Example 4
Example 5
(11) a 12-bar closed chain moving among 6 point obstacles (using sampling-based roadmap algorithm,
video plays with slow speed, but can see the motion crosses the boundary variety B(1) in
megenta color very clearly. This shows the importance of B(1) in global connectivity of C-free)
Example 1
Example 2
Example 3
(12) a 14-bar closed chain moving among 6 point obstacles (using sampling-based roadmap algorithm)
Example 1
Example 2
Example 3
Example 4
Example 5
(13) a 16-bar closed chain moving among 6 point obstacles (using sampling-based roadmap algorithm)
Example 1
Example 2
Example 3
Example 4
Example 5
(14) a 18-bar closed chain moving among 6 point obstacles (using sampling-based roadmap algorithm)
Example 1
Example 2
Example 3
Example 4
Example 5
(15) a 18-bar closed chain moving among 10 point obstacles (using sampling-based roadmap algorithm)
Example 1
Example 2
Example 3
Example 4
Example 5
(16) a 19-bar closed chain moving among 10 point obstacles (using sampling-bsed roadmap algorithm)
Example 1
(17) a 20-bar closed chain moving among 12 point obstacles (using sampling-bsed roadmap algorithm)
Example 1
Example 2
Example 3
Example 4
Example 5
Example 6
Example 7
Example 8
Note that in all above examples we dilate each point obtacle into a convex polygon (so they alreadily apply to the problem with convex polygonal obstacles, or even general convex obstacles). In fact our method applies to any type of convex obstacles.
Our source code can be downloaded from github.
[1] J.C. Trinkle and R.J. Milgram, Complete Path Planning for Closed Kinematic Chains with Spherical Joints. International Jounral of Robotics Research, 21(9):773-789, December, 2002.
[2] R.J. Milgram and J.C. Trinkle, The Geometry of Configuration Spaces of Closed Chains in Two and Three Dimensions, Homology, Homotopy and Applications, 6(1): 237-267, 2004.
[3] G.F. Liu, J.C. Trinkle, et al, "Motion Planning of Planar Closed Chains Base on Structural Sets", Under Review, 2020.