11institutetext: Department of Informatics, Bioengineering, Robotics, and
Systems Engineering, University of Genoa, Via Opera Pia 13, 16145, Genoa,
Italy. 11email<EMAIL_ADDRESS><EMAIL_ADDRESS><EMAIL_ADDRESS>
# Task Allocation for Multi-Robot Task and Motion Planning: a case for Object
Picking in Cluttered Workspaces

Hossein Karami 11 Antony Thomas 11 Fulvio Mastrogiovanni 11

###### Abstract

We present an AND/OR graph-based, integrated multi-robot task and motion
planning approach which (i) performs task allocation coordinating the activity
of a given number of robots, and (ii) is capable of handling tasks which
involve an a priori unknown number of object re-arrangements, such as those
involved in retrieving objects from cluttered workspaces. Such situations may
arise, for example, in search and rescue scenarios, while locating/picking a
cluttered object of interest. The corresponding problem falls under the
category of planning in clutter. One of the challenges while planning in
clutter is that the number of object re-arrangements required to pick the
target object is not known beforehand, in general. Moreover, such tasks can be
decomposed in a variety of ways, since different cluttering object re-
arrangements are possible to reach the target object. In our approach, task
allocation and decomposition is achieved by maximizing a combined utility
function. The allocated tasks are performed by an integrated task and motion
planner, which is robust to the requirement of an unknown number of re-
arrangement tasks. We demonstrate our results with experiments in simulation
on two Franka Emika manipulators.

###### Keywords:

Task and motion planning Manipulation planning Multi-robot system.

##  1 Introduction

Humans trivially perform complex manipulation tasks, such as picking up a tool
from a cluttered toolbox, or grabbing a book from a shelf by re-arranging
occluding objects. For us, as humans, these tasks seem to be routine, and they
do not require much thought. Yet, for robots, this is not the case. Such
complex manipulation tasks require symbolic reasoning to decide which objects
to re-arrange so as to reach a target object with motion planning to account
for the geometric feasibility of the discrete actions. This interaction
between symbolic reasoning and motion planning is the subject of integrated
Task and Motion Planning (TMP). Single-robot TMP has been an area of active
research. Yet, current approaches do not naturally account for the
capabilities afforded by the presence of multiple robots, such as tasks that
can be decomposed in a variety of ways, task allocation involving many robots,
or collision avoidance among robots and between each robot and the workspace.
A naive extension of single-robot TMP approaches to the multi-robot case would
have to treat the multi-robot system as a combined set of single-robot system,
which becomes computationally intractable as the number of robots increases.

TMP finds applications in a variety of areas. In search and rescue,
locating/picking an object of interest may require re-arranging cluttering
objects in debris. This presents two main challenges: (i) the amount of debris
to be re-arranged before being able to reach and pick the target object is
unknown beforehand, (ii) the debris should be safely re-arranged so that it
poses no further impediment to reaching the target. In this paper, we address
the above two challenges from a multi-robot perspective, that is, how multiple
robots can interact to achieve reaching a target object in clutter while (i)
and (ii) hold. As for challenge (i), in scenarios such as search and rescue,
one may not always be interested in finding optimal solutions. Therefore,
currently we are not concerned with optimality in terms of the number of
object re-arrangements or load-balancing among the involved robots. To tackle
challenge (ii), in this work we assume the availability of a safe region where
the objects to be re-arranged are placed so that they pose no further
challenge to reach the target object.

We present an approach for multi-robot integrated task and motion planning
which first allocates tasks to the available robots, and then plans via AND/OR
graphs a sequence of actions for the multiply decomposable tasks that are
optimal with respect to an available utility function for the multi-robot
system. As a case in point, we focus on re-arranging clutter in manipulation
tasks using multiple manipulators. Specifically, we consider a cluttered
table-top where different target objects are to be picked by the robots by re-
arranging occluding objects, as in Fig. 1. This requires task allocation among
the available robots, followed by a TMP method to complete the allocated
tasks. It is noteworthy that to achieve each task, that is, picking a target
object, the number of cluttering object re-arrangements is not known
beforehand. Though off-the-shelf Planning Domain Definition Language (PDDL)
[23] based planners are available for task planning, one needs additional
expertise to incorporate task-motion interaction that comply with the state-
space search of the planner. Moreover, integrating multi-robot capabilities
still remains a challenge. As it will be discussed in Section 3, we address
these challenges by encoding the task-level abstractions efficiently and
compactly within an AND/OR graph that grows iteratively to a tree.

Figure 1: Cluttered table-top with two target objects (in black) and other
objects in red. The multi-robot system consists of two Franka Emika
manipulators. The target objects are allocated to each robot or a single robot
depending on the utility function. Each target object is then picked by re-
arranging clutter via a TMP method.

##  2 Background

###  2.1 Related Work

Single-robot TMP is an area of research which has received considerable
attention in the literature, and spanning two main areas— TMP for manipulation
[8, 15, 32, 39, 4, 9] and TMP for navigation [33, 27, 22, 14, 37]. These
approaches combine symbolic-level action selection of task planning with the
continuous trajectory generation of motion planning [20]. Yet, such methods do
not consider the implication of having multiple robots in task allocation and
collision avoidance, and would have to treat the multi-robot system as a
combined single-robot system, which becomes intractable as the number of
robots increases.

