ommpc_controller - GitLite

KO | EN

gitlite — search

#hacktoberfest #llm #go #machine-learning #rust #deep-learning #java #windows #cli #awesome-list #pytorch #security #openai #open-source

ommpc_controller

(★ 121)

C++ implementation of On-Manifold MPC (TIE '23, SR '25)

파일 탐색기

최종 버전 다운로드 (.zip)

- real_flight.png
- simu_with_ego.gif
- state_machine.txt
- - state.cfg
- - params.yaml
- - ommpc_controller.hpp
  - polynomial_trajectory.h
  - so3_math.h
- - px4_example.launch
- - ommpc_example.cpp
- - circle.py
  - circle.txt
  - var_circle.py
- CMakeLists.txt
- package.xml
- - DataDisp.msg
  - MINCOTraj.msg
  - PolyTraj.msg
- - planning_visualization.cpp
- CMakeLists.txt
- package.xml
.gitignore
OMMPC_derivation.pdf
README.md

# 설치 가이드

1. 코드 내려받기

git clone https://github.com/OliverWu515/ommpc_controller

깃허브에서 프로젝트 코드 전체를 내 컴퓨터로 내려받습니다.

cd ommpc_controller

방금 내려받은 프로젝트 폴더 안으로 이동합니다.

2. 공식 설치 스크립트

쉬움 추천

사전 준비물

APT (Debian/Ubuntu 계열) Debian/Ubuntu 계열 리눅스에 기본 내장된 패키지 매니저입니다.

sudo apt install ros-noetic-ddynamic-reconfigure

APT 패키지 저장소에서 바로 설치합니다 (Debian/Ubuntu 계열).

✅ 설치 후 새 터미널을 열고, 프로그램의 버전 확인 명령(예: --version)으로 정상 설치됐는지 확인하세요.

이 레포의 README에 적힌 실제 명령어를 그대로 가져왔습니다.

3. CMake

보통

사전 준비물

Git GitHub에서 프로젝트 코드를 내려받으려면 필요합니다.
CMake 빌드 설정을 생성하는 도구입니다.
C/C++ 컴파일러 Windows는 Visual Studio(Community 무료), macOS는 Xcode Command Line Tools, Linux는 gcc/g++ 패키지가 필요합니다.

The version of cmake on Ubuntu 20.04 is a bit old and cannot compile the latest version. Therefore, it is necessary to revert to a slightly older version.

이 명령어를 터미널에 그대로 입력해 실행하세요.

✅ build 폴더 안에 실행 파일이 생성됐는지 확인하고, 직접 실행해보세요 (예: ./build/앱이름).

이 레포의 README에 적힌 실제 명령어를 그대로 가져왔습니다.

// repository documentation

Was this content helpful?

★ 0 (0 ratings)

> **Notice (2026-05-22):** I just released a ROS-decoupled refactoring branch (`refact-decouple`). Users and contributors are welcome to try it, report issues, and help improve the new modular controller architecture.

# OMMPC-Controller

