2.1 Motion planning for closed-chain manipulation

The motion planning problem for CKC systems is to find a path that satisfies robot limits, CKC constraint, and collision avoidance to connect the start and goal configuration [Reference Sintov, Borum and Bretl10]. The low dimensionality of the constraint manifold makes this problem hard to solve [Reference LaValle, Yakey and Kavraki3]. Common approach for sampling valid configurations is through projection. For instance, [Reference Xie and Amato5,Reference Cortes, Simeon and Laumond6] break the CKC into open sub-chains, and sample the configuration for the active chain and compute for the passive chain under CKC constraint. However, this approach introduces artificial singularities, and the passive chain need to be non-redundant [Reference Sintov, Borum and Bretl10]. Newton-Raphson projection method [Reference Latombe and Hauser24] iteratively reduces the projecting residual, which achieves better success rate but slower speed. Furthermore, configurations are sampled on a tangent space of the constraint manifold to improve projection performance in recent works [Reference Jaillet and Porta7–Reference Kingston, Moll and Kavraki9]. After feasible configurations are obtained, projection method [Reference Yakey, LaValle and Kavraki2,Reference Stilman25] or differential IK [Reference Xie and Amato5,Reference Pham and Stasse26] is used for local connections. However, these methods aim to address planning in a single connected component. Global-level planning, i.e., planning paths that connect different components, is required for solving complex closed-chain tasks. Gharbi et al. [Reference Gharbi, Cortes and Simeon27] solve this problem via singular configurations, but full knowledge of IK classes characterization is need. Koga et al. [Reference Koga and Latombe28] first use regrasping to get away the local minimum in the randomized potential field but joint limits are ignored. More recent work [Reference Xian, Lertkultanon and Pham11] utilizes regrasping moves between different IKs of a same grasp pose (IK-switch) to bridge different components. Although the result is promising, only a single grasp pose greatly limits the flexibility of manipulating objects and thus reduces the success rate. Moreover, the planner in [Reference Xian, Lertkultanon and Pham11] is designed for nonredundant robots, and the IK is obtained by closed-form solution using IK fast [Reference Diankov29], which restricts its extension to redundant robots. In this paper, we extend the global planner by integrating both IK-switch and Grasp-switch moves in the planning. Our planner can flexibly choose the two approaches efficiently to improve the success rate of planning. Furthermore, we use dual-redundant robots in view of their better efficacy and flexibility, and the IKs that $\mathrm{s}\mathrm{a}\mathrm{t}\mathrm{i}\mathrm{s}$ fy joint limits are obtained by a proposed QP-based IK solver quickly.

There exist more recent works solving motion planning of multi-arm robot systems. Preda et al. [Reference Preda, Manurung, Lambercy, Gassert and BonfÈ30] develop a motion planning architecture for dual-arm surgical robot completing a suturing task. Xu et al. [Reference Xu31] present a coordinated motion planning approach for a dual-arm space robot capturing the target. However, both suturing task in [Reference Preda, Manurung, Lambercy, Gassert and BonfÈ30] and capturing task in [Reference Xu31] involve separate motions for the two arms only, and thus closed-chain motions are not considered. Similar problem exists in [Reference GarcÍa, Rosell and SuÁrez32], where the authors use a dual-arm robot to perform human-demonstrated tasks without closed-chain motions. Optimization-based approaches are also proposed to address motion planning with closed kinematics. For example, the planning problem is formulated and solved as an optimization problem in [Reference Szynkiewicz and Blaszczyk33,Reference VÖlz and Graichen34], but these methods lack the ability to solve planning at a global level. A recent interesting work presented in [Reference Stouraitis, Chatzinikolaidis, Gienger and Vijayakumar35] utilizes grasp-hold changes to address human-robot collaboration tasks (joint actions for rotating the object), where the key is adapting motions according to the partner’s intension. Whereas we use grasp changes to bridge different constraint components in the global dual-robot closed-chain motion planning, which results in an improved planning performance.

2.2. Inverse kinematics (redundancy resolution) of redundant robot

A fundamental issue in motion planning and control of redundant robots is the IK (redundancy resolution) problem, since that infinite IK solutions exist due to the redundancy [Reference Hassan, El-Habrouk and Deghedie13,Reference Song, Su, Li, Zhao and Du36]. Conventional solution is the pseudo-inverse-based formulation. However, the computation is time consuming, and joint inequality constraints are difficult to be formulated [Reference Latombe and Hauser24]. Therefore, to overcome the shortcomings, many works formulate the redundancy resolution as a QP problem built on different optimization criteria, such as minimum velocity norm (MVN) [Reference Guo14], infinity norm velocity minimization (INVM) [Reference Choi, Lee and Lee37], minimum acceleration norm [Reference Guo38], infinity-norm acceleration minimization (INAM) [Reference Zhang, Yin and Cai39], infinity-norm torque minimization (INTM) [Reference Zhang40], and so on. In order to meet multiple requirements in more complex applications, multi-criteria optimization methods that combine different schemes are proposed, such as MVN-INVM [Reference Zhang, Cai, Zhang and Li41], bi-criteria torque optimization [Reference Liu and Wang18], two-infinity norm switching [Reference Baratcart, Salvucci and Koseki42], MVN-MTN [Reference Zhang, Guo and Ma16], and bi-criteria pseudoinverse minimization [Reference Liao43]. However, although proved effective in simple tasks using a single manipulator, these methods lack the ability to solve the multi-robot planning and control problem. Some recent works consider the coordination motion of dual-redundant-robot [Reference Zhang, Lin, Li and Li22,Reference Jia, Chen, Zhang, Zhong, Zhang and Qu23], but their methods are only designed for predefined $3\mathrm{D}$ position trajectory tracking tasks using two 6 $\mathrm{D}\mathrm{o}\mathrm{F}$ robots in a simulation environment. As a contrast, optimization-based redundancy resolution for dual-robot in random $6\mathrm{D}$ motion planning is rarely studied. In this paper, in order to make a more effective utilization of input power in the $6\mathrm{D}$ manipulation task of large object and remedy joint torque instability of INTM, two QPs for two robots are formulated combining MVN and INTM scheme, which is further unified into one QP for simultaneous control of dual arms. Our optimization scheme, termed Dual-MVN-INTM, is organically integrated in the closed-chain motion planning and guarantees smooth planned joint trajectories that $\mathrm{s}\mathrm{a}\mathrm{t}\mathrm{i}\mathrm{s}$ fy joint limits. Escande et al. [Reference Escande, Mansard and Wieber44] present a hierarchical QP solution for generating humanoid-robot motions with both equality and inequality constraints considered. However, the start and target configurations can be locally connected in their close-chain task, which means their method is probably not suitable for the global-level tasks discussed in this paper.

The formulated QPs that subject to both robot equality and inequality constraints are complicated and large-scale, which makes it difficult and inefficient for traditional numerical QP solvers to solve [Reference Hassan, El-Habrouk and Deghedie13,Reference Liu and Wang18]. Therefore, recurrent neural networks (RNNs) are widely applied due to their efficient parallel computational nature [Reference Hassan, El-Habrouk and Deghedie13]. Different types of RNNs that enable real-time applications are utilized to solve the QP-formed IK problem, such as the Lagrangian neural network (LNN) [Reference Wang, Hu and Jiang45], the primal-dual neural network (PDNN) [Reference Tang and Wang46], the DNN [Reference Zhang and Wang47], linear variational inequalities-based PDNN (LVI-PDNN) [Reference Zhang and Li19], simplified LVI-PDNN (S-LVI-PDNN) [Reference Zhang, Lin, Li and Li22], and simplified DNN (S-DNN) [Reference Liu and Wang18]. Among these RNNs, DNN uses the dual decision variables only and directly uses Karush-Kuhn-Tucker (KKT) condition [Reference Boyd, Vandenberghe and Faybusovich48] and projection operator to reduce network complexity and increase efficiency. S-DNN further reduces the complexity of DNN while preserves the desirable convergence property. Thus, we formulate an S-DNN model in this paper for the real-time solving of our Dual-MVN-INTM scheme, which makes sure the computation efficiency of our method.

3.1 Motion planning problem of closed-chain systems

