# Task and Motion Planning in Hierarchical 3D Scene Graphs

Aaron Ray∗,1, Christopher Bradley∗,2, Luca Carlone1, Nicholas Roy2 * denotes
equal contribution
1 LIDS, MIT, Cambridge, MA 02142, USA ({aaronray<EMAIL_ADDRESS>
2 CSAIL, MIT, Cambridge, MA 02142, USA ({cbrad<EMAIL_ADDRESS>
###### Abstract

Recent work in the construction of 3D scene graphs has enabled mobile robots
to build large-scale hybrid metric-semantic hierarchical representations of
the world. These detailed models contain information that is useful for
planning, however how to derive a planning domain from a 3D scene graph that
enables efficient computation of executable plans is an open question. 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 identify a method for building sparse problem domains which enable
scaling to large scenes, and propose a technique for incrementally adding
objects to that domain during planning time to avoid wasting computation on
irrelevant elements of the scene graph. We test our approach in two hand
crafted domains as well as two scene graphs built from perception, including
one constructed from the KITTI dataset. A video supplement is available at
https://youtu.be/63xuCCaN0I4.

##  I INTRODUCTION

We aim to enable an autonomous agent to solve large-scale Task and Motion
Planning (TAMP) problems in real-world environments. In order to solve a TAMP
problem, an abstract planning domain is needed, which efficiently and
accurately represents the robot’s environment and the available actions. Over
the past several years, significant progress has been made in the area of
generating hybrid metric-semantic representations of the world using 3D scene
graphs. One such example is the Hydra scene graph [12], which jointly
represents low-level information such as scene geometry and robot
trajectories, and higher level information such as semantically-annotated
object instances, places, rooms, and buildings. This environmental abstraction
lends itself well to large-scale TAMP problems, as it is capable of storing
both higher level abstractions like objects and connectivity of regions needed
for task planning, as well as the low-level metric information required to
check kinematic feasibility of different actions [12]. However, how best to
construct a planning domain from the 3D scene graph representation remains an
open question.

The trend in scene graph development has been toward encoding an increasing
amount of information in the hierarchical representations. Such detailed
information may be crucial to the success of a planner, as the more faithfully
any planning domain (regardless of the planning system or formalism)
represents the real world, the more likely a found solution will actually be
executable by the robot. Moreover, increased representational capacity of the
planning domain leads to a wider range of robot goals that can be specified,
thus increasing flexibility and generality.

However, as a planning problem instance grows in the number of objects, so too
does the complexity of finding a plan. TAMP is PSPACE-Hard [22], so problems
can become computationally intractable very quickly as the sizes of the state
and action spaces grow [7]. To avoid creating intractable planning problems
when converting a 3D scene graph into a planning problem, it is critical to
determine which elements of the environment (i.e., which elements of the scene
graph) are relevant. Consider, for example, a robot responding to a Chemical,
Biological, Radiological, Nuclear, and Explosive (CBRNE) scenario, receiving
instructions to inspect and potentially neutralize dangerous objects scattered
in a large area, represented as a pre-built scene graph. The robot is able to
pass near an object only after it has neutralised and cleared it, and may be
instructed to avoid particular regions entirely, to solve different tasks.
Depending on the geometry of the scene and the specified goal, only a subset
of these potentially dangerous obstacles and regions may ultimately be
relevant to finding a plan. But, for a robot building a scene graph
representation from perception, it is not at all obvious which elements of the
scene graph should be added to a planning domain to ensure a valid plan can be
found and executed. If we were to add _every_ potentially relevant element to
our planning domain, we would be guaranteed to eventually find a plan which
takes our robot to its goal, but at the expense of substantial computational
complexity. On the other hand, if we are too aggressive in pruning away
seemingly irrelevant elements, we may restrict ourselves to sub-optimal
solutions, or fail to find a plan at all.

Previous approaches to the problem of inferring a task-relevant planning
domain have relied on representations of connectivity in the scene graph to
prune superfluous elements [1]. However, these efforts have been limited to
very specific task planning problems, as the pruning approaches employed often
remove information necessary for checking the geometric feasibility of plans,
or implicitly limit the types of goals that can be specified. Alternative
approaches for reducing the planning domain size involve attempting to learn
the relevance of planning objects, then incrementally adding objects to the
domain according to the learned relevance score until the problem is solvable
[18]. Unfortunately, this approach requires training on numerous similar
planning problems, and is difficult to generalize to tasks at large scales.

In this work, we propose a novel approach to both enable and accelerate TAMP
in large environments (Fig. 1). The first contribution of this work is the
definition of a set of properties that a TAMP domain must possess to ensure
that any plans returned by solving a problem actually meet the user’s
specifications. We then define a method for translating a Hydra scene graph
[12] into a planning domain that meets these criteria. Our second contribution
is the formulation of a sufficient condition for removing symbols from a
planning problem while maintaining feasibility. This condition shows that, for
example, many of the places in a complete scene graph can be ignored when
formulating certain kinds of planning problems, greatly reducing planning
times. This improvement relies on the definition of a hierarchical planner,
which ignores certain elements of the scene at the highest level to accelerate
discrete planning, while planning over the entire scene at lower levels to
ensure constraints on the plan are not violated. The third contribution is a
method to further accelerate planning by incrementally identifying objects in
the scene as relevant during search according to how they affect the
feasibility of certain abstract plans. In contrast to the places component of
the domain, finding a small set of objects ahead of time to include in the
planning instance while guaranteeing that the problem remains feasible is
quite difficult. Instead, each time we fail to solve a sub-problem in an
abstract plan, we use the scene graph to identify which objects were
responsible. We then augment our domain with those objects, and continue
planning. We show the effectiveness of our approach across two hand-crafted
domains, in addition to two scene graphs built from real perception.

##  II Task and Motion Planning in 3D Scene Graphs

Figure 1: An illustration of how we derive our planning representation from a
3D scene graph generated from the KITTI dataset. In (A), we present three
levels of the scene graph hierarchy, with the low-level Mesh layer on the
bottom, the Places layer above that, and the Objects layer on the top. In (B),
we see an isometric view of this graph, giving an insight to the scale, while
also plotting the higher level Regions layer. In (C) we show a very simplified
version of this scene, where the agent is given a task specified in logic. The
agent is tasked with either visiting Place 6 while avoiding Place 1, or
visiting Place 5. Given the geometry of the scene, the first clause in this
disjunction is un-satisfiable. We also see that Place 5 is partially
obstructed by a suspicious object, so the agent must consider either avoiding
it (green trajectory), or inspect and neutralize the object (blue trajectory)
before continuing to its goal. To solve problems like this one at large
scales, we take advantage of the structure of the scene graph to accelerate
planning.

TAMP jointly considers elements of high-level task planning [8, 13] and low-
level motion planning [15] in an attempt to solve hybrid discrete/continuous,
multi-modal planning problems [7].

###  II-A Task and Motion Planning Preliminaries

A common formalism for encoding planning problems is Planning Domain
Definition Language (PDDL). In a PDDL problem, a _state_ $\mathcal{I}$ is a
set of facts: instances of boolean functions called predicates
$p(\bar{x})\in\mathcal{P}$, which are parameterized by tuples of objects
$\bar{x}=[x_{1},...,x_{k}]$, $x\in\mathcal{O}$. Transitions between states are
defined by actions $a(\bar{x})\in\mathcal{A}$ (also parameterized by objects)
which are expressed as two sets of predicates: preconditions
$\text{Pre}(a_{i})$ and effects $\text{Eff}(a_{i})$. An action’s preconditions
determine if an instance can be applied from a particular state, and its
effects define the set of facts that are added ($\text{Eff}^{+}(a_{i}))$ or
removed $(\text{Eff}^{-}(a_{i}))$ from the state. A planning _domain_ is
composed of lifted sets of predicates $\mathcal{P}$ and actions $\mathcal{A}$,
and a problem _instance_
$P=(\mathcal{P},\mathcal{A},\mathcal{O},\mathcal{I}_{0},\mathcal{G})$ combines
a domain with an initial state $\mathcal{I}_{0}$ and a set of goal states
$\mathcal{G}$, parameterized by objects $\mathcal{O}$. We will refer to
objects in a PDDL problem as _symbols_ when they may be confused with physical
objects from a scene graph.

One approach for representing a TAMP problem —which we use in this work— is an
extension of PDDL called PDDLStream [6]. A PDDLStream problem instance
$(\mathcal{P},\mathcal{A},\mathcal{S},\mathcal{O},\mathcal{I}_{0},\mathcal{G})$
represents the discrete search portion of a TAMP problem in PDDL, with the
same representation of predicates, actions, objects, initial state, and set of
goal states. In a TAMP problem however, certain actions may have symbol
parameterisations that are either cumbersome or intractable to add to a
problem instance. For example, consider a move action, which defines robot
motion from one configuration to another, and which may have a precondition
that requires the existence of a collision-free trajectory between those
configurations. Adding all such trajectory symbols between all possible points
in the configuration space to the planning domain is impossible, so instead,
we define sub-problems called _streams_ $s\in\mathcal{S}$ which can be solved
to return symbols that (if a solution exists) can be used to certify the
preconditions of the associated action. In the case of a trajectory, the
relevant sub-problem/stream might be a motion planner, which, if solved,
returns a symbol representing a collision-free trajectory between two
configurations that the robot can follow. We refer the reader to [6] for a
detailed description of PDDLstream.

Solutions to PDDLStream problems take the form of a sequence of parameterized
action instances
$\pi=[a_{1}(\bar{x}_{1}),a_{2}(\bar{x}_{2}),...,a_{n}(\bar{x}_{n})]$ that
define a plan, where assigned parameters $\bar{x}$ satisfy each action’s
constraints [7]. From the parameters of an action, we can derive a _motion
sequence_ , which specifies how a robot executes a particular action. For
example, from an action plan composed of a sequence of move actions, the
corresponding motion sequence would be composed of the trajectories that were
solved for by the motion-planning sub-problem and which are parameters of
those move actions. Executing that sequence would involve multiple calls to a
trajectory controller. We say that two motion sequences are equivalent if they
result in the agent acting identically. For any action sequence, there is a
corresponding sequence of states
$\mathcal{I}_{\pi}=[\mathcal{I}_{0},\mathcal{I}_{1},\mathcal{I}_{2},...,\mathcal{I}_{n}]$,
leading from the initial state to a goal state that can be constructed from
each action’s effects. For an action plan $\pi$, its corresponding state plan
$\mathcal{I}_{\pi}$ is _valid_ if $\mathcal{I}_{i}\in\text{Pre}(a_{i+1})$ for
$i=0,...,N-1$, and $\mathcal{I}_{N}\in\mathcal{G}$. Tasks specified in PDDL
can be solved by a range of solvers [9, 11], and any state plan found by such
a solver is valid by construction. A feasible planning problem is one for
which there is a valid solution.

###  II-B Building 3D Scene Graphs from Perception

In most TAMP settings, the planning domain and problem instance are assumed to
be given ahead of time. However, a robot operating in the real world must be
able to build its own world model from perception, especially the symbols and
predicates that describe the world states. Recently, 3D scene graphs such as
those implemented by Hydra [12] or S-Graphs [3] have emerged as promising
hierarchical representations that can be built in real-time onboard robots, to
provide abstractions that are useful for encoding planning domains.

Our definition of a 3D scene graph, directly based on Hydra [12], consists of
several layers of increasing abstraction (see Fig. 1). Each layer consists of
a collection of _nodes_ representing the location and other attributes of an
element, with _edges_ connecting nodes within the same layer representing
relative spatial constraints and edges between different layers representing
an inclusion relationship. The lowest layer of the hierarchy is a
semantically-annotated mesh of the scene geometry. The next layer contains
objects identified by a semantic image segmentation. The _places_ layer
represents navigable regions of the environment based on semantic and
geometric properties of the mesh. Places are clustered into groups based on
geometric and semantic information, and these groups become nodes in the
higher-level _regions_ layer (e.g., rooms in an indoor environment).

Previous work on 3D scene graphs has mainly focused on indoor uses. These
representations rely on the Generalized Voronoi Diagram (GVD) [16] to generate
places, an abstraction of 3D spatial connectivity, which are not well suited
for ground robot navigation. We use an alternate formulation of 2D places in
our _navigable scene graph_. Each of these places represents a 2D polygon with
consistent terrain classification, and is connected to adjacent places in the
scene graph. As the resolution of the places is much coarser than the mesh
resolution, planning over sequences of places can be much faster than planning
over the mesh itself, while still retaining important geometric information.
The learned metric-semantic method for extracting regions from places
presented in Strader et al. [21] is more appropriate for these 2D places
compared to the purely geometric method room-based region extraction
originally presented in Hughes et al. [12]. Hydra and its region-based
clustering extension [21] can construct this map representation in realtime
from RGBD sensor data while accounting for odometry drift, enabling large
scale, consistent, and information-rich maps.

###  II-C Inferring the Planning Domain from Scene Graphs

We introduce a method for deriving a planning problem instance from a Hydra
scene graph to demonstrate the salient aspects of solving planning problems
based on large-scale environments. In general, we generate six classes of
predicates that may be relevant for scene graph related planning. 1) Type
information derived from the nodes of the graph, where each node is a unary
predicate: (Configuration ?c), (Place ?p), and (Object ?o), etc. 2)
Agent/object predicates that define the state of the robot and objects:
(AtConfig ?c), (AtPlace ?p), (AtRoom ?r), etc. 3) Connection predicates
defined by edges in the same level of the graph: (Connected ?n1 ?n2). 4)
Hierarchical predicates indicating edges connecting nodes of different levels
of the graph: (PoseInPlace ?c ?p), (PlaceInRoom ?p ?r), etc. 5) Preconditions
of actions that are certified by solving a stream’s associated sub-problem.
For the example move action, the predicate certified by the motion-planner
would contain the two configurations, and the trajectory between them:
(Trajectory ?c1 ?t ?c2). 6) Additional, problem specific predicates defined by
the user that can be used to specify goal states and problem constraints such
as which places to visit or which objects to collect.

