### Full Rocket Landing Example with SOC + Box Constraints

Source: https://context7.com/tinympc/tinympc-python/llms.txt

This example demonstrates setting up a TinyMPC solver with affine dynamics, box constraints, and second-order cone constraints for a rocket landing scenario. It simulates a receding horizon loop for a total of NTOTAL timesteps.

```python
import tinympc, numpy as np

NSTATES, NINPUTS, NHORIZON = 6, 3, 10

A = np.array([[1,0,0,0.05,0,0],[0,1,0,0,0.05,0],[0,0,1,0,0,0.05],
              [0,0,0,1,0,0],[0,0,0,0,1,0],[0,0,0,0,0,1]], dtype=float)
B = np.diag([0.000125,0.000125,0.000125,0.005,0.005,0.005])[[0,1,2,3,4,5],:][:, [0,1,2]]
B = np.array([[1.25e-4,0,0],[0,1.25e-4,0],[0,0,1.25e-4],
              [0.005,0,0],[0,0.005,0],[0,0,0.005]])
fdyn = np.array([0, 0, -0.01226, 0, 0, -0.4905]).reshape(-1,1)

Q = np.diag([101.]*6);  R = np.diag([2.]*3)
x_min = np.array([-5,-5,-0.5,-10,-10,-20])
x_max = np.array([ 5, 5,100, 10, 10, 20])
u_min = np.array([-10,-10,-10]);  u_max = np.array([105,105,105])

solver = tinympc.TinyMPC()
solver.setup(A, B, Q, R, NHORIZON, rho=1.0, fdyn=fdyn,
             max_iter=100, abs_pri_tol=2e-3,
             x_min=x_min, x_max=x_max, u_min=u_min, u_max=u_max,
             cone_constraints={
                 'Acu': np.array([0],dtype=np.int32), 'qcu': np.array([3],dtype=np.int32), 'cu': np.array([0.25]),
                 'Acx': np.array([0],dtype=np.int32), 'qcx': np.array([3],dtype=np.int32), 'cx': np.array([0.5]),
             })

xinit = np.array([4.0, 2.0, 20.0, -3.0, 2.0, -4.5])
xgoal = np.zeros(6)
x_cur = xinit * 1.1
NTOTAL = 100

for k in range(NTOTAL - NHORIZON):
    x_ref = np.column_stack([xinit + (xgoal-xinit)*(i+k)/(NTOTAL-1) for i in range(NHORIZON)])
    u_ref = np.zeros((3, NHORIZON-1));  u_ref[2,:] = 10.0
    solver.set_x_ref(x_ref);  solver.set_u_ref(u_ref)
    solver.set_x0(x_cur)
    sol = solver.solve()
    x_cur = A @ x_cur + B @ sol["controls"] + fdyn.flatten()

print("Final position:", x_cur[:3].round(3))
# Final position: [~0.0, ~0.0, ~0.0]
print("Distance to goal:", np.linalg.norm(x_cur[:3]).round(3), "m")
```

--------------------------------

### Install TinyMPC Python Package

Source: https://github.com/tinympc/tinympc-python/blob/main/README.md

Install the package using pip. For development, clone the repository with submodules and install in editable mode.

```bash
pip install tinympc
```

```bash
git clone --recurse-submodules https://github.com/TinyMPC/tinympc-python.git
cd tinympc-python
pip install -e .
```

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

--------------------------------

### Core Methods

Source: https://github.com/tinympc/tinympc-python/blob/main/README.md

Lists the primary methods available in the TinyMPC solver for setup, configuration, solving, and code generation.

```APIDOC
## API Reference

### Description
This section provides a reference for the core methods of the TinyMPC solver, detailing their purpose and parameters.

### Methods
- `setup(A, B, Q, R, N, rho=1.0, fdyn=None, verbose=False, **settings)`: Initializes the MPC solver with system dynamics, costs, horizon, and optional settings.
- `set_x0(x0)`: Sets the initial state vector.
- `set_x_ref(x_ref)`: Sets the reference state trajectory.
- `set_u_ref(u_ref)`: Sets the reference input trajectory.
- `set_bound_constraints(x_min, x_max, u_min, u_max)`: Sets box constraints on states and inputs.
- `set_linear_constraints(Alin_x, blin_x, Alin_u, blin_u)`: Sets linear inequality constraints on states and inputs.
- `set_cone_constraints(Acu, qcu, cu, Acx, qcx, cx)`: Sets second-order cone constraints on inputs and states.
- `set_equality_constraints(Aeq_x, beq_x, Aeq_u=None, beq_u=None)`: Sets equality constraints on states and inputs.
- `update_settings(abs_pri_tol=1e-3, abs_dua_tol=1e-3, max_iter=100, ...)`: Updates solver settings such as tolerances and maximum iterations.
- `solve()`: Solves the MPC problem and returns the solution.
- `codegen(output_dir)`: Generates C++ code for the MPC controller into the specified directory.
- `codegen_with_sensitivity(output_dir, dK, dP, dC1, dC2)`: Generates C++ code with precomputed sensitivity information.
```

