# Integrated Motion Planning for Assembly Task with Part Manipulation Using Re-Grasping
## 3 Feasible Grasp, Part Manipulation, and Integrated Planning Algorithm
### Integrated Part Manipulation and Robot Motion Planner
We used Universal Robot UR5e which is an industrial robot with 6 articulating degrees of freedom as shown in Figure 7a. The Denavit-Hartenberg (DH) parameters of UR5e are given Table 1. We used a parallel gripper shown in Figure 7b which has two degrees of freedoms for fingers

* Linear motion to open and close the fingers. Both fingers move simultaneously. * Rotation of fingers by \(\pm\frac{\pi}{2}\) about the axis of linear motion of fingers. Linear motion is to grasp and release the part while the fingers rotation is to manipulate the part. Soma puzzle consisted of seven pieces with different shapes and it could be assembled in different 3D formations. All pieces had faces as flat surfaces. The surface normals of two consecutive surfaces were orthogonal to each other. A set of feasible grasps for a pair of start and target poses could be found by evaluating the geometry of part and gripper. A feasible grasp considering parallel gripper for a given pair of start and target poses of a part was considered to be one for which there was a combination of three exposed surfaces \(s_{1}\), \(s_{2}\) and \(s_{3}\) as follows. * An exposed surface \(s_{1}\) with surface normal anti-parallel to hand approach axis while grasping the part
* A pair of exposed surfaces \(s_{2}\), and \(s_{3}\) for which the surface normals were anti-parallel to each other but both should have been orthogonal to surface normal of \(s_{1}\). * While placing grasped part in final pose using \(s_{1}\), \(s_{2}\) and \(s_{3}\) as mentioned above, the hand should not have interfered with environment and pre-assembled parts. \begin{table}
\begin{tabular}{c c c c} \hline \hline \(\alpha_{i}\) & \(a_{i}\) (In mm) & \(d_{i}\) (In mm) & \(\theta_{i}\) \\ \hline \(\frac{\pi}{2}\) & 0 & 162.5 & \(\theta_{1}\) \\ 0 & \(-\)425 & 0 & \(\theta_{2}\) \\ 0 & \(-\)392.2 & 0 & \(\theta_{3}\) \\ \(\frac{\pi}{2}\) & 0 & 133.3 & \(\theta_{4}\) \\ \(-\frac{\pi}{2}\) & 0 & 99.7 & \(\theta_{5}\) \\ 0 & 0 & 99.6 & \(\theta_{6}\) \\ \hline \hline \end{tabular}
\end{table} 
Table 1: DH parameters of UR5e robot manipulator. Figure 7: (a) UR5e Robot manipulator (b) Parallel gripper with 2-dof of fingers. Both fingers of the parallel gripper can simultaneously rotate about the axis of rotation shown as dotted line. Both fingers can also translate simultaneously to grasp and release the part. A feasible grasp using the parallel gripper is presented in Figure 8.

The algorithm is implemented in C++ in Linux based system. The algorithm is executed on PC with Core i7 CPU with 1.8 GHz and 16 Gigabyte RAM. The proposed algorithm is implemented using open source Open Motion Planning Library (OMPL) [28] and it is demonstrated by assembling the soma puzzle pieces in different formations as shown in Figures 9-11 and Table 2. Figure 9 shows the step by step picking and placing poses of the hand and part in the assembly process. The Figure 9-1 show the part manipulation by re-grasping and Figure 9 shows the final assembly formation. Figures 10 and 11 show the assembly poses of the robot hand while assembling parts. The computation time in seconds of the three trials of five different formations is shown in Table 2. Additionally, a supplementary video has been added to show the execution of the assembly for a 4-piece formation of the soma puzzle using URe5 robot and the parallel gripper mentioned above. Figure 9: Soma puzzle assembly in formation 1 (shown in (**s**)). From (**a–r**) the figures show the snapshots of the grasping and assembly poses of the gripper and part during the assembly process. Figures (**e–h**) and (**i–I**) shows the part manipulation through intermediate poses of parts. Figure 11: Soma puzzle assembly in formation 3 (shown in **(i)**). From **(a–h)** the figures show the snapshots of the assembly poses of the gripper and part during the assembly process. Figure 10: Soma puzzle assembly in formation 2 (shown in **(I)**). From **(a–k)** the figures show the snapshots of the assembly poses of the gripper and part during the assembly process.