As a running example for the remainder of this paper, we define an example
problem using these predicates, motivated by CBRNE scenarios. In this
“Inspection Domain”, an agent can be commanded to visit or avoid certain
places, and inspect and neutralise objects that have been marked as
suspicious. These suspicious objects cannot be passed until they have been
inspected and neutralised. We therefore define problem specific predicates:
(VisitedPlace ?p) which indicate the current and past places the robot has
been to, and (Safe ?o) or (Suspicious ?o) which describe an object. We define
sub-problems for planning motion between two poses, sampling poses for
inspecting and neutralizing objects, and sampling poses in a specific place.
Goal specifications in this domain can include positive or negated facts based
on these predicates. The agent’s available actions are to move between poses
in connected places, and to inspect objects from appropriate poses (for
simplicity we do not separate the inspect and neutralise actions). Any valid
plan is composed of these actions, which at execution time are converted into
a motion sequence of FollowPath($t_{i}$) and InspectObject($o_{i}$) primitives
for paths $t_{i}$ and objects $o_{i}$. Places and objects are the most
numerous nodes in the scene graph, so we need to ensure solving tasks in our
domain remains tractable as the number of places and objects grows. The
complexity of dealing with places and objects will have the largest
implication for the move action, which we present here:

      (:action move
        :parameters (?p1 ?p2 ?c1 ?c2 ?t)
        :precondition (and
                  (Trajectory ?c1 ?t ?c2)
                  (PoseInPlace ?c1 ?p1)
                  (PoseInPlace ?c2 ?p2)
                  (Connected ?p1 ?p2)
                  (AtPose ?c1)
        :effect (and (AtPose ?c2)
                     (not (AtPose ?c1))
                     (VisitedPose ?c2))))
      (:derived (AtPlace ?p)
        (exists (?pose)
                (and (AtPose ?pose)
                     (PoseInPlace ?pose ?p))))

