# Hierarchical Motion Planning Framework
for Cooperative Transportation of
Multiple Mobile Manipulators

Heng Zhang, Haoyi Song, Wenhang Liu, Xinjun Sheng, Zhenhua Xiong and Xiangyang
Zhu This work was supported in part by the National Natural Science Foundation
of China (U1813224, 51675325), and Ministry of Education China Mobile Research
Fund Project (MCM20180703).All authors are with State Key Laboratory of
Mechanical System and Vibration, School of Mechanical Engineering, Shanghai
Jiao Tong University, Shanghai 200240, PR China. e-mail: {zhanghengme_sjtu,
songhaoyi, liuwenhang, xjsheng, mexiong<EMAIL_ADDRESS>(Corresponding
author: Zhenhua Xiong and Xiangyang Zhu).

###### Abstract

Multiple mobile manipulators show superiority in the tasks requiring mobility
and dexterity compared with a single robot, especially when
manipulating/transporting bulky objects. When the object and the manipulators
are rigidly connected, closed-chain will form and the motion of the whole
system will be restricted onto a lower-dimensional manifold. However, current
research on multi-robot motion planning did not fully consider the formation
of the whole system, the redundancy of the mobile manipulator and obstacles in
the environment, which make the tasks challenging. Therefore, this paper
proposes a hierarchical framework to efficiently solve the above challenges,
where the centralized layer plans the object’s motion offline and the
decentralized layer independently explores the redundancy of each robot in
real-time. In addition, closed-chain, obstacle-avoidance and the lower bound
of the formation constraints are guaranteed in the centralized layer, which
cannot be achieved simultaneously by other planners. Moreover, capability map,
which represents the distribution of the formation constraint, is applied to
speed up the two layers. Both simulation and experimental results show that
the proposed framework outperforms the benchmark planners significantly. The
system could bypass or cross obstacles in cluttered environments, and the
framework can be applied to different numbers of heterogeneous mobile
manipulators.

###### Index Terms:

Multiple mobile manipulators, closed-chain, formation, redundancy, obstacle
avoidance, capability map

##  I INTRODUCTION

Multiple robots system (_MRS_) allows simpler, cheaper, modular robotic units
to be reorganized into a group based on the task at hand. It can be as
effective as a task-specific, larger, monolithic robot, which may be more
expensive and has to be rebuilt according to the task [1]. Inspired by nature
[2], _MRS_ has evolved into a variety of forms, such as multiple mobile robots
[3], multiple manipulators [4], multiple drones [5] and multiple underwater
robots [6], and has been applied in various scenarios like construction [7],
transportation [8] and rescue [9].

Compared with a single robot, the fundamental challenge of the _MRS_ lies in
the cooperation among robots. Cooperation may happen in several aspects, such
as knowledge sharing and physical manipulation. In this paper, we focus on the
motion planning of the _MRS_ when manipulating bulky objects. In the task
shown in Fig. 9, the robots need to cooperate and transport a large object,
and first of all plan a path connecting the start and goal while satisfying a
set of user-specified constraints.

As a combination of mobile robot and manipulator, mobile manipulator (_MM_)
inherits the mobility from the mobile robot and the dexterity from the
manipulator [10]. In the multiple mobile robots system, they are implicitly or
explicitly required to maintain specific formation to improve the efficiency
of communication and collaboration [11]. In the multiple manipulators system,
they are rigidly connected to achieve robust manipulation of the object [12].
Therefore, formation and closed-chain constraints will meet in the multiple
mobile manipulators system. Furthermore, compared with the mobile robot and
the manipulator, _MM_ is redundant. This implies that the same task at the end
effector can be executed in different ways in the configuration space, which
gives the possibility of avoiding forbidden regions and optimizing the robot
configurations [13].

Closed-chain, formation, redundancy and obstacles are inevitable factors for
motion planner of multiple mobile manipulators. Firstly, closed-chain
constraint implicitly defines a lower-dimensional manifold in the
configuration space, hence the probability that a uniform sample will satisfy
this constraint is zero [14, 15, 16]. Moreover, obstacles in the environment
will affect the connectivity of this manifold and bring additional challenges
to the problem. Secondly, redundancy means that the motion of the system is
under-constrained. Although it provides the chance to achieve extra behaviors,
designing meaningful behaviors is nontrivial as they may vary significantly in
different applications [17][18]. Finally, the influences of the redundancy and
closed-chain constraint on the formation constraint are closely coupled.
Redundancy is the basis for formation optimization. For example, due to the
coordination of the torso and arms, human could transport the object while
adjusting their formation. On the other hand, closed-chain constraint and the
limited reachable workspace of the manipulator will greatly reduce the space
of formation optimization, which is even worse in cluttered environments.

###  I-A Contribution

To simultaneously deal with closed-chain and formation of the whole system,
redundancy of the mobile manipulator and obstacles in the environment, this
paper proposes a motion planning framework with the following innovations.

  * •

Hierarchical framework: the centralized layer plans the object’s motion, and
the formation of the system is optimized in the decentralized layer. A unique
feature is that closed-chain, obstacle-avoidance and the lower bound of the
formation constraints can be quickly checked in the centralized layer, which
ensures compatibility with the decentralized layer. Moreover, the
decentralized layer is distributed, and the redundancy of each robot can be
explored independently in real-time. Therefore, the complex planning problem
can be simplified while not violating the constraints.

  * •

Diversified and efficient obstacle-avoidance strategies: on the basis of the
closed-chain constraint, the system can either adjust the formation to bypass
obstacles or adjust the self-motion of the redundant robots to cross
obstacles. As a result, the motion planner exhibits excellent performance in
complex environments, which is verified by comparing it with the benchmark
planners.

  * •

The framework is unified and can be applied to different numbers of
heterogeneous mobile manipulators. Moreover, to the best of our knowledge,
this is the first time to achieve six-dimensional object manipulation by
multiple mobile manipulators in cluttered real-world environments.

###  I-B Outline of the Paper

The outline of this paper is as follows. In Section II, related works on
redundancy, closed-chain and formation constraints of the _MRS_ will be
reviewed. Section III will give a formal definition of the motion planning
problem in detail. After that, we will introduce the proposed hierarchical
framework in Section IV. The feasibility and superiority of the motion planner
will be verified by simulation and real-world experiments in Section V.
Finally, the paper is concluded in Section VI.

##  II Related Works

The motion planners of multiple mobile manipulators in the literature are
mostly inherited from its two subsystems: multiple manipulators and multiple
mobile robots. The former focuses on the closed-chain constraint, while the
latter emphasizes the formation constraint. In addition, these two classes of
algorithms usually do not solve the redundancy explicitly. Therefore, in the
following, we will first review the algorithms that deal with the redundancy
of a single _MM_ and then extend to related works from two perspectives:
closed-chain constraint and formation constraint.

###  II-A Redundancy of the Mobile Manipulator

Redundancy of the _MM_ can be solved at position-level [17][19] or velocity-
level [20][21]. For the former, redundancy parameters will be designed to
restrict the motion of the _MM_. For the latter, redundancy can be dealt with
by task-augmentation and task-priority algorithms. Compared with the task-
augmentation algorithm [20], task-priority algorithm [21] is superior when
dealing with conflict among different tasks. In this algorithm, the additional
task is only satisfied in the null space of the primary task, which gives a
higher priority to the primary task. For example, in our previous work [22],
the motion of the end effector and the mobile robot were treated separately as
the primary task and the additional task. The experimental results showed that
the manipulator could grasp the object while the mobile robot was moving.

In the position-based and velocity-based algorithms, redundancy resolution
will be transformed into nonlinear optimization problems about redundancy
parameters and additional tasks, respectively. To speed up the optimization
process and avoid local minimum, capability map (CM) can be queried to
determine the seed [23][24]. CM was initially proposed in [25][26] and used to
handle the base placement of the _MM_ [27, 28, 29, 30]. It encodes the
distribution of the user-specified constraints and could guide the iterative
direction of the optimization algorithm. The effectiveness and superiority of
CM were verified in our previous work in the human-robot collaboration task
[24]. However, _CM_ has only been used in a single _MM_ , and how to combine
it with the _MRS_ is still an open question. In this paper, we will extend it
to multiple mobile manipulators to accelerate the proposed framework.

###  II-B Closed-Chain Constraint

Closed-chain constraint manifold has its dimension lower than that of the
configuration space, hence uniform sampling in the configuration space has
zero probability of generating a valid state that satisfies this constraint
[14, 15, 16]. One solution to this problem is introducing an allowable
tolerance [31]. Therefore, the volume of the satisfying subset grows, and
various sampling-based motion planners for the unconstrained problem can be
adopted. However, much of the complexity of handling the constraint transfers
from the higher-level motion planner to the lower-level motion controller. In
the case that multiple robots are rigidly connected, motion deviation caused
by constraint relaxation may damage the robots and the transported object.

Another approach to resolve the closed-chain constraint is _Projection_
(_PJ_). Several algorithms were developed based on it, such as RGD [32] and
CBiRRT [33]. It iteratively projects a random sample onto the manifold
according to the Jacobian-inverse gradient descent operation. Combining the
projection operator with the workspace decomposition technique, [34] realized
the motion planning of three mobile manipulators in the simulated environment.
However, the constraint Jacobian is not guaranteed to be invertible all the
time, and this approach relies heavily on gradient descent operation, which is
time-consuming for high-dimensional robots. To improve the computational
efficiency, _Atlas_ (_AT_) [35], which uses piece-wise tangent spaces to
locally approximate the constraint manifold, and _Tangent Bundle_ (_TB_) [36],
which is similar to _AT_ but with lazy states checking, were proposed.
However, to get the tangent space, the kernel of the constraint Jacobian has
to be computed, which requires complex matrix decomposition. Therefore, the
computational efficiency will be reduced.

In addition, inverse kinematics can be seen as a special projection operator.
Compared with the Jacobian-based projection operator, it is more efficient,
especially for robots with the analytical inverse kinematic solvers. For
example, RLG, which combines the random sampling technique with inverse
kinematics, was developed to deal with the closed-chain constraint [37]. RLG
works well for the non-redundant multiple manipulators system. However, it is
difficult to integrate with other constraints simultaneously. Moreover, the
kinematic characteristic of the fixed base manipulator is different from the
_MM_ , hence RLG is hard to extend to multiple mobile manipulators directly.

###  II-C Formation Constraint

Formation constraint is widely studied in the field of multiple mobile robots,
and typical algorithms are the leader-follower approach, behavioral approach
and virtual structure approach [11]. In the domain of multiple mobile
manipulators, [38] defined a virtual structure named system outlined rectangle
(SOR), and the leader-follower approach was applied to plan the motion of the
system, where the leader adjusted the width of the SOR according to the
environment. Similarly, the convex region was defined in [39], and then the
formation control problem was transformed into an optimization problem with
respect to the convex region. However, the obstacle-avoidance strategies in
[38, 39, 40] are conservative. As long as the virtual structure (e.g., _SOR_
or convex region) intersects with obstacles, the system is considered in
collision. As a result, bypassing becomes the only way to avoid obstacles, no
matter how small they are. However, by utilizing the redundancy of the system,
crossing obstacles is sometimes a more reasonable choice, which is common in
the human-human collaborative tasks.

Obstacle crossing is challenging and rarely studied previously. It not only
requires cooperation among different mobile manipulators but also requires
coordination between the mobile robot and the manipulator. In our previous
work, the traversability of obstacles was studied in the multiple mobile
robots system with a deformable sheet [41]. The experimental results showed
that the system could intelligently bypass or cross obstacles, which greatly
increases the success rate of the motion planner in cluttered environments.
Therefore, in this paper, we will extend this idea from multiple mobile robots
to multiple mobile manipulators.

In summary, the coupling among redundancy, closed-chain, formation and
obstacle-avoidance makes the motion planning of multiple mobile manipulators
one of the most challenging problems in robotics. Although outstanding works
have been done from a single perspective, e.g., task-priority algorithm [21]
for redundancy, projection-based frameworks [32][33][35][36] for the closed-
chain constraint and virtual structure-based approach [39] for the formation
constraint, they are unable to solve the above challenges simultaneously. As a
result, the system cannot fully take its advantage brought by multi-robot and
redundancy, and is prone to failure in cluttered environments.

##  III PROBLEM DEFINITION

In this section, we derive the kinematic model of the system and define some
mathematical notations for subsequent discussion. Moreover, a formal
definition of the motion planning problem along with the analysis is presented
afterward.

###  III-A Modeling of the Multiple Mobile Manipulators System

Figure 1: Kinematic model of the multiple mobile manipulators system.

Consider multiple mobile manipulators ($\emph{M}^{3}$) manipulating an object
in Fig. 1. Let $O_{w}X_{w}Y_{w}Z_{w}$, $O_{obj}X_{obj}Y_{obj}Z_{obj}$,
$O_{b,i}X_{b,i}Y_{b,i}Z_{b,i}$ and $O_{e,i}X_{e,i}Y_{e,i}Z_{e,i}$ be the
frames of the world, the object, the mobile base and the end effector of the
$i$th _MM_ , where $i=1,2,...,n$ and $n$ is the number of the robots in the
system.

To simplify the model of the holonomic mobile robot, it is equivalent to an
open chain[22][42]. Therefore, the configuration of the $i$th mobile robot can
be expressed as $\boldsymbol{q}_{b,i}\in\mathbb{R}^{n_{b,i}}$, where $n_{b,i}$
is the degrees of freedom (DOF) of the mobile robot. In most cases $n_{b,i}=3$
as the holonomic mobile robot can move and rotate in all directions on the
ground. The configuration of the $i$th manipulator is
$\boldsymbol{q}_{a,i}\in\mathbb{R}^{n_{a,i}}$, where $n_{a,i}$ represents its
DOF. Therefore, the configuration space of the $i$th _MM_ can be formally
defined as $\mathcal{C}_{\scriptscriptstyle
MM}^{i}\subseteq\mathbb{R}^{n_{a,i}+n_{b,i}}$, which is the set of
configurations of the $i$th _MM_
$\boldsymbol{q}_{i}=(\boldsymbol{q}_{b,i}^{T},\boldsymbol{q}_{a,i}^{T})^{T}$.

To represent the homogenous transformation of frame $i$ with respect to frame
$j$, we define $\boldsymbol{X}_{i}^{j}\in SE(3)$ and its minimum
representation
$\boldsymbol{t}_{i}^{j}=({\boldsymbol{p}_{i}^{j}}^{\scriptscriptstyle
T},{\boldsymbol{\alpha}_{i}^{j}}^{\scriptscriptstyle
T})^{T}\in\mathbb{R}^{6}$, in which ${\boldsymbol{p}_{i}^{j}}$ denotes the
relative position, and ${\boldsymbol{\alpha}_{i}^{j}}$ denotes a minimum
description of orientation. Therefore, the configuration space of the object
is defined as $\mathcal{C}_{obj}\subseteq\mathbb{R}^{6}$, which is the set of
the object’s poses with resect to the world frame $\boldsymbol{t}_{obj}^{w}$.

Let the configuration space of the system $\mathcal{C}_{\scriptscriptstyle
M^{3}}=\mathcal{C}_{\scriptscriptstyle
MM}^{1}\times...\times\mathcal{C}_{\scriptscriptstyle
MM}^{i}\times...\times\mathcal{C}_{\scriptscriptstyle
MM}^{n}\times\mathcal{C}_{obj}\subseteq\mathbb{R}^{\sum_{i=1}^{n}{(n_{a,i}+n_{b,i})+6}}$
be the set of
$\boldsymbol{c}=(\boldsymbol{q}_{1}^{T},...,\boldsymbol{q}_{i}^{T},...,\boldsymbol{q}_{n}^{T},{\boldsymbol{t}_{obj}^{w}}^{T})^{T}$.
The forward kinematics of the $i$th _MM_ at position-level and velocity-level
are defined as Eq. (1) and Eq. (2), respectively.

$\boldsymbol{t}{{}_{e,i}^{w}}=f_{k}(\boldsymbol{q}_{i})$ (1)
$\dot{\boldsymbol{t}}{{}_{e,i}^{w}}=\frac{\partial{f_{k}(\boldsymbol{q}_{i})}}{\partial{\boldsymbol{q}_{i}}}\dot{\boldsymbol{q}}_{i}=\boldsymbol{J}_{i}(\boldsymbol{q}_{i})\dot{\boldsymbol{q}}_{i}$
(2)

where $f_{k}(.)$ is the forward kinematic operator, and
$\boldsymbol{J}_{i}(\boldsymbol{q}_{i})\in\mathbb{R}^{6\times({n_{a,i}+n_{b,i}})}$
is the analytical Jacobian matrix of the $i$th _MM_. When a six-DOF
manipulator mounting on a mobile robot, the column of
$\boldsymbol{J}_{i}(\boldsymbol{q}_{i})$ is larger than that of the row,
leading to the redundancy of the _MM_.

###  III-B Closed-Chain Constraint

Given a random sample
$\boldsymbol{c}=(\boldsymbol{q}_{1}^{T},...,\boldsymbol{q}_{i}^{T},...,\boldsymbol{q}_{n}^{T},{\boldsymbol{t}_{obj}^{w}}^{T})^{T}$,
we define the vector of the end effector’s poses as
$\boldsymbol{E}=(\boldsymbol{t}{{}_{e,1}^{w}}^{T},...,\boldsymbol{t}{{}_{e,i}^{w}}^{T},...,\boldsymbol{t}{{}_{e,n}^{w}}^{T})^{T}\in\mathbb{R}^{6n}$
where $\boldsymbol{t}{{}_{e,i}^{w}}=f_{k}(\boldsymbol{q}_{i})$. The vector of
the grasping poses on the object is denoted as
$\boldsymbol{G}=(\boldsymbol{t}{{}_{g,1}^{w}}^{T},...,\boldsymbol{t}{{}_{g,i}^{w}}^{T},...,\boldsymbol{t}{{}_{g,n}^{w}}^{T})^{T}\in\mathbb{R}^{6n}$,
in which $\boldsymbol{t}{{}_{g,i}^{w}}$ represents the $i$th grasping pose
with respect to the world frame. For convenience, a projection
$\pi:\mathcal{C}_{\scriptscriptstyle M^{3}}\rightarrow\mathcal{C}_{obj}$ is
defined so that $\pi(\boldsymbol{c})=\boldsymbol{t}_{obj}^{w}$. Assuming the
manipulated object is rigid, $\boldsymbol{G}$ can be easily derived by the
geometric transformation $g:\mathcal{C}_{obj}\rightarrow\mathbb{R}^{6n}$ in
Eq. (3).

$\boldsymbol{G}=g(\pi(\boldsymbol{c}))$ (3)

When a rigid grasp exists between $\boldsymbol{t}{{}_{e,i}^{w}}$ and
$\boldsymbol{t}{{}_{g,i}^{w}}$ for all $i=1,2,...,n$, the closed-chain
constraint ($\emph{C}^{3}$) forms, and it can be formally described by
$f_{\scriptscriptstyle C^{3}}:\mathcal{C}_{\scriptscriptstyle
M^{3}}\rightarrow\mathbb{R}^{6n}$ in Eq. (4).

$f_{\scriptscriptstyle
C^{3}}(\boldsymbol{c})=\boldsymbol{E}-\boldsymbol{G}=\boldsymbol{0}$ (4)

Therefore, the set of configurations that satisfy the closed-chain constraint
is denoted as $\mathcal{C}_{\scriptscriptstyle
C^{3}}\subseteq\mathcal{C}_{\scriptscriptstyle M^{3}}$ in Eq. (5).

$\mathcal{C}_{\scriptscriptstyle
C^{3}}=\\{\boldsymbol{c}|\boldsymbol{c}\in\mathcal{C}_{\scriptscriptstyle
M^{3}},f_{\scriptscriptstyle C^{3}}(\boldsymbol{c})=\boldsymbol{0}\\}$ (5)

###  III-C Formation Constraint

The fundamental difference between the closed-chain constraint
($\emph{C}^{3}$) and the formation constraint (FC) is that $\emph{C}^{3}$ is
“hard” but FC is “soft”, in which “hard” constraints have to be satisfied
exactly everywhere, and “soft” constraints are usually described by cost
functions and optimized as much as possible. Therefore, FC can be transformed
into an optimization problem and formally defined as follow.

$\displaystyle\max_{\boldsymbol{c}}\quad f_{\scriptscriptstyle
FC}(\boldsymbol{c})$ (6)
$\displaystyle\begin{array}[]{r@{\quad}r<EMAIL_ADDRESS>C^{3}}\cap\mathcal{C}_{free}\end{array}$

where $\mathcal{C}_{free}$ represents the set of collision-free configurations
with appropriate dimensions. $f_{\scriptscriptstyle
FC}:\mathcal{C}_{\scriptscriptstyle C^{3}}\rightarrow\mathbb{R}$ is the
formation metric of the system. Its design philosophy and expression will be
illustrated in Section IV-C in detail.

###  III-D Problem Definition

In addition to the notations above, we define a mapping
$\Pi:\mathcal{C}_{obj}\rightarrow\mathcal{C}_{\scriptscriptstyle C^{3}}$ so
that given a pose of the object $\boldsymbol{t}_{obj}^{w}$,
$\Pi(\boldsymbol{t}_{obj}^{w})=\\{\boldsymbol{c}|\boldsymbol{c}\in\mathcal{C}_{\scriptscriptstyle
C^{3}}\cap\mathcal{C}_{free},\pi(\boldsymbol{c})=\boldsymbol{t}_{obj}^{w}\\}$.
It is the set of configurations that satisfy collision and closed-chain
constraints while the object’s pose is invariant. Therefore, the motion
planning of multiple redundant mobile manipulators under closed-chain,
formation and obstacle-avoidance constraints is defined as follows.

Problem Definition: given a start configuration
$\boldsymbol{c}_{start}\in\mathcal{C}_{\scriptscriptstyle
C^{3}}\cap\mathcal{C}_{free}$ and a target pose of the object
$\boldsymbol{t}_{obj}^{w}$, find a path
$\tau:[0,1]\rightarrow\mathcal{C}_{\scriptscriptstyle
C^{3}}\cap\mathcal{C}_{free}$ such that 1) $\tau(0)=\boldsymbol{c}_{start}$;
2) $\tau(1)\in\Pi(\boldsymbol{t}_{obj}^{w})$; and 3) the formation metric
$f_{\scriptscriptstyle FC}$ is optimized throughout $\tau$.

