Manipulation Planning with Soft Constraints by Randomized Exploration of the Composite Configuration Space

In this paper, an efficient and probabilistic complete planning algorithm called Composite-space RRT is presented to address motion planning with soft constraints for spherical wrist manipulators. Firstly, we propose a novel configuration space termed Composite Configuration Space (“Composite Space” for short), which is composed of the joint space and the task space. Then, collision-free paths are generated in the composite space by the Rapidly-exploring Random Trees (RRT) algorithm. Finally, the planned paths in the composite space are mapped into the corresponding joint-space paths by a local planner. As the analytical inverse kinematics (IK) of the spherical wrist is used in the local planner, the proposed Composite-space RRT algorithm is characterized by high efficiency and no numerical iteration. Moreover, this approach can effectively improve the smoothness of the end-effector orientation path. The effectiveness of the proposed algorithm is demonstrated on the Willow Garage’s PR2 simulation platform with two typical orientation-constrained cases.