--------------------------------

### Link Example Executable to TinyMPC Library

Source: https://github.com/tinympc/tinympc-python/blob/main/src/tinympc/codegen/pywrapper/CMakeLists.txt

Links the 'tiny_codegen_example' executable against the 'tinympcstatic' library. This makes the functionality of the static library available to the executable.

```cmake
target_link_libraries(tiny_codegen_example LINK_PUBLIC tinympcstatic)
```

--------------------------------

### setup(A, B, Q, R, N, ...)

Source: https://context7.com/tinympc/tinympc-python/llms.txt

Initializes the MPC solver with system dynamics, cost matrices, prediction horizon, and optional solver settings. Constraints must be enabled separately.

```APIDOC
## setup(A, B, Q, R, N, ...)

Configures the linear dynamics `x[k+1] = A·x[k] + B·u[k] + fdyn`, the LQR cost matrices, the prediction horizon `N`, and optional constraint and solver settings. All constraints are disabled by default after `setup()` — they must be explicitly enabled via the dedicated `set_*` methods or by passing them as keyword arguments.

```python
import numpy as np
import tinympc

# Cartpole: 4 states [x, x_dot, theta, theta_dot], 1 input [force]
A = np.array([[1.0, 0.01, 0.0,   0.0  ],
              [0.0, 1.0,  0.039, 0.0  ],
              [0.0, 0.0,  1.002, 0.01 ],
              [0.0, 0.0,  0.458, 1.002]])
B = np.array([[0.0], [0.02], [0.0], [0.067]])
Q = np.diag([10.0, 1.0, 10.0, 1.0])   # state cost (diagonal, PSD)
R = np.diag([1.0])                      # input cost (diagonal, PD)
N = 20                                  # horizon length

solver = tinympc.TinyMPC()
solver.setup(
    A, B, Q, R, N,
    rho=1.0,          # ADMM penalty parameter
    max_iter=100,     # maximum ADMM iterations
    abs_pri_tol=1e-3, # primal tolerance
    abs_dua_tol=1e-3, # dual tolerance
    verbose=False
)
# Affine dynamics offset (e.g. gravity): fdyn=np.array([...]).reshape(-1,1)
# Inline bound constraints: u_min=np.array([-0.5]), u_max=np.array([0.5])
```
```

--------------------------------

### Set Public Include Directories for Example Executable

Source: https://github.com/tinympc/tinympc-python/blob/main/src/tinympc/codegen/pywrapper/CMakeLists.txt

Configures the include directories for the 'tiny_codegen_example' executable. This ensures the executable can find necessary headers during compilation.

```cmake
target_include_directories(tiny_codegen_example PUBLIC 
    ${CMAKE_CURRENT_SOURCE_DIR}/include
    ${CMAKE_CURRENT_SOURCE_DIR}/include/Eigen
    ${CMAKE_CURRENT_SOURCE_DIR}/tinympc
)
```

--------------------------------

### Create Example Executable

Source: https://github.com/tinympc/tinympc-python/blob/main/src/tinympc/codegen/pywrapper/CMakeLists.txt

Builds an executable named 'tiny_codegen_example' using specified C++ source files. This is likely used for testing or demonstrating the library's functionality.

```cmake
add_executable(tiny_codegen_example src/tiny_main.cpp src/tiny_data.cpp)
```

--------------------------------

### Setup TinyMPC Solver with System and Cost

Source: https://context7.com/tinympc/tinympc-python/llms.txt

Configures the linear dynamics, LQR cost matrices, prediction horizon, and solver settings. Constraints are disabled by default and must be explicitly enabled.

```python
import numpy as np
import tinympc

# Cartpole: 4 states [x, x_dot, theta, theta_dot], 1 input [force]
A = np.array([[1.0, 0.01, 0.0,   0.0  ],
              [0.0, 1.0,  0.039, 0.0  ],
              [0.0, 0.0,  1.002, 0.01 ],
              [0.0, 0.0,  0.458, 1.002]])
B = np.array([[0.0], [0.02], [0.0], [0.067]])
Q = np.diag([10.0, 1.0, 10.0, 1.0])   # state cost (diagonal, PSD)
R = np.diag([1.0])                      # input cost (diagonal, PD)
N = 20                                  # horizon length

solver = tinympc.TinyMPC()
solver.setup(
    A, B, Q, R, N,
    rho=1.0,          # ADMM penalty parameter
    max_iter=100,     # maximum ADMM iterations
    abs_pri_tol=1e-3, # primal tolerance
    abs_dua_tol=1e-3, # dual tolerance
    verbose=False
)
# Affine dynamics offset (e.g. gravity): fdyn=np.array([...]).reshape(-1,1)
# Inline bound constraints: u_min=np.array([-0.5]), u_max=np.array([0.5])
```

--------------------------------

### Basic MPC Workflow Setup and Solve

Source: https://github.com/tinympc/tinympc-python/blob/main/README.md

