MPC Planner

This package implements Model Predictive Control (MPC) for motion planning in 2D dynamic environments using ROS/ROS2 C++. A complete VSCode docker environment with this planner is available at https://github.com/tud-amr/mpc_planner_ws. This code is associated with the following publications:

Journal Paper: O. de Groot, L. Ferranti, D. M. Gavrila, and J. Alonso-Mora, Topology-Driven Parallel Trajectory Optimization in Dynamic Environments. IEEE Transactions on Robotics (T-RO), 2024. Available: https://doi.org/10.1109/TRO.2024.3475047

Conference Paper: O. de Groot, L. Ferranti, D. M. Gavrila, and J. Alonso-Mora, Globally Guided Trajectory Optimization in Dynamic Environments. IEEE International Conference on Robotics and Automation (ICRA), 2023. Available: https://doi.org/10.1109/ICRA48891.2023.10160379

This repository includes our implementation of Topology-Driven MPC (T-MPC++) that computes multiple distinct trajectories in parallel, each passing dynamic obstacles differently. For a brief overview of the method, see the Paper website.

Simulated Mobile Robot	Real-World Mobile Robot	Static and Dynamic Obstacles

---

Table of Contents

1. Features
2. Installation
3. Usage
4. Configuration
5. Examples
6. License
7. Citing
8. Contributing

Features

This is a planner implementation for mobile robots navigating in 2D dynamic environments. It is designed to be:

• Modular - Cost and constraint components are modular to allow stacking of functionality.
• Robot Agnostic - The robot functionality in ROS wraps the base C++ planner. This allows customization of the control loop, input and output topics and visualization.
• ROS/ROS2 Compatible: - ROS functionality is wrapped in ros_tools to support both ROS and ROS2.
• Computationally Efficient: - Typical real-time control frequencies with dynamic and static obstacle avoidance are ~20-30 Hz

To solve the MPC, we support the licensed Forces Pro and open-source Acados solvers. The solvers can be switched with a single setting when both are installed. The solver generation is implemented in Python, generating C++ code for the online planner.

Implemented MPC modules include:

• Reference Path Tracking Cost - Tracking a 2D path
  • Model Predictive Contouring Control (MPCC)
  • Curvature-Aware Model Predictive Control (CA-MPC)
