article │nmpc-acados-cpp.md

Modeling in Python, Run in C/C++: The NMPC for Quadrotor using Acados

Sokheng Din21 Oct 2026~10 min#c++#mpc#nmpc#python#quadrotor#robotics

Motivation

Nonlinear Model Predictive Control (NMPC) is one of the most powerful tools in robotics: at every control step it solves an optimization problem over a short future horizon, using a full nonlinear model of the robot, and respects hard limits on states and inputs. The catch is cost. Solving an optimal control problem tens or hundreds of times per second is demanding, and the hardware we really care about (flight controllers, single-board computers, microcontrollers) is small.

The classical answer is to hand-write the dynamics, Jacobians and solver in C. That is fast, but it is slow to develop and painful to change: every new robot, every new state, every modified cost means new derivatives and new low-level code.

This project asks a simple question:

Can we describe the robot in a high-level language (Python + CasADi), and still get solver code that is fast enough for an embedded device?

I have been there before, while my small brain trying to solve the equation needed and the constraint requires for apply, the modeling inside C+++ is already a burnout, so I have this new idea. The idea is to separate what the robot is from how the problem is solved:

Python (CasADi) is used only to describe the system: states, inputs, differential equations, cost and constraints. This is short, readable and easy to modify.
Acados takes that symbolic description and generates plain C code: the dynamics, their exact derivatives (via automatic differentiation), the integrator and a structure-exploiting QP solver (HPIPM on top of BLASFEO).
C++ loads the generated solver as a shared library at runtime and runs the control loop. There is no Python on the control path.

So Python is a modelling language here, not a runtime. The question becomes whether the generated C is fast enough, and whether the workflow stays generic enough that a new robot is "write a Python file + a JSON config", with no change to the C++ controller.

In this post I walk through the whole pipeline using the most interesting model in the repository: a 12-state quadrotor.

Link here to the repo: SokhengDin/NMPC-ACADOS-CPP

Architecture at a glance

