From 3e885e238c95fbfbb50e360097cf402805e1fa07 Mon Sep 17 00:00:00 2001 From: firestar5683 <168790843+firestar5683@users.noreply.github.com> Date: Wed, 8 Apr 2026 18:00:02 -0500 Subject: [PATCH] Updated Bolt Tunes --- selfdrive/controls/lib/latcontrol_torque.py | 31 +++++++++++++-------- selfdrive/controls/tests/test_latcontrol.py | 4 +++ tools/tuning/analyze_bolt_lateral.py | 25 +++++++++++++++++ 3 files changed, 49 insertions(+), 11 deletions(-) diff --git a/selfdrive/controls/lib/latcontrol_torque.py b/selfdrive/controls/lib/latcontrol_torque.py index 3c4a6ff7..34137e03 100644 --- a/selfdrive/controls/lib/latcontrol_torque.py +++ b/selfdrive/controls/lib/latcontrol_torque.py @@ -72,20 +72,23 @@ BOLT_2017_UNWIND_TAPER_RIGHT = 0.28 BOLT_2018_2021_LATERAL_TESTING_GROUND_ID = testing_ground.id_4 BOLT_2018_2021_STEER_RATIO_TEST_SCALE = 1.01 -BOLT_2018_2021_TORQUE_GAIN_LEFT = 0.085 -BOLT_2018_2021_TORQUE_GAIN_RIGHT = 0.055 +BOLT_2018_2021_TORQUE_GAIN_LEFT = 0.090 +BOLT_2018_2021_TORQUE_GAIN_RIGHT = 0.050 BOLT_2018_2021_TORQUE_ONSET = 0.18 BOLT_2018_2021_TORQUE_ONSET_WIDTH = 0.08 BOLT_2018_2021_TORQUE_CUTOFF = 1.05 BOLT_2018_2021_TORQUE_CUTOFF_WIDTH = 0.24 BOLT_2018_2021_JERK_TAPER_CUTOFF = 0.42 +BOLT_2018_2021_CENTER_TAPER_LAT = 0.12 +BOLT_2018_2021_CENTER_TAPER_WIDTH = 0.04 +BOLT_2018_2021_CENTER_TAPER_GAIN = 0.35 BOLT_2018_2021_TRANSITION_SPEED = 8.5 BOLT_2018_2021_PHASE_SCALE = 0.10 BOLT_2018_2021_TURN_IN_BOOST_LEFT = 0.22 BOLT_2018_2021_TURN_IN_BOOST_RIGHT = 0.12 BOLT_2018_2021_UNWIND_TAPER_GAIN_LEFT = 0.72 BOLT_2018_2021_UNWIND_TAPER_GAIN_RIGHT = 0.84 -BOLT_2018_2021_FRICTION_MULT = 1.03 +BOLT_2018_2021_FRICTION_MULT = 1.01 BOLT_2018_2021_FRICTION_LAT_RISE = 0.24 BOLT_2018_2021_FRICTION_JERK_RISE = 0.28 BOLT_2018_2021_TURN_IN_THRESHOLD_REDUCTION_LEFT = 0.16 @@ -108,19 +111,19 @@ BOLT_2022_2023_TRANSITION_SPEED = 9.0 BOLT_2022_2023_PHASE_SCALE = 0.12 BOLT_2022_2023_TURN_IN_BOOST_LEFT = 0.12 BOLT_2022_2023_TURN_IN_BOOST_RIGHT = 0.06 -BOLT_2022_2023_UNWIND_TAPER_LEFT = 0.24 -BOLT_2022_2023_UNWIND_TAPER_RIGHT = 0.20 +BOLT_2022_2023_UNWIND_TAPER_LEFT = 0.28 +BOLT_2022_2023_UNWIND_TAPER_RIGHT = 0.24 BOLT_2022_2023_FRICTION_MULT = 1.15 BOLT_2022_2023_FRICTION_LAT_RISE = 0.22 BOLT_2022_2023_FRICTION_JERK_RISE = 0.26 BOLT_2022_2023_TURN_IN_THRESHOLD_REDUCTION_LEFT = 0.14 BOLT_2022_2023_TURN_IN_THRESHOLD_REDUCTION_RIGHT = 0.08 -BOLT_2022_2023_UNWIND_THRESHOLD_INCREASE_LEFT = 0.16 -BOLT_2022_2023_UNWIND_THRESHOLD_INCREASE_RIGHT = 0.13 +BOLT_2022_2023_UNWIND_THRESHOLD_INCREASE_LEFT = 0.20 +BOLT_2022_2023_UNWIND_THRESHOLD_INCREASE_RIGHT = 0.16 BOLT_2022_2023_TURN_IN_FRICTION_BOOST_LEFT = 0.08 BOLT_2022_2023_TURN_IN_FRICTION_BOOST_RIGHT = 0.04 -BOLT_2022_2023_UNWIND_FRICTION_REDUCTION_LEFT = 0.17 -BOLT_2022_2023_UNWIND_FRICTION_REDUCTION_RIGHT = 0.14 +BOLT_2022_2023_UNWIND_FRICTION_REDUCTION_LEFT = 0.21 +BOLT_2022_2023_UNWIND_FRICTION_REDUCTION_RIGHT = 0.17 def get_friction_threshold(v_ego: float) -> float: @@ -208,7 +211,11 @@ def get_bolt_2018_2021_torque_scale(desired_lateral_accel: float) -> float: def get_bolt_2018_2021_dynamic_torque_scale(desired_lateral_accel: float, desired_lateral_jerk: float, v_ego: float) -> float: base_scale = get_bolt_2018_2021_torque_scale(desired_lateral_accel) extra_scale = max(base_scale - 1.0, 0.0) + abs_lateral_accel = abs(desired_lateral_accel) low_speed_factor = _bolt_2018_2021_low_speed_factor(v_ego) + high_speed_factor = 1.0 - low_speed_factor + center_window = _bolt_2018_2021_sigmoid((BOLT_2018_2021_CENTER_TAPER_LAT - abs_lateral_accel) / BOLT_2018_2021_CENTER_TAPER_WIDTH) + center_taper = 1.0 - (BOLT_2018_2021_CENTER_TAPER_GAIN * high_speed_factor * center_window) phase = _bolt_2018_2021_transition_phase(desired_lateral_accel, desired_lateral_jerk) turn_in_weight = max(phase, 0.0) jerk_taper = 1.0 / (1.0 + (abs(desired_lateral_jerk) / BOLT_2018_2021_JERK_TAPER_CUTOFF) ** 2) @@ -217,7 +224,7 @@ def get_bolt_2018_2021_dynamic_torque_scale(desired_lateral_accel: float, desire unwind_weight = max(-phase, 0.0) unwind_taper = 1.0 - (_bolt_2018_2021_side_value(desired_lateral_accel, BOLT_2018_2021_UNWIND_TAPER_GAIN_LEFT, BOLT_2018_2021_UNWIND_TAPER_GAIN_RIGHT) * unwind_weight * (0.55 + 0.45 * low_speed_factor)) - return 1.0 + (extra_scale * jerk_taper * turn_in_boost * max(unwind_taper, 0.0)) + return 1.0 + (extra_scale * center_taper * jerk_taper * turn_in_boost * max(unwind_taper, 0.0)) def get_bolt_2018_2021_friction_threshold(v_ego: float, desired_lateral_accel: float = 0.0, desired_lateral_jerk: float = 0.0) -> float: @@ -282,13 +289,15 @@ def get_bolt_2022_2023_ff_scale(desired_lateral_accel: float, desired_lateral_je cutoff = _bolt_2022_2023_sigmoid((BOLT_2022_2023_FF_CUTOFF - abs_lateral_accel) / BOLT_2022_2023_FF_CUTOFF_WIDTH) extra_scale = gain * onset * cutoff low_speed_factor = _bolt_2022_2023_low_speed_factor(v_ego) + transition_envelope = _bolt_2022_2023_transition_envelope(v_ego, desired_lateral_accel, desired_lateral_jerk) phase = _bolt_2022_2023_transition_phase(desired_lateral_accel, desired_lateral_jerk) turn_in_weight = max(phase, 0.0) unwind_weight = max(-phase, 0.0) turn_in_boost = 1.0 + (_bolt_2022_2023_side_value(desired_lateral_accel, BOLT_2022_2023_TURN_IN_BOOST_LEFT, BOLT_2022_2023_TURN_IN_BOOST_RIGHT) * turn_in_weight * low_speed_factor) + unwind_envelope = (0.25 + 0.75 * low_speed_factor) * (1.0 + 0.45 * transition_envelope) unwind_taper = 1.0 - (_bolt_2022_2023_side_value(desired_lateral_accel, BOLT_2022_2023_UNWIND_TAPER_LEFT, BOLT_2022_2023_UNWIND_TAPER_RIGHT) * - unwind_weight * (0.55 + 0.45 * low_speed_factor)) + unwind_weight * unwind_envelope) return 1.0 + (extra_scale * turn_in_boost * max(unwind_taper, 0.0)) diff --git a/selfdrive/controls/tests/test_latcontrol.py b/selfdrive/controls/tests/test_latcontrol.py index fc137ea7..a9b3000c 100644 --- a/selfdrive/controls/tests/test_latcontrol.py +++ b/selfdrive/controls/tests/test_latcontrol.py @@ -66,6 +66,7 @@ class TestLatControl: assert get_bolt_2018_2021_torque_scale(0.2) > get_bolt_2018_2021_torque_scale(0.08) assert get_bolt_2018_2021_torque_scale(0.4) > get_bolt_2018_2021_torque_scale(-0.4) assert get_bolt_2018_2021_torque_scale(2.0) < get_bolt_2018_2021_torque_scale(0.8) + assert get_bolt_2018_2021_dynamic_torque_scale(0.08, 0.0, 25.0) < get_bolt_2018_2021_dynamic_torque_scale(0.08, 0.0, 8.0) assert get_bolt_2018_2021_dynamic_torque_scale(0.4, 0.8, 20.0) < get_bolt_2018_2021_dynamic_torque_scale(0.4, 0.1, 20.0) assert get_bolt_2018_2021_dynamic_torque_scale(0.6, -0.6, 8.0) < get_bolt_2018_2021_dynamic_torque_scale(0.6, 0.6, 8.0) assert get_bolt_2018_2021_dynamic_torque_scale(-0.6, 0.6, 8.0) < get_bolt_2018_2021_dynamic_torque_scale(-0.6, -0.6, 8.0) @@ -81,10 +82,12 @@ class TestLatControl: def test_bolt_2018_2021_friction_scale_curve(self): base = get_bolt_2018_2021_friction_scale(25.0, 0.7, 0.8) + center_base = get_bolt_2018_2021_friction_scale(25.0, 0.0, 0.0) left_turn_in = get_bolt_2018_2021_friction_scale(6.0, 0.7, 0.8) right_turn_in = get_bolt_2018_2021_friction_scale(6.0, -0.7, -0.8) left_unwind = get_bolt_2018_2021_friction_scale(6.0, 0.7, -0.8) right_unwind = get_bolt_2018_2021_friction_scale(6.0, -0.7, 0.8) + assert center_base < 1.02 assert left_turn_in > right_turn_in > base assert base > left_unwind > right_unwind @@ -94,6 +97,7 @@ class TestLatControl: assert get_bolt_2022_2023_ff_scale(0.6, 0.7, 8.0) > get_bolt_2022_2023_ff_scale(-0.6, -0.7, 8.0) assert get_bolt_2022_2023_ff_scale(-0.6, -0.7, 8.0) > get_bolt_2022_2023_ff_scale(-0.6, 0.0, 8.0) assert get_bolt_2022_2023_ff_scale(0.6, -0.7, 8.0) < get_bolt_2022_2023_ff_scale(0.6, 0.0, 8.0) + assert get_bolt_2022_2023_ff_scale(0.6, -0.7, 6.0) < get_bolt_2022_2023_ff_scale(0.6, -0.7, 20.0) def test_bolt_2022_2023_friction_threshold_curve(self): base = get_friction_threshold(6.0) diff --git a/tools/tuning/analyze_bolt_lateral.py b/tools/tuning/analyze_bolt_lateral.py index ea210542..ac936b2f 100644 --- a/tools/tuning/analyze_bolt_lateral.py +++ b/tools/tuning/analyze_bolt_lateral.py @@ -16,6 +16,7 @@ class ControlSample: steering_pressed: bool lat_active: bool saturated: bool + roll_rad: float actual_la: float desired_la: float desired_jerk: float @@ -25,6 +26,9 @@ class ControlSample: torque_cmd: float +LOW_ROLL_THRESHOLD_DEG = 1.5 + + def siglin_torque(lat_accel: float, params: dict[str, list[float]]) -> float: side = "left" if lat_accel >= 0.0 else "right" a, b, c, d = params[side] @@ -42,6 +46,7 @@ def summarize_control_samples(samples: list[ControlSample]) -> None: steering_pressed = np.array([s.steering_pressed for s in samples], dtype=bool) lat_active = np.array([s.lat_active for s in samples], dtype=bool) saturated = np.array([s.saturated for s in samples], dtype=bool) + roll = np.array([s.roll_rad for s in samples]) actual = np.array([s.actual_la for s in samples]) desired = np.array([s.desired_la for s in samples]) jerk = np.array([s.desired_jerk for s in samples]) @@ -50,8 +55,20 @@ def summarize_control_samples(samples: list[ControlSample]) -> None: f_term = np.array([s.f_term for s in samples]) torque_cmd = np.array([s.torque_cmd for s in samples]) + roll_valid = np.isfinite(roll) + low_roll = roll_valid & (np.abs(roll) <= math.radians(LOW_ROLL_THRESHOLD_DEG)) + high_roll = roll_valid & (~low_roll) base = lat_active & (~steering_pressed) & (v > 8.0) transition_base = lat_active & (~steering_pressed) & (v > 4.0) & (~saturated) + if np.any(roll_valid): + print("\nRoll context:") + print( + f" valid={int(roll_valid.sum()):5d}/{len(samples):5d} " + f"abs_mean_deg={np.mean(np.degrees(np.abs(roll[roll_valid]))):.3f} " + f"abs_p95_deg={np.percentile(np.degrees(np.abs(roll[roll_valid])), 95):.3f} " + f"low_roll_deg<={LOW_ROLL_THRESHOLD_DEG:.1f}" + ) + masks = ( ("all", base), ("all_non_sat", base & (~saturated)), @@ -60,6 +77,11 @@ def summarize_control_samples(samples: list[ControlSample]) -> None: ("center", base & (~saturated) & (np.abs(desired) < 0.1)), ("steady_left", base & (~saturated) & (desired >= 0.1) & (np.abs(jerk) < 0.2)), ("steady_right", base & (~saturated) & (desired <= -0.1) & (np.abs(jerk) < 0.2)), + ("steady_left_low_roll", base & (~saturated) & low_roll & (desired >= 0.1) & (np.abs(jerk) < 0.2)), + ("steady_right_low_roll", base & (~saturated) & low_roll & (desired <= -0.1) & (np.abs(jerk) < 0.2)), + ("center_low_roll", base & (~saturated) & low_roll & (np.abs(desired) < 0.1)), + ("steady_left_high_roll", base & (~saturated) & high_roll & (desired >= 0.1) & (np.abs(jerk) < 0.2)), + ("steady_right_high_roll", base & (~saturated) & high_roll & (desired <= -0.1) & (np.abs(jerk) < 0.2)), ("low_speed_sharp", transition_base & (v < 14.0) & (np.abs(desired) >= 0.4) & (np.abs(jerk) >= 0.35)), ("turn_in", transition_base & (v < 14.0) & (np.abs(desired) >= 0.4) & (np.abs(jerk) >= 0.35) & ((desired * jerk) > 0.0)), ("unwind", transition_base & (v < 14.0) & (np.abs(desired) >= 0.4) & (np.abs(jerk) >= 0.35) & ((desired * jerk) < 0.0)), @@ -144,6 +166,8 @@ def main() -> None: if which in ("carState", "carControl"): latest[which] = getattr(msg, which) + elif which == "liveParameters": + latest[which] = msg.liveParameters elif which == "controlsState" and "carState" in latest and "carControl" in latest: lateral_state = msg.controlsState.lateralControlState if lateral_state.which() == "torqueState": @@ -153,6 +177,7 @@ def main() -> None: steering_pressed=latest["carState"].steeringPressed, lat_active=latest["carControl"].latActive, saturated=torque_state.saturated, + roll_rad=float(getattr(latest.get("liveParameters"), "roll", float("nan"))), actual_la=torque_state.actualLateralAccel, desired_la=torque_state.desiredLateralAccel, desired_jerk=torque_state.desiredLateralJerk,