# 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
#### 3.3.4 Move Part

After grasping the \(i\)th part at pose \(\overline{P}_{i,j}\), it has to be moved to target pose \(\overline{P}_{i,j+1}\). Algorithm 4 presentes the pseudo code for the move part planning. It starts with finding the set of feasible approach direction to reach the target pose of the part. In [26], an approach based on \"non-directional blocking graph\" is presented to dis-assemble a part from an assembly by evaluating the geometry of parts in it. The main idea of non-directional blocking graph was that if we want to remove a part \(x\) from an assembly using translational motion then how the other parts in assembly block the motion of \(x\). Using non-directional blocking graph, we can identify a set of directions of translational motions to remove a part from a given assembly. Using the same approach in reverse, given a sub-assembly of parts (including the ground surface) we can identify a set of feasible approach directions to place a part in given target pose \(\overline{P}_{i,j+1}\). However, for an intermediate target pose, there is no subassembly constraining the approach direction to place part on ground, except the ground itself. Let \(w\) be the set of feasible approach directions to place the grasped part in target pose. After finding \(w\), one of the feasible approach directions \(\hat{a}\in w\) is randomly selected in order to attempt to place the part in \(\overline{P}_{i,target}\). Since parts are assumed to have flat faces that is why placing the part in \(\overline{P}_{i,j+1}\) may become a narrow gap problem for motion planning due to presence of already assembled parts. Though the narrow gap problem is solvable using RRT, however it takes relatively longer time to find a solution [27]. Let \(\overline{P}_{i,j+1}\) be the target pose \(\overline{P}_{i,target}\) while moving the part. To find a solution faster, a pre-target pose \(P_{preTarget}\) is defined from where \(\overline{P}_{i,target}\) can be approached along a straight line. \(P_{preTarget}\) has same orientation as that of in \(\overline{P}_{i,target}\), however it is positioned at a safe distance \(d_{prasm}\) from \(fg.c\) along \(-\hat{a}\). The \(d_{prasm}\) can be chosen as multiple of finger length. The node \(q_{preTarget}\) for \(P_{preTarget}\) is computed using inverse kinematics. Basic-RRT is used to connect \(q_{grasp}\) to \(q_{preTarget}\). Once \(q_{preTarget}\) is reached, the inverse jacobian is used to approach \(\overline{P}_{i,target}\) from \(P_{preTarget}\). If the assembly of current part is failed due to collision or violation of joint limits, then the currently chosen assembly approach direction will be deleted from set \(w\). The algorithm will re-start finding path by again randomly choosing assembly approach direction from remaining set of assembly approach directions. It continues until it finds the solution. The experimental evaluation of the proposed planning algorithm is presented in next section. ```
1.\(w=Set\) of approach directions for \(P_{i,j+1}\)
2.while\(\left(\overline{P}_{i,j+1}\) is not reached\right)
3.\(\hat{a}=randomly\) choose an element from \(w\)
4.\(P_{preTarget}\), \(P_{Target}\leftarrow\left(\overline{P}_{i,j+1},\hat{a}\right)\)
5.while\(\left(q_{preTarget}\) is not connected in \(\mathcal{T}\right)\) do
6.\(Two\_Stage\_Extend\_RRT\left(\mathcal{T}\), \(\overline{P}\), \(i,j\right)\)
7.\(if\ (rand<pr)\)
8.\(RRT\_Connect\left(\mathcal{T}_{root},q_{preTarget}\right)\)
9.\(End\ if\)
10. End while
11.while\(\left(\overline{P}_{i,\ target}\) is not reached\right) do
12.\(Approach\left(\overline{P}_{i,\ target}\), \(P_{preTarget}\right)\)
13.\(if\ (failed)\)
14. delete \(\hat{a}\) from \(w\)
15. break
16.\(End\ if\)
17.\(End\) while
18.\(End\) while
19.\(Open\_fingers\)
```

**Algorithm 4**Move_Part\(\left(\overline{P}\), \(i,\ j\right)\)

## 4 Experiment Results

This section presents the experimental evaluation of the proposed planning algorithm by assembling the soma puzzle pieces in different formations.