Integrated Motion Planning for Assembly Task with Part Manipulation Using Re-Grasping

: This paper presents an integrated planner based on rapidly exploring random tree (RRT) for an assembly task with possible re-grasping. Given multiple grasp poses for the part to assemble, the planner chooses candidate grasp poses considering the environment (including the partially ﬁnished assembly) in addition to the initial and ﬁnal poses of the part. Orientation graph search based re-grasping approach is proposed for part manipulation which is needed when there is no feasible grasp solution for a part between its initial and ﬁnal poses. Orientation graph search helps ﬁnding a series of the intermediate poses of the part needed between its initial and ﬁnal poses so that robot can grasp and assemble it without interfering the pre-assembled parts. Then while extending the tree, the algorithm tries to connect the tree to a robot conﬁguration with a chosen candidate grasp pose. Also, since the task space undergoes changes at each step of the assembly task, a node or edge in the tree can become in collision during the assembly of later parts, making the node in collision and its descendant nodes disconnected from the whole tree. To handle this, Two stage extended RRT strategy is proposed. The disconnected parts of the main tree are put into forest, and attempts are made to re-connect the tree in the forest to main tree while extending the main tree, thus making it possible to use the disconnected part again. The algorithm is implemented in Linux based system using C ++ . The proposed algorithm is demonstrated experimentally using UR5e robot manipulator by assembling the soma puzzle pieces in di ﬀ erent 3D formations. ﬁnal assembly pose, using a single grasp as shown in Figure 1. In Figure 1a, the initial pose of the part to be assembled with the sub-assembly is shown, while Figure 1b shows the desired assembly formation of the parts. It can be seen that it is not possible to assemble the part in single pick, and place cycle. That is why, one or more re-grasping of a part may be needed to assemble it. Part manipulation approach using re-grasping is presented in next sub-section.


Introduction
The role of robots in the industry environment is growing with the advancement in the research tools in the robotic industry to improve the production efficiency. The robots are commonly used in assembly line in the industry to effectively perform repetitive assembly operations [1] and tele-manipulation in remote sites [2,3]. The combinatorial based approaches for assembly planning are widely addressed in literature [4,5] however, these are used only for assembly sequence planning for the parts to be assembled. Also, there is a recent research trend of using machine leaching based approached in robotic assembly such as learning by demonstration [6] but currently these can handle only simple operations such as inserting peg in a hole, fastening the screw etc. In order to make the robots capable to perform a task autonomously, the planning algorithms are developed (for review [7,8]). However, the assembling task is quite challenging for robots. One of the recent trends in the robotics research is to make the robots capable of planning and performing the assembly task autonomously. This multi-disciplinary using what we term two stage extend-RRT function. While extending main tree, it adds a new node q 1 in main tree in first stage and extend the q 1 to nearest forest root in second stage. This strategy helps to re-connect the forest roots to the main tree.

Contribution
The key contributions of this article are the following: (a) Integrated part manipulation and robot motion planner is proposed to perform an assembly task. (b) Orientation graph search based approach is proposed as re-grasping strategy to manipulate a part, if necessary, while performing the assembly task. (c) Collision checking strategy is proposed for changing task space. Since the task space for assembly task is not static that is why the node (robot configuration) which is valid in current state of task space but may be invalid in next stage of assembly. Because of this we used lazy collision checking based approach so that we can use same tree while doing assembly rather than creating new tree at every stage of the assembly task.

Organization of Paper
The paper is organized as follows, Section 2 presents an overview of problem statement, and the proposed algorithm. The integrated planning algorithm is presented in Section 3. Section 3.1 describes the idea of feasible grasp to perform the assembly task. The part manipulation approach based on orientation graph search is presented in Section 3.2. In Section 3.3, the planning algorithm is presented. The experimental demonstration is presented in Section 4. Finally, Section 5 presents the conclusion and future work.

