# Hypergraph-based Multi-Robot Task and Motion Planning

James Motes, Tan Chen, Timothy Bretl, Marco Morales, Nancy M. Amato Manuscript
created October, 2022.
James Motes (corresponding author), Marco Morales, and Nancy M. Amato are with
Parasol Lab, Department of Computer Science, University of Illinois at Urbana-
Champaign (email: {jmotes2, moralesa, namato}@illinois.edu).
Tan Chen is with the Department of Electrical and Computer Engineering at
Michigan Technological University (email: tanchen@mtu.edu).
Timothy Bretl is with the Department of Aerospace Engineering, University of
Illinois at Urbana-Champaign (email: tbretl@illinois.edu).
This work was supported in part by Foxconn Interconnect Technology (FIT) and
the Center for Networked Intelligent Components and Environments (C-NICE) at
UIUC.

###### Abstract

We present a multi-robot task and motion planning method that, when applied to
the rearrangement of objects by manipulators, produces solution times up to
three orders of magnitude faster than existing methods. We achieve this
improvement by decomposing the planning space into subspaces for independent
manipulators, objects, and manipulators holding objects. We represent this
decomposition with a hypergraph where vertices are substates and hyperarcs are
transitions between substates. Existing methods use graph-based
representations where vertices are full states and edges are transitions
between states. Using the hypergraph reduces the size of the planning
space—for multi-manipulator object rearrangement, the number of hypergraph
vertices scales linearly with the number of either robots or objects, while
the number of hyperarcs scales quadratically with the number of robots and
linearly with the number of objects. In contrast, the number of vertices and
edges in graph-based representations scale exponentially in the number of
robots and objects. Additionally, the hypergraph provides a structure to
reason over varying levels of (de)coupled spaces and transitions between them
enabling a hybrid search of the planning space. We show that similar gains can
be achieved for other multi-robot task and motion planning problems.

###### Index Terms:

Multi-Robot Systems, Task Planning, Motion and Path Planning, Cooperating
Robots.

##  I Introduction

The use of robotic systems as autonomous solutions is rapidly increasing. This
can be seen in warehouse management, manufacturing, household chores, health
care, etc.. In many of these settings, multi-robot systems are leveraged to
complete tasks single robots cannot through inter-robot cooperation and
increase throughput by working in parallel. These improvements in
effectiveness often require complex coordination for both task and motion
planning.

Unfortunately, the planning space for these problems is very large. Even for a
single robot, it is intractable to represent explicitly [1, 2], and it is
exponentially expansive for the composite space of multi-robot systems. Task
and motion planning adds additional dimensions for the tasks present.

For simpler problems, considering decoupled planning spaces results in faster
planning times. However, decoupled approaches cannot handle problems which
require high levels of coordination. Thus, existing multi-robot task and
motion planning approaches primarily plan in the composite space and cannot
efficiently plan for large complex multi-robot systems.

Some problems, such as payload transportation, model tasks abstractly and
often ignore the physical dimensions of the tasks during planning [3, 4, 5].
Other problems, especially those involving object manipulation, must consider
physical constraints [6, 7, 8, 9, 10]. Here the coordination required prevents
the use of decoupled methods, and the planning space grows too large to apply
composite methods.

In this paper, we present the Decomposable State Space Hypergraph (DaSH)
method, a general hybrid search technique based on a hypergraph representation
which captures the coordination and complexity in multi-robot task and motion
planning problems while planning for larger numbers of robots and tasks.
Hybrid search techniques that balance the strengths and tradeoffs of coupled
and decoupled approaches have shown promise in recent multi-robot motion
planning methods [11, 12]. However, before our work, hybrid search techniques
were not used for problems requiring more complex coordination such as multi-
robot task and motion planning.

Figure 1:  We demonstrate our proposed approach for the multi-manipulator
sorting problems with a pair of UR5e manipulators. The robots must sort the
blocks into bins of the corresponding color. This figure captures the moment
in which the robots are performing a handoff. A full video of the sorting demo
can be found at: https://youtu.be/MSyeYXu0Xzs.

Our approach supports varying levels of (de)coupling of the planning space at
different problem stages. The degree of coupling should be tailored to the
problem and corresponding planning space and can change to fit different
problem stages. We support this with hybrid representations for several
planning spaces which capture varying degrees of (de)coupled spaces depending
on the coordination required.

Take, for example, a multi-manipulator problem involving a handoff between two
robots as depicted in Figure 1. Different levels of coordination are required
at different stages of planning. Robots operate mostly independently in their
individual state spaces. The joint space of a single robot and an object is
important while picking/placing the object or while carrying the object, and
the joint space of both robots and an object is important while planning a
handoff between the two robots.

Prior multi-manipulator methods such as [10] struggle as the number of tasks
(or objects) increases relative to the number of robots. Our proposed
representation flips the scaling paradigm where tasks (or objects) are no
longer the limiting factor. As a result, DaSH supports higher numbers of
objects relative to the number of robots which is appropriate for manipulation
tasks where a few robots should handle many objects.

We demonstrate the ability of the DaSH method to generalize to many multi-
robot task and motion planning problems including multi-robot motion planning,
drone formation planning, and multi-manipulator planning. For the latter, we
illustrate how the number of vertices in the hypergraph scales linearly with
both the number of robots and objects, while the number of hyperarcs scales
quadratically with the number of robots and linearly with the number of
objects (Fig. 2) and demonstrate improved planning times on both physical
(Fig. 1) and simulated multi-robot systems (Fig. 22).

(a) Graph: 2 Robots,
4 Objects

(b) Hypergraph: 2 Robots,
4 Objects

Robots | Objects | Representation | Vertices | Transitions
---|---|---|---|---
|  | Hypergraph | 14 | 24
| 4 | Graph | 21 | 120
|  | Hypergraph | 26 | 48
2 | 8 | Graph | 73 | 496
|  | Hypergraph | 24 | 80
| 4 | Graph | 209 | 8672
|  | Hypergraph | 44 | 160
4 | 8 | Graph | 3393 | 213184
(c) Representation Sizes

Figure 2:  The figures and table provide a small sample of multi-manipulator
problem representation sizes using graphs and hypergraphs. (a) Depicts a graph
representation of the task space for 2 robots and 4 objects. There are 21
vertices and 120/60 directed/bi-directional edges. (b) Depicts a hypergraph
representation of the task space for 2 robots and 4 objects. There are 14
vertices and 24/12 directed/bi-directional hyperarcs. (c) The number of
vertices and directed transitions (edges/hyperarcs) are given for increasing
problem sizes of 2-4 robots and 4-8 objects. The reduction in size of the
hypergraph representation is significant when increasing both robots and
objects. The graph representation quickly becomes too large to use
efficiently, while the hypergraph remains relatively small.

In summary, our contribution is:

  * •

A novel hypergraph representation for varying decoupled spaces in multi-robot
task and motion planning problems.

  * •

A general planning algorithm that exploits this hypergraph representation.

  * •

A specific application of this approach to (i) the multi-robot motion planning
space (MRMP-DaSH) and to (ii) the multi-manipulator planning space (MM-DaSH).

  * •

A theoretical analysis of the size of the proposed hypergraph representation
compared to traditional graph representations for multi-manipulator problems.

  * •

An experimental evaluation of the planning algorithm applied to multi-
manipulator problems.

##  II Background and Related Work

In this section, we give an overview of the background and related work for
multi-robot task and motion planning.

###  II-A Motion Planning

The degrees of freedom (dofs) of a robot fully parameterize its position. They
may contain the robot’s pose, orientation, joint angles, etc.. A specification
of dof values for a robot defines a configuration. Motion planning considers a
continuous state space comprised of the set of all robot configurations known
as configuration space ($\mathcal{C}_{space}$) [13].

The motion planning problem is to find a continuous path from a start location
to a goal location through the subset of $\mathcal{C}_{space}$ consisting of
valid configurations called free space. $\mathcal{C}_{space}$ is usually
intractable to represent explicitly [1, 2]. To handle the complexity of
planning in $\mathcal{C}_{space}$, sampling-based motion planners such as the
Probabilistic Roadmap Method (PRM) [14] attempt to create a discretized
approximation of the connectivity of the $\mathcal{C}_{space}$ known as a
roadmap. A roadmap is a graph in which vertices are individual configurations
and edges represent transitions between a pair of configurations. Paths are
found by searching over this roadmap.

###  II-B Multi-robot Motion Planning

Multi-robot motion planning considers the composite $\mathcal{C}_{space}$ of
the system. This composite space is defined as the Cartesian product of the
$\mathcal{C}_{space}$ of each individual robot. This space grows exponentially
with the number of robots.

There are three main approaches to planning within this space. Coupled
planners plan directly within the composite space [15, 16]. They struggle to
plan for large numbers of robots and are often slow, but they are able to
provide probabilistic completeness. Decoupled methods consider individual
robot $\mathcal{C}_{space}$ to find a set of paths [15, 17]. They are
typically faster but usually lack completeness and optimality guarantees.
Hybrid methods such as CBS-MP [11] seek to leverage the strengths of both
composite and decoupled planners. They often iteratively plan within
individual robot $\mathcal{C}_{spaces}$ (or the composite space of a subgroup
of the robots) while reconciling plans against each other. Hybrid methods are
usually able to provide plans in significantly less time with respect to
decoupled methods while offering the completeness and optimality guarantees of
composite methods.

###  II-C Task and Motion Planning

When we consider planning problems with objects that can be moved by robots,
we enter the domain of task and motion planning [18]. If we model the state of
the entire world (robots and objects) as a configuration space $\mathcal{W}$,
we end up with a highly under actuated system. We only have direct control
over the robot dofs, while the dofs corresponding to any object pose can only
be changed indirectly through the robot acting upon them. Additionally, the
actions the robot takes affects the feasibility of motions. For example, a
valid motion for a robot not holding an object may no longer be valid if the
robot is grasping an object, and the current placement of the objects affects
the feasibility of motions.

One approach is to treat each combination of the robot and held object as a
unique robot and each placement of objects as a new environment. This is
sometimes referred to as multi-modal motion planning [19, 20, 21] although
this language is less intuitive than task planning and can be confused for
motion planning with different modes of locomotion.

Instead, we define a task space $\mathcal{T}$ where a task state
$T\in\mathcal{T}$ consists of which object the robot is holding (or none), the
grasp constraints for the held object, and the current pose of all ungrasped
objects. This defines a new configuration space $\mathcal{W}_{T}$ for the
robot (possibly while grasping an object).

One way to represent the set of constraints within a task state $T$ is using a
kinematic tree [22] (Fig. 3). The full world state within a task state
represented by a kinematic tree can be derived from the robot dof values and
forward kinematics. Vertices within a kinematic tree represent individual
geometric bodies and controllable joints of the robot. Each edge has an
associated transformation denoting the pose of the child body/joint and the
parent body/joint. This can be a physical attachment in the case of a robot
joint, or an object pose relative to a stable surface such as a table.

(a) Stable Pose

(b) Grasp

(c) Stable Pose KT

(d) Grasp KT

Figure 3: The kinematic tree in (c) encodes the kinematic constraints of the
task state in (a). The object is resting at a stable pose on the table. The
edge has an associated transformation that gives the exact pose relative to
the table. The edge between the table and world nodes gives the transformation
of the table in the world frame. The transformation to the base of the robot
is given similarly. The controllable joints of the robot are also included as
the nodes $j1$ and $j2$. By performing a pick action, the system undergoes a
task state switch from (a) to (b) which is represented by a change in the
kinematic tree from (c) to (d). The pose of the object is now given through
its transformation from the robot hand rather than the table.

A system can perform a task state switch by applying an action that changes
the kinematic tree. An example of this can be seen in Figure 3 where the left
kinematic tree represents an object resting on the table, and the right
kinematic tree shows the object held by the robot. By performing a pick
action, the system shifts from the left kinematic tree to the right one.

A system can only switch between task states at configurations that lie within
the intersection of the configuration spaces $\mathcal{W}_{T}$,
$\mathcal{W}_{T^{\prime}}$ of the current and subsequent task states $T$,
$T^{\prime}$. These configurations must satisfy the constraints of both task
states. In practice, these are often stable grasps of objects that are sitting
at stable poses on some surface in the environment or handoffs at the
intersection of stable grasp poses for a set of robots [8]. These
configurations satisfy the task state defining the robot grasping the object
and the task state where the object is located at that stable pose. Depending
on the direction of the task state switch, these configurations can represent
the moment of a pick or place action. Finding these transition configurations
is considered the most challenging part of multi-modal planning [18].

When considering classic task and motion planning problems, it is often best
to view them as a hybrid discrete-continuous search problems [18]. A solution
consists of a finite and discrete sequence of task states (eg: which object to
grasp), continuous constraint parameters (poses and grasps of objects), and
continuous motion paths within the configuration space $W_{T}$ of each task
state $T$ to a configuration in the intersection with the subsequent task
state configuration space.

###  II-D Multi-robot Task and Motion Planning

In multi-robot systems, a task state consists of the same types of constraints
(object poses and grasp constraints in manipulation problems) as in single
robot systems. Thus, the kinematic tree representation of single robot systems
can still be used in the multi-robot domain as shown in Figure 4. However, in
this domain, there is a combinatorial expansion in the number of task states
available to the system as both the number of robots and objects grow [23].

