#  Motion Planning for Multiple Mobile Manipulator System in Complex Flipping
Manipulation

Wenhang Liu, Kun Song, Meng Ren, Jiawei Hu, Michael Yu Wang, , and Zhenhua
Xiong  This work was supported in part by the National Natural Science
Foundation of China (U1813224) and MoE Key Lab of Artificial Intelligence, AI
Institute, Shanghai Jiao Tong University, China. (Corresponding author:
Zhenhua Xiong.) Wenhang Liu, Kun Song, Meng Ren, Jiawei Hu, and Zhenhua Xiong
are with the School of Mechanical Engineering, State Key Laboratory of
Mechanical System and Vibration, Shanghai Jiao Tong University, Shanghai,
China (e-mail<EMAIL_ADDRESS><EMAIL_ADDRESS><EMAIL_ADDRESS><EMAIL_ADDRESS>mexiong@sjtu.edu.cn). Michael Yu
Wang is with the Department of Mechanical and Aerospace Engineering, Monash
University, Victoria 3800, Australia (e-mail: Michael.Y.Wang@monash.edu).

###### Abstract

Multiple robot systems are favored for object manipulation and transportation,
especially for large objects. However, in more complex manipulation such as
flipping, these systems encounter a new challenge, configuration
disconnectivity of manipulators. Grasping objects by manipulators will impose
closed-chain constraints on the system, which in turn limits the feasible
motions of manipulators and further compromises the configuration
connectivity. Multiple mobile manipulator systems show much more flexibility
in object manipulation with the mobility of the mobile platform and have the
potential to address the above problem. In this paper, a novel planning
framework is proposed for complex flipping manipulation by incorporating
platform motions and regrasping. Firstly, two types of trajectories, mobile
manipulator planning and regrasping planning, are classified and can be
assigned different priorities for different tasks. Secondly, corresponding
planning methods are designed for each type of trajectory. Specifically, in
mobile manipulator planning, the configuration of the platform is determined
through optimization to ensure connectivity when the manipulator approaches
configuration boundaries. In regrasping planning, closed-chain constraints are
temporarily disregarded and the manipulation capabilities are prioritized to
facilitate subsequent planning. Finally, the structure of the overall planning
framework is provided. Experimental results demonstrate that the proposed
planner efficiently plans the motions of the system to accomplish flipping
manipulation. Additionally, a comprehensive experiment emphasizes the
significance of our planner in extending the capabilities of multiple mobile
manipulator systems in complex tasks.

###### Index Terms:

Multiple mobile manipulator system, complex manipulation, closed-chain
constraint, configuration disconnectivity.

##  I Introduction

Compared to single robots, Multiple Robot Systems (MRS) are well-suited to
handle intricate tasks since they exhibit enhanced robustness, stability, and
efficiency[1]. Hence, the versatility of MRS has gained increasing attention
and become a focal point of interest in various applications, such as
assembly[2][3], printing[4], exploration[5][6], and transporting[7][8]. Among
different forms of MRS, Multiple Mobile Manipulator Systems (MMMS) stand out
for their integration of mobility and manipulation capabilities, making them
particularly attractive for industrial automation[9][fiore2024general]. In
automated production, MMMS can accomplish complex processes by collaborative
manipulation, for example, they are applied to manipulate industrial
components in [3][10]. Sometimes, when it comes to large objects or complex
manipulations, MMMS are even irreplaceable.

Although possessing exceptional performance in object manipulation, MMMS also
face several challenges. One of the challenges arises from the closed-chain
constraint of manipulators[11][12]. The grasping of objects by manipulators
introduces closed-chain or formation constraints, consequently limiting the
feasible motions of manipulators, and further compromising the configuration
connectivity. These constraints pose a challenge to motion planning for MMMS.

There are generally two types of methods to deal with closed-chain
constraints. One type involves considering the configuration of each robot
individually. In [13], probabilistic roadmaps were utilized to efficiently
determine feasible configurations for robots. Similarly, dynamic rapidly-
exploring random tree was applied in [14]. In such scenarios, task or
restricted space regions of the end-effector are useful for the manipulators
to plan within the constraints [15][16]. In [17], the global planner first
explored the low-dimensional decomposed task space regions of each robot, and
then the local planner computed paths in the high-dimensional constrained
configuration space. Another type is to consider the overall configuration of
the system. In [18], three modes were predefined according to the objects and
closed-chain constraints, based on which, a decision-making method was
proposed to facilitate the selection of object transportation and movement
direction. In [19], the closed-chain constraints were transformed into the
problem of finding the optimal formation for the whole system. Planning the
overall configuration can conveniently facilitate the integration with other
tasks, such as obstacle avoidance.

The aforementioned works enable MMMS to plan feasible configurations under
closed-chain constraints and successfully manipulate objects, as demonstrated
through experiments or simulations[13][17, 18, 19]. However, it is worth
noting that these works primarily address tasks related to object
transportation. In this case, objects require large translational movements
mostly relying on the mobility of mobile platforms, and the manipulators only
perform minimal coordinated motions if necessary. Therefore, these works may
not fully consider the configuration disconnectivity problem under closed-
chain constraints, which will arise when tasks become more intricate. For
example, in the complex manipulation such as flipping, both configurations can
be feasible, but they may not be connected due to self-collisions or
singularities.

In [20], configuration disconnectivity of fixed manipulators was addressed in
flipping manipulations. A method termed IK-switch based on regrasping was
proposed to bridge the gap between different components of the configuration
space that would otherwise be disconnected. The effectiveness of this method
was successfully confirmed through flipping manipulations of chairs. They
further proposed a certified-complete planner of the bimanual system in [21],
which could be applied to bring an object from the initial stable placement
toward a goal stable placement. Likewise, flipping manipulations were also
discussed in [22] and [23], and the manipulators completed the manipulation
through regrasping in the experiments.

Although fixed manipulator systems can accomplish some manipulation tasks
[24], these systems heavily relies on regrasping, which may impose potential
difficulties and issues. Concerns such as efficiency, the feasibility under
heavy loads, and the potential impact or vibration caused by regrasping may
arise[25]. Fortunately, with the additional degrees of freedom provided by the
mobile platform, a mobile manipulator may improve configuration connectivity
which is inherently disconnected for the manipulator alone. Accordingly, there
is a prospect for MMMS to accomplish flipping manipulations without the need
of regrasping.

