# 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 are using semi-lazy collision checking which is as follows:

* When a node is generated, it is checked that robot is not in self-collision as well as not in collision with static environment obstacles which includes floor and other static equipment in the task space. Though the collision checking for the nodes is performed again after searching the path, collision checking for nodes at earlier stage minimizes the chances of failure at later stage. This is because a configuration node which is collision free while adding to tree, later on it may only be in collision with some movable obstacles. * Collision along the edges of the tree are not checked while growing the tree because it consumes a lot of time. In order to avoid the un-necessary computations, the collision checking along the edge is performed only for those which are included in the searched path. * If an edge along the path is found to be in collision, this edge is deleted and the child node is stored in the forest roots (\(FR\)). That is, if an edge (\(a\),\(b\)) is found to be in collision, and the node \(a\) is the parent node, the node \(b\), the edge (\(a\),\(b\)) is deleted and the node \(b\) is put into \(FR\). Note that the node \(b\) is the root of a tree which is disconnected from the main tree. The failed nodes in \(FR\) are tried to be connected to the main tree while extending the tree. The two stage extend tree is presented in the next sub-section. Figure 5: Collision status of a robot state in changing task space. (**a**,**b**) shows that the robot configuration is collision free in initial state of task space. (**c**,**d**) shows the robot state, which was collision free in initial stage of assembly task, is now in collision with environment due to change in task space. #### 3.3.2 Extending Tree in Two Steps

Due to the multi-query nature of the motion planning problem under consideration and the collision checking strategy presented in Section 3.3.1, the conventional approach to extend the tree needs to be modified as Two_Stage_Exend_RRT. The pseudo code is presented in the Algorithm 2. Considering a set of the forest roots (\(FR\)) containing failed nodes which were part of the tree but were disconnected from tree due to the edge found in collision while finding a collision free path. The main idea of the two stage extend tree is following,

* In the 1 st stage, a new collision free node \(q_{new}\) is added to the main tree in a conventional way using _Extend_Tree_ function but without checking collision along the edge connecting new node to the tree. * In the 2 nd stage, the algorithm tries to connect the main tree and one of the disconnected trees using \(q_{new}\). First it finds a closest node \(q_{cl}\) in FR, which is closest to \(q_{new}\). Then we extend the main tree from \(q_{new}\) towards \(q_{cl}\). If they are connected, it means the tree having \(q_{cl}\) as the root is reconnected to the main tree. If so, \(q_{cl}\) is removed from FR. If the fails, the node during the connecting attempt is returned. * For each \(q_{new}\), either in Stage 1 or 2, it is associated to one of the grasps for each part yet to be assembled including the part manipulation sequences. The Node association to a grasp point is described below. * Similar to the grasp-RRT, the newly added node \(q_{new}\) is associated to one of the grasps for each part yet to be assembled including the part manipulation sequences (but not to the surface of an abstract object like, grasp sphere in grasp-RRT). The details are described below. ```
1. For \(N=1:2\)
2. if\((N=1)\)
3. \(q_{new}=Extend\_Tree(\mathcal{T})\)
4. else
5. \(q_{new}=Extend\_to\_Forest(\mathcal{T}\), \(q_{new}\), \(FR)\)
6. End if
7. For \(m=i:K\)// Part index
8. \(if(m=i)\;\overline{o}=j\)
9. else\(\;\overline{o}=1\)
10. End if
11. For \(o=\overline{o}:Size(\overline{p}_{m})-1\)// Pose index for ith part
12. \(\overline{fg}=argmax(m_{k})\) and \((d>d_{safe})\)
13. \(\overline{fg}.\)Acc_node \(=add\big{(}\overline{fg}\), \(q_{new}\), \(d\big{)}\)
14. End For
15. End For
16.