I Introduction
Preparatory manipulation planning automatically finds a sequence of robot motion that manipulates and prepares an object for specific tasks [1]. 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 singlearm regrasp shown in Fig.1(a.1a.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 dualarm handover shown in Fig.1(b.1b.4), where the human picks up the electric tool using his left hand in (b.1), moves it to a handover 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 singlearm and dualarm 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 collisionfree and IKfeasible motion to move and reorient objects, and propose a super regrasp graph that helps to determine the sequence of multiarm 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 pickandplacebased preparatory manipulation motion. For dualarm manipulation, the planner builds two regrasp graphs where each one encodes the grasps of one hand. The planner connects the two graphs at handover 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 pickandplace using a single arm, handover using dual arms, or mixed single and dualarm operations, depending on feasibility and lengths of paths found by the graph search algorithms.
The planner is part of an open source project named PYHIRO^{1}^{1}1 Project link is excluded for review.. It is examined by various humanoid robots like HRP2Kai, Nextage, etc, using both simulation and realworld experiments.
Ii Related Work and Contributions
There are two problems in preparatory manipulation planning. One is to find a sequence of collisionfree and IKfeasible 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 multiarm 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 [2][3],which used graspplacement tables to find a sequence of preparatory motion. The graspplacement 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. [4] presented a framework which integrated motion planning and preparatory manipulation planning to move objects among obstacles. Xue et al. [5] used shape primitives to plan the grasps of a multifinger hand and implemented the preparatory manipulation planning of a cup using the multifinger hand. Bohg et al. [6]
used Bayesian Network to model task constraints and plan the preparatory manipulation sequence of containers and toys. King et al.
[7] used integral primitivebased prehensile and nonprehensile grasp planning to exercise preparatory manipulation of everyday objects. LozanoPerez et al. [8] 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. [9], Krontiris et al. [10], Toussaint [11], and Dantam et al. [12] respectively used symbolic reasoning to remove obstacles and pick up or rearrange objects. Especially, Toussaint [11] and Dantam et al. [12] presented algorithms to plan motion sequences for robots to stack distributed objects, considering both task and motion constraints. Mirabel et al. [13] presented a planner considering the constraints from coordinated manipulators. More extensively, Lee et al. [14] and Woodruff et al. [15] respectively presented a framework work that used integral prehensile and nonprehensile planning to generate sequential manipulation.There are also lots of studies on dualarm or multiarm preparatory manipulation planning. For example, Koga et al. [16] used predefined grasps to plan sequential manipulation for multiple manipulators in early time. Cho et al. [17] applied the graspplacement tables developed by [2] to dualarm robots. Saut et al. [18] used decomposition to plan grasps and implemented dualarm regrasp of complicated mesh models. Graphs, instead of graspplacement tables, were used in the planner. Harada et al. [19] presented regrasp and handover planning of dualarm robots across multimodal configuration spaces, and implemented a practical system in [20]. Details of multimodal motion planning could be found in [21], which made a concrete description of the concept and presented several implementations. More recent work like [22]
used dualarm 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.
[23], Zhou et al. [24], and SuarezRui et al. [25] respectively applied integral prehensile and nonprehensile sequential dualarm motion planning to the preparatory manipulation of a chair. Vezzani et al. [26] 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 multimodal configuration spaces and multiple arms. The super regrasp graph is composed of components like partial regrasp graphs for the initial and goal poses, singlearm regrasp graphs, and handover 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 dualarm operations for preparatory manipulation.
Iii Multimodal Motion Planning
Iiia The theory
The fundamental theory supporting preparatory manipulation planning is multimodal motion planning [21], which means to plan paths across multiple configuration spaces, and output a sequence of transfer and transit motion.
Fig.2 sketches an example of multimodal motion planning. Here, the goal is to plan robot motion across eight configuration spaces named , , …, . A configuration space is defined as:
(1) 
where indicates an n+2dimensional 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 multimodal 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 multimodal 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.
IiiB The modalities of preparatory manipulation
Solving a multimodal 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:
(2) 
, , , , , denote the position and orientation components of the object pose. A modalized configuration space could be written as:
(3) 
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+8dimensional manifold. By replacing the items using
(4) 
and
(5) 
, where = , = , = , = , = , = , equation (3) can be rewritten as:
(6) 
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:
(7) 
Here, and indicate the Jacobian matrix of a robot under joint angles q from configuration spaces modalized by and respectively.
Equations (37) 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 multimodal configuration spaces are shown in Fig.3(c). Here, the object is a threedirection 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 samplingbased 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 samplingbased 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 singlearm and dualarm planning.
Iv Regrasp Graph
This section presents the details of the regrasp graph, including automatic grasp planning, selection of object poses, organizations of nodes, singlearm regrasp graphs, and dualarm extensions, etc.
Iva 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 selfdeveloped 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 selfdeveloped 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 collisionfree and IKfeasible 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 threedirection 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)^{2}^{2}2Note 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 multimodal planner shall plan across 1440 configuration spaces to find a preparatory manipulation sequence for the threedirection 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 [27].
IvB Singlearm 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 collisionfree and IKfeasible grasp of the object at the pose. A node is formally defined as a sample of , following equation (6):
(8) 
The transfer edges connecting the sampled nodes in are formally defined as:
(9) 
, 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:
(10) 
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 singlearm regrasp graph, and use to denote it.
(11) 
could be replaced by or . denotes the singlearm regrasp graph for a left arm. denotes the singlearm regrasp graph for a right arm.
IvC Dualarm regrasp graph
The singlearm regrasp graph could be extended to dualarm regrasp graph by building two regrasp graphs for two arms and additionally including a handover regrasp graph.
The handover graph is made by two modals identified by a handover pose of the object. The nodes in the graph are the accessible grasps associated with the handover pose. To extend to dualarm regrasp, we build a singlearm regrasp graph for the right arm, and build another singlearm regrasp graph for the left arm. These two graphs are connected to the two modals in the handover graph. The handover graph acts as a bridge for planning handover motions.
See Fig.6 for details. First, the planner prepares candidate handover poses to identify handover modals. The handover 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 handover position. Each vector pointing to a vertex of an icosphere centered at a position determined by the 3D lattice is used as a handover orientation. Fig.
6(a) shows three handover poses. These handover poses are at the same position. The position is determined by a single crossing point of a 3D lattice. On the other hand, the handover 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 handover 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 (rightarm configuration space and leftarm 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 handover. They are connected to each other to form a handover regrasp graph, considering the collisions between the two hands and the feasibility of IK.
We use to denote a handover pose. Following equation (6), the configuration spaces modalized by is defined as:
(12) 
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 handover pose are:
(13) 
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 handover poses are:
(14) 
These edges are transit edges.
The handover regrasp graph is denoted by where
(15) 
is connected to the two singlearm regrasp graphs to build a dualarm regrasp graph like Fig.6(c). The dualarm regrasp graph is denoted by . Here, and are the two singlearm regrasp graphs built for right and left arms. and , or and , are connected to each other by .
V Regrasp Planning using Regrasp Graphs
Va Singlearm 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 singlearm 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 singlearm 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 singlearm 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 singlearm planning is a sequence of collisionfree and IKfeasible grasps, and the result of singlearm 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 endeffector constraints [28], especially in complicated workspaces. Online samplingbased motion planning is therefore employed in a later step to further generate detailed arm motion. Dynamic Domain RRT (DDRRT) [29] is used to generate samples and plan paths. DDRRT is used because we assume the workspace is not crowded with obstacles and the configuration space does not have many narrow passages. DDRRT searches more in the open space and finds paths quickly under these assumptions. For configuration spaces connected by narrow passages, DDRRT is a bit slow. Methods like [30] are preferable.
VB Dualarm planning
Dualarm planning is essentially the same as the singlearm planning, except that and are respectively connected to the and components of a dualarm regrasp graph.
Fig.8 shows an example. Like the singlearm 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 handover 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 handover motion could the exchanged by swapping the connection from , to , . Readers are recommended to refer to [31] for more details about singlearm and dualarm planning.
VC 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 dualarm 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 singlearm regrasp, dualarm handover, or mixed singlearm regrasp and dualarm handover, to perform given tasks. The search is done using conventionally heuristic search algorithm like A*[32]. 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 ^{3}^{3}3Note 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 humanlike 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
Via 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(ad) perform a simple pickandplace 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.1a.2), Fig.10(b.1b.2), (c.1c.4), and (d.1d.4), respectively. The regrasp graphs are shown in Fig. 10(a), (b), (c), and (d). Fig.10(a) and (a.1a.4) are the regrasp graphs and planned results by using the right arm. Fig.10(b) and (b.1b.4) are the regrasp graphs and planned results by using the left arm. Fig.10(c) and (c.1c.4) are the regrasp graph and results by using righttoleft handover. Fig.10(d) and (d.1d.4) are the regrasp graph and results by using lefttoright handover. In this case, the planner suggests using the shorter paths (a.1a.2) or (b.1b.2) to perform the task, which involves only one time of singlearm regrasp. (c.1c.5) and (d.1d.5) involve one time of handover. They cost more than (a.1a.2) and (b.1b.2), and are consequently not suggested.
The results in Fig.11(ab) 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 singlearm regrasp (a.1a.6) or one time of singlearm regrasp plus one time of handover (b.1b.8) to perform the task. The regrasp graph and path corresponding to (a.1a.6) is shown in (a), where the thick segment in (a)s1 implies the pickandplace sequence shown in (a.1a.3), the thick segment in (a)s2 implies a second pickandplace sequence shown in (a.4a.6). The regrasp graph and path corresponding to (b.1b.8) are shown in (b), where the thick segments in (b)s1 correspond the pickandplace sequence shown in (b.1b.4), the thick segments in (a)s2 correspond the handover sequence shown in (b.5b.8). The planner suggests using the shorter path (a.1a.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 reorient 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(ab) show the result of reorienting an electric drill using an HRP5P robot. In this case, The robot could either use two times of singlearm regrasp (a.1a.9) or one time of singlearm regrasp plus one time of handover (b.1b.9) to perform the task. The planner outputs (a.1a.9). The task cannot be done by simple pickandplace 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 (15). The robot cannot perform the task using a single arm.
Realworld experiments are conducted on a Nextage robot. In the realworld 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 [20]
. There is no reestimation during the regrasp. The results corresponding to Fig.
10(d.1d.5) are shown in Fig.16. The robot automatically uses the shortest path (either singlearm, dualarm, or mixed single and dual arm regrasp) to finish the task.Robot  DoFs  Online cost ()  Offline cost ()  # paths  

IK,CD()  IK,CD()  SG  gp  pp  IK,CD()  hp  IK,CD()  
Nextage  12  8.00  13.72  2.29  24.01  12.40  7.21  1702.11  32.59  1490.01  3244.32  13.50 
HRP5P  18  20.23  38.40  2.07  60.70  10.22  12.73  3397.10  54.29  3670.27  7177.20  6.48 
HRP2Kai  14  10.65  19.88  1.44  31.97  18.35  11.36  2552.51  26.18  1901.75  4510.05  2.22 

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.
ViB Time cost and performance
ViB1 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 prebuilding 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 singlearms, dualarms, 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 timeconsuming 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 timeconsuming 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).
ViB2 Performance
Table II compares the performance of singlearm regrasp planner, dualarm 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 singlearm regrasp planner and dualarm 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.
Sglarm planner  Dualarm planner  Auto planner  

  7/10  8/10  10/10 
  5/10  8/10  9/10 
  10/10  10/10  
  10/10  10/10  
