|
|
@ -1,4 +1,3 @@ |
|
|
|
import math |
|
|
|
|
|
|
|
import numpy as np |
|
|
|
import numpy as np |
|
|
|
from common.realtime import sec_since_boot, DT_MDL |
|
|
|
from common.realtime import sec_since_boot, DT_MDL |
|
|
|
from common.numpy_fast import interp |
|
|
|
from common.numpy_fast import interp |
|
|
@ -186,7 +185,7 @@ class LateralPlanner: |
|
|
|
self.x0[3] = interp(DT_MDL, self.t_idxs[:LAT_MPC_N + 1], self.lat_mpc.x_sol[:, 3]) |
|
|
|
self.x0[3] = interp(DT_MDL, self.t_idxs[:LAT_MPC_N + 1], self.lat_mpc.x_sol[:, 3]) |
|
|
|
|
|
|
|
|
|
|
|
# Check for infeasible MPC solution |
|
|
|
# Check for infeasible MPC solution |
|
|
|
mpc_nans = any(math.isnan(x) for x in self.lat_mpc.x_sol[:, 3]) |
|
|
|
mpc_nans = np.isnan(self.lat_mpc.x_sol[:, 3]).any() |
|
|
|
t = sec_since_boot() |
|
|
|
t = sec_since_boot() |
|
|
|
if mpc_nans or self.lat_mpc.solution_status != 0: |
|
|
|
if mpc_nans or self.lat_mpc.solution_status != 0: |
|
|
|
self.reset_mpc() |
|
|
|
self.reset_mpc() |
|
|
|