Set up the MPC solver with system dynamics (A, B), cost matrices (Q, R), and horizon length (N). Then, set initial state, references, and solve for optimal controls.

```python
import numpy as np
import tinympc

A = ...  # nx x nx
B = ...  # nx x nu
Q = ...  # nx x nx (diagonal)
R = ...  # nu x nu (diagonal)
N = 20

solver = tinympc.TinyMPC()
solver.setup(A, B, Q, R, N, rho=1.0, verbose=False)

x0 = np.array([...])
x_ref = np.zeros((A.shape[0], N))
u_ref = np.zeros((B.shape[1], N-1))

solver.set_x0(x0)
solver.set_x_ref(x_ref)
solver.set_u_ref(u_ref)

solution = solver.solve()  # dict
u0 = solution["controls"]         # first control (nu,)
X = solution["states_all"].T       # N x nx
U = solution["controls_all"].T     # (N-1) x nu
```

--------------------------------

### TinyMPC Core Methods

Source: https://github.com/tinympc/tinympc-python/blob/main/README.md

Reference for core TinyMPC methods including setup, setting references, constraints, solving, and code generation.

```python
solver.setup(A, B, Q, R, N, rho=1.0, fdyn=None, verbose=False, **settings)
solver.set_x0(x0)
solver.set_x_ref(x_ref)
solver.set_u_ref(u_ref)
solver.set_bound_constraints(x_min, x_max, u_min, u_max)
solver.set_linear_constraints(Alin_x, blin_x, Alin_u, blin_u)
solver.set_cone_constraints(Acu, qcu, cu, Acx, qcx, cx)
solver.set_equality_constraints(Aeq_x, beq_x, Aeq_u=None, beq_u=None)
solver.update_settings(abs_pri_tol=1e-3, abs_dua_tol=1e-3, max_iter=100, ...)
solution = solver.solve()
solver.codegen(output_dir)
solver.codegen_with_sensitivity(output_dir, dK, dP, dC1, dC2)
```

--------------------------------

### Instantiate TinyMPC Solver

Source: https://context7.com/tinympc/tinympc-python/llms.txt

Creates a new solver object. The dimensions (nx, nu, N) are 0 until setup() is called.

```python
import tinympc

solver = tinympc.TinyMPC()
# solver.nx, solver.nu, solver.N are all 0 until setup() is called
```

--------------------------------

### Load Necessary Packages

Source: https://github.com/tinympc/tinympc-python/blob/main/examples/interactive_quadrotor_ext.ipynb

Imports essential libraries for the TinyMPC workflow, including tinympc, math, and matplotlib for plotting. Ensure 'tinympc' is installed.

```python
import tinympc
import math
%matplotlib inline
import matplotlib.pyplot as plt
```

--------------------------------

### update_settings(**kwargs)

Source: https://context7.com/tinympc/tinympc-python/llms.txt

Allows updating ADMM solver parameters at runtime without reconstructing the solver. This can be called before or after `setup()`.

```APIDOC
## `update_settings(**kwargs)` — Change solver settings at runtime

### Description
Updates ADMM solver parameters without reconstructing the solver. Can be called before or after `setup()`.

### Parameters
Accepts keyword arguments corresponding to solver settings, such as:
- **abs_pri_tol** (float): Absolute primal tolerance.
- **abs_dua_tol** (float): Absolute dual tolerance.
- **max_iter** (int): Maximum number of iterations.
- **check_termination** (int): Frequency (in iterations) to check for convergence.
- **en_state_bound** (bool): Enable state bound constraints.
- **en_input_bound** (bool): Enable input bound constraints.

### Usage
```python
solver.update_settings(
    abs_pri_tol=1e-4,
    abs_dua_tol=1e-4,
    max_iter=200,
    check_termination=5,      # check convergence every 5 iterations
    en_state_bound=True,
    en_input_bound=True,
)
```
```

--------------------------------

### Setup TinyMPC Problem Data and Settings

Source: https://github.com/tinympc/tinympc-python/blob/main/examples/interactive_quadrotor_ext.ipynb

Define the dimensions of the system (states, inputs, horizon length), cost matrices (Q for state, R for input), ADMM penalty parameter (rho), and constraints for states and inputs. Also, set the solver's absolute primal and dual tolerances, maximum iterations, and termination checking period.

```python
n = 12  # number of states == Nx
m = 4   # number of inputs == Nu
N = 10  # horizon length

Q = np.diag(1./max_dev_x**2)  # diagonal of state cost
R = np.diag(1./max_dev_u**2)  # diagonal of input cost
rho = 0.1  # ADMM penalty parameter

x_min = np.tile([-1000.0], (n, N))       # state constraints
x_max = np.tile([1000.0], (n, N))        # state constraints
u_min = np.tile([-u0[0]], (m, N-1))      # input constraints
u_max = np.tile([1.0-u0[0]], (m, N-1))   # input constraints

abs_pri_tol = 1.0e-3    # absolute primal tolerance
abs_dua_tol = 1.0e-3   # absolute dual tolerance
max_iter = 100          # maximum number of iterations
check_termination = 1   # whether to check termination and period
```

