Formulating Model Predictive Control in Gurobi (Python)

I am trying to use gurobi to solve an example model predictive control problem that is provided on the website of OSQP. The OSQP solver can solve the problem, but when I try to formulate it in gurobipy, it says the model is infeasible or unbounded. I am not sure what is wrong with my formulation and would appreciate any help. When I set the xr vector to all zeros, it can solve the problem, but when I populate the vector, it says the model is infeasible or unbounded.

OSQP python code: Model predictive control (MPC) — OSQP 0.6.2 documentation

Objective Function:

Gurobi Formulation:

import gurobipy as gp
from gurobipy import GRB

import numpy as np
import scipy.sparse as sp

# Create a new model
m = gp.Model("MPC")

# horizon
N = 10

# Create variables

x = np.array([])
z = np.array([])
u = np.array([])

for k in range(N+1):
    x = np.append(x,[m.addMVar(12, vtype=GRB.CONTINUOUS)])

for k in range(N+1):
    z = np.append(z,[m.addMVar(12, vtype=GRB.CONTINUOUS)])

for k in range(N):
    u = np.append(u,[m.addMVar(4, vtype=GRB.CONTINUOUS)])

# Bounds
u0 = 10.5916
umin = np.array([9.6, 9.6, 9.6, 9.6]) - u0
umax = np.array([13., 13., 13., 13.]) - u0
xmin = np.array([-np.pi/6,-np.pi/6,-np.inf,-np.inf,-np.inf,-1.,
-np.inf,-np.inf,-np.inf,-np.inf,-np.inf,-np.inf])
xmax = np.array([ np.pi/6, np.pi/6, np.inf, np.inf, np.inf, np.inf,
np.inf, np.inf, np.inf, np.inf, np.inf, np.inf])

# Constants
x[0] = np.zeros(12)
xr = np.array([0.,0.,1.,0.,0.,0.,0.,0.,0.,0.,0.,0.])


Q = sp.diags([0., 0., 10., 10., 10., 10., 0., 0., 0., 5., 5., 5.])

R = 0.1*sp.eye(4)


A = sp.csc_matrix([
[1., 0., 0., 0., 0., 0., 0.1, 0., 0., 0., 0., 0. ],
[0., 1., 0., 0., 0., 0., 0., 0.1, 0., 0., 0., 0. ],
[0., 0., 1., 0., 0., 0., 0., 0., 0.1, 0., 0., 0. ],
[0.0488, 0., 0., 1., 0., 0., 0.0016, 0., 0., 0.0992, 0., 0. ],
[0., -0.0488, 0., 0., 1., 0., 0., -0.0016, 0., 0., 0.0992, 0. ],
[0., 0., 0., 0., 0., 1., 0., 0., 0., 0., 0., 0.0992],
[0., 0., 0., 0., 0., 0., 1., 0., 0., 0., 0., 0. ],
[0., 0., 0., 0., 0., 0., 0., 1., 0., 0., 0., 0. ],
[0., 0., 0., 0., 0., 0., 0., 0., 1., 0., 0., 0. ],
[0.9734, 0., 0., 0., 0., 0., 0.0488, 0., 0., 0.9846, 0., 0. ],
[0., -0.9734, 0., 0., 0., 0., 0., -0.0488, 0., 0., 0.9846, 0. ],
[0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0.9846]
])

B = sp.csc_matrix([
[0., -0.0726, 0., 0.0726],
[-0.0726, 0., 0.0726, 0. ],
[-0.0152, 0.0152, -0.0152, 0.0152],
[-0., -0.0006, -0., 0.0006],
[0.0006, 0., -0.0006, 0.0000],
[0.0106, 0.0106, 0.0106, 0.0106],
[0, -1.4512, 0., 1.4512],
[-1.4512, 0., 1.4512, 0. ],
[-0.3049, 0.3049, -0.3049, 0.3049],
[-0., -0.0236, 0., 0.0236],
[0.0236, 0., -0.0236, 0. ],
[0.2107, 0.2107, 0.2107, 0.2107]])

# MPC Formulation
obj = 0
for k in range(N):
    diff = x[k] - xr
    m.addConstr(z[k] == diff)
    obj = obj + (z[k] @ Q @ z[k]) + (u[k] @ R @ u[k])
    m.addConstr(x[k+1] == A @ x[k] + B @ u[k])
    m.addConstr(umin <= u[k])
    m.addConstr(u[k] <= umax)
    m.addConstr(xmin <= x[k+1])
    m.addConstr(x[k+1] <= xmax)

diff = x[N] - xr
m.addConstr(z[N] == diff)
obj = obj + z[N] @ Q @ z[N]


m.setObjective(obj, GRB.MINIMIZE)
m.optimize()

Gurobi Output:

Eli Towle

Gurobi Staff

September 08, 2021 03:29

By default, variables added to a model using Model.addMVar() have a lower bound of \( 0 \). When I pass \( \texttt{lb=-GRB.INFINITY} \) as a keyword argument to your calls to Model.addMVar(), Gurobi finds an optimal solution.

You can write the code more concisely by (i) using 2-D MVar objects and (ii) directly passing the variable bounds to Model.addMVar(). For example:

xmin = np.tile(xmin, (N+1,1))
xmax = np.tile(xmax, (N+1,1))
umin = np.tile(umin, (N,1))
umax = np.tile(umax, (N,1))

x = m.addMVar(shape=(N+1,12), lb=xmin, ub=xmax, name='x')
z = m.addMVar(shape=(N+1,12), lb=-GRB.INFINITY, name='z')
u = m.addMVar(shape=(N,4), lb=umin, ub=umax, name='u')

m.addConstr(x[0, :] == np.zeros(12))
for k in range(N):
    m.addConstr(z[k, :] == x[k, :] - xr)
    m.addConstr(x[k+1, :] == A @ x[k, :] + B @ u[k, :])
m.addConstr(z[N, :] == x[N, :] - xr)

obj1 = sum(z[k, :] @ Q @ z[k, :] for k in range(N+1))
obj2 = sum(u[k, :] @ R @ u[k, :] for k in range(N))
m.setObjective(obj1 + obj2, GRB.MINIMIZE)

Formulating Model Predictive Control in Gurobi (Python)

Comments

Didn't find what you were looking for?