DiMOpt: A Distributed Multi-robot Trajectory Optimization algorithm

Introduction

Method Overview

This software implements a Distributed Multi-robot trajectory Optimization Algorithm (DiMOpt) that deals with the problem of computing trajectories for multiple robots navigating in a shared space. This method exploits:

1. Consensus optimization strategies to tackle collision constrains that couple robot’s decision variables,
2. Single-robot sequential convex programming (SCP) method for efficiently handling non-convexities introduced by dynamics.

Simple Example

Trajectories computed at each iteration of the DiMOpt algorithm for a fleet of five robots.

Installation

This software dependencies can be seen bellow.

Dependencies

Eigen3 OpenCV Threads OpenMPI

sudo apt update
sudo apt install python3
sudo apt install libeigen3-dev
sudo apt install libopencv-dev python3-opencv
sudo apt install libboost-thread-dev
sudo apt-get install -y libopenmpi-dev

CasADi

Casadi installation requires several steps that can be followed in link. Build casadi with IPOPT and HSL library ma27.

Matplot++

Choose your preferred installation method in link

Build (CMake)

mkdir build
cd build/
cmake ..
make

Toy Example

This is a simple example with three robots following a Dubins model, where start and goal configuration can be set manually (i.e. x, y, theta). For that, one can tweak start and goal vectors as illustrate in the code snippet below, that can be found in /tests/3robots.cpp. Note that this code will be executed three times, so once per process/robot.

#include <mropt/mropt.h>
#include <mropt/Problem/BuilderDistributedRobot_Dubins.h>

int main(int argc, char **argv) {

    // (...) OpenMPI initialized

    // 1 - Mission
    int N = 40; // Transcription Discrete Time Steps
    double T = 7; // Trajectory's  Duraction

    // 1.1) Starting Configuration (x, y, theta) for 3 robots
    std::vector<std::vector<double>> start =
            {
                    {1, 1, -0.5 * M_PI},
                    {3, 4, 0.5 * M_PI},
                    {1, 4, 0}
            };
    // 1.2) Goal Configuration (x,y, theta) for 3 robot
    std::vector<std::vector<double>> goal =
            {
                    {2, 2.5, 0},
                    {3, 2,   0},
                    {4, 1,   0}
            };
    // 1.3) Length and Width
    std::vector<double> L = {1, 0.6, 1.3};
    std::vector<double> W = {1, 0.6, 1.3};

    //(...)

    // 2 - Build a Robot
    mropt::Problem::BuilderDistributedRobot_Dubins builder_dubins_car;
    auto params = mropt::Problem::Robot::Params{0.0, T, N};
    auto sa = new ShapeArgs_type(); sa->L = L[r];
    builder_dubins_car.make_robot(r, sa, params);
    auto robot_d = builder_dubins_car.getDistributedRobot();
    // 3 - Assign missions to the robots
    robot_d->addMission(start[r], goal[r], {});

    // 4 -Solve
    // 4.1 - Setup Decoupled Plotter and Solver

    // (...) plotter

    mropt::Problem::DecoupledProblem mrprob_d{r};
    mrprob_d.setParams(R, N);
    mrprob_d.addRobot(robot_d);

    // (...)

    // 4.2 - Solve
    try {
        mrprob_d.solve();
        // Plot Trajectories
        mrprob_d.plot_trajectories(std::vector<std::shared_ptr<mropt::Dynamics::ode>>(R, robot_d->get_ode()));
    } catch (...) {
        std::exit(1);
    }
}

After build the project you can find an executable in /bin/3robots. This can be executed with the following command

cd bin/
mpirun -np 3 3robots

If you are not able to use 3, then run the following command instead:

cd bin/
mpirun -np 3 --use-hwthread-cpus --oversubscribe 3robots

Demo

There is a set of specially challenging problems with different number of robots that you can execute under the examples folder. For example:

./square_sided_12.sh

In the directory missions/, a json file can be found “square_sided_12” where mission is defined. For that you require to define each robot radius, discrete time steps, trajectory’s duration, start and goal configuration, convex polygon halfplanes. Note: if possible for a better performance remove flags: –use-hwthread-cpus –oversubscribe

License

DiMOpt: a Distributed Multi-robot Trajectory Optimization Algorithm

Copyright (C) 2022 Joao Salvado

This program is free software: you can redistribute it and/or modify it under the terms of the GNU General Public License as published by the Free Software Foundation, either version 3 of the License, or (at your option) any later version.

This program is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details.

You should have received a copy of the GNU General Public License along with this program. If not, see http://www.gnu.org/licenses/.