Single- and dual-arm motion planning with heuristic search

Heuristic searches such as the A* search are a popular means of finding least-cost plans due to their generality, strong theoretical guarantees on completeness and optimality, simplicity in implementation and consistent behavior. In planning for robotic manipulation, however, these techniques are commonly thought of as impractical due to the high dimensionality of the planning problem. In this paper, we present a heuristic search-based approach to motion planning for manipulation that does deal effectively with the high dimensionality of the problem. Our approach achieves the necessary efficiency by exploiting the following three key principles: (a) representation of the planning problem with what we call a manipulation lattice graph; (b) use of the ARA* search which is an anytime heuristic search with provable bounds on solution suboptimality; and (c) use of informative yet fast-to-compute heuristics. The paper presents the approach together with its theoretical properties and shows how to apply it to single-arm and dual-arm motion planning with upright constraints on a PR2 robot operating in non-trivial cluttered spaces. An extensive experimental analysis in both simulation and on a physical PR2 shows that, in terms of runtime, our approach is on a par with other most common sampling-based approaches despite the high dimensionality of the problems. In addition, the experimental analysis shows that due to its deterministic cost minimization, the approach generates motions that are of good quality and are consistent, in other words, the resulting plans tend to be similar for similar tasks. For many problems, the consistency of the generated motions is important as it helps make the actions of the robot more predictable for a human controlling or interacting with the robot.