# 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
End For
```
**Algorithm 2**Two_Stage_Extend_RRT\(\big{(}\mathcal{T},\,\overline{p},\,i,j\big{)}\)

The node association with one of the feasible grasps for a part is elaborated in Figure 6. Let \(q_{new}\) be a configuration of the robot as a new node added to tree. Let \(gcp_{new}\) be the grasp center point of hand in robot configuration \(q_{new}\), defined at the center of palm of the robot hand and \(z_{hand,new}\) be the \(z\)-axis of hand frame at \(q_{new}\), defined as normal to the palm of the hand. Recall that \(FG\) be the set of feasible grasps for \(i\)th part in \(j\)th pose. Let \(fg\in FG\) then \(fg.z_{hand}\) be the \(z-axis\) of the hand frame at \(fg\) and \(fg.gcp\) be the position of grasp center point of hand at \(fg\). Let \(d\) be the component of the distance from \(gcp_{new}\) to \(fg.gcp\) along \(-(fg.z_{hand})\) and \(d_{safe}\) be a safe distance margin for hand from a feasible grasp which is defined as distance of tip of a finger from \(fg.gcp\) when the finger a fully extended along \(fg.z_{hand}\). While trying to approach the part from \(q_{new}\), the hand may need to re-orient itself beforereaching the grasp pose \(P_{i,grasp}\). That is why the distance \(d\) should be at least greater than \(d_{safe}\). Each candidate of the feasible grasp is evaluated according to metric \(m\) which is as follows: Let,

\(\bullet\)\(\alpha\) be the dot product of the \((fg.\hat{z}_{hand})\), and \(\hat{z}_{hand,new}\). \(\bullet\)\(\beta\) be the dot product of the \(-(fg.\hat{z}_{hand})\), and unit vector along line jointing \(fg.gcp\) to \(gcp_{new}\). The metric \(m\) is a weighted sum of \(\alpha\) and \(\beta\). The node \(q_{new}\) is associated to the feasible grasp \(fg\) among the candidate feasible grasps which has maximum value of \(m\) and \(d>d_{safe}\). This node association with a grasp point helps in choosing a grasp point during grasp planning which is presented in next sub-section. #### 3.3.3 Grasp Planning

The grasp planning part of the proposed algorithm is a modified version of Grasp-RRT [12]. Recall that we choose a set of feasible grasps (not just one grasp) considering the hand geometry and the assembly so that the hand and the part/pre-assembled parts are collision free. But the whole robot geometry is not considered then. Similar to grasp RRT, the algorithm will try to grasp current object while expanding the tree, but only using the feasible grasps computed earlier. If a collision free path is found from a node in a tree to a node whose hand pose is the one of the feasible grasp, it means the part can be grasped using that grasp. Note that a grasp being feasible does not mean that it can actually grasp the part; for example, there is no configuration for that grasp, or even if such a configuration exists, it may not be connected to the initial (e.g., home) configuration of the robot. That is why we keep a set of grasps (not just one grasp), and try to connect them while growing tree. The pseudo code for grasp planning algorithm is presented in Algorithm 3. Figure 6: The idea of node association to one of the feasible grasp for a part is presented. When a \(q_{new}\) be a new node added to the tree in \(C-space\), \(d\) be the distance of \(gcp_{new}\) from feasible grasp of the part. If the component of the \(d\) along the \(-\hat{z}_{hand}\) of the feasible grasp is greater than \(d_{safe}\) (minimum safe distance of hand from part to change orientation of hand), then the new node will be associated to the feasible grasp which has normal more closely aligned to the line joining the grasp center point and center of the square.