Motion Planning With Pose Constraints Based on Direct Projection for Anthropomorphic Manipulators

This paper presents an efficient planning algorithm, called Direct-Projection Rapidly-exploring Random Tree (DP-RRT), to address the motion planning with end-effector pose constraints for anthropomorphic manipulators. The key of this planning problem is to find constraint-satisfying configurations on the constraint manifolds and connect them to generate a collision-free and smooth path. In the previous works, the configurations that satisfy pose constraints are generally calculated by the numerical iteration methods based on Jacobian projection techniques. However, such approaches have many technical challenges, such as joint limits and singularity, many numerical iterations and much computing time. In this work, we propose a Direct Projection method based on the analytic inverse kinematics (IK) that can directly project configurations onto the constraint manifolds instead of the numerical iteration methods. The proposed DP-RRT algorithm combines the Direct Projection method with the Rapidly-exploring Random Tree (RRT) algorithm, where the RRT algorithm is employed to explore the ambient space by growing tree branches, and the Direct Projection method is used to project the tree branches onto the constraint manifolds for constructing a constraint-satisfying path. As the analytic IK solver is used to calculate the constraint-satisfying configurations, the DP-RRT algorithm is characterized by high efficiency and no numerical iteration. Besides, avoiding joint limits and singularity, as well as the smoothness of the end-effector and the joints trajectory are also considered. The effectiveness of the proposed algorithm is demonstrated on the Willow Garage’s PR2 simulation platform in a wide range of pose-constrained cases.


I. INTRODUCTION
The manipulator motion planning is one of the challenging problems in the field of robot manipulators, which involves finding a collision-free path under certain constraints from an initial to a goal configuration among a collection of obstacles. This problem is known to be a PSPACE-hard problem [1] and its complexity increases with the number of the robot's degrees of freedom (DOFs) and the task constraints. Sampling-based motion planners [2]- [4] are generally regarded as the state-of-the-art method for such high-dimensional planning problems due to the outstanding The associate editor coordinating the review of this manuscript and approving it for publication was Liang Hu . advantages in efficiency, easy implementation and the ability to handle a multitude of different constraints [5]. But these planners are difficult to solve the motion planning with pose (position and/or orientation) constraints imposed on the robot's end-effector [6], [7], which are ubiquitous in industry and our daily life, e.g., moving a cup full of water from one place to another while keeping it vertically all along the motion; opening a door or rotating a valve.
There are two main factors prohibiting the adoption of sampling-based planning methods for these problems. Firstly, it is difficult to directly compute feasible configurations that lie on the desired constraint manifolds [8], [9]. For the task-constrained motion planning, one always wants to find a direct way that can quickly generate configurations meeting the task constraints. However, position and orientation of the robot's end-effector usually are coupled, which complicates the solution to the analytic inverse kinematics (IK) of the desired position or orientation. Secondly, the probability that a randomly sampling configuration in configuration space (C-space) lies on the desired constraint manifolds is extremely low and usually null. The constraint manifold is the space composed of all constraint-satisfying configurations, which is a zero-measure space of the ambient space or C-space. Intuitively, the desired constraint manifolds have small volumes in C-space and thus the probability of sampling from the constraint manifolds is very low. In the previous works [6]- [19], the numerical iteration methods based on Jacobian projection techniques have been generally used to generate configurations that satisfy pose constraints. However, such approaches have many technical challenges, such as avoiding joint limits and singularity, requiring many numerical iterations and much computing time.
In this paper, we focus on the motion planning with pose constraints (MPPC) of anthropomorphic manipulators by a Direct Projection method that can directly find the constraint-satisfying configurations without the numerical iteration. By combining the Direct Projection method with the Rapidly-exploring Random Tree (RRT) [3] algorithm, an sampling-based planning algorithm termed Direct-Projection Rapidly-exploring Random Tree (DP-RRT) is presented for planning on the constraint manifolds. In detail, we first decompose the MPPC problem into two independent subproblems of the lower-dimensional subspace, namely motion planning along end-effector paths (MPEP) [10] and motion planning with orientation constraints (MPOC) [11], [12]. Then the Direct Projection method based on the analytic IK of the anthropomorphic manipulator is proposed to project configurations of the ambient C-space onto the constraint manifolds. Finally, the DP-RRT algorithm uses the RRT algorithm to explore the ambient C-space by growing tree branches and employs the Direct Projection method to project the tree branches onto the constraint manifolds for constructing a constraint-satisfying path. An overview of the DP-RRT algorithm for the MPPC problem is presented in Figure 1.
Contributions: The proposed DP-RRT algorithm can be applied to solve the general MPPC problems for anthropomorphic robots, which are generally solved by numerical iteration methods based on Jacobian projection techniques. Our approach is different from the previously proposed numerical iteration methods, where the Direct Projection method is adopted to project configurations onto the constraint manifolds. The planning time of the DP-RRT algorithm is significantly reduced, because the analytic IK solver is applied to compute the constraint-satisfying configurations directly.

