Search-based Planning for Dual-arm Manipulation with UprightOrientation Constraints

Benjamin Cohen Sachin Chitta Maxim Likhachev

Abstract— Dual-arm manipulation is an increasingly impor-tant skill for robots operating in home, retail and industrialenvironments. Dual-arm manipulation is especially essential fortasks involving large objects which are harder to grasp andmanipulate using a single arm. In this work, we address dual-arm manipulation of objects in indoor environments. We areparticularly focused on tasks that involve an upright orientationconstraint on the grasped object. Such constraints are oftenpresent in human environments, e.g. when manipulating a trayof food or a container with fluids. In this paper, we presenta search-based approach that is capable of planning dual-armmotions, often within one second, in cluttered environmentswhile adhering to the orientation constraints. Our approachsystematically constructs a graph in task space and generatesconsistent, low-cost motion trajectories, while providing guaran-tees on completeness and a bounds on the suboptimality of thesolution. The consistency of the generated motions is especiallyimportant in human environments where it helps make theactions of the robot more predictable for human co-workersand observers.

I. INTRODUCTION

Manipulation in human environments is a challenging anddifficult task. Tasks that require manipulating large objectsor objects that are not easy to hold with one arm arein particular very difficult for single-armed manipulationsystems. Examples of such tasks include the manipulationof large objects like laundry baskets, trashcans or boxes andmanipulation of food or liquid containers, e.g. carrying atray of food or glasses. Human execution of such tasks oftenrequires the use of two arms. A general mobile manipulationrobot attempting to execute such tasks will also require theuse of two arms.

In this work, we focus on dual-arm manipulation, i.e.manipulation using two arms (Figure 1). Our focus is, inparticular, on tasks where the robot is manipulating anobject with an upright constraint throughout the motion. Suchconstraints often arise in human environments, particularlyin transport of food or fluids, e.g when carrying a tray offood or a container of liquid. These tasks can often requiremanipulation in very cluttered environments, e.g. removingan open container of food from the back of a fridge whilekeeping it level.

A typical dual-arm manipulation task that we would liketo execute is shown in Figure 1 where a PR2 robot is manip-ulating a tray with wine glasses on it. The robot is required

B. Cohen is with the Grasp Laboratory, University of Pennsylvania,Philadelphia, PA 19104 bcohen@seas.upenn.edu

S. Chitta is with Willow Garage Inc., Menlo Park, CA 94025sachinc@willowgarage.com

M. Likhachev is with the Robotics Institute, Carnegie Mellon University,Pittsburgh, PA 15213 maxim@cs.cmu.edu

Fig. 1: The PR2 robot manipulating a tray with two wineglasses on it.

to maintain the tray level throughout the execution of thetask. The full execution of this task would be composed ofmultiple components including detection and tracking of thetray, determination of dual-arm grasps and motion planningand control for manipulation. In this work, we focus on themotion planning component of this task, i.e. the computationof collision free paths for moving the object from a start poseto a goal pose while maintaining the initial roll and pitch ofthe object.

Motion planning for dual-arm manipulation is inherently aconstrained task. The act of holding an object with two handsnaturally implies a constraint where the two end-effectorsof the arms have to maintain a relative configuration withrespect to each other. The rigidity of the grasp determineshow much the end-effectors can move with respect to eachother. We assume that the end-effectors are fairly constrainedin moving relative to each other, i.e. the grasp being executedby the two arms is fairly rigid.

Our approach to this problem is based on the use ofa heuristic search such as A* search [8] and its variantsbecause they have been proven to be successful in manymotion planning problems in robotics, including single armmanipulation [5], [4]. Search-based planning techniques areattractive since they typically come with strong theoreticalguarantees such as completeness and optimality or boundson the sub-optimality of the solution [17]. If speed of thesearch is a priority over the optimality of the solution,anytime searches can be used to find the best solutionpossible within the given time frame [7], [23], [24], [15].

Search-based techniques allow for explicit cost minimization,complex constraints and are deterministic so they provideguarantees on the consistency of the search. Thus, givena start and goal pose and an environment representation,they will return identical solutions over multiple planningattempts. In addition to being consistent across multiple runswith identical inputs, they are predictable between multipleruns with similar inputs. Meaning, given similar inputs,similar solutions will be found. The predictability of the gen-erated paths is a natural advantage in human environments.Predictable motions by a robot allow humans who share theenvironment with the robot to be more comfortable with therobot since they can learn to anticipate its actions in differentenvironments.

