[IROS22] Sequential Manipulation Planning on Scene Graph

Abstract

We devise a 3D scene graph representation, contact graph+ (cg+), for efficient sequential manipulation planning. Augmented with predicate-like attributes, this contact graph based representation abstracts scene layouts with succinct geometric information and valid robot-scene interactions. Goal configurations, naturally specified on contact graphs, can be produced by a genetic algorithm with a stochastic optimization method. A task plan is then initialized by computing the Graph Editing Distance (GED) between the initial contact graph and the goal configuration, which generates graph edit operations corresponding to possible robot actions. We finalize the task plan by imposing constraints to regulate the temporal feasibility of graph edit operations, ensuring valid task and motion correspondences. In a series of simulated and real experiments, robots successfully complete complex sequential object rearrangement tasks that are difficult to specify using conventional planning language like Planning Domain Definition Language (PDDL), demonstrating high potential of planning sequential manipulation tasks on cg+.