Add cost to long plan change for smoother lag comp (commaai#22923)

* add plan changing cost

* fix compile

* set weights

* try this cost

* horizon problem

* looks pretty good

* update refs

* update refs

* smoother plan changes

selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py

@@ -26,15 +26,17 @@
 
 X_DIM = 3
 U_DIM = 1
-COST_E_DIM = 4
+PARAM_DIM= 5
+COST_E_DIM = 5
 COST_DIM = COST_E_DIM + 1
 CONSTR_DIM = 4
 
 X_EGO_OBSTACLE_COST = 3.
-V_EGO_COST = 0.
 X_EGO_COST = 0.
+V_EGO_COST = 0.
 A_EGO_COST = 0.
-J_EGO_COST = 10.
+J_EGO_COST = 5.0
+A_CHANGE_COST = .5
 DANGER_ZONE_COST = 100.
 CRASH_DISTANCE = .5
 LIMIT_COST = 1e6
@@ -88,7 +90,8 @@ def gen_long_model():
   desired_TR = SX.sym('desired_TR')
   a_min = SX.sym('a_min')
   a_max = SX.sym('a_max')
-  model.p = vertcat(a_min, a_max, x_obstacle, desired_TR)
+  prev_a = SX.sym('prev_a')
+  model.p = vertcat(a_min, a_max, x_obstacle, prev_a, desired_TR)
 
   # dynamics model
   f_expl = vertcat(v_ego, a_ego, j_ego)
@@ -121,7 +124,8 @@ def gen_long_mpc_solver():
 
   a_min, a_max = ocp.model.p[0], ocp.model.p[1]
   x_obstacle = ocp.model.p[2]
-  desired_TR = ocp.model.p[3]
+  prev_a = ocp.model.p[3]
+  desired_TR = ocp.model.p[4]
 
   ocp.cost.yref = np.zeros((COST_DIM, ))
   ocp.cost.yref_e = np.zeros((COST_E_DIM, ))
@@ -136,6 +140,7 @@ def gen_long_mpc_solver():
            x_ego,
            v_ego,
            a_ego,
+           20*(a_ego - prev_a),
            j_ego]
   ocp.model.cost_y_expr = vertcat(*costs)
   ocp.model.cost_y_expr_e = vertcat(*costs[:-1])
@@ -152,7 +157,7 @@ def gen_long_mpc_solver():
 
   x0 = np.zeros(X_DIM)
   ocp.constraints.x0 = x0
-  ocp.parameter_values = np.array([-1.2, 1.2, 0.0, T_REACT])  # defaults
+  ocp.parameter_values = np.array([-1.2, 1.2, 0.0, 0.0, T_REACT])  # defaults
 
   # We put all constraint cost weights to 0 and only set them at runtime
   cost_weights = np.zeros(CONSTR_DIM)
@@ -205,14 +210,15 @@ 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.prev_a = self.a_solution
     self.j_solution = [0.0 for i in range(N)]
     self.yref = np.zeros((N+1, COST_DIM))
     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.x_sol = np.zeros((N+1, X_DIM))
     self.u_sol = np.zeros((N,1))
-    self.params = np.zeros((N+1,4))
+    self.params = np.zeros((N+1, PARAM_DIM))
     for i in range(N+1):
       self.solver.set(i, 'x', np.zeros(X_DIM))
     self.last_cloudlog_t = 0
@@ -239,8 +245,9 @@ def set_weights_for_lead_policy(self):
     j_ego_cost_multiplier = interp(self.desired_TR, TRs, [0.5, 1.0, 1.0])
     d_zone_cost_multiplier = interp(self.desired_TR, TRs, [4., 1.0, 1.0])
 
-    W = np.asfortranarray(np.diag([X_EGO_OBSTACLE_COST * x_ego_obstacle_cost_multiplier, X_EGO_COST, V_EGO_COST, A_EGO_COST, J_EGO_COST * j_ego_cost_multiplier]))
+    W = np.asfortranarray(np.diag([X_EGO_OBSTACLE_COST * x_ego_obstacle_cost_multiplier, X_EGO_COST, V_EGO_COST, A_EGO_COST, A_CHANGE_COST, J_EGO_COST * j_ego_cost_multiplier]))
     for i in range(N):
+      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,
     # causing issues with the C interface.
@@ -252,7 +259,7 @@ def set_weights_for_lead_policy(self):
       self.solver.cost_set(i, 'Zl', Zl)
 
   def set_weights_for_xva_policy(self):
-    W = np.asfortranarray(np.diag([0., 10., 1., 10., 1.]))
+    W = np.asfortranarray(np.diag([0., 10., 1., 10., 0.0, 1.]))
     for i in range(N):
       self.solver.cost_set(i, 'W', W)
     # Setting the slice without the copy make the array not contiguous,
@@ -348,7 +355,8 @@ def update(self, carstate, radarstate, v_cruise):
     x_obstacles = np.column_stack([lead_0_obstacle, lead_1_obstacle, cruise_obstacle])
     self.source = SOURCES[np.argmin(x_obstacles[0])]
     self.params[:,2] = np.min(x_obstacles, axis=1)
-    self.params[:,3] = self.desired_TR
+    self.params[:,3] = np.copy(self.prev_a)
+    self.params[:,4] = self.desired_TR
 
     self.run()
     if (np.any(lead_xv_0[:,0] - self.x_sol[:,0] < CRASH_DISTANCE) and
@@ -369,7 +377,9 @@ def update_with_xva(self, x, v, a):
     x_obstacle = 1e5*np.ones((N+1))
     desired_TR = T_REACT*np.ones((N+1))
     self.params = np.concatenate([self.accel_limit_arr,
-                             x_obstacle[:,None], desired_TR[:,None]], axis=1)
+                             x_obstacle[:,None],
+                             self.prev_a,
+                             desired_TR[:,None]], axis=1)
     self.run()
 
 
@@ -388,6 +398,8 @@ def run(self):
     self.a_solution = self.x_sol[:,2]
     self.j_solution = self.u_sol[:,0]
 
+    self.prev_a = interp(T_IDXS + 0.05, T_IDXS, self.a_solution)
+
     t = sec_since_boot()
     if self.solution_status != 0:
       if t > self.last_cloudlog_t + 5.0:

selfdrive/test/longitudinal_maneuvers/test_longitudinal.py

@@ -62,7 +62,7 @@
     speed_lead_values=[20., 20., 0.],
     prob_lead_values=[0., 1., 1.],
     cruise_values=[20., 20., 20.],
-    breakpoints=[2., 2.01, 8.51],
+    breakpoints=[2., 2.01, 8.8],
   ),
   Maneuver(
     "approach stopped car at 20m/s",

selfdrive/test/process_replay/ref_commit

@@ -1 +1 @@
-b3c61dc5f6777497fdd82fb7421a469a43efcbf1
+6540e8c5a765975fd292b1efdef97b2d6391fa9c