From d03f9d4cf669c7f81501504487eb2e5b005db3be Mon Sep 17 00:00:00 2001 From: KRKeegan Date: Fri, 31 Dec 2021 10:31:57 -0800 Subject: [PATCH 1/3] Use vel_diff to alter follow; Lower J and X Costs at low speeds Attempt to decrease sluggish start issue. Add a test_accel file for testing sluggish starts. --- .../lib/longitudinal_mpc_lib/long_mpc.py | 36 +++++++++++++++---- 1 file changed, 30 insertions(+), 6 deletions(-) diff --git a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py index 887277a069ed79..8f8dc0823a3e18 100755 --- a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py +++ b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py @@ -16,7 +16,7 @@ # from pyextra.acados_template import AcadosOcpSolver as AcadosOcpSolverFast from selfdrive.controls.lib.longitudinal_mpc_lib.c_generated_code.acados_ocp_solver_pyx import AcadosOcpSolverFast # pylint: disable=no-name-in-module, import-error -from casadi import SX, vertcat +from casadi import SX, vertcat, if_else LONG_MPC_DIR = os.path.dirname(os.path.abspath(__file__)) EXPORT_DIR = os.path.join(LONG_MPC_DIR, "c_generated_code") @@ -55,14 +55,20 @@ COMFORT_BRAKE = 2.5 STOP_DISTANCE = 6.0 -def get_stopped_equivalence_factor(v_lead): - return (v_lead**2) / (2 * COMFORT_BRAKE) +def get_stopped_equivalence_factor(v_lead, v_ego): + # KRK add linear offset based on speed differential in attempt to + # rectify sluggish start issue + v_diff_offset = if_else(v_lead - v_ego > 0, (v_lead - v_ego) * 1, 0) # Linear offset + v_diff_offset = if_else(v_diff_offset > STOP_DISTANCE, STOP_DISTANCE, v_diff_offset) # Limit max offset + v_diff_offset = v_diff_offset * ((10 - v_ego)/10) # Offset tapers off ending at v_ego of 10m/s + distance = (v_lead**2) / (2 * COMFORT_BRAKE) + v_diff_offset + return distance def get_safe_obstacle_distance(v_ego, t_follow=T_FOLLOW): return (v_ego**2) / (2 * COMFORT_BRAKE) + t_follow * v_ego + STOP_DISTANCE def desired_follow_distance(v_ego, v_lead, t_follow=T_FOLLOW): - return get_safe_obstacle_distance(v_ego, t_follow) - get_stopped_equivalence_factor(v_lead) + return get_safe_obstacle_distance(v_ego, t_follow) - get_stopped_equivalence_factor(v_lead, v_ego) def gen_long_model(): @@ -234,6 +240,24 @@ def set_weights(self): else: self.set_weights_for_lead_policy() + def get_cost_multipliers(self): + v_ego = self.x0[1] + v_ego_bps = [0, 10] + TFs = [1.0, 1.25, T_FOLLOW] + # KRKeegan adjustments to costs for different TFs + # these were calculated using the test_longitudial.py deceleration tests + x_ego_obstacle_cost_multiplier_tf = interp(self.desired_TF, TFs, [2., 1.3, 1.]) + j_ego_cost_multiplier_tf = interp(self.desired_TF, TFs, [.1, .8, 1.]) + d_zone_cost_multiplier_tf = interp(self.desired_TF, TFs, [1.8, 1.3, 1.]) + # KRKeegan adjustments to improve sluggish acceleration these also + # alter deceleration in the same range + x_ego_obstacle_cost_multiplier_v_ego = interp(v_ego, v_ego_bps, [2., 1.]) # double + j_ego_cost_multiplier_v_ego = interp(v_ego, v_ego_bps, [.5, 1.]) # halve + # Select the appropriate min/max of the options + x_ego_obstacle_cost_multiplier = max(x_ego_obstacle_cost_multiplier_tf, x_ego_obstacle_cost_multiplier_v_ego) + j_ego_cost_multiplier = min(j_ego_cost_multiplier_tf, j_ego_cost_multiplier_v_ego) + return (x_ego_obstacle_cost_multiplier, j_ego_cost_multiplier, d_zone_cost_multiplier_tf) + def set_weights_for_lead_policy(self): # WARNING: deceleration tests with these costs: # 1.0 TR fails at 3 m/s/s test @@ -343,8 +367,8 @@ def update(self, carstate, radarstate, v_cruise, prev_accel_constraint=False): # 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], v_ego) + lead_1_obstacle = lead_xv_1[:,0] + get_stopped_equivalence_factor(lead_xv_1[:,1], v_ego) # Fake an obstacle for cruise, this ensures smooth acceleration to set speed # when the leads are no factor. From e9015cc9f612732769f60977c573e847ca7a69bf Mon Sep 17 00:00:00 2001 From: Shane Smiskol Date: Sat, 8 Jan 2022 01:49:29 -0700 Subject: [PATCH 2/3] remove function --- .../lib/longitudinal_mpc_lib/long_mpc.py | 18 ------------------ 1 file changed, 18 deletions(-) diff --git a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py index 8f8dc0823a3e18..ed642c6a1c9764 100755 --- a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py +++ b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py @@ -240,24 +240,6 @@ def set_weights(self): else: self.set_weights_for_lead_policy() - def get_cost_multipliers(self): - v_ego = self.x0[1] - v_ego_bps = [0, 10] - TFs = [1.0, 1.25, T_FOLLOW] - # KRKeegan adjustments to costs for different TFs - # these were calculated using the test_longitudial.py deceleration tests - x_ego_obstacle_cost_multiplier_tf = interp(self.desired_TF, TFs, [2., 1.3, 1.]) - j_ego_cost_multiplier_tf = interp(self.desired_TF, TFs, [.1, .8, 1.]) - d_zone_cost_multiplier_tf = interp(self.desired_TF, TFs, [1.8, 1.3, 1.]) - # KRKeegan adjustments to improve sluggish acceleration these also - # alter deceleration in the same range - x_ego_obstacle_cost_multiplier_v_ego = interp(v_ego, v_ego_bps, [2., 1.]) # double - j_ego_cost_multiplier_v_ego = interp(v_ego, v_ego_bps, [.5, 1.]) # halve - # Select the appropriate min/max of the options - x_ego_obstacle_cost_multiplier = max(x_ego_obstacle_cost_multiplier_tf, x_ego_obstacle_cost_multiplier_v_ego) - j_ego_cost_multiplier = min(j_ego_cost_multiplier_tf, j_ego_cost_multiplier_v_ego) - return (x_ego_obstacle_cost_multiplier, j_ego_cost_multiplier, d_zone_cost_multiplier_tf) - def set_weights_for_lead_policy(self): # WARNING: deceleration tests with these costs: # 1.0 TR fails at 3 m/s/s test From 773cce59b66a1eba8961d277ab821e7473339b0c Mon Sep 17 00:00:00 2001 From: Shane Smiskol Date: Sat, 8 Jan 2022 02:16:56 -0700 Subject: [PATCH 3/3] clean up function --- .../controls/lib/longitudinal_mpc_lib/long_mpc.py | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py index ed642c6a1c9764..248655e5244c70 100755 --- a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py +++ b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py @@ -16,7 +16,7 @@ # from pyextra.acados_template import AcadosOcpSolver as AcadosOcpSolverFast from selfdrive.controls.lib.longitudinal_mpc_lib.c_generated_code.acados_ocp_solver_pyx import AcadosOcpSolverFast # pylint: disable=no-name-in-module, import-error -from casadi import SX, vertcat, if_else +from casadi import SX, vertcat LONG_MPC_DIR = os.path.dirname(os.path.abspath(__file__)) EXPORT_DIR = os.path.join(LONG_MPC_DIR, "c_generated_code") @@ -58,10 +58,10 @@ def get_stopped_equivalence_factor(v_lead, v_ego): # KRK add linear offset based on speed differential in attempt to # rectify sluggish start issue - v_diff_offset = if_else(v_lead - v_ego > 0, (v_lead - v_ego) * 1, 0) # Linear offset - v_diff_offset = if_else(v_diff_offset > STOP_DISTANCE, STOP_DISTANCE, v_diff_offset) # Limit max offset - v_diff_offset = v_diff_offset * ((10 - v_ego)/10) # Offset tapers off ending at v_ego of 10m/s - distance = (v_lead**2) / (2 * COMFORT_BRAKE) + v_diff_offset + v_diff_offset = v_lead - v_ego # Linear offset + v_diff_offset *= (10 - v_ego) / 10 # Offset tapers off ending at v_ego of 10m/s + v_diff_offset = min(max(v_diff_offset, 0), STOP_DISTANCE) # Clip offset + distance = (v_lead ** 2) / (2 * COMFORT_BRAKE) + v_diff_offset return distance def get_safe_obstacle_distance(v_ego, t_follow=T_FOLLOW):