This action allows the agent to move between its current place $p_{1}$ and any
place $p_{2}$ that is connected to $p_{1}$ in the scene graph, and updates the
state to note the agent’s current pose and that $p_{2}$ has been visited. We
refer to this problem representation as the _direct encoding_ of the
Inspection domain.

The direct encoding of the problem has one major drawback: moving between
places that are not physically near each other necessitates chaining together
potentially many move actions, which requires motion planning in many short
segments between neighboring places. For simple navigation, planning in short
segments can actually aid in the efficiency of long-distance motion planning,
as we will discuss in Sec.III-C. However, relying on a general task planner to
construct long-horizon action sequences, any segment of which may be
invalidated by the low-level geometry, leads to very inefficient planning in
practice. In the Inspection domain for example, solving a motion-planning sub-
problem for a trajectory requires accounting for Suspicious objects that may
block the way. However, there is no a priori logical connection in the PDDL
problem between a trajectory and the objects which might interfere with it.
Therefore, choosing the right object to inspect in order to unblock a path is
difficult for the task planner. The longer the sequence of move actions needed
to reach a goal state, the more potential plans the solver must consider. This
scales poorly both in the size of the environment, and as we consider
increasingly complex goal specifications (Sec. IV).

##  III Scalable Scene Graph Planning

Our objective is to both enable, and reduce the computational burden of
solving TAMP problems in large environments. We emphasize three main criteria
in our development of a DSG planning framework:

  1. 1.

Generating plans that we know can be executed.

  2. 2.

Preserving the set of goals that can be specified and solved.

  3. 3.

Planning at large scale.

Solving problem instances with the direct encoding described above is
computationally intractable for large scenes. To address this potential
intractability, we first define the characteristics of a planning domain that,
if met, would enable sparsification while still meeting the criteria listed
above. Next, we propose a method for constructing a simplified planning domain
that has these characteristics and enables planning in large scenes.
Critically, our method depends on factoring the task and motion planning
problem into separate task, navigation, and motion planning problems, aligned
with different layers of the scene graph hierarchy. Finally, we describe our
approach for planning, using feedback from failed attempts to solve sub-
problems to add facts to our domain.

###  III-A Simplifying Problem Instances

One method for reducing the complexity of a planning problem is to shorten the
depth and branching factor of the search that produces a plan. In the case of
the direct encoding of our _Inspect Domain_ , one way to do this would be to
prune Place symbols from the planning domain that are not relevant to the
problem instance, and redefine the move action to allow for travel over longer
distances. This pruning would remove possible regions the robot might travel
and potentially reduce the depth of search required to find a plan, thus
simplifying the search problem. However, identifying symbols that do not
impact the solution is in general as hard as solving the planning problem
itself, and simply removing places from a problem instance might render the
problem as encoded in Section II-C infeasible if it is done naively. In this
section, we will show that slight modifications to the domain and motion
planning sub-problem lead to straightforward identification of symbols that
can safely be removed from the planning problem. We begin by defining a set of
symbols that are redundant for a particular goal specification.

######  Definition 1 (Redundant Symbol).

For a set of domain actions $\mathcal{A}$ and specific goal $\mathcal{G}$, a
symbol x is redundant if both of the following hold:

  1. 1.

For every valid plan $\pi$ where x parameterizes an action, there is another
valid plan $\pi^{\prime}$ with equivalent motion sequence, where x is not an
action parameter,

  2. 2.

No action precondition or goal, expressed in negative normal form, contains a
universal quantifier that can be parameterized by x.

The brief intuition behind this notion of redundancy is that 1) if any plan
involving the symbol yields a motion sequence that can be rewritten without
the symbol, the symbol is redundant, and 2) if we solve a planning instance
where a redundant symbol has been removed, we would like to know that the plan
is still valid in the original problem. The requirements for a symbol to be
redundant are quite strong (_every_ plan that uses a symbol must have an
alternate plan that does not use the symbol and still results in the same
motion sequence), but we will later show that a slight modification to the
movement action lets many places have this property. We now prove that we do
not lose anything by removing a redundant symbol.

######  Proposition 1 (Removing Redundant Symbol Preserves Feasibility).

Consider a feasible planning instance
$(\mathcal{P},\mathcal{A},\mathcal{S},\mathcal{O},\mathcal{I}_{0},\mathcal{G})$.
For a redundant symbol $x\in\mathcal{O}$, we define a related instance
$R^{\prime}=(\mathcal{P},\mathcal{A},\mathcal{S},\mathcal{O}^{\prime},\mathcal{I}_{0}^{\prime},\mathcal{G}^{\prime})$
where $x$ has been removed, i.e., $\mathcal{O}^{\prime}=\mathcal{O}\setminus
x$ and $\mathcal{I}_{0}^{\prime}$ contains all facts in $\mathcal{I}_{0}$
except those parameterized by $x$, and similarly for $\mathcal{G}^{\prime}$.
Let $\Pi_{R}$ denote the set of valid plans for $R$. Then,
$\Pi_{R^{\prime}}\subseteq\Pi_{R}$ and $\Pi_{R^{\prime}}\neq\emptyset$.

###### Proof.

Consider $\pi\in\Pi_{R}$. If $\pi$ does not contain any actions parameterized
by $x$, then the same plan $\pi$ is also a valid solution for $R^{\prime}$.
Consider the alternative case, where $\pi$ does contain an action
parameterized by $x$. By Definition 1, there is another plan $\pi^{\prime}$
with equivalent motion sequence not parameterized by $x$, which is a valid
solution for $R^{\prime}$.

Now that we have shown that $\Pi_{R^{\prime}}$ is not empty, we need to show
that any valid plan for $R^{\prime}$ is valid for $R$. Consider plan
$\pi=[a_{1},...,a_{N}]\in\Pi_{R^{\prime}}$ with corresponding state plan
$\mathcal{I}_{\pi}=[\mathcal{I}_{0},...,\mathcal{I}_{N}]$. If the addition of
facts $\mathcal{F}$ parameterized by $x$ make $\pi$ invalid, then there must
exist a state $\mathcal{I}_{k}$ such that
$\mathcal{I}_{k}\cup\mathcal{F}\notin\text{Pre}(a_{k+1})$, which means that
$a_{k+1}$ is parameterized by a symbol that did not exist in $R^{\prime}$.
Only a universal or existential quantifier in $\text{Pre}(a_{k+1})$ can cause
$a_{k+1}$ to be parameterized by an additional symbol. Adding additional facts
cannot turn an existentially quantified formula from true to false. By
Definition 1, $a_{k+1}$ does not have any universal quantifiers that can be
parameterized by $x$ in its precondition. Thus $\pi$ must be valid for $R$.

∎