--------------------------------

### Interactive MPC Simulation

Source: https://github.com/tinympc/tinympc-python/blob/main/examples/interactive_quadrotor_ext.ipynb

Run an interactive MPC simulation using the generated C++ code. This example uses nonlinear dynamics and demonstrates setting the initial state and solving the MPC problem within a loop.

```python
x_all = []  # List of all stored states

def mpc_tasks():
    # This function is similar in firmware
    def mpc_controller(x):
        # input is 13d state vector
        q = x[3:7]
        phi = qtorp(L(qg).T @ q)
        delta_x = np.hstack([x[0:3]-rg, phi, x[7:10]-vg, x[10:13]-omgg]) # 12d state vector
        noise = np.random.normal(0, 0.01, (n,))*0
        delta_x_noise = (delta_x + noise).reshape(n)
        print(delta_x_noise)

        # 1. Set initial state from measurement    
        prob.set_x0(delta_x_noise)  # Set initial state to C code

        # 2. Set the reference state if needed
        # At step 200, set x = 1
        # if (i == 200):
        #     prob.set_xref(delta_xref_next, 0)  # Set the reference state to C code

        # 3. Solve the problem
        result = prob.solve()  # Call the solve in C code

        # 4. Get the control input
        u_mpc = result['controls']

        return np.array(u_mpc).flatten() + u0
    
    x = 1*x0  # initial state
    # x[0:3] = rg + np.random.randn(3)  # initial position
    print(x0)

    print("=== START INTERACTIVE MPC ===")

    NSIM = 100
    for i in range(NSIM):
        # Get the control input
        u_current = mpc_controller(x)
        print(u_current)
        
```

--------------------------------

### Visualize XY Trajectory with Matplotlib

Source: https://github.com/tinympc/tinympc-python/blob/main/examples/interactive_quadrotor_ext.ipynb

Plots the start, end, and reference points of a quadrotor's XY trajectory. Requires numpy and matplotlib.

```python
import matplotlib.pyplot as plt
import numpy as np

# Assuming x_all and Xref are defined numpy arrays
# x_all: contains the actual trajectory points
# Xref: contains the reference trajectory points

# Example data (replace with your actual data)
x_all = np.array([[0,0], [1,1], [2,0], [1,-1], [0,0]])
Nt = len(x_all)
N = 5 # Example value for horizon length
Xref = np.array([[0,0], [0.5,0.5], [1,1], [1.5,0.5], [2,0], [1.5,-0.5], [1,-1], [0.5,-0.5], [0,0]])

plt.scatter(x_all[0, 0], x_all[0, 1], label="xy", marker="o", linewidth=5)
plt.scatter(x_all[-1, 0], x_all[-1, 1], label="xy", marker="x", linewidth=5)
plt.plot(x_all[:, 0], x_all[:, 1], label="xy", linewidth=2)
plt.plot(Xref[1:Nt - N, 0], Xref[1:Nt - N, 1], label="xy", linewidth=2, linestyle='dashed', color='red')
plt.legend(["start", "end", "real", "ref"])
plt.xlabel("x")
plt.ylabel("y")
plt.title("xy trajectory")
plt.show()
```

--------------------------------

### Run MPC Solve and Get Solution

Source: https://context7.com/tinympc/tinympc-python/llms.txt

Executes the ADMM solver and returns a dictionary containing predicted states and controls. The first control action to apply is also provided.

```python
solver.set_x0(np.array([0.5, 0.0, 0.0, 0.0]))
solution = solver.solve()

u0       = solution["controls"]      # (nu,)  — apply this to plant
X        = solution["states_all"]    # (N, nx) — predicted state trajectory
U        = solution["controls_all"]  # (N-1, nu) — predicted input trajectory

print("First control:", u0)
# First control: [-0.1234]
```

--------------------------------

### Set Minimum CMake Version and Project Name

Source: https://github.com/tinympc/tinympc-python/blob/main/src/tinympc/codegen/pywrapper/CMakeLists.txt

Specifies the minimum required CMake version and sets the project name. This is a standard starting point for any CMake project.

```cmake
cmake_minimum_required(VERSION 3.15)
project(tinympc_codegen_ext)
```

--------------------------------

### Update Solver Settings at Runtime

Source: https://context7.com/tinympc/tinympc-python/llms.txt

Allows modification of ADMM solver parameters like tolerances and iteration limits without reconstructing the solver object. Can be called before or after `setup()`.

```python
solver.update_settings(
    abs_pri_tol=1e-4,
    abs_dua_tol=1e-4,
    max_iter=200,
    check_termination=5,      # check convergence every 5 iterations
    en_state_bound=True,
    en_input_bound=True,
)
```

--------------------------------

### Closed-loop MPC Simulation with TinyMPC

Source: https://context7.com/tinympc/tinympc-python/llms.txt