The three classes of multi-robot motion planning (coupled, decoupled, and
hybrid) apply here as well. Most task and motion planning problems require too
much coordination for decoupled methods to be useful. Although some of our
prior work has explored hybrid search techniques in the multi-robot task and
motion planning domain, the approach does not extend to manipulation problems
[3]. The rest of this section discusses coupled approaches.

(a) Stable Pose - Start

(b) Grasp 1

(c) Grasp 2

(d) Stable Pose - Goal

Figure 4:  a) The left kinematic graph shows the starting task state of the
system where the object is at rest at a stable pose on the table. b) A pick
action results in a task state switch to the middle left task state where
robot 1 is grasping the object. c) A handoff action creates another task state
switch transferring the object from robot 1 to robot 2. d) Finally, a place
action returns the system to a stable pose task state and kinematic graph, but
the transformation associated with the object$\rightarrow$table edge is
different than in (a) encoding the new stable pose.

Previous work in the area has had two main focuses to handle the space
complexity: developing representations and developing heuristics. The authors
in [24] use shared manipulator workspace to build a shared space graph to
reason over multi-robot cooperation and adapt a path planning heuristic for
multi-manipulator planning. Assumptions about reachability and the number of
robots required to securely grasp an object is used in [8] to build a
condensed graph representation. This representation is integrated in later
work with a multi-robot motion planning method [25] to create a full multi-
robot multi-modal planner [9]. This planner introduces a high-level object
mode graph to represent valid transitions between pick, place, and handoff
configurations. This graph is used as a heuristic to guide the multi-modal
motion planning through sequences of modes that form a valid solution. All
three of these methods [24, 8, 9] only plan for a single object.

In [10], the work in [9] is expanded to account for multiple objects. The
single object mode graph in [9] is replaced with an object centric mode graph
where vertices correspond to manipulators and stable surfaces. Instead of
searching over object (or task state) switches for a single object, a multi-
agent pathfinding (MAPF) technique is proposed to find non-conflicting
individual object mode sequences over the available manipulators and stable
surfaces. The transfer of multiple objects at once complicates the reasoning
over both the sequence of modes and the motion planning. The authors simplify
the problem by considering synchronized actions for the set of manipulators in
the problem. This allows a single step in the MAPF solution over the object
centric mode graph to both indicate a set of actions for the robots to perform
and define a more constrained multi-robot motion planning problem.

Using MAPF over the object centric mode graph as a heuristic, the Synchronized
Multi-Arm Rearrangement (SMART) is able to efficiently plan for up to 9 robots
[10]. The heuristic is formulated as a MAPF problem over the object centric
mode graph. The method is biased to greedily move the object centric mode
state forward along the corresponding MAPF solution. When the MAPF solution is
simple and the paths for individual objects are not likely to use the same
robots at the same time, this greedy heuristic performs exceptionally well.
However, when the paths for objects are likely to conflict, or the number of
objects increases relative to the number of robots (increasing the density of
the MAPF problem), this heuristic becomes limiting. This is reflected in [10]
as no result is shown for more than 4 objects. We directly compare our method
against SMART [10] in later sections.

###  II-E Directed Hypergraphs and Hyperpaths

A hypergraph $\mathcal{H}=(\mathcal{V},\mathcal{E})$ is a generalization of a
graph, where $\mathcal{V}=\\{v_{0},v_{1},...,v_{n}\\}$ is the set of vertices,
and $\mathcal{E}=\\{E_{0},E_{1},...E_{m}\\}$, with $E_{i}\subset\mathcal{V}$
for $i\in[0,m]$, is the set of hyperedges. Unlike edges in a regular graph, a
hyperedge $E\in\mathcal{E}$ is not restricted to pairs of vertices. In this
work, we consider directed hypergraphs as described in [26].

####  II-E1 Directed Hypergraph

A directed hypergraph has directed hyperedges, or hyperarcs, so that a
hyperarc $E_{i}$ has both a head set
$\texttt{Head}(E_{i})\subseteq\mathcal{V}$ and a tail set of vertices
$\texttt{Tail}(E_{i})\subseteq\mathcal{V}$. A visual depiction of directed
hyperarcs is given in Figure 5, and a depiction of a directed hypergraph is
given in Figure 6a. In this paper, all hypergraphs discussed will be directed
hypergraphs.

(a) Directed Hyperarc

(b) Bi-Directional Hyperarc

Figure 5:  a) A directed hyperarc represents a transition from the vertices in
the tail set on the left to the vertices in the head set on the right. b) A
bi-directional hyperarc indicates that the transition can happen in either
direction. It is essentially a pair of directed hyperarcs with opposite tail
and head sets.

(a) Directed Hypergraph

(b) Simple Path

(c) Hyperpath Path

Figure 6:  a) A directed hypergraph consists of vertices and directed
hyperarcs. (b) A simple path in a directed hypergraph from $v_{0}$ to $v_{7}$.
No hyperarc is used more than once. (c) A directed hyperpath from $v_{0}$ to
$v_{7}$. Every vertex incident a hyperarc in the hyperpath is included. All
vertices are connected to the source $v_{0}$ by a cycle-free simple path.

####  II-E2 Directed Hyperpath

We use directed hyperpaths as defined in [26]. A path in a hypergraph is a
sequence of vertices and hyperarcs $v_{0},E_{0},v_{1},E_{1},...,v_{final}$
such that $v_{i}\in\texttt{Tail}(E_{i})$ and $v_{i+1}\in\texttt{Head}(E_{i})$
for all vertices and edges in the path (Fig. 6b). A path is simple if all
hyperarcs are used at most once.

A hyperpath $\Pi_{st}$ is a minimal hypergraph
$\mathcal{H}_{\Pi}=(\mathcal{V}_{\Pi},\mathcal{E}_{\Pi})$ such that:

$\displaystyle\mathcal{E}_{\Pi}\subseteq\mathcal{E}$ (1) $\displaystyle
s,t\in\mathcal{V}_{\Pi}=\bigcup_{E_{i}\in\mathcal{E}_{\Pi}}E_{i}\subseteq\mathcal{V}$
(2) $\displaystyle\begin{split}&x\in\mathcal{V}_{\Pi}\Rightarrow x\text{ is
connected to $s$ in $\Pi_{st}$}\\\ &\text{ through a cycle-free simple
path.}\end{split}$ (3)

The set of hyperarcs $\mathcal{E}_{\Pi}$ in the hyperpath $\Pi_{st}$ must be
in the original hypergraph $\mathcal{H}$ (1). The set of vertices
$\mathcal{V}_{\Pi}$ consists of all vertices incident a hyperarc
$E_{i}\in\mathcal{E}_{\Pi}$, and the source $s$ and target $t$ must be
included in $\mathcal{V}_{\Pi}$ (2). Every vertex must be connected to the
source $s$ in the hyperpath through a cycle-free simple path (3). Thus the
hyperarc from $\\{v_{0},v_{1}\\}$ to $\\{v_{2}\\}$ in Figure 6 cannot be
included in a hyperpath from source $v_{0}$ as $v_{1}$ is not connected to
$v_{0}$ via a cycle-free simple path.

##  III Problem Definition and Method Overview

In this section, we first provide a general definition of the multi-robot task
and motion planning problem and then give a general overview of our proposed
approach.

###  III-A Problem Definition

In this section, we define the multi-robot task and motion planning problem
and the corresponding planning space. We explain this definition in the
context of three problem classes: multi-robot motion planning (MRMP), drone
fleet formation planning, and multi-manipulator planning.

####  III-A1 Planning Space

We consider a planning space consisting of a world configuration space
$\mathcal{W}$ and a task space $\mathcal{T}$ where a configuration
$w\in\mathcal{W}$ represents the configuration of all movable bodies in a
scene (e.g. robots and/or objects), and each task state $T\in\mathcal{T}$
defines a semantic state of the world and/or a set of constraints on feasible
motion in $\mathcal{W}$. We denote the constrained configuration space created
by applying the constraints in a task state $T$ as $\mathcal{W}_{T}$.

Examples: In a simple MRMP scenario, $\mathcal{W}_{\texttt{MRMP}}$ is the
composite $\mathcal{C}_{space}$ of the robots, and
$\mathcal{T}_{\texttt{MRMP}}$ has a single state $T$ which constrains motions
to the free space (no collisions with obstacles or other robots). A drone
fleet can have different sets of formation constraints embedded as task states
$T\in\mathcal{T}_{\texttt{drone}}$ each defining feasible motions while
maintaining the current formation. In the case of a multi-manipulator problem,
$\mathcal{W}_{\texttt{manip}}$ consists of not only robot
$\mathcal{C}_{spaces}$, but also all available poses for the objects in the
scene. A task state $T\in\mathcal{T}_{\texttt{manip}}$ defines which robot is
holding which object and the associated grasp constraints along with the poses
of all unheld objects.

####  III-A2 Decomposition

We can decompose a task state $T$ into a set of $k$ task substates (4).

$\displaystyle D_{T}=\\{T_{0},...,T_{k}-1\\}$ (4)

Each task substate $T_{i}\in D_{T}$ contains some subset of constraints in
$T$. We decompose $\mathcal{W}_{T}$ into subspaces corresponding to the set of
task substates $D_{T}$. We denote the subspace of the world configuration
space constrained by a task substate $T_{i}$ as $\mathcal{W}_{T_{i}}$. A
subspace $\mathcal{W}_{T_{i}}$ is valid if all moveable bodies constrained by
$T_{i}$ are in $\mathcal{W}_{T_{i}}$.

A decomposition $D_{T}$ is complete if the set of corresponding sub-
configuration spaces $\\{\mathcal{W}_{T_{0}},...,\mathcal{W}_{T_{k}-1}\\}$
contain exactly one instance of each moveable body in $\mathcal{W}$. A
decomposition $D_{T}$ is independent so long as no constraint in $T$ is split
across multiple substates $T_{i},T_{j},...\in D_{T}$. We often relax this
slightly in practice to temporarily ignore collisions between moveable bodies
in different sub-configuration spaces $W_{T_{i}},W_{T_{j}}$. Approaches that
use this relaxation must ensure the final solution satisfies the relaxed
constraint.

Examples: The MRMP task state decomposes into subsets of the robots (Fig. 7a).
The drone fleet task states decompose into squads with internal formation
constraints (Fig. 7b). The multi-manipulator task states decompose into
substates capturing which robots are holding which objects (Fig. 8).

(a) Multi-Robot Decomposition

(b) Drone Decomposition

Figure 7: a) The multi-robot state space is decomposed into individual robot
state spaces. b) The drone fleet state space is decomposed along formation
constraints. Note that all drones in a formation remain in a subspace
together.

(a) Full Multi-Manipulator Task State

(b) Robot 1 Substate

(c) Object Substate

(d) Robot 2 Substate

Figure 8: The full manipulator state space in (a) is decomposed into three
separate state spaces in (b,c,d). Note that the environment features (the
table) and thus the corresponding collision constraint is present in all three
subspaces.

####  III-A3 Transitions

Transitions between task states represent changes in the constraint sets.
Transitions also apply to task substates. These apply to sets of substates
when a transition requires more elements of $\mathcal{W}$ than are in a single
substate. A valid transition between sets of substates requires the union of
$\mathcal{W}$ elements to be the same before and after the transition.

Examples: In the drone fleet, these are transitions between formations. All
drones in the substates before a transition between formation constraints must
be present in the set of substates after the transition (Fig. 9a). In the
multi-manipulator problem, transitions are actions (e.g. pick, place,
handoff). All robot and objects present in the substates before an action must
be present in the set of substates after applying the action (Fig. 9b). This
example is expanded upon in later sections.

(a) Drone Transition

(b) Manipulator Transition

Figure 9: a) The drones transitions from a four drone formation (and
corresponding state space) into a pair of decoupled subspaces with two drones
each. The hyperarc captures this transition as both subspaces are required to
transition out of the original four drone space. (b) The hyperarc captures a
handoff of an object. The transition requires both the substate with the red
robot holding the object and the substate with the blue robot by itself to
begin the transition. Thus, they form the tail set of the hyperarc. The pair
of substates post-handoff in the head set of the hyperarc capture the transfer
of the object to the blue robot and the corresponding substates.

####  III-A4 Solution

A solution $\Pi$ to these problems consists of both a high-level sequence of
task states $T^{\texttt{start}},...,T^{\texttt{final}}$ along with the
corresponding transitions, and feasible paths within each $\mathcal{W}_{T}$,
$T\in\Pi$ between transitions. Solutions constructed from task substates can
be merged into sequences of task states so long as each task state has a
corresponding complete and independent decomposition. Any constraint relaxed
for the decomposition must be satisfied in $\Pi$.

###  III-B Method Overview

We propose the Decomposable State Space Hypergraph (DaSH) method, a general
multi-robot task and motion planning method based on the problem formulation
presented in Section III-A. Our approach consists of three stages:
representation construction, task planning, and conflict resolution.

The DaSH method first builds the representation layers then queries them to
generate task plans. We introduce a hierarchy of hypergraph-based
representation layers which capture increasing levels of information. The
highest layer contains the decoupled task space and transitions between task
substates. The middle layer incorporates the configurations spaces induced by
the task substates and feasible motions within those spaces. The final layer
represents the search itself and includes the transition history of potential
solutions. The representation layers are defined in detail in Section IV.