regrasp  
handover 

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.
ViB3 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 dualarm planning. Accordingly, connections between the partial regrasp graphs and singlearm 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 (6DoF Nextage) is around 1500. The offline time cost of an narm 6DoF 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 realworld experiments. The results showed that the planner could plan singlearm, dualarm, 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 highlevel 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 humanrobot 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 DDRRT algorithm and regrasp replanning have to be adapted to determined good timing for reactions.
References
 [1] A. Nguyen, D. Kanoulas, D. G. Caldwell, and N. G. Tsagarakis, “Preparatory Object Reorientation for Taskoriented Grasping,” in Proc. International Conference on Intelligent Robots and Systems, pp. 893–899, 2016.
 [2] P. Tournassound, T. LozanoPerez, and E. Mazer, “Regrasping,” in Proceedings of International Conference on Robotics and Automation, pp. 1924–1928, 1987.
 [3] H. Terasaki and T. Hasegawa, “Motion Planning of Intelligent Manipulation by a Parallel TwoFingered Gripper Equiped with a Simple Rotating Mechanism,” IEEE Transactions on Robotics and Automation, vol. 14, pp. 207–219, 1998.
 [4] 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.
 [5] 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.
 [6] 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, “Taskbased Grasp Adaptation on a Humanoid Robot,” in Proceedings of International IFAC Symposium on Robot Control, pp. 779–786, 2012.
 [7] J. King, M. Klingensmith, and C. Dellin, “Regrasp Manipulation as Trajectory Optimization,” in Proceedings of Robotics: Science and Systems, 2013.
 [8] T. LozanoPerez 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.
 [9] S. Srivastava, E. Fang, L. Riano, R. Chitnis, S. Russell, and P. Abbeel, “Combined Task and Motion Planning through an Extensible Plannerindependent Interface Lyer,” in Proceedings of IEEE international Conference on Robotics and Automation, pp. 639–646, 2014.
 [10] A. Krontiris and K. E. Bekris, “Dealing with Difficult Instances of Object Rearrangement,” in Proceedings of : Science and Systems, 2015.

