Inverse kinematics-based motion planning for dual-arm robot with orientation constraints

Abstract

This article proposes an efficient and probabilistic complete planning algorithm to address motion planning problem involving orientation constraints for decoupled dual-arm robots. The algorithm is to combine sampling-based planning method with analytical inverse kinematic calculation, which randomly samples constraint-satisfying configurations on the constraint manifold using the analytical inverse kinematic solver and incrementally connects them to the motion paths in joint space. As the analytical inverse kinematic solver is applied to calculate constraint-satisfying joint configurations, the proposed algorithm is characterized by its efficiency and accuracy. We have demonstrated the effectiveness of our approach on the Willow Garage’s PR2 simulation platform by generating trajectory across a wide range of orientation-constrained scenarios for dual-arm manipulation.

Introduction

To safely perform the manipulation tasks, motion planning algorithms that can generate collision-free motions from an initial to a final configuration in cluttered environments are needed. However, the motion planning problem is known to be PSPACE-hard, ¹ which means that the computational complexity will increase dramatically with the increase of a robot’s degrees of freedom (DOF). Sampling-based algorithms ^{2 –5} in general are considered the dominant approach in dealing with the motion planning problem of high-DOF robotic manipulators because of efficiency, conveniently handling kinodynamic constraints, ⁶ and probabilistic completeness. Recently, sampling-based methods ^{7 –24} have also been extended to deal with manipulation planning problem under pose or kinematic closure constraints. However, these planners usually are difficult to solve the dual-arm motion planning involving orientation constraints of the robot’s end-effector. Examples of such constraint tasks are ubiquitous in our daily life, for example, moving cups full of water from one place to another while keeping them from spilling all along the motion; manipulating a tray with food or glasses on it without tipping throughout the motion path.

In this article, we focus on the dual-arm motion planning problem with end-effector orientation constraints, that is computing a collision-free path for both arms between an initial state and a goal state while maintaining a specified orientation throughout the path. In the process of dual-arm cooperation, ²⁵ there may be also closure constraint between the two arms, for example, when two arms simultaneously grasp and manipulate a single object. In general, these problems can be interpreted as finding constraint-satisfying configurations and connecting them to generate valid paths without being trapped in local minima. ⁷ The main difficulty is that the introduction of constraint satisfaction problem (CSP) ²⁶ poses a significant challenge to the solution of constraint-satisfying configurations, which is an NP-Complete problem. On the one hand, the constraint manifolds (the set of all constraint-satisfied configurations) are zero-measure manifolds ⁸ embedded in the ambient configuration space (C-space). Therefore, the probability that a randomly sampling configuration in C-space lies on the desired constraint manifolds is extremely low and usually null. On the other hand, due to the nonlinear relationship between configuration parameters derived from orientation and closure constraints, it is complicated to calculate the analytical inverse kinematics (IK) solutions satisfied the constraint conditions.

In this article, we propose an efficient and probabilistically complete planning algorithm called orientation-constrained rapidly exploring random trees (OC-RRT) to address motion planning problem involving orientation constraints for decoupled dual-arm robots. The proposed algorithm is based on a novel combined computing framework of IK solutions and sampling-based planning approach. Although the sampling-based planning algorithm in this work takes the RRT ² as an example, it is also suitable for other sampling-based planners such as Probabilistic RoadMaps (PRMs) ³ and RRT variants. ^4,5 As is the case with human arms, the decoupled dual-arm robots with two decoupled manipulators (Figure 1(a)) are designed to allow the decoupling between position and orientation of the robot wrist, ^{27 –29} which have been widely adopted, such as the DLR’s Rollin Justin, Yaskawa Motoman’s SDA10D, Willow Garage’s PR2, and Rethink’s Baxter robot. As the last three joints of the decoupled manipulator have concurrent axes, the position of the wrist represented by Z in Figure 1(b) is determined only by the main arm joints while the orientation can be controlled just by the wrist joints. This special property (decoupling of the positioning and the orientation subtasks) makes it possible to decouple the planning for the main arm joints from the wrist joints. ³⁰ Hence, the proposed algorithm can directly calculate the constraint-satisfying joint configurations relied on the ability of solving the analytical IK. A block diagram describing the workflow of the global system architecture is shown in Figure 2. Note that as the task constraints considered in this article are collision-free and kinematic constraints only, the other constraints, such as force and dynamic constraints, are not taken into account.

Figure 1.

(a) A prototype of dual-arm robot with two decoupled manipulators. The left arm configuration consists of the main arm joints (q₁, q₂, q₃, and q₄) and the wrist joints (q₅, q₆, and q₇). Right arm has the similar structure. (b) Spherical wrist, three revolute axes intersect at a single point.

Figure 2.

The block diagram shows the workflow of the global system architecture, which gives the planning process.

The main contributions of this article can be summarized as follows. (1) an IK-based motion planning algorithm is presented for decoupled dual-arm manipulation with orientation constraints, which directly calculates the constraint-satisfying configuration by analytical IK instead of the previously proposed Jacobian pseudo-inverse projection methods. (2) As the adoption of the analytical IK solver, our planner is characterized by efficiency, accuracy, and without iteration modification. (3) We distinguish this work from the closed-chain motion planning and motion planning with orientation constraint, because considering closure constraint and orientation constraint simultaneously is more difficult than just respecting a single constraint. (4) The other constraints such as the task CSP of trajectory between nodes, probabilistic completeness, avoid singularities, and the limits of joint angle are also considered in this work.

The rest of this article is organized as follows. In the second section, a survey of related work of dual-arm motion planning involving orientation constraints is given. Some preliminaries including the classification of dual-arm operation, the representation of end-effector’s pose constraint, and the description of constrained motion planning are provided in the third section. After that, the detail of OC-RRT planner is presented in the fourth section. In the fifth section, a wide range of OC scenarios are used to validate the prominent performance of the OC-RRT planner. Finally, conclusions of this study are presented in the sixth section.

