# 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  
This section presents the RRT-based integrated motion planning algorithm to do grasp planning, motion planning for robot to move part to target pose and part manipulation to perform an assembly task. The outer loop of the algorithm is given in Algorithm 1. Let \(K\) be the number of the parts to be assembled. Knowing the geometries of the parts and their initial and final poses, we can find the sequence of poses for \(i\)th part as \(\overline{P}_{i}=\left\{\overline{P}_{i,1},\overline{P}_{i,2},\ldots\overline{ P}_{i,a}\right\}\) which are needed to grasp a part from its initial pose and move it to its final pose through intermediate poses, if any. If \(i\)th part has to undergo \(a\) number of poses during assembly process then \(\overline{P}_{i,1}\left(\overline{P}_{i,init}\right)\) is its start pose and \(\overline{P}_{i,a}\left(\overline{P}_{i,final}\right)\) is its final pose. The set of poses \(\overline{P}_{i}\) for \(i\)th part can be found by searching the orientation graph \(OG_{i}\) for given \(\overline{P}_{i,init}\) and \(\overline{P}_{i,final}\) using the part manipulation approach presented in Section 3.2. In a one pick and place cycle for a part from \(j\)th pose to \((j+1)\)th pose then \(\overline{P}_{i,j}\) will be named as the start pose and \(\overline{P}_{i,j+1}\) will be named as target pose. Let \(\overline{P}=\left\{\overline{P}_{1},\ldots,\overline{P}_{K}\right\}\) be the set containing the sequences of poses of all part to be assembled. According to the sequence of the part assembly, parts are grasped from their start pose using Grasp_Part algorithm which is presented in Section 3.3.3 and moved to target pose using Move_Part algorithm presented in Section 3.3.4 until the part reaches its final pose. Since the proposed algorithm is based on RRT algorithm which is a sampling based motion planning algorithm. Recall that an RRT based algorithm is basically a single query algorithm [11]. On contrary, the proposed algorithm is for a multi-stage problem because it plans sequentially for pairs of start and target poses for each part to be assembled. The collision check strategy, which is presented in Section 3.3.1, considers the changing task space at each stage of the assembly task. Due to changing conditions of task space while performing an assembly task, the robot state that it is collision free or not, may vary from one stage of assembly to next. To handle this issue, the algorithm maintains a set of failed nodes (that is, the nodes which are in collision with environment in later stages) as forest roots (_FR_) which, at later stages, are attempted to be connected to main tree while growing tree in _Two_Stage_Extend_RRT_ which is presented in Section 3.3.2. ```
1. For \(i=1:K\)
2. \(OG_{i}\) = Build Orientation graph for \(i\)th part
3. \(\overline{P}_{i}\) = Graph_Search (\(OG_{i}\), \(P_{i,init}\), \(P_{i,final}\))
4. End For
5. For \(i=1:K\) // Part Index
6. For \(j=1:Size(\overline{P}_{i})-1\) // Pose Index
7. Grasp_Part(\(\overline{P}_{i}\), \(i\), )
8. Move_Part(\(\overline{P}_{i}\), \(i\), )
9. End For
10. End For

#### 3.3.1 Collision Checking in Changing Task Space

During the robotic assembly operation, objects in the task space are not continuously changing their position and orientation but only the grasped part moves with robot hand. Since at each step of the assembly process, one part is moved from start pose to target pose, we consider the task space as quasi-dynamic space. Due to change in task space at every step of assembly task, the robot configuration which was collision free in previous steps may not be necessarily collision free in current step of assembly as shown in Figure 5. That is why at every stage we need to make sure that nodes in the searched path are collision free with respect to current state of the task space.