[11]
M. Toussaint, “LogicGeometric Programming: An OptimizationBased Approach to
Combined Task and Motion Planning,” in
Proceedings of Internatoinal Joint Conference on Artificial Intelligence
, pp. 1930–1936, 2015.  [12] N. Dantam, Z. Kingston, and S. Chauhuri, “Incremental Task and Motion Planning: A ConstraintBased Approach,” in Proceedings of Robotics: Science and Systems, 2016.
 [13] J. Mirabel and F. Lamiraux, “Manipulation Planning: Building Paths on Constrained Manifolds,” in Proceedings of IEEE International Conference on Robotics and Automation, 2017.
 [14] G. Lee, T. LozanoPerez, and L. P. Kaelbling, “Hierarchical Planning for Multicontact Nonprehensile Manipulation,” in Proceedings of International Conference on Intelligent Robots and Sytems, pp. 264–271, 2015.
 [15] 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.
 [16] 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.
 [17] K. Cho, M. Kim, and J.B. Song, “Complete and Rapid Regrasp Planning with Lookup Table,” Journal of Intelligent and Robotic Systems, vol. 36, pp. 371–387, 2003.
 [18] J.P. Saut, M. Gharbi, J. Cortes, D. Sidobre, and T. Simeon, “Planning Pickandplace Tasks with Twohand Regrasping,” in Proceedings of International Conference on Intelligent Robots and Sytems, pp. 4528–4533, 2010.
 [19] K. Harada et al., “A manipulation motion planner for dualarm industrial manipulators,” in Proceedings of International Conference on Robotics and Automation, pp. 928–934, 2014.
 [20] K. Harada et al., “Project on Development of a Robot System for Random PickingGrasp/Manipulation planner for a Dualarm Manipulator,” in Proc. International Symposium on System Integration, pp. 583–589, 2014.
 [21] K. Hauser and J.C. Latombe, “Multimodal Motion Planning in Nonexpansive Spaces,” International Journal of Robotics Research, vol. 29, pp. 897–951, 2010.
 [22] N. Hayashi, T. Suehiro, , and S. Kudoh, “Planning Method for a Wrappingwithfabric Task using Regrasping,” in Proceedings of International Conference on Robotics and Automation, pp. 1285–1290, 2017.
 [23] P. Lertkultanon and Q.C. Pham, “A CertifiedComplete Bimanual Manipulation Planner,” IEEE Transactions on Automation Science and Engineering, 2018.
 [24] X. Zhou, P. Lertkultanon, and Q.C. Pham, “Closedchain Manipulation of Large Objects by Multiarm Robotic Systems,” IEEE Robotics and Automation Letters, vol. 2, no. 4, pp. 1832–1839, 2017.
 [25] F. SuárezRuiz, X. Zhou, and Q.C. Pham, “Can Robots Assemble an IKEA Chair?,” Science Robotics, vol. 3, no. 17, 2018.
 [26] G. Vezzani, M. Regoli, U. Pattacini, and L. Natale, “A Novel Pipeline for Bimanual Handover Task,” Advanced Robotics, pp. 1–14, 2017.
 [27] W. Wan and K. Harada, “Regrasp Planning using 10,000 Grasps,” Proc. International Conference on Intelligent Robots and Systems, 2017.
 [28] Z. Kingston, M. Moll, , and L. E. Kavraki, “SamplingBased Methods for Motion Planning with Constraints,” Annual Review of Control, Robotics, and Autonomous Systems, vol. 1, pp. 159–185, 2018.
 [29] A. Yershova, L. Jaillet, T. Siméon, and S. M. LaValle, “Dynamicdomain RRTs: Efficient exploration by controlling the sampling domain,” in Proceedings of the IEEE International Conference on Robotics and Automation, pp. 3856–3861, 2005.
 [30] W. Wan and K. Harada, “Divideandconquer Manipulation Planning by Lazily Searching a Weighted Twolayer Manipulation Graph,” in Proceedings of IEEE International Workshop on Advanced Robotics and its SOcial ompacts, 2015.
 [31] W. Wan and K. Harada, “Developing and Comparing Singlearm and Dualarm Regrasp,” IEEE Robotics and Automation Letters, pp. 243–250, 2016.
 [32] 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.
Comments
There are no comments yet.