###  III-E Problem Analysis

The key to the closed-chain constraint is to design efficient samplers since
uniform sampling can not work directly. As discussed above, _PJ_ , _AT_ and
_TB_ are centralized frameworks and move a random state
$\boldsymbol{c}\in\mathcal{C}_{\scriptscriptstyle M^{3}}$ to a new state
$\boldsymbol{c}_{new}\in\mathcal{C}_{\scriptscriptstyle C^{3}}$ iteratively,
which is computationally expensive. Unlike the projection-based frameworks,
our motion planner will not calculate $\boldsymbol{q}_{i}$ explicitly when
dealing with the closed-chain constraint. On the contrary, we define the
allowed sampling region for the mobile robot and query the capability map of
the manipulator to quickly check $\pi(\boldsymbol{c})$ is extensible or not.

Ideally, it is necessary to optimize the formation on each sample and generate
a globally optimal path about $f_{\scriptscriptstyle FC}$. However, this is
hard to achieve both technically and theoretically. On the one hand, most
samples are not on the final path connecting $\tau(0)$ and $\tau(1)$, hence
optimizing $f_{\scriptscriptstyle FC}$ on these samples are unnecessary and
time-consuming. On the other hand, the convergence to optimality is not
guaranteed for the existing optimal sampling-based motion planners like
$\emph{RRT}^{*}$ [43] when optimizing over something other than the path
length.

Therefore, we use a hierarchical way to resolve the complex motion planning
problem. The centralized layer plans the motion of the object, and the
formation optimization is delayed in the decentralized layer. Unlike the
decoupled framework in [44], the “hard” closed-chain constraint
$f_{\scriptscriptstyle C^{3}}$ and the lower bound of the “soft” constraint
$f_{\scriptscriptstyle FC}$ are guaranteed in our centralized layer to avoid
conflict with the decentralized layer. In this way, the complex motion
planning problem of multiple mobile manipulators is simplified while not
decreasing the success rate and violating the constraints.

##  IV HIERARCHICAL MOTION PLANNING FRAMEWORK

###  IV-A Overview of the Hierarchical Framework

Figure 2: Overview of the hierarchical motion planner. The centralized layer
receives the motion planning request and computes the motion of the object.
Collision constraint, closed-chain constraint $f_{\scriptscriptstyle
C^{3}}(\boldsymbol{c})$ and the lower bound of the formation constraint
$f_{\scriptscriptstyle FC}(\boldsymbol{c})$ are checked in this layer to
ensure the motion of the object is executable by the physical system. In
addition, collision map, which represents the information of the environment,
and capability map, which encodes the distribution of the formation
constraint, are queried to speed up the motion planner. The decentralized
layer, which can be deployed in a distributed way in real-time, consists of
the formation optimizer and the task-priority motion controller. The former
takes the object’s motion as input and optimizes the formation and dexterous
configuration of each mobile manipulator. The task-priority motion controller
runs the feedback algorithm and sends the desired joint motion command to the
lower-level hardware.

Overview of the hierarchical motion planning framework is shown in Fig. 2. The
centralized layer receives the motion planning request from the user and plans
the motion of the object. Collision constraint, closed-chain constraint and
the lower bound of the formation constraint are checked for each sample to
ensure the object’s motion is executable by the physical system. Moreover,
collision map and capability map are queried to accelerate the checking
process. The decentralized layer makes full use of the computing resource on
each _MM_ and can be executed in a distributed way in real-time. It consists
of two components: the formation optimizer and the task-priority motion
controller. Taking the motion of the object as input, the former optimizes the
formation and dexterous configuration of each robot as much as possible. To
track the desired motion on the real redundant robot, task-priority motion
controller is designed to send the joint motion command to the lower-level
hardware. The details of the framework are explained as follows. Its
feasibility and superiority will be verified in Section V.

###  IV-B Centralized Layer

1 Function  __plan_($\boldsymbol{t}_{start}$, $\boldsymbol{t}_{goal}$,
$t_{obj}$)_

2 if  __validChecking_($\boldsymbol{t}_{goal}$) _is false__ then

3 return fail;

4

5 end if

6 $\mathcal{T}_{s}.\texttt{init}(\boldsymbol{t}_{start})$;

7 $\mathcal{T}_{g}.\texttt{init}(\boldsymbol{t}_{goal})$;

8 for  _$t\leftarrow 0$ to $t_{obj}$_ do

9 $\boldsymbol{t}_{rand}$ $\leftarrow$
sampleValidObjConfig($\mathcal{T}_{s}$);

10 $\mathcal{T}_{s}.\texttt{add}(\boldsymbol{t}_{rand})$;

11 if  __connect_($\mathcal{T}_{s},\mathcal{T}_{g}$) _is success__ then

12 return extractPath($\mathcal{T}_{s},\mathcal{T}_{g}$);

13

14 end if

15 swap($\mathcal{T}_{s},\mathcal{T}_{g}$);

16

17 end for

18 return fail;

19

20

21Function  __sampleValidObjConfig_($\mathcal{T}_{s}$)_

22 for  _$j\leftarrow 1$ to maxSample_ do

23 $\boldsymbol{t}_{rand}$ $\leftarrow$ sampleAndExtend($\mathcal{T}_{s}$);

24 if  __validChecking_($\boldsymbol{t}_{rand}$) _is true__ then

25 return $\boldsymbol{t}_{rand}$;

26

27 end if

28

29 end for

30 return none;

31

32

33Function  __validChecking_($\boldsymbol{t}_{rand}$)_

34 if  _$\boldsymbol{t}_{rand}$ _is in collision__ then

35 return false;

36

37 end if

// compute G according to Eq.(3)

38 $\boldsymbol{G}$ $\leftarrow$ getGraspingPoses($\boldsymbol{t}_{rand}$);