II. RELATED WORK
To address the motion planning with pose-constrained tasks, the iterative projection strategies (Figure 2), which iteratively project unsatisfying samples in C-space closer to the constraint manifolds by a projection operator, have been verified to be effective. The iterative projection technique was originally proposed for the kinematic closure constraints of closed-chain robots by the randomized gradient In the iterative projection method, the seed configurations q s represented by gray dots are pushed onto the constraint manifolds (blue space) iteratively and then connected to form a branch. descent (RGD) algorithm [13], [14]. The Tangent Space Sampling and the First-Order Retraction [15] were proposed to solve the task constraints imposed on the manipulator's endeffector, which are more invariant and effective than the RGD algorithm. More recently, Berenson et al. [16] unified the representation of pose-related constraints by the Task Space Regions (TSRs) and presented the Constrained Bi-directional Rapidly-exploring Random Tree (CBiRRT) [9] to solve the general end-effector constraints. In order to further improve the efficiency of the projection operation, planners such as Tangent Space RRT [8], Atlas RRT [17] and Tangent Bundle RRT [18] were proposed to iteratively project samples onto the approximation of constraint manifolds: tangent spaces or tangent polytopes. Kingston et al. [6] summarized the previous works and presented a framework that can decouple the constraints from sampling-based planners. Although iterative projection strategies based on Jacobian pseudoinverse are feasible, these methods have many technical challenges, such as avoiding joint limits and singularity, requiring many numerical iterations and thus taking a lot of computing time.
As for the MPEP problem of redundant anthropomorphic arms, we need to find a sequence of collision-free joint motions so that the robot's end-effector can track the desired Cartesian path. Since there are infinitely many IK solutions (self motion) for a redundant manipulator due to its kinematic redundancy, the robot can be with different joint movements to move along the same path. This provides means for avoiding obstacle under the condition of meeting the primary task. Thus, the MPEP can be considered as a special case of the redundancy resolution [19] based on the kinematic control of the manipulator, where the redundancy resolution enables the robot to achieve appropriate configurations under the constraints of velocity [20]- [23], acceleration [24], [25] or avoiding collisions. However, these approaches would suffer from the singularity and much iteration time problem, as they work at the velocity or acceleration level with the numerical iteration method. An alternative approach is the analytical inverse kinematics-based motion planning methods [10], [26] at the position level, which can only track a position path with improved efficiency. However, it is not suitable for the manipulator to track a path with both position and orientation constraints because of the nonlinear coupling between them.

III. CONSTRAINT REPRESENTATION AND DIRECT PROJECTION
In this section, a generalized representation of pose constraints will be provided firstly, which can be divided into position constraint and orientation constraint. Then the constraint manifolds will be introduced to describe the general MPPC problem. Finally, the Direct Projection method based on the analytical IK solver of 7-DOF anthropomorphic manipulators will be given to show how to project the configurations onto the constraint manifolds directly.

A. POSE CONSTRAINT REPRESENTATION
Pose constraints usually take the form of the position and/or orientation constraints of the robot's end-effector. In this work, a 6-dimensional vector function [x(t), y(t), z(t), α(t), β(t), γ (t)] T is adopted to specify the end-effector pose constraints, where we assume that the vector function only depends on a single parameter t. The vector function consists of a translation component P(t) = [x(t), y(t), z(t)] T and a 3-dimensional rotation component described by Roll-Pitch- ; y(t) ∈ (y min , y max ); z(t) ∈ (z min , z max ); α(t) ∈ (α min , α max ); β(t) ∈ (β min , β max ); γ (t) ∈ (γ min , γ max ). (1) The first three rows of C r limit the positions of the robot's end-effector in the direction of x, y and z axes (in meters) and the last three rows limit the rotations about these axes (in radians).