code

 ┌──────────────────────────┐      ┌───────────────────────────┐      ┌──────────────────────────┐
 │  python/quadrotor_model  │      │  acados code generation   │      │  C++ mpc_controller      │
 │  (CasADi symbolic model) │ ───▶ │  python/generate_*.py     │ ───▶ │  dlopen(libacados_ocp_   │
 │  states, inputs, f(x,u)  │      │  → C code + .so / .dylib  │      │  solver_quadrotor.so)    │
 └──────────────────────────┘      └───────────────────────────┘      │  + configs/*.json        │
                                                                      └──────────────────────────┘
      high-level, readable               automatic derivatives,           no Python at runtime,
                                         embedded-friendly C              no recompilation per model

The workflow has four steps:

Define the robot model in Python (python/quadrotor_model.py).
Formulate the optimal control problem and generate C code (python/generate_quadrotor.py).
Write a JSON config with initial state, target, bounds and weights (configs/quadrotor.json).
Run the generic C++ controller: ./mpc_controller ../configs/quadrotor.json.

Step 1: The quadrotor model

State and input

The quadrotor has 12 states: position, linear velocity, Euler angles and body angular rates which is a lot.

x = \begin{bmatrix} x & y & z & v_x & v_y & v_z & \phi & \theta & \psi & p & q & r \end{bmatrix}^\top \in \mathbb{R}^{12}

The input is the collective thrust and the three body torques:

u = \begin{bmatrix} F & \tau_\phi & \tau_\theta & \tau_\psi \end{bmatrix}^\top \in \mathbb{R}^{4}

Dynamics

Translational dynamics. Thrust acts along the body $z$ -axis, rotated into the world frame (ZYX Euler convention), with gravity pulling down:

\begin{aligned} \dot{x} &= v_x, \qquad \dot{y} = v_y, \qquad \dot{z} = v_z \\[4pt] \dot{v}_x &= \frac{F}{m}\left(\sin\psi \sin\phi + \cos\psi \sin\theta \cos\phi\right) \\ \dot{v}_y &= \frac{F}{m}\left(-\cos\psi \sin\phi + \sin\psi \sin\theta \cos\phi\right) \\ \dot{v}_z &= \frac{F}{m}\cos\theta \cos\phi - g \end{aligned}

Euler-angle kinematics. The body rates $(p, q, r)$ are mapped to Euler-angle rates:

\begin{aligned} \dot{\phi} &= p + \sin\phi \tan\theta \, q + \cos\phi \tan\theta \, r \\ \dot{\theta} &= \cos\phi \, q - \sin\phi \, r \\ \dot{\psi} &= \frac{\sin\phi}{\cos\theta} \, q + \frac{\cos\phi}{\cos\theta} \, r \end{aligned}

Rotational dynamics (Euler's equations with gyroscopic coupling):

\begin{aligned} \dot{p} &= \frac{\tau_\phi}{I_{xx}} + \frac{I_{yy} - I_{zz}}{I_{xx}} \, q r \\ \dot{q} &= \frac{\tau_\theta}{I_{yy}} + \frac{I_{zz} - I_{xx}}{I_{yy}} \, p r \\ \dot{r} &= \frac{\tau_\psi}{I_{zz}} + \frac{I_{xx} - I_{yy}}{I_{zz}} \, p q \end{aligned}

with $m = 1.0\,\text{kg}$ , $g = 9.81\,\text{m/s}^2$ , $I_{xx} = I_{yy} = 0.01\,\text{kg·m}^2$ , $I_{zz} = 0.02\,\text{kg·m}^2$ .

Compactly, the model is the nonlinear ODE

\dot{x} = f(x, u).

The model in Python

This is the core of the "high-level language" argument: the equations above translate almost line by line into CasADi symbols. We never write a derivative by hand; acados will differentiate this expression automatically.

code

# python/quadrotor_model.py
import casadi as ca
from acados_template import AcadosModel

def export_quadrotor_model() -> AcadosModel:
    # State: [x, y, z, vx, vy, vz, roll, pitch, yaw, p, q, r]
    x, y, z       = ca.SX.sym('x'), ca.SX.sym('y'), ca.SX.sym('z')
    vx, vy, vz    = ca.SX.sym('vx'), ca.SX.sym('vy'), ca.SX.sym('vz')
    phi, theta, psi = ca.SX.sym('phi'), ca.SX.sym('theta'), ca.SX.sym('psi')
    p, q, r       = ca.SX.sym('p'), ca.SX.sym('q'), ca.SX.sym('r')
    state = ca.vertcat(x, y, z, vx, vy, vz, phi, theta, psi, p, q, r)

    # Controls: [thrust, tau_phi, tau_theta, tau_psi]
    thrust    = ca.SX.sym('thrust')
    tau_phi   = ca.SX.sym('tau_phi')
    tau_theta = ca.SX.sym('tau_theta')
    tau_psi   = ca.SX.sym('tau_psi')
    control = ca.vertcat(thrust, tau_phi, tau_theta, tau_psi)

    m, g = 1.0, 9.81
    Ixx, Iyy, Izz = 0.01, 0.01, 0.02

    # Translational dynamics
    dvx = (ca.sin(psi)*ca.sin(phi) + ca.cos(psi)*ca.sin(theta)*ca.cos(phi)) * thrust / m
    dvy = (-ca.cos(psi)*ca.sin(phi) + ca.sin(psi)*ca.sin(theta)*ca.cos(phi)) * thrust / m
    dvz = ca.cos(theta)*ca.cos(phi) * thrust / m - g

    # Euler-angle kinematics
    dphi   = p + ca.sin(phi)*ca.tan(theta)*q + ca.cos(phi)*ca.tan(theta)*r
    dtheta = ca.cos(phi)*q - ca.sin(phi)*r
    dpsi   = ca.sin(phi)/ca.cos(theta)*q + ca.cos(phi)/ca.cos(theta)*r

    # Rotational dynamics
    dp = tau_phi   / Ixx + (Iyy - Izz) / Ixx * q * r
    dq = tau_theta / Iyy + (Izz - Ixx) / Iyy * p * r
    dr = tau_psi   / Izz + (Ixx - Iyy) / Izz * p * q

    f_expl = ca.vertcat(vx, vy, vz, dvx, dvy, dvz, dphi, dtheta, dpsi, dp, dq, dr)

    x_dot = ca.SX.sym('x_dot', state.shape)

    model = AcadosModel()
    model.name        = 'quadrotor'
    model.x           = state
    model.xdot        = x_dot
    model.u           = control
    model.f_expl_expr = f_expl
    model.f_impl_expr = x_dot - f_expl
    return model

About 60 lines describe a 12-state nonlinear system. Changing a parameter, adding drag or switching to quaternions only means editing this file. So it kinda a win win situation :) .

Step 2: The optimal control problem

Formulation

At each sampling instant, with measured state $\bar{x}_0$ , the controller solves

\begin{aligned} \min_{x_{0:N},\, u_{0:N-1}} \quad & \sum_{k=0}^{N-1} \left\| \begin{bmatrix} x_k - x^{\text{ref}}_k \\ u_k - u^{\text{ref}}_k \\ u_k - u_{\text{prev}} \end{bmatrix} \right\|^2_{W} + \left\| x_N - x^{\text{ref}}_N \right\|^2_{W_e} \\[6pt] \text{s.t.} \quad & x_0 = \bar{x}_0, \\ & x_{k+1} = \Phi(x_k, u_k), \qquad k = 0, \dots, N-1, \\ & \underline{x} \le x_k \le \overline{x}, \qquad k = 0, \dots, N, \\ & \underline{u} \le u_k \le \overline{u}, \qquad k = 0, \dots, N-1, \end{aligned}

where $\Phi$ is one step of an explicit Runge–Kutta integrator applied to $f$ , and the weights are block-diagonal:

W = \begin{bmatrix} Q & 0 & 0 \\ 0 & R & 0 \\ 0 & 0 & R_{\text{rate}} \end{bmatrix}, \qquad W_e = 2Q.

The third block, $u_k - u_{\text{prev}}$ , penalizes input changes and smooths the thrust and torque commands. $u_{\text{prev}}$ is a runtime parameter of the solver, so it can be updated without regenerating code.

For the quadrotor the horizon is $T_f = 2.0\,\text{s}$ with $N = 30$ shooting intervals ( $\Delta t \approx 0.067\,\text{s}$ ). That gives $12 \cdot 31 + 4 \cdot 30 = 492$ decision variables per solve.

Weights and constraints

\begin{aligned} Q &= \operatorname{diag}(100, 100, 80,\; 15, 15, 10,\; 5, 5, 2,\; 1, 1, 1) \\ R &= \operatorname{diag}(0.1, 0.5, 0.5, 0.5) \\ R_{\text{rate}} &= \operatorname{diag}(0.5, 0.3, 0.3, 0.3) \end{aligned}

Quantity	Lower bound	Upper bound
Position $x, y$	$-20$ m	$20$ m
Altitude $z$	$0$ m	$15$ m
Velocity $v_x, v_y, v_z$	$-5$ m/s	$5$ m/s
Roll, pitch $\phi, \theta$	$-\pi/3$	$\pi/3$
Yaw $\psi$	$-\pi$	$\pi$
Body rates $p, q, r$	$-2$ rad/s	$2$ rad/s
Thrust $F$	$0.3\,mg$	$2.0\,mg$
Torques $\tau_\phi, \tau_\theta$	$-1$ N·m	$1$ N·m
Torque $\tau_\psi$	$-0.5$ N·m	$0.5$ N·m

The roll and pitch limits of $\pm 60^\circ$ also keep the model away from the Euler-angle singularity at $\theta = \pm 90^\circ$ , where $\tan\theta$ and $1/\cos\theta$ blow up.

The generator script

code

# python/generate_quadrotor.py
import numpy as np
import casadi as ca
from acados_template import AcadosOcp, AcadosOcpSolver
from quadrotor_model import export_quadrotor_model

ocp = AcadosOcp()
model = export_quadrotor_model()
ocp.model = model

Tf, N = 2.0, 30
nx, nu = model.x.rows(), model.u.rows()
ocp.solver_options.N_horizon = N
ocp.solver_options.tf        = Tf

Q_mat      = np.diag([100, 100, 80, 15, 15, 10, 5, 5, 2, 1, 1, 1])
R_mat      = np.diag([0.1, 0.5, 0.5, 0.5])
R_rate_mat = np.diag([0.5, 0.3, 0.3, 0.3])

# u_prev is a runtime parameter -> input-rate penalty
u_prev               = ca.SX.sym('u_prev', nu)
ocp.model.p          = u_prev
ocp.parameter_values = np.zeros(nu)

# Stage cost: y = [x, u, u - u_prev]
ocp.cost.cost_type    = 'NONLINEAR_LS'
ocp.model.cost_y_expr = ca.vertcat(model.x, model.u, model.u - u_prev)
ocp.cost.yref         = np.zeros(nx + nu + nu)
ocp.cost.W            = ca.diagcat(Q_mat, R_mat, R_rate_mat).full()

# Terminal cost
ocp.cost.cost_type_e    = 'NONLINEAR_LS'
ocp.model.cost_y_expr_e = model.x
ocp.cost.yref_e         = np.zeros(nx)
ocp.cost.W_e            = 2.0 * Q_mat

# Box constraints on all states and inputs
ocp.constraints.idxbx = np.arange(nx)
ocp.constraints.idxbu = np.arange(nu)
# ... lbx/ubx/lbu/ubu as in the table above ...
ocp.constraints.x0 = np.zeros(nx)

# Solver options: tuned for embedded real-time use
ocp.solver_options.qp_solver       = 'PARTIAL_CONDENSING_HPIPM'
ocp.solver_options.hpipm_mode      = 'SPEED'
ocp.solver_options.hessian_approx  = 'GAUSS_NEWTON'
ocp.solver_options.integrator_type = 'ERK'
ocp.solver_options.nlp_solver_type = 'SQP_RTI'

ocp.code_export_directory = '../acados_generated_quadrotor'
AcadosOcpSolver(ocp, json_file='../acados_generated_quadrotor/acados_ocp.json')

Running it:

code

cd python
python generate_quadrotor.py

This produces acados_generated_quadrotor/, which holds the generated C sources and libacados_ocp_solver_quadrotor.so (.dylib on macOS).

Why these solver options matter for speed

These options are where the "fast enough for embedded" part comes from:

SQP_RTI (Real-Time Iteration). Instead of iterating SQP to convergence, the solver does one SQP step per sampling time and warm-starts from the previous solution shifted in time. The cost per control step is bounded and predictable, which is what a real-time loop needs.
GAUSS_NEWTON Hessian. With a least-squares cost $\tfrac12\|y(x,u) - y^{\text{ref}}\|_W^2$ the Hessian is approximated by $J_y^\top W J_y$ . It needs only first derivatives and is always positive semi-definite.
ERK integrator. Explicit Runge–Kutta with sensitivities generated by CasADi's algorithmic differentiation. Classic RK4 looks like this:

\begin{aligned} k_1 &= f(x_k, u_k) \\ k_2 &= f\!\left(x_k + \tfrac{h}{2} k_1,\, u_k\right) \\ k_3 &= f\!\left(x_k + \tfrac{h}{2} k_2,\, u_k\right) \\ k_4 &= f\!\left(x_k + h\, k_3,\, u_k\right) \\ x_{k+1} &= x_k + \tfrac{h}{6}\left(k_1 + 2k_2 + 2k_3 + k_4\right) \end{aligned}

PARTIAL_CONDENSING_HPIPM in SPEED mode. The QP solver exploits the stage-wise (banded) structure of the MPC problem. It is built on BLASFEO, a linear-algebra library tuned for the small dense matrices typical of embedded MPC.

Each RTI step solves one QP that is a local linearization of the NLP around the current guess $(x^{(i)}, u^{(i)})$ :

\begin{aligned} \min_{\Delta x,\, \Delta u} \quad & \tfrac12 \begin{bmatrix}\Delta x \\ \Delta u\end{bmatrix}^\top H \begin{bmatrix}\Delta x \\ \Delta u\end{bmatrix} + g^\top \begin{bmatrix}\Delta x \\ \Delta u\end{bmatrix} \\ \text{s.t.} \quad & \Delta x_{k+1} = A_k \Delta x_k + B_k \Delta u_k + \left(\Phi(x_k^{(i)}, u_k^{(i)}) - x_{k+1}^{(i)}\right), \\ & \Delta x_0 = \bar{x}_0 - x_0^{(i)}, \quad \text{box constraints on } x^{(i)} + \Delta x,\; u^{(i)} + \Delta u, \end{aligned}

with $A_k = \partial\Phi/\partial x$ and $B_k = \partial\Phi/\partial u$ computed by the generated code.

Step 3: The runtime configuration

The C++ side never needs to know which robot it is controlling. Everything it needs comes from a JSON file:

code

{
    "model_name": "quadrotor",
    "x_init":   [0.0, 0.0, 0.5, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
    "x_target": [5.0, 5.0, 3.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
    "lbx": [-20.0, -20.0, 0.0, -5.0, -5.0, -5.0, -1.047, -1.047, -3.14159, -2.0, -2.0, -2.0],
    "ubx": [ 20.0,  20.0, 15.0, 5.0,  5.0,  5.0,  1.047,  1.047,  3.14159,  2.0,  2.0,  2.0],
    "lbu": [2.943, -1.0, -1.0, -0.5],
    "ubu": [19.62,  1.0,  1.0,  0.5],
    "Q_diag":      [100.0, 100.0, 80.0, 15.0, 15.0, 10.0, 5.0, 5.0, 2.0, 1.0, 1.0, 1.0],
    "R_diag":      [0.1, 0.5, 0.5, 0.5],
    "R_rate_diag": [0.5, 0.3, 0.3, 0.3],
    "dt": 0.05,
    "distance_threshold": 0.15,
    "max_iterations": 2000
}

Bounds and weights are pushed into the solver at startup, so tuning does not require regenerating or recompiling anything. Only a change to the model structure (states, dynamics, cost expression) needs a new generate_*.py run.

Step 4: The generic C++ controller

Loading any generated solver at runtime

acados prefixes every generated function with the model name (quadrotor_acados_create, quadrotor_acados_solve, …). The AcadosMPCSolver class uses that to bind to any generated library with dlopen/dlsym:

code

// src/acados_mpc_solver.cpp
std::string lib_path = "libacados_ocp_solver_" + model_name + ".dylib";

lib_handle = dlopen(lib_path.c_str(), RTLD_NOW | RTLD_LOCAL);
if (!lib_handle) {
    throw std::runtime_error("Failed to load library: " + std::string(dlerror()));
}

create_capsule_fn = reinterpret_cast<void*(*)()>(
    dlsym(lib_handle, (model_name + "_acados_create_capsule").c_str()));
create_fn = reinterpret_cast<int(*)(void*)>(
    dlsym(lib_handle, (model_name + "_acados_create").c_str()));
solve_fn = reinterpret_cast<int(*)(void*)>(
    dlsym(lib_handle, (model_name + "_acados_solve").c_str()));
// ... get_nlp_config / dims / in / out / solver / opts, update_params ...

capsule = create_capsule_fn();
create_fn(capsule);

// Dimensions are queried from the solver, not hard-coded
nx = ocp_nlp_dims_get_from_attr(nlp_config, nlp_dims, nlp_out, 0, "x");
nu = ocp_nlp_dims_get_from_attr(nlp_config, nlp_dims, nlp_out, 0, "u");
N  = nlp_dims->N;

Overwriting bounds and weights from the config

code

for (int i = 0; i <= N; ++i) {
    ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, i, "lbx", lbx.data());
    ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, i, "ubx", ubx.data());
}

int ny = nx + nu + nu;                       // [x, u, u - u_prev]
std::vector<double> W(ny * ny, 0.0);
for (int i = 0; i < nx; ++i) W[i * ny + i]                       = Q_diag[i];
for (int i = 0; i < nu; ++i) W[(nx + i) * ny + (nx + i)]           = R_diag[i];
for (int i = 0; i < nu; ++i) W[(nx + nu + i) * ny + (nx + nu + i)] = R_rate_diag[i];

for (int i = 0; i < N; ++i)
    ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, i, "W", W.data());

The receding-horizon loop

code

// src/mpc_controller.cpp
AcadosMPCSolver solver(model_name, lbx, ubx, lbu, ubu, Q_diag, R_diag, R_rate_diag);

// Helix reference from x_init to x_target (1.5 turns while climbing)
auto x_ref_traj = generate_helix_trajectory(x_init, x_target, 150);

for (int i = 0; i < max_iterations; ++i) {
    solver.setInitialState(x_curr);                 // x_0 = measured state

    for (int j = 0; j <= N; ++j) {                  // slide the reference window
        int ref_index = std::min(i + j, (int)x_ref_traj.size() - 1);
        std::vector<double> yref(ny, 0.0);
        for (int k = 0; k < nx; ++k) yref[k] = x_ref_traj[ref_index][k];
        solver.setReference(j, yref);
    }

    if (solver.solve() != 0) { /* handle failure */ }

    std::vector<double> u = solver.getControl(0);   // apply first input only
    x_curr = solver.getState(1);                    // advance the simulation
}