39 for  __each_ $\boldsymbol{t}_{g,i}^{w}$ _in_ $\boldsymbol{G}$_ do

// defined in Alg.3

40 if  __sampleInASR_($\boldsymbol{t}_{g,i}^{w}$) _is none__ then

41 return false;

42

43 end if

44

45 end for

46 return true;

47

48

Alg. 1 _Object Motion Planner_

The sampler is designed as a primitive in the centralized layer. Therefore,
working with different sampling-based algorithms, such as PRM [45] and EST
[46], becomes a trivial task. We will introduce the sampler in the structure
of RRTConnect algorithm [47] in the following.

For brevity of notations, we omit the superscript and subscript of
$\boldsymbol{t}_{obj}^{w}$ in this part. For example,
$\boldsymbol{t}_{start}$, $\boldsymbol{t}_{goal}$ and $\boldsymbol{t}_{rand}$
represent the start, the goal and random pose of the object with respect to
the world frame, separately. Some key functions in Alg. 1 are explained as
follows.

  * •

$\texttt{plan}(\boldsymbol{t}_{start},\boldsymbol{t}_{goal},t_{obj})$
generates the path of the object when the planning request and allowed time
are specified. Before planning, the validity of $\boldsymbol{t}_{goal}$ is
first checked, and two trees are initialized. $\mathcal{T}_{s}$ starts from
$\boldsymbol{t}_{start}$ and $\mathcal{T}_{g}$ starts from
$\boldsymbol{t}_{goal}$. Then random valid pose $\boldsymbol{t}_{rand}$ is
sampled and added to the tree $\mathcal{T}_{s}$. In each iteration, the two
trees try to connect with each other by connect() and extract the valid path
by extractPath(). On the real robot, the path should be post-processed and
time-parameterized to get the smooth object’s motion. If the two trees are
unable to connect, they are swapped and continue to grow in the next iteration
until the path is found or the allowed time $t_{obj}$ is exceeded.

  * •

$\texttt{sampleValidObjConfig}(\mathcal{T}_{s})$ is the sampler of the
centralized layer. It generates valid object’s poses that are connectable to
$\mathcal{T}_{s}$. sampleAndExtend() is a standard operation in the sampling-
based algorithm. It first generates a randomized pose by uniform or gaussian
samplers and then searches the nearest pose in $\mathcal{T}_{s}$ and extends
to $\boldsymbol{t}_{rand}$ by the given step size. If $\boldsymbol{t}_{rand}$
is valid, it will be returned.

  * •

$\texttt{validChecking}(\boldsymbol{t}_{rand})$ checks whether
$\boldsymbol{t}_{rand}$ can be extended to a composite configuration
$\boldsymbol{c}_{rand}\in\mathcal{C}_{\scriptscriptstyle
C^{3}}\cap\mathcal{C}_{free}$ so that
$\pi(\boldsymbol{c}_{rand})=\boldsymbol{t}_{rand}$. Given the pose of the
object, the grasping poses vector $\boldsymbol{G}$ is calculated based on Eq.
(3). For each pose $\boldsymbol{t}_{g,i}^{w}$ in $\boldsymbol{G}$, the allowed
sampling region is sampled to check whether the closed-chain constraint and
the lower bound of the formation constraint can be satisfied for each _MM_.
When these conditions are met for all $i=1,2,...,n$, $\boldsymbol{t}_{rand}$
is seen as a valid sample. The detail of sampleInASR() is introduced in the
following.

####  IV-B1 Allowed Sampling Region

Figure 3: (a). Although the pose of the object is collision-free, it exceeds
the workspace of the robots and the closed-chain constraint is unable to be
satisfied. (b). Although a configuration can be found to satisfy the closed-
chain constraint, one of the robots must be in an awkward configuration due to
the obstacle. Therefore, these samples should be discarded as they are unable
to be executed or lead to poor and unstable formations in the decentralized
layer.

When planning the motion of the object, in addition to meeting the “hard”
closed-chain constraint $f_{\scriptscriptstyle C^{3}}(\boldsymbol{c})$, it is
necessary to ensure the lower bound of the “soft” formation constraint
$f_{\scriptscriptstyle FC}(\boldsymbol{c})$. Otherwise, the centralized layer
may conflict with the decentralized layer. For example, although the object’s
pose is collision-free in Fig. 3(a), it exceeds the workspace of the robots,
and the closed-chain constraint is unable to be satisfied. Similarly, for the
object’s pose in Fig. 3(b), although a configuration can be found to satisfy
the closed-chain constraint, one of the robots must be in an awkward
configuration due to obstacles. Therefore, the poses of the object in these
cases should be abandoned as they are inexecutable or lead to poor and
unstable formations in the decentralized layer.

To check the validity of $\boldsymbol{t}_{g,i}^{w}$, we define the allowed
sampling region (ASR) for each mobile robot in Eq. (7).

$\begin{split}ASR(\boldsymbol{t}_{g,i}^{w},&\emph{thres})=\\{\boldsymbol{q}_{b,i}\
|\ \exists\boldsymbol{q}_{a,i},\ni
f_{k}(\boldsymbol{q}_{i})=\boldsymbol{t}_{g,i}^{w}\\\ &\ \emph{and}\
f_{{\scriptscriptstyle FC},i}(\boldsymbol{q}_{i})\geq\emph{thres}\ \emph{and}\
\boldsymbol{q}_{i}\in\mathcal{C}_{free}\\}\end{split}$ (7)

where $f_{{\scriptscriptstyle FC},i}(\boldsymbol{q}_{i})$ is the formation
metric of the $i$th _MM_ , and thres is the allowed threshold of this metric.
$f_{{\scriptscriptstyle FC},i}(\boldsymbol{q}_{i})$ is a component of
$f_{\scriptscriptstyle FC}(\boldsymbol{c})$, and their expressions are defined
in Eq. (12) and Eq. (13). Therefore, we can check whether existing
$\boldsymbol{q}_{b,i}\in ASR(\boldsymbol{t}_{g,i}^{w},\emph{thres})$ to see
the validity of $\boldsymbol{t}_{g,i}^{w}$.

ASR changes with $\boldsymbol{t}_{g,i}^{w}$ and thres. Due to the complexity
of multiple mobile manipulators, it is usually obtained by sampling. According
to the definition of ASR, given a random sample $\boldsymbol{q}_{b,i}$,
$\boldsymbol{q}_{a,i}$ has to be computed based on the inverse kinematics of
the manipulator, and the following conditions should be checked. We name this
method IKCL, which means the Inverse Kinematics-based Centralized Layer.

  * •

closed-chain constraint: $f_{k}(\boldsymbol{q}_{i})=\boldsymbol{t}_{g,i}^{w}$

  * •

lower bound of the formation constraint: $f_{{\scriptscriptstyle
FC},i}(\boldsymbol{q}_{i})\geq\emph{thres}$

  * •

collision constraint: $\boldsymbol{q}_{i}\in\mathcal{C}_{free}$

Although computing $\boldsymbol{q}_{a,i}$ and checking these conditions for
one sample is fast, this module will be invoked frequently in sampleInASR(),
hence reducing the time cost of it will improve the performance of Alg. 1
significantly. In the following, a novel tool named capability map (CM) will
be developed to speed up this module. Querying CM can avoid computing
$\boldsymbol{q}_{a,i}$ and $f_{{\scriptscriptstyle FC},i}(\boldsymbol{q}_{i})$
while satisfying $f_{k}(\boldsymbol{q}_{i})=\boldsymbol{t}_{g,i}^{w}$ and
$\boldsymbol{q}_{i}\in\mathcal{C}_{free}$ automatically. We name this method
CMCL, which means the Capability Map-based Centralized Layer.

####  IV-B2 Capability Map

CM was initially proposed to solve the base placement when the mobile robot
and the manipulator move asynchronously [27, 28, 29, 30]. It was improved to
guide the coordinated motion planning of the _MM_ in our previous works
[23][24]. CM stores the configurations and/or the poses of the end effector
along with the corresponding index. It is built in advance and then queried to
reduce the online computational burden. Besides, using offline data structure
to accelerate the online computing process can be seen in other robotic
fields. For example, Şucan et al. generated a precomputed database of the
constraint-satisfying states, and the planning algorithms can sample in this
database to get valid states quickly [48]. Due to its high efficiency, it has
been merged into MoveIt [49], which is a famous open-source motion planning
software.

1 Function  __constructCM_(discreteRes, thres)_

2 _traMap_ $\leftarrow$ discreteTra(_discreteRes_);

3 _rotMap_ $\leftarrow$ discreteRot(_discreteRes_);

4 while  _j $<$ pointNumberInTraMap_ do

5 while  _k $<$ pointNumberInRotMap_ do

6 $\boldsymbol{t}_{e,i}^{b,i}\leftarrow$ getPose(_traMap_ j, _rotMap_ k);

7 if  _$\boldsymbol{q}_{i}\leftarrow$ _getIK_($\boldsymbol{t}_{e,i}^{b,i}$)
_is false__ then

8 continue

9 end if

10 $f_{{\scriptscriptstyle FC},i}\leftarrow$
getFormationMetric($\boldsymbol{q}_{i}$);

11 if  __selfColl_($\boldsymbol{q}_{i}$) _or_ $f_{{\scriptscriptstyle FC},i}<$
thres _ then

12 continue

13 end if

14  _CM_ $\leftarrow$ push($\boldsymbol{t}_{e,i}^{b,i}$,
$f_{{\scriptscriptstyle FC},i}$);

15

16 $k\leftarrow k+1$;

17

18 end while

19 $j\leftarrow j+1$;

20

21 end while

22

23

24Function  __queryCM_($\boldsymbol{t}_{e,i}^{b,i}$)_

25 if  _$\boldsymbol{t}_{round}$ $\leftarrow$
_round_($\boldsymbol{t}_{e,i}^{b,i}$) _not in _CM___ then

26 return none;

27

28 end if

29 return $\emph{CM}.\texttt{at}(\boldsymbol{t}_{round})$;

30

31

Alg. 2 _Construct and Query the Capability Map_

The relationship among $O_{w}X_{w}Y_{w}Z_{w}$, $O_{b,i}X_{b,i}Y_{b,i}Z_{b,i}$
and $O_{e,i}X_{e,i}Y_{e,i}Z_{e,i}$ can be expressed as Eq. (8).

$\boldsymbol{X}_{e,i}^{b,i}=(\boldsymbol{X}_{b,i}^{w})^{-1}\boldsymbol{X}_{e,i}^{w}$
(8)

In the mobile manipulation task, although $\boldsymbol{X}_{e,i}^{w}$ and
$\boldsymbol{X}_{b,i}^{w}$ may change significantly when the mobile robot
moving around, $\boldsymbol{X}_{e,i}^{b,i}$ must be within the reachable
workspace of the manipulator. Therefore, the distribution of
$\boldsymbol{X}_{e,i}^{b,i}$ and the corresponding collision/formation metric
can be treated as prior information and stored in advance to construct CM.

CM can be constructed by configuration space or cartesian space sampling. The
former is faster than the latter as the inverse kinematics of the manipulator
should be called for each sample in the cartesian space. However,
configuration space sampling will lead to nonuniform end effector’s poses,
which is not good for the querying process. Moreover, CM is only constructed
offline once, hence the build time is acceptable in most cases. Therefore,
cartesian space sampling is preferred in this work. A comprehensive comparison
between them is available in [50].

In addition, various helpful information about $\boldsymbol{X}_{e,i}^{b,i}$
and $\boldsymbol{q}_{a,i}$ can be stored in CM. For example, the dexterity of
the manipulator was saved in [26]. [27] also stored statically stable
configurations for the humanoid robot. In this paper, we are interested in the
formation metric and self-collision information about the _MM_ , so they will
be saved in CM.

Alg. 2 shows the pseudo-code to construct and query CM, and the key functions
are explained as follows.

  * •

$\texttt{constructCM}(\emph{discreteRes, thres})$: the workspace is first
discretized according to the given resolution. For each
$\boldsymbol{t}_{e,i}^{b,i}$, the inverse kinematics of the manipulator is
invoked, and the formation metric $f_{{\scriptscriptstyle FC},i}$ is computed
based on Eq. (12). If $\boldsymbol{q}_{i}$ is not in self-collision and
$f_{{\scriptscriptstyle FC},i}$ is larger than the given threshold thres,
$\boldsymbol{t}_{e,i}^{b,i}$ and $f_{{\scriptscriptstyle FC},i}$ are added to
CM. Poses with low $f_{{\scriptscriptstyle FC},i}$ are discarded as they waste
storage space and will lead to poor formations in the querying process. CM
with different thres is shown in Fig 4a. Colors represent the mean
$f_{{\scriptscriptstyle FC},i}$ of all 6D poses inside the 3D voxel.

  * •

$\texttt{queryCM}(\boldsymbol{t}_{e,i}^{b,i})$: the discrete CM is an
approximation of the continuous workspace, so the requested pose may not be in
CM exactly. Therefore, we round the requested pose based on discreteRes to get
$\boldsymbol{t}_{round}$. For example, 0.51 will be rounded to 0.5 when the
resolution is 0.1. When $\boldsymbol{t}_{round}$ is within the reachable
workspace of the manipulator and the formation metric is larger than the
threshold, $f_{{\scriptscriptstyle FC},i}$ stored at $\boldsymbol{t}_{round}$
is returned.

Figure 4: (a). Capability map (CM) of the mobile manipulator with different
formation metric threshold thres. Colors represent the mean formation metric
of all 6D poses inside the 3D voxel (from small to large: red, green, cyan,
blue). (b). The allowed sampling region (ASR) of the mobile robot with
different thres. The origin and direction of the arrow represent the position
and orientation of the mobile robot, respectively. The color indicates the
formation metric when the mobile robot locates at the arrow. As can be seen,
when the formation constraint is relaxed (low thres), there are lots of poses
in CM and ASR, and most of them will lead to poor formations. When the
formation constraint is strict (high thres), there are few poses in CM and
ASR, and we have to spend much time to sample a valid one. Therefore, thres
should be moderate to balance the quality of the formation and the
computational time.

