Learning the Metric of Task Constraint Manifolds for Constrained Motion Planning

Abstract

Finding feasible motion for robots with high-dimensional configuration space is a fundamental problem in robotics. Sampling-based motion planning algorithms have been shown to be effective for these high-dimensional systems. However, robots are often subject to task constraints (e.g., keeping a glass of water upright, opening doors and coordinating operation with dual manipulators), which introduce significant challenges to sampling-based motion planners. In this work, we introduce a method to establish approximate model for constraint manifolds, and to compute an approximate metric for constraint manifolds. The manifold metric is combined with motion planning methods based on projection operations, which greatly improves the efficiency and success rate of motion planning tasks under constraints. The proposed method Approximate Graph-based Constrained Bi-direction Rapidly Exploring Tree (AG-CBiRRT), which improves upon CBiRRT, and CBiRRT were tested on several task constraints, highlighting the benefits of our approach for constrained motion planning tasks.

1. Introduction

Since multi-degree-of-freedom (multi-DOF) robots are often subject to some hard kinematic constraints in real tasks, each constraint forms a low-dimensional manifold embedded in high-dimensional space, which poses a great challenge to traditional sampling-based motion planning techniques. For example, a robot may need to keep the orientation fixed when carrying an object, a robot may form a closed kinematic chain with the environment (when opening a door or pulling a drawer), or some tasks require cooperation of multiple manipulators. Due to task constraints, robots cannot move freely in the original configuration space.

Compared with artificial potential field methods [1], heuristic search techniques [2] and optimization-based methods [3,4], sampling-based motion planning algorithms such as RRT [5], PRM [6] and RRT${}^{*}$ [7] have been shown to be effective for high-dimensional robots, and become the standard for industrial solutions [8]. They rely on the feasibility information of configurations provided by the collision query module, and connect a series of collision-free configurations to generate a feasible path from the starting point to the target region, which avoids explicit description of obstacles in high-dimensional space. However, the manifold generated by hard kinematic constraints introduces several challenges to the sampling-based motion planners. First, it is almost impossible to obtain configurations satisfying the constraints by direct sampling, because the constraint manifold has zero measure with respect to the ambient space metric, as shown in Figure 1. Second, the manifold is implicitly defined by constraints in robot’s workspace, and the mapping between the workspace and the configuration space is nonlinear, which means that the constraint manifolds cannot be explicitly described by an analytic formula. Third, sampling-based motion planners cannot interpolate locally under task constraints to form a continuous path on manifolds.

To solve the above-mentioned problems, various methods of motion planning on constraint manifold are widely proposed, including relaxation [9,10], projection [11,12,13], tangent-space [14] and atlas [15,16]. The characteristics of these methods are summarized and compared in [17]. Relaxation is to relax the surface of the constraint manifold by increasing the allowed tolerance of the constraint function so that the sampling-based planners can handle the constraints without additional machinery. However, relaxation transmutes task constraint into narrow passage problem, which poses a challenge to the efficiency of constrained motion planning. Tangent space and atlas both create a piece-wise linear approximation of the manifold, which can then be used for generating satisfying configurations or local motion. However, the piece-wise linear approximation of the manifold breaks down when the manifold becomes highly curved, as tangent movement rapidly drifts away from the surface of the manifold. Additionally, the two methods also break down near singularity points, due to the Jacobian losing rank and no longer maintaining a surjective mapping to the ambient configuration space. By comparison, projection is easier to implement and captures the structure of the constraint function within the planning process, which is the most widely implemented methodology. The projection process is introduced in Section 3.5.

Constrained Bi-direction Rapidly-Exploring Random (CBiRRT) [11,18] is a typical projection-based constrained motion planning method. Based on the bidirectional extension of BiRRT, the configurations which are sampled uniformly in ambient space are projected on constraint manifold step by step, and the continuous path satisfying the task constraints is obtained. Because the process of sampling and projection is online, the method lacks the prior knowledge of constraint manifold, which leads to its blind expansion. The failure of the projection process and the exploration of invalid regions in the C-space greatly limits the efficiency and the success rate of the planning algorithm within the allowed time.

