Task and Motion Planning in Hierarchical 3D Scene Graphs

TL;DR

This paper introduces a hierarchical 3D scene graph-based method for scalable task and motion planning in large environments, enabling efficient planning and execution on real robots.

Contribution

It presents a novel approach to derive scalable planning domains from 3D scene graphs, including incremental object addition during planning.

Findings

Effective planning in large-scale 3D scene graphs

Successful real-world robot execution of plans

Scalable planning with incremental scene graph updates

Abstract

Recent work in the construction of 3D scene graphs has enabled mobile robots to build large-scale metric-semantic hierarchical representations of the world. These detailed models contain information that is useful for planning, however an open question is how to derive a planning domain from a 3D scene graph that enables efficient computation of executable plans. In this work, we present a novel approach for defining and solving Task and Motion Planning problems in large-scale environments using hierarchical 3D scene graphs. We describe a method for building sparse problem instances which enables scaling planning to large scenes, and we propose a technique for incrementally adding objects to that domain during planning time that minimizes computation on irrelevant elements of the scene graph. We evaluate our approach in two real scene graphs built from perception, including one constructed from the KITTI dataset. Furthermore, we demonstrate our approach in the real world, building our representation, planning in it, and executing those plans on a real robotic mobile manipulator. A video supplement is available at \url{https://youtu.be/v8fkwLjBn58}.