<figure><img src="image_1.png" /></figure>

HAL
open science

# Wrench Capability Analysis and Control Allocation of a Collaborative Multi-Drone Grasping Robot

Zhongmou Li, Vincent Bégoc, Abdelhamid Chriette, Isabelle Fantoni

► To cite this version:

Zhongmou Li, Vincent Bégoc, Abdelhamid Chriette, Isabelle Fantoni. Wrench Capability Analysis and Control Allocation of a Collaborative Multi-Drone Grasping Robot. Journal of Mechanisms and Robotics, 2023, 15 (2), 10.1115/1.4054610. hal-03964822

HAL Id: hal-03964822

https://hal.science/hal-03964822

Submitted on 31 Jan 2023

HAL is a multi-disciplinary open access
archive for the deposit and dissemination of sci-
entific research documents, whether they are pub-
lished or not. The documents may come from
teaching and research institutions in France or
abroad, or from public or private research centers.

L'archive ouverte pluridisciplinaire HAL, est
destinée au dépôt et à la diffusion de documents
scientifiques de niveau recherche, publiés ou non,
émanant des établissements d'enseignement et de
recherche français ou étrangers, des laboratoires
publics ou privés.

Public Domain
# Wrench capability analysis and control allocation of a collaborative multi-drone grasping robot

**Zhongmou LI\***

Laboratoire des Sciences du Numérique de Nantes
UMR 6004, 44321 Nantes, France
École Centrale de Nantes
Email: zhongmou.li@ls2n.fr

**Vincent Bégoc**

Laboratoire des Sciences du Numérique de Nantes
UMR 6004, 44321 Nantes, France
Institut Catholique d'Arts de Métirers (Icam)
Email: vincent.begoc@icam.fr

**Abdelhamid Chriette**

Laboratoire des Sciences du Numérique de Nantes
UMR 6004, 44321 Nantes, France
École Centrale de Nantes
Email: Abdelhamid.Chriette@ls2n.fr

**Isabelle Fantoni**

Laboratoire des Sciences du Numérique de Nantes
UMR 6004, 44321 Nantes, France
Centre National de la Recherche Scientifique (CNRS)
Email: Isabelle.Fantoni@ls2n.fr

This paper presents the wrench capability analysis, the controller design and experimental results of an aerial robot Flying Gripper. This robot uses four quadrotors to actuate four fingers such as to grasp and manipulate large size objects. The yaw motion of each quadrotor is used to open/close one of the four fingers. A method is proposed to analyze its manipulability considering inequality constraints such as the propellers' capabilities and equality constraints such as the yaw torque applied by each quadrotor to open or close a finger and the equilibrium conditions of passive joints. This allows concluding on the full manipulability of the robot and evaluating its force and torque capabilities. A Dynamic Control Allocation algorithm is implemented to distribute the control effort among all quadrotors while guaranteeing an achievable solution satisfying the aforementioned constraints. A proof of concept is realized and allows presenting preliminary experimental results of the Flying Gripper.

**NOMENCLATURE**

$m_t$ Total mass of the robot and the grasped object.

$^b\mathbf{R}_0$ Rotation matrix of frame $\mathcal{F}_0$ w.r.t frame $\mathcal{F}_b$.

$^0\mathbf{R}_i$ Rotation matrix of frame $\mathcal{F}_i$ w.r.t frame $\mathcal{F}_0$.

$^b\mathbf{R}_i$ Rotation matrix of frame $\mathcal{F}_i$ w.r.t frame $\mathcal{F}_b$.

$^b\mathbf{R}_{s_i}$ Rotation matrix of frame $\mathcal{F}_{s_i}$ w.r.t frame $\mathcal{F}_b$.

$\mathbf{R}_r$ Matrix mapping $\mathbf{f}_z$ to $^0\mathbf{f}_q$.

$^b\mathbf{r}_i$ Position vector of frame $\mathcal{F}_i$ w.r.t frame $\mathcal{F}_b$.

$\mathbf{q}$ Vector of generalized coordinates.

$\mathbf{p}_b$ Pose vector of the body structure.

$\mathbf{q}_a$ Vector regrouping UAV' attitude vectors $\boldsymbol{\eta}_i$.

$\boldsymbol{\eta}_i$ Vector of Euler angles represent the $i^{th}$ UAV's attitude.

$^i\mathbf{w}_i$ Wrench vector generated by the $i^{th}$ UAV expressed in $\mathcal{F}_i$.

$^i\mathbf{w}_i^r$ Reduced wrench vector generated by the $i^{th}$ UAV expressed in $\mathcal{F}_i$.

$^s\mathbf{w}_i$ Wrench vector generated by the $i^{th}$ UAV expressed in $\mathcal{F}_{s_i}$.

<footnote>\*Supported by China Scholarship Council.</footnote>
$^b\mathbf{w}_{i,b}$ Wrench vector applied to the body structure by the $i^{th}$ UAV expressed in $\mathcal{F}_b$.

$^b\mathbf{w}_{ext}$ External wrench vector acting on the body structure applied by the environment in $\mathcal{F}_b$.

$\mathbf{w}_b$ Wrench vector exerted on the body structure by UAVs composed of a force expressed in $\mathcal{F}_0$ and a torque expressed in $\mathcal{F}_b$ that $\mathbf{w}_b = [^0\mathbf{f}_b^T {}^b\tau_b^T]^T$.

$^i\mathbf{f}_i$ Thrust force vector produced by the $i^{th}$ UAV expressed in $\mathcal{F}_i$.

$^i\boldsymbol{\tau}_i$ Torque vector produced by the $i^{th}$ UAV expressed in $\mathcal{F}_i$.

$^0\mathbf{f}_q$ Vector regrouping all UAVs' thrust force vectors expressed in $\mathcal{F}_0$ that $^0\mathbf{f}_q = [^0\mathbf{f}_1^T \ ^0\mathbf{f}_2^T \ ^0\mathbf{f}_3^T \ ^0\mathbf{f}_4^T]^T$.

$^q\boldsymbol{\tau}_q$ Vector regrouping all UAVs' torques: $^q\boldsymbol{\tau}_q = [^1\boldsymbol{\tau}_1^T \ ^2\boldsymbol{\tau}_2^T \ ^3\boldsymbol{\tau}_3^T \ ^4\boldsymbol{\tau}_4^T]^T$.

$\mathbf{f}_z$ Vector regrouping all UAVs' thrust forces along $\mathbf{z}_i$ axis of $\mathcal{F}_i$ that $\mathbf{f}_z = [f_{1,z} \ f_{2,z} \ f_{3,z} \ f_{4,z}]^T$.

$^b\mathbf{W}_i$ Mapping matrix for $i^{th}$ UAV's wrench $^i\mathbf{w}_i$ from $\mathcal{F}_i$ to $\mathcal{F}_b$.

$^s\mathbf{W}_i$ Mapping matrix for $i^{th}$ UAV's wrench $^i\mathbf{w}_i$ from $\mathcal{F}_i$ to $\mathcal{F}_{si}$.

$^b\mathbf{W}_{si}$ Mapping matrix for $i^{th}$ UAV's wrench $^s\mathbf{w}_i$ from $\mathcal{F}_{si}$ to $\mathcal{F}_b$.

$\mathbf{W}_f$ Mapping matrix for all UAVs that maps $^0\mathbf{f}_q$ to $\mathbf{w}_b$.

$\mathbf{\Omega}_i$ Vector of motor speed squares of the $i^{th}$ UAV.

$\mathcal{A}_i$ Available UAV actuation set.

$\mathcal{W}_i^r$ Available UAV reduced wrench set.

$\mathcal{W}_i^l$ Available UAV wrench set.

$\mathcal{W}_i^{yaw}$ Available UAV wrench set considering yaw torque constraints.

$\mathcal{W}_i^{eq}$ Available UAV wrench set considering yaw torque constraints and static equilibrium conditions.

$\mathcal{W}_i^{s}(\eta_i)$ Available UAV wrench set expressed in $\mathcal{F}_{si}$ when the UAV's attitude is represented by $\eta_i$.

$\mathcal{W}_i^{s}$ Available UAV wrench set expressed in $\mathcal{F}_{si}$ considering all admissible attitudes.

$\mathcal{W}_i^b$ Available wrench set applied to the body structure by the $i^{th}$ UAV.

$\mathcal{W}_b$ Available body wrench set.

# 1 Introduction

Unmanned Aerial Vehicles (UAVs), thanks to their versatility, are expected to accomplish increasingly complex tasks, such as grasping and manipulating objects. The most common approach consists in attaching additional mechanisms to a multirotor, such as a gripper [1], a serial manipulator [2], a parallel manipulator [3] or a dual-arm manipulator [4]. However, such systems have a reduced payload and limited manipulation capabilities due to the underactuation of quadrotors.

To increase the payload, emerging works investigate multi-quadrotor cooperative systems. Indeed, quadrotors are largely used for their agility and low cost. Multiple quadrotors are thus connected to the payload or the end-effector using cables [5, 6] or rigid links [7, 8], which, however, are not

designed to perform grasping. Other approaches use multiple UAVs to exert contacts on the object to grasp and manipulate it [9, 10, 11]. Nevertheless, for each of these concepts, the security of the grasp highly relies on friction and could hardly be guaranteed in real conditions. Interestingly, in most of the previous works that use multiple quadrotors, the yaw motion of each quadrotor does not contribute to the manipulation task, making this degree of freedom useless.

Few aerial robot designs taking advantage of the yaw motion of a quadrotor to contribute to the manipulation task can be found in [12, 13]. Authors recently introduced a concept of a multi-drone robot intended to grasp and manipulate large objects [14, 15]. The proposed robot can be described as a flying hand composed of a body structure, four self-adaptive fingers [16] and four quadrotors. Each finger is actuated using the yaw motion of each quadrotor transmitted through a worm-gear mechanism to guarantee the robustness of the grasp.

The manipulability of a multi-drone robot can be analyzed by studying the set of wrenches that can be applied by quadrotors on a manipulated platform or a grasped object. This wrench capability analysis method has been extensively applied in the fields of robotic hands and Cable-Driven Parallel Robots (CDPRs) [17]. Both of these two families of robots are subject to unilateral constraints: unilateral contact forces for robot hands and unilateral cable tensions for CDPRs. In a same manner, aerial robots are also subject to unilateral constraints since propellers can generally turn in a unique direction. In [18] and [19], the feasible wrench set is built in order to analyze the manipulability of an hexarotor and an octorotor respectively. However these works do not give insights on the method used to compute this set. Few works have been dedicated to the wrench capability analysis of multi-drone robots and mostly apply to Aerial Cable Towed Systems (ACTS) [20, 21, 22]. The analysis of the manipulability of a different concept where quadrotors are connected to a platform through passive spherical joints is presented in [7]. In most of these works, each quadrotor is reduced to a thrust generator since the net yaw torque produced by each quadrotor cannot be transmitted through a cable or a spherical joint. A lower and an upper boundary on the thrust force produced by the quadrotor are thus computed from the capabilities of each propeller. In [22], authors compute the upper boundary considering the roll and pitch torques required to compensate for the non-coincidence of the geometric center of each quadrotor with its center of mass (CoM) or with the point of attachment of the cable. The corresponding set of inequalities defines the available thrust set which is then mapped in the wrench space to obtain the allowable wrench space. An intermediate step can be included to consider the maximum tension in cables. However, existing methods only deal with inequality constraints and, thus, cannot apply to the proposed Flying Gripper (FG) robot. Indeed, in this case, each quadrotor is more than a thrust generator since it also applies a yaw torque to close or open the corresponding finger. Obviously, the value of this yaw torque has a major influence on the maximum allowable net thrust force produced by each quadrotor and, consequently, on the
wrench capability of the whole robot.