In this paper we present an approach to dual-arm manip-ulation that relies on two key novel components to quicklygenerate feasible trajectories despite the complexity of theproblem. The first component is the chosen representationof the state space that we use to efficiently reduce the di-mensionality of the problem. The representation provided anadditional bonus by reducing the discretization artifacts com-monly found in the trajectories generated by our approachfor single arm manipulation [4]. The second component is anovel heuristic that exploits the upright orientation constraintof the object to provide an even more informative heuristic toefficiently guide the search around clutter in the workspace.This paper will focus on those two concepts.

II. RELATED WORK

Manipulation of objects in human environments has gaineda lot of interest in recent years ( [19], [18], [6], [1], [11], [2],[21]). Interest in dual-arm manipulation is also gaining, par-ticularly with the recent availability of dual-arm manipulationsystems like the PR2 [3], Intel HERB Personal Robot [21],ARMAR [22] and Justin [2]. Dual-Arm manipulation hasbeen used for tasks like cart-pushing [20], towel folding [16]and the manipulation of small kitchen objects [22].

Motion planning for dual-arm manipulation has mostlyused randomized motion planners. In [22], randomized mo-tion planners were used for planning re-grasping actions anddual-arm motion plans for a humanoid robot with two arms.Impressive results for combined grasping and motion plan-ning were achieved by interleaving the efficient computationof inverse kinematics using a pre-computed reachability mapwith the motion planning itself. In [10], an assembly task wasexecuted with two arms using combined task and motionplanning. Online manipulation planning for objects movingon a conveyer belt was carried out in [14] for two 2-DOFarms. In [12], one of the first approaches to planning for dual-arm manipulation was presented using a randomized planner.Multi-arm manipulation planning, including the planning oftransfer paths and grasp and ungrasp motions for manipulat-ing an object using multiple arms was presented in [13]. Theresults of this approach were demonstrated on a simulatedsystem manipulating a simple object using 3 arms.

The use of randomized planners for dual-arm manipulationtasks is popular because of the speed of the planners. How-

ever, plans generated by such planners require frequent post-processing, e.g. using a short-cutting approach [9], beforethey can be executed on a robot. The planned paths are alsounpredictable, i.e. paths planned in the same environmentbetween different start and goal positions will be differentacross multiple planning attempts. In contrast, our approachto dual-arm manipulation guarantees consistency betweenruns. As mentioned earlier, the deterministic nature of theplanner is important in making the actions of the robotmore predictable. In addition, we also found that our planneris capable of planning paths in many types of clutteredenvironments in less than 1 second. The plans generated alsorequired minimal post-processing for execution on the PR2robot and the resultant trajectories were smooth enough in theposition space of the manipulated object to perform delicatetasks such as carrying wine glasses on a tray.

III. THE PR2 ROBOT

The hardware platform that we used for experiments is thePR2 mobile manipulation robot (Fig. 1). The PR2 robot is atwo-armed robot with an omni-directional base and a varietyof sensors mounted on a sensor head. Each arm has 7 degreesof freedom and thus has a redundant degree of freedom.We exploit this redundancy by developing a custom inversekinematics solution for the arm that is parameterized by oneof the joint angles. The joint angle we choose is the upperarm roll joint shown in Fig. 3(b). The joint limits on therobot’s joints are also taken into account by the kinematicssolver. Thus, given the end-effector pose and a value forthe free parameter, we can deterministically compute thecorresponding inverse kinematics solution for this pose. Ingeneral, because of joint limits, we tend to find only a singlesolution (if it exists) for a given end-effector pose and freeangle parameter. If a solution does not exist, it is possible tostep through the full range of motion of the redundant jointto search for an inverse kinematics solution.

IV. MOTION PLANNING ALGORITHM

The motion planning algorithm we describe in this paperoperates by constructing and searching a motion-primitivebased graph [5] using pre-defined and runtime-generatedmotion primitives. The task of the graph search itself isto find a path in the constructed graph, from the state thatcorresponds to the current pose of the object, to the goalstate. In other words, we consider the problem of finding amotion that gets the object grasped by the two arms from itscurrent pose to the goal pose.