B. CONSTRAINT MANIFOLDS
The C-space (denoted by Q) consisting of all possible configurations is a key concept in motion planning. The advantage of using C-space is that it allows us to describe the robots of complex geometric shape with a single point in the space Q. For example, the configuration of a robot arm with n joints can be represented by an n-dimensional vector containing n joint positions, q = (q 1 , q 2 , . . . , q n ). Therefore, the robot motion planning is equivalent to the path planning of a single point in Q. The free C-space Q free ⊆ Q consists of all the collision-free configurations in Q, where the robot does not collide with obstacles or itself. The basic motion planning problem is to find a continuous path τ in Q free from an initial state q init ∈ Q free to a goal state q goal ∈ Q free , such that The above motion planning only focuses on the collision avoidance, without considering the pose constraints imposed on the robot's end-effector. It means that the manipulator's configuration parameters q 1 , q 2 , . . . , q n are independent to each other. However, for the MPPC problem, these parameters q 1 , q 2 , . . . , q n are correlated due to the nonlinear equality constraints F(q) = C r imposed on the robot. Supposing there are k (n > k > 0) equality constraints and thus a (n − k)dimensional sub-manifold can be delineated in C-space Q. The lower-dimension sub-manifold is also known as constraint manifolds M C ⊂ Q, where all the configurations must meet the constraints F(q) = C r .
As mentioned, the region of constraint manifolds is not known a priori. To generate the constraint-satisfying configurations, the planning algorithms need to either iteratively project the C-space configurations onto the constraint manifolds or directly compute analytical IK solutions that inherently satisfy the equality constraints (Direct Projection). However, computing the constraint-satisfying configurations by the analytical IK method is difficult and even infeasible due to the coupling of the end-effector position and orientation. Fortunately, for the anthropomorphic manipulators, it is possible to generate such constraint-satisfying configurations by the Direct Projection method.

C. DIRECT PROJECTION
To solve the MPPC problem, we need to project the C-space configurations onto the constraint manifolds M C . This paper presents a Direct Projection method based on the analytic IK that can directly project the C-space configurations onto M C for anthropomorphic manipulators. As is the case with human arms, anthropomorphic manipulators ( Figure 3(a)) consists of three revolute joints in the shoulder, a revolute joint in the elbow and three revolute joints in the humanoid spherical wrist (Figure 3(b)). The key feature of the spherical wrist is that it decouples the position from the orientation of end-effector motion [27], [28]. That is, the wrist position and orientation can be independently controlled by the main-arm joints q arm (the first four joints) and the wrist joints q wrist (the last three joints) respectively. This special property makes it possible to derive IK solutions in an analytic way [29] by inverse position kinematics and inverse orientation kinematics.
As shown in Figure 3(c), the Denavit-Hartenberg (D-H) [30] parameters are given to formulate the kinematic mode of a general 7-DOF anthropomorphic manipulator, where d 1 , d 3 , d 5 and d 7 are the length of shoulder, upper arm, forearm and end-effector respectively, and a 1 is the shoulder offset. Suppose that a desired pose of the end effector frame {7} relative to the base frame {0} is expressed as: where 0 7 T is the homogeneous transformation matrix consisting of a 3 × 3 rotation matrix 0 7 R for orientation and a 3 × 1 translation vector 0 7 P for position. As the robot's end-effector is mounted on the spherical wrist fixedly, we can calculate the position of the wrist 0 6 P relative to base frame {0} when the end-effector position and orientation are known.
T is a constant vector. Since the last three joint axes of the anthropomorphic manipulator intersect at the wrist, the wrist position 0 6 P is determined only by the main-arm joints q arm = (q 1 , q 2 , q 3 , q 4 ). Obviously, there are four unknowns for three scalar equations. Considering that the main-arm joint angles can be easily solved if the position of shoulder is specified, hence we choose the first angle q 1 as the redundant variable. The position of shoulder relative to the base frame {0} can be computed as 0 1 P=[a 1 c 1 , a 1 s 1 , d 1 ] T . Thus, the points of shoulder (S), elbow (E) and wrist (W) can form a triangle, and two sets of elbow joint angle q 4 can be calculated by the triangle cosine theorem where d 16 = 0 6 P − 0 1 P is the length of the triangle hypotenuse.
According to (5), the position of the wrist 0 6 P can be calculated by the known end-effector pose 0 7 T . Therefore, the following formula can be further obtained: where 0 Two sets of solution can be obtained from (8), for q 3 ∈ [ − π/2, π/2), and for q 3 ∈ [π/2, 3π/2). The notations c i and s i are the abbreviations for cos(q i ) and sin(q i ) respectively, and k 1 = d 5 c 4 + d 3 , So far, q 2 , q 3 , q 4 can be obtained by inverse position kinematic when the redundancy angle q 1 is specified. On the other hand, as the orientation of end-effector is determined by q wrist , analytical IK solutions of q wrist can be solved through inverse orientation kinematics described by 4 5 where p ij are the elements of the ( 0 4 R) −1 · 0 7 R. Thus, the following two sets of solutions can be derived for q 6 ∈ (0, π), and q 5 = atan2(−p 23 , −p 13 ) for q 6 ∈ (−π, 0). To summarize, once q 1 was chosen as the redundant variable, the remaining joint angles q 2 ∼ q 7 can be Algorithm 1 DirectProjection(q s = (q arm , q wrist )) 1: if OrientationConstraintTrue then 2: ← φ(t); 3: q wrist ← InverOrienKinem( , q arm ); 4: else 5: ← F(q s ); 6: end if 7: if PositionConstraintTrue then 8: q arm ← InverPositKinem(P(t i ), , q 1 ); 9: q wrist ← InverOrienKinem( , q arm ); 10: end if 11: return q ← (q arm , q wrist ); calculated analytically. For a given end-effector pose and an appropriate redundancy angle, up to eight sets of analytical IK solutions can be solved.
Based on the analytical IK of anthropomorphic manipulators, the pseudo-code procedure of projecting a C-space configuration q s onto the constraint manifolds is described in Algorithm 1. In detail, for the MPOC problem with a predefined orientation φ(t) (Lines 1 and 2), the inverse orientation kinematics function InverOrienKinem() tries to calculate the wrist angles q wrist for the given Euler angles without changing the position of wrist (Line 3). Obviously, the new configuration q consisting of the original q arm and the calculated q wrist can meet the given orientation constraints (Line 11). From Eq. 12 and Eq. 13, we can see that up to two configurations can be projected onto the orientation constraint manifolds.
As to the position-constrained task MPEP, the robot's end-effector needs to follow the desired path P(t) ∈ R 3 assigned with a path parameter t. In order to track the path, we discretize P(t) into a series of way-points by a sequence {t 1 , t 2 , . . . , t N }, and then complete the path planning between the way-points. Thus, the MPEP problem is to find a sequence of joint configurations {q(t 1 ), q(t 2 ), . . . , q(t N )} in C-space that satisfy the forward kinematics F(q(t i )) = P(t i ), i = 1, 2 . . . , N and such that no collision occurs along the joint-space path ( Figure 4). In order to seek the configuration(s) q such that F(q) = P(t i ), the proposed Direct Projection method projects the configuration q s of the ambient C-space onto the position constraint manifolds (or null-space manifolds) directly. Firstly, we need to check whether there are orientation constraints. If so, Euler angles is assigned the predefined orientation φ(t) (Line 2); otherwise it retains the original orientation derived by Forward Kinematic function F() (Line 5). Then, the inverse position kinematics function InverPositKinem() calculates the main-arm angles q arm with the given P(t i ) and (Line 8). Finally, InverOrienKinem() calculates the q wrist with the given and q arm (Line 9). Obviously, the new configuration q consisting of q arm and q wrist can meet the given position constraints and even orientation constraints (Line 11). As aforementioned, up to eight new configurations can be obtained.

