# Integrated Motion Planning for Assembly Task with Part Manipulation Using Re-Grasping
## 3 Feasible Grasp, Part Manipulation, and Integrated Planning Algorithm
### Re-Grasping Approach  
To perform the assembly task, there must be at least one feasible grasp for each pair of initial and final poses of a part. In case, if there does not exist any feasible grasp then part has to undergo an intermediate pose(s) before going to it final pose. One possibility can be in-hand manipulation such as used in [16] but it is computationally costly. We propose a re-grasping approach for part manipulation using \"orientation graph search\" which is explained below. We assume that at the intermediate pose, the parts are placed on a flat horizontal surface. A pose of a part is stable if part can sustain its pose without external support. At a stable pose, the part may be resting on one of its face or on edges or on vertices as shown in Figure 2b. Given a stable pose, any pose obtained by rotating the part along the vertical axis, without losing contact with ground, is also stable as shown in Figure 2a. We call the collection of poses obtained by rotating a given stable pose \(a\), along the vertical axis an \"orientation family\" \(\hat{a}\). We assume that for any part, there are a finite number of orientation families. Using all orientation families of a part based on its geometry, we build an orientation graph as shown in Figure 3. Each node of the orientation graph represents an orientation family of the part. Two nodes in orientation graph are connected bi-directionally by an edge if both nodes represent stable orientation families and there exists at least one feasible grasp between them. While building an orientation graph, we consider the collision between environment and hand, but not the robot body. Also, at this point, we do not consider the initial or final pose of the part in assembly. The orientation graph for a part is shown in Figure 3. Note that this graph can be computed given just the geometry of a part and the hand. The final pose, the geometry of other parts, and assembly sequences are not needed. The example of the part manipulation using re-grasping approach is presented in Figure 4.  
Figure 2: Part Orientation family example. (**a**) The local frame attached to part is represented with unit vectors \(\{\hat{x}_{b},\ \hat{y}_{b},z_{b}\}\). The orientation of part is represented with respect to the inertial frame \(\{\hat{x},\hat{y},z\}\). If the part is rotated about the \(z_{b}\) axis by any angle \(\theta\), the \(z_{b}\) will have same orientation with respect to \(z\) axis and the resulting pose will also be stable. (**b**) Different stable orientation families are represented. In each orientation family, one of the body frame axis has constant angle with \(\hat{z}\) of inertial frame. In the planning phase, we can perform re-grasp planning (if necessary) using the orientation graph search. First, at the beginning of the planning for a given piece, if the final pose of the part does not belong to any family in orientation graph (such as an inherently unstable pose if placed on flat surface, but that can be supported by other part in the assembly) then we add it as an extra node  
Figure 4: Part manipulation for L-shaped part (encircled in (**a**–**c**)) using orientation graph search. (**a**) Initial poses of the parts. (**b**) Intermediate pose of L-Shaped part after picking from its initial pose. (**c**) Final pose of L-shaped part. In the initial pose of the part, it can only be picked with the vertical approach direction of hand due to other parts in the neighborhood. In first manipulation, part is moved from orientation \(+x0\) (in (**a**)) to \(-y0\) (in (**b**)). In second manipulation, part is moved from \(-y0\) (in (**b**)) to \(-x0\) (in (**c**)). Figure 3: Orientation graph. Orientation families are represented as nodes of the graph and manipulation between two nodes is represented as an edge. An unstable orientation which can be a final pose of the part (encircled in figure with dotted line) in assembly is connected with stable orientations with only incoming edges.