In the following sections, we explain all of the componentsof the algorithm with a major focus on the new graphrepresentation, and the informative heuristic that efficientlyguides the search in finding the solution.

A. Graph Construction

Previously, when planning paths for single arm manipu-lation, we represented the configuration space of the armin joint space [5]. Thus, if we needed to plan motions fora robot arm with 7 DoF, it would result in a graph with

Fig. 2: The six degrees of freedom in the statespace.

7 dimensions.The combination of informative heuristics andadaptive motion primitives addressed the high dimensionalityof the statespace. However, if we were to construct a graph inthe same way for dual-arm tasks then we would end up witha 14 dimensional statespace. Fortunately though, the objectconstraint allows for a more compact graph representationthat we will now describe.

The graph is constructed using a lattice-based representa-tion. A lattice is a discretization of the configuration spaceinto a set of states, and connections between these states,where every connection represents a feasible path. Let us usethe notation G = (S,E) to denote the graph G we construct,where S denotes the set of states of the graph and E is the setof transitions between the states. The states in S are the setof possible (discretized) 4 DoF poses of the object coupledwith the joint angles of one joint (chosen to represent theredundancy) in each arm. That is, we define a state s asa 6-tuple, (x, y, z, θyaw, θ1, θ2) where (x, y, z) describe theglobal position of the center of the object, θyaw is the object’sglobal yaw angle and θ1, θ2 are the joint positions of theredundant joint in the right arm and the left arm, respectively.Refer to Figure 2 for a visualization of the 6-tuple for thePR2 robot.

The transitions in E are comprised of a set of feasiblemotion primitives. A motion primitive is defined here as avector of (translational and yaw) velocities of the object andthe two redundant joint velocities. The graph is dynamicallyconstructed by the graph search as it expands states becausepre-allocation is infeasible for a 6 dimensional graph.

The set of motion primitives that we used during exper-imentation can be seen in Figure 3. The set includes 26motions that just translate the object one cell in the sameway as the edges in a 26-connected grid. It also includestwo motions that rotate either free angle in both directions.Finally, it includes two motions that just yaw the object inthe world frame. We now describe how the motion primitivesare used.

Before a successor of state s, s′ can be added to the graphit must be checked for feasibility. That is, we check that jointconfigurations for both arms exist within the joint limits andare collision free. We use an inverse kinematics solver tocompute joint configurations for each arm that satisfy the

(a) xyz (26) (b) θ1, θ2 (4)

(c) θyaw (2)

Fig. 3: The set of 32 motion primitives we used.

state’s coordinates. One reason why the solver may fail is ifthe solution found violate the joint limits of the arms. If asolution is found for each arm, then the arm configurationsare forward simulated and checked for collisions with eachother, with obstacles in the environment, and for collisionsbetween the grasped object and the environment.

If the inverse kinematics solver fails to compute a solutionfor one or both of the arms, then rather than just reject theinvalid successor completely, we search over the redundantjoint for that arm for a valid solution. If a solution exists, thenwe generate an adaptive motion primitive [4], or ”a primitivethat is generated at runtime”. Essentially, the motion primi-tive that was used to reach the invalid state s′ is duplicatedbut with a new value for θ1, θ2 or both.

It should also be noted, that while we are using a verybasic set of motion primitives, more complicated motionscould also be generated and used by our approach, e.g. trans-lating the object along the x axis while rolling the right arm’sredundant joint. In addition, smoother motion primitives canalso be used by generating a set of intermediate states forthe motion that are not projected onto the discretized grid inthe state space. Generating and using such motion primitivesis a problem that we intend to pursue further in the future.

We found that this task space representation is advanta-geous over our previous joint space representation [4] in acouple of ways. A key advantage is the reduced discretizationeffects in the planned solution. Now that the path length isbeing optimized in the position space of the object, it allowsfor multiple joints to be moved at the same time whereaspreviously, our set of primitives was limiting the trajectory tomove one joint at a time giving off a very robotic appearance.Another advantage of this representation is that it allows forthe heuristic to be more informative and easier for the searchto follow. This will be described in detail in the Heuristicsection.

B. Cost Function