- [OMMPC-Controller](#ommpc-controller)
    - [Usage](#usage)
        - [Prerequisites](#prerequisites)
            - [Ubuntu and ROS](#ubuntu-and-ros)
            - [MAVROS](#mavros)
            - [Dynamic reconfigure](#dynamic-reconfigure)
            - [PX4-Gazebo Simulation](#px4-gazebo-simulation)
            - [OSQP](#osqp)
        - [Cloning the code and compiling](#cloning-the-code-and-compiling)
        - [Run the code](#run-the-code)
    - [Theory and parameters](#theory-and-parameters)
        - [Reading text trajectories](#reading-text-trajectories)
        - [Theory and parameters](#theory-and-parameters)
        - [Thrust Normalization](#thrust-normalization)
    - [Acknowledgment](#acknowledgment)

_The TOC is automatically generated by [Auto Markdown TOC](https://marketplace.visualstudio.com/items?itemName=huntertran.auto-markdown-toc)._

If you feel my work has been helpful, please give it a star—thank you!

Also, the control performance is not yet optimal, so if anyone is interested in helping with debugging and parameter tuning, I would be greatly appreciated!

**TODO: add parameters of rotor drag**

Simulation with Gazebo and EGO-Planner-V2:

![Simulation with Gazebo and EGO-Planner-V2](https://raw.githubusercontent.com/OliverWu515/ommpc_controller/master/misc/simu_with_ego.gif)

Error analysis of realworld experiment with a Fast-Drone-250 quadrotor:

![Error analysis of realworld experiment with a Fast-Drone-250 quadrotor](https://raw.githubusercontent.com/OliverWu515/ommpc_controller/master/misc/real_flight.png)

## Usage

### Prerequisites

#### Ubuntu and ROS

The package has been tested on Ubuntu 20.04 and ROS noetic. The **default** Eigen is enough for this package to work normally.

#### MAVROS

Follow the guide in https://docs.px4.io/main/en/ros/mavros_installation#install-mavros.

#### Dynamic reconfigure

```bash
sudo apt install ros-noetic-ddynamic-reconfigure
```

#### PX4-Gazebo Simulation

First, clone the source code of PX4-Autopilot:
```bash
git clone https://github.com/PX4/PX4-Autopilot.git --recursive
```
If the clone process completes without any errors, proceed directly to the next step. Otherwise, you can consider the following command:

```bash
cd PX4-Autopilot/
git submodule update --init --recursive
```

Then run the following script to setup:
```bash
bash Tools/setup/ubuntu.sh
```

Reboot, and
```bash
cd PX4-Autopilot/
make px4_sitl_default gazebo
```

You may need to wait a few seconds for gazebo to initialize. After initialization, type `commander takeoff` to make the drone take off, or type `commander land` to make it land.

To increase the frequency of odometry, you can modify the following code in `~/PX4-Autopilot/src/modules/mavlink/mavlink_main.cpp`:
```C++
configure_stream_local("LOCAL_POSITION_NED", 125.0f);  // originally 30.0f
configure_stream_local("ODOMETRY", 125.0f);  // originally 30.0f
```
and `make px4_sitl_default gazebo` again.

Add environment variables to the `.bashrc` file:
```bash
# >>> PX4 initialize >>>
source ~/PX4-Autopilot/Tools/simulation/gazebo-classic/setup_gazebo.bash ~/PX4-Autopilot ~/PX4-Autopilot/build/px4_sitl_default
export ROS_PACKAGE_PATH=$ROS_PACKAGE_PATH:~/PX4-Autopilot
export ROS_PACKAGE_PATH=$ROS_PACKAGE_PATH:~/PX4-Autopilot/Tools/simulation/gazebo-classic/sitl_gazebo-classic
# <<< PX4 initialize <<<
```

#### OSQP

The version of cmake on Ubuntu 20.04 is a bit old and cannot compile the latest version. Therefore, it is necessary to revert to a slightly older version.

```bash
git clone --recursive -b release-0.6.3 https://github.com/osqp/osqp.git
```

If the clone process completes without any errors, proceed directly to the next step. Otherwise, you can consider the following command:

```bash
cd osqp
git submodule update --init --recursive
```

Then, create a build directory:

```bash
cd osqp
mkdir build
cd build
```

Next, create the Makefiles and compile:
```bash
cmake -G "Unix Makefiles" ..
cmake --build .
```

Finally,
```bash
cmake --build . --target install
```
(May require administrator privileges)

Ensure that `libosqp.so` is present, then update the dynamic link library cache using the `ldconfig` command.

### Cloning the code and compiling

```bash
sudo apt install ros-noetic-ddynamic-reconfigure
cd catkin_ws/src
git clone https://github.com/OliverWu515/ommpc_controller.git
cd ..
catkin_make
```

### Run the code

```bash
source devel/setup.bash
roslaunch px4 mavros_posix_sitl.launch
roslaunch ommpc_controller px4_example.launch
```

Takeoff: First, type the following command in the terminal:
```
rosservice call /mavros/set_mode 0 "OFFBOARD"
```

Then, check the `takeoff_enabled` box in the rqt_reconfigure window. The drone will automatically arm and take off.

Command: Check the `command_or_hover` box in the rqt_reconfigure window. The drone will then execute the text trajectory or polynomial trajectory.

Landing: Check the `land_enabled` box in the rqt_reconfigure window. The drone will automatically land and attempt to disarm. If disarming is successful, you can type the following command in the terminal:
```
rosservice call /mavros/set_mode 0 "MANUAL"
```
to return to manual control mode, or you can proceed directly to execute other commands.

The finite state machine (FSM) that governs the controller's behavior is documented in [misc/state_machine.txt](misc/state_machine.txt). It describes all state transitions (HOVER, TAKEOFF, LAND, POLY_TRAJ, POINTS), global rules for MPC failure recovery, trigger auto-reset, and safe output handling.

**Important:**

The pose of Odometry is defined as forward x, left y, upward z (ENU). The nose of the aircraft points in the positive x-axis direction, and the throttle thrust direction is along the positive z-axis. They must be strictly aligned. If the coordinate frame is NED instead of ENU, the `enu_frame_` parameter in the `OMMPC_EXAMPLE` class must be set to `false`!

The definition of velocity in Odometry differs from the [ROS official documentation](http://docs.ros.org/en/noetic/api/nav_msgs/html/msg/Odometry.html). The ROS official definition expresses velocity in the body frame, but most open-source odometry systems define velocity relative to the world frame. This controller also defaults to defining velocity in the world frame. If the velocity you are using is defined in the body frame, set `vel_in_body_` to true in the `init` method of the `OMMPC_EXAMPLE` class (**for example, when using Gazebo for simulation**). Otherwise, set it to false (**for example, when using open-source odometry for realworld experiments**).

## Theory and parameters

### Reading text trajectories

If reading text trajectories for testing is enabled, after entering command control mode, the file stored at the path `ref_txt/ref_filename` will be read line by line with a fixed step size `ref_txt/time_step`. The format of each line is:
```
x-position y-position z-position x-velocity y-velocity z-velocity yaw yaw-rate
```
In the `traj` folder, several scripts are stored for generating text trajectories as needed.  
Note: The text trajectory must not contain empty lines at the end, otherwise a memory error may occur and lead to a crash!

**Related parameters**:  
ref_txt:   
&emsp; enable: Allows the use of text trajectories. If set to `false`, commands from the planner are received instead.  
&emsp; time_step: The sampling step size for the text trajectory. Special attention: The step size for MPC and the text reference trajectory must be consistent. This requires ensuring that **this parameter, `MPC_params/step_T` (the MPC step size), and the step in the script generating the text trajectory** are the same!  
&emsp; ref_filename: The path to the text trajectory file, specified as a **relative path** relative to this project.

### Theory and parameters

For the derivation with respect to the dynamic of quadrotors, refer to the file `OMMPC_derivation.pdf` in this repo.

For more general conclusions, refer to [G. Lu, W. Xu and F. Zhang, "On-Manifold Model Predictive Control for Trajectory Tracking on Robotic Systems," in IEEE Transactions on Industrial Electronics, vol. 70, no. 9, pp. 9192-9202, Sept. 2023](https://ieeexplore.ieee.org/abstract/document/9917382).

There are three ways to set the reference trajectory and control inputs:
- Set a hover reference. Related function: `setHoverReference`. This essentially sets the reference pose for the next several steps to the current pose and the velocity to 0.
- Set the reference from a text trajectory. Related functions: `setTextReference`, `get_txt_des`. This function will also be used while setting references during takeoff and landing.
- Set the reference from a polynomial trajectory. Related functions: `setTrajectoryReference`, `feed_from_traj_utils`. If it is necessary to read polynomial trajectories in other formats, you can implement them yourself.

We adopt Hopf fibration to calculate the full state from the flat outputs.

**Related parameters**:  
use_fix_yaw: Whether to fix the yaw angle   
use_trajectory_ending_pos: Whether to ensure reaching the trajectory endpoint, generally set to `true`  
MPC_params:  
&emsp; step_T: MPC step size. When using text trajectories, note that the text trajectory step size must be consistent with this.  
&emsp; Q_pos_xy, Q_pos_z: Weight for horizontal and vertical error, respectively. Should be set 1-3 orders of magnitude larger than other weights.  
&emsp; Q_attitude_rp, Q_attitude_yaw: Attitude error weight.  
&emsp; Q_velocity: Velocity error weight.  
&emsp; R_thrust: Thrust input penalty.  
&emsp; R_pitchroll, R_yaw: Angular velocity input penalty. These penalties should be relatively smaller, otherwise they affect control accuracy; but cannot be too small, otherwise the smoothness will be affected.  
&emsp; state_cost_exponential, input_cost_exponential: State/input error weight discount factor. The weight at step k is multiplied by exp{-k/total_steps * discount_factor}.  
&emsp; max_bodyrate_xy, max_bodyrate_z: Maximum angular rate. It is recommended to set the z-component relatively smaller.  
&emsp; min_thrust/max_thrust: Minimum/Maximum thrust (actually acceleration, unit: m/s^2)

### Thrust Normalization

Assume that

$$
t_{cmd}=\frac{a_{z,d}}{T_a}
$$

where $T_a$ is the normalization constant, which is determined by the physical characteristics of the quadrotor, and can be estimated by Kalman filtering

$$
\begin{aligned}
x_k&=T_{a,k} \\
z_k&=a_z=t_{cmd}\ T_{a,k}
\end{aligned}
$$

Then (with $Q = (1/\rho-1) \hat{P}_{k-1}$ and $R=1$, $\rho$ is the forgetting factor)

$$
\begin{aligned}
\breve{P}_k&=(1/\rho) \hat{P}_{k-1}\\
K_k&=\frac{\breve{P}_k\cdot t_{cmd}}{t_{cmd} \breve{P}_k\cdot t_{cmd}+1} \\
\hat{T}_{a,k}&=\breve{T}_{a,k}+K_k(a_{z,imu}-t_{cmd} \breve{T}_{a,k}) \\
\hat{P}_k&=(1-K_k\cdot t_{cmd})\cdot \breve{P}_k \\
\end{aligned}
$$

**Related parameters**  
hover_percentage: Used to set the initial value of $T_a$

## Acknowledgment

- [Huizhe Li](https://github.com/haiyu1020) and [Yuhao Fang](https://github.com/fweiI/) for instruction of parameter tuning and basic structures of code
- [Autotrans](https://github.com/HKUST-Aerial-Robotics/AutoTrans/tree/main/) and [RPG MPC](https://github.com/uzh-rpg/rpg_mpc) for providing examples of setting reference for MPC
- [SUPER](https://github.com/hku-mars/Super) for providing supporting material including derivation of OMMPC
- [GCOPTER](https://github.com/ZJU-FAST-Lab/GCOPTER) and [SE(3) Controller](https://github.com/HITSZ-MAS/se3_controller) for providing hints of differential flatness with Hopf fibration
- [Fast-Drone-250](https://github.com/ZJU-FAST-Lab/Fast-Drone-250/) for providing examples of using Finite State Machine (FSM) to manage states of quadrotors