Consider a CKC system (Fig. 1) consisting of two redundant robots and a moveable object. Let $C_{\mathrm{r}\mathrm{o}\mathrm{b}\mathrm{o}\mathrm{t}}^{i}\subseteq \mathbb{R}^{n_{i}}$ denote the configuration space of the $i^{\mathrm{t}\mathrm{h}}$ robot, where $n_{i}$ is the number of DoF. If consider the position and orientation of the end-effector, the dimension of task space $m=6$ , and the redundancy for a 7 $\mathrm{D}\mathrm{o}\mathrm{F}$ robot is 1. Let $C_{\mathrm{o}\mathrm{b}\mathrm{j}}\subseteq$ SE (3) be the object’s configuration space, then the system’s composite configuration space can be expressed as $C_{\mathrm{c}\mathrm{o}\mathrm{m}\mathrm{p}\mathrm{o}\mathrm{s}\mathrm{i}\mathrm{t}\mathrm{e}}=C_{\mathrm{r}\mathrm{o}\mathrm{b}\mathrm{o}\mathrm{t}}^{\mathrm{o}}\times C_{\mathrm{r}\mathrm{o}\mathrm{b}\mathrm{o}\mathrm{t}}^{1}\times C_{\mathrm{o}\mathrm{b}\mathrm{j}}$ . A composite configuration is denoted as $c=(\theta_{\mathrm{o}},\theta_{1},T_{\mathrm{o}\mathrm{b}\mathrm{j}})\in C_{\mathrm{c}\mathrm{o}\mathrm{m}\mathrm{p}\mathrm{o}\mathrm{s}\mathrm{i}\mathrm{t}\mathrm{e}}$ , where $\theta_{i}\in C_{\mathrm{r}\mathrm{o}\mathrm{b}\mathrm{o}\mathrm{t}}^{i}$ is the configuration of the $i^{\mathrm{t}\mathrm{h}}$ robot, and $T_{\mathrm{o}\mathrm{b}\mathrm{j}}\in C_{\mathrm{o}\mathrm{b}\mathrm{j}}$ is the configuration of the object.

Figure 1. A CKC system consisting of two redundant robots and an object, where $\{L\}$ and $\{R\}$ denote the base frame of the left robot and right robot, respectively, $\{W\}$ denotes the world frame, and $\{obj\}$ denotes the object frame

The CKC is formed when two robots manipulating a single object and can be expressed as nonlinear equations $g(c)=0$ . Hence, the set of all composite configurations that $\mathrm{s}\mathrm{a}\mathrm{t}\mathrm{i}\mathrm{s}$ fy CKC is a subset $C_{cc}$ :

(1) \begin{align}C_{cc}=\{c|c\in C_{\text{composite}},g(c)=0\}\end{align}

Note that in this case, the grasping pose $T_{\mathrm{G}\mathrm{r}\mathrm{a}\mathrm{s}\mathrm{p}}$ of the robots, i.e., the relative transformations between the robot end-effectors and the object, can be uniquely determined by the composite configuration c:

(2) \begin{align}T_{\text{Grasp}_i}=T_{\text{obj},\text{ef}_{i}}=T_{\text{obj}}^{-1} \cdot T_{\text{ef}_i}(\theta_{i})\end{align}

where $T_{\mathrm{e}\mathrm{f}_{i}}(\theta_{i})$ is the transformation of the $i^{\mathrm{t}\mathrm{h}}$ robot’s end-effector. Define a projection $f:C_{\mathrm{c}\mathrm{o}\mathrm{m}\mathrm{p}\mathrm{o}\mathrm{s}\mathrm{i}\mathrm{t}\mathrm{e}}\rightarrow C_{\mathrm{o}\mathrm{b}\mathrm{j}}$ that maps composite configurations to object configurations, such that given $c=(\theta_{\mathrm{o}},\theta_{1},T_{\mathrm{o}\mathrm{b}\mathrm{j}}), f(c)=T_{\mathrm{o}\mathrm{b}\mathrm{j}}$ . Moreover, let $C_{\mathrm{b}}\subset C_{\mathrm{c}\mathrm{o}\mathrm{m}\mathrm{p}\mathrm{o}\mathrm{s}\mathrm{i}\mathrm{t}\mathrm{e}}$ denote the restricted region due to obstacles and joint position limits, then the free configuration space can be expressed as $C_{\mathrm{f}\mathrm{r}\mathrm{e}\mathrm{e}}=C_{\mathrm{c}\mathrm{o}\mathrm{m}\mathrm{p}\mathrm{o}\mathrm{s}\mathrm{i}\mathrm{t}\mathrm{e}}\backslash C_{\mathrm{b}}$ . Accordingly, the set of feasible composite configurations that respect the CKC constraint, joint position limits, and are collision-free can be defined as

(3) \begin{align}C_{\text{feasible}}=\{c|c\in C_{\text{cc}} \cap C_{\text{free}}\}\end{align}

With the definitions determined above, the motion planning problem of a CKC system can be described as follows. Given a start composite configuration $c_{\mathrm{s}\mathrm{t}\mathrm{a}\mathrm{r}\mathrm{t}}\in C_{\mathrm{f}\mathrm{e}\mathrm{a}\mathrm{s}\mathrm{i}\mathrm{b}\mathrm{l}\mathrm{e}}$ and a goal object configuration $T_{\mathrm{o}\mathrm{b}\mathrm{j}_{\mathrm{g}\mathrm{o}\mathrm{a}1}}\in C_{\mathrm{o}\mathrm{b}\mathrm{j}}$ , find a continuous path $P:[0, 1]\rightarrow C_{\textrm{feasible}}$ such that $P(0)=c_{\mathrm{s}\mathrm{t}\mathrm{a}\mathrm{r}\mathrm{t}},$ $f(P(1))=T_{\mathrm{o}\mathrm{b}\mathrm{j}}.$

3.2 Constraint manifolds and regrasping moves

The CKC constraint reduces the composite configuration space to a lower-dimensional constraint manifold in the ambient space. Due to the physical limits, the robots sometimes cannot move from one configuration to the next one while satisfying the CKC constraint. Thus, the definition of essentially mutually disconnected (EMD) [Reference Xian, Lertkultanon and Pham11] is introduced: Given a composite configuration $c\in C_{\mathrm{f}\mathrm{e}\mathrm{a}\mathrm{s}\mathrm{i}\mathrm{b}\mathrm{l}\mathrm{e}}$ , the set of all feasible configurations that can be reached from c by continuous and feasible paths is referred to as the connected component S(c). Two component components, $S(c_1)$ and $S(c_{2})$ , are EMD if they are indeed disconnected $$(S({c_1}) \cap S({c_2}) = \emptyset )$$ or they could not be connected in a reasonable amount of time. For the planning queries shown in Fig. 2, the components containing the start and the goal are disconnected, thus the system cannot realize the required motion without breaking the CKC.

Figure 2. Start and goal configurations in two CKC motion planning problems. $(\mathrm{a},\ \mathrm{c})$ : Start configurations. $(\mathrm{b},\ \mathrm{d})$ : Goal configurations. There are no continuous feasible paths without breaking the closed chains

The key of solving the planning at global level is how to plan motions crossing different EMD components. As shown in Fig. 3, there exists two ways to connect different EMD components: a. IK- switch: regrasping moves between different IK solutions of a same grasping pose. b. Grasp-switch: regrasping moves between different grasping poses for the object. Different IK solutions or grasping poses can correspond to different EMD components, hence the conversion between different IK solutions or grasping poses can realize the required connection. However, for a specific object configuration, IK solving may fail due to the limits such as obstacle avoidance or robot physical limits. In this case, the Grasp-switch method and the utilization of redundant robots become necessary. In addition, Grasp-switch can also be an optional choice for more flexible connections of EMDs in the planning. Therefore, the two methods are integrated into a unified regrasping framework, more details are introduced in Section 4.3.

Figure 3. Connecting different EMD components using IK-switch moves and Grasp-switch moves

4.1 Overview of planning algorithm

The planner follows the classical sampling-based RRT architecture. As shown in Fig. 3, in the first stage (Section 4.2), the planer samples and plans the SE(3) path for the object, and the object trajectory is computed via Quintic Polynomial Interpolation (Section 5.1). The QP-based differential IK algorithm (Section 5.2) is then used to map the SE(3) trajectory to the joint space quickly. When the IK solution fails at some SE(3) configurations due to the CKC constraint, the planner computes the valid regrasping configuration for IK-switch or Grasp-switch moves at these nodes. The output of the first stage will be several closed-chain motion segments with regrasping requests. The second stage of the planner (Section 4.3) deals with these requests and connects the motion segments via IK-switch or Grasp-switch moves to generate the global trajectory.

4.2 Closed-chain motion planning

