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 osqp
|
||||||
import scipy as sp
|
import scipy as sp
|
||||||
from scipy import sparse
|
from scipy import sparse
|
||||||
|
import matplotlib.pyplot as plt
|
||||||
|
|
||||||
|
# Colors
|
||||||
|
PREDICTION = '#BA4A00'
|
||||||
|
|
||||||
##################
|
##################
|
||||||
# MPC Controller #
|
# MPC Controller #
|
||||||
|
@ -110,8 +114,9 @@ class MPC:
|
||||||
|
|
||||||
self.current_prediction = self.update_prediction(self.x.value)
|
self.current_prediction = self.update_prediction(self.x.value)
|
||||||
delta = np.arctan(self.u.value[0, 0] * self.model.l)
|
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):
|
def update_prediction(self, spatial_state_prediction):
|
||||||
"""
|
"""
|
||||||
|
@ -200,6 +205,10 @@ class MPC_OSQP:
|
||||||
# Offset for equality constraint (due to B * (u - ur))
|
# Offset for equality constraint (due to B * (u - ur))
|
||||||
uq = np.zeros(self.N * nx)
|
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
|
# Iterate over horizon
|
||||||
for n in range(self.N):
|
for n in range(self.N):
|
||||||
|
|
||||||
|
@ -220,6 +229,10 @@ class MPC_OSQP:
|
||||||
ur[n] = kappa_r
|
ur[n] = kappa_r
|
||||||
# Compute equality constraint offset (B*ur)
|
# Compute equality constraint offset (B*ur)
|
||||||
uq[n * nx:n * nx + nx] = B_lin[:, 0] * kappa_r
|
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
|
# Get equality matrix
|
||||||
Ax = sparse.kron(sparse.eye(self.N + 1),
|
Ax = sparse.kron(sparse.eye(self.N + 1),
|
||||||
|
@ -232,9 +245,9 @@ class MPC_OSQP:
|
||||||
A = sparse.vstack([Aeq, Aineq], format='csc')
|
A = sparse.vstack([Aeq, Aineq], format='csc')
|
||||||
|
|
||||||
# Get upper and lower bound vectors for equality constraints
|
# 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)])
|
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)])
|
np.kron(np.ones(self.N), umax)])
|
||||||
# Get upper and lower bound vectors for inequality constraints
|
# Get upper and lower bound vectors for inequality constraints
|
||||||
x0 = np.array(self.model.spatial_state[:])
|
x0 = np.array(self.model.spatial_state[:])
|
||||||
|
@ -270,8 +283,9 @@ class MPC_OSQP:
|
||||||
# Solve optimization problem
|
# Solve optimization problem
|
||||||
dec = self.optimizer.solve()
|
dec = self.optimizer.solve()
|
||||||
x = np.reshape(dec.x[:(self.N+1)*nx], (self.N+1, nx))
|
x = np.reshape(dec.x[:(self.N+1)*nx], (self.N+1, nx))
|
||||||
u = np.arctan(dec.x[-self.N] * self.model.l)
|
delta = np.arctan(dec.x[-self.N] * self.model.l)
|
||||||
self.current_prediction = self.update_prediction(u, x)
|
self.current_prediction = self.update_prediction(delta, x)
|
||||||
|
u = np.array([v, delta])
|
||||||
|
|
||||||
return u
|
return u
|
||||||
|
|
||||||
|
@ -289,7 +303,7 @@ class MPC_OSQP:
|
||||||
# get current waypoint ID
|
# get current waypoint ID
|
||||||
print('#########################')
|
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]
|
associated_waypoint = self.model.reference_path.waypoints[self.model.wp_id+n]
|
||||||
predicted_temporal_state = self.model.s2t(associated_waypoint,
|
predicted_temporal_state = self.model.s2t(associated_waypoint,
|
||||||
spatial_state_prediction[n, :])
|
spatial_state_prediction[n, :])
|
||||||
|
@ -304,3 +318,11 @@ class MPC_OSQP:
|
||||||
|
|
||||||
return x_pred, y_pred
|
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