Task plans are formed from task substates (and corresponding paths through
their induced configuration spaces). A final conflict resolution stage ensures
there are no inter-path collisions. We discuss variations of this search
process which results in different theoretical properties of the approach. The
method is expanded upon in Section V.

Algorithm 1 High-Level Approach

1:procedure Approach(Task and Motion Planning Problem)

2: $\mathcal{H}\leftarrow$ComputeRepresentation()

3: $\mathcal{S}_{\texttt{optimisitic}}\leftarrow$ComputeTaskPlan($H$)

4:
$\mathcal{S}_{\texttt{complete}}\leftarrow$ResolveConflicts($\mathcal{H},\mathcal{S}_{\texttt{optimisitic}}$)

5: if decoupled then

6: return $S_{\texttt{complete}}$

7: while not converged do

8: if no solution then

9: $\mathcal{H}\leftarrow$ExpandRepresentation($\mathcal{H}$)

10:
Iterate($\mathcal{H},\mathcal{S}_{\texttt{optimisitic}},\mathcal{S}_{\texttt{complete}}$)
return $S_{\texttt{complete}}$

We include two variants in Algorithm 1. For a fast, one shot planning
approach, a sequential execution of the task planning and the conflict
resolution layers can provide a feasible task and motion solution (lines 5-6).
This is a decoupled approach and does not offer any completeness or optimality
guarantees.

To provide probabilistic completeness and representation-optimality
(optimality with respect to the currently constructed representations), we use
a hybrid approach inspired by the work in [4]. The algorithm iteratively
builds and validates task plans until it converges to the optimal solution
(lines 7-10). If it fails to discover a valid plan, we return to the
representation construction and expand the sampled representation (line 9).

##  IV Representation

In this section, we provide the technical formulation for a hypergraph
representation which captures the decomposable planning space described in
Section III-A. We construct representation layers to first encode task space
transitions, then motion feasibility, and finally transition history
information to enable task and motion plan queries over the representation. We
will continue to use our example problems (multi-robot motion planning, drone
fleet formation planning, and multi-manipulator planning) to illustrate the
concepts in this section. In Sections VI and VII, we provide detailed
instantiations of this hypergraph representation for the multi-robot motion
planning (MRMP) and multi-manipulator planning problems.

###  IV-A Task Space Hypergraph

Figure 10:  This image depicts the task space hypergraph for a multi-
manipulator problem with two robots and one object system. The vertices
represent the set of independent substates obtainable in the system. The
hyperarcs encode the set of transitions available by applying pick, place, and
handoff actions. The set of full task states obtainable are captured in Fig.
4, and the colors generally correlate. The red vertex represents Robot 1 not
holding any object. The blue vertex represents Robot 2 not holding any object.
The yellow vertex represents the object at a stable pose. The combination of
these three vertices creates the full state depicted in Fig. 15a. The hyperarc
from the red and yellow vertices to the orange vertex denotes a pick (or
place) action and includes only the moveable bodies involved. The hyperarc
from the orange and blue vertices to the red and green vertices encodes a
handoff action (like the one depicted in Fig. 9b), and the hyperarc from the
green vertex to the yellow and blue vertices encodes a place (or pick) action.
This sequence is the same as the full system sequence of states depicted in
Fig. 4, but only the substates that experience change are involved in the
hypergraph transitions.

Task planning can be represented by a task space $\mathcal{T}$ where each task
state $T\in\mathcal{T}$ encodes a particular semantic state and/or motion
constraints for the configuration space $\mathcal{W}$. We consider
decomposition of task states into decoupled task substates
$D_{T}=\\{T_{0},...,T_{k}\\}$ and propose a task space hypergraph to model
possible transitions between task substates.

The task space hypergraph
$\mathcal{H}_{\mathcal{T}}=(\mathcal{V}_{\mathcal{T}},\mathcal{E}_{\mathcal{T}})$
represents all obtainable task substates following a set of decomposition
rules and the transitions between the task states (Fig. 10). Each vertex
represents a task substate. Each hyperarc encodes a transition from a tail set
of task substates to a head set of task substates (Fig. 9). The set of
substates in the tail (head) must be independent (i.e. no overlapping moveable
bodies in the corresponding sub-configuration spaces
$\\{\mathcal{W}_{T_{i}},\mathcal{W}_{T_{j}},...\\}$).

Examples: A complete decomposition of the single MRMP task state into
individual robot substates (like the one shown in Figure 7a) creates a task
space hypergraph $\mathcal{H}_{\texttt{MRMP}}$ with one vertex for every robot
substate and no hyperarcs. The task space hypergraph
$\mathcal{H}_{\texttt{drone}}$ for drone formation decomposition contains a
vertex for every squad with internal formation constraints which the planner
can consider. It contains hyperarcs, like the one shown in Figure 9a, which
capture transitions between squads and formation constraints. The multi-
manipulator task space hypergraph $\mathcal{H}_{\texttt{manip}}$ contains
vertices for the various decomposed task substates denoting which robots are
holding which objects (Fig. 8), and the hyperarcs capture the transitions
between substates induced by the actions the robots can perform (Fig. 9b). A
full task space hypergraph $\mathcal{H}_{\texttt{manip}}$ for the multi-
manipulator problem is shown in Figure 10 with two robots, one object, and a
set of pick, place, and handoff actions.

###  IV-B Grounded Hypergraph

Figure 11: This grounded hypergraph is an augmentation of the multi-
manipulator task space hypergraph in Figure 10. Grounded transition paths are
planned for each hyperarc in the task space hypergraph. The start and end
configurations of the transition paths are represented as colored vertices
within the corresponding task substates. Paths are planned between transition
start/end configurations within the same task substate and represented as
(colored) edges to encode movement within that substate. Note there are no
internal edges within the yellow object substate as it is unactuated.

The task space hypergraph considers only task space conditions and does not
consider motion feasibility of transitions. We build a grounded hypergraph
$\mathcal{H}_{\mathcal{G}}=(\mathcal{V}_{\mathcal{G}},\mathcal{E}_{\mathcal{G}})$
to expand the task space information in the task space hypergraph to include
motion feasibility. This grounded hypergraph is a sampled representation of
possible transitions through the multi-robot task and motion planning space.

Examples: A visual depiction of the grounded hypergraph for a multi-
manipulator problem is given in Figure 11 and for a multi-robot motion
planning problem in Figure 14.

####  IV-B1 Grounded Vertices

In an ungrounded state, some of the variables necessary to represent it as a
configuration are missing. For instance, the vertices in the task space
hypergraph contain task space constraints, but do not have any dof values to
represent an explicit configuration within the configuration space $W_{T_{i}}$
of that task substate $T_{i}$. A grounded vertex then is one in which all of
the variables have values. We create grounded vertices
$v\in\mathcal{V}_{\mathcal{G}}$ by sampling within the constrained sub-
configuration space $\mathcal{W}_{T_{i}}$ corresponding to each task substate
$T_{i}$ represented by a vertex in $\mathcal{V}_{\mathcal{T}}$. Each task
substate vertex in the task space hypergraph may map to many grounded vertices
in the grounded hypergraph.

Examples: In the MRMP problem, grounded vertices correspond to configurations
in individual robot $\mathcal{C}_{spaces}$. The multi-manipulator problem may
have grounded vertices (sampled configurations) within individual manipulator
or object $\mathcal{C}_{spaces}$ or within composite robot-object spaces where
a manipulator is holding an object. These samples must obey the constraints of
the corresponding task substate (e.g. grasp constraints).

####  IV-B2 Grounded Hyperarcs

Some transitions simply denote a switch between configuration space
composition, and do not need additional information to be grounded. When a
transition path is required (like when performing a manipulator handoff), we
ground the transitions represented by task space hyperarcs
$\mathcal{E}_{\mathcal{T}}$. As the transition involves all of the moveable
bodies included in the tail set vertices, we consider the configuration space
created by the composition of the task substate sub-configuration spaces in
the hyperarc’s tail set
$\mathcal{W}_{\texttt{transition}}=\prod_{T_{i}\in\texttt{Tail}(E)}\mathcal{W}_{T_{i}}$.
We ground transitions by sampling feasible transition paths in this space
$\mathcal{W}_{\texttt{transition}}$. Like vertices, each hyperarc in the task
space hypergraph may map to many grounded hyperarcs in the grounded
hypergraph.

Examples: In the drone fleet problem, this transition path would capture the
necessary motions to move between different squad/formation configurations. In
the multi-manipulator problem, transition paths contain the motions necessary
to perform actions such as picks, places, or handoffs.

To build this grounded hypergraph $\mathcal{H}_{\mathcal{G}}$, we go through
each of the hyperarcs in the task space hypergraph and sample transitions in
the composition of the hyperarc’s substates’ configuration spaces
$\prod_{T_{i}\in\texttt{Tail}(E)}\mathcal{W}_{T_{i}}$. The start and end of
each substate path is saved as a grounded vertex and the paths themselves are
stored as hyperarcs.

####  IV-B3 Roadmaps

We build roadmaps within each sub-configuration space $\mathcal{W}_{T_{i}}$ to
provide information on motion feasibility between transitions. The transition
start/end vertices are used as seed configurations. These roadmaps can be
added to the grounded hypergraph $\mathcal{H}_{\mathcal{G}}$ in full, with
every roadmap vertex included in $\mathcal{V}_{\mathcal{G}}$ and every roadmap
edge represented by a hyperarc with a head and tail set of size one.
Alternatively, the motion feasibility can be condensed by only adding
hyperarcs to represent internal sub-configuration space paths between
transition start and end point. These paths are queried from the roadmap.
Thus, a hyperarc from a transition end vertex to a start vertex indicates that
there is a feasible path to move from one transition to another within
$\mathcal{W}_{T_{i}}$.

Examples: These are the colored hyperarcs in Figures 11 and the single
tail/head arcs in Figure 14.

####  IV-B4 Start and Goal

In order to reason over the space for a particular task, vertices are added
for start configurations within the appropriate task substates and a vertex
for any specified goal configurations (Fig. 11, 14). A virtual source and
target vertex is included to denote task start and completion (Fig. 11, 14). A
start hyperarc is added with the tail set consisting only of the source vertex
and the head set containing all start vertices. A completion hyperarc is added
for every set of vertices satisfying the entire set of goal conditions where
the tail set is the goal vertices and the head set contains only the target
vertex. Like a roadmap which may be queried for many motions by connecting new
start and goal configurations, the grounded hypergraph can leveraged to query
additional tasks.

###  IV-C Transition-extended Hypergraph

In order to query a valid task plan, we must consider a transition-extended
search space which we represent with a transition-extended hypergraph
$\mathcal{H}_{TE}=(\mathcal{V}_{TE},\mathcal{E}_{TE})$. Each vertex
$v\in\mathcal{V}_{TE}$ corresponds to a vertex in the grounded hypergraph and
a transition history to arrive at $v$. Each hyperarc $E\in\mathcal{E}_{TE}$
corresponds to a hyperarc in the grounded hypergraph. Each vertex in the tail
(and head) set of $E$ has a corresponding vertex in the tail (and head) set of
the grounded hyperarc. We lazily construct the transition-extended hypergraph
during search.

####  IV-C1 Transition History

The transition history of a particular vertex contains all of the transitions
taken to reach the vertex and the relative order in which they were taken
(Fig. 12). As transitions are represented as hyperarcs, this corresponds to an
ordered set of hyperarcs. A valid transition history requires this set of
hyperarcs to form a hyperpath from the source vertex and meet the properties
defined in Section II-E2 (1,2,3). Additionally, no vertex in a valid
transition history can have two outgoing hyperarcs as this would imply that
the moveable bodies in that vertex performed conflicting simultaneous
transitions in parallel.

Examples: In the drone fleet problem, a transition history in which a vertex
has more than one outgoing hyperarcs implies that the drone(s) are part of two
squads at once each following different paths. In the multi-manipulator
problem, invalid transition histories occur when two different robots pick up
the same object from the same location without replacement or a robot is
supposed to pick up two objects simultaneously (Fig. 12b). Both of these are
represented by multiple outgoing hyperarcs in an action history.

As all vertices in the transition-extended hypergraph represent specific
grounded configurations, the hyperarcs represent specific grounded paths, and
the transition history is part of the transition-extended state, every vertex
in the transition-extended hypergraph has only a single incoming hyperarc
(except for the source vertex which has no incoming hyperarcs). As a result,
the transition history of a vertex can be trivially reconstructed by tracing
its parentage to the source vertex. This parentage forms a hyperpath from
source to the vertex.

In addition to mapping to an equivalent tail set in the grounded hypergraph, a
tail set in the transition-extended hypergraph must have non-conflicting
transition histories. Two transition histories are considered conflicting if
their union contains a vertex with more than one outgoing hyperarc as this
would indicate simultaneous parallel transitions for a robot and/or object.
For any given hyperarc $E$ that corresponds to a transition, the union of
transition histories of the vertices in $\texttt{Tail}(E)$ represents how the
necessary conditions are satisfied before performing the transition.

####  IV-C2 Transition-extended Hyperpath

(a) Valid History

(b) Invalid History