Current TMP approaches for multi-arm robot systems focus on coordinated
planning strategies, and consider simple pick-and-place or assembly tasks. As
such, these methods do not scale to complex manipulation tasks [29, 40]. TMP
for multi-arm robot systems in the context of welding is considered in [1],
whereas [40] discusses an approach for multi-arm TMP manipulation. However,
the considered manipulation task is a simple pick-and-place operation
involving bringing an object from an initial position to a goal position,
where two tables and a cylindrical object form the obstacles. A centralized
inverse kinematics solver is employed in [24]. Motion planning for a multi-arm
surgical robot is presented in [28] using predefined motion primitives. Yet, a
fine tuning of such primitives towards real experimental platforms remains a
challenge.

TMP for multi-robot systems has not been addressed thoroughly, and therefore
the literature is not sufficiently developed. Henkel et al. [13] consider
multi-robot transportation problems using a Task Conflict-Based Search (TCBS)
algorithm. Such an approach solves a combined task allocation and path
planning problem, but assigns a single sub-task at a time and hence may not
scale well to an increased number of robots. Interaction Templates (IT) for
robot interactions during transportation tasks are presented in [25]. The
interactions enable handing over payloads from one robot to another, but the
method does not take into account the availability of robots and assumes that
there is always a robot available for such an handover. Thus, while
considering many tasks at a time this framework does not fare well since a
robot may not be immediately available for an handover. A distributed multi-
robot TMP method for mobile robot navigation is presented in [38]. However,
they define task-level actions for a pair of robots and therefore optimal
solutions are available for an even number of tasks, and only suboptimal
solutions are returned for an odd number of tasks. Motes et al. [26] present
TMP-CBS, a multi-robot TMP approach with sub-task dependencies. They employ a
CBS method [31] in the context of transportation tasks. Constructing a
conflict tree for CBS requires the knowledge of different constraints which
depend on the sub-task conflicts, for example, two robots being present at a
given location at the same time. However, in the table-top scenario considered
in this paper, the number of sub-tasks is not known beforehand. The
capabilities of the discussed methods are summarized in Table 1.

Considered | Task | Task | Motion | Unknown number
---|---|---|---|---
method | allocation | decompostion | planning | of sub-tasks
TCBS [13] | ✓ |  |  |
IT [25] | ✓ | ✓ |  |
[38] | ✓ |  | ✓ |
TMP-CBS [26] | ✓ | ✓ | ✓ |
Our | ✓ | ✓ | ✓ | ✓

Table 1: Comparison of different MR TMP methods.

###  2.2 Task-Motion Planning and AND/OR Graphs

Task planning or classical planning is the process of finding a discrete
sequence of actions from the current state to a desired goal state [12].

######  Definition 1.

A task domain $\Omega$ can be represented as a state transition system and is
a tuple $\Omega=\langle S,A,\gamma,s_{0},S_{g}\rangle$ where:

  * •

$S$ is a finite set of states;

  * •

$A$ is a finite set of actions;

  * •

$\gamma\mathrel{\mathop{\mathchar 58\relax}}S\times A\rightarrow S$ such that
$s^{\prime}=\gamma(s,a)$;

  * •

$s_{0}\in S$ is the start state;

  * •

$S_{g}\subseteq S$ is the set of goal states.

######  Definition 2.

The task plan for a task domain $\Omega$ is the sequence of actions
$a_{0},\ldots,a_{m}$ such that $s_{i+1}=\gamma(s_{i},a_{i})$, for
$i=0,\ldots,m$ and $s_{m+1}$ satisfies $S_{g}$.

Motion planning finds a sequence of collision free configurations from a given
start configuration to a desired goal [21].

######  Definition 3.

A motion planning domain is a tuple $M=\langle C,f,q_{0},G\rangle$ where:

  * •

$C$ is the configuration space;

  * •

$f=\\{0,1\\}$, for collision $f=0$, else $f=1$;

  * •

$q_{0}\in C$ is the initial configuration;

  * •

$G\in C$ is the set of goal configurations.

######  Definition 4.

A motion plan for $M$ finds a collision free trajectory in $C$ from $q_{0}$ to
$q_{n}\in G$ such that $f=1$ for $q_{0},...,q_{n}$. Alternatively, a motion
plan for $M$ is a function of the form $\tau\mathrel{\mathop{\mathchar
58\relax}}[0,1]\rightarrow C_{free}$ such that $\tau(0)=q_{0}$ and $\tau(1)\in
G$, where $C_{free}\subset C$ is the configurations where the robot does not
collide with other objects or itself.

TMP combines discrete task planning and continuous motion planning to
facilitate efficient interaction between the two domains. Below we define the
TMP problem formally.

######  Definition 5.

A task-motion planning with task domain $\Omega$ and motion planning domain
$M$ is a tuple $\Psi=\langle C,\Omega,\phi,\xi,q_{0}\rangle$ where:

  * •

$\phi\mathrel{\mathop{\mathchar 58\relax}}S\rightarrow 2^{C}$, maps states to
the configuration space;

  * •

$\xi\mathrel{\mathop{\mathchar 58\relax}}A\rightarrow 2^{C}$, maps actions to
motion plans.

######  Definition 6.