After constructing CM and the querying process is ready, let us take a closer
look at how to check the validity of $\boldsymbol{q}_{b,i}$ based on it. In
$ASR(\boldsymbol{t}_{g,i}^{w},\emph{thres})$, $\boldsymbol{t}_{g,i}^{w}$ is
constant and the same as $\boldsymbol{X}_{e,i}^{w}$ when the closed-chain
constraint is satisfied. Given a sample $\boldsymbol{q}_{b,i}$,
$\boldsymbol{X}_{e,i}^{b,i}$ can be calculated according to Eq. (8). As the
formation metric and self-collision information about the _MM_ have been
computed and stored in advance, they can be retracted from CM and indicate the
validity of $\boldsymbol{q}_{b,i}$. Therefore, the inverse kinematic solver of
the manipulator and the computational process of $f_{{\scriptscriptstyle
FC},i}$ are avoided.

The pseudo-code of
$\texttt{sampleInASR}(\boldsymbol{t}_{g,i}^{w},\emph{bounds})$ is shown in
Alg. 3. bounds represents the lower and upper limits that the mobile robot
configuration could be. It is useful when the heuristic information about the
ASR is known. In sampleInASR() of Alg. 1, bounds is set to its default value,
which is the whole movable space of the mobile robot. Its value in the
decentralized layer will be explained later in Alg. 4.
$\texttt{sampleMobileRobot}($_bounds_ $)$ generates random mobile robot states
within bounds, and then the collision status is checked according to the 2D
collision map of the environment. collisionMap will be updated continuously in
another thread, hence the uncertainty of the environment can be captured and
colliding with obstacles is avoided. After getting
$\boldsymbol{t}_{e,i}^{b,i}$, we invoke queryCM() to retract the formation
metric stored at it. Finally, the pair of the valid sample
$\boldsymbol{q}_{b,i}$ and the corresponding $f_{{\scriptscriptstyle FC},i}$
are returned.

ASR can be built by calling sampleInASR() repeatedly. Fig. 4(b) shows the ASR
with different thres. The origin and direction of the arrow represent the
position and orientation of the mobile robot, respectively. The color
indicates the formation metric when the mobile robot locates at the arrow. As
can be seen, when thres is too low, there are lots of poses in ASR, and most
of them will lead to poor formations in the decentralized layer. When thres is
too high, there are few poses in ASR, and we have to spend much time in
sampleInASR(). Therefore, thres should be moderate to balance the quality of
the formation and the computational time of the centralized layer.

1 for  _$j\leftarrow 1$ to maxSample_ do

2

3 $\boldsymbol{q}_{b,i}$ $\leftarrow$ sampleMobileRobot(bounds);

4 if  __collision_($\boldsymbol{q}_{b,i}$, collisionMap) _is true__ then

5 continue

6 end if

7 $\boldsymbol{t}_{e,i}^{b,i}$ $\leftarrow$
getTransform($\boldsymbol{q}_{b,i},\boldsymbol{t}_{g,i}^{w}$);

8 $f_{{\scriptscriptstyle FC},i}$ $\leftarrow$
queryCM($\boldsymbol{t}_{e,i}^{b,i}$);

9 if  _$f_{{\scriptscriptstyle FC},i}$ _is not none__ then

10 return [$\boldsymbol{q}_{b,i},f_{{\scriptscriptstyle FC},i}$];

11

12 end if

13

14 end for

15return none;

Alg. 3 sampleInASR($\boldsymbol{t}_{g,i}^{w}$, bounds)

###  IV-C Decentralized Layer

As shown in Fig. 2, the decentralized layer consists of the formation
optimizer and the task-priority motion controller. It can be deployed in a
distributed way in real-time. Therefore, each robot could sense the
uncertainty of the environment and adjust its motion timely.

####  IV-C1 Formation Optimizer

The characteristic of formation control naturally leads to the question of
what variables to sense and what variables to control to achieve the desired
formation [11]. In previous works [38, 39, 40], the inter-robot variables,
such as the relative position and orientation among different robots, are
sensed and controlled. In these works, the system is usually treated as a
rigid body and losses the ability to cross obstacles. On the contrary, the
information between the robot and the object is easier to sense when rigid
grasp exists. Therefore, we measure the dexterity of each robot and control
the geometric variables relative to the object. In this way, the formation
metric of the complex system is decoupled, and the redundancy of each _MM_ can
be explored independently.

Figure 5: Geometric variables represent the relative information between the
robots and the object. $P_{O_{b,i}}$ and $P_{O_{obj}}$ are the projection of
the $i$th end effector $O_{e,i}$ and the object $O_{obj}$ on the
$X_{b,i}O_{b,i}Y_{b,i}$ plane, respectively. $\theta_{eo,i}$ is the angle
between the end effector and the object. $\theta_{eb,i}$ is the angle between
the end effector and the base. An ideal formation will be formed when
$\theta_{eo,i}$ and $\theta_{eb,i}$ are equal to zero for all $i=1,2,...,n$.

Different metrics were proposed to measure the dexterity of the open-chain
robot [51], and the most popular is the manipulability developed by Yoshikawa
[52].

$\omega(\boldsymbol{q}_{i})=\sqrt{det(\boldsymbol{J}_{i}(\boldsymbol{q}_{i})\boldsymbol{J}_{i}(\boldsymbol{q}_{i})^{T})}$
(9)

where $\boldsymbol{J}_{i}(\boldsymbol{q}_{i})$ is the analytical Jacobian
matrix of the $i$th _MM_. According to [51], $\omega(\boldsymbol{q}_{i})$ is
an unbounded index and only indicates the relative distance to singularity.
Therefore, it is normalized to obtain an absolute metric.

$\mu(\boldsymbol{q}_{i})=\frac{\omega(\boldsymbol{q}_{i})}{max(\omega(\boldsymbol{q}_{i}))}$
(10)

where ${max(\omega(\boldsymbol{q}_{i}))}$ is the maximum manipulability of the
$i$th _MM_ , hence $\mu(\boldsymbol{q}_{i})$ is limited within $[0,1]$.

To represent the relative information between the robots and the object,
geometric variables are defined in Fig. 5. $O_{e,i}$ and $O_{obj}$ are the
center of the $i$th end effector and the object, respectively. Their
projections on the $X_{b,i}O_{b,i}Y_{b,i}$ plane are represented as
$P_{O_{b,i}}$ and $P_{O_{obj}}$, respectively. $\theta_{eo,i}$ is the angle
between the $i$th end effector and the object. $\theta_{eb,i}$ represents the
angle between the $i$th end effector and the base. An ideal formation will be
formed when $\theta_{eo,i}$ and $\theta_{eb,i}$ are equal to zero for all
$i=1,2,...,n$. When they increase, the quality of the formation will decrease.
For example, the formation in Fig. 1 is better than that in Fig. 5. Therefore,
we define $r(\boldsymbol{q}_{i})$ to measure this relationship between the
$i$th _MM_ and the object.

$r(\boldsymbol{q}_{i})=f_{r}(\theta_{eb,i})\times f_{r}(\theta_{eo,i})$ (11)

where $f_{r}(.)$ is a continuous monotonic decreasing function. It reaches the
maximum of 1 when the independent variable is zero and gradually decreases to
0 as the independent variable increases. Therefore, $r(\boldsymbol{q}_{i})$ is
limited within $[0,1]$.

Combining $r(\boldsymbol{q}_{i})$ with the dexterity of the robot, the
formation metric of the $i$th _MM_ can be defined as follow.

$f_{{\scriptscriptstyle
FC},i}(\boldsymbol{q}_{i})=\mu(\boldsymbol{q}_{i})\times
r(\boldsymbol{q}_{i})$ (12)

It is a trade-off between $\mu(\boldsymbol{q}_{i})$ and
$r(\boldsymbol{q}_{i})$. As can be seen, no inter-robot information is
required in the definition of $f_{{\scriptscriptstyle
FC},i}(\boldsymbol{q}_{i})$, hence it can be optimized in a distributed way.

The formation metric of the system can be defined as the minimum
$f_{{\scriptscriptstyle FC},i}(\boldsymbol{q}_{i})$ of all robots. When
$f_{{\scriptscriptstyle FC},i}(\boldsymbol{q}_{i})$ reaches its maximum for
all robots, the system forms an ideal formation.

$f_{\scriptscriptstyle FC}(\boldsymbol{c})=min\\{f_{{\scriptscriptstyle
FC},i}(\boldsymbol{q}_{i})|1\leq i\leq n\\}$ (13)

1 Function  __optimize_($\boldsymbol{t}_{g,i}^{w}$,
$\boldsymbol{q}_{b,i}^{ref}$, $v_{max}$, $t_{opt}$, $\epsilon$)_

2 bounds $\leftarrow$ getBounds($\boldsymbol{q}_{b,i}^{ref}$, $v_{max}$,
$t_{opt}$);

3 for  _$t\leftarrow 0$ to $\epsilon$$t_{opt}$_ do

4 [$\boldsymbol{q}_{b,i}$,$f_{{\scriptscriptstyle
FC},i}$]$\leftarrow$sampleInASR($\boldsymbol{t}_{g,i}^{w}$,bounds);

5 $\boldsymbol{q}_{b,i}^{seed}$ $\leftarrow$ update($\boldsymbol{q}_{b,i}$,
$f_{{\scriptscriptstyle FC},i}$);

6

7 end for

8 $\boldsymbol{q}_{b,i}$ $\leftarrow$ iterate($\boldsymbol{q}_{b,i}^{seed}$,
$(1-\epsilon)$$t_{opt}$);

9 return $\boldsymbol{q}_{b,i}$;

10

11

Alg. 4 _Formation Optimizer_

The formation optimizer can be executed offline or online. Generally speaking,
online algorithm is superior to the offline version when dealing with the
uncertainty of the environment. However, it relies on the efficient method to
solve the constraint. As can be seen from Eq. (12), complex nonlinear
relationship exists between $f_{{\scriptscriptstyle FC},i}$ and
$\boldsymbol{q}_{i}$. Therefore, the convergence rate may be slow, and the
notorious local minimum may affect the optimization process.

To make the optimization process faster, once again, we query the precomputed
CM to determine the seed. In this way, the optimizer could start with a sub-
optimal formation and quickly converge to the optimal solution. The pseudo-
code of the formation optimizer is shown in Alg. 4.

Given the grasping pose $\boldsymbol{t}_{g,i}^{w}$, Alg. 4 generates the
mobile robot’s configuration $\boldsymbol{q}_{b,i}$ with the highest
$f_{{\scriptscriptstyle FC},i}$. $\boldsymbol{q}_{b,i}^{ref}$ is the reference
configuration of the mobile robot and can be set to the current state or the
state in the last iteration. $v_{max}$ is the maximum speed of the mobile
robot. $t_{opt}$ is the maximum time for the optimizer, and $\epsilon\in[0,1]$
is the time ratio allowed for searching the seed. bounds represents the area
that the mobile robot could travel from the reference configuration
$\boldsymbol{q}_{b,i}^{ref}$ with the maximum speed $v_{max}$ within the
allowed time $t_{opt}$. The motion of the mobile robot will violate the
maximum velocity constraint $v_{max}$ or the end effector pose constraint
$\boldsymbol{t}_{e,i}^{w}$ when $\boldsymbol{q}_{b,i}$ exceeds bounds.

Taking advantage of the precomputed CM, we sample in ASR within the allowed
time $\epsilon t_{opt}$ to get the seed, and then the optimizer starts from
$\boldsymbol{q}_{b,i}^{seed}$ and iterates to the optimal solution within the
remaining time $(1-\epsilon)$$t_{opt}$. The Nelder-Mead simplex algorithm [53]
is chosen as the solver in iterate(). It has no requirement on the
differentiability of $f_{{\scriptscriptstyle FC},i}$ and shows good
performance in practice.

####  IV-C2 Task-priority Motion Controller

The centralized layer plans the motion of the object and the end effector
$\boldsymbol{t}_{e,i}^{w}\in\mathbb{R}^{6}$. The formation optimizer in the
decentralized layer generates the optimal motion of the mobile robot
$\boldsymbol{t}_{b,i}^{w}\in\mathbb{R}^{n_{b,i}}$. To send joint motion
command to the lower level hardware, closed-loop motion controller should be
designed.

Historically, several algorithms were developed to compute the joint motion
command of the redundant robot, such as pseudo-inverse Jacobian-based,
Jacobian transpose-based, task-augmentation-based and task-priority-based
algorithms [20][21]. Task-priority-based algorithm outperforms others when
dealing with conflict among different tasks and is pruned to the mobile
manipulator in this paper.

In this algorithm, the motion of the end effector and the mobile robot are
treated as the primary task and the additional task, respectively. The
formulation of the task-priority motion controller is shown in Eq (14)
[13][22].

$\dot{\boldsymbol{q}}_{i}=\boldsymbol{J}_{i}^{{\dagger}}\dot{\boldsymbol{t}}_{e,i}^{w}+[\boldsymbol{J}_{b,i}(\boldsymbol{I}-\boldsymbol{J}_{i}^{{\dagger}}\boldsymbol{J}_{i})]^{{\dagger}}(\dot{\boldsymbol{t}}_{b,i}^{w}-\boldsymbol{J}_{b,i}\boldsymbol{J}_{i}^{{\dagger}}\dot{\boldsymbol{t}}_{e,i}^{w})$
(14)

where
$\boldsymbol{J}_{i}^{{\dagger}}=\boldsymbol{J}_{i}^{T}(\boldsymbol{J}_{i}\boldsymbol{J}^{T}_{i})^{-1}$
is the Moore-Penrose pseudoinverse of the Jacobian matrix
$\boldsymbol{J}_{i}(\boldsymbol{q}_{i})$. $\boldsymbol{I}$ is the identity
matrix with dimension $(n_{b,i}+n_{a,i})\times(n_{b,i}+n_{a,i})$.
$\boldsymbol{J}_{b,i}$ is the matrix related to the additional task and
derived as follows.