Different from [Reference Xian, Lertkultanon and Pham11] that only a single grasp pose is utilized, for the start configuration $c_{\mathrm{s}\mathrm{t}\mathrm{a}\mathrm{r}\mathrm{t}}$ and the object goal configuration $T_{\mathrm{g}\mathrm{o}\mathrm{a}1}$ , the goal configuration $c_{\mathrm{g}\mathrm{o}\mathrm{a}1}$ is computed using the grasp pose of the last node in this paper. The Graspit! simulator is used to generate valid grasp poses for a certain manipulation object, and the top ranked m grasps are saved in a predefined grasp pose list Grasplist (4) after testing and selecting.

(4) \begin{align}\text{Grasplist}=[[T_{\text{Grasp}_0}^{0},T_{\text{Grasp}_{1}}^{0}],\cdots ,[T_{\text{Grasp}_0}^{m-1},T_{\text{Grasp}_1}^{m-1}]]\end{align}

The pseudo-code of the closed-chain planning algorithm is shown in Algorithm 1. The algorithm takes the initial node $\mathcal{V}_{\mathrm{s}\mathrm{t}\mathrm{a}\mathrm{r}\mathrm{t}}$ and the target node $\mathcal{V}_{\mathrm{g}\mathrm{o}\mathrm{a}1}$ as root nodes to grow the forward tree ${\mathcal{T}}_{\mathrm{f}}$ . and the backward tree $\mathcal{T}_{b}$ , where $\mathcal{V}_{\mathrm{s}\mathrm{t}\mathrm{a}\mathrm{r}\mathrm{t}}$ and $\mathcal{V}_{\mathrm{g}\mathrm{o}\mathrm{a}1}$ are endowed with the initial grasping pose Grasplist [0]. Brief descriptions of some key functions in Algorithm 1 are given below.

• • SampleSE3Config() This function samples the object configuration $T_{\mathrm{r}\mathrm{a}\mathrm{n}\mathrm{d}}=(R,p)$ in SE(3), where R is uniformly sampled in SO (3), and p is uniformly sampled within a allowed range for the task.

• • Extend() This function expands ${\mathcal{T}}_{\mathrm{f}}$ toward $T_{\mathrm{r}\mathrm{a}\mathrm{n}\mathrm{d}}$ and generates new nodes. In Algorithm 2, NearestNeighbor() searches over all nodes in $\mathcal{T}_{\mathrm{f}}$ and finds the node with an object configuration nearest to $T_{\mathrm{r}\mathrm{a}\mathrm{n}\mathrm{d}}$ and a regrasping number less than $R_{\max}$ . For $T_{0}=(R_{0},p_{0})$ and $T_{1}=(R_{1},p_{1})$ , the minimal distance between $R_{0}$ and $R_{1}$ in SO (3) is the Euclidean length $\Vert r\Vert$ , where

  (5) \begin{align}[r]=\log(R_{0}^{\text{T}}R_{1})\end{align}
  [r] denotes the skew-symmetric of $r\in \mathbb{R}^{3}$ . The distance between $T_{0}$ and $T_{1}$ in SE(3) is then defined as the combination of $\Vert r\Vert$ and the Euclidean distance between the translation vectors:
  (6) \begin{align}d=(\lVert r\rVert^{2}+\lVert p_{0}-p_{1}\rVert^{2})^{1/2}\end{align}
  $\mathrm{N}\mathrm{e}\mathrm{w}\mathrm{S}\mathrm{E}3\mathrm{C}\mathrm{o}\mathrm{n}\mathrm{f}\mathrm{i}\mathrm{g}()$ generates the new object configuration at a predetermined step size in the direction from $f(\mathcal{V}_{\mathrm{n}\mathrm{e}\mathrm{a}\mathrm{r}}.c)$ to $T_{\mathrm{r}\mathrm{a}\mathrm{n}\mathrm{d}}$ . InterpolateSE3Traj() interpolates a collision-free object S E(3) trajectory $\mathcal{P}_{\mathrm{S}\mathrm{E}3}$ connecting $f(\mathcal{V}_{\mathrm{n}\mathrm{e}\mathrm{a}\mathrm{r}}.c)$ and $T_{\mathrm{n}\mathrm{e}\mathrm{w}}$ , then ComputeCTraj() is called to compute a closed-chain composite trajectory $\mathcal{P}_{\mathrm{c}\mathrm{o}\mathrm{m}\mathrm{p}\mathrm{o}\mathrm{s}\mathrm{i}\mathrm{t}\mathrm{e}}$ tracking $\mathcal{P}_{\mathrm{S}\mathrm{E}3}$ . When the end of $\mathcal{P}_{\mathrm{c}\mathrm{o}\mathrm{m}\mathrm{p}\mathrm{o}\mathrm{s}\mathrm{i}\mathrm{t}\mathrm{e}}$ needs regrasping, the regrasping configuration is assigned to $\mathcal{V}_{\mathrm{n}\mathrm{e}\mathrm{w}}.$ Finally, $\mathcal{V}_{\mathrm{n}\mathrm{e}\mathrm{w}}$ is added to the tree $\mathcal{T}_{\mathrm{f}}.$

• • Connect() This function tries to connect the two trees via $\mathcal{V}_{\mathrm{n}\mathrm{e}\mathrm{w}}$ and returns the connected trajectory $\mathcal{P}_{\mathrm{c}\mathrm{o}\mathrm{n}\mathrm{n}\mathrm{e}\mathrm{c}\mathrm{t}}$ . The procedure is similar with the function Extend() and shown in Algorithm 3. The difference is that the extend goal is the given node instead of a new sampled one.

• • InterpolateSE3Traj() This function computes a smooth SE(3) trajectory connecting the object configuration $f(\mathcal{V}_{\mathrm{n}\mathrm{e}\mathrm{m}}.c)$ and $T_{\mathrm{n}\mathrm{e}\mathrm{w}}$ . Details are illustrated in Section 5.1.

• • ComputeCTraj(): As shown in Algorithm 4, this function computes the closed-chain composite trajectory $\mathcal{P}_{\mathrm{c}\mathrm{o}\mathrm{m}\mathrm{p}\mathrm{o}\mathrm{s}\mathrm{i}\mathrm{t}\mathrm{e}}$ to track $\mathcal{P}_{\mathrm{S}\mathrm{E}3}$ . DifferentialIK() (Section 5.2) is called to compute the IK solutions for dual redundant robots following $\mathcal{P}_{\mathrm{S}\mathrm{E}3}$ . When the IK solution fails at certain $T_{\mathrm{o}\mathrm{b}\mathrm{j}},$ GetRegraspConfig() is used to compute the valid regrasping configuration. The final output of ComputeCTraj() will be the trajectory starting from $\mathcal{V}_{\mathrm{s}\mathrm{t}\mathrm{a}\mathrm{r}\mathrm{t}}$ and following longest possible along $\mathcal{P}_{\mathrm{S}\mathrm{E}3}.$

4.3. Regrasping motion planning

When the first stage successfully connects $\mathcal{T}_{\mathrm{f}}$ and $\mathcal{T}_{\mathrm{b}}$ , and returns the trajectory $\mathcal{P}_{\mathrm{c}\mathrm{o}\mathrm{n}\mathrm{n}\mathrm{e}\mathrm{c}\mathrm{t}}$ , the global path is found. At this point, the algorithm turns to plan the regrasping motion for the nodes with regrasping requests. The whole regrasping process is divided into three stages: a). Two robots transfer the object from the current configuration $T_{\mathrm{c}\mathrm{u}\mathrm{r}}$ to the placement configuration ${T}_{\mathrm{p}\mathrm{l}\mathrm{a}\mathrm{c}\mathrm{e}}.\ \mathrm{b}$ ). The robot that needs regrasping performs the regrasping motion. c). Two robots move the object back to $T_{\mathrm{c}\mathrm{u}\mathrm{r}}$ . The pseudo-code is shown in Algorithm 5. Some key functions are briefly described as follows.

• • ExtractGlobalVertices(): Starting from the intersection node of $\mathcal{T}_{\mathrm{f}}$ and $\mathcal{T}_{\mathrm{b}}$ , this function extracts the parent node of the node until $\mathcal{V}_{\mathrm{s}\mathrm{t}\mathrm{a}\mathrm{r}\mathrm{t}}$ and $\mathcal{V}_{\mathrm{g}\mathrm{o}\mathrm{a}1}$ , and finally returns all nodes in the global path.

• • SamplePlacementConfig(): This function samples a valid placement configuration $T_{\mathrm{p}\mathrm{l}\mathrm{a}\mathrm{c}\mathrm{e}}$ that close to the object configuration $f(\mathcal{V}_{C}),$ which depends on the environment and is similar to the one in [Reference Xian, Lertkultanon and Pham11].