In the original direct domain encoding presented in Sec. II-C, there are no
redundant places, since removing any place implies that no other plans could
result in a trajectory going through that place, which may be necessary
depending on the geometry of the scene. However, let us consider a domain with
a more general move action: moveRelaxed. This action takes $N$ places as
parameters and has a precondition that a sub-problem is solved for a
trajectory that goes through each of these $N$ places (and possibly others not
included in the set of $N$). Trajectory represents this trajectory.

      (:action moveRelaxed
        :parameters (?p1 ... ?pN ?c1 ?t ?c2)
        :precondition (and
            (Trajectory ?p1 ... ?pN ?c1 ?c2 ?t)
            (AtPose ?c1)
        :effect (and (AtPose ?c2)
                     (not (AtPose ?c1))
                     (VisitedPlace ?p1)
                     .....
                     (VisitedPlace ?pN)
                     (VisitedPose ?c2))))

With these modifications, potentially many places become redundant. For any
place that appears in a plan incidentally (i.e., not part of the goal), there
now exists an equivalent plan where these intermediate places do not appear.

For example, imagine our robot begins in Place A, and is tasked with (among
other things) inspecting an object in place C. A motion sequence corresponding
to a plan to move from Place A to Place B, then from Place B to Place C can be
equivalent to a sequence generated from a plan to move from Place A to C
directly, so it seems that we can remove Place B from the problem.
Extrapolating this pruning approach to larger scenes and more complex goals
has the potential to vastly reduce planning horizons. However, now consider
that the robot was also instructed to avoid Place B. The ability to include a
constraint on the goal states of the form (not (VisitedPlace B)) complicates
the pruning process. Executing moveRelaxed from Place A to Place C may involve
following a trajectory that takes the robot through Place B, even if the goal
specifies that Place B should not be visited, whether or not Place B is in
problem domain. Technically this is still a valid solution to the planning
problem since place B never appears as a parameter to the moveRelaxed action
(and therefore (VisitedPlace B) is not an effect), but clearly the domain with
a relaxed movement action does not fully capture how the user wants the robot
to interact with the environment.

###  III-B Execution-Consistency

To formalize the discrepancy between what happens when the robot executes a
motion sequence and the constraints that we expect a planning problem to
impose, we introduce the concept of a _verifier function_. A _verifier
function_ maps motion sub-sequences to sets of PDDL domain facts, and
“verifies” which additional domain facts must be implicitly true as a result
of the agent executing a motion sequence, even if actually adding these facts
to the problem instance during the solving process is undesirable
computationally. For example, if we want to consider if an agent’s trajectory
avoids certain places, we can imagine a verifier that would return
(VisitedPlace $p_{i}$) for each place $p_{i}$ that the continuous trajectory
overlaps with. Given a verifier $V$, the facts that hold at each step when
executing a motion sequence may be different than expected in the original
plan. We denote the facts that would be added by such a verifier applied to
the motion sequence associated with $a_{i}$ as $V(a_{i})$, and term this
sequence of expanded states the $V$-extended state plan.

######  Definition 2 (V-Extended State Plan).

For an action plan $\pi=[a_{1},...,a_{n}]$, its corresponding state plan
$\mathcal{I}_{\pi}=[\mathcal{I}_{0},...,\mathcal{I}_{n}]$, and hypothetical
verifier function $V$, the V-extended state plan for state $\mathcal{I}_{k}$
is:

$\mathcal{I}^{\prime}_{1}=\mathcal{I}_{1}\cup V(a_{1})$
$\mathcal{I}^{\prime}_{k}=\mathcal{I}_{k}\cup\left(\mathcal{I}^{\prime}_{k-1}\setminus\text{Eff}^{-}(a_{k})\right)\cup
V(a_{k})$

An extended state $\mathcal{I}^{\prime}_{k}$ is composed of the facts
$\mathcal{I}_{k}$ in the initial plan, plus any extra facts that were present
in the previous extended state $\mathcal{I}^{\prime}_{k-1}$ other than those
removed by action $a_{k}$, plus any facts that would be returned by a verifier
applied to action $a_{k}$. Informally, $\mathcal{I}^{\prime}_{k}$ is the state
at step $k$ as experienced by the verifier. As discussed in Sec. II-A, any
state plan found by a search algorithm is valid by construction. However, a
state plan that is augmented with the extra facts that would be produced by a
verifier might not be, for example, if the goal specification has a condition
that the robot should avoid certain places in the domain.

To address this problem, let us imagine a verifier $V_{place}$ that takes a
motion sub-sequence $\mu$, and returns a VisitedPlace fact for each place that
intersects with the agent’s position while executing $\mu$. For a place $p$
and a trajectory $t$ to be followed by the motion primitive
$\texttt{FollowPath}(t)$, we denote $p\cap t$ the section of $t$ that
intersects with $p$. We can then define the verifier as

$\displaystyle\begin{split}V_{place}(\mu)=\\{&\texttt{(VisitedPlace
p)}~{}|~{}\\\ &p\cap t_{i}\neq\emptyset\text{ for
}\texttt{FollowPath}(t_{i})\in\mu\\}.\end{split}$ (1)

If the motion sequence associated with the action plan would result in the
agent visiting a place that we do not expect, then the $V_{place}$-extended
state plan would include a VisitedPlace fact that may conflict with the goal.
From this idea, we define the concept of execution-consistency, which requires
that solutions to the planning problem are still valid after considering the
facts from a verifier.

######  Definition 3 (Execution-consistent).

A domain is execution-consistent with respect to verifier $V$ if, for every
valid plan $\pi$, the V-extended state plan is valid.

A domain is trivially execution-consistent for the empty verifier
$V(\cdot)=\emptyset$, as the extended state plan is equal to the original
plan. A domain is also execution-consistent if the range of $V$ applied to
each motion subsequence $\mu_{i}$ corresponding to action $a_{i}$ is limited
to facts in $\text{Eff}(a_{i})$. In other cases, a domain can still be
execution-consistent for a verifier that would introduce new facts if the
domain is carefully crafted. In defining a planning domain for any task, we
seek to have it execution-consistent with respect to any defined verifiers.

In our example, we want to prevent the agent from entering places that it
should not, and so would like to show that the Inspection domain is execution-
consistent with respect to the verifier $V_{place}$. Recall that $V_{place}$
can only introduce new VisitedPlace facts. As VisitedPlace does not appear in
any action preconditions, the only way for a VisitedPlace fact to render a
valid state plan invalid is to conflict with the goal specification. Consider
the set of places that must be avoided to satisfy some goal state:
$\mathcal{P}_{avoid}=\\{p~{}|~{}\texttt{(not (VisitedPlace
p))}\in\mathcal{G}\\}$. If a place in $\mathcal{P}_{avoid}$ can only be
visited by an action that explicitly lists it in the action effects, then the
domain will be execution-consistent with respect to $V_{place}$. This can
easily be guaranteed by preventing the motion planner from generating plans
that enter places $\mathcal{P}_{avoid}\setminus\mathcal{P}_{param}$, where
$\mathcal{P}_{param}$ is the set of places that appear as parameters to the
action. In other words, we define a motion planner that cannot enter any Place
which we might want to avoid, unless that Place is given as the parameter to
the moveRelaxed action. As a result, we can be sure to never visit a
potentially illegal place without (VisitedPlace p) being an effect of the
action.