Related work

Before going into detail of the OC-RRT algorithm, a brief summary of historical approaches to the closed-chain or OC motion planning problem are presented.

Projection strategy ^7,9,31 and randomized gradient descent (RGD) ^10,11,32 are very common methods that have a wide range of applications. In dealing with pose or closure constraints, the projection strategies iteratively projecting randomly sampled configurations onto the constraint manifolds have been proved to be feasible. LaValle et al. ¹⁰ presented the first method to handle the closure constraints of closed-chain robots using a RGD algorithm. An improved RGD algorithm was designed by Yakey ¹¹ for more general closure constraints. Some other researchers adopted a more effective decomposition approach ^12,13 to deal with the closure constraints, which combines IK computation with PRM techniques. To further reduce the computing time of handling closure constraints, random loop generator (RLG) ^14,15 was proposed to increase the probability of randomly generating valid configurations. Gharbi et al. ¹⁶ used the singular configurations to connect different self-motion manifolds. Position/orientation constraints and joint velocity constraints between cooperative robots ¹⁷ were mainly investigated. However, the above methods are only suitable for closed-chain path planning problems.

For the pose constraints on the end-effector, Stilman ⁷ compared three projection algorithms for pose-constrained tasks and the final results indicated that the Jacobian pseudo-inverse projection was typically faster and more invariant than the RGD algorithm. Berenson et al. ¹⁸ used the Task Space Regions (TSRs) to represent a unified framework of pose-related constraints and proposed the CBiRRT planner ¹⁹ to solve general end-effector constraints. To improve the efficiency of the projection operation, recently, planners such as Tangent Space RRT (TS-RRT), ²⁰ Atlas-RRT, ²¹ Tangent Bundle RRT (TB-RRT), ²² and Atlas + X planner ²³ sampled new joint configurations within the tangent spaces, which are nearby the constraint manifold. Kingston et al. ²⁴ divided these previous methodologies into five categories: (1) relaxation, (2) projection, (3) tangent space sampling, (4) incremental Atlas construction, and (5) reparameterization. But the approach of decoupling between translational and rotational motions has not been studied yet.

Researchers have proposed several kinematics-based planners capable of planning for regrasping tasks, but they only focus on how to choose the feasible grasping configurations rather than the whole path constraint. Bertram et al. ³³ calculated several joint configurations of grasping using the IK solver and then set them as goals for the randomized planner. Berenson et al. ³⁴ used workspace goal regions (WGRs) and two probabilistically complete planners (RRT-JT and IKBiRRT) to deal with end-effector pose constraints. Related IK-based approaches (RRT-JT and IK-RRT) ³⁵ for dual-arm manipulation planning and regrasping tasks were presented by Vahrenkamp et al. ³⁵ and Xian et al. ³⁶ connected nearby C-space through the IK-switch to address complex closed-chain manipulation tasks. Such methods are mainly designed for the selection of goal or switching joint configurations in the path planning, which are infeasible to maintain orientation and closure constraints throughout the whole path. There has been research on dual-arm manipulation planning with orientation constraints ³⁷ using the graph heuristic search techniques. Also, some approaches tried to achieve the approximation of the constraint manifolds by off-line computation, ³⁸ model learning, ³⁹ or demonstrations learning ^40,41 but only for certain scenarios.

We compared several classical methods as shown in Table 1. The previous research mainly focuses on closure or pose constraint problem, to the best of our knowledge, the multiple CSP of considering closure and orientation constraint simultaneously has been rarely addressed by randomized path planning approaches.

Table 1.

Comparison of several constrained motion planners.

Planners	Tasks	Advantages	Drawbacks
RGD-RRT	Pose or closure constraints	Minimizes constraint error by randomized gradient descent.	Suitable for simple configurations; requires many iterations; inefficient.
RLG-PRM	Closure constraints	Satisfies the closure constraint by the analytic IK solver with high efficiency.	Need to select appropriate redundancy angle; weak completeness guarantee.
CBiRRT	General end-effector pose constraints	Able to plan with a variety of constraints; probabilistic completeness.	Computation of Jacobian pseudo-inverses; many iterations.
TS-RRT	Pose constraints	Less consuming time, less number of iterations and fewer extended nodes overall than the CBiRRT planner.	Hard to guarantee the validity of the lazy projection procedure; computation of Jacobian pseudo-inverses.
Atlas-RRT	Singularity-free, pose or kinodynamic constraints	Creates tangent polytopes for more uniform sampling; projects at each step for more accurate sampling and interpolation.	Additional computation of tangent polytopes; computation of Jacobian pseudo-inverses.
IKBiRRT	Regrasping tasks	Finds grasping configurations by analytic IK solver with high efficiency.	Just specifies the goal configuration not for all nodes of the random tree.

IK: inverse kinematic; RGD: randomized gradient descent; PRM: probabalistic RoadMap; RLG: random loop generator; RRT: rapidly-exploring random trees; TS: Tangent Space.

Preliminaries

In this section, some mathematical notations and preliminaries of our planning algorithm are given. The preliminaries consists of three parts: classification of dual-arm operations according to the type of dual-arm manipulation tasks, representation of pose constraints between the two arms, and description of motion planning on constraint manifold.

Classification of dual-arm manipulation

A summary of dual-arm manipulation conducted by Smith was discussed, ⁴² where dual-arm operation was split into modes of non-coordinated manipulation (each arm performs a different task) and coordinated manipulation (both arms implement different parts of the same task). The coordinated manipulation mode ⁴³ was further divided into bimanual manipulation, where two arms are engaged in manipulation of a single object within the shared space and goal-coordinated manipulation, where two arms operate separately without kinematic constraints but both are solving the same task.