Overview of Algorithm
The objective of this work is to develop an integrated planning algorithm which can simultaneously plan the feasible grasp, part manipulation and collision free C-space path for robot to assemble multiple parts. The algorithm is based on RRT, which grows a tree of robot configurations that can cover the C-space. Let K be the total number of parts to be assembled. The parts are assumed to be represented as polytopes. A body frame is attached to each part to define its position and orientation with reference to the inertial frame. Let P = (p, R) be the pose of a part where p ∈ R 3 be the position and R ∈ SO(3) be the rotation matrix representing the orientation of the part. Let P i,init , and P i, f inal be the initial, and final poses of parts, respectively where i = {1, . . . , K}. It is assumed that the initial, and final poses of the parts along with the part sequence for the assembly are known. Given the set of grasps G, which are the hand poses to grasp a part, the planner will be able to choose a feasible grasp such that robot could grasp the part to be assemble it without interfering the other parts in sub-assembly. A grasp can be represented by the position and orientation of the hand frame in grasping pose and the contact points of hand on grasped object with respect to hand frame. If it is not possible to find a feasible grasp for a part in its initial pose, the planner will find the solution that how to manipulate or re-grasp the part to find a feasible grasp solution.
The outline of the algorithm is as follows: First the sequence of poses for each part are identified using orientation graph search. Then a set of feasible grasp poses of hand are identified to pick and place the part without having collision with environment. Later, the motion of the manipulator is planned using an RRT based algorithm. By taking inspiration from Grasp-RRT [12], while growing the tree using RRT, attempts are made to connect the tree to configurations whose hand poses are the chosen feasible grasp. After successfully grasping a part, robot motion is planned to move the part to is final pose. The detail of different parts of the algorithm are presented in next section.

•
This section presents an integrated planning algorithm which plans the path for the robot to perform an assembly task. The algorithm is composed of following three major parts; Feasible grasp selection • Path planning of robot with attempt to grasp and re-grasp a part, if necessary, using orientation graph search.

•
Path planning for the robot to move the grasped part to its final assembly pose.
Following sections presents the main parts of the algorithm in details.

Feasible Grasps for an Assembly Task
In order to perform an assembly task using robotic manipulator, the grasp planning is of key importance. Finding a stable grasp for objects with arbitrary shape is an active field of research [22][23][24][25], and as mentioned Grasp-RRT tries to find a grasp in a trial-and-error based way. In general, a grasp is deemed stable or feasible considering only the robot hand (or gripper) and the object, but the environment is not considered. However, for assembly, the environment must be considered and the environment also must include the partially assembled parts which change as the assembly process progresses. In this paper, we do not focus on finding stable grasps for the given object, but how to choose a feasible grasp among the given set of grasps, which make assembly possible, and how to select intermediate grasp if re-grasping is necessary. Thus, we assume that a set of grasps (G) is given and we will find a set of feasible grasps FG ⊂ G that can be used for assembly. While selecting the set of feasible grasps, only the hand but not the whole robot body, is checked for collision with environment as well as the partial assembly at initial and final poses. Note that we keep not just one feasible grasp but a set of feasible grasps since some feasible grasp maybe actually unusable due to other constraints such as collision of the robot body environment or joint limit of the robot etc. Later, during motion planning phase, the algorithm tries to find a collision free path such that the robot can grasp the part using any one of the chosen feasible grasps. Even without considering the robot body, it may not be possible to move the part from its initial pose to its final assembly pose, using a single grasp as shown in Figure 1. In Figure 1a, the initial pose of the part to be assembled with the sub-assembly is shown, while Figure 1b shows the desired assembly formation of the parts. It can be seen that it is not possible to assemble the part in single pick, and place cycle. That is why, one or more re-grasping of a part may be needed to assemble it. Part manipulation approach using re-grasping is presented in next sub-section.
Appl. Sci. 2020, 10, x FOR PEER REVIEW 4 of 19 • Path planning of robot with attempt to grasp and re-grasp a part, if necessary, using orientation graph search.

•
Path planning for the robot to move the grasped part to its final assembly pose.
Following sections presents the main parts of the algorithm in details.