$\boldsymbol{J}_{b,i}=\frac{\partial{\boldsymbol{t}_{b,i}^{w}}}{\partial{\boldsymbol{q}_{i}}}=\left(\frac{\partial{\boldsymbol{t}_{b,i}^{w}}}{\partial{\boldsymbol{q}_{b,i}}},\frac{\partial{\boldsymbol{t}_{b,i}^{w}}}{\partial{\boldsymbol{q}_{a,i}}}\right)=\left(\boldsymbol{I}_{n_{b,i}\times
n_{b,i}},\boldsymbol{O}_{n_{b,i}\times n_{a,i}}\right)$

In Eq (14), the additional task is only satisfied in the null space of the
primary task. When they are in conflict, the primary task will be satisfied as
a higher priority. More detail about this algorithm is available in
[13][21][22].

The closed-loop version of Eq (14) is shown as follows.

$\dot{\boldsymbol{q}}_{i}=\boldsymbol{J}_{i}^{{\dagger}}\boldsymbol{\omega}_{e,i}+[\boldsymbol{J}_{b,i}(\boldsymbol{I}-\boldsymbol{J}_{i}^{{\dagger}}\boldsymbol{J}_{i})]^{{\dagger}}(\boldsymbol{\omega}_{b,i}-\boldsymbol{J}_{b,i}\boldsymbol{J}_{i}^{{\dagger}}\boldsymbol{\omega}_{e,i})$
(15)

where
$\boldsymbol{\omega}_{e,i}=\boldsymbol{K}_{e,i}\left({{}^{d}\boldsymbol{t}_{e,i}^{w}}-{{}^{c}\boldsymbol{t}_{e,i}^{w}}\right)$
is the tracking error between the desired and current poses of the end
effector.
$\boldsymbol{\omega}_{b,i}=\boldsymbol{K}_{b,i}\left({{}^{d}\boldsymbol{t}_{b,i}^{w}}-{{}^{c}\boldsymbol{t}_{b,i}^{w}}\right)$
is the tracking error between the desired and current poses of the mobile
robot. $\boldsymbol{K}_{e,i}\in\mathbb{R}^{6\times 6}$ and
$\boldsymbol{K}_{b,i}\in\mathbb{R}^{n_{b,i}\times n_{b,i}}$ are the constant
positive-definite gain matrix.

##  V SIMULATION AND EXPERIMENTAL RESULTS

###  V-A Experimental Setup

The redundancy of the _MM_ is first shown in Section V-B. After that, we
evaluate the performance of CM in Section V-C, which lays a foundation for the
following experiments. In Section V-D, the task-priority motion controller and
the relationship among $\boldsymbol{t}_{obj}^{w}$, $\boldsymbol{t}_{g,i}^{w}$
and $\boldsymbol{q}_{i}$ are tested. To demonstrate the superiority of the
proposed hierarchical motion planner, numerous simulations and real-world
experiments are conducted in Section V-E and Section V-F, respectively.

Moreover, different numbers of heterogeneous robots are considered in this
paper. As shown in Fig. 1, heterogeneous manipulators, including
UR5111https://www.universal-robots.com, ZU7222https://www.jaka.com and
Jaco333https://www.kinovarobotics.com, and heterogeneous mobile robots,
including the quadruped robot444https://unitree.com and the omnidirectional
wheeled mobile robot, will be tested. All the codes are implemented in the
architecture of ROS (Robot Operating System)[54] based on C++. CM is saved in
the point cloud format [55]. The computer is Intel i7-8700 (3.2GHz) with 32GB
memory. The attached video shows the result of simulations and real-world
experiments555https://youtu.be/c2Lxm15fY-c.

In the real-world experiments, all robots are equipped with wireless
transmission modules in the same local network for efficient communication.
Leader-follower approach is adopted for the system, where the leader plans the
global path of the object and each follower is equipped with a 6D force-torque
sensor and a visual sensor on the end effector to detect the motion relative
to the leader. Moreover, each robot is equipped with a laser scan to sense the
static and dynamic obstacles in the environment.

###  V-B Redundancy of the Mobile Manipulator

Figure 6: Redundancy of the mobile manipulator. The mobile robot moves on the
ground while the end effector keeping still.

The redundancy of the _MM_ is shown in Fig. 6. As can be seen, when the end
effector keeps still, there is extra _DoF_ for the robot to move on the
ground. This is called the self-motion of the redundant robot. It represents
the null space of the Jacobian matrix $\boldsymbol{J}_{i}(\boldsymbol{q}_{i})$
and corresponds to the second term on the right side of Eq. (14). Redundancy
is the basis for formation optimization, but it also brings challenges to the
motion planning problem.

###  V-C Performance of the Capability Map

In this part, we evaluate the time cost when checking the validity of ASR by
querying CM and compare the results with other inverse kinematic solvers on
the heterogeneous robots. The mean time of the checking process for $1\times
10^{3}$ samples is shown in Table I.

TABLE I: mean time when checking the validity of _ASR_ ($1\times 10^{3}$ samples) Robot |  KDL[56] |  TRAC-IK[57] |  IKFast[58] | CM(ours)
---|---|---|---|---
UR5-based MM (ms) | 4813.4 | 471.3 | 441.7 | 0.26
ZU7-based MM (ms) | 9406.8 | 751.4 | 440.5 | 0.26
Jaco-based MM (ms) | 7843.1 | 3937.0 | 409.7 | 0.27

KDL (Kinematics and Dynamics Library) provides the Jacobian-based inverse
kinematic solver and is a component of Orocos [56]. TRAC-IK [57] is built on
top of KDL and also combines the sequential quadratic nonlinear optimization
approach to handle joint limits. IKFast is a powerful inverse kinematics
solver that can analytically solve different manipulators’ kinematic equations
[58]. From Table I, we can see that querying CM is faster than invoking the
inverse kinematic solvers. Moreover, CM querying time is stable, which is a
good property when planning the motion of heterogeneous robots.

###  V-D Task-Priority Motion Controller and Multiple Robots System Model

The task-priority motion controller and the relationship among
$\boldsymbol{t}_{obj}^{w}$, $\boldsymbol{t}_{g,i}^{w}$ and
$\boldsymbol{q}_{i}$ are tested in this part. The system consists of three
robots and one object. The orientation (_roll-pitch-yaw_) of the object is
required to move along the sinusoidal trajectory while the center (_x-y-z_)
keeps stationary. For each object’s pose $\boldsymbol{t}_{obj}^{w}$, the
grasping poses vector $\boldsymbol{G}$ will be calculated according to Eq.
(3). Given the grasping pose $\boldsymbol{t}_{g,i}^{w}$, the formation
optimizer plans the optimal mobile robot’s pose $\boldsymbol{t}_{b,i}^{w}$.
Finally, the task-priority motion controller receives
$\boldsymbol{t}_{g,i}^{w}$ and $\boldsymbol{t}_{b,i}^{w}$, and calculates the
desired joint velocity $\dot{\boldsymbol{q}}_{i}$ based on Eq. (15).

Figure 7: The orientation (_roll-pitch-yaw_) of the object is required to move
along the sinusoidal trajectory while the center (_x-y-z_) of the object,
which is represented as the blue point, keeps stationary. Figure 8: (a)
Trajectory of the object in _roll_ direction. (b)-(d) Trajectories of the
mobile robot in (_x, y, yaw_) directions. (e)-(j) Trajectories of end effector
in (_x, y, z, roll, pitch, yaw_) directions.

The snapshots during the task are shown in Fig. 7, and the motion of the
system is available in the attached video. Fig. 8(a) shows the desired and
actual trajectories of the object in _roll_ direction, and the trajectories in
_pitch_ and _yaw_ directions are similar to that in _roll_ direction and not
shown here. The desired and actual trajectories of the mobile robot and the
end effector of _MM1_ are shown in Fig. 8(b)-(j), respectively. The
trajectories of _MM2_ and _MM3_ are similar to that of _MM1_ and not shown
here. As can be seen from Fig. 8, the mobile robot and the end effector are
able to track the desired trajectories simultaneously. Meanwhile, multiple
robots could cooperate with each other and manipulate the object precisely.

###  V-E Performance of the Hierarchical Motion Planner

####  V-E1 Simulation Scene

As obstacles or the number of robots increase, the simulation scenes are shown
in Fig. 9(a)-(f). The size of the scenes is $15m\times 25m$. The start and
goal states are displayed on the most left and most right, respectively.
Snapshots during the tasks are also shown in Fig. 9. The pictorial motion of
the tasks is available in the attached video. In these tasks, the start and
goal states are assumed to satisfy the closed-chain and collision constraints,
and valid paths connecting the start and goal states exist theoretically.
Moreover, the height of some obstacles is lower than the maximum allowed
height of the object, which means that obstacles may be crossed by the system.

####  V-E2 Simulation 1

Figure 9: (a)-(f). Simulation scenes from simple to complex. The size of the scenes is $15m\times 25m$. Robot models are snapshots during the tasks. The height of some obstacles is lower than the maximum allowed height of the object, hence obstacles may be crossed by the system like the path in (b)-(f). TABLE II: comparison of the proposed hierarchical framework with centralized frameworks (_PJ_ [32][33], _AT_ [35] and _TB_ [36])  Task | Mehtod |  RRT[59] |  RRTConnect[47] |  BKPIECE[60] |  EST[46] |  STRIDE[61] |  PRM[45]
---|---|---|---|---|---|---|---
| Success/
---
Total
| Time(s)
---
| Success/
---
Total
| Time(s)
---
| Success/
---
Total
| Time(s)
---
| Success/
---
Total
| Time(s)
---
| Success/
---
Total
| Time(s)
---
| Success/
---
Total
| Time(s)
---
Fig. 9(a) |  _PJ_[32][33] | 0/10 | — | 0/10 | — | 0/10 | — | 9/10 |  7.75$\pm$6.39 | 2/10 |  11.87$\pm$3.80 | 10/10 |  23.48$\pm$2.34
_AT_[35] | 0/10 | — | 10/10 |  3.37$\pm$0.55 | 0/10 | — | 10/10 |  1.78$\pm$1.11 | 7/10 |  6.59$\pm$6.28 | 10/10 |  21.73$\pm$1.53
_TB_[36] | 3/10 |  17.64$\pm$3.80 | 10/10 |  2.29$\pm$0.43 | 0/10 | — | 10/10 |  1.58$\pm$1.71 | 7/10 |  3.60$\pm$2.72 | 10/10 |  22.31$\pm$1.55
IKCL (ours) | 10/10 |  7.34$\pm$4.34 | 10/10 |  0.97$\pm$0.10 | 0/10 | — | 7/10 |  5.20$\pm$3.62 | 4/10 |  3.60$\pm$3.70 | 10/10 |  20.25$\pm$0.19
CMCL (ours) | 10/10 |  1.10$\pm$0.28 | 10/10 | 0.74$\pm$0.20 | 9/10 |  14.21$\pm$5.24 | 10/10 |  1.42$\pm$0.66 | 10/10 |  1.36$\pm$0.74 | 10/10 |  20.19$\pm$0.16
Fig. 9(b) |  _PJ_[32][33] | 0/10 | — | 0/10 | — | 0/10 | — | 0/10 | — | 0/10 | — | 0/10 | —
_AT_[35] | 4/10 |  12.85$\pm$4.92 | 8/10 |  4.21$\pm$2.76 | 0/10 | — | 5/10 |  6.12$\pm$6.49 | 0/10 | — | 6/10 |  22.90$\pm$1.81
_TB_[36] | 7/10 |  10.64$\pm$2.40 | 8/10 |  5.71$\pm$5.75 | 0/10 | — | 5/10 |  4.67$\pm$3.81 | 0/10 | — | 9/10 |  23.75$\pm$6.42
IKCL (ours) | 10/10 |  0.81$\pm$1.11 | 10/10 |  3.34$\pm$1.56 | 3/10 |  20.01$\pm$2.56 | 10/10 |  12.17$\pm$6.05 | 10/10 |  8.77$\pm$6.56 | 8/10 |  20.20$\pm$0.18
CMCL (ours) | 10/10 | 0.18$\pm$0.10 | 10/10 |  1.74$\pm$0.61 | 8/10 |  19.25$\pm$2.93 | 10/10 |  6.35$\pm$4.39 | 10/10 |  6.38$\pm$5.88 | 10/10 |  20.69$\pm$0.05
Fig. 9(c) |  _PJ_[32][33] | 0/10 | — | 0/10 | — | 0/10 | — | 0/10 | — | 0/10 | — | 0/10 | —
_AT_[35] | 0/10 | — | 0/10 | — | 0/10 | — | 0/10 | — | 0/10 | — | 0/10 | —
_TB_[36] | 0/10 | — | 0/10 | — | 0/10 | — | 0/10 | — | 0/10 | — | 0/10 | —
IKCL (ours) | 10/10 |  8.82$\pm$6.09 | 10/10 |  6.99$\pm$5.71 | 0/10 | — | 8/10 |  14.40$\pm$8.57 | 9/10 |  17.77$\pm$8.37 | 7/10 |  35.32$\pm$5.72
CMCL (ours) | 10/10 |  2.35$\pm$1.10 | 10/10 | 1.84$\pm$0.48 | 3/10 |  12.90$\pm$6.06 | 10/10 |  5.39$\pm$2.30 | 10/10 |  8.95$\pm$7.06 | 10/10 |  31.11$\pm$0.24
Fig. 9(d) |  _PJ_[32][33] | 0/10 | — | 0/10 | — | 0/10 | — | 0/10 | — | 0/10 | — | 0/10 | —
_AT_[35] | 0/10 | — | 0/10 | — | 0/10 | — | 0/10 | — | 0/10 | — | 0/10 | —
_TB_[36] | 0/10 | — | 0/10 | — | 0/10 | — | 0/10 | — | 0/10 | — | 0/10 | —
IKCL (ours) | 10/10 |  8.30$\pm$4.48 | 10/10 |  7.98$\pm$7.06 | 0/10 | — | 7/10 |  16.79$\pm$9.34 | 4/10 |  22.46$\pm$2.99 | 10/10 |  33.47$\pm$1.10
CMCL (ours) | 10/10 | 3.24$\pm$1.31 | 10/10 |  3.36$\pm$0.97 | 2/10 |  18.73$\pm$2.37 | 10/10 |  9.15$\pm$3.05 | 9/10 |  7.94$\pm$3.91 | 10/10 |  32.20$\pm$0.45
Fig. 9(e) |  _PJ_[32][33] | 0/10 | — | 0/10 | — | 0/10 | — | 0/10 | — | 0/10 | — | 0/10 | —
_AT_[35] | 0/10 | — | 0/10 | — | 0/10 | — | 0/10 | — | 0/10 | — | 0/10 | —
_TB_[36] | 0/10 | — | 0/10 | — | 0/10 | — | 0/10 | — | 0/10 | — | 0/10 | —
IKCL (ours) | 10/10 |  23.84$\pm$12.28 | 10/10 |  27.09$\pm$12.72 | 0/10 | — | 0/10 | — | 1/10 |  46.41$\pm$0 | 6/10 |  54.41$\pm$2.66
CMCL (ours) | 10/10 | 5.88$\pm$2.20 | 10/10 |  6.53$\pm$3.15 | 0/10 | — | 10/10 |  27.47$\pm$9.96 | 5/10 |  28.49$\pm$17.64 | 8/10 |  52.72$\pm$0.78
Fig. 9(f) |  _PJ_[32][33] | 0/10 | — | 0/10 | — | 0/10 | — | 0/10 | — | 0/10 | — | 0/10 | —
_AT_[35] | 0/10 | — | 0/10 | — | 0/10 | — | 0/10 | — | 0/10 | — | 0/10 | —
_TB_[36] | 0/10 | — | 0/10 | — | 0/10 | — | 0/10 | — | 0/10 | — | 0/10 | —
IKCL (ours) | 10/10 |  10.17$\pm$3.89 | 10/10 |  5.73$\pm$1.91 | 1/10 |  47.58$\pm$0 | 10/10 |  26.06$\pm$10.89 | 10/10 |  19.10$\pm$6.51 | 10/10 |  53.95$\pm$1.35
CMCL (ours) | 10/10 |  5.12$\pm$1.74 | 10/10 | 4.77$\pm$1.65 | 2/10 |  24.71$\pm$13.34 | 10/10 |  8.25$\pm$2.94 | 10/10 |  12.15$\pm$4.67 | 10/10 |  53.11$\pm$0.87