Performs a receding-horizon control loop by repeatedly setting the initial state, solving the MPC problem, and simulating the plant forward. Requires initial setup of the solver with system matrices and reference trajectory.

```python
import numpy as np
import tinympc

A = np.array([[1.0, 0.01, 0.0, 0.0],
              [0.0, 1.0, 0.039, 0.0],
              [0.0, 0.0, 1.002, 0.01],
              [0.0, 0.0, 0.458, 1.002]])
B = np.array([[0.0], [0.02], [0.0], [0.067]])
Q = np.diag([10.0, 1.0, 10.0, 1.0])
R = np.diag([1.0])

solver = tinympc.TinyMPC()
solver.setup(A, B, Q, R, N=20, rho=1.0, max_iter=10)
solver.set_x_ref(np.array([1.0, 0.0, 0.0, 0.0]))   # drive cart to x=1

x0 = np.array([0.5, 0.0, 0.0, 0.0])
states, controls = [], []

for _ in range(200):
    solver.set_x0(x0)
    sol = solver.solve()
    u0 = sol["controls"]
    x0 = A @ x0 + B @ u0    # simulate plant forward
    states.append(x0.copy())
    controls.append(u0.copy())

states   = np.array(states)    # (200, 4)
controls = np.array(controls)  # (200, 1)
print("Final state:", states[-1])
# Final state: [1.000  0.000  0.000  0.000]
```

--------------------------------

### Initialize TinyMPC

Source: https://github.com/tinympc/tinympc-python/blob/main/examples/interactive_quadrotor_ext.ipynb

Instantiate the TinyMPC class to begin setting up the optimization problem.

```python
prob = tinympc.TinyMPC()
```

--------------------------------

### Basic MPC Workflow

Source: https://github.com/tinympc/tinympc-python/blob/main/README.md

Demonstrates the fundamental steps for setting up and solving an MPC problem using the TinyMPC solver.

```APIDOC
## Basic MPC Workflow

### Description
This section outlines the typical workflow for using the TinyMPC solver, including setup, setting references, and solving the MPC problem.

### Method
Python Class Methods

### Usage
```python
import numpy as np
import tinympc

A = ...  # nx x nx
B = ...  # nx x nu
Q = ...  # nx x nx (diagonal)
R = ...  # nu x nu (diagonal)
N = 20

solver = tinympc.TinyMPC()
solver.setup(A, B, Q, R, N, rho=1.0, verbose=False)

x0 = np.array([...])
x_ref = np.zeros((A.shape[0], N))
u_ref = np.zeros((B.shape[1], N-1))

solver.set_x0(x0)
solver.set_x_ref(x_ref)
solver.set_u_ref(u_ref)

solution = solver.solve()  # dict
u0 = solution["controls"]         # first control (nu,)
X = solution["states_all"].T       # N x nx
U = solution["controls_all"].T     # (N-1) x nu
```

### Parameters
- `A` (numpy.ndarray): State matrix.
- `B` (numpy.ndarray): Input matrix.
- `Q` (numpy.ndarray): State cost matrix.
- `R` (numpy.ndarray): Input cost matrix.
- `N` (int): Horizon length.
- `rho` (float, optional): ADMM parameter. Defaults to 1.0.
- `verbose` (bool, optional): Verbosity flag. Defaults to False.

### Returns
- `solution` (dict): A dictionary containing the MPC solution, including 'controls', 'states_all', and 'controls_all'.
```

--------------------------------

### Create TinyMPC Static Library

Source: https://github.com/tinympc/tinympc-python/blob/main/src/tinympc/codegen/pywrapper/CMakeLists.txt

Builds a static library named 'tinympcstatic' from the specified source files. Static libraries are linked directly into the executable.

```cmake
add_library(tinympcstatic STATIC ${TINY_SOURCES})
```

--------------------------------

### TinyMPC() — Constructor

Source: https://context7.com/tinympc/tinympc-python/llms.txt

Instantiates a new TinyMPC solver object. This automatically imports the necessary pybind11 compiled extension.

```APIDOC
## TinyMPC() — Constructor

Instantiate a new solver object. Imports the compiled pybind11 extension (`ext_tinympc`) automatically.

```python
import tinympc

solver = tinympc.TinyMPC()
# solver.nx, solver.nu, solver.N are all 0 until setup() is called
```
```

--------------------------------

### Quadrotor Dynamics Helper Functions (Autograd)

Source: https://github.com/tinympc/tinympc-python/blob/main/examples/interactive_quadrotor_ext.ipynb

Defines quaternion operations (hat, L, qtoQ, G, rptoq, qtorp, E) and constants for quadrotor dynamics using Autograd for automatic differentiation.