The helix reference for the quadrotor is built from the start and target:

\begin{aligned} c &= \tfrac12 (p_0 + p_f)_{xy}, \qquad \rho = \tfrac12 \left\| (p_f - p_0)_{xy} \right\| \\ \alpha(t) &= \operatorname{atan2}(y_0 - c_y,\; x_0 - c_x) + 3\pi t, \qquad t \in [0, 1] \\ x^{\text{ref}}(t) &= c_x + \rho \cos\alpha(t), \quad y^{\text{ref}}(t) = c_y + \rho \sin\alpha(t), \quad z^{\text{ref}}(t) = z_0 + (z_f - z_0)\, t \end{aligned}

Build and run

code

mkdir build && cd build
cmake ..          # set ACADOS_SOURCE_DIR to your acados install
make
DYLD_LIBRARY_PATH=. ./mpc_controller ../configs/quadrotor.json   # macOS
LD_LIBRARY_PATH=.   ./mpc_controller ../configs/quadrotor.json   # Linux

Expected console output:

code

Model: quadrotor
States: 12, Controls: 4, Horizon: 30
Time: 0s, Error: ...
...
Solver info:
 SQP iterations 1
 Solve time ...
Plotting results...
Creating animation...

The states, controls and the XY trajectory are saved to plots/, along with an animated GIF that shows the actual path, the reference and the MPC prediction at each step (the animation at the top of this post).

