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
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
part as
which are needed to grasp a part from its initial pose and move it to its final pose through intermediate poses, if any. If
has to undergo
number of poses during assembly process then
is its start pose and
is its final pose. The set of poses
for
part can be found by searching the orientation graph
for given
and
using the part manipulation approach presented in
Section 3.2. In a one pick and place cycle for a part from
pose to
pose then
will be named as the start pose and
will be named as target pose. Let
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 (
) which, at later stages, are attempted to be connected to main tree while growing tree in
which is presented in
Section 3.3.2.
Algorithm 1. Motion Planner for Assembly Task |
1. |
2. = Build Orientation graph for part |
3. = Graph_Search (, , ) |
4. |
5. // Part Index |
6. // Pose Index |
7. |
8. |
9. |
10. |
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_Extend_RRT. The pseudo code is presented in the Algorithm 2. Considering a set of the forest roots (
) 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 is added to the main tree in a conventional way using 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 . First it finds a closest node in FR, which is closest to . Then we extend the main tree from towards . If they are connected, it means the tree having as the root is reconnected to the main tree. If so, is removed from FR. If the fails, the node during the connecting attempt is returned.
For each , 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 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.
Algorithm 2. |
1. |
2. |
3. |
4. |
5. |
6. |
7. // Part index |
8. |
9. |
10. |
11. // Pose index for ith part |
12. |
13. |
14. |
15. |
16. |
The node association with one of the feasible grasps for a part is elaborated in
Figure 6. Let
be a configuration of the robot as a new node added to tree. Let
be the grasp center point of hand in robot configuration
, defined at the center of palm of the robot hand and
be the
-axis of hand frame at
, defined as normal to the palm of the hand. Recall that
be the set of feasible grasps for
part in
pose. Let
then
be the
of the hand frame at
and
be the position of grasp center point of hand at
. Let
be the component of the distance from
to
along
and
be a safe distance margin for hand from a feasible grasp which is defined as distance of tip of a finger from
when the finger a fully extended along
. While trying to approach the part from
, the hand may need to re-orient itself before reaching the grasp pose
. That is why the distance
should be at least greater than
. Each candidate of the feasible grasp is evaluated according to metric
which is as follows: Let,
be the dot product of the , and .
be the dot product of the , and unit vector along line jointing to .
The metric is a weighted sum of and . The node is associated to the feasible grasp among the candidate feasible grasps which has maximum value of and . 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.
Algorithm 3. |
1. |
2. () |
3. |
4. |
5. = |
6. |
7. |
8. |
9. |
10. |
11. |
12. |
13. |
14. |
15. |
16. |
17. |
18. |
19. |
20. |
21. |
22. |
23. |
24. |
25. |
26. |
27. |
28. |
29. |
30. |
31. |
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 . From the set of feasible grasps for pose for part in pose, choose a feasible grasp with maximum number of associated nodes . Afterwards, a candidate node is chosen from such that (grasp center point of hand in configuration ) has minimum distance from the . 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 be the pose the hand grasping part at . Knowing the , we can define a grasp pose and a pre-grasp pose . In the next step, the algorithm tries to approach the via in task space from using 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 and is removed from . 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.
3.3.4. Move Part
After grasping the
part at pose
, it has to be moved to target pose
. 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
from an assembly using translational motion then how the other parts in assembly block the motion of
. 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
. However, for an intermediate target pose, there is no subassembly constraining the approach direction to place part on ground, except the ground itself. Let
be the set of feasible approach directions to place the grasped part in target pose. After finding
, one of the feasible approach directions
is randomly selected in order to attempt to place the part in
.
Since parts are assumed to have flat faces that is why placing the part in
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
be the target pose
while moving the part. To find a solution faster, a pre-target pose
is defined from where
can be approached along a straight line.
has same orientation as that of in
, however it is positioned at a safe distance
from
along
. The
can be chosen as multiple of finger length. The node
for
is computed using inverse kinematics. Basic-RRT is used to connect
to
. Once
is reached, the inverse jacobian is used to approach
from
. 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
. 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.
Algorithm 4. |
1. |
2. |
3. |
4. |
5. |
6. () |
7. |
8. |
9. |
10. |
11. |
12. |
13. |
14. from |
15. |
16. |
17. |
18. |
19. |