```python
import autograd.numpy as np
import autograd.numpy as sqrt
from autograd.numpy.linalg import norm
from autograd.numpy.linalg import inv
from autograd import jacobian
from autograd.test_util import check_grads
np.set_printoptions(precision=3, suppress=True)

# autograd does not work with np.block

#Quaternion stuff, check `Planning with Attitude` paper for more details
def hat(v):
    return np.array([[0, -v[2], v[1]],
                     [v[2], 0, -v[0]],
                     [-v[1], v[0], 0.0]])

def L(q):
    s = q[0]
    v = q[1:4]
    up = np.hstack([s, -v])
    down = np.hstack([v.reshape(3,1), s*np.eye(3) + hat(v)])
    L = np.vstack([up,down])
    return L

T = np.diag([1.0, -1, -1, -1])
H = np.vstack([np.zeros((1,3)), np.eye(3)])

def qtoQ(q):
    return H.T @ T @ L(q) @ T @ L(q) @ H

def G(q):
    return L(q) @ H

def rptoq(phi):
    return (1./sqrt(1+phi.T @ phi)) * np.hstack([1, phi])

def qtorp(q):
    return q[1:4]/q[0]

def E(q):
    up = np.hstack([np.eye(3), np.zeros((3,3)), np.zeros((3,6))])
    mid = np.hstack([np.zeros((4,3)), G(q), np.zeros((4,6))])
    down = np.hstack([np.zeros((6,3)), np.zeros((6,3)), np.eye(6)])
    E = np.vstack([up, mid, down])
    return E

# Quadrotor parameters
mass = 0.035  # mass
J = np.array([[1.66e-5, 0.83e-6, 0.72e-6], [0.83e-6, 1.66e-5, 1.8e-6], [0.72e-6, 1.8e-6, 2.93e-5]])  # inertia
g = 9.81  # gravity
# thrustToTorque = 0.005964552
thrustToTorque = 0.0008  # thrust to torque ratio
el = 0.046/1.414213562  # arm length
scale = 65535  # PWM scale
kt = 2.245365e-6*scale # thrust coefficient, u is PWM in range [0...1], 0 is no thrust, 1 is max thrust
km = kt*thrustToTorque # moment coefficient

freq = 50.0 # >>>>>>>> CONTROL FREQUENCY <<<<<<<<<<
h = 1/freq #50 Hz 

Nx1 = 13        # number of states (quaternion) 
Nx = 12         # number of states (linearized): x, y, z, Rodriguez 3-parameters (p, q, r), vx, vy, vz, wx, wy, wz
Nu = 4          # number of controls (motor pwm signals, 0-1)
```

--------------------------------

### Code Generation

Source: https://github.com/tinympc/tinympc-python/blob/main/README.md

Explains how to generate C++ code for the MPC problem from a configured solver instance.

```APIDOC
## Code Generation

### Description
This section details the process of generating C++ source files for the MPC controller, which can be used in embedded systems or other C++ environments.

### Method
Python Class Method

### Usage
```python
solver = tinympc.TinyMPC()
solver.setup(A, B, Q, R, N, rho=1.0)

# Optional: bounds
u_min, u_max = -0.5, 0.5
solver.set_bound_constraints([], [], u_min, u_max)

solver.codegen("out")  # generates C++ sources into ./out
```

### Parameters
- `output_dir` (str): The directory where the generated C++ source files will be saved.
```

--------------------------------

### Quadrotor Figure-8 Trajectory Tracking

Source: https://github.com/tinympc/tinympc-python/blob/main/examples/interactive_quadrotor_ext.ipynb

This code snippet is intended for quadrotor trajectory tracking, specifically executing a figure-8 pattern. It assumes a linearized model around hovering and uses a 12-state reference trajectory relative to the hovering state. Input reference is not used in this example.

```python
import numpy as np
import matplotlib.pyplot as plt
from tinympc.quadrotor import Quadrotor
from tinympc.controllers import MPC
from tinympc.transforms import Rotation

# Load the quadrotor model
quad = Quadrotor()

# Load the MPC controller
mpc = MPC(quad)

# Load the trajectory
traj = np.load("figure8.npy")

# Set the reference trajectory for MPC
mpc.set_reference(traj)

# Simulate the system
mpc.simulate()

# Plot the results
mpc.plot()
plt.show()

```

--------------------------------

### Adaptive Rho (Sensitivity) Workflow

Source: https://github.com/tinympc/tinympc-python/blob/main/README.md

Compute cache terms for sensitivity analysis or generate code with precomputed sensitivity values. Requires autograd for numerical sensitivity computation.

```python
from autograd import numpy as anp

solver = tinympc.TinyMPC()
solver.setup(A, B, Q, R, N, rho=1.0)

# Option 1: numerical sensitivity (if implemented in your workflow)
# dK, dP, dC1, dC2 = solver.compute_sensitivity_autograd()

# Option 2: compute cache terms, then generate with precomputed sensitivity
Kinf, Pinf, Quu_inv, AmBKt = solver.compute_cache_terms()

# If you already have dK, dP, dC1, dC2:
# solver.codegen_with_sensitivity("out", dK, dP, dC1, dC2)
```

--------------------------------

### codegen_with_sensitivity(output_dir, dK, dP, dC1, dC2, verbose=False)

Source: https://context7.com/tinympc/tinympc-python/llms.txt