IV. DP-RRT ALGORITHM
To address the MPPC problem for anthropomorphic manipulators, the DP-RRT algorithm is proposed to directly search on the constraint manifolds for finding a collision-free path. The proposed algorithm is based on a combined Direct Projection method and the Rapidly-exploring Random Tree (RRT) algorithm, which uses the RRT algorithm to explore the ambient C-space by growing tree branches, and employs the Direct Projection method to project the tree branches onto the constraint manifolds for constructing a constraint-satisfying path. Besides, avoiding joint limits and singularity, as well as the smoothness of the end-effector and the joints trajectory are also considered in this work.

A. ALGORITHM OVERVIEW
The DP-RRT algorithm, shown in Algorithm 2, starts with a space-filling tree (V , E) rooted at an initial node q init , a vertex set V pre&cur for storing nodes of the previous and current constraint manifolds, and an empty vertex set V current for storing nodes of the current constraint manifolds. Please note that the vertex sets V pre&cur and V current are specially designed for cases with a series of constraint manifolds like the MPEP problem.
For the MPOC problem with fixed orientation constraints, the planning process is conducted on the orientation constraint manifolds from start to finish, thus the number of constraint manifolds Num is one (Line 2 of Algorithm 2). While the number of constraint manifolds is equal to the number of way-points N for the MPEP problem (Line 4). Then, the proposed DP-RRT algorithm uses V pre&cur to expand on the current constraint manifolds iteratively. In each iteration, the new nodes q nodes and the branches δ(q near , q nodes ) are V current ← Initialize(V current ); 8: for j = 1 → n do 9: q rand ← SampleFree(j); 10: q near ← Nearest(V pre&cur , q rand ); 11: q s ← Steer(q near , q rand ); 12: q nodes ,δ(q near , q nodes )←DirectProjection(q s ); 13: q new ,δ(q near , q new )←SelectOptimum(q nodes ); 14: if CollisionChecking(δ(q near , q new )) then 15: V pre&cur ← V current ; 22: end for 23: directly projected onto the current constraint manifolds by the Direct Projection method. There will be multiple nodes and branches satisfying the constraints, but we only choose the one that satisfies the limits of joint range, singularity and the ''shortest trajectory in joint space''. Thus the function SelectOptimum() is proposed to select the optimal node q new and the branch δ(q near , q new ). Finally, in Line 14, a collision detector is employed to check whether q new and δ(q near , q new ) are collision-free by incremental checking the projected intermediation nodes. If the path δ(q near , q new ) is collision-free, q new will be added to V , V pre&cur and V current , and then δ(q near , q new ) will be added to E in Lines 15∼17. In Line 19, the above loop program iterates until the goal configuration q goal is reached.
For the MPEP problem, we can complete the motion from the previous constraint manifolds to the current through the above steps. Then V pre&cur will replace V current as the data structure to search the nearest node of the next loop while V current will be emptied and prepared for adding the new nodes of the new current constraint manifolds. The above loop program is conducted N times as way-points P(t i ) goes by until the goal region is reached.
The complexity of the above DP-RRT algorithm is analyzed by the time complexity and the space complexity. The time complexity can be calculated by the sum of the time that all nodes are added to the tree. As the operations of sampling, steer, direct projection, select optimal node and collision checking can be done in linear time for n iterations (Lines 9-17 of Algorithm 2), the complexity can be represented as O(n). The complexity of selecting the nearest node depends on the number of vertices already in the tree, thus it can be calculated as O(n 2 − n) ≈ O(n 2 ) [31]. It can be seen that the time complexity of the DP-RRT algorithm is bounded by O(Num * n 2 ), where Num is the number of constraint manifolds. The space complexity is defined as the amount of memory used, which can be represented by the number nodes in the tree. Thus, the space complexity of the DP-RRT algorithm is O(Num * n).

