NMPC for Human Aerial Handover

h1. NMPC for Human Aerial Handover WIKI

{{toc}}

h2. Foreword

The present @WIKI@ may miss some details which can be added progressively.

In case of questions, queries, comments, missing details, or bug reports, feel free to use the @ISSUES@ system or contact the corresponding authors (contact details in the project's "@OVERVIEW@ panel":https://redmine.laas.fr/projects/nmpc-handover).

h2. Prerequisites

The framework has been written and tested using @Ubuntu 18.04@ since it is the OS used by the LAAS-CNRS robotic platform. It should work seamlessly on a recent Linux version, but there is no guarantee.

Some issues have been found while installing the software on @Ubuntu 16.04@ because of version incompatibility with @Protoc@ and @Protobuf@.

The installation on a non-Linux OS has to be handled by the user.

The installation assumes the use of a package manager (e.g. @apt@) to install some dependencies, as well as the @Gazebo@ simulator. Everything provided in this repository or by the LAAS-CNRS robotic platform aims to be installed locally in the repository folder to avoid polluting the user's system.

Finally, to run the simulations and interface with the middleware, it is assumed to have @MATLAB@ installed.

The provided code has been tested with @MATLAB >= r2019b@; other releases could be compatible but they have not been tested. If you run another version and notice any issue or that the code is compatible, please do not hesitate to contact the authors to point that out, so that this wiki can be updated.

h2. I. Software Overview

h3. I.A. Openrobots

Collections of all the open-source software used at LAAS. You can find more details in "Openrobots Wiki-Homepage":https://www.openrobots.org/wiki.

h3. I.B. Robotpkg

"@Robotpkg@":http://robotpkg.openrobots.org/ is a packaging system for installing robotics software developed by the robotics community.

We will use @robotpkg@ to install the required modules for the simulations (state estimation, @Gazebo@ interface...) as well as third-party dependencies (@qpOases@).

h3. I.C. GenoM

@GenoM@ is a generator of modules, designed to be middleware independent, i.e. the same module can be compiled for, e.g., ROS, YARP, or Pocolibs, without any modification.

This allows a great code re-usability and abstracts the user from any specific choice of middleware.

Originally @GenoM@ has been developed tightly with @Pocolibs@, then from version 3, aka @GenoM3@, @ROS@ templates have been provided.

Another specificity of GenoM is the interaction with and between components.

Each component is started independently like a Linux executable (within a @roscore@, for @ROS@, or an @h2@ instance, for @Pocolibs@), then the connection between ports (or topics) is made using a supervisor, "@Genomix@":https://git.openrobots.org/projects/genomix, either with "@MATLAB@":https://git.openrobots.org/projects/matlab-genomix or "@TCL@":https://git.openrobots.org/projects/tcl-genomix.

h3. I.D. Pocolibs

@Pocolibs@ is a middleware, like @ROS@.

It aims at being lighter and faster than ROS, when running on a single machine, thanks to the exploitation of shared memory. @ROS@, on the other hand, uses a network layer for sending messages between nodes, which leads to greater delays and loss of performance.

h3. I.E. TeleKyb

The "@TeleKyb@":https://git.openrobots.org/projects/telekyb3 software platform provides the aerial-robotic oriented software developed at LAAS-CNRS.

In particular, we will use:

* @mrsim@, a Multi-Robot SIMulator. It is designed to be a transparent interface w.r.t. the real aerial vehicles used in LAAS-CNRS. It makes the transition between simulation and experiments seamless, from the software point of view.

* @pom@, a UKF-based state estimator merging state-feedback measurements of different sources (e.g. Motion Capture + IMU).

* @optitrack@, to export the motion capture data to the @genom@ software stack.

* @rotorcraft@, the low-level interface, with either the simulated or real platform.

* @nhfc@, near-hovering flight controller, used for unmodeled take-off and landing.

* @maneuver@, a trajectory planner, providing position and attitude (as quaternions) as well as first- and second-order derivatives. It implements waypoint-to-waypoint trajectory generation.

h3. I.F. Gazebo

To simulate the platform, we use the @Gazebo@ simulator. To interface it with the @genom@ software stack, we use two dedicated components:

* @mrsim-gazebo@ a plugin to interface the simulated multi-rotor with the @genom@ components (in place of @mrsim@).

* @optitrack-gazebo@ emulates the optitrack network interface to publish the model poses.

The installation procedure for @Gazebo@ can be found in the official documentation "Install Gazebo using Ubuntu packages -- ver. 9":http://www.gazebosim.org/tutorials?cat=install&tut=install_ubuntu&ver=9.0.

h2. II. Installation Procedure

This section is a tutorial on how to install the software architecture to run the simulations.

h3. II.A. Clone the "NMPC for Human Aerial Handover" repository

Clone the repo associated with this project. Its root will act as the @devel@ folder for the following.

<pre><code class="shell">

git clone git://redmine.laas.fr/laas/nmpc-handover.git

cd ./nmpc-handover

</code></pre>

To simplify the installation, we provide some environment variables in the @env.sh@ file.

In order to run all the installed executables, we need to set up the path to the newly created folders.

We provide an @env.sh@ script that exports all the required variables.

*/!\ :* the @source@ command has to be called within this repository's root since it uses the @pwd@ command to export the paths.

<pre><code class="shell">

source env.sh

</code></pre>

h3. II.B. Setup robotpkg

> These steps are taken from the official documentation "Install":http://robotpkg.openrobots.org/install.html.

# Clone the robotpkg latest release.

<pre><code class="shell">

git clone git://git.openrobots.org/robots/robotpkg

</code></pre>

# Check that the @openrobots/@ folder exists in the repository root, and update the environement variables accordingly, if you haven't already sourced the @env.sh@ file.

<pre><code class="shell">

export ROBOTPKG_BASE=`pwd`/openrobots

</code></pre>

# Install @robotpkg@.

<pre><code class="shell">

cd robotpkg/bootstrap

./bootstrap --prefix=$ROBOTPKG_BASE

</code></pre>

# Install the required components and their dependencies.

The installation can be done 'manually' by navigating to the desired folder in @./robotpkg/@ and install with @make update@. +Anyway, we will simplify the process using a set+.

To do so, we need to edit the config file: @$ROBOTPKG_BASE/etc/robotpkg.conf@. Add the following lines at the end of the file:

<pre><code class="shell">

PKG_OPTIONS.%-genom3 = \

        codels \

        pocolibs-server \

        pocolibs-client-c

PKGSET.mpcset = \

    architecture/genom3 \

    architecture/genom3-pocolibs \

    localization/pom-genom3 \

    localization/optitrack-genom3 \

    hardware/joystick-genom3 \

    motion/nhfc-genom3 \

    net/genomix \

    optimization/qpoases \

    path/maneuver-genom3 \

    simulation/mrsim-gazebo \

    simulation/optitrack-gazebo \

    supervision/matlab-genomix \

    supervision/tcl-genomix \

    robots/rotorcraft-genom3

PREFER.lapack = robotpkg

PREFIX.matlab = <path/to/MATLAB>

</code></pre>

The last line (@<path/to/MATLAB>@) needs to point to the @MATLAB@ root folder in the system (e.g. @/opt/Matlab@).

It is recommended to use @MATLAB@ for the proposed simulations since the syntax is more intuitive and comprehensible for an end-user who will modify them.

If @MATLAB@ is not installed on the system, please install it, since the provided interface to the @GenoM@ components is provided only in @MATLAB@.

Also, all the above is meant for using Pocolibs, not @ROS@. Future versions of this tutorial might come to use the @ROS@ install.

Now return to the @robotpkg@ folder and install the custom set by typing:

<pre><code class="shell">

cd robotpkg

make update-mpcset

</code></pre>

During the installation, some required dependencies may need to be installed with the usual package manager (e.g. @apt@ on Ubuntu). When the install stops, install the required missing packages and re-run the command @make update-mpcset@.

# @MATLAB@ configuration.

The last step is to update the @MATLAB@ path to use the custom libraries, if relevant.

Add the following paths in the @Matlab@ path window (or in the file @startup.m):

<pre><code class="shell">

</path/to/openrobots>/lib/matlab

</path/to/openrobots>/lib/matlab/simulink

</path/to/openrobots>/lib/matlab/simulink/genomix

</code></pre>

where @</path/to/openrobots>@ shall be changed with the value of @$ROBOTPKG_BASE@.

> You can find more details in @MATLAB@'s official documentation about "startup":https://fr.mathworks.com/help/matlab/ref/startup.html#d123e1348441.

h3. II.C. Install custom components

h4. List of the extra components

The @src/@ folder contains some additional components, in particular:

* @vision-idl@: the type declarations regarding the camera modules.

* @camgazebo-genom3@: read the data from the gazebo innate cameras, via the gazebo API.

* @camviz-genom3@: record and/or display the images from a camera.

* @arucotag-genom3@: detect and the ArUco markers.

* @uavmpc-genom3@: the NMPC controller presented in the paper manuscript.

* @humancontrol-genom3@: both a Gazebo plugin and a @Genom@ component that work together, in order to move the human model in the @Gazebo@ world

h4. Install the extra components

As the modules listed above are not considered 'stable' as the one provided in robotpkg, we rather install them in a devel folder.

Go to the project root, check that the @devel@ folder exists, export the path if you haven't already sourced the @env.sh@.

> */!\ :* sourcing @env.sh@ is mandatory now, since in the following steps the aliases in that file will be used. They will simplify the installation process of these extra components.

Then go to the sources folder:

<pre><code class="shell">

export DEVEL_BASE=`pwd`/devel

cd src/

</code></pre>

For the manual installation, @asciidoctor@ is needed. It can be installed using @apt@ or any package manager.

Each component here has to be installed manually, using @autoconf@. To do so, proceed as follow:

<pre><code class="shell">

cd src/<component>/

./bootstrap.sh

mkdir build

cd build

configure_genom

make install

</code></pre>

*/!\ :* The component @vision-idl@ must be installed first since it defines some type headers used by others.

*The installation of the main component, @uavmpc-genom3@, is described in the next subsection.*

>

h4. Install the MPC controller

Before installing the MPC controller, we have to generate the @C@ sources corresponding to the desired model.

To do so, go to the @model_generation/@ folder:

<pre><code class="shell">

cd src/uavmpc-genom3/model_generation

</code></pre>

>

There is a @README.md@ file there, explaining the requirements.

In short, the model sources are exported to @C@ using @CasADi@ in @python3@.

@python3@ along with @NumPy@, @SciPy@ and @CasADi@  are required, and easily installable on most Linux distributions (e.g. with @apt@ and @pip3@).

Then, the sources are generated using:

<pre><code class="shell">

python3 gen_model.py quad_human_ergonomy

</code></pre>

Then install the component as explained before, but add the following flags to the @configure@ command:

<pre><code class="shell">

configure_uavmpc

</code></pre>

h3. II-3. Setup the environment

In order to run all the installed executables, we need to setup the path to the newly created folders.

All the required variables are exported in the @env.sh@ file.

h2. III. Running the simulation

The @ws/@ folder contains all the material to run a basic simulation with the NMPC.

Firstly, navigate to the subdirectory @sh@. In a terminal, launch the @launch_1.sh@ script. It starts all the @Genom@ components, in the background. It is used as a console since it displays warnings or errors during runtime.

Then, after the message @>> Please run also launch_2.sh in another terminal <<@ is displayed, open a second terminal, and run the @launch_2.sh@ script. This runs the last Genom component. It has been decided to keep it apart from the others since it will print out many data during the execution of the simulations. You can ignore those warning messages. This prevents polluting the first terminal window, preventing from monitoring the main components.

In another terminal, start @Gazebo@ with the world file provided in @nmpc-handover/gazebo/worlds@.

> Use the command @gazebo --verbose handover.world@.

Finally, run @MATLAB@ and move to the next subsection.

>

h3. III.A. Running the simulations with @MATLAB@

To run the simulation, you need to run the @main.m@ script. It runs the simulation scenario, it takes care of connecting all the @Genom@ modules, loading all the necessary parameters, and setting up the simulator.

Here is a description of the other files for convenience.

* The @enable_ergonomics.m@ script enables the ergonomics in the NMPC controller. It is run by @main.m@ during the execution of the simulation.

* The @ho_usropt.m@ script provides the flags related to the handover simulation.

* The @move_human.m@ script allows to move the human in a programmatically way. It is run by @main.m@ during the execution of the simulation.

* The @param_quad.m@ script provides the parameters for the colinear quadrotor (denoted @quad@). 

* The @path_to_handover.m@ script allows to generate and feed to the controller the path to reach the handover position. It is run by @main.m@ during the execution of the simulation. If the @usropt.replay_path_handover@ flag is set to @1@, then the path to handover will be replayed by loading the @.mat@ file in @etc@. Instead, if set to @0@, the path-to-handover trajectory will be generated 'on-the-fly'.

*/!\ :* The @path_to_handover.m@ requires the @MATLAB@'s "@Curve Fitting Toolbox@":https://fr.mathworks.com/products/curvefitting.html to generate the reference motion task 'on-the-fly'.

Here, instead, is a description of the subdirectories:

* The @etc@ folder contains the @traj.m@ file which contains a pre-recorded path-to-handover trajectory.

* The @utils@ folder contains some utility functions used by the @MATLAB@ code to run properly. It is not advisable to change them.

Therefore, run the @main.m@ script and wait until it displays the message @Arm the robot?@.

Then press enter between each step to proceed to the next one. The evolution can be watched in gazebo, and in the console terminal as well.

>

h3. III.B. Comments on how to use the simulator

* The MPC weights and parameters can be tuned in the @param_quad.m@ file. Those gains will be used to track the reference motion task. When, the robot is in front of the human, the ergonomics task is enabled, and the motion task disabled. Moreover, the gains in the controller will be changed accordingly. To tune this set of gains, open the @enable_ergonomics.m@ script.

* The marker can be moved, either using a joystick, or manually, using the interface of the tagcontrol component.