Currently I’m doing a project about developing a nonlinear model predictive controller for generating the path of a drone with nonlinear dynamics.

The goal is to use mpc to compute the optimised control in cvxpy in such that the mpc computes the input and moves the system to reach the gaol such as below
start position (x,y,z) = [0 0 0] and end position = [10 10 10]

The problem I have is that I dont know how to formulate the nonlinear dynamic constraint in cvxpy that the mpc computes the next optimised input. The cvxpy requires a dynamic constraint in order to drive the drone toward the goal and without this, the optimised input will be always zero.

I would appreciate if you could help me solve this issue. I have been trying different approaches but not luck.

Below the complete code:

import numpy as np
import matplotlib.pyplot as plt
import cvxpy as cp
from scipy.integrate import solve_ivp

# Define parameters
m = 1.0  # mass of the quadcopter
g = 9.81  # acceleration due to gravity
Ix = 0.1  # moment of inertia around x-axis
Iy = 0.1  # moment of inertia around y-axis
Iz = 0.2  # moment of inertia around z-axis
dt = 0.05  # Time step
N = 20  # Prediction horizon

# Define nonlinear dynamics with control input
def f(x, u):
    F = np.zeros_like(x)
    F[0] = x[3]
    F[1] = x[4]
    F[2] = x[5]
    F[3] = -((np.sin(x[8]) * np.sin(x[6]) + np.cos(x[8]) * np.cos(x[6]) * np.sin(x[7]))*u[0]) / m 
    F[4] = -((np.cos(x[6]) * np.sin(x[8]) - np.cos(x[8]) * np.sin(x[6]) * np.sin(x[7])) * u[0]) / m 
    F[5] = g - ((np.cos(x[8]) * np.cos(x[7]) * u[0]) / m)
    F[6] = x[9]
    F[7] = x[10]
    F[8] = x[11]
    F[9] = ((Iy - Iz) * x[9]*x[10]/Ix) + u[1] / Ix 
    F[10] = ((Iz - Ix) * x[9]*x[10]/Iy) + u[2] / Iy 
    F[11] = ((Iz - Iy) * x[9]*x[10]/Iz) + u[3] / Iz 
    return F

# Wrapper function for solve_ivp
def f_wrapper(t, x, u):
    return f(x, u)

# Define the MPC problem
def mpc_control(x0, pf, N, Q, R):
    # Define variables
    X = cp.Variable((12, N + 1))
    U = cp.Variable((4, N))

    # Define the cost function
    cost = 0
    constraints = []
    constraints += [X[:, 0] == x0]

    for k in range(N):
        state_cost = cp.quad_form(X[:3, k] - pf, Q)
        control_cost = cp.quad_form(U[:, k], R)

        cost += state_cost + control_cost

        # Use solve_ivp to get the state at the next time step
        x_next = solve_ivp(f_wrapper, [0, dt], X[:, k].value, args=(U[:, k].value,), t_eval=[dt]).y[:, -1]
        constraints += [X[:, k+1] == x_next]
        constraints += [U[:, k] >= -10, U[:, k] <= 10]  # Control input constraints

    # Solve the optimization problem
    prob = cp.Problem(cp.Minimize(cost), constraints)
    prob.solve()

    return U[:, 0].value  # Return the first control input

# Initial conditions
p0 = [0, 0, 0]  # Initial position
v0 = [0, 0, 0]  # Initial velocity
angles0 = [0, 0, 0]  # Initial angles (phi, theta, psi)
angular_rates0 = [0, 0, 0]  # Initial angular rates (p, q, r)
x0 = np.array(p0 + v0 + angles0 + angular_rates0)

# Desired final position
pf = np.array([10, 10, 10])

# Control inputs (initially zero)
u = np.array([0, 0, 0, 0])

# Cost matrices
Q = np.eye(3) * 100
R = np.eye(4)

# Number of steps for the simulation
num_steps = 100

# Simulation loop
x = x0
positions = [x]  # Store all 12 states

for i in range(num_steps-1):
    u = mpc_control(x, pf, N, Q, R)
    print(u)
    sol = solve_ivp(f_wrapper, [0, dt], x, args=(u,), t_eval=[dt])
    x = sol.y[:, -1]`