The cost function is designed to minimize the path lengthof the end effector while maximizing the distance betweenthe manipulator and nearby obstacles along the path. Thecost of traversing any transition between states s and s′ ingraph G can therefore be represented as c(s, s′) = ccell(s

′)+caction(s, s

′). The action cost, caction, is the cost of themotion primitive which is generally determined by the user.The soft padding cost, ccell, is a cost applied to cells close toobstacles to discourage the search from planning a path thatdrives any part of the manipulator close to nearby obstaclesif a safer path is possible.

An alternative cost function that we considered minimizesthe execution time of the planned trajectory. To do so, theplanner is configured with the desired joint velocities foreach joint in the arm. Then the cost of a transition betweenstates is computed by solving for the longest joint motionamongst all of the joints and then multiplying its executiontime by the desired cost per second. Minimizing executiontime usually has the same effect as minimizing the pathlength of the object, however, tight joint limits can sometimescause them to not correlate at all. While this cost functionoffered promising results, it was not used any further in ourexperiments.

C. Heuristic

Heuristic-based search algorithms such as the graph searchused here, depend on informative heuristics to efficientlyguide the search in promising directions towards a feasiblesolution. These algorithms require that the heuristic functionis admissible and consistent.

The ability to manipulate objects through cluttered en-vironments is the primary motivation of our research, andso a heuristic function that efficiently circumvents obstaclesis necessary. Early in our approach we used a 3D Dijkstrasearch to compute the cost of the least-cost path from theposition of the inner sphere of the object at a given state tothe object pose at the goal state. A similar method can befound in [5], however the end effector pose is used in thatcase. This proved to be an informative heuristic here as well.However, given the additional upright orientation constraintof the problem, we developed a more informative heuristicthat exploits this constraint.

Instead of modeling the object as a sphere when per-forming the 3D Dijkstra search, we can instead model itas a cylinder because we are constraining the object fromrolling or pitching. The radius of the cylinder is the radiusof the inner circle of the object, i.e. the circle centered atthe geometric center of the object (in the xy plane) with thelargest radius such that it is completely contained within theobject footprint, and the height of the cylinder is the heightof the object.

To compute the heuristic, we first inflate the obstacles oneach xy-plane by the radius of inner circle of the objectby iterating through the z-axis of the grid. Then on eachcall to the heuristic function, h(x,y,z), we check if cells

(a) z = 0 (b) z = 0, inflated

(c) z = 0, z = 1, z = 2, inflated

Fig. 4: The obstacles are shown in black and the inflatedcells are red. The radius of the inner circle of the object onthe xy plane is 1 cell. The height of the object is 3 cells so3 xy planes must be checked for collisions.

(x,y,z),(x,y,z+1), ... ,(x,y,z+n) are collision free for a radiusof n cells. A detailed example can be found in Figure 4.

Modeling the object as a cylinder is significantly moreinformative than using an inner sphere when the object’sdimensions are not similar along each axis, e.g. a tray whichis very wide and flat. The heuristic is then capable of guidingthe search through tighter spaces, e.g. when manipulating atray between two shelves of a bookshelf.

We use the radius of the inner circle along the xy-planeof the object so that the heuristic is complete, meaning if apath exists to manipulate the object to the goal state, thenit will be found. Needless to say that it does not mean thatif a feasible 3D path exists from the state to the goal thena feasible motion plan exists to manipulate the object to thegoal. It is interesting to note that using the radius of theouter circle may be much more informative when guidingthe search especially when the inner and outer circles differby a large amount. However, the outer circle sacrifices thecompleteness of the heuristic and thus the planner may notfind a solution if one exists. In the Experimental Resultssection we include a comparison of results from when weused the radius of the inner and outer circles.

It is important to note that the heuristic for a givenstate is computed when needed unlike in our previousapproaches [4], where the heuristic was precomputed for theentire workspace before planning began. This pre-planningstep required up to 0.6 seconds. Instead, now we initiallyrun the Dijkstra search until it expands the start state. Thenif the search ever encounters a state which has not yet beenadded to the Dijkstra search tree, we resume the Dijkstrasearch until the state of interest is expanded. This way weavoid expanding all of the states in the environment.

Fig. 5: Clockwise from top left: stick around a pole, woodboard in bookshelf, tray with wine glasses under a table, traywith wine glasses near wall and tray with a scotch glass inbookshelf.

