Remove MPC controller based on cvxpy

MPC.py

@@ -1,7 +1,5 @@
 import numpy as np
-import cvxpy as cp
 import osqp
-import scipy as sp
 from scipy import sparse
 import matplotlib.pyplot as plt
 
@@ -12,6 +10,7 @@ PREDICTION = '#BA4A00'
 # MPC Controller #
 ##################
 
+
 class MPC:
     def __init__(self, model, N, Q, R, QN, StateConstraints, InputConstraints):
         """
@@ -23,143 +22,6 @@ class MPC:
         :param QN: final state cost matrix
         :param StateConstraints: dictionary of state constraints
         :param InputConstraints: dictionary of input constraints
-        :param Reference: reference values for state variables
-        """
-
-        # Parameters
-        self.N = N  # horizon
-        self.Q = Q  # weight matrix state vector
-        self.R = R  # weight matrix input vector
-        self.QN = QN  # weight matrix terminal
-
-        # Model
-        self.model = model
-
-        # Constraints
-        self.state_constraints = StateConstraints
-        self.input_constraints = InputConstraints
-
-        # Current control and prediction
-        self.current_prediction = None
-
-        # Initialize Optimization Problem
-        self.problem = self._init_problem()
-
-    def _init_problem(self):
-        """
-        Initialize parametrized optimization problem to be solved at each
-        time step.
-        """
-
-        # number of input and state variables
-        nx = self.model.n_states
-        nu = 1
-
-        # system matrices
-        self.A = cp.Parameter(shape=(nx, nx*self.N))
-        self.B = cp.Parameter(shape=(nx, nu*self.N))
-        self.A.value = np.zeros(self.A.shape)
-        self.B.value = np.zeros(self.B.shape)
-
-        # reference values
-        xr = np.array([0., 0., -1.0])
-        self.ur = cp.Parameter((nu, self.N))
-        self.ur.value = np.zeros(self.ur.shape)
-
-        # constraints
-        umin = self.input_constraints['umin']
-        umax = self.input_constraints['umax']
-        xmin = self.state_constraints['xmin']
-        xmax = self.state_constraints['xmax']
-
-        # initial state
-        self.x_init = cp.Parameter(self.model.n_states)
-
-        # Define problem
-        self.u = cp.Variable((nu, self.N))
-        self.x = cp.Variable((nx, self.N + 1))
-        objective = 0
-        constraints = [self.x[:, 0] == self.x_init]
-        for n in range(self.N):
-            objective += cp.quad_form(self.x[:, n] - xr, self.Q) + cp.quad_form(self.u[:, n] - self.ur[:, n], self.R)
-            constraints += [self.x[:, n + 1] == self.A[:, n*nx:n*nx+nx] * self.x[:, n]
-                            + self.B[:, n*nu] * (self.u[:, n] - self.ur[:, n])]
-            constraints += [umin <= self.u[:, n], self.u[:, n] <= umax]
-        objective += cp.quad_form(self.x[:, self.N] - xr, self.QN)
-        constraints += [xmin <= self.x[:, self.N], self.x[:, self.N] <= xmax]
-        problem = cp.Problem(cp.Minimize(objective), constraints)
-
-        return problem
-
-    def get_control(self, v):
-        """
-        Get control signal given the current position of the car. Solves a
-        finite time optimization problem based on the linearized car model.
-        """
-
-        nx = self.model.n_states
-        nu = 1
-
-        for n in range(self.N):
-            current_waypoint = self.model.reference_path.waypoints[self.model.wp_id+n]
-            next_waypoint = self.model.reference_path.waypoints[
-                self.model.wp_id + n + 1]
-            delta_s = next_waypoint - current_waypoint
-            kappa_r = current_waypoint.kappa
-            self.A.value[:, n*nx:n*nx+nx], self.B.value[:, n*nu:n*nu+nu] = self.model.linearize(v, kappa_r, delta_s)
-            self.ur.value[:, n] = kappa_r
-
-        self.x_init.value = np.array(self.model.spatial_state[:])
-        self.problem.solve(solver=cp.OSQP, verbose=True)
-
-        self.current_prediction = self.update_prediction(self.x.value)
-        delta = np.arctan(self.u.value[0, 0] * self.model.l)
-        u = np.array([v, delta])
-
-        return u
-
-    def update_prediction(self, spatial_state_prediction):
-        """
-        Transform the predicted states to predicted x and y coordinates.
-        Mainly for visualization purposes.
-        :param spatial_state_prediction: list of predicted state variables
-        :return: lists of predicted x and y coordinates
-        """
-
-        # containers for x and y coordinates of predicted states
-        x_pred, y_pred = [], []
-
-        # get current waypoint ID
-        print('#########################')
-
-        for n in range(self.N):
-            associated_waypoint = self.model.reference_path.waypoints[self.model.wp_id+n]
-            predicted_temporal_state = self.model.s2t(associated_waypoint,
-                                            spatial_state_prediction[:, n])
-            print('delta: ', np.arctan(self.u.value[0, n] * self.model.l))
-            print('e_y: ', spatial_state_prediction[0, n])
-            print('e_psi: ', spatial_state_prediction[1, n])
-            print('t: ', spatial_state_prediction[2, n])
-            print('+++++++++++++++++++++++')
-
-            x_pred.append(predicted_temporal_state.x)
-            y_pred.append(predicted_temporal_state.y)
-
-        return x_pred, y_pred
-
-
-class MPC_OSQP:
-    def __init__(self, model, N, Q, R, QN, StateConstraints, InputConstraints):
-        """
-        Constructor for the Model Predictive Controller.
-        :param model: bicycle model object to be controlled
-        :param T: time horizon | int
-        :param Q: state cost matrix
-        :param R: input cost matrix
-        :param QN: final state cost matrix
-        :param StateConstraints: dictionary of state constraints
-        :param InputConstraints: dictionary of input constraints
-        :param Reference: reference values for state variables
         """
 
         # Parameters