Since, the number of actuators of the FG robot is larger
than the dimension of its task space, the FG is overactuated
and requires a control allocation algorithm to distribute con-
trol efforts. Control allocation is often formulated as an opti-
mization problem where redundant degrees of actuation are
used to achieve additional objectives while allowing to con-
sider system constraints at the same time. In this domain, a
2-step quadratic optimization method Dynamic Control Al-
location (DCA) is proposed in [23] which is well known for
its time-effective property. It allows dealing with a system of
linear constraints and guarantee the continuity of the control
input, so it is used to consider actuator rate constraints of a
new flight control system in [24], to consider desired control
actions and actuator constraints of a power system in [25]
when an actuator fails. The main contributions of this paper
are highlighted below. This paper presents a new version of
the Flying Gripper initially presented in [15], which intro-
duces passive universal joints between each quadrotor and
the corresponding worm screw actuating the finger. This al-
lows each quadrotor to modify its orientation w.r.t the body
frame. This design modification significantly modifies the
manipulability of the FG robot as well as the quadrotors'
control strategy. An original method is proposed to com-
pute the available wrench set of the FG robot, dealing with
equality constraints imposed by the quadrotor yaw torques
required to open or close fingers, as well as the equilibrium
conditions of each quadrotor. The obtained available wrench
set allows concluding on the full manipulability of the robot
while closing or opening fingers. Simulation results were
presented in [15] using a model predictive controller able to
deal with model uncertainties but not capable of fulfilling ac-
tuators' limits. In this paper, a DCA is implemented to find
an input solution for each quadrotor that lies inside the avail-
able quadrotor wrench set (i.e. satisfies propellers' capabil-
ities) while reducing energy consumption and managing the
control input continuity. A proof of concept is realized and
presented in order to demonstrate experimentally the full ma-
nipulability of the FG robot and show its capability to open
or close fingers using the yaw motion of quadrotors while
maintaining the stability of the body structure.

This paper is organized as follows. Section 2 introduces
the architecture of the new FG robot and develops the static
and dynamic models. Section 3 presents a method based
on the available quadrotor wrench set to obtain the avail-
able wrench space of the FG. A qualitative and a quantitative
analysis of the FG robot's manipulability are then proposed.
Section 4 demonstrates the control scheme of the FG robot.
Especially, a DCA algorithm is presented for distributing the
control input among quadrotors. In section 5, preliminary
experimental results are presented and evaluated. A conclu-
sion is drawn in section 6.

## 2 Introduction and Modeling of the Flying Gripper

2.1 Presentation of the Concept

The FG robot is a collaborative multi-drone grasping
robot that is composed of four off-the-shelf quadrotors, four

self-adaptive fingers and a body structure (see Fig. 1). An
originality of the concept relies on the fact that the quadro-
tors' yaw rotations are used to actuate the closing/opening
motion of self-adaptive fingers (Fig. 2a).

Each finger has two phalanges and is self-adaptive, i.e.
it can adapt to the shape and size of an object without requir-
ing any sensor or complex control strategy, making the grasp
more robust to positioning uncertainty of the robot w.r.t the
object. The finger's self-adaptation is achieved thanks to a
differential mechanism (i.e. a four-bar mechanism), which
allows sharing the input torque applied on the actuation bar
among the proximal and distal phalanxes. More detail on the
working principle of such finger can be found in [14, 16].

Each finger is driven by the yaw rotation of the corre-
sponding quadrotor that is transmitted through a worm-gear
mechanism. The kinematic scheme of a finger and its actua-
tion principle is given in Fig. 2b. Worm screws of quadrotors
1 and 3 and those of quadrotors 2 and 4 are chosen with the
opposite hand (left and right) such that, when the four fin-
gers are closing or opening together, the resulting net torque
applied on the body structure is canceled.

The worm-gear mechanisms are designed to be non-
backdrivable to produce form-closed grasps [26], such that
fingers cannot move backward once phalanxes are contact-
ing the object. Releasing the object thus requires quadrotors
to rotate in the opposite direction to open fingers.

This form-closure stability criterion yields secured
grasps that do not depend on the capability of actuators nor
on friction between phalanges and the grasped object. Fur-
thermore, no additional energy is required for grasping dur-
ing the flight. A guideline is given in [14] to optimize the
geometric parameters of a planar version of FG to maximize
its manipulability and its capability to produce form-closed
grasps for objects of different dimensions and despite posi-
tioning uncertainties of the objects.

This paper considers a different version of the FG pre-
sented in [15]. Indeed, a universal joint is introduced to link
each quadrotor to the corresponding worm screw, which al-
lows two supplementary Degrees of Freedom (DOF), roll
and pitch rotations, for each quadrotor w.r.t the body struc-
ture. In the previous version, quadrotors had a fixed attitude
w.r.t the body frame, only the yaw motion of each quadro-
tor was allowed to actuate the corresponding finger. With
this new version, each quadrotor is allowed to modify its at-
titude w.r.t the body structure while transmitting its yaw ro-
tation to the worm screw to open or close the finger even
if the quadrotor yaw axis and the worm screw axis are not
coincident (Fig. 2c). This design modification allows reduc-
ing energy consumption during transportation by allowing
each quadrotor to adopt a horizontal attitude. Furthermore,
it is able to increase the manipulability performances (for a
detailed comparison see [27], the manipulability analysis of
this new version is addressed in section 3). In addition, this
new design permits using a thrust-attitude low-level quadro-
tor controller (controller scheme will be presented in sec-
tion 4), instead of a propeller speed controller which is not
so well implemented on off-the-shelf quadrotors.
<figure><img src="image_2.png" /><figcaption>Fig. 1: Computer Aided Design (CAD) model and main parameters of the Flying Gripper (FG) that is composed of a body structure, four self-adaptive fingers and four quadrotors: the frame $\mathcal{F}_{s_i}$ is attached to the universal joint whose origin coincides with the CoM of quadrotor $i$ and $\mathbf{z}_{s_i}$ axis coincides with the worm screw axis (the attitude of $\mathcal{F}_{s_i}$ w.r.t $\mathcal{F}_b$ is represented by the constant matrix $^b\mathbf{R}_{s_i}$).</figcaption></figure>

<figure><img src="image_3.png" /><figcaption>Fig. 2: (a) CAD model of a self-adaptive finger actuated by a quadrotor, (b) kinematic scheme of this finger, the quadrotor's yaw motion is transmitted through a worm-gear mechanism to an actuation bar which closes or opens the two phalanxes, a universal joint is introduced between the quadrotor and the worm screw, (c) details of the transmission between the quadrotor and the worm screw, a universal joint allows the pitch and roll motions of the quadrotor, the shaft alignment angle between the quadrotor's yaw axis and the screw axis is represented by $\phi_i$ with the maximal value being $\phi_{max}$.</figcaption></figure>

## 2.2 Static Modeling

### 2.2.1 Parametrization

The main geometrical parameters of the robot are the length $L$ and the height $H$ (see Fig. 1). Let introduce $\mathbf{q} = [\mathbf{p}_b^T \mathbf{q}_a^T]^T \in \mathbb{R}^{18 \times 1}$ is the vector of generalized coor-

dinates and the vector $\mathbf{p}_b = [^0\mathbf{x}_b^T \mathbf{\eta}_b^T]^T \in \mathbb{R}^{6 \times 1}$ describes the pose of the body structure. The position vector $^0\mathbf{x}_b = [x_b \ y_b \ z_b]^T \in \mathbb{R}^{3 \times 1}$ is expressed in the world frame $\mathcal{F}_0$ and $\mathbf{\eta}_b = [\phi_b \ \theta_b \ \psi_b]^T \in \mathbb{R}^{3 \times 1}$ is the vector of Euler angles of the
body structure frame $\mathcal{F}_b$ w.r.t the world frame $\mathcal{F}_0$. The vector $\mathbf{q}_a = [\boldsymbol{\eta}_1^T \boldsymbol{\eta}_2^T \boldsymbol{\eta}_3^T \boldsymbol{\eta}_4^T]^T \in \mathbb{R}^{12 \times 1}$, where $\boldsymbol{\eta}_i = [\phi_i \theta_i \psi_i]^T \in \mathbb{R}^{3 \times 1}$ is the vector of Euler angles representing the attitude of the $i^{th}$ quadrotor local frame $\mathcal{F}_i$ w.r.t the world frame $\mathcal{F}_0$.

The origin of the body frame $\mathcal{F}_b$ is placed at the CoM of the whole system (the robot and the object). It was arbitrarily determined considering the sample case of a cylindrical object centered in the hand, with a radius of 0.7 m. For simplification purposes, the position of the CoM of the whole system is assumed to be fixed and independent from the quadrotors' attitude and from the fingers' configuration.

The static equilibrium of the robot in $\mathcal{F}_b$ is thus given by the relation:

$$ {}^b\mathbf{w}_{ext} + \sum_{i=1}^{4} {}^b\mathbf{w}_i {}^i\mathbf{w}_i - \begin{bmatrix} {}^b\mathbf{R}_0(m_t \mathbf{g}) \\ \mathbf{0}_{3 \times 1} \end{bmatrix} = \mathbf{0} \quad (1) $$

where $m_t$ is the mass of the whole system and ${}^b\mathbf{R}_0 \in \mathbb{R}^{3 \times 3}$ is the rotation matrix of $\mathcal{F}_0$ w.r.t $\mathcal{F}_b$. The term ${}^b\mathbf{w}_{ext} \in \mathbb{R}^{6 \times 1}$ represents the external wrench applied on the body structure in $\mathcal{F}_b$, and ${}^i\mathbf{w}_i = [{}^i\mathbf{f}_i^T \ {}^i\boldsymbol{\tau}_i^T]^T = [f_{i,x} f_{i,y} f_{i,z} \boldsymbol{\tau}_{i,x} \boldsymbol{\tau}_{i,y} \boldsymbol{\tau}_{i,z}]^T \in \mathbb{R}^{6 \times 1}$ represents the wrench produced by quadrotor $i$ in $\mathcal{F}_i$. The matrix ${}^b\mathbf{W}_i \in \mathbb{R}^{6 \times 6}$ maps ${}^i\mathbf{w}_i$ in $\mathcal{F}_b$:

$$ {}^b\mathbf{W}_i = \begin{bmatrix} {}^b\mathbf{R}_i \\ -[\bar{\mathbf{b}}\mathbf{r}_i]_\times {}^b\mathbf{R}_i \\ -\mathbf{b}\mathbf{0}_{3 \times 3} \\ \mathbf{b}\mathbf{R}_i \end{bmatrix} \quad (2) $$

in which the ${}^b\mathbf{r}_i \in \mathbb{R}^{3 \times 1}$ is the position vector of quadrotor $i$ in $\mathcal{F}_b$ and ${}^b\mathbf{R}_i \in \mathbb{R}^{3 \times 3}$ is the rotation matrix of $\mathcal{F}_i$ w.r.t $\mathcal{F}_b$.

Since universal joints allow the free roll and pitch rotation of each quadrotor, at static equilibrium, the corresponding roll and pitch torques of each quadrotor $i$ must be zero

$$ \boldsymbol{\tau}_{i,x} = \boldsymbol{\tau}_{i,y} = 0 \quad (3) $$

with $\boldsymbol{\tau}_{i,x}$ and $\boldsymbol{\tau}_{i,y}$ being the torques along $\mathbf{x}_i$ and $\mathbf{y}_i$ in $\mathcal{F}_i$.

### 2.2.2 Relating UAV Reduced Wrench to Motor Speeds

Since a quadrotor has only four motors, it is underactuated in SE(3). Indeed, it can only generate one force along $\mathbf{z}_i$ and three torque components independently. This four-dimensional quadrotor wrench is further denoted as the quadrotor reduced wrench ${}^i\mathbf{w}_i^r$, in order to highlight that it is not a six-dimensional wrench.