This notion of avoiding potentially illegal places implies something important
with respect to the motion planning problem. If we identify a place to be
redundant, and remove it from the planning domain, the definition of
redundancy requires that any plan that would have passed through this place if
we hadn’t removed it can be replicated. As a result, while the discrete
PDDLStream planner does not need to be aware of pruned places, the motion
planner we implement to solve for trajectories must still be able to plan
paths through these places. Our motion planner must be aware of the full scene
graph, regardless of what is pruned from the PDDLStream problem instance. We
address this further in Sec. III-C. With that in mind, we can now consider
which specific places we can remove from the PDDLStream problem instance.

######  Proposition 2 (Redundant Places).

Consider a problem instance in the Inspection Domain with no quantifiers that
can be parameterized by a place in the goal. A place $p$ is redundant if no
facts parameterized by $p$ appear in the initial or goal states, or if (not
(VisitedPlace $p$)) appears as a clause in the conjunctive normal form (CNF)
of the goal specification.

###### Proof.

First note that no actions in this domain have universal quantifiers, so we
only need to check Definition 1.1 to show that a symbol is redundant. Consider
a place $p$ such that (not (VisitedPlace $p$)) appears in the CNF of the goal.
If $p$ parameterizes moveRelaxed, then (VisitedPlace $p$) is in the effects,
violating the goal. Since moveRelaxed is the only action that can be
parameterized by a place, no plan can parameterize $p$ and Definition 1.1 is
trivially satisfied.

Next, consider a place $p$ that does not parameterize any initial or goal
facts. For any plan $\pi$ with an action parameterized by $p$, let
$a_{k}^{\mathcal{P}}$ denote an action parameterized by places $\mathcal{P}$,
including $p$. Plan $\pi^{\prime}$ where $a_{k}^{\mathcal{P}}$ is replaced by
$a_{k}^{\mathcal{P}\setminus p}$ is also valid, since state plans
$\mathcal{I}_{\pi}$ and $\mathcal{I}_{\pi^{\prime}}$ only differ by a
(VisitedPlace $p$) fact, and no action preconditions or goals involve this
fact. Since $p$ was not in $\mathcal{P}_{avoid}$, the command sub-sequence
corresponding to $a_{k}^{\mathcal{P}}$ is also valid for
$a_{k}^{\mathcal{P}\setminus p}$. Thus, for any $\pi$ parameterized by $p$, we
can construct $\pi^{\prime}$ that has an equivalent motion sequence but does
not parameterize $p$, showing $p$ is redundant. ∎

We have now identified a potentially large (depending on the sparsity and
geometry of the given scene graph) set of Place symbols that are redundant to
our instantiation of the Inspection domain. Removing these places from our
task planner’s domain, assuming the motion planner is still aware of them,
should enable our solver to find valid plans that are guaranteed to have also
been valid in the un-pruned problem. Moreover, the ability to prune these
elements does not restrict the type of goals we are able to specify to our
agent, preserving expressivity, while enabling planning at a larger scale. Our
explicit method of defining our problem’s initial state is as follows:

######  Remark 1 (Problem Initialization).

In light of Proposition 2, we only include the following places when
instantiating a problem: 1) the initial place that the robot is in and 2) any
place that appears in the goal. A place $p$ that parameterizes a negated fact
$(\texttt{not }(\texttt{VisitedPlace }p))$ that appears as a clause in the CNF
of the goal specification can also be removed. Note that if $(\texttt{not
}(\texttt{VisitedPlace }p))$ appears in the goal, but not as a standalone
clause in the CNF, we do include that place. Any problem in the Inspection
domain with this instantiation is guaranteed to return only plans that are
execution-consistent with respect to the $V_{place}$ verifier discussed
earlier, and as such, no plans need to be verified at runtime.

In implementing the resulting domain, there is a tradeoff between branching
factor and required plan depth. Letting each move action be parameterized by
$N$ places results in a search problem with large branching factor and
expensive task planner preprocessing when running the search. In exchange, a
single action can move through several places that appear in
$\mathcal{P}_{avoid}$. Alternatively, we can restrict each move action to
include only two places – those at the beginning and end points of the action
– and preserve the set of solution trajectories. This comes at the expense of
more numerous sub-problems and longer action plans, though we find this is a
worthwhile trade off in the domains we consider. We can mitigate this
computational cost by using an intermediate abstract planner to accelerate
motion planning, which we describe below.

###  III-C Motion Planning Through Places

Critical to the success of our approach is the implementation of a motion
planner that is capable of planning over large distances, while avoiding
specific regions as specified by the task planner. Solving for a valid plan
for a TAMP problem in a large environment requires solving for trajectories
between potentially many different possible start and end poses, so the
efficiency of the motion planner has a large effect on the overall runtime of
the planner. The connectivity information between places in the scene graph is
a useful tool for focusing the search space for feasible trajectories over
large distances, since each place represents a region of navigable space.
However, the connectivity information in the scene graph does not account for
robot kinematic constraints or necessary standoff distances from obstacles.
Thus, though we leverage information from the scene graph when possible, we
still need to run a kinematic motion planner.

To find a motion plan between two configurations $c_{1}$ and $c_{2}$, we plan
through the places layer of the complete, unpruned scene-graph, using A* to
find a sequence of places $[p_{1},...p_{N}]$ with corresponding centers
$[x_{1},...x_{N}]$ such that $c_{1}$ is in place $p_{1}$, $c_{2}$ is in
$p_{N}$, and edge $(p_{i},p_{i+i})$ is present in the scene graph. Though we
have pruned the PDDL planning domain to accelerate the discrete component of
search, we still retain the full information in the scene graph in order to
generate trajectories. This is a much simpler search problem than using the
PDDL domain of the full scene graph. Second, a kinematic planner plans a path
through these positions. This path ignores obstacles that may be along the
path and does not necessarily stay within places on the reference path, so it
is very fast to generate. For a robot with a kinematic bicycle model, this
initial path might be a series of Dubins curves calculated between sequential
place centers. Next, this path is checked for collisions against the geometry
of all objects in the scene. Any segments of the trajectory that are rendered
infeasible by collision with objects or overlap non-traversable places are re-
solved by a planner that considers the obstacles, such as RRT [14]. This two-
phase approach allows for fast solving of “easy” portions of the trajectory,
while being able to spend time searching for feasible solutions to more
difficult parts of the path. Better alignment between which edges are present
in the scene graph and kinematic feasibility for the robot leads to better
performance of this heuristic.

This coupling of the motion planner with the higher-level discrete planner
using the places layer of the scene graph, gives us in actuality a three level
hierarchical planner: 1) an abstract discrete task planner for interacting
with the environment using the PDDL domain based on the pruned scene graph
described in Sec. III-A, 2) a navigation planner for moving from place to
place in the places layer of the full scene graph, and 3) a low-level motion
planner using the abstract plan and the geometry of the scene graph to produce
kinematically feasible trajectories. This planner architecture aligns
naturally with the scene graph hierarchy, and we notice that with this
factorization, each planning problem we care about becomes considerably
simpler. The top-level planner only needs the subset of relevant the places
layer, so we get a speedup by eliminating the parts of the scene graph that
are redundant and reducing the planning horizon. The abstract navigation
planner only plans through the places layer to one sub-goal at a time, and so
can take advantage of Euclidean distance heuristics. Finally, the fine-grained
motion planner is guided by the solution of the plan through the places layer,
and so does not suffer from the usual challenges of finding motion plans over
large distances. Crucially, we are able to do this without restricting the
types of goals we can specify to the planner.

###  III-D Weakly Redundant Symbols and Object Pruning