The TMP problem for the TMP domain $\Psi$ is to find a sequence of discrete
actions $a_{0},...,a_{n}$ such that $s_{i+1}=\gamma(s_{i},a_{i})$, $s_{n+1}\in
S_{g}$ and a corresponding sequence of motion plans $\tau_{0},...,\tau_{n}$
such that for $i=0,...,n$, it holds that (i) $\tau_{i}(0)\in\phi(s_{i})\
\textrm{and}\ \tau_{i}(1)\in\phi(s_{i+1})$, (ii) $\tau_{i+1}(0)=\tau_{i}(1)$,
and (iii) $\tau_{i}\in\xi(a_{i})$.

We now provide a brief overview of AND/OR graphs [5, 16]. An AND/OR graph is a
graph which represents a problem-solving process [3].

######  Definition 7.

An AND/OR graph $G$ is a directed graph represented by the tuple $G=\langle
N,H\rangle$ where:

  * •

$N$ is a set of nodes;

  * •

$H$ is a set of hyper-arcs.

For a given AND/OR graph $G$, we have that $H=\\{h_{1},\ldots,h_{m}\\}$, where
$h_{i}$ is a many-to-one mapping from a set of child nodes to a parent node.
In that sense, a hyper-arc induces a logical and relationship between the
child nodes/states, that is, all the child states should be satisfied to
achieve the parent state. Similarly, a single parent node can be the co-domain
for different hyper-arcs $h_{i}$. These hyper-arcs are in logical or with the
parent node. Nodes without any successors or children are called the terminal
nodes. The terminal nodes are either a success node or a failure node. We now
mathematically define an AND/OR graph network, a detailed treatment of which
can be found in [17].

######  Definition 8.

For an AND/OR graph $G=\langle N,H\rangle$, an augmented AND/OR graph $G^{a}$
is a directed graph represented by the tuple $G^{a}=\langle
N^{a},H^{a}\rangle$ where:

  * •

$N^{a}=\\{N,n^{v}\\}$ with $n_{v}$ being the virtual node;

  * •

$H^{a}=\\{H,H^{v}\\}$ with $H^{v}=\\{h^{v}_{i}\\}_{1\leq i\leq|H^{v}|}$.

######  Definition 9.

An AND/OR graph network $\Gamma$ is a directed graph
$\Gamma=\langle\mathcal{G},T\rangle$ where:

  * •

$\mathcal{G}=\\{G^{a}_{1},\ldots,G^{a}_{n^{\prime}}\\}$ is a set of augmented
AND/OR graphs $G^{a}_{i}$;

  * •

$T=\\{t_{1},\ldots,t_{n^{\prime}-1}\\}$ is a set of transitions such that
$G^{a}_{i+1}=t_{i}(G^{a}_{i}),\ 1\leq i\leq n^{\prime}-1$;

where $n^{\prime}$ is the total number of graphs in the network.
Alternatively, $n^{\prime}$ is also the depth of the network.

###  2.3 Problem Definition

We present a framework which (i) allocates $T$ manipulation tasks to a set of
$R$ robots, and (ii) plans for a sequence of motions-actions for each robot to
achieve the respective tasks. To this end, we consider a multi-robot setting
with a cluttered table-top wherein different target objects need to be picked
by re-arranging the clutter. Thus, each manipulation task corresponds to
picking up a target object.

In order to carry out each task, objects need to be re-arranged – depending on
the clutter degree – and the number of objects to be re-arranged is not known
beforehand. By representing the task-level abstractions within an AND/OR graph
network (see Section 3), we can overcome this issue.

##  3 Multi-robot Task and Motion Planning

(a)

(b)

(c)

(d)

Figure 2: Illustration of the obstacle selection method: (a) a cluttered
table-top scenario with two robots r1 (right), r2 (left) and the target
objects in black; (b) a valid grasping angle range is computed by discretizing
a fixed grasping angle range of $-\frac{\pi}{2}$ to $\frac{\pi}{2}$; the
objects that fall within the grasping angle range of r1 (one target) are shown
in blue; (c) blue objects within the grasping angle range of r1, considering
both the targets; (d) Objects within the grasping range of both r1 (blue) and
r2 (green), considering both the targets for each robot.

In this Section, we detail our multi-robot TMP method. We begin by describing
a heuristic approach providing a rough estimate of the objects to be re-
arranged to pick a target object. The approach allows us to define a combined
utility function for the multi-robot system to perform task allocation.
Allocated tasks are then carried out using a TMP method based on an AND/OR
graph network and a sampling based motion planner.

###  3.1 Obstacles Selection

An overview of the method can be seen in Figure 2. First, it finds different
feasible plans to the target, each one corresponding to different grasping
angles, by ignoring all the obstacles in the workspace. This is done by
discretising the set of graspable angles, that is
$\left[-\frac{\pi}{2},\frac{\pi}{2}\right]$ (the axis is located at the center
of the target). It is noteworthy that we consider only side grasps to make the
scenario more challenging. Among the available plans, the maximum and minimum
grasping angles, namely $\alpha$ and $\beta$, are then obtained. The method
then constructs two lines starting at the center of the target object and
towards the robot with the corresponding angles $\alpha$ and $\beta$. The
lines are terminated when there are no more obstacles along their paths. The
end-points are then joined to form a triangle. This triangle is then enlarged
on all the three sides by the radius of the bounding volume sphere of the end-
effector. The objects within the constructed triangle are the objects to be
re-arranged to facilitate the target grasp.