D. Search

Any standard graph search algorithm can be used to searchthe graph G that is constructed. Given the complexity ofthe graph, however, optimal graph search algorithms such asA* [8] are infeasible to use. Instead we chose an anytimeversion of A* - Anytime Repairing A* (ARA*) [15].Thisalgorithm generates an initial, possibly suboptimal solutionquickly and then concentrates on improving this solutionwhile deliberation time allows. The algorithm guaranteescompleteness for a given graph G and provides a bound εon the suboptimality of the solution at any point of timeduring the search. ARA* speeds up the typical A* search byinflating the heuristic values by a desired inflation factor, ε.An ε greater than 1.0 will produce a solution guaranteed tocost no more than ε times the cost of an optimal solution.

V. EXPERIMENTAL RESULTS

Producing a set of randomly generated experiments is achallenging task for dual-arm object manipulation. Kinematicconstraints of the arms, the size of the grasped object and thepositions and orientations of the grasps result in a very tightfeasible workspace, not to mention obstacles. We manuallypicked start and goal poses for the object, by generating IKsolutions corresponding to them and checking that the solu-tions are collision free. We conducted twelve experimentsthat inspired by practical manipulation scenarios in fourdifferent cluttered environments with five different objects.All twelve experiments were implemented in simulation firstand then on the PR2 robot itself. Figure 5 shows the differentsimulation environments. The obstacles are in purple and thecollision model of the manipulated objects can be seen incyan. The actual objects that were used during the real robotexperiments can be seen in Figure 6.

Fig. 6: The objects that were modeled for the simulatedexperiments. The stick is not shown.

The results of the simulated experiments are shown inTable I. In all of the runs the planner was initialized with an ε= 100 and was given 15.0 seconds to generate a more optimalsolution if time permitted. The ε of the final solution foundis listed in the third column. The planning times include thetime it takes to compute the heuristic. The resolution of theobject’s pose in is 2 cm for the position and 2deg for the yawof the object as well as 2deg for both of the redundant joints.All of the tests require that the planner computes a path to a4-DoF pose constraint for the object with a tolerance of 0.03radians in the final yaw of the object and a 2 cm tolerance inthe position of the object. We do not require the redundantjoints to reach the goal at specified joint angles. We used theset of 32 motion primitives shown in Figure 3.

Time until FirstSoln. (s)

Expands. untilFirst Soln.

εfinal Expands. untilFinal Soln.

0.31 182 3 8,1610.15 76 3 7,5840.33 182 3 6,2652.01 544 5 5,0211.07 379 4 7,9910.98 432 4 6,44514.88 6,773 100 6,7850.56 31 3 6,7140.57 34 3 5,9601.06 322 5 4,9320.14 62 3 7,3440.13 68 3 6,437

TABLE I: Simulation results from 12 trials.

Earlier we noted that while the radius of the inner circleof the object in the xy-plane is used when computing theheuristic, the search could be sped up by using the radiusof the outer circle at the loss of the guarantee on thecompleteness of the search. Table II shows the results ofthe same set of experiments but using the radius of the outercircle to compute the heuristic. The statistics do in fact verifythe predicted speedup and without failures for this particularset of experiments. We plan to examine the incorporation ofthis informative heuristic, without sacrificing guarantees oncompleteness, in greater detail in future work.

A selected set of experiments that were performed on the

Time until FirstSoln. (s)

Expands. untilFirst Soln.

εfinal Expands. untilFinal Soln.

0.39 31 2 7,7580.32 43 2 6,0890.4 29 2 5,5170.79 145 3 5,9770.98 208 3 6,5270.28 26 2 6,3682.16 516 3 6,2900.5 40 3 6,4540.52 38 3 6,02712.16 1,996 4 2,9610.07 38 3 7,0150.12 68 3 6,703

TABLE II: Simulation results when using the radius of theouter circle in the xy-plane of the object to compute theheuristic.

Fig. 7: Shown here are four of the experiments that wererun on the PR2. Refer to the video for the execution of eightplanning instances in three of these environments.

