# 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
```
1.while(\(\mathit{Grasp\_Part}\left(\overline{P},\,i,j\right)\)
2.\(\mathit{Two\_Stage\_Extend\_RRT}\left(\mathcal{T},\,\overline{P},\,i,j\right)\)
3.if\((\mathit{rand}<\mathit{pr})\)
4.\(A=\mathit{set}\) of feasible grasp of \(\mathit{ith}\) part in \(\mathit{jth}\) pose
5.\(\mathit{fg}=\underset{\mathit{g\in\mathit{fg}}\in A}{\mathit{argmax}}\left( \mathit{size}\left(\mathit{fg}_{\mathit{j}}.\mathit{Acc\_node}\right)\right)\)
6.\(q_{\mathit{cand}}=\underset{\mathit{q\in fg}\in A\mathit{sec\_node},\mathit{ made}}{\mathit{arg\_Path}(\mathit{\mathcal{T}}_{\mathit{root}},\mathit{q_{cand}})\)
7.\(\mathit{Find\_Path}(\mathcal{T}_{\mathit{root}},\mathit{q_{cand}})\)
8.\(\mathit{if}\left(\mathit{failed}\right)\)
9.\(\mathit{Remove}(\mathit{fg}.\mathit{Acc\_node},\mathit{q_{cand}})\)
10.\(\mathit{Grasp\_attempt}\) failed
11.\(\mathit{End}\) if
12.\(\mathit{P}_{\mathit{pragusp\_hand}}\), \(\mathit{P}_{\mathit{grasp\_hand}}\leftarrow\mathit{Compute}\mathit{Hand}\) Pose for \(\mathit{fg}\)
13.\(\mathit{while}\left(\mathit{Grasp\_attempt}\mathit{is}\mathit{not}\) failed and \(\mathit{P}_{\mathit{pragusp}}\) is not reached\(\mathit{do}\)
14.\(\mathit{Approach}\left(\mathit{P}_{\mathit{pragusp}},\,\mathit{q_{cand}}\right)\)
15.\(\mathit{if}\left(\mathit{failed}\right)\)
16.\(\mathit{Remove}(\mathit{fg}.\mathit{Acc\_node},\mathit{q_{cand}})\)
17.\(\mathit{Grasp\_attempt}\) failed
18.\(\mathit{break}\)
19.\(\mathit{End}\) if
20.\(\mathit{End}\) while
21.\(\mathit{while}\left(\mathit{Grasp\_attempt}\mathit{is}\mathit{not}\) failed and \(\mathit{P}_{\mathit{grasp}}\) is not reached\(\mathit{do}\)
22.\(\mathit{Approach}\left(\mathit{P}_{\mathit{grasp}},\,\mathit{P}_{\mathit{ pragusp}}\right)\)
23.\(\mathit{if}\left(\mathit{failed}\right)\)
24.\(\mathit{Remove}(\mathit{fg}.\mathit{Acc\_node},\mathit{q_{cand}})\)
25.\(\mathit{Grasp\_attempt}\) failed
26.\(\mathit{break}\)
27.\(\mathit{End}\) if
28.\(\mathit{End}\) while
29.\(\mathit{End}\) if
30.\(\mathit{End}\) while
31.\(\mathit{Close\_fingers}()\)
```
**Algorithm 3**\(\mathit{Grasp\_Part}\left(\overline{P},\,i,j\right)\)

The grasp planning algorithm continues to grow the tree using the Two_Stage_Extend_RRT and attempts to grasp the part from given start pose with the probability \(\mathit{pr}\). From the set of feasible grasps \(\mathit{FG}\) for \(\overline{P}_{i,j}\) pose for \(\mathit{ith}\) part in \(\mathit{jth}\) pose, choose a feasible grasp \(\mathit{fg}\in\mathit{FG}\) with maximum number of associated nodes \(\mathit{fg}.\mathit{Acc\_node}\). Afterwards, a candidate node \(q_{\mathit{cand}}\) is chosen from \(\mathit{fg}.\mathit{Acc\_node}\) such that \(\mathit{gcp}_{\mathit{cand}}\) (grasp center point of hand in configuration \(q_{\mathit{cand}}\)) has minimum distance from the \(\mathit{fg}.\mathit{gcp}\). This is because while approaching the part from nearest candidate node the grasp attempt will have less chances of failure due to violation of joint limits and collision. Let \(\mathit{P}_{\mathit{grasp}}\) be the pose the hand grasping part at \(\overline{P}_{i,j}\). Knowing the \(\mathit{fg}\), we can define a grasp pose \(\mathit{P}_{\mathit{grasp}}\) and a pre-grasp pose \(\mathit{P}_{\mathit{pragusp}}\). In the next step, the algorithm tries to approach the \(\mathit{P}_{\mathit{grasp}}\) via \(\mathit{P}_{\mathit{pragusp}}\) in task space from \(\mathit{gcp}_{\mathit{cand}}\) using _inverse jacobian approach_ and incrementally adding new node to the tree. While approaching the part using inverse jacobian approach, if a new node is found to be in collision or violating the joint limits of the robot then, the algorithm stops to reach \(\mathit{fg}\) from \(q_{\mathit{cand}}\) and \(q_{\mathit{cand}}\) is removed from \(\mathit{fg}.\mathit{Acc\_node}\). If the grasp attempt failed, the grasp planning algorithm re-starts again until the part is grasped successfully. Once the grasp pose is reached, the fingers are closed to grasp the part.