However, the redundancy brought by the mobile platform also presents another
challenge for MMMS, as motion planning is extended to a higher-dimensional
space. Existing research on motion planner for mobile manipulators can be
broadly categorized into two categories. The first category is decoupled
planning [26]. For example, door traversal of mobile manipulators was
considered in [27]. They first planned the path for the mobile robot, and
second computed configurations of the manipulator from the path. In [28], for
the picking up tasks, manipulator trajectories were generated for a given
mobile platform trajectory. Decoupled planning is relatively simple and easier
to implement, but it may not effectively utilize the mobility of the platform
to help the manipulator avoid disconnected configurations. Conversely, the
second category treats the mobile platform and manipulator equally. In such
cases, there are infinite configurations due to redundancy, and it can be
challenging to determine a specific configuration[29]. For this reason, many
sample-based or optimization-based methods were proposed for redundancy
planning [15][30][31]. Regardless of the planning method employed, the planner
should be closely aligned with the specific requirements brought by the task.

To achieve complex manipulation of objects, this paper proposes a novel
planning framework for MMMS. Firstly, the trajectories involved in object
manipulation of mobile manipulators are classified, based on which the task
priorities can be specified. Subsequently, corresponding planning methods are
designed for different trajectory types, including mobile manipulator planning
and regrasping planning. Specifically, in mobile manipulator planning, when
manipulator approaches the configuration boundary, motions of the platform are
determined to ensure the configuration connectivity. In regrasping planning,
the manipulation capabilities of the manipulator are considered to facilitate
planning. Conclusively, in the framework, the attention remains on planning
the motions of the manipulator to accomplish manipulation tasks. The motions
of the platform and regrasping are introduced to enhance configuration
connectivity in originally disconnected regions. In experiments, the flipping
manipulation of object is considered, and results demonstrate that our
planners can efficiently plan the motions of MMMS. Different task priorities
are set to be compared with each other. Additionally, a comprehensive
experiment demonstrates the significant advantages of our planner in object
manipulation.

The main contributions of this paper are threefold. First, a novel planning
framework is proposed, which realizes the complex flipping manipulation of
objects by multiple mobile manipulator systems. Second, the configuration
disconnectivity of manipulators in complex manipulations and closed-chain
constraints is addressed, and regrasping or platform motion is considered to
enhance the connectivity. Third, some areas in the framework requiring further
in-depth research are highlighted, and this paper can serve as a guidance.

This remainder of this paper is organized as follows. In Section I, some
related work is introduced. In Section II, basic concepts of multiple mobile
manipulator systems are defined and the problem is formulated. In Section III,
the proposed motion planner is presented in detail. In Section IV, simulation
and experimental results are given. Moreover, limitations and areas for
further research are discussed. Finally, Section V concludes the paper and
outlines future work.

##  II Problem Formulation

Figure 1:  The collaborative manipulation of objects by MMMS.

###  II-A Preliminaries

As shown in Fig. 1, it illustrates multiple mobile manipulators manipulating
an object. Let $\mathbb{W}$ and $\mathbb{O}$ be the frames of the world and
object, respectively. Let $\mathbb{P}_{i}$, $\mathbb{B}_{i}$, and
$\mathbb{E}_{i}$ be the frames of the mobile platform, the base of the
manipulator, and the end-effector of the $i$-th mobile manipulator,
respectively, where $i=1,\ldots,n$ and $n$ is the number of mobile
manipulators. We use ${\bf{T}}\in SE(3)$ to represent the pose of a frame with
respect to another frame. For example, ${}^{\mathbb{W}}{\bf{T}}_{\mathbb{O}}$
represents the pose of frame $\mathbb{O}$ in frame $\mathbb{W}$. Based on
frame transformations, we have the following fundamental relationships

$^{\mathbb{W}}{\bf{T}}_{\mathbb{O}}={{}^{\mathbb{W}}{\bf{T}}_{\mathbb{P}_{i}}}\cdot{{}^{\mathbb{P}_{i}}{\bf{T}}_{\mathbb{B}_{i}}}\cdot{{}^{\mathbb{B}_{i}}{\bf{T}}_{\mathbb{E}_{i}}}\cdot{{}^{\mathbb{E}_{i}}{\bf{T}}_{\mathbb{O}}}$
(1)

which can be regarded as a chain in the system. It should be noted that
${{}^{\mathbb{P}_{i}}{\bf{T}}_{\mathbb{B}_{i}}}$ is invariant and solely
depends on the robot itself, specifically, the location of the manipulator
mounted on the mobile platform.

In this paper, the mobile platform is assumed to be driven by Mecanum wheels,
and the manipulator has six degrees of freedom. The configurations of the
$i$-th mobile manipulator is denoted as $\bm{q}_{i}=(\bm{q}^{\rm
T}_{p,i},\bm{q}^{\rm T}_{m,i})^{\rm T}$, where $\bm{q}_{p,i}\in\mathbb{R}^{3}$
refers to the configuration of the mobile platform, and
$\bm{q}_{m,i}\in\mathbb{R}^{6}$ refers to the configuration of the
manipulator. The whole configuration space is denoted as $\mathcal{C}^{i}$,
and the obstacle-free configuration space is denoted as
$\mathcal{C}^{i}_{\textit{free}}$.

###  II-B Problem Definition

The manipulation problem we consider is how to determine the motions of robots
when the trajectory of an object is given. Therefore, the problem can be
defined as

Input: $\displaystyle\ \
\tau_{\textit{obj}}:[0,1]\to[{{}^{\mathbb{W}}{\bf{T}}_{\mathbb{O}}}(0),{{}^{\mathbb{W}}{\bf{T}}_{\mathbb{O}}}(1)]$
(2) Output: $\displaystyle\ \
\tau_{i}:[0,1]\to\begin{cases}[{{}^{\mathbb{W}}{\bf{T}}_{\mathbb{P}_{i}}}(0),{{}^{\mathbb{W}}{\bf{T}}_{\mathbb{P}_{i}}}(1)]\\\
[{{}^{\mathbb{B}_{i}}{\bf{T}}_{\mathbb{E}_{i}}}(0),{{}^{\mathbb{B}_{i}}{\bf{T}}_{\mathbb{E}_{i}}}(1)]\end{cases}$
s.t $\displaystyle\ \ \text{Eq}.\eqref{transformations}$

The kinematics of the mobile platform is defined as $\bm{J}_{p}$, and then we
can obtain
$\bm{J}_{p}(\bm{q}_{i,p})={{}^{\mathbb{W}}{\bf{T}}_{\mathbb{P}_{i}}}$.
Similarly,
$\bm{J}_{m}(\bm{q}_{i,m})={{}^{\mathbb{B}_{i}}{\bf{T}}_{\mathbb{E}_{i}}}$.
Since ${{}^{\mathbb{P}_{i}}{\bf{T}}_{\mathbb{B}_{i}}}$ is only related to the
mounting position of manipulators on the mobile platform, the kinematics
$\bm{J}$ of the mobile manipulator can be defined as follows.

