# Task and Motion Planning in Hierarchical 3D Scene Graphs

arXiv: https://arxiv.org/abs/2403.08094 · HTML: https://mlsanity.com/paper/2403.08094/

## Key facts

- Cited by 12 papers in the map (data through 14 Oct 2026).
- First posted on arXiv 12 Mar 2024.

## Built on by (most cited first, 12 of 12)

- [SuperDec: 3D Scene Decomposition with Superquadric Primitives](https://mlsanity.com/paper/2504.00992/) (2025)
- [SPINE: Online Semantic Planning for Missions with Incomplete Natural Language Specifications in Unstructured Environments](https://mlsanity.com/paper/2410.03035/) (2024)
- [MORE: Mobile Manipulation Rearrangement Through Grounded Language Reasoning](https://mlsanity.com/paper/2505.03035/) (2025)
- [ASHiTA: Automatic Scene-grounded HIerarchical Task Analysis](https://mlsanity.com/paper/2504.06553/) (2025)
- [Language-Grounded Hierarchical Planning and Execution with Multi-Robot 3D Scene Graphs](https://mlsanity.com/paper/2506.07454/) (2025)
- [FunFact: Building Probabilistic Functional 3D Scene Graphs via Factor-Graph Reasoning](https://mlsanity.com/paper/2604.03696/) (2026)
- [3D Scene Graphs: Open Challenges and Future Directions](https://mlsanity.com/paper/2606.19383/) (2026)
- [Analogical Trajectory Transfer](https://mlsanity.com/paper/2605.14393/) (2026)
- [SceneGraphGrounder: Zero-Shot 3D Visual Grounding via Structured Scene Graph Matching](https://mlsanity.com/paper/2605.21788/) (2026)
- [Prior-SG: Task and Prior Driven Region Segmentation for Scene Graphs in Arbitrarily-Structured Environments](https://mlsanity.com/paper/2608.06170/) (2026)
- [A Review Of Robotic World Models For Dynamic Environments Based On Factor And Scene Graphs](https://mlsanity.com/paper/2610.08800/) (2026)
- [Interactive Navigation Beyond Reachability: Benchmark and Agentic Framework](https://mlsanity.com/paper/2610.17089/) (2026)

## Abstract (arXiv)

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 urlhttps://youtu.be/v8fkwLjBn58.