In this simulation, we compare the proposed hierarchical framework with the
centralized frameworks, such as Projection (PJ) [32][33], _Atlas (AT)_ [35]
and Tangent Bundle (TB) [36]. In addition, we also compare the performance of
IKCL with CMCL to see the advantage of CM. As discussed in Section II-B, _PJ_
computes the Jacobian of the system and projects a random sample
$\boldsymbol{c}$ onto the manifold based on Newton procedure666The constrained
Jacobian matrix of the system and the iterative procedure of the projection-
based framework are available in the Appendix.. _AT_ and _TB_ are built on top
of _PJ_ with some modifications. For the fairness of the comparison, in IKCL,
the inverse kinematic solver is chosen as KDL, which is based on the Jacobian
of the single _MM_.

In the tasks shown in Fig. 9(a)-(f), the start state $\boldsymbol{t}_{start}$
and goal state $\boldsymbol{t}_{goal}$ of the object are specified by the
user. However, _PJ_ , _AT_ and _TB_ are centralized frameworks. Not only the
state of the object but also the state of the robots should be specified.
Therefore, a random goal state $\boldsymbol{c}_{goal}$ will be selected for
each scene so that $\boldsymbol{c}_{goal}\in\mathcal{C}_{\scriptscriptstyle
C^{3}}\cap\mathcal{C}_{free}$ and
$\pi(\boldsymbol{c}_{goal})=\boldsymbol{t}_{goal}$. Moreover, formation
constraint can not be resolved directly in _PJ_ , _AT_ and _TB_ , hence only
closed-chain and collision constraints are considered in these centralized
frameworks. _PJ_ , _AT_ and _TB_ on the multiple mobile manipulators system
are developed based on OMPL [62], which is a famous sampling-based motion
planning software.

The performances of different frameworks are shown in Table II. The terms and
settings are explained as follows. The maximum allowed planning time $t_{obj}$
in Alg. 1 is set to $20s$, $20s$, $30s$, $30s$, $50s$ and $50s$ for the tasks
in Fig. 9(a)-(f), respectively. Each framework is combined with six random
planning algorithms, namely RRT [59], RRTConnect [47], BKPIECE [60], EST [46],
STRIDE[61] and PRM[45]. Each combination runs 10 times to get the statistical
results. “Success/Total” columns represent the successful and total
simulations conducted. “Time(s)” columns represent the mean time and the
standard deviation of the successful simulations. When all simulations fail,
“Time(s)” columns are set to “—”.

PRM constructs a roadmap of the entire environment that can be used for
multiple queries. It runs out of the given time to build the roadmap and then
searches a valid path. Due to the post-process of the motion planner, the time
cost of PRM will be slightly over the maximum allowed time (see the last
column in Table II). The other five planning algorithms construct trees that
can be used for a single query. They terminate as long as finding a valid
path. BKPIECE relies heavily on the projection evaluator to guide the
exploration of the continuous space. However, designing an efficient
projection evaluator for the high-dimensional constrained space is nontrivial,
hence it shows the worst performance in the simulations. From Fig. 9 and Table
II, we can also get the following results.

Overall, the performance of _PJ_ , _AT_ and _TB_ increases sequentially on
these tasks, which is similar to the results reported in the literature. They
work well when the scenes are simple, like the task in Fig. 9(a). When
obstacles increase, their performance degrades significantly. Moreover, we
also found that _PJ_ -like frameworks are sensitive to the hyper-parameters
when debugging the code, such as the constraint tolerance in _PJ_ and the
maximum radius of the chart validity region in _AT_ and _TB_ , which puts
extra burdens on the developers.

From Table II, we know that IKCL outperforms _PJ_ -like frameworks
significantly. This owes to the proposed framework as we plan the motion of
the object and the robots hierarchically while _PJ_ -like frameworks plan the
motion of the system simultaneously. Comparing the results of IKCL with CMCL,
we can see that CM can further improve the performance of the proposed motion
planner. The combination of these features makes our motion planner outperform
the benchmark significantly. In addition, RRTConnect (or RRT) shows the best
performance when working with our framework, hence it is chosen as the default
algorithm in the following experiments.

####  V-E3 Simulation 2

TABLE III: performance of the decoupled framework [44]

Task |  _RRT_ [59] |  _RRTConnect_ [47] |  _BKPIECE_ [60] |  _EST_ [46] |  _STRIDE_ [61] |  _PRM_ [45]
---|---|---|---|---|---|---
| Success/
---
Total
Time(s) |  | Success/
---
Total
Time(s) |  | Success/
---
Total
Time(s) |  | Success/
---
Total
Time(s) |  | Success/
---
Total
Time(s) |  | Success/
---
Total
Time(s)
Fig. 9(a) | 10/10 |  0.06$\pm$0.01 | 10/10 |  0.05$\pm$0.02 | 10/10 |  0.77$\pm$0.35 | 10/10 |  0.08$\pm$0.04 | 10/10 |  0.07$\pm$0.03 | 10/10 |  20.01$\pm$0.00
Fig. 9(b) | 10/10 |  0.04$\pm$0.01 | 10/10 |  0.05$\pm$0.01 | 10/10 |  0.53$\pm$0.20 | 10/10 |  0.07$\pm$0.03 | 10/10 |  0.09$\pm$0.03 | 10/10 |  20.01$\pm$0.00
Fig. 9(c) | 0/10 | — | 0/10 | — | 0/10 | — | 0/10 | — | 0/10 | — | 0/10 | —
Fig. 9(d) | 0/10 | — | 0/10 | — | 0/10 | — | 0/10 | — | 0/10 | — | 0/10 | —
Fig. 9(e) | 0/10 | — | 0/10 | — | 0/10 | — | 0/10 | — | 0/10 | — | 0/10 | —
Fig. 9(f) | 0/10 | — | 0/10 | — | 0/10 | — | 0/10 | — | 0/10 | — | 0/10 | —

Figure 10: Robot path planned by the decoupled framework [44]. Although the
motion of the object is collision-free, [44] is unable to find a valid path
for the robots that satisfies the collision, closed-chain and formation
constraints simultaneously.

In this simulation, we compare the proposed hierarchical framework with the
decoupled framework. When the closed-chain constraint and the lower bound of
the formation constraint are not guaranteed in Fig. 2, or ASR is not checked
in Alg. 1 (line 28-33), the centralized layer and the decentralized layer
become fully decoupled, resulting in a framework similar to [44]. The
performance of the decoupled framework in the tasks in Fig. 9(a)-(f) is shown
in Table III.

When the scenes and the tasks are simple, the centralized layer and the
decentralized layer are always compatible, so checking the closed-chain
constraint and the lower bound of the formation constraint can be avoided.
Therefore, the decoupled framework performs well in Fig. 9(a)-(b). However,
obstacles will complicate the motion planning problem and make the two layers
in conflict. Therefore, in Fig. 9(c)-(f), the performance of the decoupled
framework decreases dramatically. A failure case is shown in Fig. 10. As can
be seen, although the motion of the object is collision-free, [44] cannot find
a valid path for the robots that satisfies the collision, closed-chain and
formation constraints simultaneously.

####  V-E4 Simulation 3

In this simulation, we compare the proposed hierarchical framework with
algorithms based on the virtual structure. In [39], the object and the robots
are encircled by polygons on the ground. As long as the virtual structure
intersects with obstacles, the system is considered in collision. Therefore,
the obstacle-avoidance strategy is conservative. Take the task in Fig. 9(b) as
an example. Our motion planner is able to find a shorter path that crosses
obstacles. However, the path in Fig. 9(b) becomes infeasible for [39], and
they can only find a longer path that bypasses obstacles, which is shown in
Fig. 11. Crossing obstacles is an essential ability for the multiple robots
system and rarely mentioned in the previous works. It changes the topological
structure of the configuration space and will increase the success rate of the
motion planner in cluttered environments.

Figure 11: Robot path planned by algorithms based on the virtual structure
[39]. The dotted lines represent the virtual polygon that encircles the robots
and the object. Compared with the path in Fig. 9(b), the proposed motion
planner is able to find a shorter path that crosses obstacles, while [39] only
find a longer path that bypasses obstacles.

###  V-F Real-World Experiments

In this part, we extend the proposed motion planner to real-world multiple
mobile manipulators. $v_{max}$, $t_{opt}$ and $\epsilon$ in Alg. 4 are set to
$0.1m/s$, $40ms$ and 0.5, respectively. $\boldsymbol{K}_{b,i}$ and
$\boldsymbol{K}_{e,i}$ in the task-priority motion controller are set to
$2\times\boldsymbol{I}_{3\times 3}$ and $2\times\boldsymbol{I}_{6\times 6}$,
respectively. $f_{r}(.)$ in Eq. (11) is chosen as the sixth-order polynomial
due to its continuity and simplicity. thres is set to 0.4 to couple the
centralized and decentralized layers.

Figure 12: Scene of the real-world transportation task by two robots. Figure
13: (a)-(b) Trajectories of the object in the task shown in Fig. 12. (c)-(d)
$\mu(\boldsymbol{q})$ and $r(\boldsymbol{q})$ of _MM1_ and _MM2_ during the
task.  Figure 14: Scene of the real-world transportation task by three robots.

####  V-F1 Experiment 1

In this experiment, we conduct the cooperative transportation task by two
robots in real-world environment. Obstacles include a pedestrian and two
slopes with varying angles and heights. The system should transport the object
to the destination while avoiding static and dynamic obstacles. The
decentralized layer will optimize the formation of the system in real-time and
computes the desired joint motion command based on the task-priority motion
controller. Snapshots during the task are shown in Fig. 12. The trajectories
of the object, and $\mu(\boldsymbol{q})$ and $r(\boldsymbol{q})$ of _MM1_ and
_MM2_ are shown in Fig. 13. The motion process is explained as follows.

  * •

At _t=0s_ , the system is far away from obstacles. Both _MM1_ and _MM2_ start
with an optimal configuration.

  * •

From _t=5s_ to _t=10s_ , _MM1_ meets the pedestrian twice. As a redundant
robot, it is able to avoid the dynamic obstacle while not affecting the
transportation task. As a punishment, $\mu(\boldsymbol{q})$ of _MM1_ is
sacrificed (Fig. 13(c)). However, when the obstacle moves away,
$\mu(\boldsymbol{q})$ can be recovered to a high value.

  * •

From _t=28s_ to _t=35s_ , the system adjusts _z_ and _roll_ of the object to
suit the height and direction of _slope1_. Thanks to the real-time formation
optimizer, $\mu(\boldsymbol{q})$ and $r(\boldsymbol{q})$ of the two robots
remain at a high value during this period.

  * •

From _t=49s_ to _t=55s_ , the height of the _MM1_ ’s and _MM2_ ’s end effector
decreases and increases, respectively, which means that the system adjusts
_pitch_ of the object to suit the height and direction of _slope2_. During
this period, _MM2_ gradually approaches the boundary of the workspace in _z_
direction, hence $\mu(\boldsymbol{q})$ decreases quickly (Fig. 13(d)).

  * •

From _t=75s_ to _t=80s_ , the system has crossed all obstacles and then lowers
the object to finish the autonomous cooperative transportation task. Finally,
the height of the _MM2_ ’s end effector decreases, and $\mu(\boldsymbol{q})$
returns to a high value.

####  V-F2 Experiment 2

In this experiment, we conduct the cooperative transportation task by three
robots in real-world environment. Obstacles include a pedestrian and a cuboid
with a certain height. Other settings are similar to the former experiment.
Snapshots during the task are shown in Fig. 14, and the pictorial motion is
available in the attached video. From this and the former experiments, we can
see that multiple robots are able to cooperate with each other to realize the
transportation task in cluttered environments. Moreover, each robot could plan
the optimal joint configuration and track the desired trajectories precisely.

TABLE IV: features of different motion planning frameworks when dealing with
different constraints