B. GENERIC COMPONENTS
The DP-RRT algorithm is typically composed of the following generic components, including the Direct Projection of the Space-filling Trees, the Selection of the Optimal Node, the Collision Checking and the Vertex Sets.

1) DIRECT PROJECTION OF THE SPACE-FILLING TREES
Direct Projection of the Space-filling Trees is the core of the DP-RRT algorithm, shown in Lines 8∼12 of Algorithm 2. It starts with the RRT algorithm in C-space and uses the Direct Projection method to directly project the new C-space configuration (node) q s and branch δ(q near , q s ) onto the constraint manifolds. Specifically, in Lines 9 and 10, a random node q rand is sampled in C-space and its nearest node q near is found in V pre&cur . An attempt Steer() is made to generate a new node q s and a straight path δ(q near , q s ) in C-space by moving a step-size in the straight line from q near toward q rand . It is usually that q s and δ(q near , q s ) are not on the constraint manifolds. Then, in Line 12, the C-space q s and δ(q near , q s ) are projected onto the constraint manifolds directly by the Direct Projection method (Algorithm 1), thus the new nodes q s and the branches δ(q near , q nodes ) can be generated on the constraint manifolds. It should be noted that as the intermediate nodes of the branch δ(q near , q s ) are projected onto the constraint manifolds by the Direct Projection method ( Figure 5), the branches δ(q near , q nodes ) can satisfy the pose constraints with high accuracy. This is quite different from [10], [26], where the branches between the new and nearest configurations are generated by the linear interpolation method.

2) SELECTION OF THE OPTIMAL NODE
Due to multiple solutions of the analytic IK, multiple nodes q nodes on the constraint manifolds will be projected from q s generated by the RRT algorithm (Line 12 of Algorithm 2). So firstly we need to remove the nodes that near the singularity or outside the joint limits. For the anthropomorphic manipulators with spherical wrists, the singularity can be decoupled as the main-arm singularity and wrist singularity [32]. The main-arm singularity occurs only when q 4 = 0 or q 4 = π and the wrist singularity happens only when q 6 = 0 or q 6 = π. In Line 13 of Algorithm 2, if all the possible new nodes are removed, q nodes will be an empty set.

