Support adjustable following distance with ACADOS

distance cost doesn't work, try removing it (may not brake enough for leads!!)

test it out

add opEdit

no static analysis

fix toyota pedal gas

fix pedal

make desired distance a live parameter (is it this easy??)

probs okay using whole arr

fixes

Try this
clean up

Revert "no static analysis"

This reverts commit a05af01.

clean up

Revert "fix pedal"

This reverts commit 54cd7c2.

Revert "fix toyota pedal gas"

This reverts commit d14bdd5.

Revert "add opEdit"

This reverts commit ade5cce.

clean up

add x_ego cost tuning

revert

uncomment

clean up

consistent name

clean up

better to differentiate

update CamryH tune

theoretically 0.9 seconds is safe up to -3 m/s/s lead braking

live tuning

update tune, rate limits not permanent

Revert "update tune, rate limits not permanent"

This reverts commit 806ffdf.

more sane costs, and set on each TR update

revert

clean up

clean up

clean up

fill xva

revert debugging code

revert debugging code

revert perms change

clean up

Author: sshane

selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py

@@ -51,14 +51,14 @@
 MAX_BRAKE = 9.81
 
 
-def get_stopped_equivalence_factor(v_lead):
-  return T_REACT * v_lead + (v_lead*v_lead) / (2 * MAX_BRAKE)
+def get_stopped_equivalence_factor(v_lead, t_react=T_REACT):
+  return t_react * v_lead + (v_lead*v_lead) / (2 * MAX_BRAKE)
 
-def get_safe_obstacle_distance(v_ego):
-  return 2 * T_REACT * v_ego + (v_ego*v_ego) / (2 * MAX_BRAKE) + 4.0
+def get_safe_obstacle_distance(v_ego, t_react=T_REACT):
+  return 2 * t_react * v_ego + (v_ego*v_ego) / (2 * MAX_BRAKE) + 4.0
 
-def desired_follow_distance(v_ego, v_lead):
-  return get_safe_obstacle_distance(v_ego) - get_stopped_equivalence_factor(v_lead)
+def desired_follow_distance(v_ego, v_lead, t_react=T_REACT):
+  return get_safe_obstacle_distance(v_ego, t_react) - get_stopped_equivalence_factor(v_lead, t_react)
 
 
 def gen_long_model():
@@ -83,9 +83,10 @@ def gen_long_model():
 
   # live parameters
   x_obstacle = SX.sym('x_obstacle')
+  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)
+  model.p = vertcat(a_min, a_max, x_obstacle, desired_TR)
 
   # dynamics model
   f_expl = vertcat(v_ego, a_ego, j_ego)
@@ -118,11 +119,12 @@ 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]
 
   ocp.cost.yref = np.zeros((COST_DIM, ))
   ocp.cost.yref_e = np.zeros((COST_E_DIM, ))
 
-  desired_dist_comfort = get_safe_obstacle_distance(v_ego)
+  desired_dist_comfort = get_safe_obstacle_distance(v_ego, desired_TR)
 
   # 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
@@ -148,7 +150,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])
+  ocp.parameter_values = np.array([-1.2, 1.2, 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)
@@ -186,8 +188,10 @@ def gen_long_mpc_solver():
 
 
 class LongitudinalMpc():
-  def __init__(self, e2e=False):
+  def __init__(self, e2e=False, desired_TR=T_REACT):
     self.e2e = e2e
+    self.desired_TR = desired_TR
+    self.v_ego = 0.
     self.reset()
     self.accel_limit_arr = np.zeros((N+1, 2))
     self.accel_limit_arr[:,0] = -1.2
@@ -205,7 +209,7 @@ def reset(self):
     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,3))
+    self.params = np.zeros((N+1,4))
     for i in range(N+1):
       self.solver.set(i, 'x', np.zeros(X_DIM))
     self.last_cloudlog_t = 0