Method | Closed-Chain | Formation | Redundancy | Obstacle-Avoidance | Other Key Features
---|---|---|---|---|---
Centralized Framework [32][33][35][36]) | ✓ | ✗ | ✗ | bypass and cross | numerical projection; time-consuming
Decoupled Framework [44] | ✓ | ✗ | ✗ | bypass and cross | conflict between different layers
Virtual Structure-based [39] | ✓ | ✓ | ✗ | only bypass | only planar object’s motion
Hierarchical Framework (_ours_) | ✓ | ✓ | ✓ | bypass and cross |  | time-efficient; compatibility between
---
different layers; spatial object’s motion

###  V-G Discussions

As can be seen from Fig. 2, the proposed hierarchical framework is partially
coupled and partially decoupled. Coupling occurs between different layers,
which means that we guarantee the closed-chain constraint
$f_{\scriptscriptstyle C^{3}}(\boldsymbol{c})$ and the lower bound of the
formation constraint $f_{\scriptscriptstyle FC}(\boldsymbol{c})$ in the
centralized layer, and then optimize $f_{\scriptscriptstyle
FC}(\boldsymbol{c})$ on the basis of obeying $f_{\scriptscriptstyle
C^{3}}(\boldsymbol{c})$. Decoupling arises among different robots, which means
that the decentralized layer is fully distributed, and the redundancy of each
robot can be explored independently in real-time.

Compared with the centralized frameworks, which use the numerical projection
operator to generate valid samples and plan the motion of the object and the
robots simultaneously, the hierarchical feature can simplify the motion
planning problem of the high-dimensional system, which is verified in Section
V-E2. Compared with the decoupled frameworks, which plan the motion of the
object and the robots separately, coupling the two layers can avoid conflict
between them and enhance the performance of the motion planner significantly,
which is verified in Section V-E3. Compared with the virtual structure-based
algorithms, the proposed motion planner exhibits diversified and efficient
obstacle-avoidance skills, such as bypassing and crossing, which is also
verified in Section V-E4. The features of different frameworks when dealing
with the closed-chain, formation, redundancy and obstacle-avoidance
constraints are summarized in Table IV.

Another advantage of the proposed motion planner is CM, and its effects are
twofold. On the one hand, it accelerates the performance of the centralized
layer, which is verified by comparing IKCL with CMCL. On the other hand, it
helps the optimization algorithm determine the seed, which speeds up the
decentralized layer and effectively avoids local minimum. CM is essential when
the motion planning time is a key index. However, the disadvantage of CM is
that extra storage space is needed, and it has to be loaded from the local
hard disk when the program starts. In the future, CM can be saved in the cloud
server and treated as an infrastructure to help boost the motion planner.

The proposed framework can be applied to different numbers of heterogeneous
mobile manipulators. To the best of our knowledge, such complex applications
have not been seen before in the robotic community. However, there is still a
long way to go to deal with multiple heterogeneous robots. For example, the
speed, payload, sensing capability, kinematic and dynamic characteristics of
each robot may be different, hence the planner should allocate proper motion
for each robot. In addition, some robots may crash when working, and the
motion planner should monitor the status of each robot and prepare the
emergency measure for such cases.

Another interesting ability for multiple mobile manipulators is avoiding
dynamic obstacles. A simple case is shown in the experiment. However, dynamic
obstacles may be complex. For example, they may be fast and lock the motion of
all robots simultaneously. In such cases, what strategy to take and how to
balance the motion of different robots are still open questions.

##  VI CONCLUSIONS

In this paper, we proposed a hierarchical framework to deal with the motion
planning of multiple redundant mobile manipulators. The motion of the object
is planned in the centralized layer offline. In the decentralized layer, the
redundancy of each robot is explored independently in real-time to achieve the
desired formation and dexterous configuration. Closed-chain, obstacle-
avoidance and the lower bound of the formation constraints are checked in the
centralized layer to ensure the object’s motion is executable in the
decentralized layer. The hierarchical framework simplifies the complex motion
planning problem while not violating the constraints, and its superiority is
verified in the experiments. In addition, a novel tool named CM is introduced
to boost the performance of the motion planner. The validity of the object’s
pose in the centralized layer can be checked quickly by looking up CM. The
seed of the optimization algorithm in the decentralized can also be determined
by querying it. The proposed framework outperforms the golden benchmark motion
planners significantly and can be applied to different numbers of
heterogeneous mobile manipulators, which are also verified in the simulated
and real-world experiments.

## APPENDIX

###  VI-A Projection Operation for the Centralized Frameworks

1 Function  __projection_($\boldsymbol{c},\varepsilon$)_

// constrained error by Eq.(4)

2 $\Delta\boldsymbol{x}$ $\leftarrow$ $f_{\scriptscriptstyle
C^{3}}(\boldsymbol{c})$;

3 while  _$\|\boldsymbol{x}\| >\varepsilon$_ do

4 $\Delta\boldsymbol{c}\leftarrow\boldsymbol{J}_{\scriptscriptstyle
C^{3}}^{\dagger}(\boldsymbol{c})\Delta\boldsymbol{x}$;

5 $\boldsymbol{c}\leftarrow\boldsymbol{c}-\Delta\boldsymbol{c}$;

6 $\Delta\boldsymbol{x}$ $\leftarrow$ $f_{\scriptscriptstyle
C^{3}}(\boldsymbol{c})$;

7

8 end while

9

10

Alg. 5 _Projection Through Iteration_

Projection is the basis for the centralized frameworks [32][33][35][36] in
Section V-E2. Alg. 5 shows the iterative procedure that uses the pseudo-
inverse Jacobian matrix of the closed-chain constraint function
$f_{\scriptscriptstyle C^{3}}(\boldsymbol{c})$.

Given a random sample $\boldsymbol{c}$, Alg. 5 computes the constrained error
according to Eq.(4) and compares it with the maximum allowed error
$\varepsilon$. When the error is larger than $\varepsilon$, the pseudo-inverse
of the constrained Jacobian matrix $\boldsymbol{J}_{\scriptscriptstyle
C^{3}}^{\dagger}(\boldsymbol{c})$ will be calculated to project the random
sample by the gradient descent operation.

###  VI-B Constrained Jacobian Matrix

In this part, we derive the constrained Jacobian matrix
$\boldsymbol{J}_{\scriptscriptstyle
C^{3}}(\boldsymbol{c})\in\mathbb{R}^{6n\times\sum_{i=1}^{n}{(n_{a,i}+n_{b,i})+6}}$
of the system.

$\boldsymbol{J}_{\scriptscriptstyle
C^{3}}(\boldsymbol{c})=\frac{\partial{f_{\scriptscriptstyle
C^{3}}(\boldsymbol{c})}}{\partial{\boldsymbol{c}}}=\frac{\partial{\boldsymbol{E}}-\partial{\boldsymbol{G}}}{\partial{\boldsymbol{c}}}$
(16)

According to the definition of $\boldsymbol{E}$ and $\boldsymbol{G}$ in
Section III-B, $\boldsymbol{E}$ has no relationship with
$\boldsymbol{t}_{obj}^{w}$, and $\boldsymbol{G}$ has no relationship with
$\boldsymbol{q}_{i}$. Therefore, Eq. (16) can be simplified to Eq. (17).

$\frac{\partial{\boldsymbol{E}}-\partial{\boldsymbol{G}}}{\partial{\boldsymbol{c}}}=\left(\frac{\partial{\boldsymbol{E}}}{\partial{\boldsymbol{q}_{1}}},...,\frac{\partial{\boldsymbol{E}}}{\partial{\boldsymbol{q}_{i}}},...,\frac{\partial{\boldsymbol{E}}}{\partial{\boldsymbol{q}_{n}}},-\frac{\partial{\boldsymbol{G}}}{\partial{\boldsymbol{t}_{obj}^{w}}}\right)$
(17)

where $\frac{\partial{\boldsymbol{E}}}{\partial{\boldsymbol{q}_{i}}}$ and
$\frac{\partial{\boldsymbol{G}}}{\partial{\boldsymbol{t}_{obj}^{w}}}$ are
defined in Eq. (18) and Eq. (19).

$\displaystyle\frac{\partial{\boldsymbol{E}}}{\partial{\boldsymbol{q}_{i}}}=\left(\frac{\partial{f_{k}(\boldsymbol{q}_{1})}}{\partial{\boldsymbol{q}_{i}}},...,\frac{\partial{f_{k}(\boldsymbol{q}_{i})}}{\partial{\boldsymbol{q}_{i}}},...,\frac{\partial{f_{k}(\boldsymbol{q}_{n})}}{\partial{\boldsymbol{q}_{i}}}\right)^{T}$
(18)
$\displaystyle\frac{\partial{\boldsymbol{G}}}{\partial{\boldsymbol{t}_{obj}^{w}}}=\left(\frac{\partial{\boldsymbol{t}_{g,1}^{w}}}{\partial{\boldsymbol{t}_{obj}^{w}}},...,\frac{\partial{\boldsymbol{t}_{g,i}^{w}}}{\partial{\boldsymbol{t}_{obj}^{w}}},...,\frac{\partial{\boldsymbol{t}_{g,n}^{w}}}{\partial{\boldsymbol{t}_{obj}^{w}}}\right)^{T}$
(19)

where
$\frac{\partial{f_{k}(\boldsymbol{q}_{j})}}{\partial{\boldsymbol{q}_{i}}}$ is
given by Eq. (20).

$\frac{\partial{f_{k}(\boldsymbol{q}_{j})}}{\partial{\boldsymbol{q}_{i}}}=\begin{cases}\boldsymbol{J}_{i}(\boldsymbol{q}_{i}),&if\
i=j\\\ \boldsymbol{O}_{6\times(n_{a,i}+n_{b,i})},&if\ i\neq j\end{cases}$ (20)

where $\boldsymbol{J}_{i}(\boldsymbol{q}_{i})$ is the analytical Jacobian
matrix of the $i$th _MM_.

Suppose the constant homogeneous transformation of frame
$O_{g,i}X_{g,i}Y_{g,i}Z_{g,i}$ relative to frame
$O_{obj}X_{obj}Y_{obj}Z_{obj}$ is

$\boldsymbol{X}_{g,i}^{obj}=\left(\begin{array}[]{cc}\boldsymbol{R}_{g,i}^{obj}&\boldsymbol{p}_{g,i}^{obj}\\\
\boldsymbol{O}_{1\times 3}&1\\\ \end{array}\right).$

Suppose the velocity of a frame is
$\boldsymbol{\xi}=(\boldsymbol{v}^{T},\boldsymbol{\omega}^{T})^{T}$, in which
$\boldsymbol{v}$ and $\boldsymbol{\omega}$ represent the linear and angular
velocity of the frame. According to [63], Eq. (21) holds between
$\boldsymbol{\xi}_{g,i}^{w}$ and $\boldsymbol{\xi}_{obj}^{w}$.

$\boldsymbol{\xi}_{g,i}^{w}=\left(\begin{array}[]{cc}\boldsymbol{I}_{3\times
3}&-S(\boldsymbol{p}_{g,i}^{w})\\\ \boldsymbol{O}_{3\times
3}&\boldsymbol{I}_{3\times 3}\\\ \end{array}\right)\boldsymbol{\xi}_{obj}^{w}$
(21)

where $S(.)$ is the skew-symmetric matrix operator. In addition, we use a
minimum description to represent the orientation in $\boldsymbol{t}$, hence
Eq. (22) holds between $\dot{\boldsymbol{t}}$ and $\boldsymbol{\xi}$.

$\boldsymbol{\xi}=\left(\begin{array}[]{cc}\boldsymbol{I}_{3\times
3}&\boldsymbol{O}_{3\times 3}\\\ \boldsymbol{O}_{3\times
3}&B(\boldsymbol{\alpha})\\\ \end{array}\right)\dot{\boldsymbol{t}}$ (22)

When _roll-pitch-yaw_ angle is used to represent the orientation, we have
$\boldsymbol{\alpha}=(\phi,\psi,\theta)^{T}$ and
$B(\boldsymbol{\alpha})=\left(\begin{array}[]{ccc}c_{\psi}c_{\theta}&-s_{\theta}&0\\\
c_{\psi}s_{\theta}&c_{\theta}&0\\\ -s_{\psi}&0&1\\\ \end{array}\right)$, where
$s$ and $c$ represent sine and cosine operator, respectively.

Combining Eq. (21) and Eq. (22),
$\frac{\partial{\boldsymbol{t}_{g,i}^{w}}}{\partial{\boldsymbol{t}_{obj}^{w}}}$
can be derived in Eq. (23).

$\frac{\partial{\boldsymbol{t}_{g,i}^{w}}}{\partial{\boldsymbol{t}_{obj}^{w}}}=\boldsymbol{W}_{i}=\left(\begin{array}[]{cc}\boldsymbol{I}_{3\times
3}&-S(\boldsymbol{p}_{g,i}^{w})B(\boldsymbol{\alpha}_{obj}^{w})\\\
\boldsymbol{O}_{3\times
3}&-B^{-1}(\boldsymbol{\alpha}_{g,i}^{w})B(\boldsymbol{\alpha}_{obj}^{w})\\\
\end{array}\right)$ (23)

Combining Eq. (16)-(20) and Eq. (23), $\boldsymbol{J}_{\scriptscriptstyle
C^{3}}(\boldsymbol{c})$ is finally given in Eq. (24).

$\boldsymbol{J}_{\scriptscriptstyle
C^{3}}(\boldsymbol{c})=\left(\begin{array}[]{cccc}\boldsymbol{J}_{1}(\boldsymbol{q}_{1})&...&\boldsymbol{O}&-\boldsymbol{W}_{0}\\\
\vdots&\ddots&\vdots&\vdots\\\
\boldsymbol{O}&...&\boldsymbol{J}_{n}(\boldsymbol{q}_{n})&-\boldsymbol{W}_{n}\\\
\end{array}\right)$ (24)