Feasible Grasps for an Assembly Task
In order to perform an assembly task using robotic manipulator, the grasp planning is of key importance. Finding a stable grasp for objects with arbitrary shape is an active field of research [22][23][24][25], and as mentioned Grasp-RRT tries to find a grasp in a trial-and-error based way. In general, a grasp is deemed stable or feasible considering only the robot hand (or gripper) and the object, but the environment is not considered. However, for assembly, the environment must be considered and the environment also must include the partially assembled parts which change as the assembly process progresses. In this paper, we do not focus on finding stable grasps for the given object, but how to choose a feasible grasp among the given set of grasps, which make assembly possible, and how to select intermediate grasp if re-grasping is necessary. Thus, we assume that a set of grasps ( ) is given and we will find a set of feasible grasps ⊂ that can be used for assembly. While selecting the set of feasible grasps, only the hand but not the whole robot body, is checked for collision with environment as well as the partial assembly at initial and final poses. Note that we keep not just one feasible grasp but a set of feasible grasps since some feasible grasp maybe actually unusable due to other constraints such as collision of the robot body environment or joint limit of the robot etc. Later, during motion planning phase, the algorithm tries to find a collision free path such that the robot can grasp the part using any one of the chosen feasible grasps. Even without considering the robot body, it may not be possible to move the part from its initial pose to its final assembly pose, using a single grasp as shown in Figure 1. In Figure 1a, the initial pose of the part to be assembled with the subassembly is shown, while Figure 1b shows the desired assembly formation of the parts. It can be seen that it is not possible to assemble the part in single pick, and place cycle. That is why, one or more regrasping of a part may be needed to assemble it. Part manipulation approach using re-grasping is presented in next sub-section.

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

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"â. We assume that for any part, there are a finite number of orientation families.
Appl. Sci. 2020, 10, x FOR PEER REVIEW 5 of 19 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 , along the vertical axis an "orientation family" . We assume that for any part, there are a finite number of orientation families. , ,̂ . The orientation of part is represented with respect to the inertial frame , ,̂ . If the part is rotated about the ̂ axis by any angle , the ̂ will have same orientation with respect to ̂ 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 ̂ of inertial frame.
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. The orientation of part is represented with respect to the inertial frame x,ŷ,ẑ . If the part is rotated about theẑ b axis by any angle θ, theẑ b will have same orientation with respect toẑ 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ẑ of inertial frame.
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. 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.   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 + 0 (in (a)) to − 0 (in (b)). In second manipulation, part is moved from − 0 (in (b)) to − 0 (in (c)).
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 to the graph. This node is connected to other nodes in the graph using directed edges from stable poses using the same method above. For a part to be assembled, given its initial, and final poses and preassembled parts before it, if initial and final poses are connected by an edge in the graph, and there exists a feasible grasp in presence of assembly constraints that means the part can be grasped and moved to its assembled pose without re-grasping. Otherwise re-grasping is needed, and intermediate poses can be found by searching a path from initial to final pose in the orientation graph other than a direct edge between them. The position of an intermediate pose can be any position in the dexterous workspace of the manipulator other than the initial or final position of the part. The next sub-section 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)).
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 to the graph. This node is connected to other nodes in the graph using directed edges from stable poses using the same method above. For a part to be assembled, given its initial, and final poses and pre-assembled parts before it, if initial and final poses are connected by an edge in the graph, and there exists a feasible grasp in presence of assembly constraints that means the part can be grasped and moved to its assembled pose without re-grasping. Otherwise re-grasping is needed, and intermediate poses can be found by searching a path from initial to final pose in the orientation graph other than a direct edge between them. The position of an intermediate pose can be any position in the dexterous workspace of the manipulator other than the initial or final position of the part. The next sub-section describes the main parts of the planning algorithm.

Integrated Part Manipulation and Robot Motion Planner
This section presents the RRT-based integrated motion planning algorithm to do grasp planning, motion planning for robot to move part to target pose and part manipulation to perform an assembly task. The outer loop of the algorithm is given in Algorithm 1. Let K be the number of the parts to be assembled. Knowing the geometries of the parts and their initial and final poses, we can find the sequence of poses for ith part as P i = P i,1 , P i,2 , . . . P i,a which are needed to grasp a part from its initial pose and move it to its final pose through intermediate poses, if any. If ith part has to undergo a number of poses during assembly process then P i,1 = P i,init is its start pose and P i,a = P i, f inal is its final pose. The set of poses P i for ith part can be found by searching the orientation graph OG i for given P i,init and P i, f inal using the part manipulation approach presented in Section 3.2. In a one pick and place cycle for a part from jth pose to ( j + 1)th pose then P i,j will be named as the start pose and P i,j+1 will be named as target pose. Let P = P 1 , . . . , P K be the set containing the sequences of poses of all part to be assembled. According to the sequence of the part assembly, parts are grasped from their start pose using Grasp_Part algorithm which is presented in Section 3.3.3 and moved to target pose using Move_Part algorithm presented in Section 3.3.4 until the part reaches its final pose. Since the proposed algorithm is based on RRT algorithm which is a sampling based motion planning algorithm. Recall that an RRT based algorithm is basically a single query algorithm [11]. On contrary, the proposed algorithm is for a multi-stage problem because it plans sequentially for pairs of start and target poses for each part to be assembled. The collision check strategy, which is presented in Section 3.3.1, considers the changing task space at each stage of the assembly task. Due to changing conditions of task space while performing an assembly task, the robot state that it is collision free or not, may vary from one stage of assembly to next. To handle this issue, the algorithm maintains a set of failed nodes (that is, the nodes which are in collision with environment in later stages) as forest roots (FR) which, at later stages, are attempted to be connected to main tree while growing tree in Two_Stage_Extend_RRT which is presented in Section 3.3.2.