We have demonstrated the ability to identify elements of a planning instance
that are provably redundant when searching for a plan. However, there may be
cases where planning could be made vastly more efficient by pruning elements
of a problem that do not meet the strong condition of Definition 1. Consider,
for example, the case where a robot is tasked with travelling across a large
scene graph in the Inspection domain, containing many suspicious objects. The
robot can inspect and neutralize an obstacle to pass it safely, but depending
on the geometry of the scene, there may exist a path to the goal that is
completely free of any obstacles. Alternatively some subset of the objects may
be needed to be cleared to find safe passage. Regardless of that reality, none
of the objects in the scene meet our definition of redundancy due to the fact
that motion sequences which involve inspecting an object cannot be generated
once the object is removed by the domain. The conditions for a symbol being
redundant are quite strong – every trajectory must be preserved when removing
the symbol. We now introduce a weaker notion of redundancy.

######  Definition 4 (Weakly Redundant Symbols).

A symbol x is weakly redundant if both of the following hold:

  1. 1.

For every plan $\pi$ where x parameterizes an action, there is another plan
$\pi^{\prime}$ that is valid where x is not an action parameter.

  2. 2.

No action preconditions or goals, expressed in negative normal form, contains
a universal quantifier that can be parameterized by x.

In contrast to strongly redundant symbols, removing a weakly redundant symbol
from a planning instance may limit the set of motion sequences that can be
found. An object is weakly redundant if a valid plan can be found without that
object being necessary to the plan (e.g., if the robot can find a path around
the object in question). Because the applicability of this definition may
depend on the geometry of the scene, it is more difficult to identify symbols
with weak redundancy, though it potentially applies to a wider set of objects.
Consider two sets of symbols,
$\mathcal{O}_{1}\subseteq\mathcal{O}_{2}\subseteq\mathcal{O}$. If there is a
valid plan $p$ that is only parameterized by objects in $O_{1}$, then the
objects in $O_{2}\setminus O_{1}$ are weakly redundant and do not need to
appear in problem instance in order to find a valid plan.

Unfortunately, identifying weakly redundant objects in the Inspection domain
can be as hard as solving the original planning problem. However, we can still
take advantage of the sparsity that exists in many scene-graphs by considering
an incremental approach for identifying a subset of objects without (some)
weakly redundant objects. We begin planning by including some subset of all
objects, and attempt to solve the planning problem. If this limited problem
has a valid solution which is also a valid solution to the original problem,
then we have found a plan (and have shown the excluded objects were in fact
weakly redundant). Otherwise, we incrementally add objects to the planning
problem, and repeat (Algorithm 1).

Identifying the initial set of objects $\mathcal{O}_{S}$ is not trivial. These
objects are those that we are certain are neither redundant, nor weakly
redundant. We have proven in the Inspection domain to be able to identify
Places from the scene graph which are not redundant as described in Remark 1,
so those are added to the problem. Any Object in the scene graph is
potentially weakly redundant, depending on scene-geometry, unless it is
explicitly mentioned in the goal, so we only add those goal Objects to the
initial problem instance. For general problems, there is a body of literature
dedicated to identifying object relevance, either by a reachability analysis
[4, 5], or otherwise learning to predict importance [19] which could also be
used to augment this initial set.

We then initialize a problem instance with these objects
$\mathcal{O}_{I}=\mathcal{O}_{S}$. Let $\mathcal{O}_{R}$ denote the rest of
the objects in the scene graph. Next, in accordance with the _adaptive_
algorithm in Garrett et al. [6], we optimistically assume that certain sub-
problems are solvable, and identify candidate action-plans which, if this
assumption were true, would be a valid plans. We refer to these candidates as
a plan skeletons. The set of plan skeletons for the problem instance involving
objects $\mathcal{O}_{I}$ are generated by GetSkeletons (Line 7).
SolveSubProblems (Line 8) attempts to solve the relevant sub-problems in a
given candidate plan skeleton. If a sub-problem fails to be solved, we query
the solver for feedback information (which objects in the full set
$\mathcal{O}_{R}$ are a potential cause of failure). If all sub-problems were
solved successfully, we return the full valid plan. Otherwise, we choose to
add one or more new objects to the problem with GetNewObjects, which considers
the feedback about the failure mode that CheckSolution (Line 9) returned. This
approach will continue to incrementally add objects until a plan is found, and
so is complete111Assuming SolveSubProblems is complete and GetSkeletons will
return arbitrarily long skeletons once all objects have been added..

Algorithm 1 Incremental Object Solver

1:procedure IncrementalObjectSolver($A,S,I,G$)

2: $\mathcal{O}_{S}\leftarrow GetRelevantObjects(\mathcal{O})$
$\triangleright$ Find objects relevant for non-collision reasons

3: $\mathcal{O}_{I}\leftarrow\mathcal{O}_{S}$

4: $\mathcal{O}_{R}\leftarrow\mathcal{O}\setminus\mathcal{O}_{S}$

5: while $|\mathcal{O}_{I}|<|\mathcal{O}|$ do

6: $SkeletonInfo=[~{}]$

7: for $k\in GetSkeletons(A,S,\mathcal{O}_{I},G)$ do

8: $T\leftarrow SolveSubProblems(k)$

9: $Feedback\leftarrow CheckSolution(T,\mathcal{O}_{R})$

10: $SkeletonInfo.\texttt{append}(Feedback)$

11: end for

12: if $\pi\in SkeletonInfo$ is valid then

13: return $\pi$

14: else

15: $\mathcal{O}_{new}=GetNewObjects(C,F)$

16: $\mathcal{O}_{I}=\mathcal{O}_{I}\cup\mathcal{O}_{new}$

17: $\mathcal{O}_{R}=\mathcal{O}_{R}\setminus\mathcal{O}_{new}$

18:

19: end if

20: end while

21: return INFEASIBLE

22:end procedure

Existing literature related to identifying irrelevant objects in a planning
problem often focuses on ignoring or more efficiently encoding symbols based
on careful analysis of the PDDL problem’s _logical_ factorization [10, 5]. In
contrast, our algorithm is well suited to address the geometric dependence of
movement actions on objects that may render a trajectory infeasible. A
movement action must avoid collision with _all_ objects in the environment,
and it is difficult to show that an object cannot interfere with any plan and
can be ignored without explicitly inspecting the geometry. Algorithm 1 can be
seen as a way of lazily uncovering which of these objects do interfere with
plans that would otherwise be valid.

We take advantage of the hierarchy in the scene graph to implement
GetNewObjects, which is tasked with identifying which new objects to add to
the planning problem based off feedback from failed sub-problems. Each time we
attempt to solve a motion plan, we produce a plan that avoids the objects that
have been added to our planning domain (if one exists). Then, we check that
plan against the full set of objects. If one of those were intersected, and we
cannot find a plan that does not intersect any of the full set of objects, the
first object is potentially relevant to the planning problem, and we add it to
the planning domain. By adding this object to the planning problem, the
planner can now consider inspecting and neutralizing it, thus introducing a
new potential path to a goal state. We envision many potential extensions to
this approach, such as learning parameters for these subroutines, which we do
not explore in this work. In the next section we evaluate our planning
approach in the Inspection domain.

##  IV Evaluation

Figure 2: Three of the maps used for evaluation (not including the KITTI
environment). Top Left: narrow alley map. Top Right: A simple 10x10 grid world
map. Bottom: Scene graph built from real data collected by a robot in an
office environment.

There are three primary axes of complexity for planning problems in large
scene graphs that we are interested in investigating. One is the complexity of
the goal specification, another is correlated to the scale of the environment,
and the third is related to the geometry of the scene (and potential
obstructions therein). In our experiments, we aim to identify which types of
planning problems our planning approach is well suited for considering these
sources of complexity. To that end, we compare our encoding of the Inspection
domain to the dense, direct encoding in a variety of different settings. We
test on four map archetypes (Fig. 2)– a synthetic small constrained alleyway,
a synthetic 10x10 gridworld, a scene graph built from real data in an office
environment comprising 557 Places and 28 Objects, and a much larger scene
graph built from the KITTI dataset composed of 17861 Places and 1315 Objects
(Figs. 1 and 2). For each environment, we test several different goal clauses
across different variations in robot and object initial conditions.

