NMPC for PX4-ROS2 Deployment (C++)

C++ port of nmpc_acados_px4 — a ROS 2 Nonlinear Model Predictive Controller (NMPC) for quadrotors using the Acados solver. It reuses the same Euler-state formulation, error-based cost, and atan2-based wrapped yaw error as the Python package, implemented in C++ directly against the Acados C API for minimal solve overhead.

This package was created during my PhD at Georgia Tech's FACTSLab as a high-performance C++ counterpart to the Python NMPC baseline used for comparisons against Newton-Raphson Flow controllers. It links against the C solver emitted by the Python package's Acados code generation, so the Python package (nmpc_acados_px4) must be present in the same workspace — but the solver is generated automatically at build time, with no manual pre-step.

---

 
 Build Order 
    
 MPC Formulation 
    
 Architecture 
    
 Usage 
    
 Acados Setup 
    
 Papers 
  

---

📖 Table of Contents

• Key Features
• MPC Formulation
• Build Order (Do This First)
  • Step 1 — Generate or verify the Acados C solver
  • Step 2 — Build the C++ package
  • Step 3 — Run
• Rebuilding After MPC Formulation Changes
• Usage
  • CLI Options
• Architecture
  • Two-timer control loop
  • Flight phases
• Package Structure
• Dependencies
• Installation
• Acados Installation
• Papers and Repositories
• Website
• License

Key Features

• Acados C API — links directly against the generated C solver for minimal overhead
• Dual-timer control loop — decouples MPC solve latency from the publish rate via a 50-step control buffer
• Error-state cost formulation — references passed as stage-wise parameters, not embedded in the cost
• Differential-flatness feedforward — --ff flag enables full feedforward state+control for fig8_contraction via autodiff::real3rd
• Input constraints — hard bounds on thrust [0, 27] N and body rates [-0.8, 0.8] rad/s
• PX4 integration — publishes attitude setpoints and offboard commands via px4_msgs
• Structured logging — optional CSV logging via ros2_logger_cpp

MPC Formulation

The controller solves a finite-horizon optimal control problem at every timestep using an error-based cost in Euler angle representation, with atan2-based yaw wrapping for correct angular error computation across the +/-pi boundary.

Parameter	Value
State	9D [x, y, z, vx, vy, vz, roll, pitch, yaw]
Control	4D [thrust (N), p, q, r (rad/s)]
Horizon	2.0 s, N=50 steps, dt=0.04 s
Solver	SQP_RTI, PARTIAL_CONDENSING_HPIPM, ERK
Cost type	NONLINEAR_LS, error-based
Yaw error	atan2(sin(yaw−yaw_ref), cos(yaw−yaw_ref))
Thrust bounds	[0, 27] N
Rate bounds	[−0.8, 0.8] rad/s

The Acados solver capsule exposes a 13D stage parameter [p_ref(3), v_ref(3), euler_ref(3), u_ref(4)], so the reference trajectory (and optional feedforward control) is passed in per stage rather than baked into the cost.

Build Order (Do This First)

No manual step required. This package generates its own Acados C solver at build time: its CMake invokes the Python NMPC package's stamp-cached code generation (ensure_solver.py) automatically, regenerating only when the platform/mass/formulation changes. To bake in the hardware mass instead of sim, build with -DNMPC_SOLVER_PLATFORM=hw (or pre-generate with the commands below).

Step 1 — Generate or verify the Acados C solver

The C++ package links against a shared library produced by the Python package's code generation. This runs automatically during colcon build — no manual step is needed. To pre-generate it (or target a specific platform) you can still run:

python3 src/nmpc_acados_px4/ensure_solver.py --platform sim

From the workspace root, the equivalent convenience target is:

make generate_nmpc_solver PLATFORM=sim

The guard compares a stamp file against:

• selected platform / mass
• horizon and step count
• a source hash of the NMPC formulation files

and regenerates only when one of those changed. The generated files live at:

src/nmpc_acados_px4/nmpc_acados_px4_utils/controller/nmpc/acados_generated_files/
└── holybro_euler_err_mpc_c_generated_code/
    ├── acados_solver_holybro_euler_err.h          ← C++ includes this
    ├── libacados_ocp_solver_holybro_euler_err.so  ← C++ links against this
    └── ...

Step 2 — Build the C++ package

colcon build --packages-select nmpc_acados_px4_cpp
source install/setup.bash