In recent years, many successful applications of machine learning methods in the field of robot motion planning show that effective utilization of prior knowledge of the C-space can greatly improve the efficiency of various planners [19,20,21,22]. Therefore, this paper obtains the approximate model of high dimensional constraint manifolds by machine learning methods and selects the ideal extension direction based on the approximate metric for random tree. This process avoids the exploration of the invalid region in C-space, thus greatly improving the efficiency of the constrained motion planning algorithm. The proposed framework, which is developed from CBiRRT, and CBiRRT itself were applied to the motion planning of single-arm robot and dual-arm robot under task constraints.

The main contributions of this paper is to learn an approximate metric for constraint manifolds. The paper is organized as follows: Section 2 introduces the existing work related to the framework of the article. In Section 3, the construction and metric learning methods of approximate model for constraint manifold are introduced, and a motion planning framework under constraints is proposed. In Section 4, AG-CBiRRT and CBiRRT are applied to deal with the task constraints, including keeping a cup upright, dual-arm cooperation and the closed passive chains. The results and analysis of many performance indicators including the planning time, the success rate, the path length and the number of extended nodes are provided. In Section 5, we make conclusions and provide directions for future work.

2. Background and Related Work

In this paper, the problem is to find a collision-free path in configuration space, satisfying the task constraints of the end effectors. Because of the existence of hard kinematic constraints, the valid configuration set is restricted to a manifold embedded in C-space. Because the constraint manifold has zero measure with respect to the ambient space metric, the probability of generating valid configurations by direct sampling method is very low. A commonly used method to solve this problem is a projection operator, which can be realized by a Gauss–Newton process based on Jacobian pseudo inverse $J{\left(q\right)}^{+}$ [23].

The projection operator is used in the process of sampling and local extension. This process allows configurations to be sampled arbitrarily in the ambient space where the manifold is located. Because a projection is easy to implement, it is utilized to solve the problem of tracking the trajectory of industrial robots [24]. Berenson et al. [18] proved that, when the planning time approaches infinity, the configuration set that satisfies the constraints will be fully covered by the projection sampling process. This means that the constrained motion planning method has probabilistic completeness, which is similar to the RRT-like planners. The probabilistic completeness of sampling-based motion planning algorithms means that, if there is a path solution between the starting point and the target point, a path solution must be found as long as the planning time is long enough. Yakey et al. [12] proposed using a projection operator to enable sampling-based motion planners such as RRT and PRM to expand under constraints. They used the framework to solve the motion planning problem of closed kinematic chains. Stilman [13] defined a generic task constraints description method, and presented three projection methods based on the Jacobian pseudo inverse: Tangent Space Sampling (TS), First-Order Retraction (FR) and the Random Gradient Descent (RGD). In recent years, some humanoid robots such as HRP2 [25] and Humanoid Path Planner System [26] also adopt projection to meet task constraints.

Although the Projection-based planning techniques are easy to implement and have probabilistic completeness, the unknown structure of the constraint manifold will lead to many extension steps towards invalid regions in the C-space. To solve this problem, off-line sampling and machine learning methods are used to obtain prior knowledge of manifolds. Off-line sampling means obtaining a configuration set that satisfies constraints by precalculation. Kagami [27] and Burget [28] et al. utilized this method to realize the whole body motion planning of a humanoid robot under dynamic equilibrium constraints. Sucan et al. [29] connected the configurations sampled off-line to generate an approximate graph of the manifold, and then used a variety of sampling-based motion planners to search on the graph directly. However, because of the limited accuracy of the approximate graph, it is difficult to ensure that the local expansion process satisfies constraints. Roy N [30] and Phillips M [31] et al. proposed that the experience graph (E-graph) can be learned from constrained movements (such as opening doors, or pulling drawers) which are demonstrated by users. The E-graph can guide the planner to perform new planning tasks and achieve remarkable effects in multi-DOF robots’ motion planning such as PR2. However, the manifold prior model mentioned above will need to be resampled and relearned when the environment changes. No researcher uses an adapted manifold metric to guide the constrained motion planning.

3. Methods

We introduce the process of constructing an approximate graph of constraint manifolds by off-line sampling, and utilize an approximate model to learn the approximate metric on the manifold. This metric is combined with the projection operator to achieve fast and high-quality motion planning under task constraints.

3.1. Problem Statement