We note that what we describe here is an approximate method to identify the
objects to be re-arranged. The actual set of objects depend on other such
factors as the degrees of freedom of the robot, the size of the links and the
end-effector, or the degree of clutter. As it will be discussed later, for all
practical purposes we are interested only in an approximate measure so as to
perform multi-robot task allocation.

###  3.2 Task Allocation

Let $R$ be the number of available robots, and $T$ the number of tasks to be
allocated, that is, we have $T$ target objects to be picked, such that $T\geq
R$. Task allocation is performed offline and we assume that each task, that
is, picking up a target object from clutter, is performed by a single robot,
and that each robot is able to execute only one task at a time. We also recall
here our assumption from the Introduction that the objects to be re-arranged
to reach and pick a target are placed in a safe region. Our task allocation
strategy falls under the Single-Task, Single-Robot, Time-extended Assignment
(ST-SR-TA) taxonomy of Gerkey and Matarić [11], since the multi-robot system
contains more tasks than robots. In order to allocate tasks, we define
$U_{r_{i}t_{j}}$ as the utility function for a robot $r_{i}\in R$ executing a
task $t_{j}\in T$. In this work, utility is inversely proportional to the
number of object re-arrangements required to grasp the target object. To
determine such a measure, we first (randomly) select a target object $t_{1}$,
and then for each robot $r_{i}$ we run the obstacles selection algorithm
described above. For each $r_{i}$, such a run returns the set of objects to be
re-arranged to reach $t_{1}$. Let us denote this set by $O_{r_{i}t_{1}}$ (and
by $O_{r_{i}t_{j}}$ for the $j$th task). The robot $r_{k}$ whose set
$O_{r_{k}t_{1}}$ is of minimum cardinality (i.e., maximum utility) is then
allocated task $t_{1}$. For the next target $t_{2}$ this step is repeated. We
note here that $O_{r_{k}t_{2}}$ is computed offline and returns the number of
objects to be re-arranged by robot $r_{k}$ to execute task $t_{2}$. However,
it may be the case that some objects appear in both $O_{r_{k}t_{1}}$ and
$O_{r_{k}t_{2}}$, that is, $O_{r_{k}t_{1}}\cap
O_{r_{k}t_{2}}\neq\\{\emptyset\\}$. Since each robot executes one task at a
time, $r_{k}$ can execute $t_{2}$ only after having performed $t_{1}$. Thus,
during the execution phase, $r_{k}$ may have already removed the common
objects while executing $t_{1}$, and therefore these objects may be ignored to
avoid intra-robot double counting while computing the utility $U_{r_{k}t_{2}}$
offline.

Once each robot is allocated a task, the set of remaining tasks $T^{\prime}$
is completed only after the execution of the assigned tasks. For the remaining
$T^{\prime}$ tasks, the inter-robot or robot-robot double counting must be
considered. Reasoning in a similar manner for intra-robot double counting, the
set $O_{r_{i}t_{j}}$ restricted to $T\setminus T^{\prime}<j\leq T$ for the
remaining $T^{\prime}$ tasks may have common objects with respect to
$O_{r_{i}t_{j}}$ restricted to $1<j\leq T^{\prime}$ of the assigned tasks.
Thus, the total number of objects to be re-arranged for robot $r_{i}$ to
execute task $t_{j}$ is

$O_{r_{i}t_{j}}^{c}=|O_{r_{i}t_{j}}|-\sum_{k}|O_{r_{i}t_{j}t_{k}}|-\sum_{k}\sum_{l}|O_{r_{i}r_{k}t_{j}t_{l}}|$
(1)

where $|\cdot|$ denotes the cardinality of a set,

$O_{r_{i}t_{j}t_{k}}=\begin{cases}O_{r_{i}t_{j}}\cap O_{r_{i}t_{k}}\ &\text{if
$r_{i}$ allotted $t_{k}$ previously},\\\ 0\ &\text{otherwise}.\end{cases}$ (2)

and

$O_{r_{i}r_{k}t_{j}t_{l}}=\begin{cases}O_{r_{i}t_{j}}\cap O_{r_{k}t_{l}}\
&\text{if $r_{k}$ allotted $t_{l}$ previously},\\\ 0\
&\text{otherwise},\end{cases}$ (3)

with the terms $|O_{r_{i}t_{j}t_{k}}|$ and $|O_{r_{i}r_{k}t_{j}t_{l}}|$
modeling the intra-robot and inter-robot double counting, respectively. We
therefore have the following utility function

$U_{r_{i}t_{j}}=\frac{1}{1+O_{r_{i}t_{j}}^{c}}.$ (4)

The maximum utility of $U_{r_{i}t_{j}}=1$ is therefore achieved when no object
re-arrangement is required to execute task $t_{j}$, that is,
$O_{r_{i}t_{j}}^{c}=0$. Using the taxonomy in [18], we thus have In-schedule
Dependencies (ID) – the effective utility of an agent for a task depends on
what other tasks that agent is performing as well as Cross-schedule
Dependencies (XD) – the effective utility of an agent for a task depends not
only on its own task but also on the tasks of other agents.

We now define the combined utility of the multi-robot system, which consists
of maximising

