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.

Update: This repository now also contains our code for Safe Horizon MPC (SH-MPC) associated with the following publication:

Journal Paper: O. de Groot, L. Ferranti, D. M. Gavrila, and J. Alonso-Mora, Scenario-Based Trajectory Optimization with Bounded Probability of Collision. International Journal of Robotics Research (IJRR), 2024. Preprint: https://arxiv.org/pdf/2307.01070

SH-MPC can handle non Gaussian uncertainty in the motion predictions of obstacles. For more details on how to use it in mpc_planner, see https://github.com/oscardegroot/scenario_module.

Simulated Mobile Robot	Real-World Mobile Robot	Static and Dynamic Obstacles
		<img src=”https://imgur.com/861MmhI.gif” width=100%>

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)
• 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.

For more implementation details on SH-MPC [2], see https://github.com/oscardegroot/scenario_module.

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/tud-amr/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/tud-amr/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 ..

# For ROS1 only! The main branch is on ROS2.
cd asr_rapidxml
git checkout ros1
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.

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