PR2 robot can be seen in the attached video. During theexperiments, the planner was executed onboard the robot.The planning times and number of expansions can be foundin Table III. The radius of the inner circle was used tocompute the heuristic in all of the runs. The trajectories inthe video were generated only by parameterizing (in time)the paths generated by the planner. No smoothing operationswere performed on the paths themselves. The short jerkymotion at the start of each trajectory does not, to the best ofour knowledge, arise from the nature of the planned paths.The long delay in the video was the result of a delay encodedin our experimental and system (and not due to long planningtimes).

The video shows that while the trajectories appear to bereasonably smooth in the position space of the end-effectors,they may not be smooth in the joint space of the arms.While the speedup of the video enhances this effect, another

possible reason for this is that we are allowing the IK tosearch over the range of positions of the redundant jointswithout applying a cost for rolling either of the redundantjoints. If less erratic motion is desired in the upper arm rolljoints of the PR2, then an additional transition cost could beused.

As a demonstration of the consistency in the planner’ssolutions, the video includes two pairs of identical trajecto-ries. The trajectories titled ‘Table 1’ and ‘Table 3’ as well as‘Table 2’ and ‘Table 4’ have the same number of expansionsand produce the same exact trajectories but are the result ofseparate unique calls to the planner.

Planning Time (s) Expands.Backgammon 1 1.03 523Backgammon 2 0.61 357Wall 1 9.28 1,710Wall 2 2.59 619Table 1 0.38 39Table 2 0.38 36Table 3 0.36 39Table 4 0.34 36

TABLE III: Planning times of the trajectories shown in thevideo. An ε = 30 was used.

VI. CONCLUSIONWe presented a search-based motion planning algorithm

for dual-arm object manipulation with an upright orientationconstraint. In our approach, we leveraged a compact repre-sentation of the problem and an informative heuristic that ex-ploits the orientation constraint in creating a method that canefficiently plan dual-arm motions in less than two secondsin over ninety percent of our runs. Our algorithm relies onan anytime graph search to generate initial solutions quicklyas well as provide theoretical guarantees on completeness,consistency and provide a bound on the suboptimality ofthe solution with respect to the graph used to represent theplanning problem. We believe that the upright constrainton the object is a very practical one and it covers manyof the dual-arm manipulation scenarios found in the homeand in other human service tasks. Our experiments on thePR2 showed the ability of the planner to handle real worldcluttered manipulation scenarios and its ability to generatemotions that can be executed on the robot with minimal post-processing.

VII. ACKNOWLEDGEMENTSWe thank Willow Garage for their partial support of this

work. In addition, this research was partially sponsoredby the Army Research Laboratory Cooperative AgreementNumber W911NF-10-2-0016. We would also like to thankMike Phillips for his invaluable input.

REFERENCES

[1] Dmitry Berenson, Siddhartha S. Srinivasa, Dave Ferguson, AlvaroCollet, and James J. Kuffner. Manipulation planning with workspacegoal regions. In IEEE International Conference on Robotics andAutomation, May 2009.

[2] Christoph Borst, Christian Ott, Thomas Wimbock, Bernhard Brunner,Franziska Zacharias, Berthold Baeum, Ulrich Hillenbrand, Sami Had-dadin, Alin Albu-Schaeffer, and Gerd Hirzinger. A humanoid upperbody system for two-handed manipulation. In IEEE InternationalConference on Robotics and Automation, pages 2766–2767, April2007.

[3] Matei Ciocarlie, Kaijen Hsiao, E. Gil Jones, Sachin Chitta, Radu Bog-dan Rusu, and Ioan Alexandru Sucan. Towards reliable grasping andmanipulation in household environments. In ISER, New Delhi, India,December 2010.

[4] Benjamin J. Cohen, Sachin Chitta, and Maxim Likhachev. Planningfor Manipulation with Adaptive Motion Primitives. In Proceedingsof the IEEE International Conference on Robotics and Automation(ICRA), 2011.

[5] Benjamin J. Cohen, Gokul Subramanian, Sachin Chitta, and MaximLikhachev. Search-based Planning for Manipulation with MotionPrimitives. In Proceedings of the IEEE International Conference onRobotics and Automation (ICRA), 2010.

[6] R. Diankov, N. Ratliff, D. Ferguson, S. Srinivasa, and J. J. Kuffner.Bispace planning: Concurrent multi-space exploration. In Robotics:Science and Systems, Zurich, Switzerland 2008.

