LongitudinalMpc: minor clean up (#23296)

* correct order

* formatting

* Revert "formatting"

This reverts commit 481c390f40.

* use np.zeros

* typos and formatting

* typo

* typo
old-commit-hash: 5387806400

selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py

@@ -40,7 +40,7 @@ CRASH_DISTANCE = .5
 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 @@ class LongitudinalMpc():
 
   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 @@ class LongitudinalMpc():
       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 @@ class LongitudinalMpc():
     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()