The configuration space of the robot is defined by Q. A path in the space is defined by $\tau :[0,1]\to Q$. We consider constraints evaluated as a function of a configuration $q\in Q$ in $\tau$. The location of q in $\tau$ decides which constraints are active at the configuration. Thus, a constraint is defined as the pair $\left\{C\right(q),s\}$, where $C\left(q\right)\in \mathbb{R}\ge 0$ is the constraint-evaluation function to decide if the constraint is satisfied at q, and $s\subseteq [0,1]$ is the domain of this constraint, i.e., where in the path $\tau$ the constraint is active. We assume that $\tau$ satisfies the given constraint when $C\left(q\right)=0,\forall q\in \tau \left(s\right)$. Each constraint defined in this way implicitly defines a manifold in Q where $\tau \left(s\right)$ is allowed to exist. Configurations that satisfy the constraint form the manifold ${\mathcal{M}}_C\subseteq Q$, which is defined as:

$${\mathcal{M}}_C=\{q\in Q:C\left(q\right)=0\}$$

(1)

For $\tau$ to satisfy a constraint, all the elements of $\tau \left(s\right)$ must lie within ${\mathcal{M}}_C$. If there exists a configuration q not on ${\mathcal{M}}_C$, q is said to violate the constraint. In general, we can define any number of constrains for a given task, each with their own domain. Let a set of n constraint-evaluation functions be $\mathcal{C}$ and the set of domains corresponding to those functions be S. Then, the constrained motion planning problem is defined as:

$$\begin{array}{ll}\mathrm{find}\tau :q\in {\mathcal{M}}_{C_i}& \forall q\in \tau \left(S_i\right)\hfill \\ & \forall i\in \{1\dots n\}\hfill \end{array}$$

(2)

When the end effector of the three-DOF manipulator in Figure 1a is constrained to be on a straight line, the path satisfying the constraint must be located on the manifold in Figure 1b. Note that, if two or more constraints’ domains overlap, the way-points of the path $\tau$ need to lie within two or more constraint manifolds.

3.2. Approximating Constraint Manifolds

Given a specific task constraint, we can construct an approximate graph of the constraint manifold. Task constraints and environment constraints (e.g., caused by geometry of the robots) are decoupled, which means that collision avoidance constraints are not considered in the process of constructing a manifold approximate graph. The form of the approximate graph is G $=(V,E,D)$, where V is the set of nodes on the approximate graph corresponding to the configurations satisfying task constraint which are sampled off-line, and E is the edges on the graph, which refer to the node connections on the manifold. D is the distance matrix of nodes on the approximate graph. The construction process of manifold approximate graph is shown in Algorithm 1.

V can be sampled off-line by robot inverse kinematic calculation, projection [11,12,13], tangent space [14] or atlas [15,16] methods. Samples are drawn to cover the area of interest in the satisfying subset defined by the constraint and placed within V. Then, the KNN [32] algorithm is used to connect each node to the k nearest neighbors in V to generate edges on the approximate graph, as shown in Lines 2–7 of Algorithm 1. KNN is a lazy learning method for data classification by measuring the distance between samples in feature space. KNN is used to construct the relationship between adjacent samples to generate undirected graph. In this process, the k nearest neighbors of each sample need to be retrieved by Kdtree [33]. As shown in Figure 2a, the torus is a two-dimensional manifold in the three-dimensional space. An approximate graph covering the entire manifold can be obtained by uniform sampling and adjacent samples connection process on the torus.

The above undirected graph can be used to realize the estimation of distance metric on manifold. Distance metric plays an important role in motion planning. For example, the asymptotically optimal motion planning methods, such as ${\mathrm{RRT}}^{*}$ [7] and $\mathrm{RRTX}$ [34], use the distance metric to calculate the current cost and heuristic value of the path, which realizes the continuous optimization of the path. The geodesic distance on constraint manifolds cannot be directly calculated in practice, but it can be approximated piecewise linearly by graph distance [35]. The problem of calculating the shortest arc length between two points on a manifold can be simplified to the problem of calculating the shortest path between two points on an approximate graph. The Euclidean distance between the join points in the approximate graph is labeled as the weight of the edges to generate an undirected weight graph. Since the shortest path length on the undirected weighted graph satisfies the properties of a metric, namely being non-negative, symmetric and triangular inequalities, it can be used as an approximate metric of the manifold.