$\bm{J}(\bm{q}_{i})\cdot{{}^{\mathbb{E}_{i}}{\bf{T}}_{\mathbb{O}}}={{}^{\mathbb{W}}{\bf{T}}_{\mathbb{O}}},\
\bm{q}_{i}\in\mathbb{R}^{9}$ (3)

Obviously, Eq.(3) is a many-to-one relationship, which is the challenging of
redundant motion planning for the mobile manipulator. According to Eq.(1), we
can derive that

$\bm{J}^{-1}(\bm{q}_{j})\bm{J}(\bm{q}_{i})={{}^{\mathbb{E}_{j}}{\bf{T}}_{\mathbb{E}_{i}}},\
i\neq j$ (4)

Eq.(4) can be regarded as the closed-chain constraints between robots,
indicating that end-effectors of the manipulators must maintain a specific
relative pose. If no regrasping occurs in the manipulation,
${{}^{\mathbb{E}_{j}}{\bf{T}}_{\mathbb{E}_{i}}}$ remains unchanged.

Substituting Eq.(3)-(4) into the problem given in (2), the planning problem
can be transformed into

Input $\displaystyle\ \
\tau_{\textit{obj}}:[0,1]\to[{{}^{\mathbb{W}}{\bf{T}}_{\mathbb{O}}}(0),{{}^{\mathbb{W}}{\bf{T}}_{\mathbb{O}}}(1)]$
(5) Output $\displaystyle\ \ \tau_{i}:[0,1]\to[\bm{q}_{i}(0),\bm{q}_{i}(1)]$
s.t $\displaystyle\ \
\begin{cases}\bm{J}(\bm{q}_{i}(t))={{}^{\mathbb{W}}{\bf{T}}_{\mathbb{O}}(t)}\times{{}^{\mathbb{O}}{\bf{T}}_{\mathbb{E}_{i}}(t)}\\\
\bm{q}_{i}(t)\in\mathcal{C}^{i}_{\textit{free}}\ t\in[0,1]\end{cases}$

Once $\tau_{i}$ is obtained, planning the motions become straightforward. In
the multiple fixed manipulators system,
$\bm{q}_{i}=\bm{q}_{m,i}\in\mathbb{R}^{6}$, and hence it is relatively easy to
solve Eq.(3). However, it may not always be possible to find a connected path
in the configuration space that satisfies Eq.(4). As clarified before, some
research has employed the regrasping approach. Specifically, if
$\bm{q}_{m,i}(t_{k})$ and $\bm{q}_{m,i}(t_{k+1})$ are not connected,
$\bm{q}_{m,i}(t_{k})$ is allowed to transition to
$\bm{q}^{\prime}_{m,i}(t_{k})$ by regrasping. During the transition, the
satisfaction of the new closed-chain constraint
${{}^{\mathbb{E}_{j}}{\bf{T}}^{\prime}_{\mathbb{E}_{i}}}$ is only needed at
$\bm{q}^{\prime}_{m,i}(t_{k})$. Thus, the connectivity of the configuration
space under closed-chain constraints is improved.

In MMMS, since $\bm{q}_{i}=(\bm{q}_{p,i},\bm{q}_{m,i})\in\mathbb{R}^{9}$, its
potential is extensively expanded in the manipulation of objects. However, it
is crucial to design a feasible planner to solve the problem in (5).

##  III The Proposed Motion Planner

This paper focuses on motion planning for robots, assuming that the trajectory
of the object is already provided. In general, owing to the redundant degree
of freedom, a multiple mobile manipulator system can plan different motions to
realize the manipulation. Therefore, we initially strategize the task
priorities for each mobile manipulator, including whether the mobile platform
remains stationary, and whether the manipulator regrasps. Subsequently,
planning processes for corresponding tasks are addressed. Finally, a
comprehensive framework is introduced to integrate these methods.

###  III-A Trajectory Classification

Figure 2:  Different paths of mobile manipulators when manipulating objects.

Based on the movement of mobile manipulators during object manipulation within
a trajectory segment $\gamma=[\alpha_{1},\alpha_{2}]$, we categorize the
following types of paths:

  * $\bullet$

Path A: ${{}^{\mathbb{E}_{i}}{\bf{T}}_{\mathbb{O}}}(t)$ =
${{}^{\mathbb{E}_{i}}{\bf{T}}_{\mathbb{O}}}(\alpha_{1})$,
$t\in[\alpha_{1},\alpha_{2}]$.

  * $\bullet$

Path B:
$\tau_{B}:[\alpha_{1},\alpha_{2}]\to[{{}^{\mathbb{E}_{i}}{\bf{T}}_{\mathbb{O}}}(\alpha_{1}),{{}^{\mathbb{E}_{i}}{\bf{T}}_{\mathbb{O}}}(\alpha_{2})]$.

Two different paths are shown in Fig. 2. In Path A, there are no regrasping
motions. The platform collaborates with the manipulator to avoid configuration
disconnectivity, as shown in Fig. 2(a). Path B represents a regrasping motion,
where the manipulator releases the object, shifts to a desired configuration,
and subsequently regrasps the object, as shown in Fig. 2(b). Following Path B,
there will be a change in the closed-chain constraints. It should be noted
that we assume when a robot is executing Path B, other robots are kept
stationary. This assumption is made to ensure safety and stability during
object manipulation, and can also be found in other research papers [20][23].

The path of a robot manipulating the object is made up of these two
fundamental types, which can both address the problem of configuration
disconnectivity of manipulators. In Path A, when considering the additional
motions of the platform, it effectively introduces extra degrees of freedom,
significantly expanding the feasible space. In Path B, the old closed-chain
constraints are disregarded and the robot can change the grasping position to
form new closed-chain constraints, under which the configuration may be
connected. Based on specific requirements or the capabilities of robots, we
can establish task priorities in object manipulations. For example, regrasping
is deliberately given lower priority due to its potential instability.

Input:
$\tau_{\textit{obj}}(\gamma):[\alpha_{1},\alpha_{2}]\to[{{}^{\mathbb{W}}{\bf{T}}_{\mathbb{O}}}(\alpha_{1}),{{}^{\mathbb{W}}{\bf{T}}_{\mathbb{O}}}(\alpha_{2})],g$.