The mathematical model characterizing the relation between the quadrotor reduced wrench ${}^i\mathbf{w}_i^r = [f_{i,z} \ {}^i\boldsymbol{\tau}_i^T]^T \in \mathbb{R}^{4 \times 1}$ and the motor speed square $\boldsymbol{\Omega}_i \in \mathbb{R}^{4 \times 1}$ is introduced in [28]:

$$ {}^i\mathbf{w}_i^r = \boldsymbol{\Gamma}_i \boldsymbol{\Omega}_i \quad (4) $$

and

$$ \boldsymbol{\Gamma}_i = \begin{bmatrix} C_F & C_F & C_F & C_F \\ 0 & C_F r_q & 0 & -C_F r_q \\ -C_F r_q & 0 & C_F r_q & 0 \\ C_M & -C_M & C_M & -C_M \end{bmatrix} \quad (5) $$

where $\boldsymbol{\Gamma}_i \in \mathbb{R}^{4 \times 4}$ is a square and constant matrix, $C_F$ and $C_M$ are the coefficients of thrust and drag of the propellers, and $r_q$ is the distance between the CoM and each propeller's axis of rotation.

Since each quadrotor $i$ can only generate a thrust force along $\mathbf{z}_i$ axis in $\mathcal{F}_i$, it leads to

$$ {}^i\mathbf{f}_i = [0 \ 0 \ f_{i,z}]^T \quad (6) $$

## 2.3 Dynamic Modeling

In this section, the dynamic model of the FG is developed to be used further in the controller design in section 4.

The following assumptions are made to simplify the dynamic modeling: i) the center of rotation of each universal joint coincides with the CoM of its corresponding quadrotor; ii) universal joints are modeled as spherical joints by neglecting the yaw torques transmitted between the quadrotors and the worm-gear mechanisms 1; iii) fingers are considered to close or open slowly, thus neglecting the dynamics of fingers.

Assumptions i) and ii) allow decoupling the dynamics of each quadrotor from the dynamics of the robot's body. The resulting modeling errors will be compensated by the controller presented in section 4.

Based on these assumptions, the dynamic model of the FG is constructed as follows:

$$ \mathbf{M}_b \ddot{\mathbf{p}}_b + \mathbf{c}_b = \mathbf{w}_b \quad (7) $$

$$ \mathbf{M}_a \ddot{\mathbf{q}}_a + \mathbf{c}_a = {}^q\boldsymbol{\tau}_q \quad (8) $$

$$ \mathbf{w}_b = \mathbf{W}_f \, {}^0\mathbf{f}_q \quad (9) $$

$$ {}^0\mathbf{f}_q = \mathbf{R}_r \mathbf{f}_z \quad (10) $$

where the inputs are $\mathbf{f}_z = [f_{1,z} \ f_{2,z} \ f_{3,z} \ f_{4,z}]^T \in \mathbb{R}^{4 \times 1}$ and ${}^q\boldsymbol{\tau}_q = [{}^1\boldsymbol{\tau}_1^T \ {}^2\boldsymbol{\tau}_2^T \ {}^3\boldsymbol{\tau}_3^T \ {}^4\boldsymbol{\tau}_4^T]^T \in \mathbb{R}^{12 \times 1}$ regrouping respectively the thrust forces $f_{i,z} \in \mathbb{R}$ along the $\mathbf{z}_i$ direction of $\mathcal{F}_i$ and the torques ${}^i\boldsymbol{\tau}_i \in \mathbb{R}^{3 \times 1}$ of all quadrotors respectively expressed in $\mathcal{F}_i$. The term $\mathbf{M}_b \in \mathbb{R}^{6 \times 6}$ is the inertia matrix of the body structure, $\mathbf{M}_a = \text{diag}(\mathbf{M}_1, \mathbf{M}_2, \mathbf{M}_3, \mathbf{M}_4) \in \mathbb{R}^{12 \times 12}$ with $\mathbf{M}_i \in \mathbb{R}^{3 \times 3}$ being the inertia matrix of quadrotor $i$, $\mathbf{c}_a \in \mathbb{R}^{12 \times 1}$ represent the Coriolis/centripetal terms for the body structure and the quadrotors respectively. The vector $\mathbf{w}_b = [{}^0\mathbf{f}_b^T \ {}^b\boldsymbol{\tau}_b^T]^T \in \mathbb{R}^{6 \times 1}$ is the net wrench applied

<footnote n="1">¹As mentioned in section 2, worm gears have been chosen with the opposite hand so that, when the four fingers are closing or opening together, the resulting net torque applied on the body structure is canceled, moreover, the effect of the quadrotor's yaw torque on the net torque applied on the body structure is negligible w.r.t the effect of the thrust force $f_{i,z}$.</footnote>
by quadrotors on the body structure composed of a force $^0\mathbf{f}_b \in \mathbb{R}^{3 \times 1}$ expressed in $\mathcal{F}_0$ and a torque $^b\boldsymbol{\tau}_b \in \mathbb{R}^{3 \times 1}$ expressed in $\mathcal{F}_b$. The term $^0\mathbf{f}_q = [^0\mathbf{f}_1^T \ ^0\mathbf{f}_2^T \ ^0\mathbf{f}_3^T \ ^0\mathbf{f}_4^T]^T \in \mathbb{R}^{12 \times 1}$ is a vector regrouping thrust forces generated by four quadrotors expressed in $\mathcal{F}_0$, and $\mathbf{W}_f \in \mathbb{R}^{6 \times 12}$ is a matrix that maps $^0\mathbf{f}_q$ to $\mathbf{w}_b$. The matrix $\mathbf{R}_r \in \mathbb{R}^{12 \times 4}$ maps $\mathbf{f}_z$ to $^0\mathbf{f}_q$ and depends on the pitch and roll angles of each quadrotor $\phi_i, \theta_i$.

### 3 Wrench Capability Analysis of the Flying Gripper

This section proposes a method based on convex hull theory to analyze the manipulability of the FG from the available body wrench set, i.e. the set of wrenches that can be applied by quadrotors on the robot's body. It is based on the computation of the available quadrotor wrench set, taking into account the actuators' capabilities, the yaw torque applied to open or close the finger, the equilibrium conditions of the FG mechanism's internal mobilities due to the introduction of universal joints and the mechanical stops associated to these universal joints. Note that this method analyzes the available body wrench set when the robot is at static equilibrium, and is thus based on the static model developed in section 2.2.

The proposed method permits taking into account the following particularities of the FG robot which all affect the set of wrenches that can be applied by each quadrotor on the body structure: i) the actuators' capabilities, ii) the static equilibrium conditions, iii) the range of rotation of the universal joints, authorized by the mechanical stops, iv) the yaw torques applied to drive fingers when closing or opening fingers.

It is possible to analyze the manipulability performances of designs, with different dimension parameters $L, H$, or using different elements, like motors with different $\omega_{\min}$ and $\omega_{\max}$. This method is illustrated considering the design parameters given in Table 1.

#### 3.1 Building the Available Body Wrench Set of the Flying Gripper

In the following, it is explained that how to build, in 8 steps, the available body wrench set of the FG. Results are obtained using algorithms provided by the Multi-Parametric Toolbox 3.0 (MPT3) in Matlab [29]. Moreover, this toolbox allows representing a polytope using a set of linear inequalities and equalities. More details about operations on polytopes can be found in the Appendix and in [30].

##### Step 1. Available UAV Actuation Set $\mathcal{A}_i$

The method starts by building the available actuation set of a single quadrotor, considering the minimum and maximum velocity of each propeller. The vector $\mathbf{\Omega}_i = [\omega_{i,1}^2 \ \omega_{i,2}^2 \ \omega_{i,3}^2 \ \omega_{i,4}^2]^T \in \mathbb{R}^{4 \times 1}$ represents the quadrotor actuation state, where $\omega_{i,j}$ is the $j$th motor's speed of quadrotor $i$. Thus, each quadrotor's actuation capability is modeled with a lower bound $\mathbf{\Omega} = [\omega_{\min}^2 \ \omega_{\min}^2 \ \omega_{\min}^2 \ \omega_{\min}^2]^T \in \mathbb{R}^{4 \times 1}$ and an

<table><caption>Table 1: Main parameters used in the wrench capability analysis.</caption><thead><tr><th>Symbol</th><th>Physical meaning</th><th>Value</th></tr></thead><tbody><tr><td>$C_F$</td><td>thrust coefficient</td><td>3.65 × 10<sup>-6</sup> Ns<sup>2</sup>rad<sup>-2</sup></td></tr><tr><td>$C_M$</td><td>drag coefficient</td><td>5.40 × 10<sup>-9</sup> Nms<sup>2</sup>rad<sup>-2</sup></td></tr><tr><td>$\omega_{\min}$</td><td>minimal motor speed</td><td>300 rad s<sup>-1</sup></td></tr><tr><td>$\omega_{\max}$</td><td>maximal motor speed</td><td>1400 rad s<sup>-1</sup></td></tr><tr><td>$\tau_f$</td><td>friction torque to drive a finger</td><td>0.01 Nm</td></tr><tr><td>$\phi_{\max}$</td><td>maximal shaft alignment angle of universal joints</td><td>30°</td></tr><tr><td>$L, H$</td><td>dimension of the robot</td><td>0.72 m, 0.62 m</td></tr><tr><td rowspan="4">$^b\mathbf{R}_{s_i}$</td><td rowspan="4">attitude of the universal joint linked to quadrotor i w.r.t the body structure (i = 1, ..., 4)</td><td>$\mathbf{R}_x(28^{\circ})\mathbf{R}_y(30^{\circ})$</td></tr><tr><td>$\mathbf{R}_x(-28^{\circ})\mathbf{R}_y(30^{\circ})$</td></tr><tr><td>$\mathbf{R}_x(-28^{\circ})\mathbf{R}_y(-30^{\circ})$</td></tr><tr><td>$\mathbf{R}_x(28^{\circ})\mathbf{R}_y(-30^{\circ})$</td></tr></tbody></table>

$\mathbf{R}_x, \mathbf{R}_y$ are matrices representing the rotations around x and y axes.

upper bound $\overline{\mathbf{\Omega}} = [\omega_{\max}^2 \ \omega_{\max}^2 \ \omega_{\max}^2 \ \omega_{\max}^2]^T \in \mathbb{R}^{4 \times 1}$ as

$$ \underline{\mathbf{\Omega}} \le \mathbf{\Omega}_i \le \overline{\mathbf{\Omega}} \quad (11) $$

For the needs of the method, Eq. (11) is reformulated to obtain the $\mathcal{H}$-representation of the available UAV actuation set $\mathcal{A}_i$ as a collection of linear inequalities (see the introduction of the $\mathcal{H}$-representation in Appendix):

$$ \mathcal{A}_i = \{ \mathbf{\Omega}_i \in \mathbb{R}^{4 \times 1} \mid \mathbf{A}_{\mathbf{\Omega}_i} \mathbf{\Omega}_i \le \mathbf{b}_{\mathbf{\Omega}_i} \} \quad (12) $$

in which $\mathbf{b}_{\mathbf{\Omega}_i} = [\overline{\mathbf{\Omega}}^T \mid -\mathbf{\Omega}^T]^T \in \mathbb{R}^{8 \times 1}$, $\mathbf{A}_{\mathbf{\Omega}_i} = [\mathbf{I}_4 \mid -\mathbf{I}_4]^T \in \mathbb{R}^{8 \times 4}$ where 8 stands for the number of inequalities and 4 is the dimension of the UAV actuation space. The UAV available actuation set $\mathcal{A}_i$ is a 4-cube shown in Fig. 3.

##### Step 2. Linear Mapping of $\mathcal{A}_i$ in the UAV Local Reduced Wrench Space

Based on the linear bijection $\Gamma_i$ in Eq. (4), it is possible to compute $\mathcal{W}_i^r$, the available UAV reduced wrench set of $^i\mathbf{w}_i^r$ from the available actuation set $\mathcal{A}_i$, as follows (see linear mapping of a polytope in Appendix):