$\sum_{i\in R}\sum_{j\in T}U_{r_{i}t_{j}}x_{r_{i}t_{j}}\ $ (5)
$\begin{split}&\textrm{such that}\,\sum_{i\in R}x_{r_{i}t_{j}}=1\\\
&\textrm{where}\,x_{r_{i}t_{j}}\in\\{0,1\\}.\\\ \end{split}$ (6)

From (4) and (5) we see that the robot with the minimum number of object re-
arrangements for a given task is thus assigned the maximum utility. In case of
a tie, we select the robot which has not been allocated any task. If all the
robots with the same utility have been allocated tasks already, or if none has
been allotted, then a robot is selected randomly.

###  3.3 Task Decomposition

Each manipulation task is decomposed into a set of sub-tasks which correspond
to pick-and-place tasks, that is, re-arrangement of the objects that hinder
the target grasp. As seen above, the number of sub-tasks for a given task are
not known beforehand. Moreover, multiple decomposability [41] is possible
since the clutter can be re-arranged in different ways. We seek a
decomposition minimizing the number of sub-tasks for the multi-robot system.
This can be achieved during task allocation since utility is computed based on
the obstacles selection method. In this work, we consider complex task
decomposition [41] – a multiply decomposable task for which there exists at
least one decomposition that is a set of multi-robot allocatable sub-tasks.
Though in this work we ignore the multi-robot allocatability property, this
can be incorporated trivially. For example, let us consider the case where two
robots have the same utility to perform a task $t_{j}$. In this case, the sub-
tasks can be equally divided to achieve multi-robot allocatability or one
robot may be selected randomly (or depending on previous allocation) to
perform the entire task.

###  3.4 Task and Motion Planning

PDDL [23] is the de facto standard for task planning, and most TMP approaches
resort to it. Integrated TMP requires a mapping between the task space and the
motion space. Semantic attachments are used in [7, 6, 4, 37] to associate
algorithms to functions and predicate symbols via external procedures.
However, it is assumed by these approaches that the workspace be known in
advance. Moreover, they reduce motion space to a finite space since the robot
configuration, the grasp poses, and other geometrical constraints need to be
specified in advance. This issue is addressed in [10] using streams, which
enable procedures within PDDL to sample values of continuous variables,
therefore encoding an infinite set of actions. Though off-the-shelf PDDL
planners are available, one needs additional expertise to incorporate semantic
attachments or streams complying with the planner semantics. Furthermore, for
the cluttered table-top scenario, the number of objects that need to be re-
arranged is not known beforehand. As a result, one would need observation
based re-planning within the PDDL framework, which require additional mapping
between the PDDL action space and the observation space of the robot [2].

A cluttered table-top scenario in the context of single-robot TMP is
considered in [36, 17]. To address the above mentioned challenges associated
with PDDL modeling, in [17] the task-level abstractions of the TMP problem is
efficiently and compactly encoded within an augmented AND/OR graph (see
Section 2) that grows iteratively until solutions are found, where each
iteration is meant at managing a sub-task as defined above.

In general, AND/OR graphs are constructed offline. However, the number of
objects to be re-arranged depends on the clutter degree and is not known
beforehand. [17] introduces AND/OR graph networks, consisting of augmented
AND/OR graphs which iteratively deepen at run-time till a solution to the
modeled problem is found. The key idea is that the set of task-level
abstractions, i.e., the states (nodes) and actions (hyper-arcs) of the AND/OR
graph defined for the robot remain the same irrespective of the number of re-
arrangements, i.e., sub-tasks. Therefore, one can define an initial AND/OR
graph encoding task-level actions, which is augmented with the initial
workspace configuration. The augmented graph is then iteratively expanded with
the updated workspace configuration as long as sub-tasks are carried out
resulting in a network of AND/OR graphs. In this work, we use this AND/OR
graph network based task planner and customize it for multi-robot task
planning. For motion planning, we simply use MoveIt [34], which supports RRT
[19] from OMPL [35]. The motion planner is first employed to execute the
obstacles selection algorithm. Once the tasks are allocated, the motion
planner is called to (i) achieve the re-arrangement of each sub-task
identified by the task planner (i.e., the AND/OR graph network) and (ii) to
grasp the target object.

###  3.5 The Multi-robot Task-Motion Planning Loop

Figure 3: System’s architecture.

Fig. 3 depicts the overall system’s architecture of our multi-robot TMP
method. Once the tasks have been allocated, Task Planner selects the abstract
actions whose geometric execution feasibility is checked by Motion Planner.
The Task Planner layer consists of the AND/OR Graphs module – the initial
augmented AND/OR graphs for the robots, and the Network Search module – the
search procedure iterating the initial augmented graphs. As discussed in
Section 3.4, the initial augmented AND/OR graph consists of the task-level
actions for each robot augmented with the current workspace configuration.
AND/OR Graphs provides a set of achievable transitions between the states to
Network Search, and receives the set of allowed states and transitions as the
graph is expanded. Task Planner then associates each state or state transition
with an ordered set of actions in accordance with the workspace and robot
configurations. The Knowledge Base module stores the information regarding the
current workspace configuration, that is, the objects and their locations in
the workspace as well as the robot configuration. This module augments the
graphs with the current workspace configuration to facilitate Network Search.
Since we consider here two robots and their respective configurations need to
be updated, we have two Network Search modules corresponding to each robot.

