From be67fe782ca1702258bb8b1c6225db4c85476da7 Mon Sep 17 00:00:00 2001 From: Jonathan Frey Date: Tue, 1 Feb 2022 15:49:29 +0100 Subject: [PATCH] long MPC: comments, simplify set_cur_state --- .../controls/lib/longitudinal_mpc_lib/long_mpc.py | 13 ++++++------- 1 file changed, 6 insertions(+), 7 deletions(-) diff --git a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py index e8d86e8b56cd2f..d51c86539007fc 100644 --- a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py +++ b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py @@ -24,7 +24,7 @@ X_DIM = 3 U_DIM = 1 -PARAM_DIM= 4 +PARAM_DIM = 4 COST_E_DIM = 5 COST_DIM = COST_E_DIM + 1 CONSTR_DIM = 4 @@ -236,6 +236,7 @@ def set_weights_for_lead_policy(self, prev_accel_constraint=True): a_change_cost = A_CHANGE_COST if prev_accel_constraint else 0 W = np.asfortranarray(np.diag([X_EGO_OBSTACLE_COST, X_EGO_COST, V_EGO_COST, A_EGO_COST, a_change_cost, J_EGO_COST])) for i in range(N): + # reduce the cost on (a-a_prev) later in the horizon. W[4,4] = a_change_cost * np.interp(T_IDXS[i], [0.0, 1.0, 2.0], [1.0, 1.0, 0.0]) self.solver.cost_set(i, 'W', W) # Setting the slice without the copy make the array not contiguous, @@ -261,14 +262,12 @@ def set_weights_for_xva_policy(self): self.solver.cost_set(i, 'Zl', Zl) def set_cur_state(self, v, a): - if abs(self.x0[1] - v) > 2.: - self.x0[1] = v - self.x0[2] = a + v_prev = self.x0[1] + self.x0[1] = v + self.x0[2] = a + if abs(v_prev - v) > 2.: # probably only helps if v < v_prev for i in range(0, N+1): self.solver.set(i, 'x', self.x0) - else: - self.x0[1] = v - self.x0[2] = a @staticmethod def extrapolate_lead(x_lead, v_lead, a_lead, a_lead_tau):