$$ \mathcal{W}_i^r = \{^i\mathbf{w}_i^r \in \mathbb{R}^{4 \times 1} \mid \mathbf{A}_{i,r}^i \mathbf{w}_i^r \le \mathbf{b}_{\mathbf{\Omega}_i} \} \quad (13) $$

where $\mathbf{A}_{i,r}^i = \mathbf{A}_{\mathbf{\Omega}_i} \Gamma_i^{-1} \in \mathbb{R}^{8 \times 4}$. The polytope $\mathcal{W}_i^r$ is a 4D polytope (Fig. 4).

##### Step 3: Lifting $\mathcal{W}_i^r$ in the UAV Local Wrench Space

This step aims to reformulate the $\mathcal{H}$-representation of the available UAV reduced wrench set $\mathcal{W}_i^r \subseteq \mathbb{R}^4$ in the 6D
<figure><img src="image_4.png" /><figcaption>Fig. 3: Representation of the available UAV actuation set $\mathcal{A}_i$ (a 4-cube). For visualization purpose, its intersection with the hyper-plane $\mathcal{H}P(\omega_{i,4}^2 = \omega_{\min}^2)$ is shown in the 3D space $(\omega_{i,1}^2, \omega_{i,2}^2, \omega_{i,3}^2)$.</figcaption></figure>

<figure><img src="image_5.png" /><figcaption>Fig. 4: Representation of the available UAV reduced wrench set $\mathcal{W}_i^r$ (a 4D polytope). For visualization purpose, its intersection with the hyperplane $\mathcal{H}P(\tau_{i,y} = 0)$ is shown in the 3D space $(f_{i,z}, \tau_{i,x}, \tau_{i,z})$.</figcaption></figure>

UAV local wrench space. This will later permit applying a linear mapping of this polytope using the method introduced in the Appendix.

This so-called lifting operation requires increasing the dimension of all raw vectors of matrix $\mathbf{A}_{i,r}^i$, namely the normal vectors<fnref n="2" />, such that the inequalities of Eq. (13) remain true whatever the values of the additional wrench components $f_{i,x}, f_{i,y}$. The following matrix is obtained:

$$ \mathbf{A}_i^i = [\mathbf{0}_{8 \times 2} : \mathbf{A}_{i,r}^i] \in \mathbb{R}^{8 \times 6} \quad (14) $$

Then, it is necessary to consider the equalities $f_{i,x} = 0, f_{i,y} = 0$ as the quadrotor cannot produce any thrust force along $\mathbf{x}_i$ or $\mathbf{y}_i$ directions (see Eq. (6)), which remain true

whatever the values of $f_{i,z}$ and $i\tau_i$

$$ \mathbf{C}_i^{ii} \mathbf{w}_i = \mathbf{0} \quad (15) $$

where $\mathbf{C}_i^i = [\mathbf{e}_1 \ \mathbf{e}_2]^T \in \mathbb{R}^{2 \times 6}$ where $\mathbf{e}_i$ is a unit vector. Thus, the available UAV wrench set $\mathcal{W}_i$, i.e. available set of $i\mathbf{w}_i$, is defined as the intersection of a set of inequalities and equalities:

$$ \mathcal{W}_i = \{ i\mathbf{w}_i \in \mathbb{R}^{6 \times 1} \mid \mathbf{A}_i^{ii} \mathbf{w}_i \le \mathbf{b}_{\Omega_i}, \mathbf{C}_i^{ii} \mathbf{w}_i = \mathbf{0} \} \quad (16) $$

where $\mathbf{A}_i^i = [\mathbf{0}_{8 \times 2} : \mathbf{A}_{\Omega_i} \Gamma_i^{-1}] \in \mathbb{R}^{8 \times 6}$ represents 8 inequality constraints, and $\mathbf{C}_i^i = [\mathbf{e}_1 \ \mathbf{e}_2]^T \in \mathbb{R}^{2 \times 6}$ stands for 2 equality constraints. Because of these two equalities, $\mathcal{W}_i$ is a polytope with a dimension of 4 in $\mathbb{R}^6$ recalling that the dimension of $\mathcal{W}_i^r$ is not modified by the lifting operation.

### Step 4: Introducing the Yaw Torque Equality Constraint

When grasping or releasing an object, each quadrotor must generate a yaw torque to actuate its associated finger

$$ \tau_{i,z} = (-1)^i \tau_f \quad (17) $$

where $(-1)^i$ is introduced as left-hand worm gears mechanisms are used for quadrotors 1 and 3 and right-hand ones are used for quadrotors 2 and 4 such that when they generate opposite yaw rotations to close/open fingers thus allowing to obtain a zero net yaw torque exerted on the robot's body. This equality constraint is added to the polytope obtained in Eq. (16) to calculate $\mathcal{W}_i^{yaw}$, the available UAV wrench set considering this yaw torque constraint:

$$ \mathcal{W}_i^{yaw} = \{ i\mathbf{w}_i \in \mathbb{R}^{6 \times 1} \mid \mathbf{A}_i^{ii} \mathbf{w}_i \le \mathbf{b}_{\Omega_i}, \mathbf{C}_i^{yaw} \mathbf{w}_i = \mathbf{d}_i^{yaw} \} \quad (18) $$

where $\mathbf{C}_i^{yaw} = [\mathbf{e}_1 \ \mathbf{e}_2 \ \mathbf{e}_6]^T \in \mathbb{R}^{3 \times 6}$ and $\mathbf{d}_i^{yaw} = [0 \ 0 \ (-1)^i \tau_f]^T \in \mathbb{R}^3$. Due to this additional equality constraint, the dimension of the obtained available UAV wrench set $\mathcal{W}_i^{yaw}$ reduces to at most 3 (see Fig. 5).

### Step 5: Introducing Static Equilibrium Conditions

Because of universal joints, no torque around $\mathbf{x}_i$ and $\mathbf{y}_i$ can be transmitted by the corresponding quadrotor to the body structure. The static equilibrium condition of each quadrotor $i$ is modeled by Eq.(3). These two equality constraints are added to the polytope $\mathcal{W}_i^{yaw}$ (see Eq.(18)) to obtain $\mathcal{W}_i^{eq}$:

$$ \mathcal{W}_i^{eq} = \{ i\mathbf{w}_i \in \mathbb{R}^{6 \times 1} \mid \mathbf{A}_i^{ii} \mathbf{w}_i \le \mathbf{b}_{\Omega_i}, \mathbf{C}_i^{eq} \mathbf{w}_i = \mathbf{d}_i^{eq} \} \quad (19) $$

where $\mathbf{C}_i^{eq} = [\mathbf{e}_1 \ \mathbf{e}_2 \ \mathbf{e}_6 \ \mathbf{e}_5]^T \in \mathbb{R}^{5 \times 6}$ and $\mathbf{d}_i^{eq} = [0 \ 0 \ (-1)^i \tau_f \ 0 \ 0]^T \in \mathbb{R}^{5 \times 1}$ stands for 5 equality constraints. Because of these 5 independent equality constraints, the

<footnote n="2">²Each raw vector of $\mathbf{A}_{i,r}^i$ matrix corresponds to a normal vector of the corresponding halfspace.</footnote>
<figure><img src="image_6.png" /><figcaption>Fig. 5: Representation of the available UAV wrench set considering equality yaw torque constraint $W_i^{yaw}$. For visualization purpose, (a) the hyperplane $\mathcal{H}P(\tau_{i,z} = \tau_f)$ ($\tau_f = 0.01 \text{ N m}$), and the intersection of $W_i$ with $\mathcal{H}P(\tau_{i,y} = 0)$ are shown in the 3D space ($f_{i,z}, \tau_{i,x}, \tau_{i,z}$); (b) the intersection of $W_i^{yaw}$ with $\mathcal{H}P(\tau_{i,y} = 0)$ is shown in the 3D space ($f_{i,z}, \tau_{i,x}, \tau_{i,z}$); (c) $W_i^{yaw}$ that is a polytope with a dimension of 3 is shown in the 3D space ($f_{i,z}, \tau_{i,x}, \tau_{i,y}$).</figcaption></figure>

dimension of the available UAV wrench set $W_i^{eq}$ is at most 1 (see Fig. 6a and Fig. 6b).

It implies that each quadrotor at static equilibrium can only generate a thrust force along $z_i$ direction and a constant yaw torque $(-1)^i \tau_f$. This thrust force has a minimum and a maximum value (see Fig. 6b) depending on the actuator's capabilities and on the yaw torque value.

### Step 6: Linear Mapping of $W_i^{eq}$ in $\mathcal{F}_s$ and Consideration of Universal Joint Mechanical Stops

Because of the passive universal joints, each quadrotor can modify its attitude w.r.t the body structure as long as it satisfies the mechanical stops of the universal joint. The method presented below considers all admissible attitudes of quadrotors under constraints imposed by the mechanical stops. Therefore, it is assumed that each quadrotor can instantly switch from a configuration to another, i.e. neglecting its rotational dynamics. This assumption is done in [7] but not explicitly.

Here, the frame $\mathcal{F}_{s_i}$ is introduced and it is attached to the universal joint to describe central attitude of quadrotor $i$ w.r.t the body structure. The frame $\mathcal{F}_{s_i}$ is chosen so that its origin coincides with that of $\mathcal{F}_i$, and $z_{s_i}$ axis coincides with the worm screw axis (Fig. 2c).

Then, expressing the UAV wrench in $\mathcal{F}_{s_i}$ gives

$$ ^s\mathbf{w}_i = ^s\mathbf{W}_i(\boldsymbol{\eta}_i)^i \mathbf{w}_i \qquad (20) $$

where $^s\mathbf{W}_i(\boldsymbol{\eta}_i) \in \mathbb{R}^{6 \times 6}$ indeed depends on the attitude of quadrotor $i$ and is a square and invertible matrix mapping $^i\mathbf{w}_i$ in $\mathcal{F}_{s_i}$. As a consequence, the available UAV wrench set expressed in $\mathcal{F}_{s_i}$ is calculated as

$$ W_i^s(\boldsymbol{\eta}_i) = \left\{ \begin{array}{l} ^s\mathbf{w}_i \in \mathbb{R}^{6 \times 1} \mid \mathbf{A}_i^s \mathbf{W}_i^{-1}(\boldsymbol{\eta}_i)^s \mathbf{w}_i \le \mathbf{b}_{\Omega_i}, \\ \mathbf{C}_i^{\text{eq},s} \mathbf{W}_i^{-1}(\boldsymbol{\eta}_i)^s \mathbf{w}_i = \mathbf{d}_i^{\text{eq}} \end{array} \right\} \quad (21) $$

The obtained $W_i^s(\boldsymbol{\eta}_i)$ is still a polytope with a dimension of 1 in $\mathbb{R}^6$.

It is recalled here that the relative orientation of quadrotor $i$ w.r.t $z_{s_i}$ axis, can be described by the shaft alignment angle $\varphi_i$ (Fig. 2c) whose maximal angle allowed by mechanical stops is $\varphi_{max}$. To consider all admissible attitudes, the roll and pitch rotations satisfying $0 \le \varphi_i \le \varphi_{max}$ are discretized to compute a new set $W_i^s(\boldsymbol{\eta}_i)$ for each quadrotor's attitude $\boldsymbol{\eta}_i$. Then, the convex hull of the union of all these sets is computed to obtain a linearized approximation of the set of available wrenches that can be applied by the quadrotor on the robot's body expressed in $\mathcal{F}_{s_i}$

$$ W_i^s = \text{conv} \left( \bigcup_{\varphi_i \in [0, \varphi_{\text{max}}]} W_i^s(\boldsymbol{\eta}_i) \right) = \text{conv} \left( S_i^f \right) \quad (22) $$

