Gate acceleration on model gas press predictions (#33643)

* no ui squash

* already calibrated

* Already calibrated

* Add longitudinal maneuver tests

* Fix linter errors

* Added get_coast_accel function

---------

Co-authored-by: Bruce Wayne <harald.the.engineer@gmail.com>

selfdrive/controls/lib/longitudinal_planner.py

@@ -21,6 +21,8 @@ A_CRUISE_MIN = -1.2
 A_CRUISE_MAX_VALS = [1.6, 1.2, 0.8, 0.6]
 A_CRUISE_MAX_BP = [0., 10.0, 25., 40.]
 CONTROL_N_T_IDX = ModelConstants.T_IDXS[:CONTROL_N]
+ALLOW_THROTTLE_THRESHOLD = 0.5
+ACCEL_LIMIT_MARGIN = 0.05
 
 # Lookup table for turns
 _A_TOTAL_MAX_V = [1.7, 3.2]
@@ -30,6 +32,9 @@ _A_TOTAL_MAX_BP = [20., 40.]
 def get_max_accel(v_ego):
   return interp(v_ego, A_CRUISE_MAX_BP, A_CRUISE_MAX_VALS)
 
+def get_coast_accel(pitch):
+  return np.sin(pitch) * -5.65 - 0.3  # fitted from data using xx/projects/allow_throttle/compute_coast_accel.py
+
 
 def limit_accel_in_turns(v_ego, angle_steers, a_target, CP):
   """
@@ -69,6 +74,7 @@ class LongitudinalPlanner:
     self.mpc = LongitudinalMpc(dt=dt)
     self.fcw = False
     self.dt = dt
+    self.allow_throttle = True
 
     self.a_desired = init_a
     self.v_desired_filter = FirstOrderFilter(init_v, 2.0, self.dt)
@@ -93,11 +99,20 @@ class LongitudinalPlanner:
       v = np.zeros(len(T_IDXS_MPC))
       a = np.zeros(len(T_IDXS_MPC))
       j = np.zeros(len(T_IDXS_MPC))
-    return x, v, a, j
+    if len(model_msg.meta.disengagePredictions.gasPressProbs) > 1:
+      throttle_prob = model_msg.meta.disengagePredictions.gasPressProbs[1]
+    else:
+      throttle_prob = 1.0
+    return x, v, a, j, throttle_prob
 
   def update(self, sm):
     self.mpc.mode = 'blended' if sm['selfdriveState'].experimentalMode else 'acc'
 
+    if len(sm['carControl'].orientationNED) == 3:
+      accel_coast = get_coast_accel(sm['carControl'].orientationNED[1])
+    else:
+      accel_coast = ACCEL_MAX
+
     v_ego = sm['carState'].vEgo
     v_cruise_kph = min(sm['carState'].vCruise, V_CRUISE_MAX)
     v_cruise = v_cruise_kph * CV.KPH_TO_MS
@@ -130,6 +145,13 @@ class LongitudinalPlanner:
     self.v_desired_filter.x = max(0.0, self.v_desired_filter.update(v_ego))
     # Compute model v_ego error
     self.v_model_error = get_speed_error(sm['modelV2'], v_ego)
+    x, v, a, j, throttle_prob = self.parse_model(sm['modelV2'], self.v_model_error)
+    self.allow_throttle = throttle_prob > ALLOW_THROTTLE_THRESHOLD
+
+    if not self.allow_throttle and v_ego > 5.0:  # Don't clip at low speeds since throttle_prob doesn't account for creep
+      # MPC breaks when accel limits would cause negative velocity within the MPC horizon, so we clip the max accel limit at vEgo/T_MAX plus a bit of margin
+      clipped_accel_coast = max(accel_coast, accel_limits_turns[0], -v_ego / T_IDXS_MPC[-1] + ACCEL_LIMIT_MARGIN)
+      accel_limits_turns[1] = min(accel_limits_turns[1], clipped_accel_coast)
 
     if force_slow_decel:
       v_cruise = 0.0
@@ -140,7 +162,6 @@ class LongitudinalPlanner:
     self.mpc.set_weights(prev_accel_constraint, personality=sm['selfdriveState'].personality)
     self.mpc.set_accel_limits(accel_limits_turns[0], accel_limits_turns[1])
     self.mpc.set_cur_state(self.v_desired_filter.x, self.a_desired)
-    x, v, a, j = self.parse_model(sm['modelV2'], self.v_model_error)
     self.mpc.update(sm['radarState'], v_cruise, x, v, a, j, personality=sm['selfdriveState'].personality)
 
     self.v_desired_trajectory = np.interp(CONTROL_N_T_IDX, T_IDXS_MPC, self.mpc.v_solution)
@@ -179,6 +200,6 @@ class LongitudinalPlanner:
     longitudinalPlan.aTarget = a_target
     longitudinalPlan.shouldStop = should_stop
     longitudinalPlan.allowBrake = True
-    longitudinalPlan.allowThrottle = True
+    longitudinalPlan.allowThrottle = self.allow_throttle
 
     pm.send('longitudinalPlan', plan_send)

selfdrive/test/longitudinal_maneuvers/maneuver.py

@@ -12,11 +12,14 @@ class Maneuver:
     self.breakpoints = kwargs.get("breakpoints", [0.0, duration])
     self.speed_lead_values = kwargs.get("speed_lead_values", [0.0 for i in range(len(self.breakpoints))])
     self.prob_lead_values = kwargs.get("prob_lead_values", [1.0 for i in range(len(self.breakpoints))])
+    self.prob_throttle_values = kwargs.get("prob_throttle_values", [1.0 for i in range(len(self.breakpoints))])
     self.cruise_values = kwargs.get("cruise_values", [50.0 for i in range(len(self.breakpoints))])
+    self.pitch_values = kwargs.get("pitch_values", [0.0 for i in range(len(self.breakpoints))])
 
     self.only_lead2 = kwargs.get("only_lead2", False)
     self.only_radar = kwargs.get("only_radar", False)
     self.ensure_start = kwargs.get("ensure_start", False)
+    self.ensure_slowdown = kwargs.get("ensure_slowdown", False)
     self.enabled = kwargs.get("enabled", True)
     self.e2e = kwargs.get("e2e", False)
     self.personality = kwargs.get("personality", 0)
@@ -42,9 +45,11 @@ class Maneuver:
     logs = []
     while plant.current_time < self.duration:
       speed_lead = np.interp(plant.current_time, self.breakpoints, self.speed_lead_values)
-      prob = np.interp(plant.current_time, self.breakpoints, self.prob_lead_values)
+      prob_lead = np.interp(plant.current_time, self.breakpoints, self.prob_lead_values)
       cruise = np.interp(plant.current_time, self.breakpoints, self.cruise_values)
-      log = plant.step(speed_lead, prob, cruise)
+      pitch = np.interp(plant.current_time, self.breakpoints, self.pitch_values)
+      prob_throttle = np.interp(plant.current_time, self.breakpoints, self.prob_throttle_values)
+      log = plant.step(speed_lead, prob_lead, cruise, pitch, prob_throttle)
 
       d_rel = log['distance_lead'] - log['distance'] if self.lead_relevancy else 200.
       v_rel = speed_lead - log['speed'] if self.lead_relevancy else 0.
@@ -57,13 +62,18 @@ class Maneuver:
                             speed_lead,
                             log['acceleration']]))
 
-      if d_rel < .4 and (self.only_radar or prob > 0.5):
+      if d_rel < .4 and (self.only_radar or prob_lead > 0.5):
         print("Crashed!!!!")
         valid = False
 
       if self.ensure_start and log['v_rel'] > 0 and log['speeds'][-1] <= 0.1:
         print('LongitudinalPlanner not starting!')
         valid = False
+
+    if self.ensure_slowdown and log['speed'] > 5.5:
+      print('LongitudinalPlanner not slowing down!')
+      valid = False
+
     if self.force_decel and log['speed'] > 1e-1 and log['acceleration'] > -0.04:
       print('Not stopping with force decel')
       valid = False

selfdrive/test/longitudinal_maneuvers/plant.py

@@ -57,13 +57,14 @@ class Plant:
   def current_time(self):
     return float(self.rk.frame) / self.rate
 
-  def step(self, v_lead=0.0, prob=1.0, v_cruise=50.):
+  def step(self, v_lead=0.0, prob_lead=1.0, v_cruise=50., pitch=0.0, prob_throttle=1.0):
     # ******** publish a fake model going straight and fake calibration ********
     # note that this is worst case for MPC, since model will delay long mpc by one time step
     radar = messaging.new_message('radarState')
     control = messaging.new_message('controlsState')
     ss = messaging.new_message('selfdriveState')
     car_state = messaging.new_message('carState')
+    car_control = messaging.new_message('carControl')
     model = messaging.new_message('modelV2')
     a_lead = (v_lead - self.v_lead_prev)/self.ts
     self.v_lead_prev = v_lead
@@ -73,14 +74,14 @@ class Plant:
       v_rel = v_lead - self.speed
       if self.only_radar:
         status = True
-      elif prob > .5:
+      elif prob_lead > .5:
         status = True
       else:
         status = False
     else:
       d_rel = 200.
       v_rel = 0.
-      prob = 0.0
+      prob_lead = 0.0
       status = False
 
     lead = log.RadarState.LeadData.new_message()
@@ -94,7 +95,7 @@ class Plant:
     # TODO use real radard logic for this
     lead.aLeadTau = float(_LEAD_ACCEL_TAU)
     lead.status = status
-    lead.modelProb = float(prob)
+    lead.modelProb = float(prob_lead)
     if not self.only_lead2:
       radar.radarState.leadOne = lead
     radar.radarState.leadTwo = lead
@@ -112,6 +113,7 @@ class Plant:
     acceleration = log.XYZTData.new_message()
     acceleration.x = [float(x) for x in np.zeros_like(ModelConstants.T_IDXS)]
     model.modelV2.acceleration = acceleration
+    model.modelV2.meta.disengagePredictions.gasPressProbs = [float(prob_throttle) for _ in range(6)]
 
     control.controlsState.longControlState = LongCtrlState.pid if self.enabled else LongCtrlState.off
     ss.selfdriveState.experimentalMode = self.e2e
@@ -120,10 +122,12 @@ class Plant:
     car_state.carState.vEgo = float(self.speed)
     car_state.carState.standstill = self.speed < 0.01
     car_state.carState.vCruise = float(v_cruise * 3.6)
+    car_control.carControl.orientationNED = [0., float(pitch), 0.]
 
     # ******** get controlsState messages for plotting ***
     sm = {'radarState': radar.radarState,
           'carState': car_state.carState,
+          'carControl': car_control.carControl,
           'controlsState': control.controlsState,
           'selfdriveState': ss.selfdriveState,
           'modelV2': model.modelV2}

selfdrive/test/longitudinal_maneuvers/test_longitudinal.py

@@ -82,6 +82,44 @@ def create_maneuvers(kwargs):
       breakpoints=[0.0, 2., 2.01],
       **kwargs,
     ),
+    Maneuver(
+      "approach stopped car at 20m/s, with prob_throttle_values and pitch = -0.1",
+      duration=30.,
+      initial_speed=20.,
+      lead_relevancy=True,
+      initial_distance_lead=120.,
+      speed_lead_values=[0.0, 0., 0.],
+      prob_throttle_values=[1., 0., 0.],
+      cruise_values=[20., 20., 20.],
+      pitch_values=[0., -0.1, -0.1],
+      breakpoints=[0.0, 2., 2.01],
+      **kwargs,
+    ),
+    Maneuver(
+      "approach stopped car at 20m/s, with prob_throttle_values and pitch = +0.1",
+      duration=30.,
+      initial_speed=20.,
+      lead_relevancy=True,
+      initial_distance_lead=120.,
+      speed_lead_values=[0.0, 0., 0.],
+      prob_throttle_values=[1., 0., 0.],
+      cruise_values=[20., 20., 20.],
+      pitch_values=[0., 0.1, 0.1],
+      breakpoints=[0.0, 2., 2.01],
+      **kwargs,
+    ),
+    Maneuver(
+      "slow to 5m/s with allow_throttle = False and pitch = +0.1",
+      duration=25.,
+      initial_speed=20.,
+      lead_relevancy=False,
+      prob_throttle_values=[1., 0., 0.],
+      cruise_values=[20., 20., 20.],
+      pitch_values=[0., 0.1, 0.1],
+      breakpoints=[0.0, 2., 2.01],
+      ensure_slowdown=True,
+      **kwargs,
+    ),
     Maneuver(
       "approach slower cut-in car at 20m/s",
       duration=20.,