In am implementing an MPC controller and for solving the QP I am using CVXPY. I have the issue that the feasibility of the problem depends on the cost function (this has no theoretical support) and depending on the cost matrices I choose I obtain "optimal innacurate" on "infeasible" problem.
I write the code below:
# MAIN CODE
ns = 2
ni = 1
def main():
params = Parameters_Fast()
t_end = 20.0
t_hor = 10.0
num_steps = int(t_end/params.dt)
# Initialize Controller
MPC = MPCController(parameters=params)
MPC.build_optcon_problem(t_hor=t_hor, dt=params.dt)
# Collect vectors for plots
xk = np.zeros((ns, num_steps))
uk = np.zeros((ni, num_steps-1))
Fmax = np.zeros(num_steps-1)
Fmin = np.zeros(num_steps-1)
# Initial conditions
xk[0,0] = -5.0
xk[1,0] = 1.0
# Target point
xk_tar = np.zeros(ns)
xk_tar[0] = 0.0
xk_tar[1] = 0.0
for t in range(0, num_steps-1):
# Find input constraints
Fmax[t] = params.Cm1 - params.Cd * (xk[1,t]**2) - params.Croll
Fmin[t] = -params.Cd * (xk[1,t]**2) - params.Croll
xk_opt, uk_opt = MPC.solve_optcon_problem(xk_meas=xk[:,t], xk_tar=xk_tar, Fmax=Fmax[t], Fmin=Fmin[t])
uk[:,t] = uk_opt[:,0]
xk[:,t+1] = params.A @ xk[:,t] + params.B @ uk[:,t]
# Params class
class Parameters_Fast():
dt = 0.1
# Vehicle Parameters
m = 300
Cf = 10
Cd = 2.15
Croll = 80
Cm1 = 920
A = np.array([[1, dt],
[0, 1-dt*(Cf/m)]])
B = np.array([[0],
[dt/m]])
Qo = np.diag([40, 40])
Ro = np.array([[1e-1]])
# Controller class
class MPCController():
""" MPC for steering to a set point"""
def __init__(self, parameters) -> None:
self.params = parameters
self.A = self.params.A
self.B = self.params.B
self.Qo = self.params.Qo
self.Ro = self.params.Ro
def build_optcon_problem(self, t_hor, dt):
ns = 2
ni = 1
num_steps = int(t_hor/dt)
self.xk = cp.Variable((ns, num_steps))
self.uk = cp.Variable((ni, num_steps-1))
self.x0 = cp.Parameter(ns)
self.xk_tar = cp.Parameter(ns)
self.Fmax = cp.Parameter()
self.Fmin = cp.Parameter()
constraints = []
cost = 0.0
constraints += [self.xk[:,0] == self.x0]
for t in range(0, num_steps-1):
constraints += [self.xk[:,t+1] == self.A @ self.xk[:,t] + self.B @ self.uk[:,t]]
# Input constraints
constraints += [self.uk[0,t] <= self.Fmax,
self.uk[0,t] >= self.Fmin]
# Cost function
cost += cp.quad_form(self.xk[:,t] - self.xk_tar, self.Qo) + cp.quad_form(self.uk[:,t], self.Ro)
# Zero terminal constraint
constraints += [self.xk[:,-1] == self.xk_tar]
self.prob = cp.Problem(cp.Minimize(cost), constraints)
def solve_optcon_problem(self, xk_meas, xk_tar, Fmax, Fmin):
""" Solves optimal control problem for given initial condition """
self.x0.value = xk_meas
self.xk_tar.value = xk_tar
self.Fmax.value = Fmax
self.Fmin.value = Fmin
self.prob.solve(solver = cp.OSQP, warm_start=True)
if self.prob.status != cp.OPTIMAL:
print("Error in solving optimization problem")
xk_opt = self.xk.value
uk_opt = self.uk.value
return xk_opt, uk_opt
If I change the cost Qo matrix with for example the following
Qo = np.diag([40000, 40000])
the solver returns me "Optimal innacurate" and if I increase again the value I obtain "infeasible".
While the feasibility is cost-dependent?
I need to increase and be able to change the cost matrices without them to create an infeasible problem because I have to deal with a most complicated task that requires different weights.