Figure 12:  Both figures (a,b) consider a transition-extended version of the
multi-manipulator space depicted in the task space hypergraph (Fig. 10) and
grounded hypergraph (Fig. 11) where robot 2 can now reach the object’s
starting position and there may be more robots and objects involved outside
the vertices and hyperarcs depicted. The colors of the vertices correspond to
the previous hypergraphs (yellow-object, red-robot 1, blue-robot 2). In either
figure, we consider the transition history for the tail set of the light grey
hyperarc. Figure (a) depicts a valid transition history where no vertex has
more than one outgoing hyperarc. Figure (b) depicts an invalid transition
history. The conflict in the transition history is in the dotted red box. By
including the pair of outgoing hyperarcs of the yellow object vertex in the
same transition history, the object must exist in two conflicting states at
once.

A transition-extended hyperpath $\Pi=(\mathcal{V}_{\Pi},\mathcal{E}_{\Pi})$ is
a hyperpath on the transition-extended hypergraph
$\mathcal{H}_{TE}=(\mathcal{V}_{TE},\mathcal{E}_{TE})$ where
$\mathcal{V}_{\Pi}\subseteq\mathcal{V}_{TE}$ and
$\mathcal{E}_{\Pi}\subseteq\mathcal{E}_{TE}$. The hyperpath $\Pi$ is rooted at
the transition-extended source vertex and must contain a single transition-
extended extension of the grounded hypergraph target vertex. The set of
hyperarcs $\mathcal{E}_{\Pi}$ must form a valid transition history.

This transition-extended hyperpath represents an optimistic solution. An
optimistic solution has a valid transition plan and locally feasible motion
paths but does not guarantee collision-free paths between sub-configuration
spaces (this is due to the relaxation of moveable body collision constraints
between sub-configuration spaces in the decomposition). The set of hyperarcs
represents a valid set of task space transitions and paths between them that
moves the system from the start state (the source vertex) to a satisfying goal
state (the target vertex).

This transition-extended hyperpath also encodes a continuous path for every
moveable body in $\mathcal{W}$. We define a set of origin vertices and set of
terminal vertices where the set of origin vertices is the head set of the
outgoing hyperarc from the source vertex, and the terminal vertices in a
hyperpath are all vertices without an outgoing hyperarcs or for which the
outgoing hyperarc enters the sink vertex. Each moveable body in $\mathcal{W}$
exists in a single origin vertex and a single terminal vertex. A simple path
exists in the hyperpath for each moveable body from its origin vertex to its
terminal vertex. This follows from three conditions described previously:

  1. 1.

Every vertex has at most one outgoing hyperarc.

  2. 2.

In every transition hyperarc, the union of moveable bodies in the tail set
must be equivalent to the union of moveable bodies in the head set.

  3. 3.

The substates in every head set must be disjoint, thus each moveable body
involved in a transition is contained within only a single head vertex.

These simple paths contains the continuous path for each moveable body. The
Cartesian product of these paths is a continuous path through $\mathcal{W}$
from the start to the goal though this path is not guaranteed to be valid due
to collisions between individual moveable body paths.

Examples: In the MRMP problem, the transition-extended hyperpath contains an
individually valid path for each robot which must then be checked for inter-
path collisions. The drone formation planning transition-extended hyperpath
represents both the sequence of formation and squad transitions and the
corresponding motions; the motions must be verified for inter-path collisions
between subspaces. Finally, the multi-manipulator transition-extended
hypergraph captures both the discrete high-level sequence of actions to
complete the task and optimistic motion plans for executing these actions.

The optimistic solution cost is captured in the transition-extended hyperpath.
Each hyperarc $E$ has an associated cost $w(E)$. The cost-from-source of any
vertex $v_{i}$ in the head set $\texttt{Head}(E)$ is the sum of $w(E)$ and the
maximum cost-from-source of all $v_{j}$ in the tail set $\texttt{Tail}(E)$.
The difference in the cost-from-source for a vertex $v_{j}\in\texttt{Tail}(E)$
and the maximum cost-from-source of all $v_{k}\in\texttt{Tail}(E)$ represents
the time the moveable bodies present in the task substate of $v_{j}$ wait at
the grounded configuration of $v_{j}$ before performing the transition
represented by $E$. The cost of the maximum cost-from-source vertex provides
the makespan of the optimistic solution. In a minimal hyperpath, this will be
the target vertex.

##  V Method Details

In this section we provide a detailed explanation of the Decomposable State
Space Hypergraph (DaSH) method outlined in Section III-B and Algorithm 1. We
first describe the high-level hybrid search across the task and motion domains
in Section V-A followed by a detailed description in Section V-B of how we
search the representations proposed in Section IV to find a task plan.
Finally, we describe the motion planning conflict resolution stage of the
algorithm in Section V-C.

We will continue to leverage our example problems to illustrate these
concepts. Detailed applications of this method to the multi-robot motion
planning (MRMP) and the multi-manipulator problems are provided in Sections VI
and VII.

###  V-A Next-Best Search

The Next-Best Search algorithm (Alg. 2) takes in the grounded hypergraph
$\mathcal{H}_{\mathcal{G}}$ and uses it to generate task plans. These plans
are then validated and modified to avoid collisions between sub-configuration
space paths thus producing valid solutions. The algorithm iteratively builds
and validates task plans until it converges to the optimal solution.

Each task plan represents an optimistic solution consisting of the sequence of
paths and transitions without a guarantee of collision-free paths which we
will refer to as a schedule. The cost of this optimistic schedule serves as a
lower-bound for the solution cost as the solution cost can only increase from
resolving conflicts. The algorithm requires each successive call to
FindTaskPlan (line 6) to return the next-best task plan in terms of cost (i.e.
each optimistic schedule cost must be at least as much as the previous
schedule cost). This allows each successive iteration to update the lower
bound of the solution.

Each optimistic schedule must be converted into a valid solution by resolving
any potential conflicts. The ConflictResolution function (line 8) uses the
roadmaps in the grounded hypergraph along with the schedule to construct a set
of collision free paths following the scheduled generated by FindTaskPlan. If
the cost of this valid solution is lower than the previous upper bound, we
update the upper bound to reflect this solution and save this as the best
solution so far (lines 9,10). If no valid solution can be found in the current
representation, the cost is treated as infinite, and the upper bound is
unchanged. Scheduled are saved in $\mathcal{S}$ (line 11)and given to the task
planner to ensure that it returns the next-best solution (line 6).

The algorithm iterates until an optimistic schedule is produced with a cost
greater than the current upper bound (line 5). At this point, the current best
solution is determined to be the optimal solution with respect to the current
representation, and this solution is returned (line 12).

This iteration follows the strategy proposed by [4]. If it fails to discover a
valid plan, it returns to the representation construction and expands the
grounded hypergraph.

Algorithm 2 Next-Best Search

1:procedure NBS(Grounded Hypergraph $\mathcal{H}_{\mathcal{G}}$)

2: $(S^{*},T^{*})\leftarrow(\emptyset,\infty)$$\triangleright$ Best solution

3: $\underline{T}\leftarrow 0$$\triangleright$ best lower-bound

4: $\mathcal{S}\leftarrow\emptyset$$\triangleright$ Set of discovered
solutions

5: while $T^{*}>\underline{T}$ do

6:
$(P,\underline{T})\leftarrow$FindTaskPlan($\mathcal{H}_{\mathcal{G}},\mathcal{S}$)

7: if $T^{*}>\underline{T}$ then

8: $(S,T)\leftarrow$ConflictResolution($\mathcal{H}_{\mathcal{G}},P$)

9: if $T<T^{*}$ then

10: $(S^{*},T^{*})\leftarrow(S,T)$

11: $\mathcal{S}\leftarrow\mathcal{S}\cup\\{P\\}$

12: return $S*$

###  V-B Task Search

To generate a task plan, Algorithm 3 searches and constructs the transition-
extended hypergraph $\mathcal{H}_{TE}$ by expanding the grounded hypergraph
vertices to include the transition history as described in Section IV-C.

####  V-B1 Hyperpath Query

Algorithm 3 QueryTaskPlan

1:procedure FindTaskPlan(Grounded Hypergraph $\mathcal{H}_{\mathcal{G}}$,
Previous Solutions $\mathcal{S}$)

2: $v_{source}\leftarrow<\emptyset,\emptyset,\emptyset>$

3: Root set $R\leftarrow$DecomposeToSubstates(start)

4: $\mathcal{H}.\mathcal{V}\leftarrow R\cup\\{v_{source}\\}$$\triangleright$
Add initial root vertices

5:
$\mathcal{H}.\mathcal{E}\leftarrow\\{<\\{v_{source}\\},R>\\}$$\triangleright$
Add arc from source to roots

6: for all $v_{i}\in\mathcal{H.V}$ do

7: $W(v_{i})\leftarrow 0$$\triangleright$ Set initial weight to 0

8: Priority Queue $Q\leftarrow R$$\triangleright$ Initialize priority queue

9: Partial Hyperarcs $U\leftarrow\emptyset$$\triangleright$ Initial partial
hyperarc set

10: while $Q\neq\emptyset$ do

11: $u\leftarrow Q$.pop()

12: for all $E_{j}\in\texttt{FS}_{\texttt{complete}}(u,U)$ do

13: $\mathcal{H.E}\leftarrow\mathcal{H.E}\cup\\{E_{j}\\}$$\triangleright$ Add
arc to hypergraph

14: $f\leftarrow F(\texttt{Tail}(E_{j}))$

15: for all $y\in\texttt{Head}(E_{j})$ such that $W(y)>w(E_{j})+f$ do

16: if $y\in\mathcal{S}$ then

17: Continue

18: if $y\notin Q$ then

19: $\mathcal{H.V}\leftarrow\mathcal{H.V}\cup\\{y\\}$

20: $\mathcal{Q}\leftarrow\mathcal{Q}\cup\\{y\\}$

21: $W(y)=w(E_{j})+f$

22: $P_{v}[y]=E_{j}$

23: if IsGoal($y.q$) then

24: return TraceHyperpath($y,H$)

Algorithm 3 presents a modification of the hyperpath query algorithm described
in [26]. The main difference here is the search time construction of the
hypergraph as a result of the implicit representation of the transition-
extended space.

In addition to adding vertices and hyperarcs to the hypergraph as they are
discovered, the entire forward star of a vertex $FS(v)$ (i.e. all outgoing
hyperarcs) cannot be computed directly as there might be an infinite set of
valid transition-extended tail sets for each grounded hyperarc. In a hyperpath
query of an explicit hypergraph, a hyperarc is only expanded once all vertices
in its tail set have been expanded [26]. As each transition-extended hyperarc
maps to a grounded hyperarc under the conditions discussed in Section IV-C, we
can determine the subset of the forward star of a vertex $v$ for which all
hyperarcs $E_{j}\in FS_{\texttt{complete}}(v)$ have complete tail sets in the
current construction of the transition-extended hypergraph and thus can be
expanded as all tail vertices have been visited. Rather than checking all
combinations of expanded vertices for valid tail sets, we keep a set of
partial hyperarcs informed by the forward stars of grounded hypergraph
vertices and previously expanded transition-extended vertices.

Example: In the multi-manipulator planning problem, a partial hyperarc is
created when a vertex satisfies a subset of the pre-conditions necessary to
perform an action. Partial hyperarcs are expanded as other vertices are
discovered which satisfy more of the pre-conditions, and a complete hyperarc
is added to the transition-extended hypergraph when a tail set satisfying all
pre-conditions is found.

The hyperpath query starts by generating an empty source vertex (line 2) and
root vertices corresponding to the system start state (line 3). These are the
transition-extensions of the grounded hypergraph origin vertices. These
vertices are all added to the hypergraph (line 4) followed by a hyperarc
connecting the source vertex to the set of root vertices. The weight
$W(v_{i})$ of all initial vertices is set to 0 (line 6) reflecting the start
time of the plan. The queue $Q$ is then initialized to the set of root
vertices (line 8).

Once initialized, the query loops through the queue until it is empty (line
10) or a satisfying hyperpath is found (line 24). The loop starts by removing
the lowest weighted vertex $u$ (line 11). The expansion of $u$ (line 12)
iterates over all hyperarcs in the completed forward star of the vertex
$FS_{\texttt{complete}}(u,U)$ (i.e. all outgoing hyperarcs for which all tail
vertices have been expanded). This call to the forward star function both
utilizes the cached partial hyperarc set $U$ to construct complete hyperarcs
and updates the cache with any additionally discovered partial hyperarcs.

Each complete hyperarc $E_{j}$ is added to the hypergraph (line 13). The cost-
from-source $f$ of $E_{j}$ is computed (line 14) then used to see if $E_{j}$
presents a lower cost path to any vertex $y$ in $\texttt{Head}(E_{j}$). In the
transition-extended hypergraph, all vertices have only a single incoming
hyperarc, so this will always evaluate to true as will the check if the vertex
does not yet exists in the hypergraph (line 18). New vertices are added to the
hypergraph (line 19) and the queue (line 20). The weight $W(y)$ and parentage
$P_{v}$ of $y$ is then updated (lines 21, 22). Finally, the query checks if
this vertex presents a satisfying goal state (i.e. does the transition-
extended vertex correspond to the grounded hypergraph target vertex). If so,
the query returns the hyperpath from $v_{source}$ to $y$ (lines 23, 24).