The remaining problem is constructing the distance matrix D between all nodes in the undirected weight graph, which can be realized by repeatedly calling the Dijkstra algorithm [36]. The Dijkstra algorithm can calculate the shortest distance between a source node and all other nodes on the weight graph. As shown in Figure 2b, the approximate geodesic distance between two points on the torus can be obtained by searching the approximate graph in Figure 2a with Dijkstra algorithm.

Constructing distance matrix between all nodes on the manifold approximate graph is the process of metric learning for the constraint manifolds. It is noteworthy that the distance matrix is a real symmetric matrix with a size of $\left|V\right|\times \left|V\right|$. When the dimension of planning problems are relatively high, the number of samples required is large, resulting in large memory occupied required by the distance matrix. Thus, in practice, we use a constant lookup data structure to read the distance matrix online.

3.3. Planning on the Constraint Manifold

In this part, the manifold prior knowledge will be used for constrained motion planning. Similar to CBiRRT [11], the method proposed in this paper implements the exploration of configuration space by growing two trees from the starting point and the goal point, respectively. However, the difference is that the method is based on the approximate graph and manifold metric to select the local optimal expansion direction, rather than by random sampling expansion. Since this algorithm is based on CBiRRT and utilizes the manifold approximate graph for motion planning, we named the algorithm the Approximate Graph CBiRRT (AG-CBiRRT). As shown in Algorithm 2, the input includes the starting point $q_{start}$, goal point $q_{goal}$ of the planning problem and the approximate graph G of the constraint manifold. The output is the path $\tau$ that satisfies the task constraints.

As shown on Lines 3–5 of Algorithm 2, the configurations obtained by random sampling are not set as local expansion targets directly. Algorithm 2 searches the nearest neighbors $q_{near}^a$ on the current tree $T_a$, and uses SelectNeighbor function, which is explained in Algorithm 3, to select the optimal extension direction for the $q_{near}^a$.

As shown on Line 2 of Algorithm 3, the function NearestNeighbor is utilized to search the nearest neighbor of the other tree $T_{other}$’s root node, and set it as the global extension target $q_g$. Then, the function K-NearestNeighbors is utilized to retrieve the k nearest neighbors of $q_{near}$ on the approximate graph, which are stored in the data structure $q_{near}.list$. In this way, k candidate extension targets are obtained. The geodesic distance between each candidate nearest neighbor and the global extension target $q_g$ is difficult to calculate in practice, but, as mentioned above, it can be estimated by the metric GraphMetric $(a,b)$ on the manifold approximate graph. Since the distance matrix of the approximate graph is obtained by off-line computing, the GraphMetric $(a,b)$ can be read directly so that it will not generate additional computing overhead. Since the manifold ${\mathcal{M}}_C$ has the property of its ambient Euclidean space Q in local, the geodesic distance between candidate neighbors and $q_{near}$ can be estimated by Euclidean metric function EuclideanMetric $(a,b)$. In this way, as shown on Line 11 of Algorithm 3, the path cost between each candidate neighbor and $q_g$ can be calculated. The candidate nearest neighbor with the lowest path cost will be chosen as the target of this extension. As shown in Figure 3, through this process, it can select the most promising local extension target for $q_{near}$.

Note that the manifold approximate graph obtained by the previous learning process only considers task constraints, and does not consider the collision avoidance constraint of environment. Therefore, the expansion failure caused by environmental obstacles may cause the planning algorithm to fall into dead circulation (an infeasible candidate neighbor is repeatedly selected). Therefore, it is necessary to ensure that:

• Each node on the search tree only performs once k nearest neighbors searching, as shown on Line 3 of Algorithm 3.
• Each candidate nearest neighbor can only be selected once (but it can be selected as other nodes’ candidate neighbor). As shown on Line 16 of Algorithm 3, when the candidate neighbor is selected, it will be erased from the data structure $q_{near}.list$.
• If $q_{near}$’s k nearest neighbors have been searched but the $q_{near}.list$ is empty (Line 7 of Algorithm 3), which indicates that all the neighbors have been selected as $q_{rand}$ and erased from $q_{near}.list$, the $q_{rand}$ sampled in configuration space will be returned without change.