If the solver is missing at configure time, CMake generates it automatically (you'll see [acados] Regenerating solver ... in the build log). It stops only if generation itself fails — e.g. the Python package or acados_template isn't importable (use the PX4-ROS2-Docker image).

Step 3 — Run

ros2 run nmpc_acados_px4_cpp run_node --platform sim --trajectory helix --spin

Rebuilding After MPC Formulation Changes

If you modify acados_model.py or generate_nmpc.py in the Python package (e.g., change weights, horizon, constraints, or the platform mass), rerun the guard:

python3 src/nmpc_acados_px4/ensure_solver.py --platform sim

# Rebuild the C++ package
colcon build --packages-select nmpc_acados_px4_cpp
source install/setup.bash

Usage

# Simulation
ros2 run nmpc_acados_px4_cpp run_node --platform sim --trajectory circle_horz
ros2 run nmpc_acados_px4_cpp run_node --platform sim --trajectory helix --spin --double-speed
ros2 run nmpc_acados_px4_cpp run_node --platform sim --trajectory fig8_vert --short

# fig8_contraction (no feedforward)
ros2 run nmpc_acados_px4_cpp run_node --platform sim --trajectory fig8_contraction

# fig8_contraction with differential-flatness feedforward
ros2 run nmpc_acados_px4_cpp run_node --platform sim --trajectory fig8_contraction --ff

# Hardware with logging
ros2 run nmpc_acados_px4_cpp run_node --platform hw --trajectory circle_horz --log
ros2 run nmpc_acados_px4_cpp run_node --platform hw --trajectory helix --spin --log --log-file my_run
ros2 run nmpc_acados_px4_cpp run_node --platform hw --trajectory fig8_contraction --ff --log

CLI Options

Flag	Description
--platform {sim,hw}	Target platform (required)
--trajectory TRAJ	Trajectory type (required)
--hover-mode N	Hover sub-mode (required when --trajectory=hover)
--double-speed	2× trajectory speed
--short	Short variant (fig8_vert only)
--spin	Enable yaw rotation (circle_horz, helix)
--ff	Differential-flatness feedforward (only valid with fig8_contraction)
--flight-period SEC	Override default duration (sim: 30 s, hw: 60 s)
--log	Enable CSV data logging
--log-file NAME	Custom log filename stem (requires --log)

Trajectories: hover, yaw_only, circle_horz, circle_vert, fig8_horz, fig8_vert, helix, sawtooth, triangle, fig8_contraction

Architecture

Two-timer control loop

Timer	Rate	Role
compute_control_timer	100 Hz	Runs MPC solve, writes to 50-step control buffer
publish_control_timer	100 Hz	Reads from control buffer and publishes one step
offboard_mode_timer	10 Hz	Manages arm/offboard/heartbeat

The control buffer decouples solver latency from the publish rate. If the MPC solve takes >10 ms, the buffer plays back the previous solution until the next solve completes.

Flight phases

t=0         t=10s              t=10+flight_period    t=10+flight_period+10s
|-- HOVER --|------ CUSTOM ----|------ RETURN --------|-- LAND --|
   position     body-rate ctrl     position setpoint    descend

Package Structure

nmpc_acados_px4_cpp/
├── CMakeLists.txt
├── package.xml
├── include/nmpc_acados_px4_cpp/
│   ├── nmpc_solver.hpp              # Acados solver wrapper class
│   ├── offboard_control_node.hpp    # ROS 2 node class
│   ├── px4_utils/
│   │   ├── core_funcs.hpp           # PX4 interface helpers
│   │   └── flight_phases.hpp        # Flight phase state machine
│   └── transformations/
│       └── adjust_yaw.hpp           # Yaw wrapping utilities
└── src/
    ├── nmpc_solver.cpp              # Acados C API solver implementation
    ├── offboard_control_node.cpp    # ROS 2 node (subscriptions, publishers, control loop)
    └── run_node.cpp                 # CLI entry point and argument parsing

Dependencies

• quad_platforms_cpp — platform mass and throttle mapping
• quad_trajectories_cpp — trajectory definitions and autodiff velocities
• ros2_logger_cpp — CSV logging
• px4_msgs — PX4 message types
• nmpc_acados_px4 — Python package for C solver code generation
• Eigen3
• Acados — installed at ~/acados/

Installation

# Inside a ROS 2 workspace src/ directory
git clone git@github.com:evannsmc/nmpc_acados_px4_cpp.git
# Run the Python package first to generate the Acados C solver (see Build Order above)
cd .. && colcon build --packages-select nmpc_acados_px4_cpp

Acados Installation

If Acados is not yet installed, follow the Python package README for the full setup steps, or the quick summary below.

git clone https://github.com/acados/acados.git ~/acados
cd ~/acados && git submodule update --recursive --init
mkdir build && cd build
cmake -DACADOS_WITH_QPOASES=ON .. && make install -j$(nproc)

pip install -e ~/acados/interfaces/acados_template

# Add to ~/.bashrc
export LD_LIBRARY_PATH=$LD_LIBRARY_PATH:$HOME/acados/lib
export ACADOS_SOURCE_DIR=$HOME/acados

Download the t_renderer binary from tera_renderer releases, place it at ~/acados/bin/t_renderer, and chmod +x it.

Papers and Repositories

American Control Conference 2024 — paper | Personal repo | FACTSLab repo

Transactions on Control Systems Technology 2025 — paper | Personal repo | FACTSLab repo

Transactions on Robotics 2025 | Personal repo | FACTSLab repo

Website

This project is part of the evannsmc open-source portfolio.

License

MIT