As each vertex contains the entire transition history, the final vertex
$y_{\texttt{final}}$ not only contains the entire solution, but is the only
vertex to contain that exact solution. As a result, we can represent previous
solutions simply by the final vertex $y_{\texttt{final}}$ in the corresponding
hyperpath. When we require the next-best property, we can tell the search to
ignore vertices that provided previous solutions (line 16). Additionally, the
transition-extended hypergraph (and the queue) can be preserved between
queries if multiple queries of the hypergraph are expected. The next
satisfying goal vertex found after resuming the search provides the next-best
solution.

Examples: In the drone problem, each successive hyperpath corresponds to a
unique sequence of squad and formation transitions. In the multi-manipulator
problem, each successive solution would contain a unique optimistic sequence
of grounded actions and motions.

####  V-B2 Heuristic

Figure 13:  This figure illustrates the concept of entry and exit vertices
used in the heuristic formulation. Entry vertices have an incoming hyperarc
where the tail set vertices are in different subspaces. Exit vertices have an
outgoing hyperarc where the head set vertices are in different subspaces.
Grounded hypergraphs which use the condensed representation of the underlying
roadmaps (and the corresponding transition-extended hypergraphs) alternate
between entry and exit vertices.

To help guide the search, we design a cost-to-go heuristic for the hyperpath
query similar to A* in a traditional graph search. The heuristic $h(v)$
captures an informative lower-bound of the cost-to-go from any vertex $v$ to a
satisfying goal state (target vertex). The heuristic is integrated into
Algorithm 3 by ordering the queue $Q$ by the sum of the cost-from-source and
the cost-to-go for each vertex.

The heuristic leverages two sources of information. An independent cost-to-go
based on optimistic paths to the goal gives a lower-bound of individual
vertices, and the structure of the transition-extended hypergraph is used to
capture dependencies between vertex cost-to-gos in a hyperpath cost-to-go.

The independent cost-to-go function $I(v)$ provides a lower-bound cost-to-go
of including a vertex $v$ in the hyperpath. Vertices can be split into two
classes, those with goal conditions variables, and those without. Vertices
with goal condition variables contain a moveable body (i.e. robot or object)
for which a condition must be met for the system to reach a goal state.
Vertices without goal condition variables contain no moveable bodies specified
in the goal conditions. If the task substate of a vertex $v$ contains no goal
condition variables, then the lower-bound cost-to-go to a terminating vertex
is 0 as this could be a final vertex in a hyperpath.

Examples: In the MRMP problem, all moveable bodies (robots) have a goal
condition. In the multi-manipulator problem, only the objects have goal
conditions, and thus any grounded vertex $v$ containing only a manipulator has
an independent cost-to-go function $I(v)$ of 0.

If a vertex $v$ has a single outgoing hyperarc $E$ with a head set
$\texttt{Head}(E)$ such that $|\texttt{Head}(E)|>1$, the cost-to-go from $v$
is the maximum of the cost-to-go from the vertices in $\texttt{Head}(E)$ plus
the cost of $E$ as all vertices in a hyperpath must reach a terminating state.
If $v$ has more than one outgoing hyperarc, then the cost-to-go is the minimum
such cost between all outgoing hyperarcs of $v$.

We use the transition-extended hypergraph structure to create a more informed
heuristic (Fig. 13). Using the condensed representation of roadmaps/paths
within sub-configuration spaces, vertices can be classified as either entry or
exit vertices indicating the relationship with the task substate. Entry
vertices have a parent vertex in a different substate. Exit vertices a single
parent which is in the same substate, and outgoing hyperarcs with head set
vertices in other substates. Hyperarcs switch between vertex types. Entry
set$\rightarrow$exit set denotes inter-substate motion. Exit
set$\rightarrow$entry set denotes substate transitions.

Examples: In the drone formation planning problem, hyperarcs denote
transitions from exit vertex tail sets in the previous squad formations to
entry vertex head sets in the new squad formations. Hyperarcs which denote a
transition from an entry vertex to an exit vertex represent motions within the
same squad under the same formation constraints. In the multi-manipulator
problem, hyperarcs from exit vertices to entry vertices represent transitions
from the pre-conditions to the post-conditions of actions. Hyperarcs from an
entry vertex to an exit vertex represent motion paths within that task
substate (e.g. manipulator moving with the object it is holding).

When a hyperarc $E$ is included in the hyperpath $\Pi$, all vertices in the
head set $\texttt{Head}(E)$ must have a simple path in $\Pi$ to a valid
terminal state. The minimum cost-to-go to a terminal state after $E$ then is
the maximum individual cost-to-go of all vertices $v\in\texttt{Head}(E)$. As
every vertex has a single incoming hyperarc $E_{\text{incoming}}$, we can
examine $\texttt{Head}(E_{\text{incoming}})$ to compute a more informed lower
bound for each vertex $v\in\texttt{Head}(E_{\text{incoming}})$. If a vertex
$v_{\text{entry}}\in\texttt{Head}(E_{\text{incoming}})$, is an entry vertex,
then $|\texttt{Head}(E_{\text{incoming}})|\geq 1$. For a more informed lower-
bound, we define a local cost-to-go function $h_{\text{local}}$ for entry
vertices as the maximum individual cost-to-go of all vertices in
$\texttt{Head}(E_{\text{incoming}})$ in (5).

$\displaystyle
h_{\texttt{local}}(v_{\texttt{entry}})=\max_{v_{\texttt{sibling}}\in
E.\texttt{head}}\\{I(v_{\texttt{sibling}})\\}$ (5)

For any given exit vertex $v_{\text{exit}}$ in the hypergraph, all outgoing
hyperarcs correspond to transitions.

The local cost-to-go function $h_{\text{local}}$ for exit vertices can be
inferred by examining the local cost-to-go in the head sets of outgoing
hyperarcs. The cost-to-go for an exit vertex after choosing particular
outgoing hyperarc $E$ in its forward star, is given by the maximum individual
cost-to-go of the vertices in the head set $\texttt{Head}(E)$ plus the cost
$w(E)$ of $E$. The local cost-to-go for an exit vertex, given in (6), thus is
the minimum such cost across all hyperarcs in its forward star
$\texttt{FS}(v_{\texttt{exit}})$.

$\displaystyle
h_{\texttt{local}}(v_{\texttt{exit}})=\min_{E\in\texttt{FS}(v_{\texttt{exit}})}\\{\max_{v_{\texttt{child}}\in
E.\texttt{head}}\\{I(v_{\texttt{child}})+w(E)\\}\\}$ (6)

As each vertex $v$ has a transition history which corresponds to a hyperpath,
the complete cost-to-go $h_{TE}(v)$ is the maximum local cost-to-go of all
vertices without an outgoing hyperarc in the transition history hyperpath.
Rather than computing all of the local cost-to-go values for every vertex’s
transition history, we can pass down the maximum cost-to-go discovered so far.
This equation is given in (7).

$\displaystyle
h_{\texttt{TE}}(v)=\max\\{h_{\texttt{local}}(v),H_{\texttt{TE}}(v_{\texttt{parent}})-w(E)\\}$
(7)

###  V-C Conflict Resolution

The plan produced in Algorithm 3 only cares about feasibility within
individual task substates. As such, there is no guarantee that the paths
associated with the task plan are collision free. We address this with
conflict resolution layer.

The layer takes in the optimistic paths and produces collision free paths
through each task substate and transition. It is not required to provide
optimal paths to provide a feasible solution, however, the Next-Best Search
version of our approach does require (representation) optimality.

##  VI Application to Multi-Robot Motion Planning

We examine the application of the DaSH method to the multi-robot motion
planning (MRMP-DaSH) problem to illustrate the concepts discussed in the
previous sections. We first formally define the problem: Given a set of robots
$\mathcal{R}$ and an environment $\mathcal{E}$, find continuous collision-free
paths for each robot $r_{i}\in\mathcal{R}$ from a given start $s_{i}$ to a
given goal $g_{i}$.

###  VI-A Planning Space

The configuration space $\mathcal{W}_{\texttt{MRMP}}$ is the Cartesian product
of the individual $\mathcal{C}_{space}$ for each robot $r_{i}\in\mathcal{R}$
(8).

$\displaystyle\mathcal{W}_{\texttt{MRMP}}=\prod^{n}_{i=1}\mathcal{C}_{r_{i}}$
(8)

The task space $\mathcal{T}_{\texttt{MRMP}}$ consists of a single state
$T_{\texttt{MRMP}}$ as the only constraints on feasible motions are for each
robot not to collide with any obstacle or other robots.

###  VI-B Decomposition

The single task state $T_{\texttt{MRMP}}$ can be decomposed into arbitrary
subsets of $\mathcal{R}$ (relaxing inter-robot collision constraints between
decomposed configuration spaces). A decomposition
$D_{T_{\texttt{MRMP}}}=\\{T_{0},...,T_{k}\\}$ is complete so long as it
includes each robot $r\in\mathcal{R}$ in exactly one task substate $T_{i}\in
D_{T_{\texttt{MRMP}}}$. As $T_{\texttt{MRMP}}$ does not impose any constraints
dependent upon other robot motions outside of collision,
$D_{T_{\texttt{MRMP}}}$ is an independent decomposition.

In this illustration, we choose to decompose $T_{\texttt{MRMP}}$ into
$|\mathcal{R}|$ substates such that each robot $r_{i}\in\mathcal{R}$ is in a
substate $T_{i}\in D_{T_{\texttt{MRMP}}}$ by itself. This is the same approach
used in decoupled and some hybrid approaches to multi-robot motion planning
such as Decoupled-PRM [15] and CBS-MP [11].

###  VI-C Representation

The representation for this choice of decomposition $D_{T_{\texttt{MRMP}}}$ is
rather simple. As there are no transitions in the task space
$\mathcal{T}_{\texttt{MRMP}}$, the task space hypergraph
$\mathcal{H}_{\mathcal{T}}$ consist only of disconnected vertices for each
$T_{i}\in D_{T_{\texttt{MRMP}}}$.

To build the grounded hypergraph, a roadmap is constructed in the
configuration space $\mathcal{C}_{r_{i}}=\mathcal{W}_{T_{i}}$ for each robot
$r_{i}$. A start configuration $s_{i}$ and goal configuration $g_{i}$ are
connected to the roadmap in $\mathcal{C}_{r_{i}}$, and a path is queried
between them on the roadmap. The path start and goal configurations, $s_{i}$
and $g_{i}$, are added as vertices in the grounded hypergraph with a hyperarc
between them denoting the queried path. These are the origin and terminal
vertices described in Section IV-B. The set $S$ of all start vertices forms
the head set of a hyperarc rooted at the source vertex, and the set $G$ of all
goal vertices forms the tail set of a hyperarc connected to the target vertex.
This grounded hypergraph is depicted in Figure 14.

Figure 14:  The grounded hypergraph for a multi-robot motion planning problem
contains single hyperarc within each individual robot subspace in addition to
the source and sink hyperarcs. The hyperarc within each robot’s subspace is an
abstraction of the path across the corresponding roadmap within that subspace.
All robots must be included in the tail set of the sink hyperarc to capture
the requirement that all robots reach their goal.

The transition-extended hypergraph mirrors the grounded hypergraph in this
scenario as there is only a single hyperpath from source to target in the
condensed roadmap/path representation. The hyperpath from source to target
contains a single decoupled path from $s_{i}$ to $g_{i}$ for each robot
$r_{i}$ in its decoupled configuration space
$\mathcal{C}_{r_{i}}=\mathcal{W}_{T_{i}}$. This is an optimistic solution as
it provides no guarantee that individual paths do not collide.

###  VI-D Method

In this simple MRMP problem, the task planning is trivial as only a single
valid hyperpath exists. This allows us to focus on the conflict resolution
stage.

There are many options for how to resolve potential conflicts between
individual robot paths. A decoupled approach may determine a priority sequence
of robots, and iteratively replan each robot’s path treating the previously
computed paths as dynamic obstacles to be avoided. This approach would result
in the entire approach, decomposition, representation, and query reducing to
Decoupled-PRM [15]. Decoupled-PRM often quickly finds a solution though does
not offer a completeness guarantee. Decoupled search variants such as
Priority-Based Search [27] can offer further improvements though still do not
obtain completeness.

Alternatively, hybrid approaches such as Conflict-Based Search (CBS) [28] (and
its adaptation to $\mathcal{C}_{space}$ roadmaps [11]) can be used to resolve
conflicts. If the CBS variant is used, then this MRMP-DaSH instance reduces to
our prior work, CBS-MP [11], which builds decoupled roadmaps, and adapts CBS
to find conflict-free paths over the roadmaps. Unlike decoupled approaches,
this is complete and optimal with respect to the current representation (i.e.
roadmaps) while maintaining efficient runtimes in most scenarios.

###  VI-E Alternative Decompositions

The nature of decomposing the full composite configuration space $\mathcal{W}$
into subspaces $\mathcal{W}_{T_{0}},...,\mathcal{W}_{T_{k}-1}$ naturally lends
itself towards hybrid search methods. However, not all hybrid searches work in
completely decoupled spaces like CBS-MP [11] but instead consider varying
levels of $\mathcal{C}_{space}$ composition. For example, the subdimensional
expansion approach of M* [29] moves in and out of higher dimensional spaces
composed of various individual robot $\mathcal{C}_{spaces}$ to resolve path
conflicts.