• • ComputeCTraj(): In stage a), this function computes the closed-chain trajectory $\mathcal{P}_{\mathrm{b}\mathrm{e}\mathrm{f}\mathrm{o}\mathrm{r}\mathrm{e}}$ from current configuration $f(\mathcal{V}_{C})$ to $T_{\mathrm{p}\mathrm{l}\mathrm{a}\mathrm{c}\mathrm{e}}$ . The grasping poses are set to the ones corresponding to the current node’s parent node. In stage c), the grasping poses are same with the current node (has been changed by GetRegraspConfig()). The returned trajectory moves the object from $f(\mathcal{V}_{C})$ to $T_{\mathrm{p}\mathrm{l}\mathrm{a}\mathrm{c}\mathrm{e}},$ then the order of the path points is reversed to get the correct trajectory (from $T_{\mathrm{p}\mathrm{l}\mathrm{a}\mathrm{c}\mathrm{e}}$ to $f(\mathcal{V}_{C})$ ).

• • GetRegraspConfig(): This function is the key of integrating both IK-switch and Grasp-switch into a unified regrasping framework. As shown in Algorithm alg66, when regrasping is needed, it first tries to compute the regrasping configuration using IK-switch (with the grasping poses unchanged), then the grasping pose is changed to each one in Grasplist in turn. The configuration farthest from the joint position limits is selected. All the configurations corresponding to IK-switch and Grasp-switch are compared and the one closest to the original configuration is chosen as the regrasping configuration. Finally, the regrasping configuration, the regrasping type, and the grasping pose index are returned.

• • BiRRT() In stage c), this function plans the regrasping path. The start point is the robot configuration the end of $\mathcal{P}_{\mathrm{b}\mathrm{e}\mathrm{f}\mathrm{o}\mathrm{r}\mathrm{e}}$ , and the goal is the robot configuration corresponding to the start of $\mathcal{P}_{\mathrm{a}\mathrm{f}\mathrm{t}\mathrm{e}\mathrm{r}}.$

5.1. Trajectory interpolation in SE(3)

The rotational motion and translation motion are decoupled in the interpolation. For the rotation part, for two rotation matrices $R_{\mathrm{o}},R_{f}\in SO(3)$ , the corresponding angular velocity vectors $\omega_{0},\omega_{f}\in \mathbb{R}^{3}$ and the angular acceleration vectors $\dot{\omega}_{\mathrm{o}},\dot{\omega}_{f}\in \mathbb{R}^{3}$ , the smooth trajectory $[R(t)]_{t\in[\mathrm{o},t_{f}]}$ in SO(3) connecting $R_{\mathrm{o}}$ and $R_{f}$ is computed through quintic polynomial interpolation for the axis-angle

(7) \begin{align}R(t)=R_{0}e^{[r]},r=\sum\limits_{i=0}^5 a_i t^i\end{align}

where $a_{0}, a_{1}, a_{2}, a_{3}, a_{4}, a_{5}\in \mathbb{R}^{3}$ are constant vectors and can be computed through the following boundary conditions

(8) \begin{align}r_{0}&=a_{0}=0_{3\times 1} \nonumber\\[3pt]r_{f}&=a_{0}+a_{1}t_{f}+a_{2}t_{f}^{2}+a_{3}t_{f}^{3}+a_{4}t_{f}^{4}+a_{5}t_{f}^{5}\nonumber\\[3pt]\dot{r}_{0}&=a_{1}=\omega_{0} \nonumber\\[3pt]\dot{r}_{f}&=a_{1}+2a_{2}t_{f}+3a_{3}t_{f}^{2}+4a_{4}t_{f}^{3}+5a_{5}t_{f}^{4}=\varLambda^{-1}(r_{f})\omega_{f}\nonumber\\[3pt]\ddot{r}_{0}&=2a_{2}=\dot{\omega}_{0}\nonumber\\[3pt]\ddot{r}_{f}&=2a_{2}+6a_{3}t_{f}+12a_{4}t_{f}^{2}+20a_{5}t_{f}^{3}=\varLambda^{-1}(r_{f})(\dot{\omega}_{f}-\varOmega(r_{f},\dot{r}_{f}))\end{align}

\begin{align} \nonumber\end{align}

The exponential coordinates $r_{f}$ , the matrix $\Lambda$ , and the vector $\Omega$ can be computed as follows

(9) \begin{align}[r_{f}]&=\log(R_{0}^{T}R_{f})\end{align}

(10) \begin{align}\varLambda(r)&=I-\frac{1-\cos||r||}{||r||^{2}}[r]+\frac{||r||-\sin||r||}{||r||^{3}}[r]^{2}\end{align}

(11) \begin{align}\rm \Omega(r,\dot{r})&=\frac{\lVert r\rVert-\sin\lVert r\rVert}{\lVert r \rVert^{3}}\dot{r}\times(r\times \dot{r})-\frac{2\cos\lVert r \rVert+ \lVert r \rVert\sin\lVert r\rVert-2}{\lVert r \rVert^{4}}r^{\top}\dot{r}(r\times \dot{r}) \nonumber \\[3pt]&\quad +\frac{3\sin\lVert r\rVert-\lVert r\rVert\cos\lVert r \rVert-2\lVert r\rVert}{\lVert r\Vert^{5}}r^{\top}r(r\times(r\times \dot{r}))\end{align}

For the translation part, given the translation vectors $p_{0}, p_{f}\in \mathbb{R}^{3}$ , the linear velocity vectors $v_{0},v_{f}\in\mathbb{R}^{3}$ and the linear acceleration vectors $\dot{v}_{\mathrm{o}},\dot{v}_{f}\in \mathbb{R}^{3}$ , a quintic polynomial is used to interpolate the position trajectory $[p(t)]_{t\in[0,t_{f}]}$ between $p_{0}$ and $p_{f}$ :

(12) \begin{align}p(t)_{t\in[0,t_{f}]}= \sum_{i=0}^{5}k_{i}t^{i}\end{align}

Similar with the rotation part, the coefficients $k_{5}, k_{4}, k_{3}, k_{2}, k_{1}, k_{\mathrm{o}}$ can be computed by the boundary condition $p(t)=p_{t},v(t)=v_{t},\dot{v}(t)=\dot{v}_{t},t=0,t_{f}$ . As shown in Section 6.2, such interpolation method has many advantages. First, decoupling the rotational motion from the translational motion yields a straight-line motion in Cartesian space, where the frame origin follows a straight line while the axis of rotation is constant in the body frame. Second, the interpolated trajectory is short and smooth, and the start and final velocity and acceleration conditions can be considered. Finally, the first and second derivatives are smooth and easy to compute.

5.2. Quadratic program-based redundant resolution

To minimize the input power of redundant robots in the large object manipulation task, the joint torque minimization criteria are used for the redundancy resolution. And to remedy the joint torque instability and divergence phenomenon exists in the INTM scheme, a Dual-MVN-INTM different-level minimization scheme is proposed in this paper. As shown in Fig. 4, MVN scheme and INTM scheme are integrated via a weighting factor in the form of a QP for each robot, then the two QPs are unified into a standard QP and formulated to an S-DNN model for fast solving. Note that the proposed scheme is solved at acceleration level with joint position, velocity, and acceleration limits considered.

Figure 4. System structure of Dual-MVN-INTM scheme for the dual-redundant robot

Given an end-effector trajectory in terms of position, orientation and velocity, acceleration twist, and the joint acceleration $\ddot{\theta}(t)$ computed through Dual-MVN-INTM scheme, suppose the initial joint position and velocity $\theta(0),\dot{\theta}(0)$ is known, joint positions can then be computed by Euler integration:

(13) \begin{align}\theta(t_{k+1})&=\theta(t_{k})+\dot{\theta}(t_{k})\varDelta t\end{align}

(14) \begin{align}\dot{\theta}(t_{k+1})&=\dot{\theta}(t_{k})+\ddot{\theta}(t_{k})\varDelta t\end{align}

5.2.1 QP formulation and unification

For an $\mathrm{n}\ \mathrm{D}\mathrm{o}\mathrm{F}$ robot, the relationship between the end-effector velocity twist $\mathcal{V}_{b}=[\omega_{b},\nu_{b}]\in \mathbb{R}^{6}$ in the end-effector (or body) frame and the joint velocity $\dot{\theta}\in \mathbb{R}^{n}$ can be expressed as

(15) \begin{align}\mathcal{V}_{b}=J_{b}(\theta)\dot{\theta}\end{align}

where $J_{b}(\theta)\in \mathbb{R}^{m\times n}$ is the Jacobian expressed in end-effector frame. Note that $\mathcal{V}, J$ are used to replace $\mathcal{V}_{b}, J_{b}$ for simplicity in remainder of this paper.