The same binary runs the other models with no change on the model that I predefined inside the repository.

code

./mpc_controller ../configs/bicycle.json
./mpc_controller ../configs/differential_drive.json
./mpc_controller ../configs/rocket.json

Does it answer the question? Really ?

The pipeline confirms the main point of the motivation: the robot is described entirely in Python which makes a bit easy to write, and the control loop runs entirely in generated C/C++. Specifically:

Development speed. The 12-state quadrotor model is roughly 60 lines of CasADi. All Jacobians needed by the solver are generated automatically and exactly, with no hand derivation.
Runtime independence from Python. The executable only links acados, BLASFEO and HPIPM. Python is needed at code-generation time, not on the device.
Bounded, predictable cost per step. RTI performs one QP solve per control step, and HPIPM exploits the stage-wise structure, so the cost per step is bounded rather than iteration-dependent.
One controller, many robots. Bicycle, differential drive, quadrotor and rocket all run through the same mpc_controller binary changing robots means switching the JSON file.

Measuring it on your own hardware

The controller prints the solver timing reported by acados (time_tot, which acados reports in seconds). To evaluate the embedded claim properly, record for each model:

Model	$n_x$	$n_u$	$N$	Mean solve time	Max solve time
Bicycle	4	2	20	(measure)	(measure)
Differential drive	3	2	30	(measure)	(measure)
Quadrotor	12	4	30	(measure)	(measure)
Rocket	6	3	20	(measure)	(measure)

Compare the maximum solve time with the control period. The real-time requirement is a worst-case bound, not an average. Good targets are a desktop CPU, a Raspberry Pi / Jetson-class board and, if possible, the actual flight computer. Also the generated C/C++ can be use as a library for ROS2 integration.

More Example

Obstacle Avoidance of Car

multi_car_animation

Four Quadrotor Avoidance

multi_quadrotor_animation

Limitations and next steps

Honestly about what the current code does and does not show:

Closed loop on the prediction. The loop advances with x_curr = solver.getState(1), i.e. the solver's own one-step prediction. A more realistic test integrates the true dynamics, for example with the RK4 runge_kutta helper already in mpc_controller.cpp, and adds noise or model mismatch.
Timing consistency. The quadrotor solver is generated with a shooting interval of $T_f/N \approx 0.067\,\text{s}$ , while the config uses dt = 0.05. For a fair real-time test, the shooting interval and the control period should match.
Input-rate term. u_prev should be updated every step with setParameter(...) so that the $R_{\text{rate}}$ penalty really acts on $u_k - u_{k-1}$ .
Portability. The loader currently looks for .dylib; on Linux it needs .so.
Attitude representation. Euler angles are simple but singular at $\theta = \pm 90^\circ$ . A quaternion model would allow aggressive manoeuvres, and only quadrotor_model.py would need to change.
Multi-agent. configs/multi_quadrotor.json sketches several quadrotors with obstacle and inter-agent distance constraints. That is the next step, and a good stress test for embedded solve times.

Conclusion

This repository is a project I set aside after my Robocon competitions in 2023, and coming back to it has been a lot of fun. The part that impressed me most about acados is how little I had to write myself: I described the quadrotor in about 60 lines of Python, and acados generated the derivatives, the integrator and a real-time solver in plain C. That makes it a great playground for experimenting with MPC/NMPC formulations and solver settings without hand-coding anything.

So, back to the question from the start: can a high-level language help us get fast control on embedded hardware? My answer so far is yes, as long as the high-level language is used to describe the robot rather than to run it. The next step is to back this up with real solve-time numbers on an embedded board.

MPC is also far from being replaced by Reinforcement Learning. The two are increasingly combined: learned models or policies paired with MPC's ability to respect hard constraints. In the future I'd like to explore that combination, add more use cases such as multi-quadrotor control with obstacles, and maybe even see where LLMs fit into robot control. Who knows?

article │nmpc-acados-cpp.md

Modeling in Python, Run in C/C++: The NMPC for Quadrotor using Acados

Sokheng Din21 Oct 2026~10 min#c++#mpc#nmpc#python#quadrotor#robotics

Motivation

This project asks a simple question:

Can we describe the robot in a high-level language (Python + CasADi), and still get solver code that is fast enough for an embedded device?

Python (CasADi) is used only to describe the system: states, inputs, differential equations, cost and constraints. This is short, readable and easy to modify.
Acados takes that symbolic description and generates plain C code: the dynamics, their exact derivatives (via automatic differentiation), the integrator and a structure-exploiting QP solver (HPIPM on top of BLASFEO).
C++ loads the generated solver as a shared library at runtime and runs the control loop. There is no Python on the control path.

In this post I walk through the whole pipeline using the most interesting model in the repository: a 12-state quadrotor.

Link here to the repo: SokhengDin/NMPC-ACADOS-CPP

Architecture at a glance