[7] D. Furcy. Chapter 5 of Speeding Up the Convergence of OnlineHeuristic Search and Scaling Up Offline Heuristic Search. PhD thesis,Georgia Institute of Technology, 2004.

[8] P. E. Hart, N. J. Nilsson, and B. Raphael. A formal basis for theheuristic determination of minimum cost paths. IEEE Transactions onSystems, Science, and Cybernetics, SSC-4(2):100–107, 1968.

[9] K. Hauser and V. Ng-Thow-Hing. Fast smoothing of manipulatortrajectories using optimal bounded-acceleration shortcuts. In IEEEInternational Conference on Robotics and Automation, May 2010.

[10] Andreas Hormann. On-line planning of action sequences for a two-armmanipulator system. In IEEE International Conference on Roboticsand Automation, May 1992.

[11] Dov Katz, Emily Horrell, Yuandong Yang, Brendan Burns, ThomasBuckley, Anna Grishkan, Volodymyr Zhylkovskyy, Oliver Brock, andErik Learned-Miller. The UMass Mobile Manipulator UMan: AnExperimental Platform for Autonomous Mobile Manipulation. In IEEEWorkshop on Manipulation for Human Environments, Philadelphia,USA, August 2006.

[12] Yoshihito Koga and Jean-Claude Latombe. Experiments in dual-armmanipulation planning. In IEEE International Conference on Roboticsand Automation, May 1992.

[13] Yoshihito Koga and Jean-Claude Latombe. On multi-arm manipu-lation planning. In IEEE International Conference on Robotics andAutomation, May 1994.

[14] Tsai-Yen Li and Jean-Claude Latombe. On-line manipulation planningfor two robot arms in a dynamic environment. In IEEE InternationalConference on Robotics and Automation, May 1995.

[15] M. Likhachev, G. Gordon, and S. Thrun. ARA*: Anytime A* withprovable bounds on sub-optimality. In Advances in Neural InformationProcessing Systems (NIPS) 16. Cambridge, MA: MIT Press, 2003.

[16] J. Maitin-Shepard, M. Cusumano-Towner, J. Lei, and P. Abbeel. Clothgrasp point detection based on multiple-view geometric cues withapplication to robotic towel folding. In IEEE Intl. Conf. on Roboticsand Automation, 2010.

[17] J. Pearl. Heuristics: Intelligent Search Strategies for ComputerProblem Solving. Addison-Wesley, 1984.

[18] Nathan Ratliff, Matt Zucker, J. Andrew Bagnell, and SiddharthaSrinivasa. CHOMP: Gradient optimization techniques for efficientmotion planning. In IEEE International Conference on Robotics andAutomation, pages 489–494, 12–17 May 2009.

[19] Radu Bogdan Rusu, Ioan A. Sucan, Brian Gerkey, Sachin Chitta,Michael Beetz, and Lydia E. Kavraki. Real-time perception guidedmotion planning for a personal robot. In International Conference onIntelligent Robots and Systems, St. Louis, USA, October 2009.

[20] Jonathan Scholz, Sachin Chitta, Bhaskara Marthi, and MaximLikhachev. Cart pushing with a mobile manipulation system: Towardsnavigation with moveable objects. In IEEE International Conferenceon Robotics and Automation, Shanghai, China, 2011.

[21] Siddhartha Srinivasa, David Ferguson, Michael Vande Weghe, RosenDiankov, Dmitry Berenson, Casey Helfrich, and Hauke Strasdat. TheRobotic Busboy: Steps Towards Developing a Mobile Robotic HomeAssistant. In Intl. Conference on Intelligent Autonomous Systems (IAS-10), July 2008.

[22] Nikolaus Vahrenkamp, Dmitry Berenson, Tamim Asfour, JamesKuffner, and Rudiger Dillmann. Humanoid motion planning for dual-arm manipulation and re-grasping tasks. In IEEE/RSJ InternationalConference on Intelligent Robots and Systems, October 2009.

[23] R. Zhou and E. A. Hansen. Multiple sequence alignment using A*.In Proceedings of the National Conference on Artificial Intelligence(AAAI), 2002. Student abstract.

[24] R. Zhou and E. A. Hansen. Beam-stack search: Integrating backtrack-ing with beam search. In Proceedings of the International Conferenceon Automated Planning and Scheduling (ICAPS), pages 90–98, 2005.