Take the derivative of (15) with respect to time t:

(16) \begin{align}J(\theta)\ddot{\theta}=\dot{\mathcal{V}}-\dot{J}(\theta)\dot{\theta}=\dot{\mathcal{V}}_{\tau}\end{align}

Without considering the friction force at the joint, the dynamic equation of the n DOF robot is

(17) \begin{align}\tau = M(\theta)\ddot{\theta}+c(\theta,\dot{\theta})+g(\theta)\end{align}

where $M(\theta)\in \mathbb{R}^{n\times n}$ is the symmetric positive-definite mass matrix, $c(\theta,\dot{\theta})\in \mathbb{R}^{n}$ is a vector of Coriolis and centripetal torques, and $g(\theta)\in \mathbb{R}^{n}$ is a vector of gravitational torques.

The bi-criteria scheme that combined MVN and INTM for a single robot can be formulated as:

(18) \begin{gather}\underset{\dot{\theta},\tau}{\min}\frac{1}{2}\{\alpha||\dot{\theta}||_{2}^{2}+(1-\alpha)||\tau||_{\infty}^{2}\}\nonumber\\\text{s}.\text{t}.\ J(\theta)\ddot{\theta}=\dot{\mathcal{V}}_{\tau} \nonumber\\\tau=M(\theta)\ddot{\theta}+c(\theta,\dot{\theta})+g(\theta) \nonumber\\\theta^{-}\leqslant\theta\leqslant\theta^{+} \nonumber\\\dot{\theta}^{-}\leqslant\dot{\theta}\leqslant\dot{\theta}^{+} \nonumber\\\ddot{\theta}^-\leqslant\ddot{\theta}\leqslant\ddot{\theta}^{+}\end{gather}

where $\Vert\cdot\Vert_{2}$ and $\Vert\cdot\Vert_{\infty}$ represent the two-norm and infinity-norm of a vector. $\alpha\in[0,\ 1]$ is the weighting factor to balance the two schemes. $\theta^{+},\dot{\theta}^{+},\ddot{\theta}^{+}\in \mathbb{R}^{n}$ and $\theta^{-},\dot{\theta}^{-},\ddot{\theta}^{-}\in \mathbb{R}^{n}$ denote the upper and lower limits of joint position, velocity, and acceleration, respectively.

The MVN-INTM scheme is built on different levels (velocity and torque). To solve the optimization problem at the same level, the MVN scheme and INTM scheme are unified at the acceleration level. More precisely, the velocity-level minimization $\mathrm{o}\mathrm{f}||\dot{\theta}||_{2}^{2}/2$ is approximately equivalent to the minimization of $\ddot{\theta}^{\mathrm{T}}\ddot{\theta}/2+\lambda\dot{\theta}^{T}\ddot{\theta}$ [Reference Cai17], where $\lambda>0\in R$ should be set as large as the robot system would permit. The torque-level minimization of $(1-\alpha)||\tau||_{\infty}^{2}/2$ is equivalent to the minimization of $(1-\alpha)s^{2}/2$ [Reference Guo15], where $s=\Vert_{\mathcal{T}}\Vert_{\infty}$ , such that

(19) \begin{align}\left[\begin{matrix}M & -1_{7\times 1}\\ \\[-7pt]-M & -1_{7\times 1}\end{matrix}\right]\left[\begin{array}{l}\ddot{\theta}\\ \\[-7pt]s\\ \end{array}\right]\leqslant\left[\begin{array}{c}-c-g\\ \\[-7pt]c+g\end{array}\right]\end{align}

To solve the MVN-INTM scheme at acceleration level, the joint position and velocity limits should also be converted in the form of joint acceleration [Reference Guo38]:

(20) \begin{align}\eta^{-}\leqslant\ddot{\theta}\leqslant\eta^{+}\end{align}

where the $i\mathrm{t}\mathrm{h}$ element of the new upper and lower bound constraints are

\begin{align*}\eta_{i}^{-}&=\max\{\kappa_{p}(\mu\theta_{i}^{-}-\theta_{i}),\kappa_{v}(\dot{\theta}_{i}^{-}-\dot{\theta}_{i}),\ddot{\theta}_{i}^{-}\}\\[3pt]\eta_{i}^{+}&=\min\{\kappa_{p}(\mu\theta_{i}^{+}-\theta_{i}),\ \kappa_{v}(\dot{\theta}_{i}^{+}-\dot{\theta}_{i}),\ddot{\theta}_{i}^{+}\}, 0 <\mu<1,\ \kappa_{p}>0,\ \kappa_{v}>0\end{align*}

When the joints enter into the dangerous zone $[\theta^{-},\mu\theta^{-}]$ and $[\mu\theta^{+},\theta^{+}]$ , the adjustment coefcient $\kappa_{p}$ prevents the joints from getting closer to the limits. Coefcients $\kappa_{p}>0, \kappa_{v}>0$ determine the deceleration magnitude and should be selected such that the feasible region of $\ddot{\theta}$ converted by the joint position and velocity limits is not smaller than the original one. However, without elaborate selection of the coefficients in practice, such conversion may encounter the phenomenon that the converted lower bounds are beyond the converted upper bounds. Therefore, modifications have been made to the aforementioned conversion in this paper to make it more robust for different robots:

(21) \begin{align}\eta_{i}^{-}=\min\{\max\{\kappa_{p}(\mu\theta_{i}^{-}-\theta_{i}),\\kappa_{v}(\dot{\theta}_{i}^{-}-\dot{\theta}_{i}),\ddot{\theta}_{i}^{-}\}, \ddot{\theta}_{i}^{+} \}\end{align}

(22) \begin{align}\eta_{i}^{+}=\max\{\min\{\kappa_{p}(\mu\theta_{i}^{+}-\theta_{i}),\\kappa_{v}(\dot{\theta}_{i}^{+}-\dot{\theta}_{i}),\ddot{\theta}_{i}^{+}\},\ddot{\theta}_{i}^{-}\}\end{align}

With the conversions made above, by defining the decision variable $x=[\ddot{\theta}^{T},s]^{T}\in R^{n+1}$ , the MVN-INTM scheme for a single robot can be expressed as the following QP :

(23) \begin{align}\begin{array}{c}\underset{x}{\min}\,\,x^{T}Qx/2+p^{T}x \\\text{s}.\text{t}.\ Ax=b \\Cx\leqslant d \\x^{-}\leqslant x\leqslant x^{+}\end{array}\end{align}

where the coefficient matrices and vector are defined as

\begin{align*}Q=\left[\begin{matrix}\alpha I & 0 & \\ \\[-7pt]0 & (1- & \alpha)\end{matrix}\right]\in R^{(n+1)\times(n+1)},\ p=\left[\begin{array}{c}\alpha\lambda\dot{\theta}\\ \\[-7pt]0\end{array}\right]\in R^{n+1}\\[3pt]A=[J\ 0]\in R^{m\times(n+1)},\ b=\dot{\mathcal{V}}_{\tau}\in R^{m}\\[3pt]C=\left[\begin{matrix}M & -1_{n\times 1}\\ \\[-7pt]-M & -1_{n\times 1}\end{matrix}\right]\in R^{2n\times(n+1)},\ d=\left[\begin{array}{c}-c-g\\c+g\end{array}\right]\in R^{2n}\end{align*}

$x^{-}=[\eta^{-},\ 0]^{T}\in R^{n+1},x^{+}=[\eta^{+},w]^{T}\in R^{n+1}$ , here w is a large constant used to replace $+\infty.$ In order to improve the computational efficiency, by defining the decision variable $u=[x_{L},x_{R}]^{T}$ , the two QPs of left and right robot can be unified into a standard $\mathrm{Q}\mathrm{P}$ :

(24) \begin{align}\begin{array}{c}\underset{u}{\min}\,\, u^{T}Wu/2+P^{T}u \\\text{s}.\text{t}.\ Eu=f \\Gu\leqslant h \\u^{-}\leqslant u\leqslant u^{+}\end{array}\end{align}

where the coefficient matrices and vector are defined as