@@ -222,15 +226,25 @@ def set_weights(self):
       self.set_weights_for_lead_policy()
 
   def set_weights_for_lead_policy(self):
-    W = np.asfortranarray(np.diag([X_EGO_OBSTACLE_COST, X_EGO_COST, V_EGO_COST, A_EGO_COST, J_EGO_COST]))
+    # WARNING: deceleration tests with these costs:
+    # 1.0 TR fails at 3 m/s/s test
+    # 1.1 TR fails at 3+ m/s/s test
+    # 1.2-1.8 TR succeeds at all tests with no FCW
+
+    TRs = [1.2, 1.8, 2.7]
+    x_ego_obstacle_cost_multiplier = interp(self.desired_TR, TRs, [3., 1.0, 0.1])
+    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]))
     for i in range(N):
       self.solver.cost_set(i, 'W', W)
     # 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])
+    Zl = np.array([LIMIT_COST, LIMIT_COST, LIMIT_COST, DANGER_ZONE_COST * d_zone_cost_multiplier])
     for i in range(N):
       self.solver.cost_set(i, 'Zl', Zl)
 
@@ -291,7 +305,12 @@ def set_accel_limits(self, min_a, max_a):
     self.cruise_min_a = min_a
     self.cruise_max_a = max_a
 
+  def set_desired_TR(self, desired_TR):
+    self.desired_TR = clip(desired_TR, 1.2, 2.7)
+    self.set_weights()
+
   def update(self, carstate, radarstate, v_cruise):
+    self.v_ego = carstate.vEgo
     v_ego = self.x0[1]
     self.status = radarstate.leadOne.status or radarstate.leadTwo.status
 
@@ -305,8 +324,8 @@ def update(self, carstate, radarstate, v_cruise):
     # 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])
+    lead_0_obstacle = lead_xv_0[:,0] + get_stopped_equivalence_factor(lead_xv_0[:,1], self.desired_TR)
+    lead_1_obstacle = lead_xv_1[:,0] + get_stopped_equivalence_factor(lead_xv_1[:,1], self.desired_TR)
 
     # Fake an obstacle for cruise, this ensures smooth acceleration to set speed
     # when the leads are no factor.
@@ -315,11 +334,12 @@ def update(self, carstate, radarstate, v_cruise):
     v_cruise_clipped = np.clip(v_cruise * np.ones(N+1),
                                cruise_lower_bound,
                                cruise_upper_bound)
-    cruise_obstacle = T_IDXS*v_cruise_clipped + get_safe_obstacle_distance(v_cruise_clipped)
+    cruise_obstacle = T_IDXS*v_cruise_clipped + get_safe_obstacle_distance(v_cruise_clipped, self.desired_TR)
 
     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.run()
     if (np.any(lead_xv_0[:,0] - self.x_sol[:,0] < CRASH_DISTANCE) and
@@ -338,8 +358,9 @@ def update_with_xva(self, x, v, a):
     self.accel_limit_arr[:,0] = -10.
     self.accel_limit_arr[:,1] = 10.
     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]], axis=1)
+                             x_obstacle[:,None], desired_TR[:,None]], axis=1)
     self.run()
 
 

selfdrive/controls/lib/longitudinal_planner.py

@@ -88,6 +88,7 @@ def update(self, sm, CP):
     accel_limits_turns[1] = max(accel_limits_turns[1], self.a_desired - 0.05)
     self.mpc.set_accel_limits(accel_limits_turns[0], accel_limits_turns[1])
     self.mpc.set_cur_state(self.v_desired, self.a_desired)
+    # self.mpc.set_desired_TR(1.8)
     self.mpc.update(sm['carState'], sm['radarState'], v_cruise)
     self.v_desired_trajectory = np.interp(T_IDXS[:CONTROL_N], T_IDXS_MPC, self.mpc.v_solution)
     self.a_desired_trajectory = np.interp(T_IDXS[:CONTROL_N], T_IDXS_MPC, self.mpc.a_solution)