TMP interface acts as a bridge between the task planning and the motion
planning layers. It receives action commands from Task Planner, converts them
to their geometric values (for example, a grasping command requires various
geometric values such as the target pose or the robot base pose), and passes
them on to Motion Planner to check motion feasibility. To this end, the module
retrieves information regarding both the workspace and robots from Knowledge
Base. If an action is found to be feasible, it is then sent for execution.
Upon execution, Task Planner receives an acknowledgment regarding action
completion and the Knowledge Base is updated accordingly.

##  4 Experimental Results

Activity | Average [s] | Std. Dev. [s]
---|---|---
AND/OR Graphs | 0.03213 | 0.02174
Network Search | 0.8992 | 0.1862
Motion planner (attempts) | 18.320 | 6.450
Motion execution (attempts) | 6.3126 | 0.8176
Motion planner (time) | 0.8010 | 0.2170
Motion execution (time) | 4.7490 | 2.0240

Table 2: Average computation times for different modules.

We validate and demonstrate the performance of our multi-robot TMP approach by
performing experiments in the state-of-the-art robotics simulator CoppeliaSim
[30], employing two Franka Emika manipulators. As seen in Fig. 1, we consider
a cluttered table-top scenario where manipulation tasks correspond to picking
up different target objects. For each task, the sub-tasks consist of removing
objects hindering each target grasp. In this work, such objects are picked and
placed outside the working area, in a safe space. The number of objects to be
removed is unknown (and therefore likewise the number of sub-tasks) at
planning time. Experiments are conducted on a workstation equipped with an
Intel(R) core<EMAIL_ADDRESS>GHz $\times$ 12 CPU’s and 16 GB of RAM. The
architecture is developed using C++ and Python under ROS Kinetic. A video
demonstrating the results can be found at `https://youtu.be/nkB_LHorg1g`.

(a)

(b)

(c)

Figure 4: Various histograms with and increasing AND/OR graph network depth.

We begin with 6 objects in the workspace with two of them being target
objects. To test the scalability to an increasing number of objects, we
perform experiments with up to 64 objects. For a given number of objects the
experiment is conducted 3 times, and in each experiment the target objects are
chosen randomly. Table 2 reports the average combined planning and execution
times for robots $r_{1}$ and $r_{2}$, focusing on the architecture’s modules.
AND/OR graph and graph network search times are per number of grown graphs. It
must be noted that motion planning failures due to actuation errors or
grasping failures lead to re-plans, and therefore to a larger number of motion
planning attempts.

Table 3 report the average network depth $d$, the average total task planning
time, the average total motion planning time, the average number of motion
planning attempts, and the average number of objects to be re-arranged for
robots $r_{1}$ and $r_{2}$, respectively. As the AND/OR graph network depth
$d$ increases, task planning times are almost linear with respect to $d$. This
is so because for an AND/OR graph network with each graph consisting of $n$
nodes, the time complexity is only $O(nd)$ [17]. In contrast, PDDL-based
planners are characterized by a search complexity of $O(n\log n)$, where
$n\approx 2^{13}$ for the table-top scenario.

| $r_{1}$ | $r_{2}$
---|---|---
Objects | d | TP [s] | MP [s] | MP attempts | Objects re-arranged | TP [s] | MP [s] | MP attempts | Objects re-arranged
6 | 2.33 | 1.5505 | 16.339 | 36.66 | 1.66 | 1.1201 | 13.207 | 31.33 | 1.0
8 | 2.66 | 1.422 | 13.721 | 32.66 | 1.66 | 1.1478 | 18.154 | 28.66 | 1.0
9 | 2.33 | 1.3532 | 16.909 | 30.66 | 1.0 | 2.415 | 18.520 | 40.0 | 2.33
12 | 4 | 2.4025 | 20.3363 | 42.66 | 2.66 | 2.0532 | 17.825 | 37.66 | 2.33
16 | 4.66 | 2.755 | 20.1718 | 47.66 | 3.33 | 2.6326 | 22.830 | 45.66 | 3.33
20 | 3.66 | 2.125 | 17.618 | 42.33 | 2.66 | 4.022 | 25.182 | 61.33 | 5.33
30 | 5.75 | 3.464 | 25.854 | 56.75 | 4.25 | 4.748 | 45.226 | 83.75 | 5.75
49 | 17 | 8.642 | 81.361 | 147.0 | 6.33 | 6.886 | 49.593 | 106.66 | 10.66
64 | 22.6 | 9.053 | 132.179 | 203.0 | 12.2 | 5.271 | 66.994 | 109.875 | 7.0

Table 3: Statistics for robots $r_{1}$ and $r_{2}$. Legenda: $d$ \- average
AND/OR graph network depth, TP - average total task planning time, MP -
average total motion planning time.

Fig. 4 shows different histograms for an increasing network depth $d$. In
particular, Fig. 4(a) shows the total task planning time with $d$. One can
readily observes that the linearity in planning time is not strictly followed.
For example, the time for $d=25$ is greater than the time for $d=41$. In many
cases, due to motion planning failures a new graph is expanded before reaching
the terminal node, which implies that more nodes are traversed for $d=25$
compared to $d=41$, therefore explaining the variations. Fig. 4(b) plots the
number of motion planning attempts and the total executions with an increasing
$d$. An increase in $d$ in most cases correspond to a higher degree of
clutter. Therefore, as depth $d$ or the graphs increase the motion planning
attempts increases as well. However, motion planning failures can also
increase the depth $d$ since a new graph need to be expanded. This explains
the slight deviation in the trend. Fig. 4(c) reports the total motion planning
and execution times with an increasing $d$, and the plot readily follows from
the discussions above.