\begin{align*}W&=\left[\begin{matrix}Q_{L} & 0\\ \\[-7pt]0 & Q_{R}\end{matrix}\right]\in R^{2(n+1)\times 2(n+1)},\ P=\left[\begin{array}{l}p_{L}\\ \\[-7pt]p_{R}\end{array}\right]\in R^{2(n+1)}\\[3pt]E&=\left[\begin{matrix}A_{L} & 0\\ \\[-7pt]0 & A_{R}\end{matrix}\right]\in R^{2m\times 2(n+1)},\ f=\left[\begin{array}{l}b_{L}\\ \\[-7pt]b_{R}\end{array}\right]\in R^{2m}\ G=\left[\begin{matrix}C_{L} & 0\\ \\[-7pt]0 & C_{R}\end{matrix}\right]\in R^{4n\times 2(n+1)},\ h=\left[\begin{array}{c}d_{L}\\ \\[-7pt]d_{R}\end{array}\right]\in R^{4n}\\[3pt]u^{-}&=\left[\begin{array}{l}x_{L}^{-}\\ \\[-7pt]x_{R}^{-}\\\end{array}\right]\in R^{2(n+1)},\ u^{+}=\left[\begin{array}{l}x_{L}^{+}\\ \\[-7pt]x_{R}^{+}\end{array}\right]\in R^{2(n+1)}\end{align*}

coefficient matrices and variables $Q_{L/R}, p_{L/R}, A_{L/R}, b_{L/R}, C_{L/R}, d_{L/R}\ x_{L/R}^{-}, x_{L/R}^{+}$ are the same as those defined in (23), and the subscript L, R correspond to left and right robot, respectively.

5.2.2 QP solver based on simplified dual neural network

At this point, the IK problem of dual-redundant-robot is transformed into a unified time-varying QP problem. The matrix W is positive definite, so the objective function in (24) is strictly convex. And the feasible region of the constraints in (24) is a closed convex set. Therefore, the optimal solution of the QP problem (24) is unique and satisfies the KKT conditions [Reference Boyd, Vandenberghe and Faybusovich48]. Since traditional numerical QP solver is inefficient for solving of the large-scale QP-formed IK like (24) [Reference Hassan, El-Habrouk and Deghedie13], many researchers use the parallel computational RNNs instead for real-time computations [Reference Hassan, El-Habrouk and Deghedie13–Reference Zhang and Li19]. In this paper, in order to solve our Dual-MVN-INTM scheme in real time, we formulate (24) into an architecture-efficient RNN model S-DNN [Reference Liu and Wang18], which further reduces the compute complexity than DNN while preserving the desirable convergence property.

By defining $K:=[G,I]^{T}\in R^{(6n+2)\times(2n+2)},\zeta^{-}= [-\infty,\ u^{-}]^{T}\in R^{6n+2},\zeta^{+}=[h,u^{+}]^{T}\in R^{6n+2}$ the inequality constraints in (24) can be reformulated as

(25) \begin{align}\zeta^{-}\leqslant Ku\leqslant\zeta^{+}\end{align}

Consider (24) as the primal problem P and D as its dual problem. The Lagrangian function $\mathrm{o}\mathrm{f}\ P$ is

(26) \begin{align}L(u,\ \lambda,\nu)=u^{T}Wu/2+g^{T}u+\lambda_{1}^{T}(\zeta^{-}-Ku)+\lambda_{2}^{T}(Ku-\zeta^{+})+\nu^T\ (Eu-f)\end{align}

where $\lambda_{1}, \lambda_{2}\in \mathbb{R}^{p}$ is the dual variables for the inequality constraints, $\nu\in \mathbb{R}^{m}$ is the dual variable for the equality constraints. According to KKT conditions, if $u^{*}$ is the optimal solution of P, and $(\lambda_{1}^{*},\ \lambda_{2}^{*},\nu^{*})$ is the optimal solution $\mathrm{o}\mathrm{f}\ D$ , then $(u^{*},\ \lambda_{1}^{*},\ \lambda_{2}^{*},\nu^{*})$ should $\mathrm{s}\mathrm{a}\mathrm{t}\mathrm{i}\mathrm{s}$ fy

(27) \begin{gather}Wu+P+E^{T}\nu-K^{T}\lambda_{1}+K^{T}\lambda_{2}=0\end{gather}

(28) \begin{gather}Eu=f\end{gather}

(29) \begin{gather}\zeta^{-}\leqslant Ku\leqslant\zeta^{+}\end{gather}

(30) \begin{gather}\lambda_{1},\ \lambda_{2}\geqslant 0\end{gather}

(31) \begin{gather}\lambda_{1}^{T}(\zeta^{-}-Ku)=\lambda_{2}^{T}(Ku-\zeta^{+})=0\end{gather}

Define $\psi=\lambda_{1}-\lambda_{2}$ , then the constraints (29) $\sim$ (31) are equivalent to

(32) \begin{align}Ku=g(Ku-\psi)\end{align}