Each relevant composition of robot $\mathcal{C}_{spaces}$ can be represented
as a vertex in the task space hypergraph with hyperarcs representing
transitions between $\mathcal{C}_{space}$ compositions. Transition history is
required to ensure all robots have a single path from start to goal. The
grounded hypergraph can consist of roadmaps within each $\mathcal{C}_{space}$
composition along with hyperarcs capturing specific transition points in and
out of the various spaces. This brings up questions of composite roadmap
representations and when to include transitions between spaces. These
questions are beyond the scope of this work, though the representation
proposed offers an approach in which to build a structure suited for these
hybrid searches.

##  VII Application to Multi-Manipulator Planning

We consider multi-manipulator sorting and rearrangement problems to illustrate
our proposed approach and its benefits. This application is referred to as MM-
DaSH. We first define the problem: Given a set of robot manipulators
$\mathcal{R}$, a set of manipulable objects $\mathcal{O}$, an action space
$\mathcal{A}$, and an environment $\mathcal{E}$, the planner must find a
sequence of actions and motions for the robots to take such that the
manipulable objects move from a start pose $p_{\texttt{start}}$ to a
satisfactory goal pose $p_{\texttt{goal}}$.

###  VII-A Planning Space

We first define the world configuration space $\mathcal{W}_{\texttt{manip}}$
followed by the task space $\mathcal{T}_{\texttt{manip}}$ for this problem.

####  VII-A1 Configuration Space

The world configuration space $\mathcal{W}_{\texttt{manip}}$ is the Cartesian
product of all robot configuration spaces and object poses (11). The
configuration space of each robot $r_{i}\in\mathcal{R}$ is denoted
$\mathcal{C}_{r_{i}}$. The composite configuration space for all robots in the
system is given in (9).

$\displaystyle\mathcal{C}_{R}=\prod^{n}_{i=1}\mathcal{C}_{r_{i}}$ (9)

Each object $o_{j}\in\mathcal{O}$ has its own configuration space
$\mathcal{C}_{o_{j}}\in$ SE(3). The space of all object poses is given in
(10).

$\displaystyle\mathcal{C_{O}}=\prod^{m}_{j=1}\mathcal{C}_{o_{j}}$ (10)
$\displaystyle\mathcal{W}_{\texttt{manip}}=\mathcal{C}_{R}\times\mathcal{C}_{O}$
(11)

####  VII-A2 Task Space

We consider a multi-manipulator task space $\mathcal{T}_{\texttt{manip}}$
defined by the following:

  * •

Robot constraints indicating which (if any) object a robot is holding.

  * •

Object constraints indicating which robot is holding the object or its current
stable pose.

  * •

Task states denoting a particular set of robot and object constraints.

  * •

A task space comprising all possible task states.

The task space $\mathcal{T}_{\texttt{manip}}$ consists of robot constraints
for each $r_{i}\in\mathcal{R}$ and object constraints for each
$o_{j}\in\mathcal{O}$. A task state $T\in\mathcal{T}_{\texttt{manip}}$ has
values for all robot and object constraints.

####  VII-A3 Robot Constraints

A robot constraint $c_{r_{i}}$ for robot $r_{i}$ is defined by an object
allocation $o_{i}$ and associated grasp parameters $g_{i}$ when
$o_{i}\neq\emptyset$ (12). An empty $o_{i}$ indicates $r_{i}$ is not holding
an object. A nonempty $o_{i}$ specifies the object held by $r_{i}$ and
requires $g_{i}$ to be nonempty.

$\displaystyle
c_{r_{i}}=\\{o_{i},g_{i}\\},r_{i}\in\mathcal{R},o_{i}\in\mathcal{O}\cup\\{\emptyset\\}$
(12)

####  VII-A4 Object Constraint

An object constraint $c_{o_{j}}$ for object $o_{j}$ is defined by a robot
assignment $r_{j}$ and a pose $p_{j}$ where either $r_{j}$ or $p_{j}$ must be
empty (13). A nonempty $r_{j}$ implies the object is held. Otherwise, $p_{j}$
specifies the pose of the object $o_{j}$. We consider problems in which robots
can only hold one object and objects are only held by one robot, but $o_{i}$
and $r_{j}$ can be expanded to a set of objects or robots for other problems.

$\displaystyle
c_{o_{i}}=\\{r_{j},p_{j}\\},o_{j}\in\mathcal{O},r_{j}\in\mathcal{R}\cup\\{\emptyset\\},p_{j}\in\text{SE(3)}\cup\\{\emptyset\\}$
(13)

####  VII-A5 Task State

A task state is a complete set of robot and object states for the system. Each
task state $T\in\mathcal{T}_{\texttt{manip}}$ constrains feasible motions in
$\mathcal{W}_{\texttt{manip}}$ based on which robots are holding which objects
and the poses of unheld objects. This creates the constrained configuration
space $\mathcal{W}_{T}$.

####  VII-A6 Action Transitions

Transitions between task states in $\mathcal{T}_{\texttt{manip}}$ correspond
to actions. We consider an action space $\mathcal{A}$ consisting of pick,
place, and handoff actions. Actions $a_{i}\in\mathcal{A}$ are defined by a set
pre-conditions and post-conditions. These conditions correspond to sets of
robot and object constraints, while an action transitions the system from a
task state satisfying the pre-conditions to a state satisfying the post-
conditions. Actions also transition between sets of task substates so long as
the initial set of substates satisfies the pre-conditions and the final set of
substates satisfies the post-conditions.

A high-level solution to both sorting and rearrangement task problems consists
of a sequence of task states $T^{\texttt{start}},..,T^{\texttt{goal}}$ with
actions for each task state transition $T^{i}\rightarrow T^{i+1}$.

###  VII-B Decomposition

We propose a set of decomposition rules for the multi-manipulator task space
$\mathcal{T}_{\texttt{manip}}$ based on kinematic tree (KT) representations of
task states. For the multi-manipulator problems considered here, we decompose
task states into the set of minimal dimension substates. This results in task
substates which contain only the robots and objects whose configurations have
explicitly dependent relationships defined by the task state constraints. For
example, a robot and the object it is holding are in the same substate (Fig.
16b), but a robot not holding anything and an object not held are in separate
substates (Fig. 15b, 15c). Additionally, in this decomposition, no two robots
are ever in the same task substate.

When a task state is represented by a KT, this decomposition can be computed
by removing the KT vertices for the world and the fixed obstacles (Fig. 15,
16). Each resulting connected component is a task substate, and the connected
components contain both the moveable bodies defining the subspace and the
relevant constraints encoded in the KT edges. Any task state in the considered
task space can be decomposed into and reconstructed from a set of task
substates (with the standard relaxation of collision between sub-configuration
spaces).

(a) Task State

(b) Robot 1

(c) Object

(d) Robot 2

Figure 15:  a) Depiction of the multi-manipulator stable pose task state. This
task state can be split into three independent substates. (b) A substate for
robot 1 not holding any object. (c) A substate for the object resting at a
stable pose. (d) A substate for robot 2 not holding any object. These
substates are depicted in Figure 10. (b) corresponds to the red robot 1
vertex. (c) corresponds to the yellow object vertex, and (d) corresponds to
the blue robot 2 vertex.

(a) Task State

(b) Robot 1 + Object

(c) Robot 2

Figure 16:  a) Depiction of the multi-robot grasp 1 task state discussed in
earlier sections. This task state can be split into two independent substates.
(b) A substate for robot 1 holding the object. (c) A substate for robot 2 not
holding any object. These substates are depicted in Figure 10. (b) corresponds
to the orange robot 1 + object vertex, and (c) corresponds to the blue robot 2
vertex.

###  VII-C Representation

When considering the KT representation decomposition of task substates, the
union of the KT components in the tail set for an action’s hyperarc must
satisfy the action’s pre-conditions, and the union of the KT components in the
head set for an action’s hyperarc must satisfy the action’s post conditions.
Thus, the hyperarc encodes the prerequisites and effects of the action (Fig.
17).

The task space hypergraph $\mathcal{H}_{\mathcal{T}_{\texttt{manip}}}$
represents all obtainable task substates following the KT decomposition rules
and action transitions between them (Fig. 10). Each hyperarc encodes an action
which transitions a subset of the robots and objects from a tail set of task
substates to a head set of task substates (Fig. 17). The set of substates in
the tail (head) must be disjoint from each other in terms of constrained
robots and objects.

In the KT representation, the KT vertices representing tail (head) set
substates must be disjoint (ignoring the world and fixed obstacles). For
example, the red and orange vertices in Figure 10 cannot form a tail (head)
set as they both contain R1. The union of robots and objects (and thus KT
vertices) in the tail set must be equivalent to that of the head set.

Figure 17:  The hyperarc encodes the transition between task substates. When
using kinematic tree (KT) representations, this can be depicted as a change in
the KTs. Ignoring the world and obstacle vertices, the KT vertices in the tail
set substates should be disjoint. The union of KT vertices in the tail set
should be equivalent to the union of KT vertices in the head set.

####  VII-C1 Grounded Hypergraph

The multi-manipulator grounded hypergraph construction follows a similar
procedure to multi-robot motion planning. Start and goal vertices are added to
the appropriate task substates and connected to source and sink. The start
vertices consist of one vertex per robot/object in the task substate in which
it is alone (no objects are held at the beginning). The goal vertices encode
object poses that satisfy the goal conditions of the problem.

Additionally, each action hyperarc in the task space hypergraph must be
grounded. For each hyperarc, we sample feasible paths in the configuration
space created by the union of the task substates in the hyperarc’s tail set.
We can sample several instances of an action for a more informed
representation though this increases the search space in later stages. The
start and end of the action paths are saved as grounded vertices, and the
paths themselves are stored as hyperarcs.

As is done in multi-robot motion planning problems, a roadmap is built within
each the configuration space $\mathcal{W}_{T_{i}}$ for each task substate
$T_{i}$. Internal substate hyperarcs where both tail and head sets contain a
single vertex are added. These connect the start and end vertices of the
action paths (and the problem start and goal vertices) and represent feasible
paths within the task substate’s configuration space $\mathcal{W}_{T_{i}}$. A
hyperarc from an action end vertex to an action start vertex indicates that
there is a feasible path to transition from one action to another within
$\mathcal{W}_{T_{i}}$. These are the colored hyperarcs in Figure 11.

####  VII-C2 Transition-Extended Hypergraph

The transition history for multi-manipulator problems encodes all of the
actions that have occurred for the system to reach a particular vertex. In
Figure 12a, the transition history of the lower yellow vertex includes the
entire sequence of robot 1 picking the object, handing it off to robot 2, and
finally robot 2 placing the object.

A minimal hyperpath through this transition-extended hypergraph is an
optimistic solution. Each object and robot has a single continuous path from
its origin vertex to its terminal vertex. Each object’s terminal vertex
corresponds to its goal position. The paths are collision-free with respect to
static obstacles but might contain inter-path conflicts. The natural cost
function of the hyperpath captures any waiting time required for one robot
waiting on another to achieve the necessary pre-conditions to perform an
action.

###  VII-D Method

Here we discuss the application of the method to the multi-manipulator
problem. The task planning stage describes the instantiation of the cost-to-go
heuristic in the hyperpath query. The conflict resolution stage describes the
scheduled mutli-robot motion planning problem produced by the task planning
stage and the approach we implement to solve it.

####  VII-D1 Task Planning

Figure 18: This task graph is derived from the grounded hypergraph depicted in
Figure 11, and contains edges that change the object state. The source and
sink vertices in the task graph are the same as in the grounded hypergraph.
The orange (green) vertices are the robot 1 + object (robot 2 + object)
vertices in the grounded hypergraph. All grounded vertices with no object are
not included in the task graph.

The task planning stage utilizes the hypergraph query described in Section
V-B. The heuristic is adapted to the multi-manipulator problem by tailoring
the individual cost-to-go function $I(v)$ to the problem.

The individual cost-to-go function must provide a lower-bound for the given
vertex $v$ to reach a terminal state. As goal conditions only apply to objects
in this problem, all robot-only vertices may be terminal and thus have
$I(v)=0$ for all robot-only vertices. For any vertex $v$ containing an object,
$I(v)$ must capture the minimum cost-to-go to a satisfying goal state.

This can be done by constructing a task graph from the grounded hypergraph
(Fig. 18). It contains only vertices with at least one goal condition variable
in their task substate (i.e. vertices with an object). All hyperarcs become
directed edges.

We run a Dijkstra search rooted at the sink vertex in the task graph and
consider all edges in reverse. The individual cost-to-go $I(v)$ for any vertex
$v$ in the transition-extended hypergraph containing an object is given by the
cost-to-sink of the task graph vertex derived from its corresponding grounded
vertex.

As we are looking for a lower-bound cost-to-go from an independent vertex, the
transition history is not needed. We assume that all necessary pre-conditions
are met before the path from this vertex takes an action, and all other goal
conditions are satisfied by other actions occurring in parallel. This is the
absolute minimum remaining cost to the terminal state if the vertex in
question if is included in the plan.

####  VII-D2 Conflict Resolution