code

 ┌──────────────────────────┐      ┌───────────────────────────┐      ┌──────────────────────────┐
 │  python/quadrotor_model  │      │  acados code generation   │      │  C++ mpc_controller      │
 │  (CasADi symbolic model) │ ───▶ │  python/generate_*.py     │ ───▶ │  dlopen(libacados_ocp_   │
 │  states, inputs, f(x,u)  │      │  → C code + .so / .dylib  │      │  solver_quadrotor.so)    │
 └──────────────────────────┘      └───────────────────────────┘      │  + configs/*.json        │
                                                                      └──────────────────────────┘
      high-level, readable               automatic derivatives,           no Python at runtime,
                                         embedded-friendly C              no recompilation per model

The workflow has four steps:

Define the robot model in Python (python/quadrotor_model.py).
Formulate the optimal control problem and generate C code (python/generate_quadrotor.py).
Write a JSON config with initial state, target, bounds and weights (configs/quadrotor.json).
Run the generic C++ controller: ./mpc_controller ../configs/quadrotor.json.

Step 1: The quadrotor model

State and input

The quadrotor has 12 states: position, linear velocity, Euler angles and body angular rates which is a lot.

x = \begin{bmatrix} x & y & z & v_x & v_y & v_z & \phi & \theta & \psi & p & q & r \end{bmatrix}^\top \in \mathbb{R}^{12}

The input is the collective thrust and the three body torques:

u = \begin{bmatrix} F & \tau_\phi & \tau_\theta & \tau_\psi \end{bmatrix}^\top \in \mathbb{R}^{4}

Dynamics

Translational dynamics. Thrust acts along the body $z$ -axis, rotated into the world frame (ZYX Euler convention), with gravity pulling down:

\begin{aligned} \dot{x} &= v_x, \qquad \dot{y} = v_y, \qquad \dot{z} = v_z \\[4pt] \dot{v}_x &= \frac{F}{m}\left(\sin\psi \sin\phi + \cos\psi \sin\theta \cos\phi\right) \\ \dot{v}_y &= \frac{F}{m}\left(-\cos\psi \sin\phi + \sin\psi \sin\theta \cos\phi\right) \\ \dot{v}_z &= \frac{F}{m}\cos\theta \cos\phi - g \end{aligned}

Euler-angle kinematics. The body rates $(p, q, r)$ are mapped to Euler-angle rates:

\begin{aligned} \dot{\phi} &= p + \sin\phi \tan\theta \, q + \cos\phi \tan\theta \, r \\ \dot{\theta} &= \cos\phi \, q - \sin\phi \, r \\ \dot{\psi} &= \frac{\sin\phi}{\cos\theta} \, q + \frac{\cos\phi}{\cos\theta} \, r \end{aligned}

Rotational dynamics (Euler's equations with gyroscopic coupling):

\begin{aligned} \dot{p} &= \frac{\tau_\phi}{I_{xx}} + \frac{I_{yy} - I_{zz}}{I_{xx}} \, q r \\ \dot{q} &= \frac{\tau_\theta}{I_{yy}} + \frac{I_{zz} - I_{xx}}{I_{yy}} \, p r \\ \dot{r} &= \frac{\tau_\psi}{I_{zz}} + \frac{I_{xx} - I_{yy}}{I_{zz}} \, p q \end{aligned}

with $m = 1.0\,\text{kg}$ , $g = 9.81\,\text{m/s}^2$ , $I_{xx} = I_{yy} = 0.01\,\text{kg·m}^2$ , $I_{zz} = 0.02\,\text{kg·m}^2$ .

Compactly, the model is the nonlinear ODE

\dot{x} = f(x, u).

The model in Python

code

# python/quadrotor_model.py
import casadi as ca
from acados_template import AcadosModel

def export_quadrotor_model() -> AcadosModel:
    # State: [x, y, z, vx, vy, vz, roll, pitch, yaw, p, q, r]
    x, y, z       = ca.SX.sym('x'), ca.SX.sym('y'), ca.SX.sym('z')
    vx, vy, vz    = ca.SX.sym('vx'), ca.SX.sym('vy'), ca.SX.sym('vz')
    phi, theta, psi = ca.SX.sym('phi'), ca.SX.sym('theta'), ca.SX.sym('psi')
    p, q, r       = ca.SX.sym('p'), ca.SX.sym('q'), ca.SX.sym('r')
    state = ca.vertcat(x, y, z, vx, vy, vz, phi, theta, psi, p, q, r)

    # Controls: [thrust, tau_phi, tau_theta, tau_psi]
    thrust    = ca.SX.sym('thrust')
    tau_phi   = ca.SX.sym('tau_phi')
    tau_theta = ca.SX.sym('tau_theta')
    tau_psi   = ca.SX.sym('tau_psi')
    control = ca.vertcat(thrust, tau_phi, tau_theta, tau_psi)

    m, g = 1.0, 9.81
    Ixx, Iyy, Izz = 0.01, 0.01, 0.02

    # Translational dynamics
    dvx = (ca.sin(psi)*ca.sin(phi) + ca.cos(psi)*ca.sin(theta)*ca.cos(phi)) * thrust / m
    dvy = (-ca.cos(psi)*ca.sin(phi) + ca.sin(psi)*ca.sin(theta)*ca.cos(phi)) * thrust / m
    dvz = ca.cos(theta)*ca.cos(phi) * thrust / m - g

    # Euler-angle kinematics
    dphi   = p + ca.sin(phi)*ca.tan(theta)*q + ca.cos(phi)*ca.tan(theta)*r
    dtheta = ca.cos(phi)*q - ca.sin(phi)*r
    dpsi   = ca.sin(phi)/ca.cos(theta)*q + ca.cos(phi)/ca.cos(theta)*r

    # Rotational dynamics
    dp = tau_phi   / Ixx + (Iyy - Izz) / Ixx * q * r
    dq = tau_theta / Iyy + (Izz - Ixx) / Iyy * p * r
    dr = tau_psi   / Izz + (Ixx - Iyy) / Izz * p * q

    f_expl = ca.vertcat(vx, vy, vz, dvx, dvy, dvz, dphi, dtheta, dpsi, dp, dq, dr)

    x_dot = ca.SX.sym('x_dot', state.shape)

    model = AcadosModel()
    model.name        = 'quadrotor'
    model.x           = state
    model.xdot        = x_dot
    model.u           = control
    model.f_expl_expr = f_expl
    model.f_impl_expr = x_dot - f_expl
    return model

About 60 lines describe a 12-state nonlinear system. Changing a parameter, adding drag or switching to quaternions only means editing this file. So it kinda a win win situation :) .

Step 2: The optimal control problem

Formulation

At each sampling instant, with measured state $\bar{x}_0$ , the controller solves

\begin{aligned} \min_{x_{0:N},\, u_{0:N-1}} \quad & \sum_{k=0}^{N-1} \left\| \begin{bmatrix} x_k - x^{\text{ref}}_k \\ u_k - u^{\text{ref}}_k \\ u_k - u_{\text{prev}} \end{bmatrix} \right\|^2_{W} + \left\| x_N - x^{\text{ref}}_N \right\|^2_{W_e} \\[6pt] \text{s.t.} \quad & x_0 = \bar{x}_0, \\ & x_{k+1} = \Phi(x_k, u_k), \qquad k = 0, \dots, N-1, \\ & \underline{x} \le x_k \le \overline{x}, \qquad k = 0, \dots, N, \\ & \underline{u} \le u_k \le \overline{u}, \qquad k = 0, \dots, N-1, \end{aligned}

where $\Phi$ is one step of an explicit Runge–Kutta integrator applied to $f$ , and the weights are block-diagonal:

W = \begin{bmatrix} Q & 0 & 0 \\ 0 & R & 0 \\ 0 & 0 & R_{\text{rate}} \end{bmatrix}, \qquad W_e = 2Q.

Weights and constraints

\begin{aligned} Q &= \operatorname{diag}(100, 100, 80,\; 15, 15, 10,\; 5, 5, 2,\; 1, 1, 1) \\ R &= \operatorname{diag}(0.1, 0.5, 0.5, 0.5) \\ R_{\text{rate}} &= \operatorname{diag}(0.5, 0.3, 0.3, 0.3) \end{aligned}

Quantity	Lower bound	Upper bound
Position $x, y$	$-20$ m	$20$ m
Altitude $z$	$0$ m	$15$ m
Velocity $v_x, v_y, v_z$	$-5$ m/s	$5$ m/s
Roll, pitch $\phi, \theta$	$-\pi/3$	$\pi/3$
Yaw $\psi$	$-\pi$	$\pi$
Body rates $p, q, r$	$-2$ rad/s	$2$ rad/s
Thrust $F$	$0.3\,mg$	$2.0\,mg$
Torques $\tau_\phi, \tau_\theta$	$-1$ N·m	$1$ N·m
Torque $\tau_\psi$	$-0.5$ N·m	$0.5$ N·m

The roll and pitch limits of $\pm 60^\circ$ also keep the model away from the Euler-angle singularity at $\theta = \pm 90^\circ$ , where $\tan\theta$ and $1/\cos\theta$ blow up.

The generator script

code

# python/generate_quadrotor.py
import numpy as np
import casadi as ca
from acados_template import AcadosOcp, AcadosOcpSolver
from quadrotor_model import export_quadrotor_model

ocp = AcadosOcp()
model = export_quadrotor_model()
ocp.model = model

Tf, N = 2.0, 30
nx, nu = model.x.rows(), model.u.rows()
ocp.solver_options.N_horizon = N
ocp.solver_options.tf        = Tf

Q_mat      = np.diag([100, 100, 80, 15, 15, 10, 5, 5, 2, 1, 1, 1])
R_mat      = np.diag([0.1, 0.5, 0.5, 0.5])
R_rate_mat = np.diag([0.5, 0.3, 0.3, 0.3])

# u_prev is a runtime parameter -> input-rate penalty
u_prev               = ca.SX.sym('u_prev', nu)
ocp.model.p          = u_prev
ocp.parameter_values = np.zeros(nu)

# Stage cost: y = [x, u, u - u_prev]
ocp.cost.cost_type    = 'NONLINEAR_LS'
ocp.model.cost_y_expr = ca.vertcat(model.x, model.u, model.u - u_prev)
ocp.cost.yref         = np.zeros(nx + nu + nu)
ocp.cost.W            = ca.diagcat(Q_mat, R_mat, R_rate_mat).full()

# Terminal cost
ocp.cost.cost_type_e    = 'NONLINEAR_LS'
ocp.model.cost_y_expr_e = model.x
ocp.cost.yref_e         = np.zeros(nx)
ocp.cost.W_e            = 2.0 * Q_mat

# Box constraints on all states and inputs
ocp.constraints.idxbx = np.arange(nx)
ocp.constraints.idxbu = np.arange(nu)
# ... lbx/ubx/lbu/ubu as in the table above ...
ocp.constraints.x0 = np.zeros(nx)

# Solver options: tuned for embedded real-time use
ocp.solver_options.qp_solver       = 'PARTIAL_CONDENSING_HPIPM'
ocp.solver_options.hpipm_mode      = 'SPEED'
ocp.solver_options.hessian_approx  = 'GAUSS_NEWTON'
ocp.solver_options.integrator_type = 'ERK'
ocp.solver_options.nlp_solver_type = 'SQP_RTI'

ocp.code_export_directory = '../acados_generated_quadrotor'
AcadosOcpSolver(ocp, json_file='../acados_generated_quadrotor/acados_ocp.json')

Running it:

code

cd python
python generate_quadrotor.py

This produces acados_generated_quadrotor/, which holds the generated C sources and libacados_ocp_solver_quadrotor.so (.dylib on macOS).

Why these solver options matter for speed

These options are where the "fast enough for embedded" part comes from:

SQP_RTI (Real-Time Iteration). Instead of iterating SQP to convergence, the solver does one SQP step per sampling time and warm-starts from the previous solution shifted in time. The cost per control step is bounded and predictable, which is what a real-time loop needs.
GAUSS_NEWTON Hessian. With a least-squares cost $\tfrac12\|y(x,u) - y^{\text{ref}}\|_W^2$ the Hessian is approximated by $J_y^\top W J_y$ . It needs only first derivatives and is always positive semi-definite.
ERK integrator. Explicit Runge–Kutta with sensitivities generated by CasADi's algorithmic differentiation. Classic RK4 looks like this:

\begin{aligned} k_1 &= f(x_k, u_k) \\ k_2 &= f\!\left(x_k + \tfrac{h}{2} k_1,\, u_k\right) \\ k_3 &= f\!\left(x_k + \tfrac{h}{2} k_2,\, u_k\right) \\ k_4 &= f\!\left(x_k + h\, k_3,\, u_k\right) \\ x_{k+1} &= x_k + \tfrac{h}{6}\left(k_1 + 2k_2 + 2k_3 + k_4\right) \end{aligned}

PARTIAL_CONDENSING_HPIPM in SPEED mode. The QP solver exploits the stage-wise (banded) structure of the MPC problem. It is built on BLASFEO, a linear-algebra library tuned for the small dense matrices typical of embedded MPC.

Each RTI step solves one QP that is a local linearization of the NLP around the current guess $(x^{(i)}, u^{(i)})$ :

\begin{aligned} \min_{\Delta x,\, \Delta u} \quad & \tfrac12 \begin{bmatrix}\Delta x \\ \Delta u\end{bmatrix}^\top H \begin{bmatrix}\Delta x \\ \Delta u\end{bmatrix} + g^\top \begin{bmatrix}\Delta x \\ \Delta u\end{bmatrix} \\ \text{s.t.} \quad & \Delta x_{k+1} = A_k \Delta x_k + B_k \Delta u_k + \left(\Phi(x_k^{(i)}, u_k^{(i)}) - x_{k+1}^{(i)}\right), \\ & \Delta x_0 = \bar{x}_0 - x_0^{(i)}, \quad \text{box constraints on } x^{(i)} + \Delta x,\; u^{(i)} + \Delta u, \end{aligned}

with $A_k = \partial\Phi/\partial x$ and $B_k = \partial\Phi/\partial u$ computed by the generated code.

Step 3: The runtime configuration

The C++ side never needs to know which robot it is controlling. Everything it needs comes from a JSON file:

code

{
    "model_name": "quadrotor",
    "x_init":   [0.0, 0.0, 0.5, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
    "x_target": [5.0, 5.0, 3.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
    "lbx": [-20.0, -20.0, 0.0, -5.0, -5.0, -5.0, -1.047, -1.047, -3.14159, -2.0, -2.0, -2.0],
    "ubx": [ 20.0,  20.0, 15.0, 5.0,  5.0,  5.0,  1.047,  1.047,  3.14159,  2.0,  2.0,  2.0],
    "lbu": [2.943, -1.0, -1.0, -0.5],
    "ubu": [19.62,  1.0,  1.0,  0.5],
    "Q_diag":      [100.0, 100.0, 80.0, 15.0, 15.0, 10.0, 5.0, 5.0, 2.0, 1.0, 1.0, 1.0],
    "R_diag":      [0.1, 0.5, 0.5, 0.5],
    "R_rate_diag": [0.5, 0.3, 0.3, 0.3],
    "dt": 0.05,
    "distance_threshold": 0.15,
    "max_iterations": 2000
}

Step 4: The generic C++ controller

Loading any generated solver at runtime

code

// src/acados_mpc_solver.cpp
std::string lib_path = "libacados_ocp_solver_" + model_name + ".dylib";

lib_handle = dlopen(lib_path.c_str(), RTLD_NOW | RTLD_LOCAL);
if (!lib_handle) {
    throw std::runtime_error("Failed to load library: " + std::string(dlerror()));
}

create_capsule_fn = reinterpret_cast<void*(*)()>(
    dlsym(lib_handle, (model_name + "_acados_create_capsule").c_str()));
create_fn = reinterpret_cast<int(*)(void*)>(
    dlsym(lib_handle, (model_name + "_acados_create").c_str()));
solve_fn = reinterpret_cast<int(*)(void*)>(
    dlsym(lib_handle, (model_name + "_acados_solve").c_str()));
// ... get_nlp_config / dims / in / out / solver / opts, update_params ...

capsule = create_capsule_fn();
create_fn(capsule);

// Dimensions are queried from the solver, not hard-coded
nx = ocp_nlp_dims_get_from_attr(nlp_config, nlp_dims, nlp_out, 0, "x");
nu = ocp_nlp_dims_get_from_attr(nlp_config, nlp_dims, nlp_out, 0, "u");
N  = nlp_dims->N;

Overwriting bounds and weights from the config

code

for (int i = 0; i <= N; ++i) {
    ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, i, "lbx", lbx.data());
    ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, i, "ubx", ubx.data());
}

int ny = nx + nu + nu;                       // [x, u, u - u_prev]
std::vector<double> W(ny * ny, 0.0);
for (int i = 0; i < nx; ++i) W[i * ny + i]                       = Q_diag[i];
for (int i = 0; i < nu; ++i) W[(nx + i) * ny + (nx + i)]           = R_diag[i];
for (int i = 0; i < nu; ++i) W[(nx + nu + i) * ny + (nx + nu + i)] = R_rate_diag[i];

for (int i = 0; i < N; ++i)
    ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, i, "W", W.data());

The receding-horizon loop

code

// src/mpc_controller.cpp
AcadosMPCSolver solver(model_name, lbx, ubx, lbu, ubu, Q_diag, R_diag, R_rate_diag);

// Helix reference from x_init to x_target (1.5 turns while climbing)
auto x_ref_traj = generate_helix_trajectory(x_init, x_target, 150);

for (int i = 0; i < max_iterations; ++i) {
    solver.setInitialState(x_curr);                 // x_0 = measured state

    for (int j = 0; j <= N; ++j) {                  // slide the reference window
        int ref_index = std::min(i + j, (int)x_ref_traj.size() - 1);
        std::vector<double> yref(ny, 0.0);
        for (int k = 0; k < nx; ++k) yref[k] = x_ref_traj[ref_index][k];
        solver.setReference(j, yref);
    }

    if (solver.solve() != 0) { /* handle failure */ }

    std::vector<double> u = solver.getControl(0);   // apply first input only
    x_curr = solver.getState(1);                    // advance the simulation
}