Collision Checking in Changing Task Space
During the robotic assembly operation, objects in the task space are not continuously changing their position and orientation but only the grasped part moves with robot hand. Since at each step of the assembly process, one part is moved from start pose to target pose, we consider the task space as quasi-dynamic space. Due to change in task space at every step of assembly task, the robot configuration which was collision free in previous steps may not be necessarily collision free in current step of assembly as shown in Figure 5. That is why at every stage we need to make sure that nodes in the searched path are collision free with respect to current state of the task space. 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.
Appl. Sci. 2020, 10, x FOR PEER REVIEW 8 of 19 • 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 ( ). 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.
(a) (b) (c) (d) 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.
The failed nodes in 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.

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_Extend_RRT. The pseudo code is presented in the Algorithm 2.  . (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.
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.

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_Extend_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 1st 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 2nd 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.
End i f 11.
For o = o : Size P m − 1 // Pose index for ith part End For

End For
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ẑ 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 ith part in jth pose. Let f g ∈ FG then f g.ẑ hand be the z − axis of the hand frame at f g and f g.gcp be the position of grasp center point of hand at f g. Let d be the component of the distance from gcp new to f g.gcp along −( f g.ẑ hand ) and d sa f e be a safe distance margin for hand from a feasible grasp which is defined as distance of tip of a finger from f g.gcp when the finger a fully extended along f g.ẑ hand . While trying to approach the part from q new , the hand may need to re-orient itself before reaching the grasp pose P i,grasp . That is why the distance d should be at least greater than d sa f e . Each candidate of the feasible grasp is evaluated according to metric m which is as follows: Let, • α be the dot product of the ( f g.ẑ hand ), andẑ hand,new . • β be the dot product of the −( f g.ẑ hand ), and unit vector along line jointing f g.gcp to gcp new . Appl. Sci. 2020, 10, x FOR PEER REVIEW 10 of Figure 6. The idea of node association to one of the feasible grasp for a part is presented. When a be a new node added to the tree in − , be the distance of from feasible grasp of the part. If the component of the along the −̂ of the feasible grasp is greater than (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.

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 an the assembly so that the hand and the part/pre-assembled parts are collision free. But the whole rob geometry is not considered then. Similar to grasp RRT, the algorithm will try to grasp current obje while expanding the tree, but only using the feasible grasps computed earlier. If a collision free pa is found from a node in a tree to a node whose hand pose is the one of the feasible grasp, it means th part can be grasped using that grasp. Note that a grasp being feasible does not mean that it ca actually grasp the part; for example, there is no configuration for that grasp, or even if such configuration exists, it may not be connected to the initial (e.g., home) configuration of the robot. Th is why we keep a set of grasps (not just one grasp), and try to connect them while growing tree. Th pseudo code for grasp planning algorithm is presented in Algorithm 3. The metric m is a weighted sum of α and β. The node q new is associated to the feasible grasp f g among the candidate feasible grasps which has maximum value of m and d > d sa f e . This node association with a grasp point helps in choosing a grasp point during grasp planning which is presented in next sub-section.

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. f g = argmax f g j ∈A size f g j .Asc_node 6. q cand = argmin q∈ f g.Asc_node (q.d) 7.
End i f 12.
P pregrasp,hand , P grasp,hand ← Compute Hand Pose f or f g 13.
while Grasp attempt is not f ailed and P pregrasp is not reached do
End i f 20.
while Grasp attempt is not f ailed and P grasp is not reached do
End i f 28.
End while 29. End i f 30. End while 31. Close_ f ingers() 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 pr. From the set of feasible grasps FG for P i,j pose for ith part in jth pose, choose a feasible grasp f g ∈ FG with maximum number of associated nodes f g.Asc_node. Afterwards, a candidate node q cand is chosen from f g.Asc_node such that gcp cand (grasp center point of hand in configuration q cand ) has minimum distance from the f g.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 P grasp be the pose the hand grasping part at P i,j . Knowing the f g, we can define a grasp pose P grasp and a pre-grasp pose P pregrasp . In the next step, the algorithm tries to approach the P grasp via P pregrasp in task space from gcp 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 fg from q cand and q cand is removed from f g.Asc_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.

Move Part
After grasping the ith part at pose P i,j , it has to be moved to target pose 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 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â ∈ w is randomly selected in order to attempt to place the part in P i,target .
Since parts are assumed to have flat faces that is why placing the part in 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 P i,j+1 be the target pose P i,target while moving the part. To find a solution faster, a pre-target pose P preTarget is defined from where P i,target can be approached along a straight line. P preTarget has same orientation as that of in P i,target , however it is positioned at a safe distance d preasm from f g.c along −â. The d preasm 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 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. 2. while P i, j+1 is not reached 3.â = randomly choose an element f rom w 4. P preTarget , P Target ← P i,j+1 ,â 5. while q preTarget is not connected in T do 6.
End i f 10. End while 11. while P i, target is not reached do

Experiment Results
This section presents the experimental evaluation of the proposed planning algorithm by assembling the soma puzzle pieces in different formations. We used Universal Robot UR5e which is an industrial robot with 6 articulating degrees of freedom as shown in Figure 7a. The Denavit-Hartenberg (DH) parameters of UR5e are given Table 1. We used a parallel gripper shown in Figure 7b which has two degrees of freedoms for fingers

•
Linear motion to open and close the fingers. Both fingers move simultaneously. Linear motion is to grasp and release the part while the fingers rotation is to manipulate the part. Soma puzzle consisted of seven pieces with different shapes and it could be assembled in different 3D formations. All pieces had faces as flat surfaces. The surface normals of two consecutive surfaces were orthogonal to each other. A set of feasible grasps for a pair of start and target poses could be found by evaluating the geometry of part and gripper. A feasible grasp considering parallel gripper for a given pair of start and target poses of a part was considered to be one for which there was a combination of three exposed surfaces , and as follows. • An exposed surface with surface normal anti-parallel to hand approach axis while grasping the part • A pair of exposed surfaces , and for which the surface normals were anti-parallel to each other but both should have been orthogonal to surface normal of .

•
While placing grasped part in final pose using , and as mentioned above, the hand should not have interfered with environment and pre-assembled parts.
A feasible grasp using the parallel gripper is presented in Figure 8.
(a) (b) Figure 8. The idea of the feasible grasp using parallel gripper is presented using 2D example. (a) Part A (Green), and part B (Blue) are already assembled and part C (Red) is to be assembled. Part C contains three surfaces , , and which define a feasible grasp for parallel grasping as defined   Linear motion is to grasp and release the part while the fingers rotation is to manipulate the part. Soma puzzle consisted of seven pieces with different shapes and it could be assembled in different 3D formations. All pieces had faces as flat surfaces. The surface normals of two consecutive surfaces were orthogonal to each other. A set of feasible grasps for a pair of start and target poses could be found by evaluating the geometry of part and gripper. A feasible grasp considering parallel gripper for a given pair of start and target poses of a part was considered to be one for which there was a combination of three exposed surfaces s 1 , s 2 and s 3 as follows.

•
An exposed surface s 1 with surface normal anti-parallel to hand approach axis while grasping the part • A pair of exposed surfaces s 2 , and s 3 for which the surface normals were anti-parallel to each other but both should have been orthogonal to surface normal of s 1 .

•
While placing grasped part in final pose using s 1 , s 2 and s 3 as mentioned above, the hand should not have interfered with environment and pre-assembled parts.
• An exposed surface with surface normal anti-parallel to hand approach axis while grasping the part • A pair of exposed surfaces , and for which the surface normals were anti-parallel to each other but both should have been orthogonal to surface normal of .
• While placing grasped part in final pose using , and as mentioned above, the hand should not have interfered with environment and pre-assembled parts.
A feasible grasp using the parallel gripper is presented in Figure 8.
(a) (b) Figure 8. The idea of the feasible grasp using parallel gripper is presented using 2D example. (a) Part A (Green), and part B (Blue) are already assembled and part C (Red) is to be assembled. Part C contains three surfaces , , and which define a feasible grasp for parallel grasping as defined in Section 3.1. The surfaces for feasible grasp are exposed in initial pose (in (a)) as well as final pose (in (b)) pose of the part C.   Table 2. Figure 9 shows the step by step picking and placing poses of the hand and part in the assembly process. The Figure 9i-l show the part manipulation by re-grasping and Figure 9s shows the final assembly formation. Figures 10 and 11 show the assembly poses of the robot hand while assembling parts. The computation time in seconds of the three trials of five different formations is shown in Table 2. Additionally, a supplementary video has been added to show the execution of the assembly for a 4-piece formation of the soma puzzle using URe5 robot and the parallel gripper mentioned above.  Figure 9 shows the step by step picking and placing poses of the hand and part in the assembly process. The Figure 9i-l show the part manipulation by re-grasping and Figure 9s shows the final assembly formation. Figures  10 and 11 show the assembly poses of the robot hand while assembling parts. The computation time in seconds of the three trials of five different formations is shown in Table 2. Additionally, a supplementary video has been added to show the execution of the assembly for a 4-piece formation of the soma puzzle using URe5 robot and the parallel gripper mentioned above.  Figure 9 shows the step by step picking and placing poses of the hand and part in the assembly process. The Figure 9i-l show the part manipulation by re-grasping and Figure 9s shows the final assembly formation. Figures  10 and 11 show the assembly poses of the robot hand while assembling parts. The computation time in seconds of the three trials of five different formations is shown in Table 2. Additionally, a supplementary video has been added to show the execution of the assembly for a 4-piece formation of the soma puzzle using URe5 robot and the parallel gripper mentioned above.  Figure 9 shows the step by step picking and placing poses of the hand and part in the assembly process. The Figure 9i-l show the part manipulation by re-grasping and Figure 9s shows the final assembly formation. Figures  10 and 11 show the assembly poses of the robot hand while assembling parts. The computation time in seconds of the three trials of five different formations is shown in Table 2. Additionally, a supplementary video has been added to show the execution of the assembly for a 4-piece formation of the soma puzzle using URe5 robot and the parallel gripper mentioned above.  Figure 9 shows the step by step picking and placing poses of the hand and part in the assembly process. The Figure 9i-l show the part manipulation by re-grasping and Figure 9s shows the final assembly formation. Figures  10 and 11 show the assembly poses of the robot hand while assembling parts. The computation time in seconds of the three trials of five different formations is shown in Table 2. Additionally, a supplementary video has been added to show the execution of the assembly for a 4-piece formation of the soma puzzle using URe5 robot and the parallel gripper mentioned above.  Figure 9 shows the step by step picking and placing poses of the hand and part in the assembly process. The Figure 9i-l show the part manipulation by re-grasping and Figure 9s shows the final assembly formation. Figures  10 and 11 show the assembly poses of the robot hand while assembling parts. The computation time in seconds of the three trials of five different formations is shown in Table 2. Additionally, a supplementary video has been added to show the execution of the assembly for a 4-piece formation of the soma puzzle using URe5 robot and the parallel gripper mentioned above.

Conclusions and Future Work
This paper proposed an integrated planning algorithm to perform an assembly task using robotic manipulator. The algorithm takes into account the geometry of parts and their initial, and final poses to choose feasible grasps. Afterwards, it chooses a feasible grasp while planning motion of manipulator to perform assembly task. For this, each node in the tree is associated with one of the feasible grasp for each parts yet to be assembled. If the part manipulation is necessary to do an assembly operation, a re-grasping approach is proposed using orientation graph search (for each part). The algorithm finds the sequences of poses of a part between its initial and final poses so that the part can be assembled without interfering the obstacles in the environment. While assembly multiple parts, the task space undergoes changes at every step of assembly, making some previously collision-free nodes be in collision. To handle this and to be able to use the disconnected parts of the tree, a two-stage-extend-RRT method is proposed in which while growing a tree, attempts are made to reconnect the disconnected parts to the main tree.
The motion planning to perform an assembly operation is inherently a multi-query problem. While planning feasible configuration space paths for grasping a part and assembling it, often they are connected through the root node of the tree, resulting in an inefficient path. One can optimize the solution after finding the collision free paths of the assembly operation or reconnect the disconnect trees so that the path between the start and the target poses are in general shorter. In addition, the proposed planner considers a pre-computed set of grasps are given at the beginning. Later on, it chooses feasible grasp during the motion planning phase. In future work, the authors are intended to integrate the grasp planning in the algorithm. In future, the authors are intended to incorporate the safety issue while planning and executing the algorithm using proximity sensor such as [10,29].

Conflicts of Interest:
The authors declare no conflict of interest.