diff --git a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py index 1bb6716850..c07110eefa 100644 --- a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py +++ b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py @@ -194,9 +194,6 @@ class LongitudinalMpc: def __init__(self, e2e=False): self.e2e = e2e self.reset() - self.accel_limit_arr = np.zeros((N+1, 2)) - self.accel_limit_arr[:,0] = -1.2 - self.accel_limit_arr[:,1] = 1.2 self.source = SOURCES[2] def reset(self): @@ -224,6 +221,9 @@ class LongitudinalMpc: def set_weights(self): if self.e2e: self.set_weights_for_xva_policy() + self.params[:,0] = -10. + self.params[:,1] = 10. + self.params[:,2] = 1e5 else: self.set_weights_for_lead_policy() @@ -348,15 +348,9 @@ class LongitudinalMpc: for i in range(N): self.solver.cost_set(i, "yref", self.yref[i]) self.solver.cost_set(N, "yref", self.yref[N][:COST_E_DIM]) - self.accel_limit_arr[:,0] = -10. - self.accel_limit_arr[:,1] = 10. - x_obstacle = 1e5*np.ones(N+1) - self.params = np.concatenate([self.accel_limit_arr, - x_obstacle[:, None], - self.prev_a[:,None]], axis=1) + self.params[:,3] = np.copy(self.prev_a) self.run() - def run(self): for i in range(N+1): self.solver.set(i, 'p', self.params[i])