The helix reference for the quadrotor is built from the start and target:

\begin{aligned} c &= \tfrac12 (p_0 + p_f)_{xy}, \qquad \rho = \tfrac12 \left\| (p_f - p_0)_{xy} \right\| \\ \alpha(t) &= \operatorname{atan2}(y_0 - c_y,\; x_0 - c_x) + 3\pi t, \qquad t \in [0, 1] \\ x^{\text{ref}}(t) &= c_x + \rho \cos\alpha(t), \quad y^{\text{ref}}(t) = c_y + \rho \sin\alpha(t), \quad z^{\text{ref}}(t) = z_0 + (z_f - z_0)\, t \end{aligned}

Build and run

code

mkdir build && cd build
cmake ..          # set ACADOS_SOURCE_DIR to your acados install
make
DYLD_LIBRARY_PATH=. ./mpc_controller ../configs/quadrotor.json   # macOS
LD_LIBRARY_PATH=.   ./mpc_controller ../configs/quadrotor.json   # Linux

Expected console output:

code

Model: quadrotor
States: 12, Controls: 4, Horizon: 30
Time: 0s, Error: ...
...
Solver info:
 SQP iterations 1
 Solve time ...
Plotting results...
Creating animation...

The same binary runs the other models with no change on the model that I predefined inside the repository.