At this stage, $W_i^s$ is defined using the $\mathcal{V}$-representation (see Appendix) due to the convex hull operation with $S_i^f$ being the set of vertices of $W_i^s$. So, $W_i^s$ shown in Fig. 6c is a cone with a dome on the top. The angle of the cone depends on $\varphi_{max}$, the height of the cone depends on the actuator's capabilities and also on the yaw torque value exerted to open or close the corresponding finger.

### Step 7: Linear Mapping of $W_i^s$ in the Body Wrench Space

The UAV wrench expressed in the body frame $\mathcal{F}_b$ can be obtained using the following formula:

$$ ^b\mathbf{w}_{i,b} = ^b\mathbf{W}_{s_i} \, ^s\mathbf{w}_i \qquad (23) $$

where $^b\mathbf{W}_{s_i} \in \mathbb{R}^{6 \times 6}$ is a constant matrix mapping $^s\mathbf{w}_i$ in $\mathcal{F}_b$, since $\mathcal{F}_{s_i}$ is fixed w.r.t $\mathcal{F}_b$ and the relative attitude of $\mathcal{F}_{s_i}$ w.r.t $\mathcal{F}_b$ is represented by the constant matrix $^b\mathbf{R}_{s_i} \in \mathbb{R}^{3 \times 3}$.

It is recalled that $W_i^s$ in the previous step, see Eq. (22), is expressed using the $\mathcal{V}$-representation that is a set of vertices.
<figure><img src="image_7.png" /><figcaption>Fig. 6: Representation of the available UAV wrench set considering static equilibrium and mobility imposed by universal joints $W_i^s$. (a) $W_i^{yaw}$ and the hyperplane $\mathcal{H}\mathcal{P}(\tau_{i,x} = \tau_{i,y} = 0)$ representing the static equilibrium condition of UAV are illustrated in the 3D space $(f_{i,z}, \tau_{i,x}, \tau_{i,y})$; (b) as the result of the intersection of $W_i^{yaw}$ with $\mathcal{H}\mathcal{P}(\tau_{i,x} = \tau_{i,y} = 0)$, $W_i^{eq}$ is the available UAV wrench set considering static equilibrium, which is a 1D polytope in the 3D space $(f_{i,z}, \tau_{i,x}, \tau_{i,y})$; (c) $W_i^s$, the convex hull of union of multiple available UAV wrench sets in $\mathcal{F}_{s_i}$, is shown in the 3D space $(^s f_{i,x}, ^s f_{i,y}, ^s f_{i,z})$ that is a cone with a dome on the top with $\phi_{max} = 30^{\circ}$.</figcaption></figure>

For computational efficiency, this representation is kept for steps 7 and 8. The available UAV wrench set $W_i^b$ expressed in $\mathcal{F}_b$ is obtained as follows (see Appendix about linear mapping of a polytope)

$$W_i^b = \mathbf{conv}(^b W_{s_i}(S_i^f)) \quad (24)$$

### Step 8: Minkowski Sum of $W_i^b$

Computing the linear sum of $^b w_{i,b}$ gives the body wrench $w_b = [^b f_{b,x} \ ^b f_{b,y} \ ^b f_{b,z} \ ^b \tau_{b,x} \ ^b \tau_{b,y} \ ^b \tau_{b,z}] \in \mathbb{R}^{6 \times 1}$. Therefore, the available body wrench set $W_b$ is computed using the Minkowski sum of the four UAV wrench sets:

$$W_b = W_1^b \oplus W_2^b \oplus W_3^b \oplus W_4^b \quad (25)$$

The resulting polytope is further discussed in the next paragraph.

## 3.2 Analysis of the Manipulability Performance of the Flying Gripper

The FG robot has four different operating modes: free flight, grasping, manipulation and placing. In the free flight and manipulation modes, each quadrotor produces a zero torque to keep its yaw angle fixed and maintain fingers in a fixed configuration. In the grasping and placing modes, each quadrotor produces a yaw torque to close or open fingers. The analysis of the robot's grasping and the manipulation modes is shown in the following.

### 3.2.1 Qualitative Criterion

From the available body wrench set, it is possible to check if the FG robot achieves the full manipulability, by checking if the wrench compensating the gravity effects

$w_g = [m_t g^T \mathbf{0}_{1 \times 3}]^T$ strictly lies inside $W_b$, with $m_t$ the mass of the whole system. If so, then the robot can generate a wrench in any direction while compensating gravity and thus achieves the full manipulability in $SE(3)$.

Analyzing $W_b$ permits obtaining the minimal and maximal total mass for which the robot achieves the full manipulability. For instance, in the manipulation mode, considering the body structure is horizontal, the FG robot can achieve the full manipulability with a mass $m_t$ comprised between 0.2 kg and 9.8 kg. In the same manner, it is possible to compute the maximal attitude angle of the body structure allowing the full manipulability. Considering a total mass $m_t = 5.5$ kg, the maximal roll angle of the body structure w.r.t the horizontal attitude allowing to achieve the full manipulability is 26°.

### 3.2.2 Quantitative Criteria

Let introduce the available force set $W_f \subseteq \mathbb{R}^3$ as the intersection of $W_b$ with a zero torque vector, and the available torque set $W_\tau \subseteq \mathbb{R}^3$ as the intersection of $W_b$ with the force vector compensating the gravity. Two criteria are thus introduced which are $r_f$ the radius of the largest sphere centered on $(^b f_{b,x}, ^b f_{b,y}, ^b f_{b,z}) = (0, 0, m_t g)$ inscribed in $W_f$, and $r_\tau$ the radius of the largest sphere centered on the origin $(^b \tau_{b,x}, ^b \tau_{b,y}, ^b \tau_{b,z}) = (0, 0, 0)$ inscribed in $W_\tau$. Indeed, $r_f$ describes the maximal force that the robot can exert in any direction when the robot generates no torque, while $r_\tau$ describes the maximal torque that the robot can exert in any direction when the robot compensates gravity.

Consider a robot with the parameters shown in Table 1 and $m_t = 5.5$ kg, then the results in the grasping and manipulation modes are given in Fig. 7. In the grasping mode, $r_f = 9.5$ N, $r_\tau = 8.1$ N m which are smaller than $r_f = 20$ N, $r_\tau = 12$ N m in the manipulation mode. It shows that the robot has a better manipulability performance in the manipulation mode than the grasping mode, due to the yaw torques that are applied to actuate fingers in the grasping mode.

Since these results are based on the assumption that
quadrotors can change their attitude instantly <fnref n="3" />, an allocation algorithm is proposed in section 4 that allows optimizing the wrenches applied by each quadrotor, while taking into account the continuity of the quadrotor wrenches. Moreover, this method can model the available quadrotor wrench set using linear inequalities, which will be used to find an achievable solution for each quadrotor in the controller design in section 4.

## 4 Dynamic Control Allocation of the Flying Gripper

There are two main objectives in the controller design of the FG robot: i) enable the robot to move from an initial to a final position following a reference trajectory; ii) ensures the closing/opening of fingers while tracking the trajectory. The controller is designed based on the dynamic model presented in section 2. The inputs of the dynamic model $\mathbf{f}_z \in \mathbb{R}^{4 \times 1}$ and $^q\boldsymbol{\tau}_q \in \mathbb{R}^{12 \times 1}$ (see Eq. (8) and Eq. (10)) are regrouped in $^q\mathbf{W}_q^r = [\mathbf{1}_{\mathbf{w}_1^r}^T \mathbf{2}_{\mathbf{w}_2^r}^T \mathbf{3}_{\mathbf{w}_3^r}^T \mathbf{4}_{\mathbf{w}_4^r}^T]^T \in \mathbb{R}^{16 \times 1}$ with the reduced UAV wrench $^i\mathbf{w}_i^r = [f_{i,z} \ ^i\boldsymbol{\tau}_i^T]^T \in \mathbb{R}^{4 \times 1}$.

The dynamic model in Eqs. (7)-(10) shows the decoupling between the body structure's dynamics and the quadrotors' rotational dynamics. Therefore, a strategy is proposed to control the FG robot by a reference trajectory specifying the robot's motion by $\mathbf{p}_b^* \in \mathbb{R}^{6 \times 1}$ and the closing/opening fingers by the quadrotors' yaw angles $\boldsymbol{\psi}^* = [\psi_1^* \ \psi_2^* \ \psi_3^* \ \psi_4^*] \in \mathbb{R}^{4 \times 1}$.

### 4.1 High-Level Motion Controller

As the control scheme depicted in Fig. 8, the high-level motion controller controls the robot's pose to track a reference trajectory $\mathbf{p}_b^*$.

Control methods used in the high-level motion controller have to deal with variations of dynamic parameters caused by holding the object and changing fingers' configurations. Those variations are considered as uncertainties and disturbances in the controller design.

For the high-level motion controller, let first introduce a virtual input $\mathbf{v}_b$ to compute $\mathbf{w}_b$ in Eq. (7)

$$ \mathbf{w}_b = \mathbf{M}_b \mathbf{v}_b + \mathbf{c}_b \quad (26) $$

A standard PID control law can be implemented

$$ \mathbf{v}_b = \ddot{\mathbf{p}}_b^* + \mathbf{K}_{b,p} \mathbf{e}_p + \mathbf{K}_{b,d} \dot{\mathbf{e}}_p + \mathbf{K}_{b,i} \int \mathbf{e}_p \quad (27) $$

where $\mathbf{e}_p$ is the tracking error and $\mathbf{K}_{b,p}, \mathbf{K}_{b,d}, \mathbf{K}_{b,i}$ are matrices corresponding to the proportional, derivative and integral gains. These matrices must be tuned to ensure the convergence of the tracking error $\mathbf{e}_p$.

### 4.2 Control Allocation

The control allocation module, in Fig. 8, is built to distribute the body wrench $\mathbf{w}_b \in \mathbb{R}^{6 \times 1}$ to the quadrotors' thrust forces $^0\mathbf{f}_q \in \mathbb{R}^{12 \times 1}$ (see Eq. (9)). Since $\dim(\mathbf{w}_b) < \dim(^0\mathbf{f}_q)$, there exists an infinity of solutions for $^0\mathbf{f}_q$. Therefore, the DCA algorithm is implemented in the control allocation module. This algorithm is based on a 2-step optimization allowing to minimize the energy consumption and ensure the control input continuity. It is solved by using quadratic programming for computational efficiency reasons and, therefore, the available quadrotor wrench set to obtain a set of linearized constraints and guarantee that the solution is achievable.

1. Ensure producing total control effort $\mathbf{w}_b$ by minimizing $\|\mathbf{W}_f^0\mathbf{f}_q - \mathbf{w}_b\|^2$ in the 1st optimization as

$$ \mathbf{u}_{\Lambda} = \underset{^0\mathbf{f}_q(t)}{\operatorname{argmin}} \|\mathbf{W}_f^0\mathbf{f}_q(t) - \mathbf{w}_b(t)\|^2 \quad (28) $$

$$ \text{s.t.} \quad \mathbf{A}_f^0 \mathbf{f}_q(t) \le \mathbf{b}_f \quad (29) $$

where $\mathbf{A}_f^0\mathbf{f}_q(t) \le \mathbf{b}_f$ are linear inequalities that describe constraints of the robot. Consequently, solving this optimization gives the solution set $\Lambda = \{\mathbf{u}(t) | \mathbf{u}(t) = \mathbf{u}_{\Lambda}(t) + \mathbf{Z}\mathbf{u}_0(t)\}$, where the columns of $\mathbf{Z}$ are bases of kernel of $\mathbf{W}_f$ ($\mathbf{W}_f\mathbf{Z} = \mathbf{0}$) and $\mathbf{u}_0(t)$ is an arbitrary vector.

2. Select $^0\mathbf{f}_q$ inside the set $\Lambda$ for the 2nd optimization to guarantee the control input continuity and to keep the solution close to a desired state ($\mathbf{g}_1$ and $\mathbf{g}_2$ will be introduced later) as