Output:
$\tau_{A}(\gamma):[\alpha_{1},\alpha_{2}]\to[\bm{q}(\alpha_{1}),\bm{q}(\alpha_{2})]$.

1 $\tau_{A},\tau$ $\leftarrow\varnothing$; $\gamma_{1}\leftarrow\varnothing$;

2 while  _$\gamma_{1}\prec\gamma$_ do

3 $\tau,\gamma_{1}$ $\leftarrow$
PlanManipulator$(\tau_{\textit{obj}}(\gamma\setminus\gamma_{1}),g)$;

4 $\tau_{A}$ $\leftarrow$ TrajectorySynthesis$(\tau_{A},\tau)$;

5 if  _$\gamma_{1}==\gamma$_ then

6 break;

7 end if

8 $\tau,\gamma_{1}$ $\leftarrow$
ConnetPlan$(\tau_{\textit{obj}}(\gamma\setminus\gamma_{1}),g)$;

9 $\tau_{A}$ $\leftarrow$ TrajectorySynthesis$(\tau_{A},\tau)$;

10

11 end while

return $\tau_{A}$;

Algorithm 1 PathA Planner

###  III-B Mobile Manipulator Planning

Since Path A is preferred, planning Path A becomes the most important part in
a trajectory segment $\gamma=[\alpha_{1},\alpha_{2}]$. Specifically, when the
grasping position $g$ remains unchanged, plan a configuration trajectory
$\tau_{A}(\gamma):[\alpha_{1},\alpha_{2}]\to[\bm{q}(\alpha_{1}),\bm{q}(\alpha_{2})]$
for the robot. It should be pointed out that the manipulator is generally more
accurate and flexible than the mobile platform, and therefore completing the
manipulation task primarily relies on the motion of the manipulator. Hence,
the platform will remain as stationary as possible. With this motivation, the
planning process is detailed in Algorithm 1 and some key functions are
explained as follows.

  * $\bullet$

PlanManipulator plans the trajectory of manipulators while keeping the mobile
platform stationary. $\gamma_{1}$ is the trajectory that has been planned, and
this step plans for the remaining trajectory segment
$\gamma\setminus\gamma_{1}$. There are numerous proven methods for manipulator
trajectory planning [32]. Relying exclusively on the manipulator may suffer
the configuration disconnectivity due to limited reachable space or self-
collisions. Therefore, the trajectory has not been completely planned and
further planning is required.

  * $\bullet$

TrajectorySynthesis integrates the planned trajectories.

  * $\bullet$

ConnectPlan plans the collaborative configuration of the platform and
manipulator, enhancing the connectivity of the manipulator. The following
methods is introduced to align better with this requirement. Assuming the
planned trajectory is
$\tau(\gamma_{1}):[\alpha_{1},\beta_{1}]\to[\bm{q}(\alpha_{1}),\bm{q}(\beta_{1})]$,
$\bm{q}(\beta_{1})=(\bm{q}^{\rm T}_{p}(\beta_{1}),\bm{q}^{\rm
T}_{m}(\beta_{1}))^{\rm T}$, and the remaining trajectory segment is
$\gamma_{2}=[\beta_{1},\alpha_{2}]$. The desired configuration of the mobile
platform is determined through the following optimal planning

$\displaystyle\bm{q}_{p}$ $\displaystyle=\ \text{argmax}\ \beta_{2}^{\prime}$
(6) s.t $\displaystyle\beta_{1}-\beta_{1}^{\prime}\geq\xi$
$\displaystyle\bm{q}_{m}(\beta_{1})\in[\bm{q}_{m}(\beta_{1}^{\prime}),\bm{q}_{m}(\beta_{2}^{\prime})]$
$\displaystyle\text{where}\ [\beta_{1}^{\prime},\beta_{2}^{\prime}]$
$\displaystyle\leftarrow\texttt{PlanManipulator}(\tau_{\textit{obj}},\bm{q}_{p},g)$

$\texttt{PlanManipulator}(\tau_{\textit{obj}},\bm{q}_{p},g)$ yields a
connected trajectory $[\beta_{1}^{\prime},\beta_{2}^{\prime}]$ with the
platform remaining stationary at $\bm{q}_{p}$. $\xi$ is a positive
coefficient, where a larger value indicates more shared solutions between the
two trajectory segments $[\beta_{1}^{\prime},\beta_{2}^{\prime}]$ and
$[\alpha_{1},\beta_{1}]$. After obtaining $\bm{q}_{p}$, the direction of the
platform’s movement is determined, and then $\tau$ can be planned. This
optimization is aimed at ensuring the connectivity of the manipulator when
platform moves and enhancing subsequent planning if necessary.

In the planning phase, our emphasis remains on effectively deploying the
manipulator for object manipulation. The motions of the platform are aimed at
facilitating the connection of configurations that would otherwise be
disconnected of the manipulator. Hence, the platform moves only when the
manipulator is close to the disconnectivity boundary, which will be further
displayed in the experiment section.

###  III-C Regrasping Planning

Input:
$\tau(\gamma):[\alpha_{1},\alpha_{2}]\to[\bm{q}(\alpha_{1}),\bm{q}(\alpha_{2})],g$.

Output:
$\tau(\gamma^{\prime}):[\alpha_{1},\alpha_{3}]\to[\bm{q}(\alpha_{1}),\bm{q}(\alpha_{3})]$.

1 $\left\\{\bm{q}\right\\}$ $\leftarrow$ IKsolution$(g)$;

2 $\bm{q}(\alpha_{3})$ $\leftarrow$
ManiOptimization$(\left\\{\bm{q}\right\\})$;

3 $\tau_{B}$ $\leftarrow$ Regrasping$(\bm{q}(\alpha_{2}),\bm{q}(\alpha_{3}))$;

4 $\tau(\gamma^{\prime})$ $\leftarrow$
TrajectorySynthesis$(\tau(\gamma),\tau_{B})$;

return $\tau(\gamma^{\prime})$;

Algorithm 2 PathB Planner

As illustrated in Figure 2, under closed-chain constraints, configurations may
need regrasping to achieve connectivity. For a robot, we assume that the
trajectory before regrasping has already been planned and is denoted as
$\tau(\gamma):[\alpha_{1},\alpha_{2}]\to[\bm{q}(\alpha_{1}),\bm{q}(\alpha_{2})]$,
and the grasping position $g$ for the next segment has been determined. Then,
the regrasping planning problem is to plan
$\tau_{B}:[\alpha_{2},\alpha_{3}]\to[\bm{q}(\alpha_{2}),\bm{q}(\alpha_{3})]$,
where the new closed-chain constraint ${{}^{\mathbb{E}}{\bf{T}}_{\mathbb{O}}}$
must be re-established at $\bm{q}(\alpha_{3})$. Hence, the most critical part
is to determine $\bm{q}(\alpha_{3})$, i.e., the configuration under the new
closed-chain constraint.