FIGURE 5.
A sketch of the DP-RRT algorithm. The whole process generates random branches δ(q near , q s ) iteratively in C-space and the random branches will be projected onto the constraint manifolds by Direct Projection method (red solid arrow). Then, the optical new node q new and branch (yellow curve) will be retained while other nodes and branches will be eliminated. Finally, collision detection is carried out to determine whether the new node and branch should be added to the space-filling tree.
The other problem is how to select the new node when there are multiple nodes in q nodes . According to the differential kinematics of robot manipulators, differential motion of the robot's joints will lead to differential motion of its end-effector. That is to say, the differential motion of the robot's end-effector can be caused by the differential motion of its joints, but it may result from the jumping motion of the joints. Considering that the joint angles between the adjacent nodes should not change dramatically, we use the criterion of ''shortest trajectory in joint space'' to select the optimal node q new from q nodes . Thus, an Euclidean distance of joint angles is adopted as the criteria of choose. As 2π radian yields the same rotation as 0 radian for continuous joint (identification), the Euclidean distance d(q near , q new ) between vectors q near and q new is defined as: where q k near , q k new are the kth element of q near and q new respec- new |} is the S 1 metric. We will discard the IK solutions that lead to joint angles jump through this criteria. Therefore, the joint angles between the nearest and new configurations would not jump and the smoothness of the joint trajectories can be achieved. According to the differential kinematics of robot manipulators, the smoothness of the end-effector trajectory can be ensured.

3) COLLISION CHECKING
In the RRT planner, a collision detector is employed to check whether the straight path between the new node and the nearest node are collision-free by incremental checking at some interval from one end to the other along the path. For the DP-RRT algorithm, the projected path δ(q near , q new ) in joint space is not a straight path. Instead of checking the straight path between q near and q new , we incrementally check the straight segments between the projected intermediation nodes and the δ(q near , q new ) is collision-free only when all the segments are collision-free.

4) VERTEX SETS
There is only one vertex set V in RRT algorithm, which is used to store the vertices of random space-filling trees. However, for the cases of connecting a series of constraint manifolds by sequence in the MPEP problem, the current constraint manifolds V current and the current and previous constraint manifolds V pre&cur are needed. V current stores the vertices corresponding to the current pose and V pre&cur stores the vertices corresponding to the current pose and the previous pose. Thus, two different connection modes can be obtained, i.e. the forward motion from the previous constraint manifolds to the current ones, and the self motion connecting two vertices on the same constraint manifolds.
As shown in Algorithm 2, the initial node is located at the previous constraint manifolds V pre&cur = {q init }, and the space to be sampled is the current constraint manifolds V current = ∅. For the fixed orientation tasks, the planning process is conduct on the orientation constraint manifolds from beginning to end (Lines 6∼22), thus the number of constraint manifolds Num is one. For MPEP problem, the number of constraint manifolds is equal to the number of way-points N (Line 4) and the planned path can be the self motion or the forward motion. As depicted in Figure 6 (1)-(3), the DP-RRT algorithm starts from an initial node q init in the previous constraint manifolds and repeatedly generates new nodes q new on the current constraint manifolds M c (1) until n new nodes have been added to V current . Then V pre&cur will replace V current as the data structure to search the nearest node of the next loop while V current will be emptied and prepared for adding the new nodes of the new current constraint manifolds. This process corresponds to Figure 6 (4), (5) and Lines 20, 21 of Algorithm 2. As presented in Figure 6 (6), the above loop program is conducted N times as way-point P(t i ) goes by until the goal region is reached.

V. PLANNING EXPERIMENTS
To demonstrate the validity of the proposed algorithm, three sets of simulation experiments are performed by the DP-RRT planner on the humanoid PR2 (Willow Garage) simulation robot in the Robot Operating System (ROS) [33]. The ROS is a flexible framework for writing robot software and creating complex and robust robot behavior across a wide variety of robotic platforms, which has been widely used and considered as the main development framework in robotics. The CBiRRT planner [9], the RRT-connect planner [34], the Control_Based planner [21], the Task Space RRT (TS-RRT) planner [35] and the Global Redundancy Resolution PRM (GRR-PRM) planner [36] are used as the comparative methods in these simulation experiments, which are typically adopted to deal with such constraint tasks. All the algorithms are run on ROS MoveIt! simulator [37] with an open-source Open Motion Planning Library (OMPL) [38].

