Remove MPC controller based on cvxpy
matssteinweg
parent
bf90a57adc
commit
73d968c2f2
140
MPC.py
140
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
|
||||
|
|
|
|||
Loading…
Reference in New Issue