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):

File truncated at 100 lines see the full file