A. EXPERIMENTAL PLATFORM
The simulation platform of the PR2 is one of the representative humanoid robots, which has been widely used to verify the function and performance of the anthropomorphic robot [12], [17], [39]. We consider the MPPC problems for the 7-DOF anthropomorphic manipulator of the PR2 platform. The main-arm joints of the PR2 manipulator consist of four joints: shoulder pan joint (q 1 ), shoulder lift joint (q 2 ), upper arm roll joint (q 3 ) and elbow flex joint (q 4 ), which determine the position movement of the robot's wrist. While the orientation of the robot's wrist can be controlled by the wrist joints consisting of forearm roll joint (q 5 ), wrist flex joint (q 6 ) and wrist roll joint (q 7 ). The shoulder pan joint (q 1 ) is selected as the redundant joint for the analytical IK solution of the main-arm configuration.

B. EXPERIMENTS AND RESULTS
We evaluate the performance of our algorithm by applying it to three typical pose-constrained cases: end-effector orientation constraint, narrow passage constraint and end-effector path constraint. Since all the planners are randomized, 20 times repetitive experiments are performed for each planner and the statistic data of comparative experiments are given in Table 1 for Case 1, Table 2 for Case 2 and  Table 3 for Case 3. The first column in Table 1, 2 and 3 VOLUME 8, 2020 FIGURE 7. The PR2 robot moves a cup from the green pose to the yellow pose in a cluttered environment while subjecting to upright constraints by using the DP-RRT planner. Green: the start configuration. Yellow: the goal configuration. Red: the obstacles (a table, a baffle  on the table). Blue: the trajectory of the end-effector.

1) CASE 1: END-EFFECTOR ORIENTATION CONSTRAINT
When manipulating objects, it is often necessary to maintain its orientation fixed. For example, the robot is asked to pick a cup of water from one place to another while keep it from tipping or spilling all along the motion [12], [40]. In this case, orientation need to be specified in the planned trajectory. The experiments in this paper only allow for enforcing an exact value of the Euler angles, i.e., the yaw angle γ , pitch angle β and roll angle α are set to zero. Thus, the constraint can be represented as The first simulation experiment is represented in Figure 7, the PR2 robot has to move a cup vertically from the green pose to the yellow pose in a cluttered environment. According to the task, both the starting pose (green pose) and the target pose (yellow pose) must satisfy the orientation constraint with Euler angles α = β = γ = 0 radians. In order to compare the performance of the proposed planner, the CBiRRT planner and the TS-RRT planner are conducted for the same scenario. The CBiRRT planner is a typical projection-based planning method [16] that has been widely used for pose-constrained tasks. While the TS-RRT is a popular task-space planning method [35] that samples and expands nodes in the low-dimensional task space and thus can find paths with fewer nodes than the C-space planners.
According to the averaged results of 20 times repetitive experiments in Table 1, all the planners can complete the task on the PR2 simulation robot with respect to success rate. We can find that the proposed DP-RRT planner is obviously superior to the CBiRRT planner in the indicators of planning time, number of nodes and path accuracy. Besides, it consumes less computing time and gets higher precision than the TS-RRT planner, while there is little difference in the number of extended nodes between the two approaches.
The advantage in planning time can be attributed to that the DP-RRT planner uses the fast analytical IK technique to sample configurations on the orientation constraint manifolds directly rather than the numerical iteration methods. And the high accuracy of the DP-RRT planner owes to the accuracy of its intermediate nodes, which are all obtained by the direct projection method rather than the linear interpolation methods. A typical result of the snapshots from the execution of the DP-RRT planner is shown in Figure 7. The corresponding Euler angles of the robot's end-effector are fixed to zero with high accuracy as shown in Figure 8(a) and the corresponding joint position trajectories are smooth as shown in Figure 8(b).