where g $_{i}(v_{i})=\left\{\begin{array}{l}\zeta_{i}^{-}\quad \textrm{if}\ v_{i}<\zeta_{i}^{-}\\v_{i}\quad \textrm{if}\ \zeta_{i}^{-}\leq v_{i} \leq \zeta_{i}^{+},\qquad \textrm{i}=1,...,p\\\zeta_{i}^{+}\quad \textrm{if}\ v_{i}<\zeta_{i}^{+}\end{array}\right.$

Since W is invertible, (27) can be reformulated as

(33) \begin{align}u=-W^{-1} (P+E^{T} \nu -K^{T}\psi)\end{align}

Substituting (33) into (28) yields

(34) \begin{align}EW^{-1}(P+E^T \nu -K^{T}\psi)=-f\end{align}

Because $EW^{-1}E^{T}$ is invertible, $\nu$ can be expressed by $\psi$ :

(35) \begin{align}\nu=-(EW^{-1}E^{T})^{-1}(f+EW^{-1}P-EW^{-1}K^{T}\psi)\end{align}

Substituting (35) into (33) yields

(36) \begin{align}u&=(-W^{-1}E^{T}(EW^{-1}E^{T})^{-1}EW^{-1}+W^{-1})(K^{T}\psi-P) \nonumber\\[3pt]&\quad +W^{-1}E^{T}(EW^{-1}E^{T})^{-1}f\end{align}

Since W is symmetric, by defining $T=W^{-1}E^{T}$ and $J=T(T^{T}WT)^{-1},$ (36) can be reformulated as

(37) \begin{align}u=(-JT^{T}+W^{-1})(K^{T}\psi-P)+Jf\end{align}

Therefore, the optimal solution to the primal problem P is equivalent to the solution to (32) and (37), which yield a S-DNN model [Reference Liu and Wang18] whose governing differential and output equations are

(38) \begin{align}\dot{\psi}&=\varepsilon(K(JT^{T}-W^{-1})(K^{T}\psi-P)-K^{T}Jf) \nonumber\\[3pt]&\quad +\varepsilon g(K(-JT^{T}+W^{-1})(K^{T}\psi-P)+K^{T}Jf-\psi) \nonumber\\[3pt]u&=(-JT^{T}+W^{-1})(K^{T}\psi-P)+Jf\end{align}

where $\epsilon$ is the positive scaling parameter to control the convergence rate of the neural network.

The S-DNN defined by (38) is composed of only one layer of $6n+2$ neurons (number of inequality constraints), which is much fewer than $6n+2m+2$ neurons of the DNN, $18n+2m+8$ neurons of LNN, and $10n+2m+2$ neurons of PDNN for solving the same QP (24). As shown in the experiments, the formulated S-DNN model converges within less than $5\times 10^{-6}\mathrm{s}$ , which is very fast for computing the required IK solutions.

6.1. Experiment set-up and task description

As shown in Fig. 2, the experiment platform consists of two 7 DOF robots and a manipulated object. The motion planner is implemented in Python and the QP-based IK solver was integrated as subfunctions. The collision checking is implemented using FCL [Reference Pan, Chitta and Manocha49]. All tasks were simulated on OpenRAVE [Reference Diankov29] and run on a PC platform with a 3. $6\mathrm{G}\mathrm{H}\mathrm{z}$ Intel Core i7-9700K CPU. Two different kinds of robots- 7 $\mathrm{D}\mathrm{o}\mathrm{F}$ Franka Emika Panda (Panda) and 7 $\mathrm{D}\mathrm{o}\mathrm{F}$ Kinova Jaco2 (Jaco2) are used. Each robot is equipped with a parallel gripper, which can provide a clamping force ranging from $40\mathrm{N}$ to $140\mathrm{N}$ . The distance between robot bases is set to 1.6 m. Three objects, including a 40 $\mathrm{c}\mathrm{m}\times 40\mathrm{c}\mathrm{m}\times 37$ cm square table, a 56 $\mathrm{c}\mathrm{m}\times 44\mathrm{c}\mathrm{m}\times 50$ cm stool, and a 43 $\mathrm{c}\mathrm{m}\times 22\mathrm{c}\mathrm{m}\times 43$ cm book shelf, are manipulated in three different tasks.

In Task 1, two Panda robots are required to transfer and turn the square table upside down (Fig. 11). In Task 2, the stool is moved and flipped back and forth by two Panda robots (Fig. 12). Task 3 is similar with Taskl, where two Jaco2 robots are required to transfer and reverse the book shelf (Fig. 13). Our planner is used in all these tasks to plan smooth global composite trajectories. The initial configurations and corresponding target configurations are essentially mutually disconnected, which makes these three tasks difficult and unsolvable for local planners.

6.2. SE(3) trajectory interpolation

Take a segment of the closed-chain motion in Task 1 as an example, we first show the process and result of SE (3) trajectory interpolation. $T_{\mathrm{o}\mathrm{b}\mathrm{j}0}, T_{\mathrm{o}\mathrm{b}\mathrm{j}1}\in SE(3)$ are the start and the end configurations of this segment, and $T_{\mathrm{G}\mathrm{r}\mathrm{a}\mathrm{s}\mathrm{p}_{l}}, T_{\mathrm{G}\mathrm{r}\mathrm{a}\mathrm{s}\mathrm{p}_{r}}$ are the corresponding grasping poses of the left and the right robot. For the left robot, the start configuration $T_{l0}=(R_{l0},p_{l0})\in SE(3)$ and the end configuration $T_{l1}=(R_{l1},p_{l1})\in SE(3)$ of the end-effector expressed in the coordinate frame $\{L\rangle$ can be computed through the closed-chain constraint. The time duration of this trajectory $t_{f}=1.5s$ . The start and end velocity twists $\mathcal{V}_{0},\ \mathcal{V}_{1}\in \mathbb{R}^{6}$ , and the start and end acceleration twist $\dot{\mathcal{V}}_{0},\dot{\mathcal{V}}_{1}\in \mathbb{R}^{6}$ expressed in end-effector frame are set to zero:

\begin{align*}\mathcal{V}_{0}=(\omega_{0},v_{0})=0_{6\times1},\mathcal{V}_{1}=(\omega_{1},v_{1})=0_{6\times 1}\\[3pt]\dot{\mathcal{V}}_{0}=(\dot{\omega}_{0},\dot{v}_{0})=0_{6\times1},\dot{\mathcal{V}}_{1}=(\dot{\omega}_{1},\dot{v}_{1})=0_{6\times 1}\end{align*}

We interpolate the SO(3) trajectory between $R_{0}$ and $R_{1}$ according to (7)

\begin{align*}R_{l}(t)=R_{l0}e^{[a_{5}t^{5}+a_{4}t^{4}+a_{3}t^{3}+a_{2}t^{2}+a_1t+a_0]},t\in[0,t_{f}]\end{align*}

Then we interpolate the translational trajectory between $p_{0}$ and $p_{1}$ according to (12)

\begin{align*}p_{l}(t)=k_{5}t^{5}+k_{4}t^{4}+k_{3}t^{3}+k_{2}t^{2}+k_{1}t+k_{0},t\in[0,t_{f}]\end{align*}

So far, the SE(3) trajectory $(R_{l}(t),p_{l}(t)),t\in[0,t_{f}]$ of the left robot end-effector is ready, and the corresponding velocity and acceleration twists $\mathcal{V}_{l}(t),\dot{\mathcal{V}}_{l}(t)$ expressed in the end-effector coordinate frame can be computed as follows:

\begin{align*}\mathcal{V}_{l}(t)&=\left[\begin{array}{c}w_{l}(t)\\v_{l}(t)\end{array}\right]=\left[\begin{array}{c}\varLambda(r(t))\dot{r}(t)\\R^{T}(t)\dot{p}(t)\end{array}\right]\\[3pt]\dot{\mathcal{V}}_{l}(t)&=\left[\begin{array}{c}\dot{w}_{l}(t)\\\dot{v}_{l}(t)\end{array}\right]=\left[\begin{array}{c}\varLambda(r(t))\ddot{r}(t)+\varOmega(r,\dot{r})\\\dot{R}^{T}(t)\dot{p}(t)+R^{T}(t)\ddot{p}(t)\end{array}\right]\end{align*}

where $\Lambda(r(t))$ and $\Omega(r,\dot{{r}})$ can be computed via (10) and (11), respectively.

The SE(3) trajectory and the velocity twists of the right robot can be computed through the same process. The final SE(3) trajectory of both end-effectors and the velocity and acceleration twist profiles are shown in Fig. 5, where the rotational trajectory is shown in the form of axis-angle r(t). Note that the segment shown in the figure is extracted for illustration convenience, while the whole closed-chain trajectory consists of many motion segments like this. As can be seen form Fig. 5, the interpolated method provides a straight-line motion in Cartesian space and the velocity and acceleration curves are smooth, which make the interpolated trajectory ideal for the bimanual manipulation task discussed in this paper.

Figure 5. Interpolated SE(3) trajectories for two robots. (a)-(d) are for the left robot and (e)-(h) are for the right robot. (a) and (e) are end-effector motions in Cartesian space. (b) and (f) are SE(3) configuration profiles, where the orientations are denoted as axis angles. (c) and (g) are velocity profiles. (d) and (h) are acceleration profiles

6.3. Joint space trajectory conversion and comparing

In this section, the trajectory of the end-effector in task space is converted to joint space using different IK algorithms for comparing and verifying the effectiveness of the proposed Dual-MVN-INTM redundancy resolution scheme. The initial state of the left and right robot is

\begin{align*}\theta_{L}(0)&=[-0.26,\ -1.02,1.50,\ -1.10,0.72,2.82,1.16]^{T}\\text{rad}\\[3pt]\theta_{R}(0)&=[-1.51,\ -0.56,\ -0.16,\ -2.31,\ -0.38,3.34,1.08]^{T}\ \text{rad}\\[3pt]\dot{\theta}_{L}(0)&=\dot{\theta}_{R}(0)=[0,0,0,0,0,0,0]^{T}\\text{rad/s}\end{align*}

The kinematic and dynamic parameters, joint limits of Panda robot can be obtained from [Reference Gaz, Cognetti, Oliva, Giordano and Luca50]. Moreover, the design parameters used in INTM scheme and Dual-MVN-INTM $\lambda=30, \mu=0.9,$ $\kappa_{p}=\kappa_{v}=15.$

In order to compare with the proposed Dual-MVN-INTM scheme, the pseudoinverse-based method [Reference Siciliano, Sciavicco, Villani and Oriolo12] is used first for the conversion: $\dot{\theta}=J^{+}\mathcal{V}+k_{0}(I_{n}-J^{\dagger}J)\partial w(\theta)/\partial\theta$ , where the secondary objective function $\omega(\theta)$ is chosen as $\omega( \theta)=-1/2n\sum_{i=1}^{n}((\theta_{i}-\overline{\theta}_{i})/(\theta_{i}^{+}-\theta_{i}^{-}))^{2}$ , which measures the distance from joint position limits and is frequently-used. By maximizing this distance, redundancy is exploited to keep the joint positions as close as possible to the middle value of the joint range $\overline{\theta}_{i}$ . The computed joint position, velocity, acceleration, and torque profiles are shown in Fig. 6. As can be seen, the joint positions are kept within the limits, and the joint variables are changed smoothly. However, such method only considers the joint position limits. While dragging the joint positions to the middle, the joint velocities, accelerations, or torques might exceed their limits. For example, for the left robot, at the time interval [0.4,0.6], the velocity of joint 7 exceeds the limit. And for the right robot, at time interval [0.4,0.7], the velocities of joint 1 and joint 3 exceed their limits, respectively, which is unacceptable in practice.

Figure 6. Dual Panda end-effectors tracking the interpolated straight-line trajectory synthesized by the pseudoinverse-based method. (a)-(d) are joint position, velocity, acceleration, torque profiles of the left robot. (e)-(h) are corresponding profiles of the right robot, respectively

Fig. 7(a) and Fig. 7(d) show the joint trajectories of dual Panda robots synthesized by INTM scheme [Reference Zhang40] $(\mathrm{i}.\mathrm{e}.\ \alpha=0)$ with joint position, velocity, acceleration limits considered. The corresponding joint velocity, acceleration, torque profiles of dual robots are shown in Fig. 7(b)–(d) and Fig. 7 $(\mathrm{f})-(\mathrm{h})$ , respectively. As seen from Fig. 7, the joint variables are kept within their limits by incorporating joint physical limits. However, some of the joint accelerations, joint velocities are so large that limits are reached, such as joint 5 of the left robot and joint 5, 7 of the right robot. And abrupt increase or decrease in these joint variables exhibited, which is because that INVM scheme minimizes the largest element of the joint velocity or acceleration vector in magnitude. This phenomenon is not desirable in practice and would cause damages to the robot. Furthermore, the final velocities of some joints are too large in the real robotic applications. Therefore, the INVM scheme is unsuitable in practical applications.

Figure 7. Dual Panda end-effectors tracking the interpolated straight-line trajectory synthesized by INTM scheme $(\alpha=0)$ . (a)-(d) are joint position, velocity, acceleration, torque profiles of the left robot. (e)-(h) are corresponding profiles of the right robot, respectively

Fig. 8(a) and Fig. 8(d) illustrates the joint trajectories of dual Panda robots synthesized by the proposed Dual-MVN-INTM scheme $(\alpha=0.6)$ with joint position, velocity, acceleration limits considered. The corresponding joint velocity, acceleration, and torque profiles of dual robots are shown in Fig. 8(b)–(d) and Fig. 8 $(\mathrm{f})-(\mathrm{h})$ , respectively. As seen from Fig. 8, the joint variables are all kept within the feasible regions, and the curves are quite smooth without abrupt changes or chattering phenomena, which illustrates the discontinuous problem occurred in the INTM scheme is remedied by the Dual-MVN-INTM scheme. And the joint velocities, accelerations, and torques are smaller comparing with INTM scheme (some variables in Fig. 7 are too large even reach the limits). To further verify the effectiveness of trajectory tracking, the translational and rotational tracking errors of the dual robots are shown in Fig. 9, where the rotational tracking errors are measured in the form of axis-angle, $e_{r}$ and $e_{p}$ represent the rotational and translational tracking error, respectively. As can be seen, the converted trajectories track the end-effectors trajectories well with the maximal tracking error less than $1.6\times 10^{-4}$ for the left robot and $1.2\times 10^{-4}$ for the right robot, respectively. In addition, Fig. 8(b) and Fig. 8(f) show the Dual-MVN-INTM scheme guarantee the joint velocity to approach zero at the end of the motion.

Figure 8. Dual Panda end-effectors tracking the interpolated straight-line trajectory synthesized by Dual-MVN-INTM scheme $(\alpha=0.6)$ . (a)–(d) are joint position, velocity, acceleration, torque profiles of the left robot. (e)–(h) are joint position, velocity, acceleration, torque profiles of the right robot, respectively

Figure 9. Tracking errors of dual Panda synthesized by Dual-MVN-INTM scheme $(\alpha=0.6)$ . (a). The tracking error of the left robot. (b). The tracking error of the right robot

Moreover, the final joint velocities and tracking errors with different $\alpha$ used in Dual-MVN-INTM scheme are reported in Tables 1 and 2. As shown in Table 1, the joint velocities at the end of the motion are very close to zero $(10^{-3}\mathrm{r}\mathrm{a}\mathrm{d}/\mathrm{s})$ , which is preferable in applications. Furthermore, the tracking errors shown in Table 2 are quite small $(10^{-4}\textrm{m})$ , which illustrates that the proposed scheme can track the end-eff ector trajectory precisely. Fig. 10 shows the joint energy cost $\tau^TM^{-1}\tau$ synthesized with different $\alpha$ . As can be seen, when Dual-MVN-INTM scheme $(\alpha\in(0,1))$ is utilized, the joint energy costs of both robots are much smaller than that of INTM scheme $(\alpha=0)$ . As mentioned before, the Dual-MVN-INTM scheme is solved by a simplified dual network in this paper, and the scaling parameter of the network is $\epsilon=3\times 10^{5}$ , which leads to a very fast convergence of the dual network and the equilibrium point is reached in less than $5\times 10^{-6}\mathrm{s}$ . In summary, the comparison results verify the effectiveness, accuracy, and high efficiency of the proposed Dual-MVN-INTM scheme used for dual-redundant-robot cooperatively tracking the end-effector trajectories in bimanual operation tasks.

Table I. Final joint velocities $(10^{-3}rad/s)$ with different $\alpha$ used in Dual-MVN-INTM scheme

Table II. Tracking errors $(10^{-4}{m})$ with different $\alpha$ used in Dual-MVN-INTM scheme

Figure 10. Energy costs $\tau^TM^{-1}\tau$ profiles of dual Panda synthesized by Dual-MVN-INTM scheme with different $\alpha.$ (a). The energy cost profiles of the left robot. (b) The energy cost profiles of the right robot

6.4. Planning results and comparing

In this section, we display and analyze the planning results for three tasks. As illustrated in Section 3, the key of solving the global motion planning of CKC systems is connecting the closed-chain motions through regrasping motions. Therefore, for comparing the performance of the proposed planner and the IK-switch planner in [Reference Xian, Lertkultanon and Pham11], contrast experiments are conducted by sending the same planning query of three tasks to two planners. Each planner runs 50 times to solve one task, and we set the allowed total number of regrasping moves for two robots as 6, considering the complexity of the tasks and quality of planned trajectory. Note that since the planner in [Reference Xian, Lertkultanon and Pham11] is designed for nonredundant robots, we replace the IKfast solver with our Dual-MVN-INTM solver for a fair comparison.

Table 3 reports the comparisons between our planner and the IK-switch planner for all three tasks in terms of the averages of planning success rate, planning time, and number of required regrasping moves.

Table III. Results of three manipulation tasks using two planners

From the results in Table 3, we can see that 1) with flexible grasp-switch and IK-switch connections, our planner achieves much better success rates than IK-switch planner (95%/75%). 2) Regrasping moves are essential for the success of global planning and also take most of the total planning time. IK-switch planner solely depends on regrasping moves between different IKs of a same grasping pose, which undoubtedly increases the computation time for finding a valid regrasping configuration and also the number of required regrasping moves. While in our planner, the regrasping moves have much more options, thus it is easier for our planner to find a valid regrasping configuration. As a result, our planer is $1.78\times(7.89\,\mathrm{s}/14.05\,\mathrm{s})$ faster than IK-switch planner with 56% (3/4.7) less regrasping moves needed. 3) The planning success rate and required time are task-dependent, involving robots, objects, and the start and goal configurations. Fig. 11, Fig. 12, and Fig. 13 display the snapshots of composite motions of Task 1, Task 2, and Task 3 that planned by our planner, where our planner is able to plan smooth composite trajectories for different robots and objects that include closed-chain motions and Grasp-switch or IK-switch moves, which demonstrates the great effectiveness and applicability of the proposed framework.