To randomize tasks across trials, we define a mechanism for sampling goal
specifications according to an increasing number of clauses in Disjunctive
Normal Form (DNF). More explicitly, we generate goals in DNF with N clauses,
where each clause has K conjunctions. For example, if N = 2 and K = 3, the
goal looks like this: (Or ($C_{1}$, $C_{2}$)), where we sample 3 clauses for
$C_{i}$, e.g., (And ((Visited P1), (Safe O4), (Not (Visited P9)))).

###  IV-A Scene Graph Size

Figure 3: This log-log plot compares the time (in seconds) to solve tasks of
comparable complexity across different environmental scales for the dense
formulation and the proposed sparse formulation. In the smallest environment,
the dense baseline planner performs just as well as our approach. However, as
we increase the scale, first to a 10x10 grid, and then to a real scene graph
constructed on the floor of an academic building, we see the benifits of our
approach.

First, we investigate the effect of scene graph size on our ability to plan to
a set of goals of consistent complexity. For this set of trials, our goal
complexity is (N, K) = (3, 3), and we compare planning time for the direct
encoding to our planner, highlighted in Fig. 3. Each point on the scatter plot
corresponds to a single trial and, as highlighted in the legend, different
colors correspond to different environment types. We begin in the Alley
environment which, as shown in Fig. 2, is a 2x6 grid world containing only
connected places. In this environment, for this level of complexity, our
planner performs about as well as the dense encoding, as there is not much
advantage to sparsification in such a small environment. As we scale up
however, this changes. The next environment we consider is a 10x10 grid, where
we see modest improvement as shown in Fig. 3. In the figure, any samples above
the black line indicate that our planner outperforms the dense baseline. As we
scale up even further, into the indoor DSG, we see the baseline planner taking
on the order of hundreds of seconds to plan on average, while our planner
averages in the tens of seconds. We note that we only considered goals that
involved visiting or not visiting certain Places in the graph. When we
attempted to introduce object inspection, the dense planner timed out before
finding a plan in all instances. Similarly, when testing the baseline in the
KITTI scene graphs, it was also unable to find solutions for goals of any
complexity. Planning with these relatively simple goals, our planner
experienced only a modest increase in planning time as the size of the map
scaled.

###  IV-B Goal Complexity

Figure 4: Here we highlight how our approach scales with the complexity of the
goal specification in the simple 10x10 grid world. As we increase the number
of unique PDDL objects in the goal specification, the problem is no longer
sparse, and so no longer benefits from our approach. Eventually, the dense,
direct encoding begins to outperform our method.

Next, we consider the effect of increasing goal complexity on planning time.
To do this, we investigate a series of various different goal constructions in
the Grid World environment. Specifically, we run experiments for K = 5 and K =
10, incrementing the N value from 1 to 5 for each K value. Stated differently,
we have conjunctive clauses that are either 5 or 10 atoms long, and “Or”
together 1, 2, 3, 4, or 5 of these conjunctions. Each of these clauses is once
again either (Visited P1) or (Not (Visited P1)). Figure 4 presents a plot
comparing the complexity of the goal in terms of total unique symbol
referenced vs planning time. For less complex goals in this environment, our
planner outperforms the dense planner, up to a crossover point at around 20
unique objects. At this point, the benefits of sparsity disappear for a scene
of this size. Notably however, if we were to introduce obstructing objects,
the dense direct encoding baseline is unable to solve goals of any meaningful
complexity. Once again, not included in this plot are the results on the KITTI
dataset, as the direct encoding never successfully completes a trial in this
setting due to timing out.

###  IV-C Object Obstruction in KITTI

Figure 5: An example plan from the KITTI environment. The robot begins in the
bottom left corner and is tasked with inspecting one object (denoted by the
red triangle at the end of the black trajectory). Along the way, there are
numerous objects potentially blocking the agent’s path, so it must add at
least one to its planning domain. After inspect and neutralizing this object,
the robot is free to reach its goal.

Finally, we investigate the performance of the incremental object solver
algorithm in task instances where objects not directly listed in the goal must
be inspected in order to solve the task. In this experiment, we give a robot
one of two goal types in a scene graph built from the KITTI dataset (Figs. 1
and 5): either (Visited $P_{i}$) or (Safe $O_{j}$). Given the size of the map,
satisfying these goals may require the agent to traverse a large distance, but
more importantly, if there are obstructing objects in the way, it may be
forced to inspect and neutralize them to find a safe path to the goal. As a
baseline, we sample 20 goals in the map shown in Fig. 5 using our planner
without any of the objects being labeled as suspicious. In this case, they do
not obstruct the agent’s path, and we find plans in 19 of 20 trials.

Next, we “activate” 13 objects in the scene by labeling them as suspicious. A
suspicious object has an inflated radius that is only safe for the robot to
enter after it has been inspected and neutralized (in the KITTI scene, this
radius is large enough to block an entire road as shown by the blue objects in
Fig 5). For the agent to inspect the object, it has to sample a pose that is
traversable and within line-of-sight and range of the object. Then, by taking
the inspect action, the object is now safe, and can be passed. To demonstrate
the difficulty of this task, and to highlight the importance of object
pruning, we attempt to solve these same tasks without using our incremental
feedback approach for pruning potentially weakly redundant objects (Sec.
III-D). Instead, we add all suspicious objects to the scene directly. Using
this encoding, the planner only succeeds in finding a plan in 4 out of 20
trials. Inspecting these solutions further reveals that in all 4 of these
successful cases, there was a direct path to the goal without inspecting any
objects. This result makes sense, as the odds of sampling the correct object
to inspect is low without the benefit of geometric information.

Finally, we test our proposed approach with pruning objects from the planning
domain, and incrementally adding them back if they impacted the failure of a
sub-problem during search (Sec. III-D). Our planner is able to solve 12 of the
20 trials, including 9 cases wherein the agent inspected one or more
obstructing objects on the way to its goal. These experiments further
demonstrate the importance of our proposed approach to sparsifying otherwise
dense, long-horizon planning problems. An example plan, where the agent
investigates two objects on the way to its goal is shown in Fig. 5.

##  V Related Work

There has substantial recent work enabling construction of information-rich
scene graphs. Wu et al. [23] use a graph neural network to build higher-level
abstractions from sensor data. Hughes et al. [12] and Bavle et al. [3] build
scene graphs while localizing a robot’s motion, allowing for co-optimization
of pose and scene estimation. Other recent directions include the registration
of scene graph pairs [17].

There has been recent work in the area of deriving PDDL representations for
task planning from scene graphs [1], with a particular focus on using the
hierarchical nature of scene graphs to sparsify the representation in order to
make planning tractable. The approach used by Agia et al. [1] is only
guaranteed to produce valid solutions for very specific planning domains,
where only constraints between symbols with a clear “ancestor” relationships
are expressible, and it is unclear how to extend this to a more general set of
planning tasks. Unfortunately the scene graph abstraction may not satisfy the
property of downward-refinement [2], breaking many of the assumptions in task
planning. The existance of low-level geometric constraints requires going
beyond task planning approaches, and into the realm of TAMP.

