Long MPC cleanup (#22462)

* cleaner extrapolation

* some comments

* new ref

* more comments

* new ref

selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py

@@ -1,6 +1,5 @@
 #!/usr/bin/env python3
 import os
-import math
 import numpy as np
 
 from common.realtime import sec_since_boot
@@ -24,8 +23,6 @@ U_DIM = 1
 COST_E_DIM = 3
 COST_DIM = COST_E_DIM + 1
 CONSTR_DIM = 4
-MIN_ACCEL = -3.5
-
 
 X_EGO_COST = 3.
 X_EGO_E2E_COST = 10.
@@ -34,8 +31,11 @@ J_EGO_COST = 10.
 DANGER_ZONE_COST = 100.
 CRASH_DISTANCE = .5
 LIMIT_COST = 1e6
-T_IDXS = np.array(T_IDXS_LST)
 
+T_IDXS = np.array(T_IDXS_LST)
+T_DIFFS = np.diff(T_IDXS, prepend=[0.])
+
+MIN_ACCEL = -3.5
 T_REACT = 1.8
 MAX_BRAKE = 9.81
 
@@ -113,6 +113,10 @@ def gen_long_mpc_solver():
 
   desired_dist_comfort = get_safe_obstacle_distance(v_ego)
 
+  # The main cost in normal operation is how close you are to the "desired" distance
+  # from an obstacle at every timestep. This obstacle can be a lead car
+  # or other object. In e2e mode we can use x_position targets as a cost
+  # instead.
   costs = [((x_obstacle - x_ego) - (desired_dist_comfort)) / (v_ego + 10.),
            x_ego,
            a_ego,
@@ -120,6 +124,9 @@ def gen_long_mpc_solver():
   ocp.model.cost_y_expr = vertcat(*costs)
   ocp.model.cost_y_expr_e = vertcat(*costs[:-1])
 
+  # 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),
                         (a_ego - a_min),
                         (a_max - a_ego),
@@ -131,10 +138,7 @@ def gen_long_mpc_solver():
   ocp.constraints.x0 = x0
   ocp.parameter_values = np.array([-1.2, 1.2, 0.0])
 
-  # These constraints put hard limits on speed and
-  # acceleration as well as giving an assymetrical
-  # cost on approaching a lead. We only use lower
-  # bounds with an L2 cost.
+  # We put all constraint cost weights to 0 and only set them at runtime
   cost_weights = np.zeros(CONSTR_DIM)
   ocp.cost.zl = cost_weights
   ocp.cost.Zl = cost_weights
@@ -147,12 +151,17 @@ def gen_long_mpc_solver():
   ocp.constraints.uh_e = 1e4*np.ones(CONSTR_DIM)
   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
+  # 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'
   ocp.solver_options.hessian_approx = 'GAUSS_NEWTON'
   ocp.solver_options.integrator_type = 'ERK'
   ocp.solver_options.nlp_solver_type = 'SQP_RTI'
 
+  # More iterations take too much time and less lead to inaccurate convergence in
+  # some situations. Ideally we would run just 1 iteration to ensure fixed runtime.
   ocp.solver_options.qp_solver_iter_max = 4
 
   # set prediction horizon
@@ -202,9 +211,11 @@ class LongitudinalMpc():
     W = np.diag([X_EGO_COST, 0.0, A_EGO_COST, J_EGO_COST])
     Ws = np.tile(W[None], reps=(N,1,1))
     self.solver.cost_set_slice(0, N, 'W', Ws, api='old')
-    #TODO hacky weights to keep behavior the same
-    self.solver.cost_set(N, 'W', (3./5.)*np.copy(W[:COST_E_DIM, :COST_E_DIM]))
+    # Setting the slice without the copy make the array not contiguous,
+    # causing issues with the C interface.
+    self.solver.cost_set(N, 'W', np.copy(W[:COST_E_DIM, :COST_E_DIM]))
 
+    # Set L2 slack cost on lower bound constraints
     Zl = np.array([LIMIT_COST, LIMIT_COST, LIMIT_COST, DANGER_ZONE_COST])
     Zls = np.tile(Zl[None], reps=(N+1,1,1))
     self.solver.cost_set_slice(0, N+1, 'Zl', Zls, api='old')
@@ -213,8 +224,11 @@ class LongitudinalMpc():
     W = np.diag([0.0, X_EGO_E2E_COST, 0., J_EGO_COST])
     Ws = np.tile(W[None], reps=(N,1,1))
     self.solver.cost_set_slice(0, N, 'W', Ws, api='old')
+    # Setting the slice without the copy make the array not contiguous,
+    # causing issues with the C interface.
     self.solver.cost_set(N, 'W', np.copy(W[:COST_E_DIM, :COST_E_DIM]))
 
+    # Set L2 slack cost on lower bound constraints
     Zl = np.array([LIMIT_COST, LIMIT_COST, LIMIT_COST, 0.0])
     Zls = np.tile(Zl[None], reps=(N+1,1,1))
     self.solver.cost_set_slice(0, N+1, 'Zl', Zls, api='old')
@@ -229,47 +243,34 @@ class LongitudinalMpc():
       self.x0[1] = v
       self.x0[2] = a
 
-  def extrapolate_lead(self, x_lead, v_lead, a_lead_0, a_lead_tau):
-    lead_xv = np.zeros((N+1,2))
-    lead_xv[0, 0], lead_xv[0, 1] = x_lead, v_lead
-    for i in range(1, N+1):
-      dt = T_IDXS[i] - T_IDXS[i-1]
-      a_lead = a_lead_0 * math.exp(-a_lead_tau * (T_IDXS[i]**2)/2.)
-      x_lead += v_lead * dt
-      v_lead += a_lead * dt
-      if v_lead < 0.0:
-        a_lead = 0.0
-        v_lead = 0.0
-      lead_xv[i, 0], lead_xv[i, 1] = x_lead, v_lead
+  def extrapolate_lead(self, 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)
+    lead_xv = np.column_stack((x_lead_traj, v_lead_traj))
     return lead_xv
 
   def process_lead(self, lead):
     v_ego = self.x0[1]
     if lead is not None and lead.status:
       x_lead = lead.dRel
-      v_lead = max(0.0, lead.vLead)
-      a_lead = clip(lead.aLeadK, -10.0, 5.0)
-
-      # MPC will not converge if immidiate crash is expected
-      # Clip lead distance to what is still possible to brake for
-      min_x_lead = ((v_ego + v_lead)/2) * (v_ego - v_lead) / (-MIN_ACCEL * 2)
-      if x_lead < min_x_lead:
-        x_lead = min_x_lead
-
-      if (v_lead < 0.1 or -a_lead / 2.0 > v_lead):
-        v_lead = 0.0
-        a_lead = 0.0
-
-      self.a_lead_tau = lead.aLeadTau
-      lead_xv = self.extrapolate_lead(x_lead, v_lead, a_lead, self.a_lead_tau)
-
+      v_lead = lead.vLead
+      a_lead = lead.aLeadK
+      a_lead_tau = lead.aLeadTau
     else:
       # Fake a fast lead car, so mpc can keep running in the same mode
       x_lead = 50.0
       v_lead = v_ego + 10.0
       a_lead = 0.0
-      self.a_lead_tau = _LEAD_ACCEL_TAU
-      lead_xv = self.extrapolate_lead(x_lead, v_lead, a_lead, self.a_lead_tau)
+      a_lead_tau = _LEAD_ACCEL_TAU
+
+    # MPC will not converge if immediate crash is expected
+    # Clip lead distance to what is still possible to brake for
+    min_x_lead = ((v_ego + v_lead)/2) * (v_ego - v_lead) / (-MIN_ACCEL * 2)
+    x_lead = clip(x_lead, min_x_lead, 1e8)
+    v_lead = clip(v_lead, 0.0, 1e8)
+    a_lead = clip(a_lead, -10., 5.)
+    lead_xv = self.extrapolate_lead(x_lead, v_lead, a_lead, a_lead_tau)
     return lead_xv
 
   def set_accel_limits(self, min_a, max_a):
@@ -287,14 +288,14 @@ class LongitudinalMpc():
     self.params[:,0] = interp(float(self.status), [0.0, 1.0], [self.cruise_min_a, MIN_ACCEL])
     self.params[:,1] = self.cruise_max_a
 
-    # To consider a safe distance from a moving lead, we calculate how much stopping
+    # To estimate a safe distance from a moving lead, we calculate how much stopping
     # distance that lead needs as a minimum. We can add that to the current distance
     # and then treat that as a stopped car/obstacle at this new distance.
     lead_0_obstacle = lead_xv_0[:,0] + get_stopped_equivalence_factor(lead_xv_0[:,1])
     lead_1_obstacle = lead_xv_1[:,0] + get_stopped_equivalence_factor(lead_xv_1[:,1])
 
-    # Fake an obstacle for cruise
-    # TODO find cleaner way to write hacky fake cruise obstacle
+    # Fake an obstacle for cruise, this ensures smooth acceleration to set speed
+    # when the leads are no factor.
     cruise_lower_bound = v_ego + (3/4) * self.cruise_min_a * T_IDXS
     cruise_upper_bound = v_ego + (3/4) * self.cruise_max_a * T_IDXS
     v_cruise_clipped = np.clip(v_cruise * np.ones(N+1),

selfdrive/test/process_replay/ref_commit

@@ -1 +1 @@
-d643d6ff47522e00d06035ab0cb9e14d1c0c0ae0
+641884dca2102fe74e3164f8ce001cf3294b3255