code

./mpc_controller ../configs/bicycle.json
./mpc_controller ../configs/differential_drive.json
./mpc_controller ../configs/rocket.json

Does it answer the question? Really ?

Development speed. The 12-state quadrotor model is roughly 60 lines of CasADi. All Jacobians needed by the solver are generated automatically and exactly, with no hand derivation.
Runtime independence from Python. The executable only links acados, BLASFEO and HPIPM. Python is needed at code-generation time, not on the device.
Bounded, predictable cost per step. RTI performs one QP solve per control step, and HPIPM exploits the stage-wise structure, so the cost per step is bounded rather than iteration-dependent.
One controller, many robots. Bicycle, differential drive, quadrotor and rocket all run through the same mpc_controller binary changing robots means switching the JSON file.

Measuring it on your own hardware

The controller prints the solver timing reported by acados (time_tot, which acados reports in seconds). To evaluate the embedded claim properly, record for each model:

Model	$n_x$	$n_u$	$N$	Mean solve time	Max solve time
Bicycle	4	2	20	(measure)	(measure)
Differential drive	3	2	30	(measure)	(measure)
Quadrotor	12	4	30	(measure)	(measure)
Rocket	6	3	20	(measure)	(measure)

More Example

Obstacle Avoidance of Car

multi_car_animation

Four Quadrotor Avoidance

multi_quadrotor_animation

Limitations and next steps

Honestly about what the current code does and does not show:

Closed loop on the prediction. The loop advances with x_curr = solver.getState(1), i.e. the solver's own one-step prediction. A more realistic test integrates the true dynamics, for example with the RK4 runge_kutta helper already in mpc_controller.cpp, and adds noise or model mismatch.
Timing consistency. The quadrotor solver is generated with a shooting interval of $T_f/N \approx 0.067\,\text{s}$ , while the config uses dt = 0.05. For a fair real-time test, the shooting interval and the control period should match.
Input-rate term. u_prev should be updated every step with setParameter(...) so that the $R_{\text{rate}}$ penalty really acts on $u_k - u_{k-1}$ .
Portability. The loader currently looks for .dylib; on Linux it needs .so.
Attitude representation. Euler angles are simple but singular at $\theta = \pm 90^\circ$ . A quaternion model would allow aggressive manoeuvres, and only quadrotor_model.py would need to change.
Multi-agent. configs/multi_quadrotor.json sketches several quadrotors with obstacle and inter-agent distance constraints. That is the next step, and a good stress test for embedded solve times.