From 5387806400ff9258e158e0400683a2c9a5e6c043 Mon Sep 17 00:00:00 2001 From: Shane Smiskol Date: Tue, 28 Dec 2021 04:05:52 -0700 Subject: [PATCH] LongitudinalMpc: minor clean up (#23296) * correct order * formatting * Revert "formatting" This reverts commit 481c390f400179f5d0d8f9b5a3066cb68e484d0c. * use np.zeros * typos and formatting * typo * typo --- .../lib/longitudinal_mpc_lib/long_mpc.py | 25 ++++++++++--------- 1 file changed, 13 insertions(+), 12 deletions(-) diff --git a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py index 1b0afe1cf7beb1..1bb67168507ff0 100644 --- a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py +++ b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py @@ -40,7 +40,7 @@ LIMIT_COST = 1e6 -# Less timestamps doesn't hurt performance and leads to +# Fewer timestamps don't hurt performance and lead to # much better convergence of the MPC with low iterations N = 12 MAX_T = 10.0 @@ -84,9 +84,9 @@ def gen_long_model(): model.xdot = vertcat(x_ego_dot, v_ego_dot, a_ego_dot) # live parameters - x_obstacle = SX.sym('x_obstacle') a_min = SX.sym('a_min') a_max = SX.sym('a_max') + x_obstacle = SX.sym('x_obstacle') prev_a = SX.sym('prev_a') model.p = vertcat(a_min, a_max, x_obstacle, prev_a) @@ -143,8 +143,8 @@ def gen_long_mpc_solver(): # Constraints on speed, acceleration and desired distance to # the obstacle, which is treated as a slack constraint so it - # behaves like an assymetrical cost. - constraints = vertcat((v_ego), + # behaves like an asymmetrical cost. + constraints = vertcat(v_ego, (a_ego - a_min), (a_max - a_ego), ((x_obstacle - x_ego) - (3/4) * (desired_dist_comfort)) / (v_ego + 10.)) @@ -169,7 +169,7 @@ def gen_long_mpc_solver(): ocp.constraints.idxsh = np.arange(CONSTR_DIM) # The HPIPM solver can give decent solutions even when it is stopped early - # Which is critical for our purpose where the compute time is strictly bounded + # Which is critical for our purpose where compute time is strictly bounded # We use HPIPM in the SPEED_ABS mode, which ensures fastest runtime. This # does not cause issues since the problem is well bounded. ocp.solver_options.qp_solver = 'PARTIAL_CONDENSING_HPIPM' @@ -190,7 +190,7 @@ def gen_long_mpc_solver(): return ocp -class LongitudinalMpc(): +class LongitudinalMpc: def __init__(self, e2e=False): self.e2e = e2e self.reset() @@ -201,10 +201,10 @@ def __init__(self, e2e=False): def reset(self): self.solver = AcadosOcpSolverFast('long', N, EXPORT_DIR) - self.v_solution = [0.0 for i in range(N+1)] - self.a_solution = [0.0 for i in range(N+1)] + self.v_solution = np.zeros(N+1) + self.a_solution = np.zeros(N+1) self.prev_a = np.array(self.a_solution) - self.j_solution = [0.0 for i in range(N)] + self.j_solution = np.zeros(N) self.yref = np.zeros((N+1, COST_DIM)) for i in range(N): self.solver.cost_set(i, "yref", self.yref[i]) @@ -264,7 +264,8 @@ def set_cur_state(self, v, a): self.x0[1] = v self.x0[2] = a - def extrapolate_lead(self, x_lead, v_lead, a_lead, a_lead_tau): + @staticmethod + def extrapolate_lead(x_lead, v_lead, a_lead, a_lead_tau): a_lead_traj = a_lead * np.exp(-a_lead_tau * (T_IDXS**2)/2.) v_lead_traj = np.clip(v_lead + np.cumsum(T_DIFFS * a_lead_traj), 0.0, 1e8) x_lead_traj = x_lead + np.cumsum(T_DIFFS * v_lead_traj) @@ -351,8 +352,8 @@ def update_with_xva(self, x, v, a): 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) + x_obstacle[:, None], + self.prev_a[:,None]], axis=1) self.run()