Preparatory manipulation planning automatically finds a sequence of robot motion that manipulates and prepares an object for specific tasks . The reason preparatory manipulation is needed is that objects in the real world are not always well posed. Take Fig.1 for example. In the figure, an electric drill is in two initial poses shown in Fig.1(a.1) and Fig.1(b.1). To use the drill, a human worker has to reorient the drill to let the tooltip face to targets like Fig.1(a.4) and (b.4). The reorienting process is named preparatory manipulation.
Fig.1 shows two ways to do preparatory manipulation. The first one is single-arm regrasp shown in Fig.1(a.1-a.4), where the human worker picks up the electric tool using his right hand in (a.1), places it down on the table in (a.2), changes the pose of his right hand to regrasp the tool in (a.3), and successfully reorients the tooltip in (a.4). Only a single arm (the right arm and right hand) is used in the process. The second one is dual-arm handover shown in Fig.1(b.1-b.4), where the human picks up the electric tool using his left hand in (b.1), moves it to a hand-over pose in (b.2), hands it over to his right hand in (b.3), and reorients the tooltip in (b.4). Both arms are used in the second case.
Preparing objects using single-arm and dual-arm manipulation is common and important to handle objects in our daily life. It motivates us to develop algorithms for robots to have similar manipulation skills. In the paper, we challenge the difficulty of planning multiple robot motions together and automatically selecting the necessary number of arms. We develop algorithms to find a sequence of collision-free and IK-feasible motion to move and reorient objects, and propose a super regrasp graph that helps to determine the sequence of multi-arm handover and whether to use a single arm, dual arms, or their combinations to perform given tasks. Lots of related studies have been devoted to the topic in the past few decades. They are summarized in the related work section. Compared with them, this paper initially performs preparatory manipulation planning using fully automatically generated grasps and object poses. It is also the first work that automatically determines the choices of arms.
The mechanism under the planner is a regrasp graph, where each node in the graph represents one grasp configuration. Given the initial and goal poses of an object, the planner connects the accessible grasps of initial and goal poses of an object to the graph and searches the graph to find a sequence of pick-and-place-based preparatory manipulation motion. For dual-arm manipulation, the planner builds two regrasp graphs where each one encodes the grasps of one hand. The planner connects the two graphs at hand-over poses to make a super regrasp graph, and searches the super regrasp graph to find a sequence of preparatory manipulation motion. The results could be either pick-and-place using a single arm, hand-over using dual arms, or mixed single and dual-arm operations, depending on feasibility and lengths of paths found by the graph search algorithms.
The planner is part of an open source project named PYHIRO111 Project link is excluded for review.. It is examined by various humanoid robots like HRP2Kai, Nextage, etc, using both simulation and real-world experiments.
Ii Related Work and Contributions
There are two problems in preparatory manipulation planning. One is to find a sequence of collision-free and IK-feasible motion to move and reorient objects. The problem, in this case, is usually named the sequential manipulation planning problem or the combined task and motion planning problem. The other one is to incorporate multiple arms and determines the sequence of multi-arm handover and whether to use a single arm or dual arms.
Lots of studies have been devoted to the first problem. Some early publications include ,which used grasp-placement tables to find a sequence of preparatory motion. The grasp-placement tables orchestrate the stable states and accessible grasps of an object to provide a data structure for searching manipulation sequences. The grasps in these early work were predefined or generated by planners designed for specific grippers, and the objects were simple polygonal objects.
Recent work included more complicated grasp planners and was integrally considered with backtracking, geometrical reasoning, and motion planning. For example, Simeon et al.  presented a framework which integrated motion planning and preparatory manipulation planning to move objects among obstacles. Xue et al.  used shape primitives to plan the grasps of a multi-finger hand and implemented the preparatory manipulation planning of a cup using the multi-finger hand. Bohg et al. 
used Bayesian Network to model task constraints and plan the preparatory manipulation sequence of containers and toys. King et al. used integral primitive-based prehensile and non-prehensile grasp planning to exercise preparatory manipulation of everyday objects. Lozano-Perez et al.  used symbolic search to find manipulation sequences under various constraints. The method was used to plan sequences for a mobile manipulator to rearrange objects. Similarly, Srivastava et al. , Krontiris et al. , Toussaint , and Dantam et al.  respectively used symbolic reasoning to remove obstacles and pick up or rearrange objects. Especially, Toussaint  and Dantam et al.  presented algorithms to plan motion sequences for robots to stack distributed objects, considering both task and motion constraints. Mirabel et al.  presented a planner considering the constraints from coordinated manipulators. More extensively, Lee et al.  and Woodruff et al.  respectively presented a framework work that used integral prehensile and non-prehensile planning to generate sequential manipulation.
There are also lots of studies on dual-arm or multi-arm preparatory manipulation planning. For example, Koga et al.  used predefined grasps to plan sequential manipulation for multiple manipulators in early time. Cho et al.  applied the grasp-placement tables developed by  to dual-arm robots. Saut et al.  used decomposition to plan grasps and implemented dual-arm regrasp of complicated mesh models. Graphs, instead of grasp-placement tables, were used in the planner. Harada et al.  presented regrasp and hand-over planning of dual-arm robots across multi-modal configuration spaces, and implemented a practical system in . Details of multi-modal motion planning could be found in , which made a concrete description of the concept and presented several implementations. More recent work like 
used dual-arm sequential manipulation planning to wrap up the fabric. It is also quite relevant except that reorienting directions are constrained by the wrapping heuristics. Lertkultanon et al., Zhou et al. , and Suarez-Rui et al.  respectively applied integral prehensile and non-prehensile sequential dual-arm motion planning to the preparatory manipulation of a chair. Vezzani et al.  presented a framework for simultaneous handover and visual recognition.
Compared with the related work, our contribution is a super regrasp graph which is extensible over multi-modal configuration spaces and multiple arms. The super regrasp graph is composed of components like partial regrasp graphs for the initial and goal poses, single-arm regrasp graphs, and hand-over regrasp graphs. The proposed planner could search the super regrasp graph to automatically decide whether to use a single arm or both arms to finish given tasks. To our best knowledge, this paper is the first work that automatically chooses the number of arms and uses mixed single and dual-arm operations for preparatory manipulation.
Iii Multi-modal Motion Planning
Iii-a The theory
The fundamental theory supporting preparatory manipulation planning is multi-modal motion planning , which means to plan paths across multiple configuration spaces, and output a sequence of transfer and transit motion.
Fig.2 sketches an example of multi-modal motion planning. Here, the goal is to plan robot motion across eight configuration spaces named , , …, . A configuration space is defined as:
where indicates an n+2-dimensional topology torus. denotes the jawwidth of a robotic gripper, indicates the th joint angle of a manipulator. The seven configuration spaces are connected to each other by overlapped manifolds. As is shown in Fig.2(a), the areas named are the overlapped manifolds. A connects two adjacent configuration spaces and . It is composed by the shared configurations of and . Formal definitions of will be presented in the next subsection. In multi-modal motion planning, the overlapped manifolds are named transit spaces. The remaining configuration spaces are named transfer spaces. Fig.2(b) shows a path found by the multi-modal planner. The initial configuration is denoted by a red point. It is in configuration space . The goal configuration is denoted by a blue point. It is in configuration space . The planner finds a path crossing six configuration spaces , , , , , and . Along the path, the black edges connect two identical poses, they are transfer edges. The cyan edges connect two different poses, they are the transit edges. The planned result is a sequence of transfer and transit motion along the transfer and transit edges.
Iii-B The modalities of preparatory manipulation
Solving a multi-modal motion planning problem requires properly identifying the modalities. Considering that the goal of the preparatory manipulation planning is (1) to reorient the poses of objects, and (2) to find a sequence of arm motion, we use the configuration space of a robot arm as a transfer space, and use object poses to identify various modals. Fig.3(a) exemplifies the identification. Each modal is essentially the configuration space of a single arm. It is varied by object poses and is written as (), which means the configuration space is modalized by an object pose . Let be defined as:
, , , , , denote the position and orientation components of the object pose. A modalized configuration space could be written as:
The jawwidth is a value irrelevant to the pose of an object. The total dimension of a modalized configuration space is . It is a n+8-dimensional manifold. By replacing the items using
, where = , = , = , = , = , = , equation (3) can be rewritten as:
Two modals, for example () and () shown in the Fig.3(a), are connected to each other by a transit manifold named (, ). The transit manifold could be either modalized by or . When modalized by , the manifold is defined as . When modalized by , the manifold is defined as The two definitions are equal to each in the object’s local coordinate system:
Here, and indicate the Jacobian matrix of a robot under joint angles q from configuration spaces modalized by and respectively.
Equations (3-7) show: (1) A configuration in (, ) is on a manifold; (2) The configuration belongs to both () and (); (3) The configuration implies a grasp that is identical in the local coordinate system of the object.
Fig.3(b) shows the configurations and their relations in the modalized configuration spaces. The configuration spaces are sampled. Each node indicates a sampled configuration. Especially, the two nodes under (, ) correspond to four grasps shown in an upper shadow and a lower shadow respectively. In the upper shadow, the objects are at two different poses. The two grasps associated with the two poses are identical in the local coordinate system of the object. They are essentially the same grasp that represents the upper node in the transit manifold.
Details of planning across multi-modal configuration spaces are shown in Fig.3(c). Here, the object is a three-direction pipe (the orange object in the figure). is a standing pose shown in the left part of Fig.3(b). is a resting pose shown in the right part of Fig.3(b). The arm configurations in () are related to the standing pose. The arm configurations in () are related to the resting pose.
When doing sampling-based motion planning in a transfer space, a planner will sample the space considering grasp configurations. An example is shown in Fig.3(c). There are two configuration spaces and in the figure. The two spaces are connected to each other by a transit space (. A sampling-based planner samples the configuration spaces considering grasps, and each of the two configuration spaces in the figure are sampled into five nodes (see the yellow and purple points in the figure). Nodes , , , , belong to . Nodes , , , , belong to . In these sampled nodes, , , are further illustrated using green hand under the shadow. They belong to the transfer space. , are also further illustrated using green hand under the ( shadow. They belong to the transit space. , , , and , are illustrated in the same way under the shadow and ( shadow respectively.
The sampled nodes in (), (), and (, ), are connected into a graph considering collisions and feasibility of IK. The graph is a preliminary form of the regrasp graph. We will further organize the preliminary graph considering both arms as well as handover states in following sections to perform single-arm and dual-arm planning.
Iv Regrasp Graph
This section presents the details of the regrasp graph, including automatic grasp planning, selection of object poses, organizations of nodes, single-arm regrasp graphs, and dual-arm extensions, etc.
Iv-a Grasps and object poses
The fundamental elements of a regrasp graph are grasp configurations. In our planner, the grasp configurations of an object are planned considering force closure using self-developed grasp planners.
The object poses that modalize the configuration spaces are defined by the stable placements of an object on a table. The stable placements are planned considering the projection of (center of mass) on a supporting area using a self-developed placement planner. Especially we limit the number of object poses by discretizing the positions of a stable placement and its rotation around the vertical axis. A discretized placement, together with its accessible grasps and the collision-free and IK-feasible configurations of a robot, defines a configuration space . The accessible grasps will be the nodes of a graph in .
Fig.4 shows the modalization of configuration spaces using the three-direction pipe model. The planned grasps are shown in Fig.4(a). The stable placements of the pipe on a table without considering variation in positions and rotations are shown in Fig.4(b)222Note that we do not consider symmetry in computing these poses. For example, pose 2 and pose 3 are laying on an upper face and bottom face respectively. They are symmetrically the same, but are treated as different stable placements.. Fig.4(c) shows stable placements considering rotation around the vertical axis. In the figure, the rotation is descritized by intervals for clear visualization and easy computation. Each placement in Fig.4(b) therefore spawns eight poses, and the total number of poses reaches to 48.
Fig.4(d) further shows the results after considering the variation in positions. The figure is a top view where the black frame indicates a table. Like rotation, the positions on the table are sampled by grids for clear visualization and easy computation. 56 grids are used for sampling and in total, the positions on the table are discretized into 56 locations. The 48 poses in Fig.4(c) are further moved to these locations. In total, after considering the positions on a table, the number of poses is 1440. Each of the 1440 poses identifies one configuration space, and the multi-modal planner shall plan across 1440 configuration spaces to find a preparatory manipulation sequence for the three-direction pipe.
Note that the discretization of rotations and positions significantly impair the scale of the regrasp graph. More rotations and positions increase manipulability, as well as increase the scale of a regrasp graph. It adds heavy work load to building the regrasp graph. The scale of a regrasp graph is asymptotic to where is the number of planned grasps, is the number of discretized rotation, is the number of discretized positions. The number of planned stable poses is small compared to these values and is therefore considered as a constant. Using modern computers and databases, the planner may plan across as many as 10,000s of nodes .
Iv-B Single-arm regrasp graph
To plan across the multiple configuration spaces, we sample nodes in each of the configuration space and build a roadmap for search by connecting the sampled node. Fig.5(a) shows a configuration space and the sampled nodes (denoted by the circled grasps). The configuration space is modalized by a pose of the object shown in the center. Each sampled node is one collision-free and IK-feasible grasp of the object at the pose. A node is formally defined as a sample of , following equation (6):
The transfer edges connecting the sampled nodes in are formally defined as:
, where and are the q components of and respectively.
In the case of Fig.5(a), there are 12 sampled grasps and consequently 12 nodes. These nodes are connected into a graph by transfer edges. The cyan edges in Fig.5(a) indicate the transfer edges. The graph is named a partial graph since it is part of the whole roadmap.
Fig.5(b) further shows the result after considering rotation around the vertical axis. There are 8 configuration spaces and consequently 8 partial regrasp graphs (shown in cyan color). Inside each partial regrasp graph, the transfer edges connect sampled nodes. It is the partial roadmap of a transfer space. Across the partial regrasp graphs, transit edges (the black edges) connect the sampled nodes of the transfer spaces through transit manifolds. The transit edges are formally defined as:
Fig.5(c) shows the regrasp graph after considering variation in poses. There are 48 configuration spaces and the regrasp graph involves 48 partial regrasp graphs as well as the transit edges connecting them. Fig.5(d) further considers the different positions on a table. In this case, there are 1440 configuration spaces and consequently 1440 partial regrasp graphs. Together with the transit edges, the regrasp graph grows into a big graph shown in Fig.5(d). The graph includes 30 components like Fig.5(c). Each component corresponds to a different position on a table. The additional edges in (c) and (d) follow the same definition as equation (9), except that more object poses with varying rotation around the vertical axis and different positions on a table are further considered.
The regrasp graph in Fig.5(d) is for a single arm. The grasps represented by the nodes are only feasible to one arm. Thus, we name the graph single-arm regrasp graph, and use to denote it.
could be replaced by or . denotes the single-arm regrasp graph for a left arm. denotes the single-arm regrasp graph for a right arm.
Iv-C Dual-arm regrasp graph
The single-arm regrasp graph could be extended to dual-arm regrasp graph by building two regrasp graphs for two arms and additionally including a hand-over regrasp graph.
The hand-over graph is made by two modals identified by a hand-over pose of the object. The nodes in the graph are the accessible grasps associated with the hand-over pose. To extend to dual-arm regrasp, we build a single-arm regrasp graph for the right arm, and build another single-arm regrasp graph for the left arm. These two graphs are connected to the two modals in the hand-over graph. The hand-over graph acts as a bridge for planning hand-over motions.
See Fig.6 for details. First, the planner prepares candidate hand-over poses to identify hand-over modals. The hand-over poses of an object are discretized using 3D lattice and icosphere. Each crossing point of a 3D lattice in the workspace
is used as a candidate hand-over position. Each vector pointing to a vertex of an icosphere centered at a position determined by the 3D lattice is used as a hand-over orientation. Fig.6(a) shows three hand-over poses. These hand-over poses are at the same position. The position is determined by a single crossing point of a 3D lattice. On the other hand, the hand-over poses are at different orientations. The orientations are determined by three vertices on an icosphere centered at the aforementioned single crossing point. The accessible grasps for two hands are shown in red and blue respectively. Each of the hand-over poses modalizes two configuration spaces. One is for the right hand, the other is for the left hand.
The first pose of Fig.6(a) corresponds to two configuration spaces (right-arm configuration space and left-arm configuration space) shown in Fig.6(b). The sampled nodes of the configuration spaces are shown by the grasps in the red or blue circles. These sampled nodes are used for hand-over. They are connected to each other to form a hand-over regrasp graph, considering the collisions between the two hands and the feasibility of IK.
We use to denote a hand-over pose. Following equation (6), the configuration spaces modalized by is defined as:
Here, could be the joint angles of left arm () or right arm (). Accordingly, is written as or . The edges connecting two grasps associated with a hand-over pose are:
Here, indicates the mesh models of the gripper at the two configurations do not collide with each other. The object poses at the end of the edges are the same, thus the edges are transfer edges.
The edges connecting the placements and hand-over poses are:
These edges are transit edges.
The hand-over regrasp graph is denoted by where
is connected to the two single-arm regrasp graphs to build a dual-arm regrasp graph like Fig.6(c). The dual-arm regrasp graph is denoted by . Here, and are the two single-arm regrasp graphs built for right and left arms. and , or and , are connected to each other by .
V Regrasp Planning using Regrasp Graphs
V-a Single-arm planning
Given the initial and goal poses of an object, the proposed planner finds the accessible grasps of the two poses and generates the partial regrasp graphs for the initial and goal poses. and are used to denote the partial regrasp graphs for the initial and goal poses. The planner connects and to a single-arm regrasp graph , and searches the connected graph to find preparatory manipulation sequences for a single arm. Fig.7 shows an example. The partial regarsp graphs for the initial and goal poses and are similar to the one shown in Fig.5(a), as the initial and goal poses are assumed to be a stable placement on a table. and are connected to a single-arm regrasp graph to build in Fig.7(a). Here, and can be hardly seen since they are overwhelmed by the red and blue segments (they are near the spots with heavy red and blue colors). The planner searches the shortest path from to to plan single-arm preparatory manipulation sequences for the task. The initial and goal grasps could be any node on and . The green path in Fig.7(b) shows one path found by the planner.
The output of the single-arm planning is a sequence of collision-free and IK-feasible grasps, and the result of single-arm planning is essentially a sequence of grasps and robot configurations. The motion between the configurations in the sequence is not necessarily feasible due to obstacles, unavailable IKs, and some other task assumptions like the end-effector constraints , especially in complicated workspaces. On-line sampling-based motion planning is therefore employed in a later step to further generate detailed arm motion. Dynamic Domain RRT (DD-RRT)  is used to generate samples and plan paths. DD-RRT is used because we assume the workspace is not crowded with obstacles and the configuration space does not have many narrow passages. DD-RRT searches more in the open space and finds paths quickly under these assumptions. For configuration spaces connected by narrow passages, DD-RRT is a bit slow. Methods like  are preferable.
V-B Dual-arm planning
Dual-arm planning is essentially the same as the single-arm planning, except that and are respectively connected to the and components of a dual-arm regrasp graph.
Fig.8 shows an example. Like the single-arm case, the planner searches the shortest path from the initial regrasp graph to the goal regrasp graph, and performs iterative motion planning to generate preparatory manipulation motion. In the case of Fig.8, the hand-over is from right hand to left hand since is connected to the component (red segments) and is connected to the component (blue segments). The direction of the hand-over motion could the exchanged by swapping the connection from , to , . Readers are recommended to refer to  for more details about single-arm and dual-arm planning.
V-C Automatically choosing the number of arms
The most advantageous the new planner proposed in this work is it is able to automatically choose the number of arms by building and searching a super regrasp graph which holds all connections among , , and the components of dual-arm regrasp graph. The connections include:
Connection between and .
Connection between and .
Connection between and .
Connection between and .
Connection between and .
Making up all the five connections leads to a super regrasp graph. Our planner could search the super regrasp graph to find a preparatory manipulation sequence which automatically determines whether to use single-arm regrasp, dual-arm handover, or mixed single-arm regrasp and dual-arm handover, to perform given tasks. The search is done using conventionally heuristic search algorithm like A*. The heuristic we used is the length of a path. Since the length of a path corresponds to the times of transfer and transit changes or the times of regrasps, our planner may find a path that requires fewest times of regrasp. Meanwhile, it automatically determines if the path spans the graphs of a single arm or both arms, hence automatically determines the necessary number of arms. The shortest path heuristic is also reasonable in that fewer times of regrasp reduce the propagation of noises, making the preparatory manipulation more robust 333Note that there might be multiple solutions that require the same times of regrasp. We do not further differentiate them in this work. However, practitioners may choose their own strategy to rank these solutions. E.g. One may give high priority to human-like poses..
Fig.9 shows an example. In Fig.9(a), is connected to (red segments) and is connected to (blue segments). In Fig.9(b), is additionally connected to (red segments) and is additionally connected to (blue segments). In Fig.9(c), is connected to (brown segments). Fig.9(c) is the super regrasp graph after making up all the five connections.. A shortest path found by searching the super graph is shown in Fig.9(d).
Vi Experiments and Analysis
Vi-a Results using various robots
The planner is tested using various robots including Nextage, HRP5P, and HRP2Kai. Results of the planner and the correspondent regrasp graphs are shown in Fig.10. The task is to move objects from initial poses marked in transparent red color to goal poses marked in transparent green color. The results in Fig.10(a-d) perform a simple pick-and-place task. The pose of the object is not changed. The robot is Kawada Nextage. Four sequences to finish the task are shown in Fig.10(a.1-a.2), Fig.10(b.1-b.2), (c.1-c.4), and (d.1-d.4), respectively. The regrasp graphs are shown in Fig. 10(a), (b), (c), and (d). Fig.10(a) and (a.1-a.4) are the regrasp graphs and planned results by using the right arm. Fig.10(b) and (b.1-b.4) are the regrasp graphs and planned results by using the left arm. Fig.10(c) and (c.1-c.4) are the regrasp graph and results by using right-to-left handover. Fig.10(d) and (d.1-d.4) are the regrasp graph and results by using left-to-right handover. In this case, the planner suggests using the shorter paths (a.1-a.2) or (b.1-b.2) to perform the task, which involves only one time of single-arm regrasp. (c.1-c.5) and (d.1-d.5) involve one time of handover. They cost more than (a.1-a.2) and (b.1-b.2), and are consequently not suggested.
The results in Fig.11(a-b) use the same robot and object, but the initial and goal poses of the object are changed. In this case, the robot could either use two times of single-arm regrasp (a.1-a.6) or one time of single-arm regrasp plus one time of handover (b.1-b.8) to perform the task. The regrasp graph and path corresponding to (a.1-a.6) is shown in (a), where the thick segment in (a)-s1 implies the pick-and-place sequence shown in (a.1-a.3), the thick segment in (a)-s2 implies a second pick-and-place sequence shown in (a.4-a.6). The regrasp graph and path corresponding to (b.1-b.8) are shown in (b), where the thick segments in (b)-s1 correspond the pick-and-place sequence shown in (b.1-b.4), the thick segments in (a)-s2 correspond the handover sequence shown in (b.5-b.8). The planner suggests using the shorter path (a.1-a.6) to perform the task. The results in Fig.12 show a special case where the robot picks up an object using its right arm, hands it over to uses its left arm to re-orient the object, and hands it back to the right arm. The planner suggested two times of handover to finish the task. The results in Fig.13(a-b) show the result of reorienting an electric drill using an HRP5P robot. In this case, The robot could either use two times of single-arm regrasp (a.1-a.9) or one time of single-arm regrasp plus one time of handover (b.1-b.9) to perform the task. The planner outputs (a.1-a.9). The task cannot be done by simple pick-and-place or direct handover. The results in Fig.14 show the result of reorienting a workpiece using an HRP2Kai robot. In this case, the only solution is to use handover shown in (1-5). The robot cannot perform the task using a single arm.
Real-world experiments are conducted on a Nextage robot.
In the real-world experiments, we used two external Kinect
V1 cameras and evaluated the pose of an object by minimizing the
distance between the 3D point clouds and a mesh model. The hardware
configurations and two matching results are shown in Fig.15. Each
detection takes about 1 second, which had been analyzed in detail in
 . There is no re-estimation during the regrasp.
corresponding to Fig.
. There is no re-estimation during the regrasp. The results corresponding to Fig.10(d.1-d.5) are shown in Fig.16. The robot automatically uses the shortest path (either single-arm, dual-arm, or mixed single and dual arm regrasp) to finish the task.
|Robot||DoFs||Online cost ()||Offline cost ()||# paths|
The online cost and number of paths are the average value of 10 executions. All time costs are measured in seconds. Abbreviations: IK - Inverse Kinematics; CD - Collision Detection; SG - Super Graph; gp - grasp planning; pp - placement planning; hp - handover planning.
One thing to note is we build the regrasp graph with a large number of nodes. It is redundant and offers many choices with equal times of regrasp. A robot may switch to a different choice if the object is not well recognized by the vision system. The visual system assumes a single object without surrounding obstacles. The detection is robust under this assumption. We didn’t spot a failure caused by the vision system during the planning.
Vi-B Time cost and performance
Vi-B1 Time cost
The planner includes an offline part and an online part. Both of them are computed automatically. The offline part includes grasp planning, placement planning, handover planning, and pre-building of partial roadmaps. These computations are independent of specific environments since they are irrelevant to obstacles. They can be built once and reused later. The online part builds supper roadmaps and performs graph search. Super roadmaps have to be built online since the initial and goal poses of objects, as well as the size of tables (obstacles) change from task to task. The associated grasps and the edges that connect these grasps to the partial roadmaps must be computed in realtime. The planner searches all possible paths by using single-arms, dual-arms, or their combinations and outputs the shortest path as the result. The cost of the algorithms, including both the online and offline part are shown in Table I. The processor of our computer is Intel Xeon 2.8GHz. Its graphics card is NVIDIA Quadro M3000M. The version of the programming language is Python 2.7.11. As is shown in Table I, the offline part may cost hours, depending on the number of DoFs. The online part, in contrast, could be done in a few seconds. The most time-consuming part of the online cost is the computation of IK and CD. In the worst case, the cost reaches to 38.40. In contrast, building super graphs and searching the graph is very fast. The most time-consuming part of the offline cost is still the computation of IK and CD. Grasp planning and placement planning also cost more than ten seconds. Handover planning costs several tens of seconds. The best way to lower time cost is to reduce the number of automatically planned grasps. The time cost table shows that the Nextage robot has much better kinematic design than the other two robots. It has fewer DoFs, but could finish the given tasks with more sequences (see the # paths column), and within less time (see the online cost column).
Table II compares the performance of single-arm regrasp planner, dual-arm regrasp planner, and the newly proposed mixed planner using ten regrasp tasks and the objects shown in Fig.17. The initial and goal poses of these tasks are randomly generated. Compared to the single-arm regrasp planner and dual-arm handover planner, the newly proposed planner does preparatory manipulation planning using automatically determined single and dual arms. It has higher success rates in reorienting. Also, the planner is able to do both regrasp and handover, which significantly improves the ability of humanoid robots.
|Sgl-arm planner||Dual-arm planner||Auto planner|
The success rates are based on ten times of executions. - means initializing from right arm and reaching to the goal using right arm. - means initializing from right arm and reaching to the goal using left arm. - and - follow similar definitions.
Vi-B3 Discussions on more than two arms
Extending the algorithms to more than two arms is not difficult since the graphs are built incrementally. More than two arms require including more where indicates the identifier of all arms, rather than the binary values and in dual-arm planning. Accordingly, connections between the partial regrasp graphs and single-arm regrasp graphs should include: (1) Connections between and all . (2) Connection between and all . (3) Connection between and . Although incrementally building the connections between , , and all other do not significantly increase computational cost, preparing the additional s cost many extra offline resources. Accordingly to Table I, the offline time cost for a single (6-DoF Nextage) is around 1500. The offline time cost of an n-arm 6-DoF robot will be as much as 1500n. The high cost makes it impractical to extend the present algorithms to multiple arms.
Vii Conclusions and Future work
In this paper, we presented a preparatory manipulation planner to plan a sequence of manipulation motion that prepared an object into expected poses. The planner was demonstrated by various robots using both simulation and real-world experiments. The results showed that the planner could plan single-arm, dual-arm, as well as mixed single and dual arm preparatory manipulation sequences. It could automatically determine the employment of single and dual arms by minimizing the number of regrasp and handover. The planner is a high-level component and is applicable to various humanoid robots like HRP2Kai, Nextage, etc.
There are several future directions. First, as discussed in the experimental section, the present algorithms are not suitable for more than two arms due to the high offline computational cost. The present algorithms plan over both arms in a centralized way, which we believe is not applicable to more than two arms since few creatures in nature control more than two dexterous arms using a central brain. Thus, we will explore decentralized methods and study planning across multiple industrial robots (more than two arms) or human-robot coordination using a combination of the proposed algorithms and decentralized approaches. Second, the current visual detection system assumes a single object without surrounding obstacles. Although the assumption eases visual estimation, in more complicated environments where the object is obstructed and can hardly be recognized from one view, we may use hand mounted cameras and active visual perception to perform mixed view and preparatory manipulation planning. Third, the placement planning algorithm is specially designed for tables. It would be interesting and promising to develop general stable state planners that is applicable to any supporting surfaces and unstructured environments. We have done some preliminary studies on it, and will explore more in the future. Moreover, changing environments, especially the environments where human works together with the robot and has to be treated as a moving obstacle, also deserves exploration. The current DD-RRT algorithm and regrasp replanning have to be adapted to determined good timing for reactions.
-  A. Nguyen, D. Kanoulas, D. G. Caldwell, and N. G. Tsagarakis, “Preparatory Object Reorientation for Task-oriented Grasping,” in Proc. International Conference on Intelligent Robots and Systems, pp. 893–899, 2016.
-  P. Tournassound, T. Lozano-Perez, and E. Mazer, “Regrasping,” in Proceedings of International Conference on Robotics and Automation, pp. 1924–1928, 1987.
-  H. Terasaki and T. Hasegawa, “Motion Planning of Intelligent Manipulation by a Parallel Two-Fingered Gripper Equiped with a Simple Rotating Mechanism,” IEEE Transactions on Robotics and Automation, vol. 14, pp. 207–219, 1998.
-  T. Simeon, J.-P. Laumond, J. Cortes, and A. Shbani, “Manipulation Planning with Probabilistic Roadmaps,” International Journal of Robotics Research, vol. 23, pp. 729–746, 2004.
-  Z. Xue et al., “Planning Regrasp Operations For a Multifingered Robotic Hand,” in Proceedings of International Conference on Automation Sicence and Engineering, pp. 778–783, 2008.
-  J. Bohg, K. Welke, B. Leon, M. Do, D. Song, W. Wohlkinger, M. Madry, A. Aldoma, M. Przybylski, T. Asfour, H. Marti, D. Kragic, A. Morales, and M. Vincze, “Task-based Grasp Adaptation on a Humanoid Robot,” in Proceedings of International IFAC Symposium on Robot Control, pp. 779–786, 2012.
-  J. King, M. Klingensmith, and C. Dellin, “Regrasp Manipulation as Trajectory Optimization,” in Proceedings of Robotics: Science and Systems, 2013.
-  T. Lozano-Perez and L. P. Kaelbling, “A Constraint based Method for Solving Sequential Manipulation Planning Problems,” in Proceedings of International Conference on Intelligent Robots and Systems, pp. 3684–3691, 2014.
-  S. Srivastava, E. Fang, L. Riano, R. Chitnis, S. Russell, and P. Abbeel, “Combined Task and Motion Planning through an Extensible Planner-independent Interface Lyer,” in Proceedings of IEEE international Conference on Robotics and Automation, pp. 639–646, 2014.
-  A. Krontiris and K. E. Bekris, “Dealing with Difficult Instances of Object Rearrangement,” in Proceedings of : Science and Systems, 2015.
M. Toussaint, “Logic-Geometric Programming: An Optimization-Based Approach to
Combined Task and Motion Planning,” in
Proceedings of Internatoinal Joint Conference on Artificial Intelligence, pp. 1930–1936, 2015.
-  N. Dantam, Z. Kingston, and S. Chauhuri, “Incremental Task and Motion Planning: A Constraint-Based Approach,” in Proceedings of Robotics: Science and Systems, 2016.
-  J. Mirabel and F. Lamiraux, “Manipulation Planning: Building Paths on Constrained Manifolds,” in Proceedings of IEEE International Conference on Robotics and Automation, 2017.
-  G. Lee, T. Lozano-Perez, and L. P. Kaelbling, “Hierarchical Planning for Multi-contact Non-prehensile Manipulation,” in Proceedings of International Conference on Intelligent Robots and Sytems, pp. 264–271, 2015.
-  J. Z. Woodruff and K. M. Lynch, “Planning and Control for Dynamic, Nonprehensile, and Hybrid Manipulation Tasks,” in Proceedings of IEEE International Conference on Robotics and Automation, pp. 4066–4073, 2017.
-  Y. Koga, K. Kondo, J. Kuffner, and J.-C. Latombe, “Planning Motions with Intentions,” in Proceedings of Annual Conference on Computer Graphics and Interactive Techniques, pp. 395–408, 1994.
-  K. Cho, M. Kim, and J.-B. Song, “Complete and Rapid Regrasp Planning with Look-up Table,” Journal of Intelligent and Robotic Systems, vol. 36, pp. 371–387, 2003.
-  J.-P. Saut, M. Gharbi, J. Cortes, D. Sidobre, and T. Simeon, “Planning Pick-and-place Tasks with Two-hand Regrasping,” in Proceedings of International Conference on Intelligent Robots and Sytems, pp. 4528–4533, 2010.
-  K. Harada et al., “A manipulation motion planner for dual-arm industrial manipulators,” in Proceedings of International Conference on Robotics and Automation, pp. 928–934, 2014.
-  K. Harada et al., “Project on Development of a Robot System for Random Picking-Grasp/Manipulation planner for a Dual-arm Manipulator,” in Proc. International Symposium on System Integration, pp. 583–589, 2014.
-  K. Hauser and J.-C. Latombe, “Multi-modal Motion Planning in Non-expansive Spaces,” International Journal of Robotics Research, vol. 29, pp. 897–951, 2010.
-  N. Hayashi, T. Suehiro, , and S. Kudoh, “Planning Method for a Wrapping-with-fabric Task using Regrasping,” in Proceedings of International Conference on Robotics and Automation, pp. 1285–1290, 2017.
-  P. Lertkultanon and Q.-C. Pham, “A Certified-Complete Bimanual Manipulation Planner,” IEEE Transactions on Automation Science and Engineering, 2018.
-  X. Zhou, P. Lertkultanon, and Q.-C. Pham, “Closed-chain Manipulation of Large Objects by Multi-arm Robotic Systems,” IEEE Robotics and Automation Letters, vol. 2, no. 4, pp. 1832–1839, 2017.
-  F. Suárez-Ruiz, X. Zhou, and Q.-C. Pham, “Can Robots Assemble an IKEA Chair?,” Science Robotics, vol. 3, no. 17, 2018.
-  G. Vezzani, M. Regoli, U. Pattacini, and L. Natale, “A Novel Pipeline for Bi-manual Handover Task,” Advanced Robotics, pp. 1–14, 2017.
-  W. Wan and K. Harada, “Regrasp Planning using 10,000 Grasps,” Proc. International Conference on Intelligent Robots and Systems, 2017.
-  Z. Kingston, M. Moll, , and L. E. Kavraki, “Sampling-Based Methods for Motion Planning with Constraints,” Annual Review of Control, Robotics, and Autonomous Systems, vol. 1, pp. 159–185, 2018.
-  A. Yershova, L. Jaillet, T. Siméon, and S. M. LaValle, “Dynamic-domain RRTs: Efficient exploration by controlling the sampling domain,” in Proceedings of the IEEE International Conference on Robotics and Automation, pp. 3856–3861, 2005.
-  W. Wan and K. Harada, “Divide-and-conquer Manipulation Planning by Lazily Searching a Weighted Two-layer Manipulation Graph,” in Proceedings of IEEE International Workshop on Advanced Robotics and its SOcial ompacts, 2015.
-  W. Wan and K. Harada, “Developing and Comparing Single-arm and Dual-arm Regrasp,” IEEE Robotics and Automation Letters, pp. 243–250, 2016.
-  P. E. Hart, N. J. Nilsson, and B. Raphael, “A Formal Basis for the Heuristic Determination of Minimum Cost Paths,” IEEE transactions on Systems Science and Cybernetics, vol. 4, no. 2, pp. 100–107, 1968.