After completing the selection of candidate neighbors, the ConstraintExtend function is used to implement local extensions from $q_{near}$ to target $q_{rand}$ under constraints. If the extension is successful, the ConstraintExtend function is called again to extend the other tree $T_b$ to the $q_{reached}^a$ which is reached last time. If $q_{reached}^a=q_{reached}^b$, it means that two search trees are connected and the planning task is completed. Otherwise, the trees will be swapped and expanded until the allowed planning time is reached.

3.4. Local Extension under Constraint

Although the selection of an optimal candidate neighbor on the manifold approximate graph can make most of the extension on the manifold, the Projection operation is still necessary. The main reasons are as follows:

• The number of samples is limited, which will lead to the error between an approximate graph and the constraint manifold.
• When the $q_{near}.list$ is emptied, $q_{rand}$ will be returned as local extension target, but the configuration does not satisfy the task constraints, as shown on Lines 7–9 of Algorithm 3. Therefore, to ensure that all way-points on the path satisfy the task constraints, the ConstrainedExtend function is used to implement local extension under constraints.

As shown in Algorithm 4, The ConstrainedExtend function works by iteratively moving from $q_{near}$ to the configuration $q_{target}$ with step size $△q_{step}$. $q_{target}$ represents the target of local extension under task constraints. During each move, the ConstrainConfig function is used to project the new configuration $q_s$ onto the constraint manifold, so that $q_s$ satisfies the task constraint. The ConstrainConfig function is defined by specific problems and is discussed in Section 4. The CollisionFree function checks whether the interval between the two configurations is feasible by calling the collision query module. After each move, we whether the new configuration $q_s$ reaches $q_{target}$ is checked. There are three cases of local extension results: REACHED represents the local extension target $q_{target}$ is reached; ADVANCED indicates that it moves toward the target, but hasn’t arrived; and TRAPPED indicates that there is no successful movement. Because the planner may fail to reach the local expansion target $q_{target}$, we need to keep track of where the planner actually reached in $q_{reached}$, which could be used as the local expansion target of next round as shown on Line 9 of Algorithm 2.

3.5. Projection Operation to Satisfy Constraints

In this section, the method of satisfying task constraints by projection technique is briefly introduced. CBiRRT [11] extends the Bi-directional RRT (BiRRT) algorithm by using projection techniques to explore the configuration space manifolds that correspond to constraints and to find bridges between them. The projection technique used in this article is similar to that of CBiRRT.

Firstly, T${}_c^0$ is defined as the relative transformation matrix of the task constraints relative to the world coordinate system. T${}_{obj}^0$ is the relative transformation matrix between the end effector and the world coordinate system, which can be calculated by the positive kinematics. Constraints in Cartesian space are defined according to the allowable tolerance between T${}_{obj}^0$ and T${}_c^0$, as shown in Equation (3).

$$\mathbf{C}=\left[\begin{array}{cc}{x}_{min}& x_{max}\\ y_{min}& y_{max}\\ z_{min}& z_{max}\\ {\psi }_{min}& {\psi }_{max}\\ {\theta }_{min}& {\theta }_{max}\\ {\varphi }_{min}& {\varphi }_{max}\end{array}\right]$$

(3)

The first three lines of C provide upper and lower boundaries for the translation transformations along X, Y, and Z axes, respectively, and the last three lines of C provide upper and lower bounds for the rotation transformations around these three axes.

Given a configuration $q_s$, the DisplacementFromConstraint $(\mathbf{C},{\mathbf{T}}_c^0,q_s)$ function is defined as follows:

First, we use forward kinematics at $q_s$ to get ${\mathbf{T}}_{obj}^0$. The relative coordinate transformation between the end-effector and the task constraint can be calculated by Equation (4):

$${\mathbf{T}}_{obj}^c={\left({\mathbf{T}}_c^0\right)}^{-1}{\mathbf{T}}_{obj}^0$$

(4)

Then, T${}_{obj}^c$ can be converted to a six-dimensional vector $d^c$, including X, Y, Z, Roll, Pitch and Yaw, which represents displacement vector in Cartesian space:

$$d^c=\left[\begin{array}{c}{\mathbf{t}}_{obj}^c\\ \arctan 2({\mathbf{R}}_{obj32}^c,{\mathbf{R}}_{obj33}^c)\\ -\arcsin \left({\mathbf{R}}_{obj31}^c\right)\\ \arctan 2({\mathbf{R}}_{obj21}^c,{\mathbf{R}}_{obj11}^c)\end{array}\right]$$

