From 08ac1b33ba49d5f6711aaedf4c4bf414e2364ecf Mon Sep 17 00:00:00 2001
From: HaraldSchafer <harald.the.engineer@gmail.com>
Date: Thu, 2 Dec 2021 14:42:13 -0800
Subject: [PATCH] Human inspired follow distance (#22937)

* Add cost for creep

* more reasonable cost

* seems better than before

* science tune

* more chill

* closer follow

* stopping takes some leeway

* better cruise

* needed to be cumsum all along

* jerk not even needed

* matches better

* 6 is too much

* add back

* a little extra buffer is good for badly tuned cars

* new refs

* refs again
old-commit-hash: 46fd5892bf41add3dc15d413bced81693d0f8853
---
 .../lib/longitudinal_mpc_lib/long_mpc.py      | 21 +++++++++----------
 .../controls/tests/test_following_distance.py |  2 +-
 selfdrive/test/process_replay/ref_commit      |  2 +-
 3 files changed, 12 insertions(+), 13 deletions(-)

diff --git a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py
index 33b996a59c..32dac71285 100644
--- a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py
+++ b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py
@@ -49,15 +49,14 @@ T_IDXS_LST = [index_function(idx, max_val=MAX_T, max_idx=N+1) for idx in range(N
 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
-
+T_REACT = 1.45
+COMFORT_BRAKE = 2.0
 
 def get_stopped_equivalence_factor(v_lead):
-  return T_REACT * v_lead + (v_lead*v_lead) / (2 * MAX_BRAKE)
+  return v_lead**2 / (2 * COMFORT_BRAKE) - T_REACT * v_lead
 
 def get_safe_obstacle_distance(v_ego):
-  return 2 * T_REACT * v_ego + (v_ego*v_ego) / (2 * MAX_BRAKE) + 4.0
+  return (v_ego*v_ego) / (2 * COMFORT_BRAKE) + 6.0
 
 def desired_follow_distance(v_ego, v_lead):
   return get_safe_obstacle_distance(v_ego) - get_stopped_equivalence_factor(v_lead)
@@ -242,7 +241,7 @@ class LongitudinalMpc():
       self.solver.cost_set(i, 'Zl', Zl)
 
   def set_weights_for_xva_policy(self):
-    W = np.asfortranarray(np.diag([0., 10., 1., 10., 0.0, 1.]))
+    W = np.asfortranarray(np.diag([0., 10., 1., 10., 1.]))
     for i in range(N):
       self.solver.cost_set(i, 'W', W)
     # Setting the slice without the copy make the array not contiguous,
@@ -318,12 +317,12 @@ class LongitudinalMpc():
 
     # 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_lower = v_ego + (T_IDXS * self.cruise_min_a * 1.05)
+    v_upper = v_ego + (T_IDXS * self.cruise_max_a * 1.05)
     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)
+                               v_lower,
+                               v_upper)
+    cruise_obstacle = np.cumsum(T_DIFFS * v_cruise_clipped) + get_safe_obstacle_distance(v_cruise_clipped)
 
     x_obstacles = np.column_stack([lead_0_obstacle, lead_1_obstacle, cruise_obstacle])
     self.source = SOURCES[np.argmin(x_obstacles[0])]
diff --git a/selfdrive/controls/tests/test_following_distance.py b/selfdrive/controls/tests/test_following_distance.py
index 5b3fa65228..6661847b7f 100644
--- a/selfdrive/controls/tests/test_following_distance.py
+++ b/selfdrive/controls/tests/test_following_distance.py
@@ -29,7 +29,7 @@ class TestFollowingDistance(unittest.TestCase):
       simulation_steady_state = run_following_distance_simulation(v_lead)
       correct_steady_state = desired_follow_distance(v_lead, v_lead)
 
-      self.assertAlmostEqual(simulation_steady_state, correct_steady_state, delta=(correct_steady_state*.1 + .3))
+      self.assertAlmostEqual(simulation_steady_state, correct_steady_state, delta=(correct_steady_state*.1 + 1.0))
 
 
 if __name__ == "__main__":
diff --git a/selfdrive/test/process_replay/ref_commit b/selfdrive/test/process_replay/ref_commit
index 2996331fe4..914a17bdb1 100644
--- a/selfdrive/test/process_replay/ref_commit
+++ b/selfdrive/test/process_replay/ref_commit
@@ -1 +1 @@
-e15e446543ea74cce4864f07902a39bdb8b10688
\ No newline at end of file
+fb4318e66ce98c9bc267b0778d9238265c1c5b17
\ No newline at end of file