Here, we consider the following optimal planning

$\displaystyle\bm{q}(\alpha_{3})=$ $\displaystyle\ \text{argmax}\
det\sqrt{\bm{J}_{m}(\bm{q}(\alpha_{3}))\bm{J}_{m}^{\rm
T}(\bm{q}(\alpha_{3}))}$ (7) s.t $\displaystyle\ \
\bm{J}(\bm{q}(\alpha_{3}))\cdot{{}^{\mathbb{E}}{\bf{T}}_{\mathbb{O}}}={{}^{\mathbb{W}}{\bf{T}}_{\mathbb{O}}}$

where $det\sqrt{\bm{J}_{m}(\bm{q}(\alpha_{3}))\bm{J}_{m}^{\rm
T}(\bm{q}(\alpha_{3}))}$ is the manipulability of robots [33]. This
optimization aims to enhance the robot’s operational capabilities after
regrasping and facilitate the subsequent planning. Since the other robots
remain stationary while one robot performs regrasping, it is convenient to
plan the trajectory after obtaining $\bm{q}(\alpha_{3})$. The planning process
is detailed in Algorithm 2 and some key functions are explained as follows.

  * $\bullet$

IKsolution and ManiOptimization are employed to solve the optimization
problem. The configurations of mobile platform are sampled first, and then the
configurations of manipulator can be solved through the inverse kinematics
solvers. Finally, the optimal configuration with highest manipulability is
selected for regrasping.

  * $\bullet$

Regrasping plans a trajectory for robot from the initial configuration to the
planned configuration. After releasing the object, the robot can move
relatively freely and the trajectory can be planned easily. The only
consideration during Regrasping is collision avoidance. Then, the input
$\tau(\gamma)$ and planned $\tau_{B}$ are integrated as the output
$\tau(\gamma^{\prime})$ through TrajectorySynthesis.

###  III-D General Framework of Planner

Figure 3:  The framework of our planner, which involves three inputs, object
trajectory, task priorities, and grasping positions. The outputs are the robot
trajectories, which are determined through four primary steps.

Input:
$\tau_{\textit{obj}}:[0,1]\to[{{}^{\mathbb{W}}{\bf{T}}_{\mathbb{O}}}(0),{{}^{\mathbb{W}}{\bf{T}}_{\mathbb{O}}}(1)]$;
$\mathcal{G}=\left\\{\mathcal{G}_{1},\dots,\mathcal{G}_{n}\right\\}$; Task
priorities.

Output: $\tau_{i}:[0,1]\to[\bm{q}_{i}(0),\bm{q}_{i}(1)],i=1,\dots,n$.

1 for  _$i=1$ to $n$_ do

2 $\Gamma_{i}\leftarrow$ IKcheck$(\tau_{\textit{obj}},\mathcal{G}_{i})$;

3 if  _ $\Gamma_{i}\prec[0,1]$ _ then

4 return $\varnothing$;

5 end if

6

7 end for

8for  _$i=1$ to $n$_ do

9 $\tau_{i}$ $\leftarrow\varnothing$;

10 while  _TrajectoryCheck $(\tau_{i})$ is true_ do

11
$\left\\{\gamma_{1},\dots,\gamma_{m}\right\\},\left\\{g(\gamma_{1}),\dots,g(\gamma_{m})\right\\}$
$\leftarrow$ Segmentation$(\Gamma_{i})$;

12 for  _$j=1$ to $m-1$_ do

13 $\tau_{i}(j)$ $\leftarrow$
PathA$(\tau_{\textit{obj}}(\gamma_{j}),g(\gamma_{j}))$;

14 $\tau_{i}(j)$ $\leftarrow$ PathB$(\tau_{i}(j),g(\gamma_{j+1}))$;

15

16 end for

17 $\tau_{i}(m)$ $\leftarrow$
PathA$(\tau_{\textit{obj}},\gamma_{m},g(\gamma_{m}))$;

18

19 end while

20

21 end for

return $\tau_{1},\dots,\tau_{n}$;

Algorithm 3 Global Planner

The framework of our planner is illustrated in Fig. 3. As previously
mentioned, an input to our planner is the object trajectory
$\tau_{\textit{obj}}$. Besides, task priorities should be introduced. In this
paper, the framework considers priorities Path A $>$ Path B, which implies
that robots will initially adjust the platform as needed, followed by
regrasping. Depending on the requirements or performance of robots, the
priority can be accordingly adjusted. Additionally, it is exceedingly
challenging to consider all possible grasping positions during regrasping.
Hence, a set of stable grasping positions will be predefined and included as
inputs to the planner. The set of grasping positions of the $i$-th robot is
denoted as $\mathcal{G}_{i}=\left\\{g_{1},\dots,g_{m}\right\\}$.

Once the inputs are determined, our planner generates the trajectories of
robots through four main steps, which are detailed in Algorithm 3 and some key
functions are explained as follows.

  * $\bullet$

IKcheck verifies whether feasible configurations exist under the given
trajectory and closed-chain constraints. Specifically, it checks for the
existence of $\bm{q}_{i}$ that satisfies Eq.(3), and
${{}^{\mathbb{E}_{i}}{\bf{T}}_{\mathbb{O}}}$ is determined by the grasping
position $g\in\mathcal{G}_{i}$. The output of IKcheck is a trajectory segment
$\Gamma_{i}$ containing feasible configurations. $\Gamma_{i}\prec[0,1]$
signifies that there are no feasible configurations in certain points
regardless of the grasping positions employed. In this case, the planning is
bound to fail and the remaining steps will be skipped.

  * $\bullet$

Since some feasible configurations are disconnected without regrasping,
Segmentation divides the trajectory into several segments
$\left\\{\gamma_{1},\dots,\gamma_{m}\right\\}$ and corresponding grasping
positions $\left\\{g(\gamma_{1}),\dots,g(\gamma_{m})\right\\}$. There are two
situations leading to disconnectivity. The first is that subsequent feasible
configurations exist at different grasping positions, necessitating a switch
to a new grasping position. The second is that although feasible
configurations exist at the same grasping position, robots could not achieve
connectivity due to singular configurations or self-collisions. During this
step, it is preferable to choose grasping positions that have more feasible
configurations for segmentation.

  * $\bullet$