When there is no orientation constraint imposed on the end-effector of the manipulators, the dual-arm manipulation can often be realized by well-investigated motion planners for closed kinematic chains or two single-arm robots (please refer to “Related work” section for general reviews). However, the introduction of orientation constraint makes dual-arm manipulation more complicated because of the difficulty of parameterizing lower dimensional orientation constraint manifolds and solving analytic IK solutions for the desired orientation. Thus, the dual-arm manipulation with orientation constraints is an important and difficult part of the dual-arm manipulation, which can be categorized into open-chain manipulation mode and close-chain manipulation mode.

As only kinematic constraints are considered in this article, dual-arm manipulation can be grouped into open-chain manipulation and close-chain manipulation according to whether they form a closed-chain mechanism. In the mode of open-chain manipulation, the two robot arms operate separately with loose or without closed kinematic chains. Compared with open-chain manipulation, close-chain manipulation has more stringent constraints on the closed kinematic chains, which makes it more complicated. As for dual-arm manipulation, the cases of non-coordinated manipulation can be considered as the open-chain manipulation and the bimanual manipulation can be referred to as the close-chain manipulation. In the cases of goal-coordinated manipulation, both open-chain manipulation and close-chain manipulation exist in the process of operation.

Constraints description

The orientation and kinematic closure constraints of dual-arm robot most commonly take the form of the position and/or orientation of the robot’s end-effector. In this article, homogeneous transformation of frame $\left\{b\right\}$ relative to frame $\left\{a\right\}$ is used to describe position and orientation of end-effector, which consists of a $3\times 3$ rotation matrix ${}_b^{a}R$ and a $3\times 1$ translation vector ${}_b^{a}p$

${}_b^{a}T=\left[\begin{array}{cc}{}_b^{a}R& {}_b^{a}p\\ 0& 1\end{array}\right]$

1

To express pose constraint (denoted by $C^r$ ) of the task frame relative to the world frame intuitively, a six-dimensional vector ${[x,y,z,\alpha ,\beta ,\gamma ]}^{\text{T}}\in {ℝ}^6$ is adopted here. The three-dimensional vector ${[x,y,z]}^{\text{T}}$ indicates translation and three-dimensional Roll-Pitch-Yaw (RPY) Euler angles $\Phi =[\alpha ,\beta ,\gamma {]}^{\text{T}}$ about fixed axes indicates rotation

