From c288a7792ce8ddfa47ac98fa4ec3f6708606fea3 Mon Sep 17 00:00:00 2001 From: Jason Wen Date: Tue, 23 Jul 2024 23:30:01 -0400 Subject: [PATCH] super fast take off!!! --- .../lib/longitudinal_mpc_lib/long_mpc.py | 19 +++++++++++++++---- 1 file changed, 15 insertions(+), 4 deletions(-) diff --git a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py index a2df527a55..0762e204ee 100755 --- a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py +++ b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py @@ -105,6 +105,17 @@ def get_dynamic_personality(v_ego, personality=custom.LongitudinalPersonalitySP. def get_stopped_equivalence_factor(v_lead): return (v_lead**2) / (2 * COMFORT_BRAKE) +def get_stopped_equivalence_factor_krkeegen(v_lead, v_ego): + # KRKeegan this offset rapidly decreases the following distance when the lead pulls + # away, resulting in an early demand for acceleration. + v_diff_offset = 0 + if np.all(v_lead - v_ego > 0): + v_diff_offset = ((v_lead - v_ego) * 1.) + v_diff_offset = np.clip(v_diff_offset, 0, STOP_DISTANCE / 2) + v_diff_offset = np.maximum(v_diff_offset * ((10 - v_ego)/10), 0) + distance = (v_lead**2) / (2 * COMFORT_BRAKE) + v_diff_offset + return distance + def get_safe_obstacle_distance(v_ego, t_follow): return (v_ego**2) / (2 * COMFORT_BRAKE) + t_follow * v_ego + STOP_DISTANCE @@ -302,11 +313,11 @@ class LongitudinalMpc: def set_weights(self, prev_accel_constraint=True, personality=custom.LongitudinalPersonalitySP.standard): jerk_factor = get_jerk_factor(personality) if self.mode == 'acc': - a_change_cost = A_CHANGE_COST if prev_accel_constraint else 0 + a_change_cost = A_CHANGE_COST if prev_accel_constraint else 40 cost_weights = [X_EGO_OBSTACLE_COST, X_EGO_COST, V_EGO_COST, A_EGO_COST, jerk_factor * a_change_cost, jerk_factor * J_EGO_COST] constraint_cost_weights = [LIMIT_COST, LIMIT_COST, LIMIT_COST, DANGER_ZONE_COST] elif self.mode == 'blended': - a_change_cost = 40.0 if prev_accel_constraint else 0 + a_change_cost = 40.0 if prev_accel_constraint else 40 cost_weights = [0., 0.1, 0.2, 5.0, a_change_cost, 1.0] constraint_cost_weights = [LIMIT_COST, LIMIT_COST, LIMIT_COST, 50.0] else: @@ -369,8 +380,8 @@ class LongitudinalMpc: # 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_krkeegen(lead_xv_0[:,1], v_ego) + lead_1_obstacle = lead_xv_1[:,0] + get_stopped_equivalence_factor_krkeegen(lead_xv_1[:,1], v_ego) cruise_target_e2ex = T_IDXS * np.clip(v_cruise, v_ego - 2.0, 1e3) + x[0] e2e_xforward = ((v[1:] + v[:-1]) / 2) * (T_IDXS[1:] - T_IDXS[:-1])