diff --git a/selfdrive/controls/lib/lead_mpc_lib/lead_mpc.py b/selfdrive/controls/lib/lead_mpc_lib/lead_mpc.py index 26b4858eca..de4f1b3276 100644 --- a/selfdrive/controls/lib/lead_mpc_lib/lead_mpc.py +++ b/selfdrive/controls/lib/lead_mpc_lib/lead_mpc.py @@ -21,10 +21,10 @@ MPC_T = list(np.arange(0,1.,.2)) + list(np.arange(1.,10.6,.6)) N = len(MPC_T) - 1 -def RW(v_ego, v_l): +def desired_follow_distance(v_ego, v_lead): TR = 1.8 G = 9.81 - return (v_ego * TR - (v_l - v_ego) * TR + v_ego * v_ego / (2 * G) - v_l * v_l / (2 * G)) + return (v_ego * TR - (v_lead - v_ego) * TR + v_ego * v_ego / (2 * G) - v_lead * v_lead / (2 * G)) + 4.0 def gen_lead_model(): @@ -85,21 +85,16 @@ def gen_lead_mpc_solver(): ocp.cost.yref_e = np.zeros((3, )) x_lead, v_lead = ocp.model.p[0], ocp.model.p[1] - G = 9.81 - TR = 1.8 - desired_dist = (v_ego * TR - - (v_lead - v_ego) * TR - + v_ego*v_ego/(2*G) - - v_lead * v_lead / (2*G)) - dist_err = (desired_dist + 4.0 - (x_lead - x_ego))/(sqrt(v_ego + 0.5) + 0.1) + desired_dist = desired_follow_distance(v_ego, v_lead) + dist_err = (desired_dist - (x_lead - x_ego))/(sqrt(v_ego + 0.5) + 0.1) # TODO hacky weights to keep behavior the same ocp.model.cost_y_expr = vertcat(exp(.3 * dist_err) - 1., - ((x_lead - x_ego) - (desired_dist + 4.0)) / (0.05 * v_ego + 0.5), + ((x_lead - x_ego) - (desired_dist)) / (0.05 * v_ego + 0.5), a_ego * (.1 * v_ego + 1.0), j_ego * (.1 * v_ego + 1.0)) ocp.model.cost_y_expr_e = vertcat(exp(.3 * dist_err) - 1., - ((x_lead - x_ego) - (desired_dist + 4.0)) / (0.05 * v_ego + 0.5), + ((x_lead - x_ego) - (desired_dist)) / (0.05 * v_ego + 0.5), a_ego * (.1 * v_ego + 1.0)) ocp.parameter_values = np.array([0., .0]) diff --git a/selfdrive/controls/tests/test_following_distance.py b/selfdrive/controls/tests/test_following_distance.py index e8701a42c0..69fb471e11 100644 --- a/selfdrive/controls/tests/test_following_distance.py +++ b/selfdrive/controls/tests/test_following_distance.py @@ -5,7 +5,7 @@ import numpy as np from cereal import log import cereal.messaging as messaging from selfdrive.config import Conversions as CV -from selfdrive.controls.lib.lead_mpc_lib.lead_mpc import RW, LeadMpc +from selfdrive.controls.lib.lead_mpc_lib.lead_mpc import desired_follow_distance, LeadMpc class FakePubMaster(): @@ -68,7 +68,7 @@ class TestFollowingDistance(unittest.TestCase): v_lead = float(speed_mph * CV.MPH_TO_MS) simulation_steady_state = run_following_distance_simulation(v_lead) - correct_steady_state = RW(v_lead, v_lead) + 4.0 + correct_steady_state = desired_follow_distance(v_lead, v_lead) self.assertAlmostEqual(simulation_steady_state, correct_steady_state, delta=.2) diff --git a/selfdrive/test/longitudinal_maneuvers/maneuver.py b/selfdrive/test/longitudinal_maneuvers/maneuver.py index 9de546a6c0..8bf6754039 100644 --- a/selfdrive/test/longitudinal_maneuvers/maneuver.py +++ b/selfdrive/test/longitudinal_maneuvers/maneuver.py @@ -9,8 +9,9 @@ class Maneuver(): self.speed = kwargs.get("initial_speed", 0.0) self.lead_relevancy = kwargs.get("lead_relevancy", 0) - self.speed_lead_values = kwargs.get("speed_lead_values", [0.0, 0.0]) self.speed_lead_breakpoints = kwargs.get("speed_lead_breakpoints", [0.0, duration]) + self.speed_lead_values = kwargs.get("speed_lead_values", [0.0, 0.0]) + self.prob_lead_values = kwargs.get("prob_lead_values", [1.0 for i in range(len(self.speed_lead_breakpoints))]) self.only_lead2 = kwargs.get("only_lead2", False) self.only_radar = kwargs.get("only_radar", False) @@ -31,13 +32,19 @@ class Maneuver(): logs = [] while plant.current_time() < self.duration: speed_lead = np.interp(plant.current_time(), self.speed_lead_breakpoints, self.speed_lead_values) - log = plant.step(speed_lead) + prob = np.interp(plant.current_time(), self.speed_lead_breakpoints, self.prob_lead_values) + log = plant.step(speed_lead, prob) 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. log['d_rel'] = d_rel log['v_rel'] = v_rel - logs.append(np.array([plant.current_time(), log['distance'], log['distance_lead'], log['speed'], speed_lead, log['acceleration']])) + logs.append(np.array([plant.current_time(), + log['distance'], + log['distance_lead'], + log['speed'], + speed_lead, + log['acceleration']])) if d_rel < 1.0: print("Crashed!!!!") diff --git a/selfdrive/test/longitudinal_maneuvers/plant.py b/selfdrive/test/longitudinal_maneuvers/plant.py index 02df860ccd..6cdd5bf828 100755 --- a/selfdrive/test/longitudinal_maneuvers/plant.py +++ b/selfdrive/test/longitudinal_maneuvers/plant.py @@ -48,7 +48,7 @@ class Plant(): def current_time(self): return float(self.rk.frame) / self.rate - def step(self, v_lead=0.0): + def step(self, v_lead=0.0, prob=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') @@ -61,10 +61,11 @@ class Plant(): d_rel = np.maximum(0., self.distance_lead - self.distance) v_rel = v_lead - self.speed if self.only_radar: - prob = 0.0 + status = True + elif prob > .5: + status = True else: - prob = 1.0 - status = True + status = False else: d_rel = 200. v_rel = 0. @@ -81,7 +82,7 @@ class Plant(): lead.aLeadK = float(a_lead) lead.aLeadTau = float(1.5) lead.status = status - lead.modelProb = prob + lead.modelProb = float(prob) if not self.only_lead2: radar.radarState.leadOne = lead radar.radarState.leadTwo = lead diff --git a/selfdrive/test/longitudinal_maneuvers/test_longitudinal.py b/selfdrive/test/longitudinal_maneuvers/test_longitudinal.py index 8ea1af572f..ddd6a2b61d 100755 --- a/selfdrive/test/longitudinal_maneuvers/test_longitudinal.py +++ b/selfdrive/test/longitudinal_maneuvers/test_longitudinal.py @@ -88,6 +88,7 @@ maneuvers = [ lead_relevancy=True, initial_distance_lead=10., speed_lead_values=[0., 0.], + prob_lead_values=[0., 0.], speed_lead_breakpoints=[1., 11.], only_radar=True, ),