PathA and PathB are carried out through Algorithm 1 and Algorithm 2,
respectively.

  * $\bullet$

TrajectoryCheck is employed to verify whether the generated trajectory
$\tau_{i}$ meets the requirements and whether self-collisions occur during
execution.

After planning the configuration trajectories of all robots, we can carry out
time synchronization and motion planning on them to complete the planning
task.

Figure 4:  Flipping manipulation of a chair by two mobile JAKA Zu7
manipulators. Trajectories of the chair are the same, changing from an
orientation of (90, 0, 0) degrees to (-180, 45, 90) degrees. (a) Trajectory
priorities Path A $>$ Path B. In this situation, robots prioritize the motion
of the platform. During the manipulation, there was no regrasping. The
platform coordinated its motions with the manipulator to accomplish the
flipping. (b) Trajectory priorities Path B $>$ Path A. In this situation,
robots prioritize regrasping. During the manipulation, platforms remained
stationary and the manipulator switched its grasping position four times
through regrasping to accomplish the flipping.  Figure 5:  Flipping
manipulation of a desk by three mobile UR5 manipulators. Trajectory priorities
of robots are the same Path A $>$ Path B. Trajectories of the desk are
different, but the orientation requires a change from (0, 0, 0) degrees to
(180, 0, 0) degrees. (a) In this trajectory, the desk was lifted to a greater
height. The robot passed through under the desk and there was no regrasping.
(b) In this trajectory, the robot regrasped its grasping position from the
lower part of the desk to the upper part.

##  IV Simulation and Experiment

###  IV-A Simulation and Experiment Setup

To validate the proposed framework, two sets of simulations are conducted,
including different numbers of mobile manipulators and the flipping of
different large objects. Before planning, we assume that the trajectory of
object and the grasping positions on the object are given. The mobile
platforms are omnidirectional wheeled and manipulators are UR5 and JAKA Zu7,
and the end of each manipulator is equipped with a Robotiq 2F-85 gripper. All
our simulations are performed on a computer with Intel Core i7-10700 CPU at
2.90 GHz (16 cores) and 64GB RAM. The results are obtained by the simulation
platform ROS-Gazebo based on C++. PlanManipulator is implemented based on the
open-source package [34] and IKcheck is implemented based on our previous work
[35].

Real-world experiments are conducted on the mobile JAKA Zu7 robots. Each robot
is controlled separately by its onboard Next Unit of Computing (NUC) computer
with Intel Core i7-1165G7 CPU, and all robots are equipped with wireless
transmission modules in the same local network for communication. Due to
control errors and time delays, it is almost impossible for physical robots to
achieve consistency. Hence, the planned motions cannot be perfectly replicated
and require adjustments. A simple feedback correction mechanism is considered
as follows.

The leader-follower structure is enrolled for the system. Assuming the leader
robot of the system is $l$. When executing the planned motions, the
configurations at time $t$ should satisfy the closed-chain constraints

$\displaystyle\bm{J}^{-1}(\bm{q}_{l}(t))\bm{J}_{p}(\bm{q}_{i,p}$
$\displaystyle(t)){{}^{\mathbb{P}_{i}}{\bf{T}}_{\mathbb{B}_{i}}}\bm{J}_{m}(\bm{q}_{i,m}(t))={{}^{\mathbb{E}_{l}}{\bf{T}}_{\mathbb{E}_{i}}}(t)$
(8) $\displaystyle i=1,\dots,n\ i\neq l$

In order to maintain Eq.(8), the planned configuration of the follower robot
$\bm{q}_{i}(t)$ needs to be instantaneously modified to
$\bm{q}^{\prime}_{i}(t)$ according to the actual configuration of the leader.
For the sake of real-time performance, the configuration of the manipulator
$\bm{q}_{i,p}(t)$ is modified while the motions of the platform remains
unchanged. In this correction mechanism, the followers need to obtain
information about their own and the leader’s configurations.

###  IV-B Simulation Results

####  IV-B1 Flip a Chair in Simulation

The first set of simulations is using two mobile JAKA Zu7 manipulators to flip
a chair. During the flipping, the position of the chair remains unchanged,
where the orientation, represented as roll-pitch-yaw angles, needs to change
from (90, 0, 0) degrees to (-180, 45, 90) degrees. In such complex
manipulation of large object, the manipulators may easily suffer the
configuration disconnectivity due to limited configuration space or
collisions. To display the effect of different task priorities on planning,
two simulations with identical chair trajectory are implemented, and the
results are shown in Fig. 4.

In the first simulation, the trajectory priorities of robots are set as Path A
$>$ Path B, which corresponds to the motion of platforms being prioritized
over regrasping. As shown in Fig. 4(a), the robots successfully completed the
manipulation without the need of regrasping. The robot on the left side kept
its platform stationary during the manipulation. In contrast, the robot on the
right side moved its platform three times. As stated previously, completing
the manipulation primarily relies on the motions of the manipulator itself.
When the manipulator approached the configuration boundary, which was mainly
caused by exceeding the configuration space, connectivity was maintained by
the platform adjusting its position accordingly.

In the second simulation, the trajectory priorities of robots are set as Path
B $>$ Path A, which corresponds to regrasping being prioritized over the
motion of platforms. As shown in Fig. 4(b), the robots successfully completed
the manipulation without any movement of the platform. When platforms remain
stationary, the reachable space of manipulators is limited. Under a fixed
closed-chain constraint, manipulating large objects within this limited space
may easily lead to self-collisions. Therefore, the robot on the right side
regrasped the chair and changed the grasping position four times. The robot
disregarded the closed-chain constraint during regrasping and formed a new
closed-chain constraint after regrasping.

####  IV-B2 Flip a Desk in Simulation

The second set of simulations is using three mobile UR5 manipulators to flip a
desk. Trajectory priorities of robots are set as Path A $>$ Path B. The
orientation of desk needs to change from (0, 0, 0) degrees to (180, 0, 0)
degrees.

In the first simulation, the desk was lifted higher, as shown in Fig. 5(a).
There was enough room under the desk for the robot to pass through. Hence, the
robot did not regrasp the desk since the motion of platforms are prioritized.
The system successfully completed the manipulation and two of the robots moved
their platforms.

In the second simulation, the desk was lifted not too high, as shown in Fig.
5(b). It can be seen that if the closed-chain constraints remained unchanged,
which means robots do not change grasping positions on the desk, then this
manipulation could not be completed due to collisions. Therefore, the robot
regrasped its grasping position from the lower part of the desk to the upper
part to avoid collisions, and the system successfully completed the
manipulation.