Generates C++ code similar to `codegen()`, but also embeds precomputed sensitivity matrices. This enables online adaptive-rho updates on the target hardware.

```APIDOC
## codegen_with_sensitivity(output_dir, dK, dP, dC1, dC2, verbose=False)

### Description
Like `codegen()`, but also embeds the sensitivity matrices into the generated C++ code to enable online adaptive-rho updates on the target hardware.

### Parameters
- **output_dir** (string) - Required - The directory where the C++ source files and Python extension will be generated.
- **dK** (numpy.ndarray) - Required - Precomputed derivative of Kinf with respect to rho.
- **dP** (numpy.ndarray) - Required - Precomputed derivative of Pinf with respect to rho.
- **dC1** (numpy.ndarray) - Required - Precomputed derivative of Quu_inv with respect to rho.
- **dC2** (numpy.ndarray) - Required - Precomputed derivative of AmBKt with respect to rho.
- **verbose** (boolean) - Optional - If True, prints verbose output during the code generation process.

### Request Example
```python
from autograd import jacobian
import autograd.numpy as anp
import tinympc, numpy as np

# 12-state, 4-input quadrotor
solver = tinympc.TinyMPC()
solver.setup(Adyn, Bdyn, Q, R, N=20, rho=5.0, max_iter=100,
             u_min=-np.ones(4)*2.0, u_max=np.ones(4)*2.0,
             adaptive_rho=1)

Kinf, Pinf, Quu_inv, AmBKt = solver.compute_cache_terms()
dK, dP, dC1, dC2 = solver.compute_sensitivity_autograd()

solver.codegen_with_sensitivity("out_adaptive", dK, dP, dC1, dC2, verbose=True)
```

### Output
- Generates C++ code in the specified `output_dir` with sensitivity matrices embedded in `tiny_data.hpp`.
```

--------------------------------

### solve()

Source: https://context7.com/tinympc/tinympc-python/llms.txt

Executes the MPC optimization algorithm and returns the computed optimal state and control trajectories, along with the first control action to be applied.

```APIDOC
## solve()

Runs the ADMM solver and returns a dictionary with keys `"states_all"` (shape `(N, nx)`), `"controls_all"` (shape `(N-1, nu)`), and `"controls"` (shape `(nu,)` — the first control action to apply).

```python
solver.set_x0(np.array([0.5, 0.0, 0.0, 0.0]))
solution = solver.solve()

u0       = solution["controls"]      # (nu,)  — apply this to plant
X        = solution["states_all"]    # (N, nx) — predicted state trajectory
U        = solution["controls_all"]  # (N-1, nu) — predicted input trajectory

print("First control:", u0)
# First control: [-0.1234]
```
```

--------------------------------

### Quadrotor Dynamics and Linearization (Autograd)

Source: https://github.com/tinympc/tinympc-python/blob/main/examples/interactive_quadrotor_ext.ipynb

Implements the quadrotor's nonlinear dynamics using RK4 integration and linearizes them around a hovering state using Autograd's Jacobian.

```python
# Quadrotor dynamics -- single rigid body dynamics
def quad_dynamics(x, u):
    r = x[0:3]  # position
    q = x[3:7]/norm(x[3:7])  # normalize quaternion
    v = x[7:10]  # linear velocity
    omg = x[10:13]  # angular velocity
    Q = qtoQ(q)  # quaternion to rotation matrix

    dr = v
    dq = 0.5*L(q)@H@omg
    dv = np.array([0, 0, -g]) + (1/mass)*Q@np.array([[0, 0, 0, 0], [0, 0, 0, 0], [kt, kt, kt, kt]])@u
    domg = inv(J)@(-hat(omg)@J@omg + np.array([[-el*kt, -el*kt, el*kt, el*kt], [-el*kt, el*kt, el*kt, -el*kt], [-km, km, -km, km]])@u)
    
    return np.hstack([dr, dq, dv, domg])

# RK4 integration with zero-order hold on u
def quad_dynamics_rk4(x, u):
    f1 = quad_dynamics(x, u) 
    f2 = quad_dynamics(x + 0.5*h*f1, u)
    f3 = quad_dynamics(x + 0.5*h*f2, u)
    f4 = quad_dynamics(x + h*f3, u)
    xn = x + (h/6.0)*(f1 + 2*f2 + 2*f3 + f4)
    xnormalized = xn[3:7]/norm(xn[3:7])  # normalize quaternion
    return np.hstack([xn[0:3], xnormalized, xn[7:13]])

# Linearize the dynamics around x0, u0
A_jac = jacobian(quad_dynamics_rk4, 0)  # jacobian wrt x
B_jac = jacobian(quad_dynamics_rk4, 1)  # jacobian wrt u

# Hovering state and control input
rg = np.array([0.0, 0, 0.0])
qg = np.array([1.0, 0, 0, 0])
vg = np.zeros(3)
omgg = np.zeros(3)
x0 = np.hstack([rg, qg, vg, omgg])
u0 = (mass*g/kt/4)*np.ones(4)  # ~each motor thrust to compensate for gravity
print(x0, u0)

check_grads(quad_dynamics_rk4, modes=['rev'], order=2)(x0, u0)

Anp1 = A_jac(x0, u0)  # jacobian of the dynamics wrt x at x0, u0
Bnp1 = B_jac(x0, u0)  # jacobian of the dynamics wrt u at x0, u0
```

