Integrated Motion Planning for Assembly Task with Part Manipulation Using Re-Grasping

This paper presents an integrated planner based on rapidly exploring random tree (RRT) for an assembly task with possible re-grasping. Given multiple grasp poses for the part to assemble, the planner chooses candidate grasp poses considering the environment (including the partially finished assembly) in addition to the initial and final poses of the part. Orientation graph search based re-grasping approach is proposed for part manipulation which is needed when there is no feasible grasp solution for a part between its initial and final poses. Orientation graph search helps finding a series of the intermediate poses of the part needed between its initial and final poses so that robot can grasp and assemble it without interfering the pre-assembled parts. Then while extending the tree, the algorithm tries to connect the tree to a robot configuration with a chosen candidate grasp pose. Also, since the task space undergoes changes at each step of the assembly task, a node or edge in the tree can become in collision during the assembly of later parts, making the node in collision and its descendant nodes disconnected from the whole tree. To handle this, Two stage extended RRT strategy is proposed. The disconnected parts of the main tree are put into forest, and attempts are made to re-connect the tree in the forest to main tree while extending the main tree, thus making it possible to use the disconnected part again. The algorithm is implemented in Linux based system using C++. The proposed algorithm is demonstrated experimentally using UR5e robot manipulator by assembling the soma puzzle pieces in different 3D formations.