A ROS 2 Nonlinear Model Predictive Controller (NMPC) for quadrotors using the Acados solver. Formulates the tracking problem with an error-based cost in Euler angle representation and uses atan2-based yaw wrapping for correct angular error computation.
This package was created during my PhD originally as a basis of comparison with the well-established NMPC technique in order to make useful comparisons against novel control strategies (namely, Newton-Raphson Flow) developed at Georgia Tech's FACTSLab. We have compared this against the Newton-Raphson controller available in NRFlow_PX4_PKG.
This controller solves a finite-horizon optimal control problem at every timestep:
- Model — 9D state
[x, y, z, vx, vy, vz, roll, pitch, yaw]with 4D input[thrust, p, q, r] - Error-based cost — the stage cost penalizes
[position_err, velocity_err, euler_err, input_err](13D); the terminal cost drops the input term (9D) - Wrapped yaw error — uses
atan2(sin(yaw - yaw_ref), cos(yaw - yaw_ref))to avoid discontinuities at +/-pi - Acados solver — generates and compiles C code for the QP sub-problems, enabling real-time MPC
- Platform-aware Acados cache — solver artifacts are reused when the platform/mass and NMPC formulation stamp still match, and regenerated automatically when they do not
- Error-state cost formulation — references are passed as stage-wise parameters, not embedded in the cost
- Nonlinear least squares — cost type is
NONLINEAR_LSwith configurable weight matrices - Input constraints — hard bounds on thrust
[0, 27] Nand 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
Stage cost (13D):
| Component | Weight | Dimension |
|---|---|---|
| Position error | 2e3 |
3 |
| Velocity error | 2e1 |
3 |
| Roll/pitch error | 2e1 |
2 |
| Yaw error | 2e2 |
1 |
| Thrust | 1e1 |
1 |
| Body rates (p, q) | 1e3 |
2 |
| Yaw rate (r) | 1e2 |
1 |
Terminal cost (9D): same as stage cost without input terms (position 1e3, velocity 1e1, roll/pitch 1e1, yaw 1e2).
source install/setup.bash
# Fly a figure-8 in simulation
ros2 run nmpc_acados_px4 run_node --platform sim --trajectory fig8_horz
# Hardware flight with logging
ros2 run nmpc_acados_px4 run_node --platform hw --trajectory helix --log
# fig8_contraction with feedforward, logged with _ff marker in filename
ros2 run nmpc_acados_px4 run_node --platform sim --trajectory fig8_contraction --ff --log
# -> logs to: sim_nmpc_acados_px4_fig8_contraction_ff_1x.csv| Flag | Description |
|---|---|
--platform {sim,hw} |
Target platform (required) |
--trajectory {hover,yaw_only,circle_horz,...} |
Trajectory type (required) |
--hover-mode {1..8} |
Hover sub-mode (1-4 for hardware) |
--log |
Enable CSV data logging |
--log-file NAME |
Custom log filename |
--double-speed |
2x trajectory speed |
--short |
Short variant (fig8_vert) |
--spin |
Enable yaw rotation |
--flight-period SEC |
Custom flight duration |
--ff |
Mark log filename with _ff (only valid with fig8_contraction) |
When the fig8_contraction trajectory is selected, the node computes a differential-flatness feedforward over the full NMPC horizon at each control step, using the same approach as the contraction controller.
How it works:
generate_feedforward_trajectorycallsflat_to_x_uat each of theNhorizon time steps viajax.vmap.flat_to_x_udifferentiates the flat output[px, py, pz, psi](t)twice to recover:- Velocity
[vx, vy, vz] - Specific thrust
f = sqrt(ax² + ay² + (az - g)²)and Euler angles[phi, th, psi] - A third differentiation yields
u_ff = [df, dphi, dth, dpsi]
- Velocity
- The NMPC reference is updated per stage:
- Euler reference
[phi, th, psi]comes fromx_ffinstead of[0, 0, yaw]— the NMPC now tracks the physically correct roll/pitch attitude the trajectory demands. - Control reference
u_ref=[df, dphi, dth, dpsi]is passed as the stage parameter instead of hover control — the cost penalizes deviation from the feedforward rates rather than from rest.
- Euler reference
This gives the NMPC a fully consistent reference trajectory in both state space and control space, rather than treating every stage as if hover is the desired input.
- quad_trajectories — trajectory definitions
- quad_platforms — platform abstraction <<<<<<< Updated upstream <<<<<<< Updated upstream
- ros2_logger — experiment logging =======
- ROS2Logger — experiment logging
Stashed changes =======
- ros2_logger — experiment logging
Stashed changes
nmpc_acados_px4/
├── nmpc_acados_px4/
│ ├── run_node.py # CLI entry point and argument parsing
│ └── ros2px4_node.py # ROS 2 node (subscriptions, publishers, control loop)
└── nmpc_acados_px4_utils/
├── controller/
│ └── nmpc/
│ ├── generate_nmpc.py # NMPC problem formulation and C-code generation
│ └── acados_model.py # Quadrotor Euler dynamics model for Acados
├── px4_utils/ # PX4 interface and flight phase management
├── transformations/ # Yaw adjustment utilities
├── main_utils.py # Helper functions
└── jax_utils.py # JAX configuration
# Inside a ROS 2 workspace src/ directory
git clone git@github.com:evannsmc/nmpc_acados_px4.git
cd .. && colcon build --symlink-installAcados must be installed separately before building this package. Follow the steps below or the official instructions.
Clone and build from source:
git clone https://github.com/acados/acados.git
cd acados
git submodule update --recursive --initmkdir -p build
cd build
cmake -DACADOS_WITH_QPOASES=ON ..
# add more optional arguments e.g. -DACADOS_WITH_DAQP=ON
make install -j4Install the template as per the Python interface instructions:
pip install -e <acados_root>/interfaces/acados_templateAdd these environment variables to your shell init (e.g., ~/.bashrc), then source ~/.bashrc:
acados_root="your_acados_root" # e.g. "/home/user/acados"
export LD_LIBRARY_PATH=$LD_LIBRARY_PATH:"$acados_root/lib"
export ACADOS_SOURCE_DIR="$acados_root"Required for rendering C code templates:
- Download the correct binaries from the t_renderer releases
- Place the binary in
<acados_root>/bin - Rename it to
t_renderer(e.g.t_renderer-v0.2.0-linux-arm64->t_renderer) - Make it executable:
chmod +x <acados_root>/bin/t_rendererpython3 <acados_root>/examples/acados_python/getting_started/minimal_example_ocp.pyIf it runs and plots with no errors, you're done!
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
- 2025_NewtonRaphson_QuadrotorComplete
- Blimp_SimHardware_NR_MPC_FBL_BodyOfWork2024 <<<<<<< Updated upstream <<<<<<< Updated upstream =======
Stashed changes
This project is part of the evannsmc open-source portfolio.
- Project page =======
Stashed changes
MIT