$$ ^0\mathbf{f}_q(t) = \underset{^0\mathbf{f}_q(t) \in \Lambda}{\operatorname{argmin}} \|\mathbf{W}_1^{-\frac{1}{2}} \mathbf{g}_1(t)\|^2 + \|\mathbf{W}_2^{-\frac{1}{2}} \mathbf{g}_2(t)\|^2 \quad (30) $$

$$ \text{s.t.} \quad \mathbf{A}_f^0 \mathbf{f}_q(t) \le \mathbf{b}_f \quad (31) $$

The DCA algorithm deals with constraints in linear inequalities (see Eq. (29)). At the end of step 6 in section 3, the obtained available UAV wrench set $W_i^s$ in Eq. (22) is a polytope that is a cone with a dome on the top (see Fig. 6c). From its $\mathcal{H}$-representation, the linear inequality constraints for $^s\mathbf{f}_i$ that is the $i$th quadrotor's thrust force in $\mathcal{F}_{si}$ is obtained as

$$ \mathbf{A}_{fs}^s \mathbf{f}_i \le \mathbf{b}_{fs} \quad (32) $$

which, then, can be rewritten w.r.t $^0\mathbf{f}_i$ as

$$ \mathbf{A}_{fs}^s \mathbf{R}_0^0 \mathbf{f}_i \le \mathbf{b}_{fs} \quad (33) $$

where $^s\mathbf{R}_0 \in \mathbb{R}^{3 \times 3}$ is the rotation matrix of $\mathcal{F}_0$ w.r.t $\mathcal{F}_{si}$. Till now, the linear inequalities for $^0\mathbf{f}_i$ are obtained.

<footnote n="3">³Because the rotational dynamics of a quadrotor is significantly faster than the dynamics of the body structure, quadrotors are assumed to rotate instantly.</footnote>
<table><thead><tr><th></th><th>W<sub>τ</sub></th><th>W<sub>f</sub></th></tr></thead><tbody><tr><td>grasping mode</td><td>r<sub>τ</sub> = 8.1 Nm</td><td>r<sub>f</sub> = 9.5 N</td></tr><tr><td>manipulation mode</td><td>r<sub>τ</sub> = 12 Nm</td><td>r<sub>f</sub> = 20 N</td></tr></tbody></table>

<figure><img src="image_8.png" /></figure>

<figure><img src="image_9.png" /></figure>

<figure><img src="image_10.png" /></figure>

<figure><img src="image_11.png" /><figcaption>Fig. 7: In grasping and manipulation modes, $W_f$ and $W_τ$ of the Flying Gripper with a mass $m_t = 5.5$ kg. The available force set $W_f$ is calculated as the intersection of $W_b$ with $\mathcal{H}\mathcal{P}(^b\tau_{b,x} = 0, ^b\tau_{b,y} = 0, ^b\tau_{b,z} = 0)$, and the available torque set $W_τ$ is computed as the intersection of $W_b$ with $\mathcal{H}\mathcal{P}(^b f_{b,x} = 0, ^b f_{b,y} = 0, ^b f_{b,z} = m_t g)$. Then, $r_f$ is the radius of the largest sphere centered on $(^b f_{b,x}, ^b f_{b,y}, ^b f_{b,z}) = (0,0,m_t g)$ inscribed in $W_f$ that represents the maximal force that the robot can exert in any direction when the robot generates no torque, while $r_τ$ is the radius of the largest sphere centered on the origin $(^b\tau_{b,x}, ^b\tau_{b,y}, ^b\tau_{b,z}) = (0,0,0)$ inscribed in $W_τ$ that represents the maximal torque that the robot can exert in any direction when the robot compensates gravity.</figcaption></figure>

<figure><img src="image_12.png" /><figcaption>Fig. 8: Control scheme of the Flying Gripper consisting of a high-level motion controller module, a control allocation module and a quadrotor controller module. The high-level motion controller module computes the body wrench $\mathbf{w}_b \in \mathbb{R}^{6 \times 1}$ to track a reference trajectory $\mathbf{p}_b^* \in \mathbb{R}^{6 \times 1}$. The control allocation module distributes $\mathbf{w}_b$ to $^0\mathbf{f}_q \in \mathbb{R}^{12 \times 1}$ that is the vector regrouping all quadrotors' thrust force vectors expressed in $\mathcal{F}_0$. Then, for each quadrotor $i$, the quadrotor controller computes the dynamic system input $^i\mathbf{w}_i^r \in \mathbb{R}^{4 \times 1}$ from the inputs $^0\mathbf{f}_i \in \mathbb{R}^{4 \times 1}$, the reference yaw angle $\psi_i^*$, and the attitude angle $\eta_i \in \mathbb{R}^{3 \times 1}$. Note that closing/opening the fingers is imposed by $\mathbf{\Psi}^* = [\psi_1^* \ \psi_2^* \ \psi_3^* \ \psi_4^*] \in \mathbb{R}^{4 \times 1}$.</figcaption></figure>

Then, physical constraints imposed by the mechanical stops of universal joints and the motor speed limits concerning the decision variable $^0\mathbf{f}_q$ in Eq. (28) are modeled using linear inequalities: $\mathbf{A}_f \, ^0\mathbf{f}_q(t) \le \mathbf{b}_f$ in Eq. (29), where $\mathbf{A}_f = \text{diag}(\mathbf{A}_{f_s}^{s_1}\mathbf{R}_0, \mathbf{A}_{f_s}^{s_2}\mathbf{R}_0, \mathbf{A}_{f_s}^{s_3}\mathbf{R}_0, \mathbf{A}_{f_s}^{s_4}\mathbf{R}_0)$ and $\mathbf{b}_f = [\mathbf{b}_{fs}^T \ \mathbf{b}_{fs}^T \ \mathbf{b}_{fs}^T \ \mathbf{b}_{fs}^T]^T$.

The 2nd optimization aims to penalize the change in $^0\mathbf{f}_q$ by minimizing $\mathbf{g}_1$ in Eq. (30) that is defined as

$$ \mathbf{g}_1(t) = \mathbf{0}\mathbf{f}_q(t) - \mathbf{0}\mathbf{f}_q(t - T_u) \quad (34) $$

where $T_u$ is the control sampling period.
Also, DCA offers a choice to keep the solution $^0\mathbf{f}_q$ close to a desired steady state $^0\mathbf{f}_{qdes}$. This is conducted by minimizing $\mathbf{g}_2$ in Eq. (30) that is defined as

$$ \mathbf{g}_2(t) = \mathbf{0}^{\mathbf{f}_q}(t) - \mathbf{0}^{\mathbf{f}_{qdes}} \quad (35) $$

where

$$ \mathbf{0}^{\mathbf{f}_{qdes}} = [\mathbf{0}^{\mathbf{f}_{des,1}} \ \mathbf{0}^{\mathbf{f}_{des,2}} \ \mathbf{0}^{\mathbf{f}_{des,3}} \ \mathbf{0}^{\mathbf{f}_{des,4}}]^T \quad (36) $$

and $^0\mathbf{f}_{des,i}$ represents the desired state for $^0\mathbf{f}_i$. It permits specifying the desired thrust forces for all quadrotors with $^0\mathbf{f}_{qdes}$. Here, $^0\mathbf{f}_{qdes}$ is chosen based on an energetic effective point of view: all quadrotors are horizontal and generate thrust forces just to compensate the gravity of the whole system (robot plus object), which is modeled by equation

$$ \mathbf{0}^{\mathbf{f}_{des,i}} = \frac{m_t}{4} \mathbf{g} \quad (37) $$

such that

$$ \mathbf{W}_f \mathbf{0}^{\mathbf{f}_{qdes}} = m_t \mathbf{g} \quad (38) $$

## 4.3 Quadrotor Controller

For each quadrotor $i$, $^0\mathbf{f}_i$ depends on its thrust $f_{i,z}$ and its attitude $^0\mathbf{R}_i$ as

$$ \mathbf{0}^{\mathbf{f}_i} = \mathbf{0}^{\mathbf{R}_i} \mathbf{i}^{\mathbf{f}_i} = \begin{bmatrix} \cos \phi_i \sin \theta_i \\ -\sin \phi_i \\ \cos \phi_i \cos \theta_i \end{bmatrix} f_{i,z} \quad (39) $$

It can be found from this relation that $^0\mathbf{f}_i$ depends on the thrust $f_{i,z}$, the roll angle $\phi_i$, and the pitch angle $\theta_i$, but it is independent of the yaw angle $\psi_i$. Considering this property, a control strategy is proposed for quadrotor $i$ (see Fig. 8):

1. controlling the quadrotor's thrust $f_{i,z}$, roll and pitch angles $\phi_i, \theta_i$ to generate the required thrust force $^0\mathbf{f}_i$ that is the output of the control allocation module,

2. controlling the quadrotor's yaw angle $\psi_i$ to track $\psi_i^*$.

Firstly, based on Eq. (39), the thrust $f_{i,z}$ and the desired roll and pitch angles $\phi_i^*, \theta_i^*$ with a given thrust force $^0\mathbf{f}_i$ are computed

$$ f_{i,z} = \| \mathbf{0}^{\mathbf{f}_i} \| \quad (40) $$

$$ \phi_i^* = -\sin^{-1}(\mathbf{0}^{f_{i,x}}) \quad (41) $$

$$ \theta_i^* = \operatorname{atan2}(\mathbf{0}^{f_{i,x}}, \mathbf{0}^{f_{i,z}}) \quad (42) $$

Secondly, with $\phi_i^*, \theta_i^*$, computed by Eq.(41) and Eq.(42) and $\psi_i^*$ from the reference trajectory, the quadrotor con-

troller calculates the torque $^i\boldsymbol{\tau}_i$ (element of $^q\boldsymbol{\tau}_q$ in Eq. (8)) for forcing the quadrotor's attitude $\boldsymbol{\eta}_i$ to track the reference $\boldsymbol{\eta}_i^* = [\phi_i^* \ \theta_i^* \ \psi_i^*]^T$. There are several available algorithms for the quadrotor controller. One approach is to use the algorithms of the flight control software PX4 4 [31] to realize the quadrotor controller of the robot. Indeed, the PX4 offers a “thrust and attitude” mode, which takes the thrust force $f_{i,z}$ and the attitude $\boldsymbol{\eta}_i^*$ as the inputs and then controls the motors' speed to finally produce the needed wrench $^i\mathbf{w}_i^r$.

# 5 Experimentation with the Flying Gripper

## 5.1 Setup

A prototype of the FG robot was built at LS2N (see Fig. 9) as a proof of concept aiming to validate that the FG robot can achieve the full manipulability and that fingers can be closed or open using the yaw motions of quadrotors while keeping the pose of the body structure fixed.

The body structure, the gear boxes and the self-adaptive fingers have been constructed using 3D printing. Each quadrotor has a mass of 1.05 kg and a maximum thrust of 25 N. It is composed of a Lynxmotion Crazy2fly frame, a Raspberry Pi 3B+ as an onboard computer and a Pixhawk flight control unit running the PX4 software. The pose information of quadrotors and the body structure is provided by a Qualisys motion capture system with 1 mm precision.

Preliminary tests have been led on this prototype and are presented below as well as the obtained results.

<figure><img src="image_13.png" /><figcaption>Fig. 9: Prototype of the Flying Gripper robot at LS2N</figcaption></figure>

## 5.2 Case Study 1: the Flying Gripper Robot Hovering under Disturbances

For the first test of the controller, a taking off and hovering reference trajectory is designed to test its robustness under disturbances (see Fig. 10a).