$C^r=\left[\begin{array}{c}x\\ y\\ z\\ \alpha \\ \beta \\ \gamma \end{array}\right],\hspace{0.17em}\hspace{0.17em}\text{where}\{\begin{array}{c}x\in (x_{\text{min}},x_{\text{max}})\\ y\in (y_{\text{min}},y_{\text{max}})\\ z\in (z_{\text{min}},z_{\text{max}})\\ \alpha \in ({\alpha }_{\text{min}},{\alpha }_{\text{max}})\\ \beta \in ({\beta }_{\text{min}},{\beta }_{\text{max}})\\ \gamma \in ({\gamma }_{\text{min}},{\gamma }_{\text{max}})\end{array}$

2

As there is no explicit kinematic closure constraint in open-chain manipulation mode, the dual-arm system can be considered as two independent single-arm systems with predefined orientation constraints on each arm. That is, Euler angles $\Phi =[\alpha ,\beta ,\gamma {]}^{\text{T}}$ are set to fixed values while the position variables ${[x,y,z]}^{\text{T}}$ are random values within the operating space for each arm. In terms of close-chain manipulation mode, two arms are engaged in manipulation of one common object with fixed orientation of both right and left arm. In such cases, each arm not only has specified orientation all along the entire path but also has kinematic position constraint between the two arms. That means Euler angles $\Phi =[\alpha ,\beta ,\gamma {]}^{\text{T}}$ of the two arms are set to the same fixed values and translation vector of end-effector ${[x,y,z]}^{\text{T}}$ between the two arms maintains a fixed value.

Constraint manifold

It has long been recognized that the notion of C-space ( $\mathcal{Q}$ ) allows the complicated motion planning problems to be easily described in a unified way. The advantage of using the $\mathcal{Q}$ is that motion planning for the robot with complex geometric shape is equivalent to motion planning for a point in $\mathcal{Q}$ . For example, a n-DOF robot’s configuration $q=(q_1,q_2,\dots ,q_n)$ can be represented as a point in $\mathcal{Q}$ and a homogeneous transformation of the robot can be converted into a path in $\mathcal{Q}$ , where $q_1,q_2,\dots ,q_n$ are the joint angles of the robotic arm. The set of points in $\mathcal{Q}$ that correspond to valid configurations of the robot (the configurations do not cause collision with any obstacles or itself in workspace) is defined as free space ${\mathcal{Q}}_{free}\subseteq \mathcal{Q}$ . Given an initial configuration ${\mathit{q}}_{init}\in {\mathcal{Q}}_{free}$ and a goal configuration ${\mathit{q}}_{goal}\in {\mathcal{Q}}_{free}$ , the classical version of the motion planning problem can be defined as finding a continuous path τ such that

$\begin{array}{c}\tau :[0,1]\to {\mathcal{Q}}_{free}\\ \tau (0)={\mathit{q}}_{init};\hspace{0.17em}\tau (1)={\mathit{q}}_{goal}\end{array}$

In general, sampling-based planners are typically efficient in planning motions for high-dimensional systems and provide probabilistic completeness guarantees. Instead of computing ${\mathcal{Q}}_{free}$ exactly, these planners incrementally sample collision-free configurations and connect them to construct the approximation of the ${\mathcal{Q}}_{free}$ until a solution is eventually found. For a more in-depth review, the randomly sampling configuration consists of multiple independent random variables $q_1,q_2,\dots ,{\mathit{q}}_n$ as there is no strict constraint on the pose of the robot’s end-effector.

However, for kinematic constrained motion planning, these random variables $q_1,q_2$ , ${\mathit{q}}_n$ are usually correlated (nonlinear constraints imposed on some or all of these joint variables) because the configuration need to satisfy specific nonlinear equality constraints $f(\mathit{q})=C^r$ according to task requirements. Such equality constraints have the potential to reduce the dimensionality of $\mathcal{Q}$ and thus a lower dimensional manifold is delineated. This lower dimensional constraint manifold embedded in the ambient $\mathcal{Q}$ is also known as constraint manifold ${\mathcal{M}}_{\mathcal{C}}\subset \mathcal{Q}$ , where all the configurations must meet the nonlinear equality constraints. Supposing there are k ( $n>k>0$ ) equality constraints imposed, the $(n-k)$ -dimensional implicit ${\mathcal{M}}_{\mathcal{C}}\subset {ℝ}^{n-k}$ within the n-dimensional $\mathcal{Q}\subset {ℝ}^n$ can be defined as

${\mathcal{M}}_{\mathcal{C}}=\left\{\mathit{q}=(q_1,q_2,\dots ,{\mathit{q}}_n)\in \mathcal{Q}\hspace{0.17em}|\hspace{0.17em}f(\mathit{q})=C^r\right\}$

4

As the structure of the constraint manifold region is not known a priori, motion planners need to be able to either directly calculate analytical IK solutions that inherently satisfy the special equality constraints or iteratively project the invalid configurations onto the constraint manifold. Obviously, computing analytical IK solutions can be much more rapid than the projection method, but the process of IK is fairly complicated or even null. Fortunately, the analytical IK solutions that satisfy the pose constraints can be solved for decoupled manipulators (see “Generating IK solutions” section).

OC-RRT planner

To solve the constrained motion planning problem, algorithms must conduct a search on constraint manifold ${\mathcal{M}}_{\mathcal{C}}$ . In this article, an analytical IK solver is adopted to directly generate constraint-satisfying joint configurations that lie on ${\mathcal{M}}_{\mathcal{C}}$ for decoupled dual-arm robots.

Generating IK solutions

Decoupled manipulators are frequently equipped with a humanoid spherical wrist from the viewpoint of the anthropomorphic arm structure, which allows decoupling motion of the position problem from the orientation problem. This makes it possible to generate configurations that lie on constraint manifold directly. A general decoupled redundant manipulator with seven revolute joints is employed in this work, whose world frame $\left\{w\right\}$ is fixed to the base frame $\left\{0\right\}$ . We assume that the robot’s end-effector is mounted on the wrist fixedly, which can effectively transform constraints of the end-effector frame $\left\{e\right\}$ into the wrist frame $\left\{7\right\}$

${}_7{}^{0}T={}_e{}^{0}T\cdot {\left({}_e{}^{7}T\right)}^{-1}$

5

where ${}_e^{7}T$ will be a constant matrix once the end-effector is fixed and ${}_e^{0}T$ is predefined according to the task constraint.

As the last three joint axes of the decoupled manipulator intersect at the wrist point, ${}_7^{0}p$ is equal to ${}_5^{0}p$ , thereby

${}_7{}^{0}p={}_1{}^{0}p+{}_1{}^{0}R\cdot {}_2{}^{1}p+{}_2{}^{0}R\cdot {}_3{}^{2}p+{}_3{}^{0}R\cdot {}_4{}^{3}p+{}_4{}^{0}R\cdot {}_5{}^{4}p$

6

where ${}_7{}^{0}p={}_e{}^{0}R\cdot {}_7{}^{e}p+{}_e{}^{0}p$ according to equation (5). Apparently, the position of wrist is independent of the wrist angles ${\mathit{q}}_{wrist}$ , which consists of the last three joint angles ${\mathit{q}}_{wrist}=\left(q_5,q_6,q_7\right)$ . Hence, only main arm angles ${\mathit{q}}_{marm}=\left(q_1,q_2,q_3,q_4\right)$ are considered for the analysis of equation (6) and analytical IK solutions of ${\mathit{q}}_{marm}$ can be solved through equation (6) by parametrizing a redundancy. When the first angle q₁ is chosen as the redundant angle ${\mathit{q}}_{redund}$ , equation (6) can be readily rewritten in the form

${}_0{}^{1}R\cdot \left({}_7{}^{0}p-{}_1{}^{0}p\right)={}_2{}^{1}p+{}_2{}^{1}R\cdot {}_3{}^{2}p+{}_3{}^{1}R\cdot {}_4{}^{3}p+{}_4{}^{1}R\cdot {}_5{}^{4}p$

7

Thus, we have three scalar equations and three unknowns, the analytical IK solutions of ${\mathit{q}}_{marm}$ can be solved.

Likewise, the orientation of wrist can be controlled by ${\mathit{q}}_{wrist}$ independently from ${\mathit{q}}_{marm}$ . Analytical IK solution(s) of ${\mathit{q}}_{wrist}$ can be solved through equation (8)

${\left({}_4{}^{0}R\right)}^{-1}\cdot {}_e{}^{0}R\cdot {\left({}_e{}^{7}R\right)}^{-1}={}_5{}^{4}R\cdot {}_6{}^{5}R\cdot {}_7{}^{6}R$

8

Please refer to Angeles ⁴⁴ for more detailed explanation.

Sampling constraint-satisfying configurations

Although the study of motion planners with orientation or kinematic closure constraints (see “Related work” section) has been proved feasible using Jacobian pseudo-inverse projection techniques, these methods have many technical challenges, such as avoiding joint limits and singularity, iterations, and computational efficiency. What is more, dual-arm manipulation with orientation constraint not only has specified orientation all along the entire path, but also need to maintain a relatively fixed position constraint between the two arms’ end-effector in close-chain manipulation mode. The multiple constraints on end-effector make the problem more complicated than only imposed orientation or kinematic closure constraint.

To overcome these difficulties, an analytical IK solver is applied to calculate constraint-satisfying configurations directly, which has preferable efficiency and satisfactory accuracy. The dual-arm system is decomposed into the “active arm” (named arm1) and the “passive arm” (named arm2). Due to the predefined orientation constraint of tasks, the active arm needs to maintain fixed Euler angles Φ all along the whole motion, while the position values are random values within the operating space. The same situation applies to passive arm in open-chain manipulation mode. The orientation constraint manifold ${\mathcal{M}}_{\mathcal{C}}^{\mathcal{O}}$ of active arm, for example, is expressed as

${\mathcal{M}}_{\mathcal{C}}^{\mathcal{O}}\hspace{0.17em}=\hspace{0.17em}\left\{{\mathit{q}}_{arm1}=\hspace{0.17em}(q_1,\dots ,q_7)\in \mathcal{Q}\hspace{0.17em}|f({\mathit{q}}_{arm1})\hspace{0.17em}=\hspace{0.17em}\Phi \right\}$

9

where ${\mathit{q}}_{arm1}$ is the active arm configuration consisting of seven joint angles. Function $f()$ is the forward kinematics mapping from C-space to Cartesian space.

As for mode of close-chain manipulation, we suppose that a grasping object by the two hands is rigid and the length of the rigid object determines the relatively fixed position between the two arms. As aforementioned, the configuration of active arm is assigned to maintain a predefined orientation in the entire movement and then the configuration of passive chain is computed to meet the closure constraint by IK solver. Because of the kinematic redundancy of the passive arm, there are an infinite number of IK solutions ⁴⁵ corresponding to the desired end-effector pose obtained from the forward kinematics of the active arm. The set of all these IK solutions in joint space is referred to as self-motion manifold ${\mathcal{M}}_{\mathcal{C}}^{\mathcal{S}}$ , which can be written as

${\mathcal{M}}_{\mathcal{C}}^{\mathcal{S}}\hspace{0.17em}=\hspace{0.17em}\left\{{\mathit{q}}_{arm2}\hspace{0.17em}=\hspace{0.17em}(q_8,\dots ,{\mathit{q}}_{14})\hspace{0.17em}\in \hspace{0.17em}\mathcal{Q}\hspace{0.17em}|f({\mathit{q}}_{arm2})\hspace{0.17em}=\hspace{0.17em}{p}_{arm2}\right\}$

10

where ${\mathit{q}}_{arm2}$ is the passive arm configuration consisting of seven joint angles and $p_{arm2}$ is the end-effector pose of passive arm calculated by the forward kinematics of active arm.

As the kinematics analysis of decoupled manipulators mentioned before, position exploration can be done through random sampling of the main arm angles, while the orientation of wrist can be controlled by the wrist angles. The pseudocode procedure of generating a constraint-satisfying configuration is described in Algorithm 1. For the active arm (arm1), the random sampling function $RandomConfig()$ repeatedly samples new random states of main arm angles ${\mathit{q}}_{marm1}^{rand}$ and analytic IK of wrist angles $Wrist\_Inv\_Kin()$ tries to calculate the wrist angles ${\mathit{q}}_{wrist1}^{rand}$ for the given Euler angles Φ. Obviously, the random state of active arm ${\mathit{q}}_{arm1}^{rand}$ consisting of ${\mathit{q}}_{marm1}^{rand}$ and ${\mathit{q}}_{wrist1}^{rand}$ lies on the orientation constraint manifold. In terms of the passive arm (arm2), the similar steps mentioned above are extended to the passive arm (line 6 and 7) and also the similar results can be obtained when operating in open-chain manipulation mode. Otherwise, in close-chain manipulation mode, the forward kinematic function $FK()$ is used to figure out the pose of the passive arm’s end-effector $p_{arm2}$ by ${\mathit{q}}_{arm1}^{rand}$ and the length of the rigid object. Then analytical IK solutions of main arm angles ${\mathit{q}}_{marm2}^{rand}$ and wrist angles ${\mathit{q}}_{wrist2}^{rand}$ can be solved by a random redundant angle ${\mathit{q}}_{redund}^{rand}$ through equations (6) and (3), respectively. Obviously, the state of passive arm ${\mathit{q}}_{arm2}^{rand}$ consisting of ${\mathit{q}}_{marm2}^{rand}$ and ${\mathit{q}}_{wrist2}^{rand}$ lies on the self-motion manifold. The above code loops until IK is solved successfully ( $Inv\_Kin\_Success=true$ ), that is, ${\mathit{q}}_{rand}$ meets the restrictive requirements of joint angles range and avoiding singularity.

Algorithm 1.

OCSampleConfig(Φ).

OC-RRT algorithm

Sampling-based planners, such as RRT-Connect (a bidirectional search version of RRT) ⁴ , RRT* (an asymptotically optimal RRT), ⁵ anytime RRT*, ⁴⁶ and RABIT* (regionally accelerated batch informed trees), ⁴⁷ are state-of-the-art techniques for solving the motion planning problem, which use a variety of distributions for sampling the C-space they search. Instead of running the planner on the C-space, the OC-RRT algorithm works on the constraint manifold to meet the OC requirement of task directly. As shown in Algorithm 2, the OC-RRT algorithm strives to search the constraint manifold for a feasible path by growing a space-filling tree T. The tree is initially rooted at the start point ${\mathit{q}}_{init}$ , and a random node ${\text{q}}_{rand}$ is generated by $OCSampleConfig()$ at each incremental expansion of the tree-grow. Then the nearest node (parent node) ${\mathit{q}}_{nearest}$ is selected from all the nodes on T and an attempt $OCSteer()$ is made to generate the new node (child node) ${\mathit{q}}_{new}$ by moving a step size ε from ${\mathit{q}}_{nearest}$ toward ${\mathit{q}}_{rand}$ . If the path between the ${\mathit{q}}_{nearest}$ and ${\mathit{q}}_{new}$ is collision-free, then ${\mathit{q}}_{new}$ is added to V (the vertices of T) and the edge $({\mathit{q}}_{nearest},{\mathit{q}}_{new})$ is added to E (the edges of T). The extending tree T never halts until a solution has been found or some failure rules are satisfied.

Algorithm 2.

OC-RRT.

To depict the functionality of the presented OC-RRT, it is reasonable to compare with the RRT algorithm ³ and point out the relevant modifications. The main differences between OC-RRT and RRT are as following:

Sampling: compared with RRT algorithm, OC-RRT uses $OCSampleConfig()$ (Algorithm 1) to generate random sampling node ${\mathit{q}}_{rand}$ uniformly on constraint manifold rather than C-space. By this way, all the possible constraint-satisfying configuration on the constraint manifold can be found.

Steering: In the RRT algorithm, the function $Steer({\mathit{q}}_{nearest},{\mathit{q}}_{rand})$ generates a new node by moving a step size ε in the straight line from the nearest node ${\mathit{q}}_{nearest}$ to the randomly sampled node ${\mathit{q}}_{rand}$ . It is obvious that ${\mathit{q}}_{new}$ may escape from the constraint manifold by applying the $Steer()$ to the OC-RRT algorithm. Hence, $OCSteer()$ is proposed to generate new node ${\mathit{q}}_{new}$ lying on constraint manifold (Figure. 3). For the active arm (arm1), $Steer()$ (line 2 of Algorithm 3) generates the main arm angles of the new node ${\mathit{q}}_{marm1}^{new}$ by moving a step size ε away from the main arm angles of the nearest node ${\mathit{q}}_{marm1}^{nearest}$ toward the direction of the main arm angles of randomly sampled node ${\mathit{q}}_{marm1}^{rand}$ . Then, analytic IK of wrist angles $Wrist\_Inv\_Kin()$ tries to calculate the wrist angles of the new node ${\mathit{q}}_{wrist1}^{new}$ for the given Euler angles Φ. Obviously, the new state of active arm ${\mathit{q}}_{arm1}^{new}$ consisting of ${\mathit{q}}_{marm1}^{new}$ and ${\mathit{q}}_{wrist1}^{new}$ lies on the orientation constraint manifold. In addition, as a small change in the joint angles results in a slight change in the pose of the end-effector and in turn it is also true, the final motion of the active arm’s end-effector does not jump between adjacent nodes of the random trees when the step size ε is small enough. That means the edge between ${\mathit{q}}_{arm1}^{nearest}$ and ${\mathit{q}}_{arm1}^{new}$ also lies on the orientation constraint manifold when ε is small.

Figure 3.

New node (gray) generated by Steer() is outside of constraint manifold (blue). Hence, OCSteer() is proposed to generate new node (yellow) lying on the constraint manifold.

Algorithm 3.

$OCSteer({\text{q}}_{nearest},{\text{q}}_{rand},\Phi )$ .

In terms of the passive arm, similar steps mentioned above are extended to the passive arm and also the similar results can be obtained when operating in open-chain manipulation mode. Otherwise, the forward kinematic function $FK()$ is used to figure out the position of the passive arm’s end-effector $p_{arm2}$ by ${\mathit{q}}_{arm1}^{new}$ , and the length of the rigid object in close-chain manipulation mode. $Steer\left({\mathit{q}}_{redund}^{nearest},{\mathit{q}}_{redund}^{rand}\right)$ generates the new redundant joint ${\mathit{q}}_{redund}^{new}$ and then analytical IK solutions of main arm angles ${\mathit{q}}_{marm2}^{new}$ and wrist angles ${\mathit{q}}_{wrist2}^{new}$ can be solved through equation (6) and (3), respectively. Obviously, the new node ${\mathit{q}}_{new}$ is a constraint-satisfying configuration, and the final motion of the robot’s end-effectors does not jump between adjacent nodes of the random trees when the ε is small. The above code loops until IK is solved successfully ( $Inv\_Kin\_Success=true$ ), that is, ${\mathit{q}}_{new}$ meets the restrictive requirements of joint angles range and avoiding singularity.

Probabilistic completeness guarantee of the OC-RRT algorithm follows from the property: given infinite time, every possible constraint-satisfying configuration on the constraint manifolds will be added to the space-filling tree. What is more, the trajectory of joint angle and the final motion of the robot’s end-effector do not jump between adjacent nodes of the random trees when the step size ε is small enough ( $\epsilon \in (0.01,0.05)$ in our simulation of “Simulation experiments” section). Thus, the trajectory between the ${\mathit{q}}_{nearest}$ and ${\mathit{q}}_{new}$ can also respect the constraints of the task, which is verified to be effective by the simulation results of “Simulation experiments” section.

Simulation experiments

To validate the performance of the proposed OC-RRT algorithm, four scenarios (named Industrial I, Industrial II, Tabletop, and Passageway) were implemented on the Willow Garage’s PR2 robot in MoveIt! ⁴⁸ simulator. PR2 is designed with two 7-DOF decoupled manipulators, which allows us to decouple planning for the main arm joints (the first four joints) from the wrist joints (the last three joints) as shown in Figure 4. The shoulder pan joint (q₈) of right arm was selected as the redundant joint for the analytical IK solution(s) of the right arm’s main arm configuration, the joint limits and singularities were also taken into account in the process of solving the IK solutions. For decoupled manipulators with spherical wrist, it is possible to split the singularity computation into two separate problems: (1) computation of main arm singularities resulting from the motion of the main arm joints and (2) computation of wrist singularities resulting from the motion of the wrist joints. Main arm singularities only happen when $\text{sin}\hspace{0.17em}(q_4)=0$ , while wrist singularities only happen when $\text{sin}\hspace{0.17em}(q_6)=0$ . To ensure that the trajectory between nodes meets the task constraints while ensuring the planning speed, the step size ε was set to a random value between 0.01 and 0.05. There is no doubt that the smaller the step size ε, the better performance our approach will be, but the planning time will increase. Thus, how to choose the optimal ε is another problem that is not discussed here.

Figure 4.

The right arm configuration of the PR2 robot consists of the main arm joints (q₈, q₉, ${\mathit{q}}_{10}$ , and ${\mathit{q}}_{11}$ ) and the wrist joints ( ${\mathit{q}}_{12}$ , ${\mathit{q}}_{13}$ , and ${\mathit{q}}_{14}$ ). Changes in the position of the PR2 robot’s wrist, for example, from current state in gray to a new state in pink, are only relevant for the main arm joints, which have nothing to do with the wrist joints. The orientation of the PR2 robot’s wrist can be controlled only by the wrist joints, for instance, from the gray state to the yellow state. The left arm has the similar properties.

For the sake of comparison, the CBiRRT proposed by Berenson et al. ¹⁹ was implemented for dealing with the same task. As the CBiRRT planner and our OC-RRT planner are randomized, we carried out 20 times repetitive experiments for each simulated scenarios. The two planners are coded in the open-source Open Motion Planning Library (OMPL), ⁴⁹ and the laptop used to install the Robot Operating System (ROS) has 6 GB of RAM and an Intel i3-6100 CPU quadcore processor running at 2.5 GHZ.

Evaluation index

To evaluate the performance capabilities of the proposed algorithm, several indicators were introduced: (1) planning time, (2) number of nodes, (3) success rate, and (4) root-mean-square deviations (RMSDs) of position or orientation for path accuracy. The RMSD or RMSE ^50,51 is a well-known evaluating index, especially in probability statistics. The orientation RMSD (in radians) is defined as

$O\_RMSD\hspace{0.17em}=\hspace{0.17em}\sqrt{\frac{{\displaystyle {\sum }_{k=1}^n\left({(\Delta {\alpha }_k)}^2+\hspace{0.17em}{(\Delta {\beta }_k)}^2+\hspace{0.17em}{(\Delta {\gamma }_k)}^2\right)}}{3\times n}}$

11

and the position RMSD (in meters) is defined as

$P\_RMSD\hspace{0.17em}=\hspace{0.17em}\sqrt{\frac{{\displaystyle {\sum }_{k=1}^n\left({(\Delta p_k^x)}^2+\hspace{0.17em}{(\Delta p_k^y)}^2+\hspace{0.17em}{(\Delta p_k^z)}^2\right)}}{3\times n}}$

12

where n is the number of sampling nodes; $\Delta \alpha ,\Delta \beta ,\Delta \gamma$ are the difference between the measured value (derived from forward kinematics) and the desired Euler angles of end-effector; $\Delta p^x,\hspace{0.17em}\Delta p^y,\hspace{0.17em}\Delta p^z$ are the difference between the measured value (derived from forward kinematics) and the desired value of two end-effectors’ distance.

Experimental results in open-chain mode

As shown in Figure 5, the simulated PR2 robot has to vertically move two cups full of water from the initial (green) to the target configuration (yellow) in the cluttered environment (Industrial I). According to requirement of the task, not only the start pose and the target pose but also the entire planned trajectory needs to satisfy the end-effector’s orientation constraint with Euler angles $\alpha =\beta =\gamma =0$ radians.

Figure 5.

Industrial I scenario: (1)–(4) snapshots of moving two cups full of water from start pose (green) to goal pose (yellow) by using the OC-RRT planner. Constraints: upright orientation and collision-free constraint. Blue: trajectories of two arms’ end-effector. Green: start configuration. Yellow: goal configuration. (a) Trajectories of two arms’ end-effector. (b) RMSD of orientation. (c) Joint position trajectories of left arm. (d) Joint position trajectories of right arm. OC-RRT: orientation-constrained rapidly exploring random trees; RMSD: root-mean-square deviation.

Figure 5(1)–(4) showed a typical result of the snapshots from the execution of the OC-RRT planner, where the planned collision-free path (shown in blue) is smooth. The accompanying video showed the motion of the robot for this task. Figure 5(a) presented the path of the two arms’ end-effector. The corresponding orientation RMSD of the two arms’ end-effectors was fixed to zero with high accuracy as shown in Figure 5(b) and the trajectories of two arms’ each joint moved smoothly as shown in Figure 5(c) (left arm) and (d) (right arm).

Experimental results in close-chain mode

Due to the orientation and closure constraints of the arms, the distance between two grasping points of the arms and the cluttered environments caused a very tight cooperative workspace for dual-arm manipulation. The multiple simultaneous constraints make the problem particularly complicated. We evaluated the performance of the proposed planner by presenting numerical results in three different scenarios ⁵² (Industrial II, Table top and Narrow Passageway) as shown in Figures 6 to 8. The simulated PR2 robot was required to vertically manipulate a tray with a cup on it from the start configuration (green) to the goal configuration (yellow) in the cluttered environments, while maintaining a relatively fixed position constraint between the two arms’ end-effector.

Figure 6.

Industrial II scenario: (1)–(4) snapshots of moving a tray with a cup on it from start pose (green) to goal pose (yellow). (a) Trajectories of two arms. (b) RMSDs of orientation and position. (c) Joint position of left arm. (d) Joint position of right arm. RMSD: root-mean-square deviation.

Figure 7.

Tabletop scenario: (1)–(5) snapshots of moving a tray with a cup on it from start pose to goal pose. (a) Trajectories of two arms. (b) RMSDs of orientation and position. (c) Joint position of left arm. (d) Joint position of right arm. RMSD: root-mean-square deviation.

Figure 8.

Narrow passageway scenario: (1)–(8) snapshots of moving a tray with a cup on it from start pose to goal pose. (a) Trajectories of two arms. (b) RMSDs of orientation and position. (c) Joint position of left arm. (d) Joint position of right arm. RMSD: root-mean-square deviation.

Upper part of each figure (Figures 6 to 8) presented the typical result of the snapshots from the execution of the OC-RRT planner for the three different scenarios, where the planner can compute a collision-free path successfully while satisfying the orientation and closure constraints. The motion of the robot for the different scenarios is fully visible in the accompanying video. The subgraph (a) of each figure showed the entire trajectory of the two arms, where the left arm is identified as the active arm (Leader) and right arm as the passive (Follower). We can visually see that the planned trajectory is smooth and meets the kinematic closure constraints. The position and orientation RMSD of the two arms’ end-effectors were fixed to zero with high accuracy as shown in subgraph (b) of each figure. The corresponding joint trajectories of the two arms were shown in subgraph (c) (left arm) and (d) (right arm) of each figure.

Discussion

The comparative results of simulation between the proposed OC-RRT and the CBiRRT ¹⁹ in four different scenarios are given in Table 2. According to the results in Table 2, the OC-RRT and the CBiRRT both can successfully complete the motion planning of dual-arm with the same constraints on PR2 simulation platform, but the OC-RRT algorithm can plan constraint-satisfying trajectory with efficiency and accuracy.

Table 2.

Simulation results (efficiency and accuracy) from 20 times trials for the four different scenarios.

Scenario		Industrial I		Industrial II		Tabletop		Passageway
Planner		CBiRRT	OC-RRT	CBiRRT	OC-RRT	CBiRRT	OC-RRT	CBiRRT	OC-RRT
Planning time (s)	Mean	0.73	0.26	1.73	0.31	1.36	0.39	7.21	0.43
	Min	0.49	0.11	0.86	0.17	0.51	0.22	3.59	0.29
	Max	1.53	0.39	4.07	0.43	5.10	0.58	9.74	0.82
Number of nodes	Mean	331	128	585	142	463	189	928	257
	Min	118	52	397	76	309	91	691	139
	Max	474	163	764	194	952	252	1472	594
ORMSD ( ${10}^{-7}$ rad)	Mean	5.9	0.95	5.4	0.85	8.1	0.7	11.7	1.2
	Min	2.7	0.5	3.7	0.3	3.1	0.52	6.3	0.2
	Max	9.2	1.3	7.1	1.4	11.8	1.7	15.5	2.9
PRMSD ( ${10}^{-5}$ m)	Mean	(No)	(No)	12.8	3.1	10.8	1.9	15.0	3.7
	Min	(No)	(No)	9.3	0.5	8.7	0.7	8.5	1.3
	Max	(No)	(No)	16.9	7.6	15.1	6.9	19.4	8.4
Success rate (%)		100	100	100	100	100	100	100	100

OC-RRT: orientation-constrained rapidly exploring random trees; RMSD: root-mean-square deviation.

In open-chain manipulation mode, the proposed OC-RRT algorithm is superior to the CBiRRT planner in terms of the indicators of Planning Time, Number of Nodes, and ORMSD. All these superiorities can be attributed to that the OC-RRT planner uses the analytical IK technique to sample constraint-satisfying configurations directly without iteration modification. Besides, the extremely tiny ORMSD can not only illustrate that the nodes generated by the OC-RRT planner meet the orientation constraint accurately but also verify that the trajectories between nodes respect the orientation constraint of the task.

Similarly, the proposed OC-RRT planner is more accurate and efficient than the CBiRRT planner in terms of all indicators in close-chain manipulation mode. In addition, the extremely tiny RMSD of orientation and position can not only illustrate that the nodes generated by the OC-RRT planner meet the orientation and kinematic closure constraints of the task with high accuracy but also verify that the trajectories between nodes respect these constraints.

Conclusion

In this article, we have presented an efficient and probabilistic complete planning algorithm-combined sampling-based technique with analytical IK solver to address the dual-arm motion planning problem involving orientation constraints for decoupled dual-arm robots. The general problem is interpreted as finding constraint-satisfying configurations and connecting them to generate valid paths in joint space without being trapped in local minima. The key idea of our approach is to efficiently explore the constraint-satisfying configurations by directly sampling them on constraint manifold through analytic IK. Besides, we also limit the joint space displacements between nearest node and new node by setting a small step size ε, so that the trajectories between nodes on the space-filling tree can satisfy the task constraints. Without losing the major advantage of the RRT algorithm, our method can explore all the possible constraint-satisfying configurations when given infinite time and can avoid falling into local minima by uniform sampling strategy. As the closed-form IK solver is applied to calculate constraint-satisfying configurations, our approach is significantly efficient and accurate. Four typical scenarios were used to demonstrate the effectiveness of the OC-RRT algorithm on the Willow Garage’s PR2 simulation platform in ROS MoveIt!.

In our recent work, we have successfully validated the proposed OC-RRT algorithm on the Baxter robot built by Rethink Robotics. The proposed algorithm has also been extended to other sampling-based motion planners such as PRM planner and RRT-Connect planner. Especially when our method was applied to the RRT-connect algorithm, the planning time and path were obviously improved. To make the proposed algorithm framework more versatile, future research will focus on solving general pose constraint tasks for decoupled manipulators. Additionally, considering the influence of the contact force between two arms in the close-chain mode, another future research is how to integrate motion planner and forces/position controller to deal with closure constrained motion planning for dual-manipulators.