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.