(5)

where ${\mathbf{T}}_{obj}^c$ is the 3 × 1 translation vector in T${}_{obj}^c$, and ${\mathbf{R}}_{obj}^c$ is the 3 × 3 rotation matrix in T${}_{obj}^c$.

Finally, by comparing with the constraint bounds in Equation (3), we can get the displacement from the constraint in the Cartesian space △x${}_i$:

$$△{\mathbf{x}}_i=\left\{\begin{array}{c}{d}_i^c-{\mathbf{C}}_{imax},\mathrm{if}{d}_i^c>{\mathbf{C}}_{imax}\hfill \\ d_i^c-{\mathbf{C}}_{imin},\mathrm{if}{d}_i^c<{\mathbf{C}}_{imin}\hfill \\ 0,\mathrm{otherwise}\hfill \end{array}\right.i=x,y,z,\psi ,\theta ,\varphi ;$$

(6)

To satisfy the constraint, we use a gradient descent projection method based on the Jacobian pseudo inverse, which is consistent with the method used in CBiRRT (as shown in Algorithm 5).

Through the ProjectConfig function, the configuration $q_s$ which satisfies the task constraints of the end effector can be obtained. Due to the manifold approximate graph based local extension, most of the way-points are satisfied with the task constraint, and the times of Projection operations is reduced greatly (as shown on Lines 3–5 of Algorithm 5).

4. Results

In this section, the simulation and experiment results of the proposed method in the constrained motion planning problem of keeping a cup upright, dual-arm cooperation and passive chains are shown. All of the above algorithms were implemented in the framework of the Open Motion Planning Library (OMPL 1.3.1) [37]. All experiments were implemented on an Intel Core i5-4590 3.3GHz personal computer. ROS Indigo and Gazebo were used to build the simulation platform, and the Flexible Collision Library (FCL) [38] was used for collision query. In all experiments, search step length was $△q_{step}=0.1$, and the constraint tolerance was $ϵ=0.1$.

4.1. Constraint Motion Planning for Keeping a Cup Upright

For single-arm robots, the end effector holding a cup and keeping it upright is a typical task constraint. Firstly, we determined the constraint boundary $\mathbf{B}$ of the task in the form of Equation (3):

$$\mathbf{B}=\left[\begin{array}{cc}-\infty & +\infty \\ -\infty & +\infty \\ -\infty & +\infty \\ 0& 0\\ 0& 0\\ -\infty & +\infty \end{array}\right]$$

(7)

The two degrees of freedom under Cartesian space are restricted, so the feasible configuration of a 6-DOF manipulator under this task is constrained on a continuous four-dimensional manifold. In this task, the constraint’s coordinate system ${\mathbf{T}}_c^0$ can be set to any fixed point in the space. For convenience, we set it as the base coordinate system of the manipulator. Next, we designed the ConstrainConfig function for the task, as shown in Algorithm 6.

By traversing the workspace of the robot and the inverse kinematics, a configuration satisfying the task constraint could be obtained. The approximate graph G$(V,E,D)$ of the four-dimensional manifold was constructed using 4183 samples in 49.1 min, and occupied a memory of 189.2 Mb. As shown in Figure 4, a 6-DOF UR10 manipulator was used to build the simulation platform. AG-CBiRRT, which is proposed in this paper, and CBiRRT were used for motion planning under constraints in four scenarios, respectively. Note that, since our method decouples the task constraints from collision avoidance constraints, the obtained manifold metric is applicable in all four scenarios and does not need to be relearned.

As shown in

Table 1. Single manipulator motion planning under task constraints.

	CBiRRT				AG-CBiRRT
	Scene 1	Scene 2	Scene 3	Scene 4	Scene 1	Scene 2	Scene 3	Scene 4
Success Rate (%)	54.2	100	100	89.6	100	100	100	100
Average Planning Time (s)	5.11	1.25	2.87	3.13	0.54	0.25	0.31	0.53
Average Path Length (rad)	22.76	39.30	30.28	15.49	9.35	8.90	12.25	10.46
Average Nodes Number	1738	646	1142	1155	369	144	172	293

, the performance comparison between CBiRRT and AG-CBiRRT algorithm under four planning scenarios could be obtained through multiple tests. The success rate refers to the probability that a feasible path could be searched within the 10 s allowed planning time. The path length refers to the sum of Euclidean distances between adjacent path points on the final path. The number of nodes refers to the number of feasible nodes on the tree when planning was successful.

To show the algorithm’s effect more intuitively, we present box-plots of the planning time and the path length. As shown in Figure 5a, since the extension of AG-CBiRRT was carried out on the manifold approximate graph, many projection operations and the exploration of the invalid regions in C-space were avoided. The planning time was controlled within 1 s, and the success rate in each planning scene was up to 100%. In most of the scenes, the planning efficiency was improved by an order of magnitude. As shown in Figure 5b, the method proposed in this paper used the approximate graph to select the lowest cost expansion direction in each step. Compared with the CBiRRT, the path length was significantly shortened.

4.2. Constraint Motion Planning for Dual-Arm Cooperation

A dual-arm robot forms a kinematic closed chain when it manipulates an object, which is a common task constraint. For the task of holding an object and maintaining its levelness with the end effectors of the dual-arm robot, the configurations that satisfy the constraint constitute a continuous four-dimensional manifold in a $2d$-dimensional space (d is the single manipulator’s DOF). For this task, similar to the case of the single-arm robot, the constraints’ coordinate system ${\mathbf{T}}_c^0$ could be set to any fixed point in work space. Since the end effectors of the two arms were connected by the manipulated objects, the coordinate system ${\mathbf{T}}_{right}^0$ of the right end effector had the following relationship with the left end effector’s coordinate system. ${\mathbf{T}}_{left}^0$:

$${\mathbf{T}}_{left}^0={\mathbf{T}}_{obj}{\mathbf{T}}_{right}^0$$

(8)

where ${\mathbf{T}}_{obj}$ is the coordinate transformation between the two end effectors, which is related to the size and shape of the object.

The motion constraints of the manipulated object were decomposed into two end effectors’ constraints:

$${\mathbf{B}}_{right}=\left[\begin{array}{cc}-\infty & +\infty \\ -\infty & +\infty \\ -\infty & +\infty \\ 0& 0\\ 0& 0\\ -\infty & +\infty \end{array}\right]{\mathbf{B}}_{left}=\left[\begin{array}{cc}0& 0\\ 0& 0\\ 0& 0\\ 0& 0\\ 0& 0\\ 0& 0\end{array}\right]$$

(9)

${\mathbf{T}}_{right}^0$ needs to satisfy the constraint of keeping the object level. ${\mathbf{T}}_{left}^0$ could be solved according to Equation (8), and its projection operation was equivalent to solving inverse kinematics by numerical iteration. Therefore, the task constraint of the dual-arm robot needed to be satisfied by two projection operations (as shown in Algorithm 7).

Subsequently, AG-CBiRRT and CBiRRT were used for the constrained motion planning tasks of dual-arm robots, respectively, as shown in Figure 6. Two 6-DOF UR10 robots formed a dual-arm robot simulation platform, which aimed to maintain the levelness of the object in the process of holding and transferring the object. The time limit for the motion planning was set to 200 s. The approximate graph of the constraint manifold was constructed using 4286 samples in 53.6 min, and occupied a memory of 204.9 Mb.

Table 2. Dual-arm robot motion planning under task constraint.

	CBiRRT		AG-CBiRRT
	Scene 1	Scene 2	Scene 1	Scene 2
Success Rate (%)	66.3	4.0	100	100
Average Planning Time (s)	2.97	144	0.043	17.3
Average Path Length (rad)	7.82	16.79	5.65	12.15
Average Nodes Number	87	1,121	42	715

shows the performance comparison of the two motion planning algorithms. In Scene 1, since the AG-CBiRRT algorithm used the manifold metric, the planning success rate significantly improved compared with the CBiRRT. In addition, the time required to complete the planning task was reduced by two orders of magnitude, and the path length was greatly shortened. In Scene 2, because of the smaller shelf space in the feasible space (as shown in Figure 6b), CBiRRT’s success rate within limited time was very low. However, AG-CBiRRT still had 100% planning success rate in this task.

4.3. Constraint Manipulation Planning for Passive Chains

The task in this problem was for the robot to pull a drawer or open a door. In these processes, the robot and the operated object formed a closed kinematics chain. The operated object was assumed to be completely passive and the kinematics were assumed to be known.

In the task of pulling a drawer, the end effector was constrained on a line that is parallel to the opening direction. For convenience, we set the constraint’s coordinate system ${\mathbf{T}}_c^0$ at the midpoint of the sliding axis of the drawer. To ensure a smooth opening, the orientation of the end effector was fixed during the motion. We could determine the constraint boundary ${\mathbf{B}}_{\mathbf{1}}$ of the pulling task in the form of Equation (3).

In the task of opening a door, the end effector was constrained on an arc centered on the door hinge. The constraint’s coordinate system ${\mathbf{T}}_c^0$ was set on the door hinge. Thus, the end effector can only rotate around ${\mathbf{T}}_c^0$’s Z axis and the constraint boundary ${\mathbf{B}}_{\mathbf{2}}$ can be determined:

$${\mathbf{B}}_{\mathbf{1}}=\left[\begin{array}{cc}0& 0\\ -\infty & +\infty \\ 0& 0\\ 0& 0\\ 0& 0\\ 0& 0\end{array}\right]{\mathbf{B}}_{\mathbf{2}}=\left[\begin{array}{cc}rcos\left(P\right)& rcos\left(P\right)\\ -rsin\left(P\right)& -rsin\left(P\right)\\ 0& 0\\ 0& 0\\ 0& 0\\ -\infty & \infty \end{array}\right]$$

(10)

where P is the Pitch of end effector relative to the constraint’s coordinate system ${\mathbf{T}}_c^0$, and r is the door’s revolving radius.

Except for ${\mathbf{T}}_c^0$’s position, the ConstrainConfig function for these two tasks was exactly the same as Algorithm 6.

Then, we prepared the manifold approximate graph for the two tasks, respectively, according to Algorithm 1, and the graph information is shown in

Table 3. The information of approximate graphs for passive chain constraint manifolds.

	Pulling the Drawer	Opening the Door
Node Number	512	422
Construction Time (s)	2.97	2.51
Occupied Memory (Mb)	8.75	5.95

.

Subsequently, AG-CBiRRT and CBiRRT were used for constrained motion planning tasks of passive chains, respectively, as shown in Figure 7. The time limit for the motion planning was set to 100 s.

Table 4. Robot performing constrained manipulation on passive chains.

	CBiRRT		AG-CBiRRT
	Drawer	Door	Drawer	Door
Success Rate (%)	37.5	0	100	100
Average Planning Time (s)	3.777	–	0.236	2.112
Average Path Length (rad)	1.747	–	1.076	1.536
Average Nodes Number	211	–	8	26

shows the performance comparison of the two motion planning algorithms for passive chains. In pulling a drawer task, since the AG-CBiRRT algorithm makes use of the manifold approximate metric, the planning success rate significantly improved compared with the CBiRRT algorithm. In addition, the time required to complete the planning task was reduced by one order of magnitude, and the path length was shorter. In opening a door task, CBiRRT could not complete the planning task. However, AG-CBiRRT still had 100% success rate within limited time.

5. Conclusions

In this paper, a motion planning method based on manifold metric learning is proposed to solve the problem of robot motion planning under task constraints. The off-line sampling method is used to obtain the configurations satisfying the task constraints, and the KNN algorithm is used to construct a graph which can approximate the constraint manifold. Then, the shortest path length between the nodes on the approximate graph is calculated using the Dijkstra algorithm, and the distance matrix is constructed off-line to realize the metric learning of the constraint manifolds. During on-line planning, the approximate graph and metric of the manifold are used to select the most promising direction in the local expansion, thus reducing the exploration of the invalid regions in C-space. Compared with CBiRRT, the proposed method is faster and has lower path length in experiments of keeping a cup upright, the dual-arm cooperation and the closed passive chains. In the dual-arm robot’s constrained motion planning tasks, the planning efficiency is improved by 1–2 orders of magnitude. Therefore, the method proposed in this paper has the ability to perform various constrained motion planning tasks.

In the future, under the premise of satisfying the task constraints and environmental constraints, we will improve the algorithm to shorten the path length and make the path smoother. Besides, we plan to extend the approximate graph method to all RRT-like motion planning algorithms within the framework of OMPL.