Add min_TR mod to change the minimum clipped TR (commaai#114)

* add new param to change the minimum allowed TR

* update README.md

* test

* revert

* New implementation of global_df_mod, should perform much better

* rearrange

README.md

@@ -108,7 +108,8 @@ Here are the main parameters you can change with this fork:
   - `disengage_on_gas`: Whether you want openpilot to disengage on gas input or not
 - **Dynamic params**:
   - `dynamic_gas`: Whether to use [dynamic gas](#dynamic-gas) if your car is supported
-  - `global_df_mod` **`(live!)`**: The modifer for the current distance used by dynamic follow. The range is limited from 0.7 to 1.1. Smaller values will get you closer, larger will get you farther. This is applied to ALL profiles!
+  - `global_df_mod` **`(live!)`**: The multiplier for the current distance used by dynamic follow. The range is limited from 0.85 to 1.2. Smaller values will get you closer, larger will get you farther. This is applied to ALL profiles!
+  - `min_TR` **`(live!)`**: The minimum allowed following distance in seconds. Default is 0.9 seconds, the range of this mod is limited from 0.85 to 1.3 seconds. This is applied to ALL profiles!
   - `hide_auto_df_alerts`: Hides the alert that shows what profile the model has chosen
   - `dynamic_follow`: *Deprecated, use the on-screen button to change profiles*
 

common/op_params.py

@@ -45,9 +45,11 @@ def __init__(self):
                            'dynamic_follow': {'default': 'auto', 'allowed_types': [str], 'description': "Can be: ('traffic', 'relaxed', 'roadtrip'): Left to right increases in following distance.\n"
                                                                                                         "All profiles support dynamic follow so you'll get your preferred distance while\n"
                                                                                                         "retaining the smoothness and safety of dynamic follow!"},
-                           'global_df_mod': {'default': None, 'allowed_types': [type(None), float, int], 'description': 'The modifer for the current distance used by dynamic follow. The range is limited from 0.7 to 1.1\n'
+                           'global_df_mod': {'default': None, 'allowed_types': [type(None), float, int], 'description': 'The multiplier for the current distance used by dynamic follow. The range is limited from 0.85 to 1.2\n'
                                                                                                                         'Smaller values will get you closer, larger will get you farther\n'
                                                                                                                         'This is multiplied by any profile that\'s active. Set to None to disable', 'live': True},
+                           'min_TR': {'default': None, 'allowed_types': [type(None), float, int], 'description': 'The minimum allowed following distance in seconds. Default is 0.9 seconds.\n'
+                                                                                                                 'The range is limited from 0.85 to 1.3. Set to None to disable', 'live': True},
                            'alca_nudge_required': {'default': True, 'allowed_types': [bool], 'description': ('Whether to wait for applied torque to the wheel (nudge) before making lane changes. '
                                                                                                              'If False, lane change will occur IMMEDIATELY after signaling')},
                            'alca_min_speed': {'default': 25.0, 'allowed_types': [float, int], 'description': 'The minimum speed allowed for an automatic lane change (in MPH)'},

selfdrive/controls/lib/dynamic_follow/__init__.py

@@ -41,6 +41,9 @@ def __init__(self, mpc_id):
     self.v_ego_retention = 2.5
     self.v_rel_retention = 1.5
 
+    self.sng_TR = 1.8  # reacceleration stop and go TR
+    self.sng_speed = 18.0 * CV.MPH_TO_MS
+
     self._setup_collector()
     self._setup_changing_variables()
 
@@ -64,6 +67,7 @@ def _setup_changing_variables(self):
     self.last_cost = 0.0
     self.last_predict_time = 0.0
     self.auto_df_model_data = []
+    self._get_live_params()  # so they're defined just in case
 
   def update(self, CS, libmpc):
     self._get_live_params()
@@ -227,12 +231,27 @@ def _calculate_relative_accel_new(self):
         return calc_mod
     return None
 
-  def global_profile_mod(self, TR, profile_mod_pos, profile_mod_neg):
-    if self.global_df_mod is not None:  # only apply when not in sng
-      TR *= self.global_df_mod
-      profile_mod_pos *= (1 - self.global_df_mod) + 1
-      profile_mod_neg *= self.global_df_mod
-    return TR, profile_mod_pos, profile_mod_neg
+  def global_profile_mod(self, profile_mod_x, profile_mod_pos, profile_mod_neg, x_vel, y_dist):
+    """
+    This function modifies the y_dist list used by dynamic follow in accordance with global_df_mod
+    It also intelligently adjusts the profile mods at each breakpoint based on the change in TR
+    """
+    if self.global_df_mod is None:
+      return profile_mod_pos, profile_mod_neg, y_dist
+    global_df_mod = 1 - self.global_df_mod
+
+    # Calculate new TRs
+    speeds = [0, self.sng_speed, 18, x_vel[-1]]  # [0, 18 mph, ~40 mph, highest profile mod speed (~78 mph)]
+    mods = [0, 0.1, 0.7, 1]  # how much to limit global_df_mod at each speed, 1 is full effect
+    y_dist_new = [y - (y * global_df_mod * np.interp(x, speeds, mods)) for x, y in zip(x_vel, y_dist)]
+
+    # Calculate how to change profile mods based on change in TR
+    # eg. if df mod is 0.7, then increase positive mod and decrease negative mod
+    calc_profile_mods = [(np.interp(mod_x, x_vel, y_dist) - np.interp(mod_x, x_vel, y_dist_new) + 1) for mod_x in profile_mod_x]
+    profile_mod_pos = [mod_pos * mod for mod_pos, mod in zip(profile_mod_pos, calc_profile_mods)]
+    profile_mod_neg = [mod_neg * ((1 - mod) + 1) for mod_neg, mod in zip(profile_mod_neg, calc_profile_mods)]
+
+    return profile_mod_pos, profile_mod_neg, y_dist_new
 
   def _get_TR(self):
     x_vel = [0.0, 1.8627, 3.7253, 5.588, 7.4507, 9.3133, 11.5598, 13.645, 22.352, 31.2928, 33.528, 35.7632, 40.2336]  # velocities
@@ -261,29 +280,25 @@ def _get_TR(self):
     else:
       raise Exception('Unknown profile type: {}'.format(df_profile))
 
+    # Global df mod
+    profile_mod_pos, profile_mod_neg, y_dist = self.global_profile_mod(profile_mod_x, profile_mod_pos, profile_mod_neg, x_vel, y_dist)
+
     # Profile modifications - Designed so that each profile reacts similarly to changing lead dynamics
     profile_mod_pos = interp(self.car_data.v_ego, profile_mod_x, profile_mod_pos)
     profile_mod_neg = interp(self.car_data.v_ego, profile_mod_x, profile_mod_neg)
 
-    sng_TR = 1.8  # reacceleration stop and go TR
-    sng_speed = 18.0 * CV.MPH_TO_MS
-
-    if self.car_data.v_ego > sng_speed:  # keep sng distance until we're above sng speed again
+    if self.car_data.v_ego > self.sng_speed:  # keep sng distance until we're above sng speed again
       self.sng = False
 
-    if (self.car_data.v_ego >= sng_speed or self.df_data.v_egos[0]['v_ego'] >= self.car_data.v_ego) and not self.sng:
+    if (self.car_data.v_ego >= self.sng_speed or self.df_data.v_egos[0]['v_ego'] >= self.car_data.v_ego) and not self.sng:
       # if above 15 mph OR we're decelerating to a stop, keep shorter TR. when we reaccelerate, use sng_TR and slowly decrease
       TR = interp(self.car_data.v_ego, x_vel, y_dist)
-      TR, profile_mod_pos, profile_mod_neg = self.global_profile_mod(TR, profile_mod_pos, profile_mod_neg)  # only within normal driving conditions
     else:  # this allows us to get closer to the lead car when stopping, while being able to have smooth stop and go when reaccelerating
       self.sng = True
-      x = [sng_speed * 0.7, sng_speed]  # decrease TR between 12.6 and 18 mph from 1.8s to defined TR above at 18mph while accelerating
-      y = [sng_TR, interp(sng_speed, x_vel, y_dist)]
+      x = [self.sng_speed * 0.7, self.sng_speed]  # decrease TR between 12.6 and 18 mph from 1.8s to defined TR above at 18mph while accelerating
+      y = [self.sng_TR, interp(self.sng_speed, x_vel, y_dist)]
       TR = interp(self.car_data.v_ego, x, y)
 
-    # with open('/data/mpc_debug_{}.txt'.format(self.mpc_id), 'a') as f:
-    #   f.write('{}\n'.format({'profile': df_profile, 'TR': TR, 'v_ego': self.car_data.v_ego}))
-
     TR_mods = []
     # Dynamic follow modifications (the secret sauce)
     x = [-26.8224, -20.0288, -15.6871, -11.1965, -7.8645, -4.9472, -3.0541, -2.2244, -1.5045, -0.7908, -0.3196, 0.0, 0.5588, 1.3682, 1.898, 2.7316, 4.4704]  # relative velocity values
@@ -300,7 +315,7 @@ def _get_TR(self):
       if self.lead_data.v_lead - deadzone > self.car_data.v_ego:
        TR_mods.append(rel_accel_mod)
 
-    x = [sng_speed / 5.0, sng_speed]  # as we approach 0, apply x% more distance
+    x = [self.sng_speed / 5.0, self.sng_speed]  # as we approach 0, apply x% more distance
     y = [1.05, 1.0]
     profile_mod_pos *= interp(self.car_data.v_ego, x, y)  # but only for currently positive mods
 
@@ -309,13 +324,10 @@ def _get_TR(self):
 
     if self.car_data.left_blinker or self.car_data.right_blinker and df_profile != self.df_profiles.traffic:
       x = [8.9408, 22.352, 31.2928]  # 20, 50, 70 mph
-      y = [1.0, .75, .65]  # reduce TR when changing lanes
-      TR *= interp(self.car_data.v_ego, x, y)
-    min_TR_clip = 0.9
-    username = self.op_params.get('username', None)
-    if isinstance(username, str) and username.lower() in ['mmmkaay', 'ShaneSmiskol'.lower()]:
-      min_TR_clip = 0.75
-    return clip(TR, min_TR_clip, 2.7)
+      y = [1.0, .75, .65]
+      TR *= interp(self.car_data.v_ego, x, y)  # reduce TR when changing lanes
+
+    return float(clip(TR, self.min_TR, 2.7))
 
   def update_lead(self, v_lead=None, a_lead=None, x_lead=None, status=False, new_lead=False):
     self.lead_data.v_lead = v_lead
@@ -336,4 +348,10 @@ def _update_car(self, CS):
   def _get_live_params(self):
     self.global_df_mod = self.op_params.get('global_df_mod', None)
     if self.global_df_mod is not None:
-      self.global_df_mod = clip(self.global_df_mod, 0.7, 1.1)
+      self.global_df_mod = clip(self.global_df_mod, 0.85, 1.2)
+
+    self.min_TR = self.op_params.get('min_TR', None)
+    if self.min_TR is not None:
+      self.min_TR = clip(self.min_TR, 0.85, 1.3)
+    else:
+      self.min_TR = 0.9  # default