The conflict resolution stage resolves any conflicts between substate paths in
the optimistic solution. If a decoupled, single shot approach is desired, any
conflict resolution method can be used. In order to use the Next-Best Search
hybrid approach, a conflict resolution method must provide (representation)
optimal solutions.

We adapt CBS-MP [11] to perform scheduled multi-robot motion planning. Each
hyperarc in the transition-extended hyperpath (i.e. actions or robot path
between actions) is represented as a task in the schedule. Each task has
precedent constraints dependent on the hyperarcs immediately preceding it in
the hyperpath. For example, the pick action in Figure 12a between the top
yellow and second red vertex corresponds to a task in the schedule. This task
can only be started once the first robot 1 motion task (the first red
hyperarc) is completed. Likewise, the orange hyperarc following the pick
action cannot be started until the pick action is complete.

The scheduled CBS-MP finds and resolves conflicts between paths for all tasks
until discovering the optimal solution. The conflict resolution must employ
the same cost metric as the task planning stage which in our case is makespan.

##  VIII Representation Analysis

In many applications, the graph representation of the full task space is too
large to ever use explicitly. Existing methods use heuristics tailored to
their problem to greedily search an implicit representation of the graph [10].
In constrast, if the hypergraph representation is small enough to build, it
allows the use of more generic heuristics compute from the hypergraph itself
such as the cost-to-go heuristic discussed in Section V-B2. In this section,
we compare representing the task space with the traditional graph approach to
the proposed hypergraph approach for the multi-manipulator problems discussed
in Section VII. We show that the size of the hypergraph representation scales
well with the size of the problem and thus is able to be constructed and used
in heuristics.

We consider the pick, place, handoff action space, and assume that all robots
can reach each other to perform handoffs and all objects to perform pick/place
actions. This is an overestimate as it ignores reachability constraints. It
still serves as an informative upperbound on the space complexity as a
planning method must still reason over reachability before eliminating
potential transitions.

###  VIII-A Graph Representation

We first consider a graphical representation of the task space. For
simplicity, we ignore specific grasp constraints parameters or object poses
and just consider allocation of objects to robots.

###  VIII-B Vertex Constraints

A task state consists of a set of robots $n$ robots and $m$ objects. We
represent a task state as an allocation matrix $A^{n\text{x}m}$ where
$A_{ij}=1$ indicates that robot $r_{i}$ is holding object $o_{j}$ (14).

$\displaystyle A_{i,j}\in\\{0,1\\},\text{ }i\in 1:n,j\in 1:m$ (14)

Each object is either grasped by a single robot or on a stable surface (15).

$\displaystyle\sum^{n}_{i=1}A_{i,j}\leq 1,\text{ }j\in[1,m]$ (15)

Each robot is either holding a single object or not holding anything (16).

$\displaystyle\sum^{m}_{j=1}A_{i,j}\leq 1,\text{ }i\in[1,n]$ (16)

###  VIII-C Edge Constraints

The robots can perform pick, place, or handoff actions. For a pick action to
occur between a robot $r_{i}$ and an object $o_{j}$, $r_{i}$ must not be
holding any object (17) and $o_{j}$ must not be held by any robot (18).

$\displaystyle\sum^{m}_{l=1}A_{il}=0$ (17)
$\displaystyle\sum^{n}_{k=1}A_{kj}=0$ (18)

After performing the pick action, $r_{i}$ will be holding $o_{j}$ (19).

$\displaystyle A_{ij}=1$ (19)

Place actions have the reverse sequence of constraints. Prior to a robot
$r_{i}$ placing an object $o_{j}$, $r_{i}$ must be holding $o_{j}$ (19). After
performing the place action, $r_{i}$ will not be holding any object (17), and
$o_{j}$ will not be held by any robot (18).

In order to perform a handoff of object $o_{j}$ from robot $r_{i}$ to robot
$r_{k}$, $r_{i}$ must first be holding $o_{j}$ (20). Additionally, $r_{k}$
must not be holding any object (21).

$\displaystyle A_{ij}=1$ (20) $\displaystyle\sum^{m}_{l=0}A_{kl}=0$ (21)

After performing the handoff, $r_{i}$ will not be holding anything (22), and
$r_{k}$ will be holding $o_{j}$ (23).

$\displaystyle\sum^{m}_{l=0}A_{il}=0$ (22) $\displaystyle A_{kj}=1$ (23)

A valid edge exists between any two states $A^{1},A^{2},A^{1}\neq A^{2}$, if
and only if, each object is handled by at most two robots (24), and each robot
handles at most one object (25).

$\displaystyle\sum^{n}_{i=1}A^{1}_{ij}+A^{2}_{ij}\leq 2,\text{ }j\in[1,m]$
(24) $\displaystyle\sum^{m}_{j=1}|A^{1}_{ij}-A^{2}_{ij}|\leq 1,\text{
}i\in[1,n]$ (25)

###  VIII-D Total Number of Vertices

Let $P_{\text{max}}=\texttt{min}(m,n)$ be the maximum number of objects that
can be held in a scene with $n$ robots and $m$ objects. Let
$p\in\\{0,...,P_{\text{max}}\\}$ be the number of objects currently held in a
given task state $A^{n\text{x}m}$. Then the number of unique sets of $p$
objects that are held in a given task state is $m\choose p$. The number of
unique sets of $p$ robots holding those objects in a given task state is
$n\choose p$. Considering all possible assignments of the held objects to the
robots and the different values of $p$, the total number of states
$\mathcal{S}$ is given in (26).

$\displaystyle\mathcal{S}(m,n)=\sum^{P_{\text{max}}}_{p=0}{m\choose
p}{n\choose p}p!$ (26)

###  VIII-E Total Number of Edges

We first consider the set of available place actions. Let $l\in\\{0,..,p\\}$
be the number of objects placed in a given transition. Then the number of
unique sets of objects placed in a given transition is $p\choose l$. The total
number of transitions just performing a place actions is given by (27) where
the 1 is subtracted for the case $l=0$ and no transition occurs.

$\displaystyle-1+\sum^{p}_{l=0}{p\choose l}$ (27)

Let $n-p$ be the number of free robots in a given state. Let
$H_{\text{max}}=\texttt{min}(p-l,n-p)\\}$ be the maximum number of objects
handed off in a given transition. Let $h\in\\{0,...,H_{\text{max}}\\}$ be the
number of objects handed off in a given transition. Then the number of unique
sets of objects being handed off in a given transition is ${p-l}\choose h$,
and the number of unique robots receiving the handoffs is ${n-p}\choose h$.
Considering all possible assignments, the total number of transitions just
performing place and handoff actions is given by (28) where the 1 is
subtracted for the case $l=0,h=0$ and no transition occurs.

$\displaystyle-1+\sum^{p}_{l=0}{p\choose
l}\sum^{H_{\text{max}}}_{h=0}{{p-l}\choose h}{{n-p}\choose h}h!$ (28)

Let $m-p$ be the number of free objects in a given state. Let $n-p-h$ be the
number of robots not holding an object in a state and not receiving an object
via a handoff in a given transition. Let
$G_{\text{max}}=\texttt{min}(m-p,n-p-h)$ be the maximum number of objects
picked in a given transition. Let $g\in\\{0,G_{\text{max}}\\}$ be the number
of objects picked in a given transition. Then the number of unique sets of
objects being picked is ${m-p}\choose g$, and the the number of unique sets of
robots picking is ${n-p-h}\choose g$. Considering all possible assignments,
the total number of transitions from a given state performing a set of handoff
actions is given by (29) where the 1 is subtracted for the case $l=0,h=0,g=0$
and no transition occurs.

$\displaystyle\mathcal{G}(m,n,p,h)=-1+\sum^{G_{\text{max}}}_{g=0}{{m-p}\choose
g}{{n-p-h}\choose g}g!$ (29)

Let $\mathcal{H}$ be the total number of handoff and grasp action combinations
possible given $m,n,p,$ and $l$, and let $\mathcal{L}$ be the total number of
transitions possible consisting of a combination of place, handoff, and pick
actions given $m,n,$ and $p$.

$\displaystyle\mathcal{H}(m,n,p,l)=\sum^{H_{\text{max}}}_{h=0}{{p-l}\choose
h}{{n-p}\choose h}h!\mathcal{G}(m,n,p,h)$ (30)
$\displaystyle\mathcal{L}(m,n,p)=\sum^{p}_{l=0}{p\choose
l}\mathcal{H}(m,n,p,l)$ (31)

The total number of edges $\mathcal{T}$ in the graph is given by the total
number of edges from each of the vertices (32).

$\displaystyle\mathcal{T}(m,n)=\sum^{P_{\text{max}}}_{p=0}{m\choose
p}{n\choose p}p!\mathcal{L}(m,n,p)$ (32)

###  VIII-F Hypergraph Representation

The hypergraph representation considers task substates rather than entire task
states. The set of substates consists of every pairing of a robot grasping an
object, each robot not holding an object, and each object not being grasped
for a total of $mn+m+n$ vertices.

Each pick action is a transition from a robot substate vertex and an object
vertex to the substate where that robot holds that object. A place action is
this transition in reverse. This results in $2mn$ transition hyperarcs for all
pick place actions.

Each handoff action is a transition from a pair of vertices where robot 1 is
grasping the object and robot 2 is free to a pair of vertices where robot 1 is
free and robot 2 is grasping the object. Each of the $mn$ grasping substate
vertices can be paired with any of the $n-1$ free robot vertices for a total
of $mn^{2}-mn$ handoff hyperarcs. This results in a total of $mn^{2}+mn$
hyperarcs to capture all possible transitions.

###  VIII-G Comparison

In Figure 19, the graph representation maintains linear growth in the number
of vertices when considering either a single robot or a single object (bottom
red in both (a) and (b)), but exhibits exponential growth as soon there are
both multiple robots and multiple objects.

In contrast, in the hypergraph representation, the number of vertices scales
linearly with the number of robots while holding the number of objects
constant, regardless of the number of objects. The same is true if the number
of objects changes and the number of robots is held constant. This can be seen
in both Figure 19a (robots) and Figure 19b (objects).

(a) States - Robots

(b) States - Objects

Figure 19: The Y axis represents the total number of task (sub)states. The
dashed red lines show the number of task states represented as vertices in the
graph representation, and the blue lines show the number of task substates
represented as vertices in the hypergraph representation. (a) The X axis
represents the number of robots. Each line of the same color indicates one
more object. (b) The X axis represents the number of objects. Each line of the
same color indicates one more robot.

The number of transitions follows a similar trend. The bottom line of either
plot in Figure 20 is equal for graph and hypergraph representations for both
increasing objects and robots. As soon as a second object (Fig. 20a) or a
second robot (Fig. 20b) is introduced, the growth of the number of state
transitions (edges) in the graph representation becomes exponential. In the
hypergraph representation, the number of hyperarcs scales quadratically with
the number of robots and linearly with the number of objects. This follows
directly from the total number of hyperarcs discussed in Section VIII-F.

(a) Transitions - Robots

(b) Transitions - Objects

Figure 20: The Y axis represents the total number of (sub)state transitions.
The dashed red lines show the number of task state transitions represented as
edges in the graph representation, and the blue lines show the number of task
substate transitions represented as hyperarcs in the hypergraph
representation. (a) The X axis represents the number of robots. Each line of
the same color indicates one more object. (b) The X axis represents the number
of objects. Each line of the same color indicates one more robot.

To provide an intuitive visualization of the difference in representation
size, we depict the graph and hypergraph representation for 2 robots and 4
objects along with a table showing the immediate size difference in smaller
problems (Fig. 2).

##  IX Experimental Evaluation

We evaluate the MM-DaSH method described in Section VII on a set of physical
and simulated multi-robot manipulation problems depicted in Figures 1, 22. In
all scenarios, the objective is to move the given objects from their
respective start locations to specified goal locations. Robots can perform
pick, place, and handoff actions. We compare the performance of our method to
SMART [10], and we include a set of experiments similar to the ones performed
in that work. As SMART is an anytime algorithm, we record the time to compute
an initial solution.

All experiments were run using a desktop computer with an Intel Core
i9-10900KF CPU at 3.7 GHz, 128 GB of RAM. Both our method and SMART [10] were
implemented in C++ in the Parasol Planning Library. SMART was adapted to
account for action paths to perform task state switches instead of single
configuration switches to address the added complexity of using a two finger
gripper instead of the vacuum style gripper used in the original SMART
experiments.

###  IX-A Sorting

This scenario imitates sorting tasks that may be encountered in warehouses or
factories. A set of objects lies in a common workspace for a set of robots.
Each object belongs to a particular class. The team of robots must sort the
objects from the mixed group in the common workspace into bins of the
corresponding class. In a full industry application, a computer vision
component would be utilized to identify object classes and poses, but we focus
only on the planning portion here, and assume knowledge of this information.
Our objects are simple cubes, and the class is denoted by the color in Figure
1.

The comparison of our approach to SMART [10] is given in Figures 21a, 21b, and
the break down of the task planning and motion planning in our method is given
in Figures 21c, 21d.

####  IX-A1 Two Robot Lab Demo

In this physical demonstration, we consider two UR5e manipulators with hand-e
grippers which must sort the blocks by color (Fig. 1). We evaluate the runtime
of our method as the number of objects increases.