In this experiment, external disturbance forces are manually applied on the robot in the horizontal direction at $t = 8$ s, $t = 13$ s, in the vertical direction at $t = 18$ s (see Fig. 10b,

<footnote n="4">⁴An open source flight control software that can run on the Pixhawk flight controller.</footnote>
<figure><img src="image_14.png" /><figcaption>Fig. 10: Experimentation with the Flying Gripper: the Flying Gripper under external disturbances in (a), its translation and attitude tracking in (b) and quadrotors’ yaw angles tracking in (c); the Flying Gripper approaching, grasping and releasing an object in (d), its translation and attitude tracking in (e) and quadrotors’ yaw angles tracking in (f).</figcaption></figure>

Fig. 10c). Since this task does not involve grasping, so the reference yaw angles of quadrotors are kept fixed to be zeros $\Psi^* = [\psi_1^* \ \psi_2^* \ \psi_3^* \ \psi_4^*]^T = [0 \ 0 \ 0 \ 0]^T$ (see Fig. 10c). A video of this test is available online with the link<fnref n="5" />.

## 5.3 Case Study 2: the Flying Gripper Robot Approaching, Grasping and Releasing a Large Size Object

For the second test, the robot is located differently from an object (see Fig. 10d). To perform grasping, the robot takes off and moves to the above of the object in 0s – 20s (see Fig. 10e, Fig. 10f). In 20s – 27s, the robot stabilizes its position and attitude, while closing fingers to grasp the object in the air. Then, the robot opens the fingers to release the object in 27s – 30s. Note that $\Psi^*$ specifies the closing and opening motion of fingers (see Fig. 10f). A video of this test is available online with the link<fnref n="6" />.

## 5.4 Discussion on Results

In the first test, after being applied the external disturbances, the robot moves away from the desired pose, while the controller enables the robot to recover its desired pose (see Fig. 10b). It is worthy to mention that, if the robot is forced to move horizontally, the quadrotors will need to change their attitudes to compensate influences caused by

such disturbances. Compared to the vertical disturbances that do not need to change the quadrotors’ attitudes, the recovering from the horizontal disturbances is more time consuming. It is also necessary to note that a static error exists in the position tracking, which requires tuning the controller in the future.

In the second test, the robot is controlled to perform a more complex task that involves taking off, approaching, grasping and releasing the object. The robot takes off from its initial position. Following the defined trajectory, the robot approaches the object, and stabilizes its position and attitude above the object (see Fig. 10e) while producing the quadrotors’ yaw rotations to close and open the fingers (see Fig. 10f). Note that, for each quadrotor, the PX4 algorithm is used to control its thrust and attitude in order to fulfill the control signal from control allocation as well as to track a reference yaw angle trajectory for closing/opening the finger. Therefore, the tracking performances depend on the quadrotor’s autonomy and other factors, such as batteries.

## 6 Conclusions

This paper introduced a collaborative multi-drone grasping robot FG. Based on its static model, a wrench capability analysis method has been developed to analyze the manipulability of the multi-drone robot. For each quadrotor, it uses linear inequalities to model the actuators’ capabilities, equal-

<footnote n="5">⁵https://youtu.be/ACi1druEm8k</footnote>

<footnote n="6">⁶https://youtu.be/SYqHC2_Ng9k</footnote>
ities to model the yaw torque applied to actuate the finger and the equilibrium conditions, and consider the mechanical stops of the universal joints introduced between the quadrotor and the body structure. Results show that the FG robot achieves the full manipulability in SE(3). The manipulability performances of the FG in different working modes have been analyzed by studying the maximal force and torque that the robot can produce at static equilibrium. This method permits to build a linearized set of inequalities derived from the available UAV wrench set, in order to enable the DCA algorithm to find a solution that satisfies mechanical constraints of the mechanism, while reducing energy consumption and managing the control input continuity.

A prototype of the FG robot has been constructed. The designed controller is tested on this prototype in real-time experiments. This proof of concept allowed leading preliminary tests which demonstrate that the FG robot achieves the full manipulability and can resist external perturbations. These tests also demonstrate the capability of the FG robot to close or open fingers using the yaw motion of quadrotors while maintaining the body structure stable. The experimental results validate the effectiveness of the controller and its robustness against external disturbances and noise. Future works will investigate the realization of a more complete task involving grasping, manipulation and transporting of an object.

## References

[1] Zhang, H., Sun, J., and Zhao, J., 2019. "Compliant bistable gripper for aerial perching and grasping". In Proceedings of IEEE International Conference on Robotics and Automation (ICRA), pp. 1248-1253.

[2] Tognon, M., Yüksel, B., Buondonno, G., and Franchi, A., 2017. "Dynamic decentralized control for protocentric aerial manipulators". In Proceedings of IEEE International Conference on Robotics and Automation (ICRA), pp. 6375-6380.

[3] Kamel, M., Alexis, K., and Siegwart, R., 2016. "Design and modeling of dexterous aerial manipulator". In Proceedings of IEEE International Conference on Intelligent Robots and Systems (IROS), pp. 4870-4876.

[4] Yu, P., Wang, Z., and Wong, K., 2019. "Exploring aerial perching and grasping with dual symmetric manipulators and compliant end-effectors". *International Journal of Micro Air Vehicles, 11*. https://doi.org/10.1177/1756829319877416.

[5] Sreenath, K., and Kumar, V., 2013. "Dynamics, control and planning for cooperative manipulation of payloads suspended by cables from multiple quadrotor robots". In Proceedings of Robotics: Science and Systems Foundation. Berlin, Germany.

[6] Sanalitro, D., Savino, H. J., Tognon, M., Cortés, J., and Franchi, A., 2020. "Full-pose manipulation control of a cable-suspended load with multiple uavs under uncertainties". *IEEE Robotics and Automation Letters, 5*(2), pp. 2185-2191.

[7] Nguyen, H.-N., Park, S., Park, J., and Lee, D., 2018. "A novel robotic platform for aerial manipulation using quadrotors as rotating thrust generators". *IEEE Transactions on Robotics, 34*(2), pp. 353-369.

[8] Six, D., Briot, S., Chriette, A., and Martinet, P., 2018. "The kinematics, dynamics and control of a flying parallel robot with three quadrotors". *IEEE Robotics and Automation Letters, 3*(1), pp. 559-566.

[9] Gioioso, G., Franchi, A., Salvietti, G., Scheggi, S., and Prattichizzo, D., 2014. "The flying hand: A formation of UAVs for cooperative aerial tele-manipulation". In Proceedings of IEEE International Conference on Robotics and Automation (ICRA), pp. 4335-4341.

[10] Saldana, D., Gabrich, B., Whitzer, M., Prorok, A., Campos, M. F., Yim, M., and Kumar, V., 2017. "A decentralized algorithm for as-

sembling structures with modular robots". In Proceedings of IEEE International Conference on Intelligent Robots and Systems (IROS), pp. 2736-2743.

[11] Zhao, M., Kawasaki, K., Anzai, T., Chen, X., Noda, S., Shi, F., Okada, K., and Inaba, M., 2018. "Transformable multirotor with two-dimensional multilinks: Modeling, control, and whole-body aerial manipulation". *The International Journal of Robotics Research, 37*(9), pp. 1085-1112.

[12] Korpela, C., Orsag, M., and Oh, P., 2014. "Towards valve turning using a dual-arm aerial manipulator". In Proceedings of IEEE International Conference on Intelligent Robots and Systems (IROS), pp. 3411-3416.

[13] Car, M., Ivanovic, A., Orsag, M., and Bogdan, S., 2018. "Impedance based force control for aerial robot peg-in-hole insertion tasks". In Proceedings of IEEE International Conference on Intelligent Robots and Systems (IROS), pp. 6734-6739.

[14] Saint-Sevin, M., Bégoc, V., Briot, S., Chriette, A., and Fantoni, I., 2019. "Design and optimization of a multi-drone robot for grasping and manipulation of large size objects". In ROMANSY 22 - Robot Design, Dynamics and Control, Springer International Publishing, pp. 458-465.

[15] Li, Z., Song, X., Bégoc, V., Chriette, A., and Fantoni, I., 2021. "Dynamic modeling and controller design of a novel aerial grasping robot". In ROMANSY 23 - Robot Design, Dynamics and Control, Springer International Publishing, pp. 538-546.

[16] Birglen, L., Laliberté, T., and Gosselin, C., 2008. *Underactuated Robotic Hands*. Springer Berlin Heidelberg.

[17] Ebert-Uphoff, I., and Voglewede, P. A., 2004. "On the connections between cable-driven robots, parallel manipulators and grasping". In Proceedings of IEEE International Conference on Robotics and Automation (ICRA), Vol. 5, pp. 4521-4526.

[18] Ryll, M., Bicego, D., Giurato, M., Lovera, M., and Franchi, A., 2020. "Fast-hex – a morphing hexarotor: Design, mechanical implementation, control and experimental validation". Accessed 17 May 2020, https://arxiv.org/abs/2004.06612.

[19] Sarkisov, Y. S., Kim, M. J., Bicego, D., Tsetserukou, D., Ott, C., Franchi, A., and Kondak, K., 2019. "Development of sam: cable-suspended aerial manipulator". In 2019 International Conference on Robotics and Automation (ICRA), pp. 5323-5329.

[20] Jamshidifar, H., and Khajepour, A., 2020. "Static workspace optimization of aerial cable towed robots with land-fixed winches". *IEEE Transactions on Robotics, 36*(5), pp. 1603-1610.

[21] Erskine, J., Chriette, A., and Caro, S., 2019. "Wrench Analysis of Cable-Suspended Parallel Robots Actuated by Quadrotor Unmanned Aerial Vehicles". *Journal of Mechanisms and Robotics, 11*(2). 020909.

[22] Li, Z., Erskine, J., Caro, S., and Chriette, A., 2020. "Design and control of a variable aerial cable towed system". *IEEE Robotics and Automation Letters, 5*(2), pp. 636-643.

[23] Härkegård, O., 2004. "Dynamic control allocation using constrained quadratic programming". *Journal of Guidance, Control, and Dynamics, 27*(6), pp. 1028-1034.

[24] Wang, H., Yi, J., and Fan, G., 2009. "Flight control system design with hierarchy-structured dynamic inversion and dynamic control allocation". In 2009 IEEE International Conference on Systems, Man and Cybernetics, IEEE, pp. 5162-5167.

[25] Raoufat, M. E., Tomsovic, K., and Djouadi, S. M., 2017. "Dynamic control allocation for damping of inter-area oscillations". *IEEE Transactions on Power Systems, 32*(6), pp. 4894-4903.

[26] Krut, S., Bégoc, V., Dombre, E., and Pierrot, F., 2010. "Extension of the form-closure property to underactuated hands". *IEEE Transactions on Robotics, 26*(5), pp. 853-866.

[27] Li, Z., 2021. "Theoretical developments and experimental evaluation of a novel collaborative multi-drones grasping and manipulation system of large objects". PhD thesis, École centrale de Nantes, Nantes, France.

[28] Mahony, R., Kumar, V., and Corke, P., 2012. "Multirotor aerial vehicles: Modeling, estimation, and control of quadrotor". *IEEE Robotics and Automation Magazine, 19*(3), pp. 20-32.
[29] Herceg, M., Kvasnica, M., Jones, C., and Morari, M., 2013. "Multi-Parametric Toolbox 3.0". In Proceedings of the European Control Conference (ECC), pp. 502–510.

[30] Fukuda, K., 2020. Polyhedral computation. Accessed 21 January 2021, https://doi.org/10.3929/ethz-b-000426218.

[31] Meier, L., Honegger, D., and Pollefeys, M., 2015. "Px4: A node-based multithreaded open source robotics framework for deeply embedded platforms". In Proceedings of IEEE International Conference on Robotics and Automation (ICRA), pp. 6235–6240.

[32] Grünbaum, B., 2003. Convex polytopes. Springer-Verlag New York.

## Appendix: Notations Linear Mapping of Polytopes