• Goal Tracking Cost - Tracking a user defined goal position
• State/Input Penalization - To limit control inputs and track references
• Dynamic Obstacle Avoidance Constraints - Avoiding humans
  • Ellipsoidal constraints (https://ieeexplore.ieee.org/document/8768044)
  • Linearized constraints
• Chance Constrained Obstacle Avoidance Constraints - Incorporating uncertainty in the future motion of humans
  • Avoiding obstacle predictions modeled as Gaussians (CC-MPC)
  • Avoiding obstacle predictions modeled as Gaussian Mixtures (Safe Horizon MPC publication pending)
• Static Obstacle Avoidance Constraints - Avoiding non-moving obstacles in the environment
  • Using decomp_util (see original paper and modified implementation)

These functionalities can be stacked to implement the desired behavior (see Configuration).

The Topology-Driven MPC [1] module parallelizes the above functionality over multiple distinct initial guesses, computing several trajectories that pass the obstacles differently.

Installation

We recommend to use the complete VSCode containerized environment provided here https://github.com/tud-amr/mpc_planner_ws, if you can, to automatically install the planner and its requirements.

Note: While we support Forces Pro and Acados, we used Forces Pro for the results in our paper.

Note: To use Forces Pro in the containerized environment, a floating license is required. If you have a regular license, please following the steps below to install the planner manually.

The following steps denote the manual installation of the planner.

---

Step 1: Clone repos

In your ROS/ROS2 workspace ws/src, clone the following:

Planner repos:

git clone https://github.com/oscardegroot/mpc_planner.git
git clone https://github.com/oscardegroot/ros_tools.git

Guidance planner (for T-MPC) and decomp_util (for static obstacle avoidance):

git clone https://github.com/oscardegroot/guidance_planner.git
git clone https://github.com/oscardegroot/DecompUtil.git

Pedestrian simulator:

git clone https://github.com/oscardegroot/pedestrian_simulator.git
git clone https://github.com/oscardegroot/pedsim_original.git
git clone https://github.com/oscardegroot/asr_rapidxml.git

Other simulator packages:

git clone https://github.com/oscardegroot/roadmap.git
git clone https://github.com/oscardegroot/jackal_simulator.git

Step 2: Set your ROS version

From ws/src and with your ROS version either 1 or 2:

cd mpc_planner
python3 switch_to_ros.py 1
cd ..

cd ros_tools
python3 switch_to_ros.py 1
cd ..

cd guidance_planner
python3 switch_to_ros.py 1
cd ..

cd pedestrian_simulator
python3 switch_to_ros.py 1
cd ..

Step 3: Install dependencies

From ws:

rosdep install -y -r --from-paths src --ignore-src --rosdistro noetic

Step 4: Install your solver

The solver generation uses Python3. Requirements for the python environment are in requirements.txt and pyproject.toml. For Forces Pro, Python 3.8 is required. You can set-up the environment yourself if you are familiar with virtual environments. Otherwise, explicit instructions are provided below.

We recomment to either use poetry or conda.

Conda

If you have miniconda installed you can use:

From ws/src/mpc_planner:

conda create -n mpc_planner --file requirements.txt
conda activate mpc_planner

You may have to add the conda-forge channel if you do not have it yet: conda config --append channels conda-forge.

Note: In the remainder of this readme, leave out poetry run ... if you installed with conda.

Instead of conda, another option is to use pyenv and poetry.

Pyenv

To install pyenv (see https://github.com/pyenv/pyenv?tab=readme-ov-file#installation), first install system dependencies

sudo apt update; sudo apt install build-essential libssl-dev zlib1g-dev \
libbz2-dev libreadline-dev libsqlite3-dev curl \
libncursesw5-dev xz-utils tk-dev libxml2-dev libxmlsec1-dev libffi-dev liblzma-dev

Then get pyenv

curl https://pyenv.run | bash

And modify your shell, e.g., ~/.bashrc:

export PYENV_ROOT="$HOME/.pyenv"
[[ -d $PYENV_ROOT/bin ]] && export PATH="$PYENV_ROOT/bin:$PATH"
eval "$(pyenv init -)"

Poetry

Install Python 3.8.10 and activate it:

pyenv install 3.8.10
pyenv local 3.8.10
# pyenv global 3.8.10 # required in some cases

To setup the Poetry environment run:

pip3 install poetry
poetry install --no-root

---

With the environment set up, install Acados and/or Forces Pro.

Solver: Acados

In any folder, clone and build Acados:

git clone https://github.com/acados/acados.git
cd acados
git submodule update --recursive --init
mkdir -p build
cd build
cmake -DACADOS_SILENT=ON ..
make install -j4

Then link Acados in your Poetry environment

poetry add -e <path_to_acados>/interfaces/acados_template # Poetry

or

pip install -e <path_to_acados>/interfaces/acados_template # Conda

And add the acados path in your ~/.bashrc or similar:

export LD_LIBRARY_PATH="$LD_LIBRARY_PATH:<path_to_acados>/lib"
export ACADOS_SOURCE_DIR="<path_to_acados>"

Solver: Forces Pro

Go to my.embotech.com, log in to your account. Assign a regular license to your computer. Download the client to ~/forces_pro_client/ outside of the container. If you have the solver in a different location, add its path to PYTHONPATH.

---

Finally, to generate a solver, first define which solver to use in mpc_planner_<your_system>/config/settings.yaml by setting solver_settings/solver to acados or forces. Then generate a solver (e.g., for jackalsimulator):

poetry run python mpc_planner_jackalsimulator/scripts/generate_jackalsimulator_solver.py

You should see output indicating that the solver is being generated.

Step 5: Build the planner

Source the ROS workspace source devel/setup.sh. Then build with:

catkin config --cmake-args -DCMAKE_BUILD_TYPE=Release # Release, Debug, RelWithDebInfo, MinSizeRel
catkin build mpc_planner_<your_system>

Usage

Each system type has its own package (mpc_planner_<system>) that usually includes

• a main file (src/mpc_planner_<system>),
• configuration (config/settings.yaml),
• launch file (e.g., launch/ros1_<system>.launch), and
• solver definition script (scripts/generate_<system>_solver.py).

To launch the planner for your system run its launch file, e.g.,

roslaunch mpc_planner_jackalsimulator ros1_jackalsimulator.launch

Note: For some systems, detailed instructions are available in the README.md inside their packages.

Configuration

The MPC problem is configured in the solver definition script (e.g., mpc_planner_<system>/scripts/generate_<system>_solver.py). The following defines a T-MPC that follows a reference path while avoiding dynamic obstacles.

settings = load_settings() # Load config/settings.yaml

modules = ModuleManager()
model = ContouringSecondOrderUnicycleModel() # Define the dynamic model

base_module = modules.add_module(MPCBaseModule(settings))
base_module.weigh_variable(var_name="a", weight_names="acceleration") # Quadratic cost on input acceleration
base_module.weigh_variable(var_name="w", weight_names="angular_velocity") # Quadratic cost on angular velocity
base_module.weigh_variable(var_name="v",
                          weight_names=["velocity", "reference_velocity"], 
                          cost_function=lambda x, w: w[0] * (x-w[1])**2) # Track a reference velocity

modules.add_module(ContouringModule(settings)) # MPCC cost

modules.add_module(GuidanceConstraintModule(settings, constraint_submodule=EllipsoidConstraintModule)) # T-MPC with ellipsoidal constraints

generate_solver(modules, model, settings) # Generate the solver

Settings of the online solver can be modified in config/settings.yaml. Important settings are:

• N - Steps in the planner horizon
• integrator_step - Time between planner steps
• n_discs - Number of discs that model the robot area
• solver_settings/solver - Solver to use
• debug_output - Print debug information when enabled
• control_frequency - Planner control frequency
• max_obstacles - Maximum number of dynamic obstacles to avoid in the MPC
• robot/ - The robot area (com_to_back is the distance from the center of mass to the back of the robot)
• t-mpc/use_t-mpc++ - Enable T-MPC++, adding the non guided planner in parallel
• weights/ - Default weights of the MPC. Can be modified online in rqt_reconfigure.

Examples

Custom System

Please see the mpc_planner_jackalsimulator package for an example of how to customize this planner. Explicit comments are provided throughout this package. See:

• C++ Main File - Defining the input/output topics and control loop
• Python Solver Generation File - Defining the MPC problem
• Parameters - Configuring the planner
• Launch File - Launching the planner on the right topics

Launching this package simulates the Jackal robot in an environment with pedestrians.

Custom Modules

To implement your own module, you need to define how it is handled in the solver in Python and what it computes online in C++.

For a Python cost and constraint example, see goal_module.py and ellipsoid_constraints.py.

For a C++ cost and constraint example see goal_module.cpp and ellipsoid_constraints.py.

As an example, by replacing the contouring cost with the goal module, the robot navigates to the user clicked goal instead of following a reference path.

License

This project is licensed under the Apache 2.0 license - see the LICENSE file for details.

Citing

This repository was developed at the Cognitive Robotics group of Delft University of Technology by Oscar de Groot in partial collaboration with Dennis Benders and Thijs Niesten and under supervision of Dr. Laura Ferranti, Dr. Javier Alonso-Mora and Prof. Dariu Gavrila.

If you found this repository useful, please cite our paper!

• [1] Journal Paper: O. de Groot, L. Ferranti, D. M. Gavrila, and J. Alonso-Mora, Topology-Driven Parallel Trajectory Optimization in Dynamic Environments. IEEE Transactions on Robotics (T-RO) 2024. Available: https://doi.org/10.1109/TRO.2024.3475047

Contributing

We welcome issues and contributions! If you encounter any bugs, have suggestions for new features, or want to propose improvements, feel free to open an issue or pull request.