--------------------------------

### Adaptive Rho (Sensitivity) Workflow

Source: https://github.com/tinympc/tinympc-python/blob/main/README.md

Illustrates how to compute cache terms and optionally sensitivity information for adaptive rho adjustments and code generation.

```APIDOC
## Adaptive Rho (Sensitivity) Workflow

### Description
This workflow covers computing essential cache terms and sensitivity information, which can be used for adaptive rho adjustments or generating code with precomputed sensitivity.

### Method
Python Class Methods

### Usage
```python
from autograd import numpy as anp

solver = tinympc.TinyMPC()
solver.setup(A, B, Q, R, N, rho=1.0)

# Option 1: numerical sensitivity (if implemented in your workflow)
# dK, dP, dC1, dC2 = solver.compute_sensitivity_autograd()

# Option 2: compute cache terms, then generate with precomputed sensitivity
Kinf, Pinf, Quu_inv, AmBKt = solver.compute_cache_terms()

# If you already have dK, dP, dC1, dC2:
# solver.codegen_with_sensitivity("out", dK, dP, dC1, dC2)
```

### Returns
- `Kinf` (numpy.ndarray): Infinite horizon gain.
- `Pinf` (numpy.ndarray): Infinite horizon cost-to-go matrix.
- `Quu_inv` (numpy.ndarray): Inverse of the input cost matrix.
- `AmBKt` (numpy.ndarray): System matrix related term.
```

--------------------------------

### Codegen with Sensitivity Matrices for Adaptive Rho

Source: https://context7.com/tinympc/tinympc-python/llms.txt

Employ `codegen_with_sensitivity` to embed sensitivity matrices into C++ code, enabling online adaptive-rho updates. Requires `autograd` for sensitivity computation.

```python
from autograd import jacobian
import autograd.numpy as anp
import tinympc, numpy as np

# 12-state, 4-input quadrotor
solver = tinympc.TinyMPC()
solver.setup(Adyn, Bdyn, Q, R, N=20, rho=5.0, max_iter=100,
             u_min=-np.ones(4)*2.0, u_max=np.ones(4)*2.0,
             adaptive_rho=1)

Kinf, Pinf, Quu_inv, AmBKt = solver.compute_cache_terms()
dK, dP, dC1, dC2 = solver.compute_sensitivity_autograd()

solver.codegen_with_sensitivity("out_adaptive", dK, dP, dC1, dC2, verbose=True)
# Generates ./out_adaptive/ with sensitivity matrices baked into tiny_data.hpp
```

--------------------------------

### Include Project Directories

Source: https://github.com/tinympc/tinympc-python/blob/main/src/tinympc/codegen/pywrapper/CMakeLists.txt

Adds essential include directories for the project, including custom headers and the Eigen library. Ensure these paths are correct relative to the source directory.

```cmake
include_directories(
    ${CMAKE_CURRENT_SOURCE_DIR}/include
    ${CMAKE_CURRENT_SOURCE_DIR}/include/Eigen
    ${CMAKE_CURRENT_SOURCE_DIR}/tinympc
)
```

--------------------------------

### Closed-loop MPC simulation

Source: https://context7.com/tinympc/tinympc-python/llms.txt

Demonstrates a full receding-horizon control loop using `set_x0` and `solve` at each timestep. This is the core usage pattern for closed-loop control.

```APIDOC
## Closed-loop MPC simulation

### Description
Full receding-horizon control loop using `set_x0` / `solve` at each timestep.

### Usage
```python
import numpy as np
import tinympc

A = np.array([[1.0, 0.01, 0.0, 0.0],
              [0.0, 1.0, 0.039, 0.0],
              [0.0, 0.0, 1.002, 0.01],
              [0.0, 0.0, 0.458, 1.002]])
B = np.array([[0.0], [0.02], [0.0], [0.067]])
Q = np.diag([10.0, 1.0, 10.0, 1.0])
R = np.diag([1.0])

solver = tinympc.TinyMPC()
solver.setup(A, B, Q, R, N=20, rho=1.0, max_iter=10)
solver.set_x_ref(np.array([1.0, 0.0, 0.0, 0.0]))   # drive cart to x=1

x0 = np.array([0.5, 0.0, 0.0, 0.0])
states, controls = [], []

for _ in range(200):
    solver.set_x0(x0)
    sol = solver.solve()
    u0 = sol["controls"]
    x0 = A @ x0 + B @ u0    # simulate plant forward
    states.append(x0.copy())
    controls.append(u0.copy())

states   = np.array(states)    # (200, 4)
controls = np.array(controls)  # (200, 1)
print("Final state:", states[-1])
# Final state: [1.000  0.000  0.000  0.000]
```
```