## References

  * [1] G.-Z. Yang, J. Bellingham, P. E. Dupont, P. Fischer, L. Floridi, R. Full, N. Jacobstein, V. Kumar, M. McNutt, R. Merrifield _et al._ , “The grand challenges of science robotics,” _Science robotics_ , vol. 3, no. 14, p. eaar7650, 2018.
  * [2] J. K. Parrish and L. Edelstein-Keshet, “Complexity, pattern, and evolutionary trade-offs in animal aggregation,” _Science_ , vol. 284, no. 5411, pp. 99–101, 1999.
  * [3] L. E. Parker, D. Rus, and G. S. Sukhatme, “Multiple mobile robot systems,” in _Springer Handbook of Robotics_. Springer, 2016, pp. 1335–1384.
  * [4] F. Suárez-Ruiz, X. Zhou, and Q.-C. Pham, “Can robots assemble an ikea chair?” _Science Robotics_ , vol. 3, no. 17, p. eaat6385, 2018.
  * [5] S.-J. Chung, A. A. Paranjape, P. Dames, S. Shen, and V. Kumar, “A survey on aerial swarm robotics,” _IEEE Transactions on Robotics_ , vol. 34, no. 4, pp. 837–855, 2018.
  * [6] A. Branch, M. M. Flexas, B. Claus, A. F. Thompson, Y. Zhang, E. B. Clark, S. Chien, D. M. Fratantoni, J. C. Kinsey, B. Hobson _et al._ , “Front delineation and tracking with multiple underwater vehicles,” _Journal of Field Robotics_ , vol. 36, no. 3, pp. 568–586, 2019.
  * [7] K. H. Petersen, N. Napp, R. Stuart-Smith, D. Rus, and M. Kovac, “A review of collective robotic construction,” _Science Robotics_ , vol. 4, no. 28, p. eaau8479, 2019.
  * [8] D. Koung, O. Kermorgant, I. Fantoni, and L. Belouaer, “Cooperative multi-robot object transportation system based on hierarchical quadratic programming,” _IEEE Robotics and Automation Letters_ , vol. 6, no. 4, pp. 6466–6472, 2021\.
  * [9] Y. Tian, K. Liu, K. Ok, L. Tran, D. Allen, N. Roy, and J. P. How, “Search and rescue under the forest canopy using multiple uavs,” _The International Journal of Robotics Research_ , vol. 39, no. 10-11, pp. 1201–1221, 2020.
  * [10] O. Khatib, K. Yokoi, O. Brock, K. Chang, and A. Casal, “Robots in human environments: Basic autonomous capabilities,” _The International Journal of Robotics Research_ , vol. 18, no. 7, pp. 684–696, 1999.
  * [11] K.-K. Oh, M.-C. Park, and H.-S. Ahn, “A survey of multi-agent formation control,” _Automatica_ , vol. 53, pp. 424–440, 2015.
  * [12] A. Marino, “Distributed adaptive control of networked cooperative mobile manipulators,” _IEEE Transactions on Control Systems Technology_ , vol. 26, no. 5, pp. 1646–1660, 2017.
  * [13] S. Chiaverini, G. Oriolo, and A. A. Maciejewski, “Redundant robots,” in _Springer Handbook of Robotics_. Springer, 2016, pp. 221–242.
  * [14] D. Berenson, _Constrained manipulation planning_. Carnegie Mellon University, 2011.
  * [15] Z. Kingston, M. Moll, and L. E. Kavraki, “Sampling-based methods for motion planning with constraints,” _Annual review of control, robotics, and autonomous systems_ , vol. 1, pp. 159–185, 2018.
  * [16] A. H. Qureshi, J. Dong, A. Baig, and M. C. Yip, “Constrained motion planning networks x,” _IEEE Transactions on Robotics_ , 2021.
  * [17] R. Ancona, “Redundancy modelling and resolution for robotic mobile manipulators: a general approach,” _Advanced Robotics_ , vol. 31, no. 13, pp. 706–715, 2017.
  * [18] B. Navarro, A. Cherubini, A. Fonte, G. Poisson, and P. Fraisse, “A framework for intuitive collaboration with a mobile manipulator,” in _2017 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS)_. IEEE, 2017, pp. 6293–6298.
  * [19] W. Wiedmeyer, P. Altoé, J. Auberle, C. Ledermann, and T. Kröger, “A real-time-capable closed-form multi-objective redundancy resolution scheme for seven-dof serial manipulators,” _IEEE Robotics and Automation Letters_ , vol. 6, no. 2, pp. 431–438, 2020.
  * [20] H. Seraji, “A unified approach to motion control of mobile manipulators,” _The International Journal of Robotics Research_ , vol. 17, no. 2, pp. 107–118, 1998.
  * [21] S. Chiaverini, “Singularity-robust task-priority redundancy resolution for real-time kinematic control of robot manipulators,” _IEEE Transactions on Robotics and Automation_ , vol. 13, no. 3, pp. 398–410, 1997.
  * [22] H. Zhang, Y. Zhang, Z. Xiong, X. Sheng, and X. Zhu, “A task-priority coordinated motion planner combined with visual servo for mobile manipulator,” in _2019 IEEE/ASME International Conference on Advanced Intelligent Mechatronics (AIM)_. IEEE, 2019, pp. 7–12.
  * [23] H. Zhang, Q. Sheng, Y. Sun, X. Sheng, Z. Xiong, and X. Zhu, “A novel coordinated motion planner based on capability map for autonomous mobile manipulator,” _Robotics and Autonomous Systems_ , vol. 129, p. 103554, 2020\.
  * [24] H. Zhang, Q. Sheng, J. Hu, X. Sheng, Z. Xiong, and X. Zhu, “Cooperative transportation with mobile manipulator: A capability map-based framework for physical human–robot collaboration,” _IEEE/ASME Transactions on Mechatronics_ , 2022.
  * [25] F. Zacharias, C. Borst, and G. Hirzinger, “Capturing robot workspace structure: representing robot capabilities,” in _2007 IEEE/RSJ International Conference on Intelligent Robots and Systems_. Ieee, 2007, pp. 3229–3236.
  * [26] N. Vahrenkamp, T. Asfour, G. Metta, G. Sandini, and R. Dillmann, “Manipulability analysis,” in _2012 12th ieee-ras international conference on humanoid robots (humanoids 2012)_. IEEE, 2012, pp. 568–573.
  * [27] F. Burget and M. Bennewitz, “Stance selection for humanoid grasping tasks by inverse reachability maps,” in _2015 IEEE International conference on robotics and automation (ICRA)_. IEEE, 2015, pp. 5669–5674.
  * [28] F. Chen, M. Selvaggio, and D. G. Caldwell, “Dexterous grasping by manipulability selection for mobile manipulator with visual guidance,” _IEEE Transactions on Industrial Informatics_ , vol. 15, no. 2, pp. 1202–1210, 2018.
  * [29] J. Xu, K. Harada, W. Wan, T. Ueshiba, and Y. Domae, “Planning an efficient and robust base sequence for a mobile manipulator performing multiple pick-and-place tasks,” in _2020 IEEE International Conference on Robotics and Automation (ICRA)_. IEEE, 2020, pp. 11 018–11 024.
  * [30] F. Wang, J. R. G. Olvera, and G. Cheng, “Optimal order pick-and-place of objects in cluttered scene by a mobile manipulator,” _IEEE Robotics and Automation Letters_ , vol. 6, no. 4, pp. 6402–6409, 2021.
  * [31] M. Bonilla, L. Pallottino, and A. Bicchi, “Noninteracting constrained motion planning and control for robot manipulators,” in _2017 IEEE International Conference on Robotics and Automation (ICRA)_. IEEE, 2017, pp. 4038–4043.
  * [32] J. H. Yakey, S. M. LaValle, and L. E. Kavraki, “Randomized path planning for linkages with closed kinematic chains,” _IEEE Transactions on Robotics and Automation_ , vol. 17, no. 6, pp. 951–958, 2001.
  * [33] D. Berenson, S. Srinivasa, and J. Kuffner, “Task space regions: A framework for pose-constrained manipulation planning,” _The International Journal of Robotics Research_ , vol. 30, no. 12, pp. 1435–1460, 2011.
  * [34] X. Zhang, L. Yan, T. L. Lam, and S. Vijayakumar, “Task-space decomposed motion planning framework for multi-robot loco-manipulation,” in _2021 IEEE International Conference on Robotics and Automation_. Institute of Electrical and Electronics Engineers (IEEE), 2021\.
  * [35] L. Jaillet and J. M. Porta, “Path planning with loop closure constraints using an atlas-based rrt,” in _Robotics Research_. Springer, 2017, pp. 345–362.
  * [36] B. Kim, T. T. Um, C. Suh, and F. C. Park, “Tangent bundle rrt: A randomized algorithm for constrained motion planning,” _Robotica_ , vol. 34, no. 1, pp. 202–225, 2016.
  * [37] M. Gharbi, J. Cortés, and T. Simeon, “A sampling-based path planner for dual-arm manipulation,” in _2008 IEEE/ASME International Conference on Advanced Intelligent Mechatronics_. IEEE, 2008, pp. 383–388.
  * [38] Q. Tang, Y. Zhang, F. Yu, and J. Zhang, “An obstacle avoidance approach based on system outlined rectangle for cooperative transportation of multiple mobile manipulators,” in _2018 IEEE International Conference on Intelligence and Safety for Robotics (ISR)_. IEEE, 2018, pp. 533–538.
  * [39] J. Alonso-Mora, S. Baker, and D. Rus, “Multi-robot formation control and object transport in dynamic environments via constrained optimization,” _The International Journal of Robotics Research_ , vol. 36, no. 9, pp. 1000–1021, 2017.
  * [40] J. Jiao, Z. Cao, N. Gu, S. Nahavandi, Y. Yang, and M. Tan, “Transportation by multiple mobile manipulators in unknown environments with obstacles,” _IEEE Systems Journal_ , vol. 11, no. 4, pp. 2894–2904, 2015.
  * [41] J. Hu, W. Liu, H. Zhang, J. Yi, and Z. Xiong, “Multi-robot object transport motion planning with a deformable sheet,” _IEEE Robotics and Automation Letters_ , vol. 7, no. 4, pp. 9350–9357, 2022.
  * [42] J. Cheong, “A consistent kinematic modeling method for mobile manipulators,” in _2010 11th International Conference on Control Automation Robotics & Vision_. IEEE, 2010, pp. 805–810.
  * [43] S. Karaman and E. Frazzoli, “Sampling-based algorithms for optimal motion planning,” _The international journal of robotics research_ , vol. 30, no. 7, pp. 846–894, 2011.
  * [44] T. Hekmatfar, E. Masehian, and S. J. Mousavi, “Cooperative object transportation by multiple mobile manipulators through a hierarchical planning architecture,” in _2014 Second RSI/ISM International Conference on Robotics and Mechatronics (ICRoM)_. IEEE, 2014, pp. 503–508.
  * [45] L. E. Kavraki, P. Svestka, J.-C. Latombe, and M. H. Overmars, “Probabilistic roadmaps for path planning in high-dimensional configuration spaces,” _IEEE transactions on Robotics and Automation_ , vol. 12, no. 4, pp. 566–580, 1996.
  * [46] D. Hsu, J.-C. Latombe, and R. Motwani, “Path planning in expansive configuration spaces,” in _Proceedings of International Conference on Robotics and Automation_ , vol. 3. IEEE, 1997, pp. 2719–2726.
  * [47] J. J. Kuffner and S. M. LaValle, “Rrt-connect: An efficient approach to single-query path planning,” in _Proceedings 2000 ICRA. Millennium Conference. IEEE International Conference on Robotics and Automation. Symposia Proceedings (Cat. No. 00CH37065)_ , vol. 2. IEEE, 2000, pp. 995–1001.
  * [48] I. A. Şucan and S. Chitta, “Motion planning with constraints using configuration space approximations,” in _2012 IEEE/RSJ International Conference on Intelligent Robots and Systems_. IEEE, 2012, pp. 1904–1910.
  * [49] D. Coleman, I. Sucan, S. Chitta, and N. Correll, “Reducing the barrier to entry of complex robotic software: a moveit! case study,” _arXiv preprint arXiv:1404.3785_ , 2014.
  * [50] O. Porges, T. Stouraitis, C. Borst, and M. A. Roa, “Reachability and capability analysis for manipulation tasks,” in _ROBOT2013: First Iberian robotics conference_. Springer, 2014, pp. 703–718.
  * [51] S. Patel and T. Sobh, “Manipulator performance measures-a comprehensive literature survey,” _Journal of Intelligent & Robotic Systems_, vol. 77, no. 3, pp. 547–570, 2015.
  * [52] T. Yoshikawa, “Manipulability of robotic mechanisms,” _The international journal of Robotics Research_ , vol. 4, no. 2, pp. 3–9, 1985.
  * [53] J. A. Nelder and R. Mead, “A simplex method for function minimization,” _The computer journal_ , vol. 7, no. 4, pp. 308–313, 1965.
  * [54] Stanford Artificial Intelligence Laboratory et al., “Robotic operating system.” [Online]. Available: https://www.ros.org
  * [55] R. B. Rusu and S. Cousins, “3d is here: Point cloud library (pcl),” in _2011 IEEE international conference on robotics and automation_. IEEE, 2011, pp. 1–4.
  * [56] R. Smits, “KDL: Kinematics and Dynamics Library,” http://www.orocos.org/kdl.
  * [57] P. Beeson and B. Ames, “Trac-ik: An open-source library for improved solving of generic inverse kinematics,” in _2015 IEEE-RAS 15th International Conference on Humanoid Robots (Humanoids)_ , 2015.
  * [58] R. Diankov, “Automated construction of robotic manipulation programs,” 2010.
  * [59] S. M. LaValle _et al._ , “Rapidly-exploring random trees: A new tool for path planning,” 1998.
  * [60] I. A. Şucan and L. E. Kavraki, “Kinodynamic motion planning by interior-exterior cell exploration,” in _Algorithmic Foundation of Robotics VIII_. Springer, 2009, pp. 449–464.
  * [61] B. Gipson, M. Moll, and L. E. Kavraki, “Resolution independent density estimation for motion planning in high-dimensional spaces,” in _2013 IEEE International Conference on Robotics and Automation_. IEEE, 2013, pp. 2437–2443.
  * [62] I. A. Sucan, M. Moll, and L. E. Kavraki, “The open motion planning library,” _IEEE Robotics & Automation Magazine_, vol. 19, no. 4, pp. 72–82, 2012\.
  * [63] F. Caccavale and M. Uchiyama, “Cooperative manipulation,” in _Springer Handbook of Robotics_. Springer, 2016, pp. 989–1006.