Add dynamic constraints on e_y via update_bounds method.
Add show_prediction function to display prediction.master
parent
12329708a7
commit
296d1db030
34
MPC.py
34
MPC.py
|
@ -3,6 +3,10 @@ import cvxpy as cp
|
|||
import osqp
|
||||
import scipy as sp
|
||||
from scipy import sparse
|
||||
import matplotlib.pyplot as plt
|
||||
|
||||
# Colors
|
||||
PREDICTION = '#BA4A00'
|
||||
|
||||
##################
|
||||
# MPC Controller #
|
||||
|
@ -110,8 +114,9 @@ class MPC:
|
|||
|
||||
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 delta
|
||||
return u
|
||||
|
||||
def update_prediction(self, spatial_state_prediction):
|
||||
"""
|
||||
|
@ -200,6 +205,10 @@ class MPC_OSQP:
|
|||
# Offset for equality constraint (due to B * (u - ur))
|
||||
uq = np.zeros(self.N * nx)
|
||||
|
||||
# Dynamic state constraints
|
||||
xmin_dyn = np.kron(np.ones(self.N + 1), xmin)
|
||||
xmax_dyn = np.kron(np.ones(self.N + 1), xmax)
|
||||
|
||||
# Iterate over horizon
|
||||
for n in range(self.N):
|
||||
|
||||
|
@ -220,6 +229,10 @@ class MPC_OSQP:
|
|||
ur[n] = kappa_r
|
||||
# Compute equality constraint offset (B*ur)
|
||||
uq[n * nx:n * nx + nx] = B_lin[:, 0] * kappa_r
|
||||
lb, ub = self.model.reference_path.update_bounds(self.model,
|
||||
self.model.wp_id + n)
|
||||
xmin_dyn[nx * n] = lb
|
||||
xmax_dyn[nx * n] = ub
|
||||
|
||||
# Get equality matrix
|
||||
Ax = sparse.kron(sparse.eye(self.N + 1),
|
||||
|
@ -232,9 +245,9 @@ class MPC_OSQP:
|
|||
A = sparse.vstack([Aeq, Aineq], format='csc')
|
||||
|
||||
# Get upper and lower bound vectors for equality constraints
|
||||
lineq = np.hstack([np.kron(np.ones(self.N + 1), xmin),
|
||||
lineq = np.hstack([xmin_dyn,
|
||||
np.kron(np.ones(self.N), umin)])
|
||||
uineq = np.hstack([np.kron(np.ones(self.N + 1), xmax),
|
||||
uineq = np.hstack([xmax_dyn,
|
||||
np.kron(np.ones(self.N), umax)])
|
||||
# Get upper and lower bound vectors for inequality constraints
|
||||
x0 = np.array(self.model.spatial_state[:])
|
||||
|
@ -270,8 +283,9 @@ class MPC_OSQP:
|
|||
# Solve optimization problem
|
||||
dec = self.optimizer.solve()
|
||||
x = np.reshape(dec.x[:(self.N+1)*nx], (self.N+1, nx))
|
||||
u = np.arctan(dec.x[-self.N] * self.model.l)
|
||||
self.current_prediction = self.update_prediction(u, x)
|
||||
delta = np.arctan(dec.x[-self.N] * self.model.l)
|
||||
self.current_prediction = self.update_prediction(delta, x)
|
||||
u = np.array([v, delta])
|
||||
|
||||
return u
|
||||
|
||||
|
@ -289,7 +303,7 @@ class MPC_OSQP:
|
|||
# get current waypoint ID
|
||||
print('#########################')
|
||||
|
||||
for n in range(self.N):
|
||||
for n in range(2, 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, :])
|
||||
|
@ -304,3 +318,11 @@ class MPC_OSQP:
|
|||
|
||||
return x_pred, y_pred
|
||||
|
||||
def show_prediction(self):
|
||||
"""
|
||||
Display predicted car trajectory in current axis.
|
||||
"""
|
||||
|
||||
plt.scatter(self.current_prediction[0], self.current_prediction[1],
|
||||
c=PREDICTION, s=5)
|
||||
|
||||
|
|
Loading…
Reference in New Issue