2) CASE 2: NARROW PASSAGE CONSTRAINT
Narrow passage constraints are common in our daily life and industrial environment, e.g., the PR2 robot is assigned to take a long T-shaped broom through a hallway and then into a room [41]. Sampling-based planners are notoriously challenging at handling such problems efficiently as there are some special restrictions on the orientation and/or position of robot's end-effector within the narrow passage. Indeed, narrow passages represented in C-space is a small region but critical to the connectivity of the free space [17]. Thus, the probability of randomly sampling configurations in the narrow passage space is extremely low by the uniform  sampling strategy. Solutions for the narrow passage problem are often to generate multiple trees simultaneously like RRT-connect planner [34]. Another solution is to boost the sampling density inside narrow passage and improve the connectivity of roadmaps by the task space planner [35]. In other words, the orientation and/or position of the end-effector derived from the random sampling should be constrained within the narrow passage space.
A typical scenario is portrayed in Figure 9, the PR2 manipulator need to move a long stick from the green pose to the yellow pose through a narrow passage formed by two pillars. The narrow passage prevent the stick to go through it, unless an appropriate orientation is set [42]. Here the narrow passage region is a rectangular space in light blue: S np ={(x, y, z) | x ∈ [0.2, 0.8], y ∈ [−0.8, 0.4], z ∈ [0, 2] (in meters), whose space range depends on the constraint requirements of the task. Euler angles (α, β, γ ) of the end-effector are set to zero within the narrow passage space according to the environmental prior information. Thus, the constraint can be represented as To evaluate the DP-RRT algorithm, the comparative simulation experiment is conducted by two typical planners: the RRT-connect planner and the TS-RRT planner. To measure the efficiency improvement, we compare the Avg.Time, Min.Time, Max.Time and Avg.Nodes of the planners, as shown in Table 2.
By comparing these date of planning time, the DP-RRT planner for narrow passage constraint task is superior to the RRT-connect planner and the TS-RRT planner obviously. A typical result of the snapshots from the execution of the DP-RRT planner is shown in Figure 9. The corresponding Euler angles of the robot's end-effector and the joint positions are shown in Figure 10(a) and Figure 10(b) respectively.

3) CASE 3: END-EFFECTOR PATH CONSTRAINT
In many applications, the manipulator is assigned to trace a given end-effector path while avoiding collisions. Door opening is an evident example: the robot need grasp the door handle and rotate a given circular path around the door-hinge to open the door [43]. There are many other applications in industry like painting [23]- [25], welding [44] and so on.
We tested our algorithm on a typical scenario shown in Figure 11. The end-effector of PR2 manipulator is arranged to follow a circle path P(t) with a radius of 0.15 meters through a small window (in red) and returns to its original pose when finished. The Euler angles of the end-effector is set to α=β=γ =0. Thus, the constraint can be represented as VOLUME 8, 2020 The Euler angle trajectory of the whole motion is smooth by using the DP-RRT algorithm within narrow passage space, no jumping occurs even at the switching point. In the beginning, Euler angles of robot's end-effector are set to zero in the narrow passage space by using the DP-RRT algorithm. When out of the space, the RRT planner is adopted. The output joint position trajectories are smooth all along the motion and without jump even at the switching point. (b) The joint position trajectories are smooth throughout the motion and without jumping even at the switching point.  To compare the performance of the proposed planner, the Control_Based approach and the GRR-PRM planner are conducted for the same scenario. The Control_Based approach is a typical kinematic control method [21] that uses the Jacobian pseudoinverse technique to track a given path. And the GRR-PRM planner focuses on finding the global redundancy resolution [36] for tracking the task path.
In this case, the Control_Based approach with N = 72, the GRR-PRM planner with N = 72 and the DP-RRT planner with N = 72 and N = 180 are applied and the performance results are illustrated in Table 3. We can find that with the same number of way-points N=72, the DP-RRT planner gives better results in terms of planning time but lose superiority in path precision compared with the Control_Based planner and the GRR-PRM planner. By increasing the way-points (N ), the tracking error can be reduced, but the planning time and the number of position constraint manifolds will increase. In order to get higher tracking accuracy, we increase the number of the way-points with N =180 and the results indicate that the DP-RRT planner with N =180 outperforms the Control_Based planner and the GRR-PRM planner in terms of tracking error, while the average running time is about 9 s for both the DP-RRT planner and the Control_Based planner. The higher tracking accuracy can be achieved because the number of intermediate nodes increases significantly with the increase of way-points. And the intermediate nodes are all obtained by the Direct Projection method. A typical result of the snapshots from the execution of the DP-RRT planner with N = 72 is shown in Figure 11. The corresponding path trajectory of the robot's end-effector and joint positions are shown in Figure 12(a) and Figure 12(b), respectively.

VI. CONCLUSION
In this paper, we have presented an efficient planning algorithm to address the MPPC problem for anthropomorphic manipulators. The algorithm is to combine the RRT algorithm with the Direct Projection method, which uses the RRT algorithm to explore the ambient space by growing tree branches, and then directly projects the tree branches onto the constraint manifolds for constructing a constraint-satisfying path. As the Direct Projection method based on the analytic IK solver is applied to calculate constraint-satisfying configurations, our approach can be more efficient and accurate than the numerical methods. Three different pose-constrained cases were performed with a simulated PR2 robot in ROS MoveIt! to verify the proposed DP-RRT algorithm compared with several state-of-the-art planners planners. The results show that our approach can provide significant benefits for motion planning with pose constraints. The direct projection method also has been successfully applied to the closed-chain motion planning with orientation constraints for a class of dual-arm robots in our work [45]. It is shown that the proposed method is a widely-fit method for constrained motion planning of the anthropomorphic manipulators.