####  IV-B3 Results Discussion

Results similar to Fig. 4(b), which can be regarded as multiple fixed
manipulator systems flipping objects, can also be found in [20, 21, 22, 23].
As far as we know, the results shown in Fig. 4(a), Fig. 5 or Fig. 6 have not
appeared in other literature yet. These experiments demonstrate that both
regrasping and platform motions can effectively enhance the connectivity of
the manipulator. However, the essence of these two strategies is different.
The motions of the platform will expand the reachable space of the
manipulator, providing more flexibility in planning. In contrast, regrasping
does not change the reachable space of the manipulator, but forms other
closed-chain constraints, under which the planning can be continued. This
significantly relies on the feasible grasping positions on the object, and may
not be applicable when the object does not have suitable grasping positions.
Furthermore, when a robot performs regrasping, the other robots remain
stationary, leading to inefficiency. While the results may indicate that
platform motion is better than regrasping, they are not a substitute for each
other. As shown in Fig. 5(b), the manipulation can not be completed without
regrasping or platform motions. Therefore, we introduce trajectory priorities
in the planner to offer adaptability in meeting different requirements.

Figure 6:  Flipping manipulation of a chair by two mobile JAKA Zu7
manipulators in real world. The setup is similar to Fig. 4(a). The orientation
of the chair needs to change from (90, 0, 0) degrees to (-180, 0, 90) degrees.
The right-side robot moved its platform two times to coordinate with the
manipulator to accomplish the flipping.  Figure 7:  A manipulation task
requires placing the table in a specified location. Due to the limited
reachable space of the robots, three mobile JAKA Zu7 manipulators have to flip
the object by 180 degrees before being able to place it.

###  IV-C Physical Experiment Results

An experiment similar to Fig. 4(a) is conducted in the physical world, and the
results are shown in Fig. 6. When the right-side manipulator approached the
configuration boundary due to limited configuration space or self-collisions,
the robot moved its platform to coordinate with the manipulator to accomplish
the flipping. We also attempted to conduct Fig. 4(b) in the physical world.
However, due to the large and heavy object, it resulted in unstable grasping
when a robot released the object. As mentioned earlier, relying solely on
regrasping to accomplish flipping manipulations poses additional challenges.

Finally, we conducted a comprehensive experiment requiring a table to be
placed in a specified location. The results are shown in Fig. 7, where the
first image highlights the designated placement location. If a conventional
planner is employed, this task would be impossible to complete since the table
would need to be lifted to a height beyond the reachable range of the robot.
However, within our planning framework, the table can be manipulated to
achieve the flipping, and subsequently, the table can be placed within the
reachable range of the robot, as illustrated in Fig. 7. Such scenarios
involving flipping manipulations emphasizes the significance of our planner in
extending the capabilities of multiple mobile manipulator systems in complex
tasks.

###  IV-D Limitations of Proposed Framework

The results successfully demonstrate the effectiveness of our planning
framework for multiple mobile manipulator systems in complex manipulations.
Both regrasping and platform motions can be employed to enhance the
configuration connectivity under closed-chain constraints. Additionally, the
framework offers the flexibility to adjust trajectory priorities according to
specific requirements. Still, there are certain limitations within our
planning framework warranting further investigation.

####  IV-D1 Completeness of Proposed Framework

In our planner, two specific optimization problems are introduced to ensure
the configuration connectivity, based on which a feasible path for the
manipulation can be found. However, the completeness of the framework has not
been verified. In other words, when planning fails, it remains uncertain
whether it is due to a genuinely unavailable path or an unreasonable planning
method. Since verifying completeness in planning for systems with such high
degrees of freedom under closed-chain constraints is extremely challenging,
the framework requires further study.

####  IV-D2 Trajectories of Object and Grasping Positions

The trajectory of the object and grasping positions are predetermined and
considered as input to the planner. However, in some cases, it is only
necessary to specify the initial pose and final pose of the object, and the
specific trajectory or grasping positions are not required. As shown in Fig.
5, different trajectories of object may have a significant impact on the
motions of robots. Similarly, different grasping positions can lead to
different motions of robots and may even determine whether the manipulation
can be successfully completed. In this paper, these problems are avoided, but
these pre-specified inputs should also be considered as outputs of the planner
in a more general framework.

##  V Conclusion and Future Work

This paper proposes a novel planning framework of multiple mobile manipulator
systems in complex manipulation of large object. The trajectory and grasping
positions of object are considered as inputs of the planner and the outputs
are motions of robots. The manipulation is mainly completed through motions of
the manipulator. When manipulators suffer configuration disconnectivity,
platform motions or regrasping are enrolled, where the priorities between them
can be adjusted. Experimental results successfully demonstrate the
effectiveness of our framework.

Regarding future work, on the one hand, we will further refine the planning
framework, for example, the completeness of the planner. Moreover, the
framework can be extended to consider the trajectory of object and grasping
positions as outputs, which may provide greater flexibility in planning. On
the other hand, we will further integrate the practical manipulation issues,
for example, considering the load distribution and force interaction problems
between robots.