##  5 Conclusion

We present a task allocation approach for integrated multi-robot task and
motion planning capable of handling tasks with an unknown number of sub-tasks.
Our obstacles selection strategy combined with the introduced utility function
allocates task to the available robots. Allocated tasks are then achieved by
our TMP method, which comprises an AND/OR graph network based task planner
that is robust to the issue of an unknown number of sub-tasks. We show that
our method is capable of handling varying degree of clutter, i.e., the number
of objects in the workspace. Currently, the objects hindering each tasks are
removed from the workspace. As a result, subsequent tasks need not consider
theses objects while computing the utility. There may be situations in which
removing objects from the workspace will not be possible, and the planner will
have to select free space for object placement. Future work will seek to
develop a non-myopic technique to predict possible future trajectories based
on the previous ones and the current environment configuration so as to enable
the selection of smart object placements.

## References

  * [1] Basile, F., Caccavale, F., Chiacchio, P., Coppola, J., Curatella, C.: Task-oriented motion planning for multi-arm robotic systems. Robotics and Computer-Integrated Manufacturing 28(5), 569–582 (2012)
  * [2] Bertolucci, R., Capitanelli, A., Dodaro, C., Leone, N., Maratea, M., Mastrogiovanni, F., Vallati, M.: Manipulation of articulated objects using dual-arm robots via answer set programming. Theory and Practice of Logic Programming 21(3), 372–401 (2021)
  * [3] Chang, C.L., Slagle, J.R.: An admissible and optimal algorithm for searching AND/OR graphs. Artificial Intelligence 2(2), 117–128 (1971)
  * [4] Dantam, N.T., Kingston, Z.K., Chaudhuri, S., Kavraki, L.E.: Incremental task and motion planning: A constraint-based approach. In: Robotics: Science and Systems (2016)
  * [5] Darvish, K., Simetti, E., Mastrogiovanni, F., Casalino, G.: A hierarchical architecture for human–robot cooperation processes. IEEE Transactions on Robotics , to appear (2020)
  * [6] Dornhege, C., Eyerich, P., Keller, T., Trüg, S., Brenner, M., Nebel, B.: Semantic Attachments for Domain-Independent Planning Systems. In: International Conference on Automated Planning and Scheduling (ICAPS). pp. 114–121. Thessaloniki, Greece (September 2009)
  * [7] Dornhege, C., Gissler, M., Teschner, M., Nebel, B.: Integrating symbolic and geometric planning for mobile manipulation. In: Safety, Security & Rescue Robotics (SSRR), IEEE International Workshop on. pp. 1–6. IEEE (2009)
  * [8] Erdem, E., Haspalamutgil, K., Palaz, C., Patoglu, V., Uras, T.: Combining high-level causal reasoning with low-level geometric reasoning and motion planning for robotic manipulation. In: 2011 IEEE International Conference on Robotics and Automation. pp. 4575–4581. IEEE (2011)
  * [9] Garrett, C.R., Lozano-Perez, T., Kaelbling, L.P.: FFRob: Leveraging symbolic planning for efficient task and motion planning. The International Journal of Robotics Research 37(1), 104–136 (2018)
  * [10] Garrett, C.R., Lozano-Pérez, T., Kaelbling, L.P.: Pddlstream: Integrating symbolic planners and blackbox samplers via optimistic adaptive planning. In: Proceedings of the International Conference on Automated Planning and Scheduling. vol. 30, pp. 440–448 (2020)
  * [11] Gerkey, B.P., Matarić, M.J.: A formal analysis and taxonomy of task allocation in multi-robot systems. The International Journal of Robotics Research 23(9), 939–954 (2004)
  * [12] Ghallab, M., Nau, D., Traverso, P.: Automated planning and acting. Cambridge University Press (2016)
  * [13] Henkel, C., Abbenseth, J., Toussaintl, M.: An optimal algorithm to solve the combined task allocation and path finding problem. In: 2019 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS). pp. 4140–4146. IEEE (2019)
  * [14] Jiang, Y., Yang, F., Zhang, S., Stone, P.: Task-Motion Planning with Reinforcement Learning for Adaptable Mobile Service Robots. In: IROS. pp. 7529–7534 (2019)
  * [15] Kaelbling, L.P., Lozano-Pérez, T.: Integrated task and motion planning in belief space. The International Journal of Robotics Research 32(9-10), 1194–1227 (2013)
  * [16] Karami, H., Darvish, K., Mastrogiovanni, F.: A task allocation approach for human-robot collaboration in product defects inspection scenarios. In: 2020 29th IEEE International Conference on Robot and Human Interactive Communication (RO-MAN). pp. 1127–1134. IEEE
  * [17] Karami, H., Thomas, A., Mastrogiovanni, F.: A Task-Motion Planning Framework Using Iteratively Deepened AND/OR Graph Networks. arXiv preprint arXiv:2104.01549 (2021)
  * [18] Korsah, G.A., Stentz, A., Dias, M.B.: A comprehensive taxonomy for multi-robot task allocation. The International Journal of Robotics Research 32(12), 1495–1512 (2013)
  * [19] Kuffner, J.J., LaValle, S.M.: Rrt-connect: An efficient approach to single-query path planning. In: Robotics and Automation, 2000. Proceedings. ICRA’00. IEEE International Conference on. vol. 2, pp. 995–1001. IEEE (2000)
  * [20] Lagriffoul, F., Dantam, N.T., Garrett, C., Akbari, A., Srivastava, S., Kavraki, L.E.: Platform-independent benchmarks for task and motion planning. Robotics and Automation Letters (2018)
  * [21] Latombe, J.C.: Robot Motion Planning. Kluwer Academic Publishers (1991)
  * [22] Lo, S.Y., Zhang, S., Stone, P.: Petlon: Planning efficiently for task-level-optimal navigation. In: Proceedings of the 17th International Conference on Autonomous Agents and MultiAgent Systems. pp. 220–228. International Foundation for Autonomous Agents and Multiagent Systems (2018)
  * [23] McDermott, D., Ghallab, M., Howe, A., Knoblock, C., Ram, A., Veloso, M., Weld, D., Wilkins, D.: PDDL- The Planning Domain Definition Language. In: AIPS-98 Planning Competition Committee (1998)
  * [24] Mirrazavi Salehian, S.S., Figueroa, N., Billard, A.: A unified framework for coordinated multi-arm motion planning. The International Journal of Robotics Research 37(10), 1205–1232 (2018)
  * [25] Motes, J., Sandström, R., Adams, W., Ogunyale, T., Thomas, S., Amato, N.M.: Interaction templates for multi-robot systems. IEEE Robotics and Automation Letters 4(3), 2926–2933 (2019)
  * [26] Motes, J., Sandström, R., Lee, H., Thomas, S., Amato, N.M.: Multi-robot task and motion planning with subtask dependencies. IEEE Robotics and Automation Letters 5(2), 3338–3345 (2020)
  * [27] Muñoz, P., R-Moreno, M.D., Barrero, D.F.: Unified framework for path-planning and task-planning for autonomous robots. Robotics and Autonomous Systems 82, 1–14 (2016)
  * [28] Preda, N., Manurung, A., Lambercy, O., Gassert, R., Bonfè, M.: Motion planning for a multi-arm surgical robot using both sampling-based algorithms and motion primitives. In: 2015 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS). pp. 1422–1427. IEEE (2015)
  * [29] Rodríguez, C., Suárez, R.: Combining motion planning and task assignment for a dual-arm system. In: 2016 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS). pp. 4238–4243. IEEE (2016)
  * [30] Rohmer, E., Singh, S.P.N., Freese, M.: CoppeliaSim (formerly V-REP): a Versatile and Scalable Robot Simulation Framework. In: Proc. of The International Conference on Intelligent Robots and Systems (IROS) (2013), www.coppeliarobotics.com
  * [31] Sharon, G., Stern, R., Felner, A., Sturtevant, N.R.: Conflict-based search for optimal multi-agent pathfinding. Artificial Intelligence 219, 40–66 (2015)
  * [32] Srivastava, S., Fang, E., Riano, L., Chitnis, R., Russell, S., Abbeel, P.: Combined task and motion planning through an extensible planner-independent interface layer. In: Robotics and Automation (ICRA), IEEE International Conference on. pp. 639–646. IEEE (2014)
  * [33] Stilman, M., Kuffner, J.: Planning among movable obstacles with artificial constraints. The International Journal of Robotics Research 27(11-12), 1295–1307 (2008)
  * [34] Sucan, I.A., Chitta, S.: Moveit! (2013)
  * [35] Şucan, I.A., Moll, M., Kavraki, L.E.: The Open Motion Planning Library. IEEE Robotics & Automation Magazine 19(4), 72–82 (December 2012). https://doi.org/10.1109/MRA.2012.2205651, https://ompl.kavrakilab.org
  * [36] Thomas, A., Karami, H., Mastrogiovanni, F.: Iterative AND/OR Graphs for Task-Motion Planning. In: Italian Conference on Robotics and Intelligent Machines (I-RIM) (2020)
  * [37] Thomas, A., Mastrogiovanni, F., Baglietto, M.: Task-Motion Planning for Navigation in Belief Space. In: The International Symposium on Robotics Research (2019)
  * [38] Thomas, A., Mastrogiovanni, F., Baglietto, M.: Towards Multi-Robot Task-Motion Planning for Navigation in Belief Space. In: European Starting AI Researchers’ Symposium. CEUR (2020)
  * [39] Toussaint, M.: Logic-geometric programming: An optimization-based approach to combined task and motion planning. In: Twenty-Fourth International Joint Conference on Artificial Intelligence (2015)
  * [40] Umay, I., Fidan, B., Melek, W.: An integrated task and motion planning technique for multi-robot-systems. In: 2019 IEEE International Symposium on Robotic and Sensors Environments (ROSE). pp. 1–7. IEEE (2019)
  * [41] Zlot, R.: An auction-based approach to complex task allocation for multirobot teams, PhD thesis. Robotics Institute, Carnegie Mellon University, 5000 Forbes Ave. (2006)