(a) Two Robot Lab Demo Runtimes

(b) Four Robot Sorting Runtimes

(c) Two Robot Lab Demo Runtime Breakdown

(d) Four Robot Sorting Runtime Breakdown

Figure 21:  In all four plots, the X axis denotes the number of objects. The
Y-axis denotes the runtime in seconds. Figures (a,b) show the runtimes of MM-
DaSH and SMART [10] for the sorting scenarios. In both cases, DaSH-MP
demonstrates faster runtimes. Figures (c,d) show the breakdown of the runtime
of DaSH-MP into task planning and motion planning. In both scenarios, task
planning becomes the limiting factor as the number of robots and objects
increase.

####  IX-A2 Four Robot Scenario

In this simulated scenario, a group of four robots again must sort an
increasing number of blocks. This is an expansion of the Two Robot Scenario
with the robots arranged around a square table. There are four classes of
objects, one for each robot. This evaluates the performance of the method as
the number of robots increases and how the relationship the number of robots
has on the performance of the method as the number of objects increases as
well.

####  IX-A3 Analysis

The runtimes for both MM-DaSH and SMART [10] are shown in Figures 21a, 21b. In
both scenarios, MM-DaSH is able to find a solution in less time, and we are
able to handle scenarios with more objects than SMART.

A breakdown of the runtime of MM-DaSH into task planning and motion planning
is given in Figures 21c, 21d. The task planning becomes the bottleneck as the
number of objects increases. The variance in the total runtime comes primarily
from the motion planning (though the task planning variance increases as the
number of robots increase). This is expected as the motion planning adapts
CBS-MP [11] which exhibits high variance. This comes from the randomness in
the roadmap generation resulting in varying difficulties in conflict
resolution as some roadmaps contain hard to find solutions.

###  IX-B Rearrangement

We also performed a set of rearrangement experiments inspired by the original
experiments in the SMART work [10]. In these scenarios, a set of objects is
distributed across two tables as shown in Figure 22. A set of robots is
distributed in the space between the two tables. The robots must move each
object from its originating table, to the other table. Different initial
distributions of the objects are considered.

The comparison of MM-DaSH to SMART [10] is given in Figures 23a, 23b. The
breakdown of the task and motion planning in MM-DaSH is given in Figures 23c,
23d.

Figure 22: The manipulators must transfer the objects from their starting
table to opposite table. No robot can reach both tables. Each robot can reach
at least one object. Some objects can be reached by multiple robots, and the
planner must decide which robots should move which objects.

(a) Swap Runtimes

(b) Relay Runtimes

(c) Swap Runtime Breakdown

(d) Relay Runtime Breakdown

Figure 23: In all four plots, the X axis denotes the number of robots and the
number of objects (R-O). The Y-axis denotes the runtime in seconds. Figures
(a,b) show the runtimes of MM-DaSH and SMART [10] for the Swap and Relay
scenarios. In both cases our proposed method demonstrates faster runtimes
except in the 2 robot, 6 object Relay scenario. Figures (c,d) show the break
down of the runtime of our proposed method into task planning and motion
planning.

####  IX-B1 Swap

In this scenario, half of the objects start on either table and must be moved
to the other table. No robot can reach both tables, so handoffs are required
to move objects to goal locations. We evaluate this with increasing robots and
objects.

####  IX-B2 Relay

In this scenario, all of the objects start on one table and must be moved to
the other table. The robot placement and tables are the same as the Swap
scenario. We evaluate this with increasing numbers of robots and objects.

####  IX-B3 Analysis

The runtimes for both MM-DaSH and SMART [10] are given in Figures 23a, 23b. We
show improved performance in the Swap scenario as SMART fails to plan for 6
objects in the 2 robot scenario. SMART does outperform our method for the 2
robot - 6 object Relay scenario. This is not surprising as the one way flow
structure of the relay problem (all objects moving from the first table to the
second) lends itself naturally to the heuristics in SMART.

A breakdown of the runtimes for our proposed method into task planning and
motion planning is given in Figures 23c, 23d. Notice again that the task
planning becomes the bottleneck as the number of objects increase. The
variance again comes primarily from the motion planning.

The runtimes are mostly consistent for our proposed method across both the
Swap and Relay scenarios. This demonstrates that the performance, particularly
the task planning performance is not dependent on the underlying task flow the
way that tailored heuristics (like in SMART) are likely to be.

##  X Conclusion

In this paper, we present the Decomposable State Space Hypergraph (DaSH)
method, a general task and motion planning framework which considers varying
levels of coupled and decoupled spaces. The framework enables tailoring of the
space coupling to the coordination required for different problem stages
rather than a static trade off between coordination and planning speed. We
provide a decomposition of task and motion planning spaces into subspaces and
present a hypergraph representation capable of representing the decomposed
space. We present a method for leveraging this hypergraph representation to
solve task and motion planning problems.

We illustrate the application of the general approach to the multi-robot
motion planning problem (MRMP-DaSH) and the multi-manipulator problem (MM-
DaSH). We provide an analysis of the representation for the multi-manipulator
problem and empirically evaluate our approach against a state-of-the-art
multi-manipulator task and motion planning method where we show significant
improvement in planning.

The approach still has limits to the problem size it is able to plan for as a
result of the size of task planning space as the number of robots and objects
increase. Prior work on hierarchically composing multi-robot motion planning
problems HC-CBS [30] may be extendable to the task planning space. HC-CBS has
the added benefit of being parallelizable (PHC-CBS) which can help handle
larger problems.

The multi-manipulator problem discussed here has a decomposition with a
hypergraph small enough to be explicitly constructed, but this is not be the
case for all task and motion planning problems. Future work should consider
how to reason over implicit representations of the decomposed space.

##  XI Acknowledgements

We would like to acknowledge the other members of the Parasol Lab at the
University of Illinois Urbana-Champaign (UIUC) for their suggestions regarding
this work. In particular, we are grateful to Isaac Ngui and Isaac Burton Love
for their input and discussion and to Hannah Lee for the creation of the
figures in this manuscript. This work was performed with support from Foxconn
Interconnect Technology (FIT) and the Center for Networked Intelligent
Components and Environments (C-NICE) at UIUC.

## References

  * [1] J. T. Schwartz and M. Sharir, “On the “piano movers” problem. ii. general techniques for computing topological properties of real algebraic manifolds,” _Advances in applied Mathematics_ , vol. 4, no. 3, pp. 298–351, 1983.
  * [2] J. F. Canny, _The Complexity of Robot Motion Planning_. Cambridge, MA: MIT Press, 1988.
  * [3] J. Motes, R. Sandström, H. Lee, S. Thomas, and N. M. Amato, “Multi-robot task and motion planning with subtask dependencies,” _IEEE Robotics and Automation Letters_ , vol. 5, no. 2, pp. 3338–3345, 2020.
  * [4] K. Brown, O. Peltzer, M. A. Sehr, M. Schwager, and M. J. Kochenderfer, “Optimal sequential task assignment and path finding for multi-agent robotic assembly planning,” in _2020 IEEE International Conference on Robotics and Automation (ICRA)_. IEEE, 2020, pp. 441–447.
  * [5] C. Henkel, J. Abbenseth, and M. Toussaint, “An optimal algorithm to solve the combined task allocation and path finding problem,” in _Proc. IEEE Int. Conf. Intel. Rob. Syst. (IROS)_ , 2019.
  * [6] C. R. Garrett, T. Lozano-Pérez, and L. P. Kaelbling, “FFRob: An efficient heuristic for task and motion planning,” in _Alg. Found. Robot. XI_. Springer, 2015, pp. 179–195, (WAFR ‘2014).
  * [7] K. Hauser and J.-C. Latombe, “Integrating task and prm motion planning: Dealing with many infeasible motion planning queries,” in _ICAPS09 Workshop on Bridging the Gap between Task and Motion Planning_. Citeseer, 2009.
  * [8] A. Dobson and K. E. Bekris, “Planning representations and algorithms for prehensile multi-arm manipulation,” in _2015 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS)_. IEEE, 2015, pp. 6381–6386.
  * [9] R. Shome and K. E. Bekris, “Anytime multi-arm task and motion planning for pick-and-place of individual objects via handoffs,” in _2019 International Symposium on Multi-Robot and Multi-Agent Systems (MRS)_. IEEE, 2019, pp. 37–43.
  * [10] ——, “Synchronized multi-arm rearrangement guided by mode graphs with capacity constraints,” in _International Workshop on the Algorithmic Foundations of Robotics_. Springer, 2020, pp. 243–260.
  * [11] I. Solis, J. Motes, R. Sandström, and N. M. Amato, “Representation-optimal multi-robot motion planning using conflict-based search,” _IEEE Robotics and Automation Letters_ , vol. 6, no. 3, pp. 4608–4615, 2021.
  * [12] L. Cohen, T. Uras, T. S. Kumar, and S. Koenig, “Optimal and bounded-suboptimal multi-agent motion planning,” in _Twelfth Annual Symposium on Combinatorial Search_ , 2019.
  * [13] T. Lozano-Pérez and M. A. Wesley, “An algorithm for planning collision-free paths among polyhedral obstacles,” _Communications of the ACM_ , vol. 22, no. 10, pp. 560–570, Oct. 1979.
  * [14] L. E. Kavraki, P. Švestka, J. C. Latombe, and M. H. Overmars, “Probabilistic roadmaps for path planning in high-dimensional configuration spaces,” _IEEE Trans. Robot. Automat._ , vol. 12, no. 4, pp. 566–580, Aug. 1996.
  * [15] G. Sanchez and J.-C. Latombe, “Using a prm planner to compare centralized and decoupled planning for multi-robot systems,” in _Proc. IEEE Int. Conf. Robot. Autom. (ICRA)_ , vol. 2, 2002, pp. 2112–2119.
  * [16] K. Solovey, O. Salzman, and D. Halperin, “Finding a needle in an exponential haystack: Discrete rrt for exploration of implicit roadmaps in multi-robot motion planning,” _The International Journal of Robotics Research_ , vol. 35, no. 5, pp. 501–513, 2016.
  * [17] M. Saha and P. Isto, “Multi-robot motion planning by incremental coordination,” in _2006 IEEE/RSJ International Conference on Intelligent Robots and Systems_. IEEE, 2006, pp. 5960–5963.
  * [18] C. R. Garrett, R. Chitnis, R. Holladay, B. Kim, T. Silver, L. P. Kaelbling, and T. Lozano-Pérez, “Integrated task and motion planning,” _Annual review of control, robotics, and autonomous systems_ , vol. 4, pp. 265–293, 2021\.
  * [19] K. Hauser and J.-C. Latombe, “Multi-modal motion planning in non-expansive spaces,” _Int. J. Robot. Res._ , vol. 29, no. 7, pp. 897–915, 2010.
  * [20] K. Hauser and V. Ng-Thow-Hing, “Randomized multi-modal motion planning for a humanoid robot manipulation task,” _The International Journal of Robotics Research_ , vol. 30, no. 6, pp. 678–698, 2011.
  * [21] J. Barry, L. P. Kaelbling, and T. Lozano-Pérez, “A hierarchical approach to manipulation with diverse actions,” in _2013 IEEE International Conference on Robotics and Automation_. IEEE, 2013, pp. 1799–1806.
  * [22] S. M. LaValle, _Planning Algorithms_. Cambridge University Press, 2006.
  * [23] R. Shome, “The problem of many: Efficient multi-arm, multi-object task and motion planning with optimality guarantees,” Ph.D. dissertation, Rutgers The State University of New Jersey, School of Graduate Studies, 2020.
  * [24] I. Umay, B. Fidan, and W. Melek, “An integrated task and motion planning technique for multi-robot-systems,” in _2019 IEEE International Symposium on Robotic and Sensors Environments (ROSE)_. IEEE, 2019, pp. 1–7.
  * [25] R. Shome, K. Solovey, A. Dobson, D. Halperin, and K. E. Bekris, “drrt*: Scalable and informed asymptotically-optimal multi-robot motion planning,” _Autonomous Robots_ , vol. 44, no. 3, pp. 443–467, 2020.
  * [26] G. Gallo, G. Longo, S. Pallottino, and S. Nguyen, “Directed hypergraphs and applications,” _Discrete applied mathematics_ , vol. 42, no. 2-3, pp. 177–201, 1993.
  * [27] H. Ma, D. Harabor, P. J. Stuckey, J. Li, and S. Koenig, “Searching with consistent prioritization for multi-agent path finding,” in _Proceedings of the AAAI Conference on Artificial Intelligence_ , vol. 33, no. 01, 2019, pp. 7643–7650.
  * [28] G. Sharon, R. Stern, A. Felner, and N. Sturtevant, “Conflict-based search for optimal multi-agent path finding,” _AAAI (to appear)_ , 2012.
  * [29] G. Wagner and H. Choset, “Subdimensional expansion for multirobot path planning,” _Artificial Intelligence_ , vol. 219, pp. 1–24, 2015.
  * [30] H. Lee, J. Motes, M. Morales, and N. M. Amato, “Parallel hierarchical composition conflict-based search for optimal multi-agent pathfinding,” _IEEE Robotics and Automation Letters_ , vol. 6, no. 4, pp. 7001–7008, 2021\.