There has been additional work in pruning superfluous elements of scenes to
accelrate TAMP. Silver et al. [19] learn to predict which symbols are relevant
to a particular TAMP problem. Similarly, Srivastava et al. [20] attempt to
guide TAMP from failed motion plans, much like we do in Sec III-D, however do
so by adding additional goal conditions, which can complicate planning.

##  VI CONCLUSIONS

In this work we proposed an approach for enabling and accelerating TAMP in
large scene graphs. To do this, we defined characteristics of planning domains
that permit the pruning of certain symbols. Then, we proposed a method for
deriving a domain from a Hydra scene graph which has these characteristics,
and demonstrate how we prune Places and Objects from the domain. We also prove
how the plans we produce from this pruned scene graph are valid and conform to
the constraints of the full planning domain. Finally, we demonstrate
experimentally how our approach scales with scene graph size, goal complexity,
and geometric constraints in several environments, including a scene graph
built from the KITTI dataset.

In future work, we hope to demonstrate under what conditions we can extend our
pruning method to other domains derived from large-scale scene graphs.
Furthermore, augmenting our approach with learned methods for object pruning
is a natural extension. The metric-semantic information in the scene graph is
potentially a strong signal for a learner to identify further irrelevant
symbols in a planning domain.

## Acknowledgements

This work was partially funded by the ARL DCIST program, by Lincoln
Laboratory’s Autonomy Al Fresco program, and by the National Defense Science
and Engineering Graduate Fellowship program.

## References

  * Agia et al. [2022] Christopher Agia, Krishna Murthy Jatavallabhula, Mohamed Khodeir, Ondrej Miksik, Vibhav Vineet, Mustafa Mukadam, Liam Paull, and Florian Shkurti.  Taskography: Evaluating robot task planning over large 3D scene graphs.  pages 46–58. PMLR, January 2022.
  * Bacchus and Yang [1991] Fahiem Bacchus and Qiang Yang.  The downward refinement property.  In _IJCAI_ , pages 286–293, 1991.
  * Bavle et al. [2022] Hriday Bavle, Jose Luis Sanchez-Lopez, Muhammad Shaheer, Javier Civera, and Holger Voos.  S-graphs+: Real-time localization and mapping leveraging hierarchical representations.  _arXiv preprint arXiv:2212.11770_ , 2022.
  * Blum and Furst [1997] Avrim L Blum and Merrick L Furst.  Fast planning through planning graph analysis.  _Artificial intelligence_ , 90(1-2):281–300, 1997\.
  * Fishman et al. [2023] Michael Fishman, Nishanth Kumar, Cameron Allen, Natasha Danas, Michael Littman, Stefanie Tellex, and George Konidaris.  Task scoping: Generating task-specific simplifications of open-scope planning problems.  In _PRL Workshop Series $\\{$$\backslash$textendash$\\}$ Bridging the Gap Between AI Planning and Reinforcement Learning_, 2023.
  * Garrett et al. [2020] Caelan Reed Garrett, Tomás Lozano-Pérez, and Leslie Pack Kaelbling.  Pddlstream: Integrating symbolic planners and blackbox samplers via optimistic adaptive planning.  In _Proceedings of the International Conference on Automated Planning and Scheduling_ , 2020.
  * Garrett et al. [2021] Caelan Reed Garrett, Rohan Chitnis, Rachel Holladay, Beomjoon Kim, Tom Silver, Leslie Pack Kaelbling, and Tomás Lozano-Pérez.  Integrated task and motion planning.  _Annual review of control, robotics, and autonomous systems_ , 2021\.
  * Ghallab et al. [2016] Malik Ghallab, Dana Nau, and Paolo Traverso.  _Automated planning and acting_.  Cambridge University Press, 2016.
  * Helmert [2006a] Malte Helmert.  The fast downward planning system.  _Journal of Artificial Intelligence Research_ , 26:191–246, 2006a.  URL https://www.jair.org/index.php/jair/article/view/10457.
  * Helmert [2006b] Malte Helmert.  The fast downward planning system.  _Journal of Artificial Intelligence Research_ , 26:191–246, 2006b.
  * Hoffmann [2001] Jörg Hoffmann.  Ff: The fast-forward planning system.  _AI magazine_ , 22(3):57–57, 2001.  URL https://ojs.aaai.org/aimagazine/index.php/aimagazine/article/view/1572.
  * Hughes et al. [2024] Nathan Hughes, Yun Chang, Siyi Hu, Rajat Talak, Rumaisa Abdulhai, Jared Strader, and Luca Carlone.  Foundations of spatial perception for robotics: Hierarchical representations and real-time systems.  _Intl. J. of Robotics Research_ , 2024.  URL https://journals.sagepub.com/doi/10.1177/02783649241229725.
  * Karpas and Magazzeni [2020] Erez Karpas and Daniele Magazzeni.  Automated planning for robotics.  _Annual Review of Control, Robotics, and Autonomous Systems_ , 2020\.
  * LaValle [1998] Steven LaValle.  Rapidly-exploring random trees: A new tool for path planning.  _Research Report 9811_ , 1998.
  * LaValle [2006] Steven M LaValle.  _Planning algorithms_.  Cambridge university press, 2006.
  * Oleynikova et al. [2018] Helen Oleynikova, Zachary Taylor, Roland Siegwart, and Juan Nieto.  Sparse 3D topological graphs for micro-aerial vehicle planning.  In _IEEE/RSJ Intl. Conf. on Intelligent Robots and Systems (IROS)_ , 2018.
  * Sarkar et al. [2023] Sayan Deb Sarkar, Ondrej Miksik, Marc Pollefeys, Daniel Barath, and Iro Armeni.  Sgaligner: 3d scene alignment with scene graphs.  _arXiv preprint arXiv:2304.14880_ , 2023.
  * [18] Tom Silver, Rohan Chitnis, Aidan Curtis, Joshua Tenenbaum, Tomas Lozano-Perez, and Leslie Pack Kaelbling.  Planning with learned object importance in large problem instances using graph neural networks.
  * Silver et al. [2021] Tom Silver, Rohan Chitnis, Aidan Curtis, Joshua B Tenenbaum, Tomas Lozano-Perez, and Leslie Pack Kaelbling.  Planning with learned object importance in large problem instances using graph neural networks.  In _Proceedings of the AAAI conference on artificial intelligence_ , 2021.
  * Srivastava et al. [2014] Siddharth Srivastava, Eugene Fang, Lorenzo Riano, Rohan Chitnis, Stuart Russell, and Pieter Abbeel.  Combined task and motion planning through an extensible planner-independent interface layer.  In _2014 IEEE international conference on robotics and automation (ICRA)_ , pages 639–646. IEEE, 2014.  URL https://aair-lab.github.io/Publications/icra14.pdf.
  * Strader et al. [2023] Jared Strader, Nathan Hughes, William Chen, Alberto Speranzon, and Luca Carlone.  Indoor and outdoor 3D scene graph generation via language-enabled spatial ontologies.  _arXiv preprint arXiv:2312.11713_ , 2023.  URL https://arxiv.org/pdf/2312.11713.pdf.
  * Vega-Brown and Roy [2020] William Vega-Brown and Nicholas Roy.  Task and motion planning is pspace-complete.  In _Proceedings of the AAAI Conference on Artificial Intelligence_ , volume 34, pages 10385–10392, 2020.
  * Wu et al. [2021] Shun-Cheng Wu, Johanna Wald, Keisuke Tateno, Nassir Navab, and Federico Tombari.  Scenegraphfusion: Incremental 3d scene graph prediction from rgb-d sequences.  In _Proceedings of the IEEE/CVF Conference on Computer Vision and Pattern Recognition_ , pages 7515–7525, 2021.