Figure 11. Snapshots of planned composite motion for Task 1 with Panda robots. $(\mathrm{a},\ \mathrm{b},\ \mathrm{d},\ \mathrm{e},\ \mathrm{h}{-}\mathrm{k},$ $\mathrm{m},\ \mathrm{n})$ : Closed-chain motions. $(\mathrm{c},\ \mathrm{l})$ : Grasp-switch motions. $(\mathrm{f},\ \mathrm{g})$ : IK-switch motions

Figure 12. Snapshots of planned composite motion for Task 2 with Panda robots. (a-d, f-i, k-n): Closed- chain motions. (e): IK-switch motion. (j): Grasp-switch motion

Figure 13. Snapshots of planned composite motion for Task 3 with Jaco2 robots. (a-e, e-g, j-l): Closed- chain motions. $(\mathrm{d},\ \mathrm{i})$ : IK-switch motions. (h): Grasp-switch motion

6.5. Real-world robot experiment

In order to further verify the effective of our method in real world, we conduct the robot experiment using two real 7 $\mathrm{D}\mathrm{o}\mathrm{F}$ Jaco2 robots to complete Task 3. The experiment setup is shown in Fig. 13, and the start and goal configurations are shown in Fig. 2 $(\mathrm{c})-(\mathrm{d})$ . Note that the rigid contact between the book shelf and grippers should be considered. Even though the tracking error of our Dual-MVN-INTM scheme is very small as shown in Table 2, the discrepancies of the simulation model and robot location errors could cause damages to the object and robots. Therefore, we past a syntactic foam on each fingertip to enhance the passive compliance and the grasping stability, as shown in Fig. 14. Such measure ensures that the experiment can carry out with an allowable pose error. We first send the plan query to our planner and use the hardware system to implement the planned composite trajectory. As shown in Fig. 15, our system executes the planned trajectory smoothly and completes the task successfully, which further demonstrate that our method is effective to solve the complex closed-chain manipulation task in real world.

Figure 14. Hardware system setup

Figure 15. Snapshots of Jaco2 robots completing Task 3 in real world. (a-c, e-i, k, l): Closed-chain motions. (d): Grasp-switch motion. (j): IK-switch motion