## References

  * [1] M. Dorigo, G. Theraulaz, and V. Trianni, “Reflections on the future of swarm robotics,” _Sci. Robot._ , vol. 5, no. 49, p. eabe4385, 2020.
  * [2] F. Suárez-Ruiz, X. Zhou, and Q.-C. Pham, “Can robots assemble an ikea chair?” _Sci. Robot._ , vol. 3, no. 17, p. eaat6385, 2018.
  * [3] M. Dogar, R. A. Knepper, A. Spielberg, C. Choi, H. I. Christensen, and D. Rus, “Multi-scale assembly with robot teams,” _Int. J. Robot. Res._ , vol. 34, no. 13, pp. 1645–1659, 2015.
  * [4] X. Zhang, M. Li, J. H. Lim, Y. Weng, Y. W. D. Tay, H. Pham, and Q.-C. Pham, “Large-scale 3d printing by a team of mobile robots,” _Automat. Constr._ , vol. 95, pp. 98–106, 2018.
  * [5] P. Arm, G. Waibel, J. Preisig, T. Tuna, R. Zhou, V. Bickel, G. Ligeza, T. Miki, F. Kehl, H. Kolvenbach _et al._ , “Scientific exploration of challenging planetary analog environments with a team of legged robots,” _Sci. Robot._ , vol. 8, no. 80, p. eade9548, 2023.
  * [6] C. Cao, H. Zhu, Z. Ren, H. Choset, and J. Zhang, “Representation granularity enables time-efficient autonomous exploration in large, complex worlds,” _Sci. Robot._ , vol. 8, no. 80, p. eadf0970, 2023.
  * [7] X. Jin and Z. Hu, “Adaptive cooperative load transportation by a team of quadrotors with multiple constraint requirements,” _IEEE Trans. Intell. Transport. Syst._ , vol. 24, no. 1, pp. 801–814, 2022.
  * [8] H. Kim, C. Oh, I. Jang, S. Park, H. Seo, and H. J. Kim, “Learning and generalizing cooperative manipulation skills using parametric dynamic movement primitives,” _IEEE Trans. Automat. Sci. Eng._ , vol. 19, no. 4, pp. 3968–3979, 2022.
  * [9] O. Khatib, K. Yokoi, K. Chang, D. Ruspini, R. Holmberg, and A. Casal, “Coordination and decentralized cooperation of multiple mobile manipulators,” _Journal of Robotic Systems_ , vol. 13, no. 11, pp. 755–764, 1996.
  * [10] X. Zhao, B. Tao, and H. Ding, “Multimobile robot cluster system for robot machining of large-scale workpieces,” _IEEE/ASME Trans. Mechatronics_ , vol. 27, no. 1, pp. 561–571, 2021.
  * [11] F. Caccavale and M. Uchiyama, “Cooperative manipulation,” _Springer handbook of robotics_ , pp. 989–1006, 2016.
  * [12] J. Cortés, “Motion planning algorithms for general closed-chain mechanisms,” Ph.D. dissertation, Institut National Polytechnique de Toulouse-INPT, 2003.
  * [13] J. Cortes, T. Siméon, and J.-P. Laumond, “A random loop generator for planning the motions of closed kinematic chains using prm methods,” in _Proc. IEEE Int. Conf. Robot. Autom._ , 2002, pp. 2141–2146.
  * [14] R. Bordalba, L. Ros, and J. M. Porta, “A randomized kinodynamic planner for closed-chain robotic systems,” _IEEE Trans. Robotics_ , vol. 37, no. 1, pp. 99–115, 2020.
  * [15] D. Berenson, S. Srinivasa, and J. Kuffner, “Task space regions: A framework for pose-constrained manipulation planning,” _Int. J. Robot. Res._ , vol. 30, no. 12, pp. 1435–1460, 2011.
  * [16] Z. Kingston, M. Moll, and L. E. Kavraki, “Sampling-based methods for motion planning with constraints,” _Annu. Rev. Control Robot. Auton. Syst._ , vol. 1, pp. 159–185, 2018.
  * [17] X. Zhang, L. Yan, T. L. Lam, and S. Vijayakumar, “Task-space decomposed motion planning framework for multi-robot loco-manipulation,” in _Proc. IEEE Int. Conf. Robot. Autom._ , 2021, pp. 8158–8164.
  * [18] 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 J._ , vol. 11, no. 4, pp. 2894–2904, 2015.
  * [19] J. Alonso-Mora, S. Baker, and D. Rus, “Multi-robot formation control and object transport in dynamic environments via constrained optimization,” _Int. J. Robot. Res._ , vol. 36, no. 9, pp. 1000–1021, 2017.
  * [20] Z. Xian, P. Lertkultanon, and Q.-C. Pham, “Closed-chain manipulation of large objects by multi-arm robotic systems,” _IEEE Robot. Automat. Lett._ , vol. 2, no. 4, pp. 1832–1839, 2017.
  * [21] P. Lertkultanon and Q.-C. Pham, “A certified-complete bimanual manipulation planner,” _IEEE Trans. Automat. Sci. Eng._ , vol. 15, no. 3, pp. 1355–1368, 2018.
  * [22] Y. Wu, Y. Fu, and S. Wang, “Global motion planning and redundancy resolution for large objects manipulation by dual redundant robots with closed kinematics,” _Robotica_ , vol. 40, no. 4, pp. 1125–1150, 2022.
  * [23] K. Jang, J. Baek, S. Park, and J. Park, “Motion planning for closed-chain constraints based on probabilistic roadmap with improved connectivity,” _IEEE/ASME Trans. Mechatronics_ , vol. 27, no. 4, pp. 2035–2043, 2022.
  * [24] T. Marcucci, M. Petersen, D. von Wrangel, and R. Tedrake, “Motion planning around obstacles with convex optimization,” _Sci. Robot._ , vol. 8, no. 84, p. eadf7843, 2023.
  * [25] P. Lertkultanon, “Planning algorithms for complex manipulation tasks,” Ph.D. dissertation, 2017.
  * [26] O. Brock and L. E. Kavraki, “Decomposition-based motion planning: A framework for real-time motion planning in high-dimensional configuration spaces,” in _Proc. IEEE Int. Conf. Robot. Autom._ , 2001, pp. 1469–1474.
  * [27] K. Jang, S. Kim, and J. Park, “Motion planning of mobile manipulator for navigation including door traversal,” _IEEE Robot. Automat. Lett._ , 2023\.
  * [28] S. Thakar, P. Rajendran, A. M. Kabir, and S. K. Gupta, “Manipulator motion planning for part pickup and transport operations from a moving base,” _IEEE Trans. Automat. Sci. Eng._ , vol. 19, no. 1, pp. 191–206, 2020.
  * [29] Q. Huang, K. Tanie, and S. Sugano, “Coordinated motion planning for a mobile manipulator considering stability and manipulation,” _Int. J. Robot. Res._ , vol. 19, no. 8, pp. 732–742, 2000.
  * [30] F. Burget, M. Bennewitz, and W. Burgard, “Bi 2 rrt*: An efficient sampling-based path planning framework for task-constrained mobile manipulation,” in _Proc. IEEE/RSJ Int. Conf. Intell. Robot. Syst._ , 2016, pp. 3714–3721.
  * [31] G. Oriolo and C. Mongillo, “Motion planning for mobile manipulators along given end-effector paths,” in _Proc. IEEE Int. Conf. Robot. Autom._ , 2005, pp. 2154–2160.
  * [32] S. M. LaValle, _Planning algorithms_. Cambridge university press, 2006.
  * [33] T. Yoshikawa, “Manipulability of robotic mechanisms,” _Int. J. Robot. Res._ , vol. 4, no. 2, pp. 3–9, 1985.
  * [34] S. Chitta, I. Sucan, and S. Cousins, “Moveit![ros topics],” _IEEE Robot. Automat. Mag._ , vol. 19, no. 1, pp. 18–19, 2012.
  * [35] 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 Trans. Mechatronics_ , vol. 27, no. 6, pp. 4396–4405, 2022.