The aforementioned wrench capability analysis method is based on the concept of polytopes. A polytope is a bounded convex set. It can be represented mathematically in two different manners: using the $\mathcal{H}$-representation (as the intersection of a set of halfspaces $\mathcal{HS}$ and hyperplanes $\mathcal{HP}$) or using the $\mathcal{V}$-representation (as the convex hull of a set of vertices) [32]. A polytope can be represented in $\mathcal{H}$-representation and $\mathcal{V}$-representation respectively as follows:

1. $\mathcal{H}$-representation

$$ \mathcal{P} = \{ \mathbf{x} \in \mathbb{R}^{m \times 1} | \mathbf{A}\mathbf{x} \leq \mathbf{b}, \mathbf{C}\mathbf{x} = \mathbf{d} \} \quad (43) $$

where $\mathbf{A} \in \mathbb{R}^{m \times n}$ and $\mathbf{b} \in \mathbb{R}^{m \times 1}$ represent $m$ halfspaces in $\mathbb{R}^n$, while $\mathbf{C} \in \mathbb{R}^{p \times n}$ and $\mathbf{d} \in \mathbb{R}^{p \times 1}$ represent $p$ halfplanes in $\mathbb{R}^n$. Rows of matrix $\mathbf{A}$ (respectively $\mathbf{C}$) contain each normal vector associated to a corresponding halfspace (respectively hyperplane).

2. $\mathcal{V}$-representation

$$ \mathcal{P} = \mathbf{conv}(\mathcal{S}) \quad (44) $$

where $\mathcal{S} = \{\mathbf{x}_1, \dots, \mathbf{x}_k\}$ is the set of $k$ vertices of $\mathcal{P}$ and $\mathcal{P}$ is the convex hull noted $\mathbf{conv}(\mathcal{S})$

$$ \mathbf{conv}(\mathcal{S}) = \left\{ \sum_{i=1}^{k} \alpha_i \mathbf{x}_i \mid \mathbf{x}_i \in \mathcal{S}, \alpha_i \ge 0, \sum_{i=1}^{k} \alpha_i = 1 \right\} \quad (45) $$

Given a certain convex polytope in the $\mathcal{H}$-representation, computing its $\mathcal{V}$-representation is called the vertex enumeration problem. The reverse problem is referred to as the facet enumeration problem [30]. After introducing polytopes, the concern is to define the linear transformation of a polytope. Let introduce the $\mathcal{H}$-representation and $\mathcal{V}$-representation of a polytope $\mathcal{P}$ as defined in Eq. (43) and Eq. (44).

Let introduce a linear mapping $\mathbf{L} \in \mathbb{R}^{m \times n}$ that maps $\mathbf{x}$ to $\mathbf{x}'$

$$ \mathbf{x}' = \mathbf{L}\mathbf{x} \quad (46) $$

if $\mathbf{L}$ is a square and invertible matrix with $m = n$. Substituting $\mathbf{x} = \mathbf{L}^{-1}\mathbf{x}'$ into Eq.(43) gives the $\mathcal{H}$-representation of $\mathcal{P}'$

from that of $\mathcal{P}$

$$ \mathcal{P}' = \{ \mathbf{x}' \in \mathbb{R}^{m \times 1} | \mathbf{A}\mathbf{L}^{-1}\mathbf{x}' \leq \mathbf{b}, \mathbf{C}\mathbf{L}^{-1}\mathbf{x}' = \mathbf{d} \} \quad (47) $$

It is also possible to calculate the $\mathcal{V}$-representation of $\mathcal{P}'$ by using the following relation:

$$ \mathcal{P}' = \mathbf{conv}(\mathcal{S}') = \mathbf{conv}(\mathbf{L}(\mathcal{S})) \quad (48) $$

where $\mathcal{S}' = \{\mathbf{x}'_1, \dots, \mathbf{x}'_k\}$ is the set of mapped vertices with $\mathbf{x}'_i = \mathbf{L}\mathbf{x}_i \quad \forall i \in \{1, \dots, k\}$. The set $\mathcal{S}' = \mathbf{L}(\mathcal{S})$ is the set of transformed vertices.
List of Tables

<table><tr><td>1</td><td>Main parameters used in the wrench capability analysis.</td><td>6</td></tr></table>

List of Figures

<table><tr><td>1</td><td>Computer Aided Design (CAD) model and main parameters of the Flying Gripper (FG) that is composed of a body structure, four self-adaptive fingers and four quadrotors: the frame F<sub>s</sub> is attached to the universal joint whose origin coincides with the CoM of quadrotor i and z<sub>s</sub> axis coincides with the worm screw axis (the attitude of F<sub>s</sub> w.r.t F<sub>b</sub> is represented by the constant matrix <sup>b</sup>R<sub>s</sub>).</td><td>4</td></tr><tr><td>2</td><td>(a) CAD model of a self-adaptive finger actuated by a quadrotor, (b) kinematic scheme of this finger, the quadrotor's yaw motion is transmitted through a worm-gear mechanism to an actuation bar which closes or opens the two phalanxes, a universal joint is introduced between the quadrotor and the worm screw, (c) details of the transmission between the quadrotor and the worm screw, a universal joint allows the pitch and roll motions of the quadrotor, the shaft alignment angle between the quadrotor's yaw axis and the screw axis is represented by φ<sub>i</sub> with the maximal value being φ<sub>max</sub>.</td><td>4</td></tr><tr><td>3</td><td>Representation of the available UAV actuation set A<sub>i</sub> (a 4-cube). For visualization purpose, its intersection with the hyperplane H&P(ω<sup>2</sup><sub>i,4</sub> = ω<sub>min</sub>) is shown in the 3D space (ω<sup>2</sup><sub>i,1</sub>, ω<sup>2</sup><sub>i,2</sub>, ω<sup>2</sup><sub>i,3</sub>).</td><td>7</td></tr><tr><td>4</td><td>Representation of the available UAV reduced wrench set W<sup>r</sup><sub>i</sub> (a 4D polytope). For visualization purpose, its intersection with the hyperplane H&P(τ<sub>i,y</sub> = 0) is shown in the 3D space (f<sub>i,z</sub>, τ<sub>i,x</sub>, τ<sub>i,z</sub>).</td><td>7</td></tr><tr><td>5</td><td>Representation of the available UAV wrench set considering equality yaw torque constraint W<sup>yaw</sup><sub>i</sub>. For visualization purpose, (a) the hyperplane H&P(τ<sub>i,z</sub> = τ<sub>f</sub>) (τ<sub>f</sub> = 0.01 N m), and the intersection of W<sub>i</sub> with H&P(τ<sub>i,y</sub> = 0) are shown in the 3D space (f<sub>i,z</sub>, τ<sub>i,x</sub>, τ<sub>i,z</sub>); (b) the intersection of W<sup>yaw</sup><sub>i</sub> with H&P(τ<sub>i,y</sub> = 0) is shown in the 3D space (f<sub>i,z</sub>, τ<sub>i,x</sub>, τ<sub>i,z</sub>); (c) W<sup>yaw</sup><sub>i</sub> that is a polytope with a dimension of 3 is shown in the 3D space (f<sub>i,z</sub>, τ<sub>i,x</sub>, τ<sub>i,y</sub>).</td><td>8</td></tr><tr><td>6</td><td>Representation of the available UAV wrench set considering static equilibrium and mobility imposed by universal joints W<sup>s</sup><sub>i</sub>. (a) W<sup>yaw</sup><sub>i</sub> and the hyperplane H&P(τ<sub>i,x</sub> = τ<sub>i,y</sub> = 0) representing the static equilibrium condition of UAV are illustrated in the 3D space (f<sub>i,z</sub>, τ<sub>i,x</sub>, τ<sub>i,y</sub>); (b) as the result of the intersection of W<sup>yaw</sup><sub>i</sub> with H&P(τ<sub>i,y</sub> = τ<sub>i,y</sub> = 0), W<sup>eq</sup><sub>i</sub> is the available UAV wrench set considering static equilibrium, which is a 1D polytope in the 3D space (f<sub>i,z</sub>, τ<sub>i,x</sub>, τ<sub>i,y</sub>); (c) W<sup>s</sup><sub>i</sub>, the convex hull of union of multiple available UAV wrench sets in F<sub>s</sub>, is shown in the 3D space (<sup>s</sup>f<sub>i,x</sub>, <sup>s</sup>f<sub>i,y</sub>, <sup>s</sup>f<sub>i,z</sub>) that is a cone with a dome on the top with φ<sub>max</sub> = 30°.</td><td>9</td></tr><tr><td>7</td><td>In grasping and manipulation modes, W<sub>f</sub> and W<sub>τ</sub> of the Flying Gripper with a mass m<sub>t</sub> = 5.5 kg. The available force set W<sub>f</sub> is calculated as the intersection of W<sub>b</sub> with H&P(<sup>b</sup>τ<sub>b,x</sub> = 0, <sup>b</sup>τ<sub>b,y</sub> = 0, <sup>b</sup>τ<sub>b,z</sub> = 0), and the available torque set W<sub>τ</sub> is computed as the intersection of W<sub>b</sub> with H&P(<sup>b</sup>f<sub>b,x</sub> = 0, <sup>b</sup>f<sub>b,y</sub> = 0, <sup>b</sup>f<sub>b,z</sub> = m<sub>t</sub>g). Then, r<sub>f</sub> is the radius of the largest sphere centered on (<sup>b</sup>f<sub>b,x</sub>, <sup>b</sup>f<sub>b,y</sub>, <sup>b</sup>f<sub>b,z</sub>) = (0,0, m<sub>t</sub>g) inscribed in W<sub>f</sub> that represents the maximal force that the robot can exert in any direction when the robot generates no torque, while r<sub>τ</sub> is the radius of the largest sphere centered on the origin (<sup>b</sup>τ<sub>b,x</sub>, <sup>b</sup>τ<sub>b,y</sub>, <sup>b</sup>τ<sub>b,z</sub>) = (0,0,0) inscribed in W<sub>τ</sub> that represents the maximal torque that the robot can exert in any direction when the robot compensates gravity.</td><td>11</td></tr><tr><td>8</td><td>Control scheme of the Flying Gripper consisting of a high-level motion controller module, a control allocation module and a quadrotor controller module. The high-level motion controller module computes the body wrench w<sub>b</sub> ∈ ℝ<sup>6×1</sup> to track a reference trajectory p<sup>*</sup><sub>b</sub> ∈ ℝ<sup>6×1</sup>. The control allocation module distributes w<sub>b</sub> to <sup>0</sup>f<sub>q</sub> ∈ ℝ<sup>12×1</sup> that is the vector regrouping all quadrotors' thrust force vectors expressed in F<sub>0</sub>. Then, for each quadrotor i, the quadrotor controller computes the dynamic system input <sup>i</sup>w<sup>r</sup><sub>i</sub> ∈ ℝ<sup>4×1</sup> from the inputs <sup>0</sup>f<sub>i</sub> ∈ ℝ<sup>4×1</sup>, the reference yaw angle ψ<sup>*</sup><sub>i</sub>, and the attitude angle η<sub>i</sub> ∈ ℝ<sup>3×1</sup>. Note that closing/opening the fingers is imposed by ψ<sup>*</sup> = [ψ<sup>*</sup><sub>1</sub> ψ<sup>*</sup><sub>2</sub> ψ<sup>*</sup><sub>3</sub> ψ<sup>*</sup><sub>4</sub>] ∈ ℝ<sup>4×1</sup></td><td>11</td></tr><tr><td>9</td><td>Prototype of the Flying Gripper robot at LS2N.</td><td>12</td></tr><tr><td>10</td><td>Experimentation with the Flying Gripper: the Flying Gripper under external disturbances in (a), its translation and attitude tracking in (b) and quadrotors' yaw angles tracking in (c); the Flying Gripper approaching, grasping and releasing an object in (d), its translation and attitude tracking in (e) and quadrotors' yaw angles tracking in (f).</td><td>13</td></tr></table>