From d21e351003a6f1df08676b06ba2eb61c9e061180 Mon Sep 17 00:00:00 2001 From: Jason Wen Date: Thu, 20 Mar 2025 16:01:08 -0400 Subject: [PATCH] Controls: Lateral Accel Torque Control Extension (#690) * init * more init * keep it alive * fixes * more fixes * more fix * new submodule for nn data * bump submodule * update path to submodule * spacing??? * update submodule path * update submodule path * bump * dump * bump * introduce params * Add Neural Network Lateral Control toggle to developer panel This introduces a new toggle for enabling Neural Network Lateral Control (NNLC), providing detailed descriptions of its functionality and compatibility. It includes UI integration, car compatibility checks, and feedback links for unsupported vehicles. * decouple even more * static * codespell * remove debug * in structs * fix import * convert to capnp * fixes * debug * only initialize if NNLC is enabled or allow to enable * oops * fix initialization * only allow engage if nnlc is off * fix toggle param * fix tests * lint * fix more test * capnp test * try this out * validate if it's not None * make it 33 to match * align * share the same friction input calculation * return stock values if not enabled * unused * split base and child * space * rename * NeuralNetworkFeedForwardModel * less * just use file name * try this * more explicit * rename * move it * child class for additional controllers * rename * time to split out custom lateral acceleration * move around * space * fix * TODO-SP * TODO-SP * split nnlc and custom lat accel * more * not yet * comment * fix --------- Co-authored-by: DevTekVE --- selfdrive/car/tests/test_car_interfaces.py | 8 +- selfdrive/controls/controlsd.py | 8 +- selfdrive/controls/lib/latcontrol.py | 2 +- selfdrive/controls/lib/latcontrol_angle.py | 4 +- selfdrive/controls/lib/latcontrol_pid.py | 4 +- selfdrive/controls/lib/latcontrol_torque.py | 13 +- .../controls/lib/tests/test_latcontrol.py | 4 +- .../controls/lib/latcontrol_torque_ext.py | 26 ++++ .../lib/latcontrol_torque_ext_base.py | 121 ++++++++++++++++++ 9 files changed, 175 insertions(+), 15 deletions(-) create mode 100644 sunnypilot/selfdrive/controls/lib/latcontrol_torque_ext.py create mode 100644 sunnypilot/selfdrive/controls/lib/latcontrol_torque_ext_base.py diff --git a/selfdrive/car/tests/test_car_interfaces.py b/selfdrive/car/tests/test_car_interfaces.py index 9e6b96ba5..d5aa53249 100644 --- a/selfdrive/car/tests/test_car_interfaces.py +++ b/selfdrive/car/tests/test_car_interfaces.py @@ -42,7 +42,7 @@ class TestCarInterfaces: car_params = CarInterface.get_params(car_name, args['fingerprints'], args['car_fw'], experimental_long=args['experimental_long'], docs=False) car_params_sp = CarInterface.get_params_sp(car_params, car_name, args['fingerprints'], args['car_fw'], - experimental_long=args['experimental_long'], docs=False) + experimental_long=args['experimental_long'], docs=False) car_params = car_params.as_reader() car_interface = CarInterface(car_params, car_params_sp) assert car_params @@ -100,8 +100,8 @@ class TestCarInterfaces: # hypothesis also slows down significantly with just one more message draw LongControl(car_params) if car_params.steerControlType == CarParams.SteerControlType.angle: - LatControlAngle(car_params, car_interface) + LatControlAngle(car_params, car_params_sp, car_interface) elif car_params.lateralTuning.which() == 'pid': - LatControlPID(car_params, car_interface) + LatControlPID(car_params, car_params_sp, car_interface) elif car_params.lateralTuning.which() == 'torque': - LatControlTorque(car_params, car_interface) + LatControlTorque(car_params, car_params_sp, car_interface) diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index f9ba54bfa..294722c06 100755 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -55,11 +55,11 @@ class Controls: self.VM = VehicleModel(self.CP) self.LaC: LatControl if self.CP.steerControlType == car.CarParams.SteerControlType.angle: - self.LaC = LatControlAngle(self.CP, self.CI) + self.LaC = LatControlAngle(self.CP, self.CP_SP, self.CI) elif self.CP.lateralTuning.which() == 'pid': - self.LaC = LatControlPID(self.CP, self.CI) + self.LaC = LatControlPID(self.CP, self.CP_SP, self.CI) elif self.CP.lateralTuning.which() == 'torque': - self.LaC = LatControlTorque(self.CP, self.CI) + self.LaC = LatControlTorque(self.CP, self.CP_SP, self.CI) def update(self): self.sm.update(15) @@ -85,6 +85,8 @@ class Controls: self.LaC.update_live_torque_params(torque_params.latAccelFactorFiltered, torque_params.latAccelOffsetFiltered, torque_params.frictionCoefficientFiltered) + self.LaC.extension.update_model_v2(self.sm['modelV2']) + long_plan = self.sm['longitudinalPlan'] model_v2 = self.sm['modelV2'] diff --git a/selfdrive/controls/lib/latcontrol.py b/selfdrive/controls/lib/latcontrol.py index dcf000343..6bbce95bb 100644 --- a/selfdrive/controls/lib/latcontrol.py +++ b/selfdrive/controls/lib/latcontrol.py @@ -7,7 +7,7 @@ MIN_LATERAL_CONTROL_SPEED = 0.3 # m/s class LatControl(ABC): - def __init__(self, CP, CI): + def __init__(self, CP, CP_SP, CI): self.sat_count_rate = 1.0 * DT_CTRL self.sat_limit = CP.steerLimitTimer self.sat_count = 0. diff --git a/selfdrive/controls/lib/latcontrol_angle.py b/selfdrive/controls/lib/latcontrol_angle.py index 1b249a3d1..7bd0abc11 100644 --- a/selfdrive/controls/lib/latcontrol_angle.py +++ b/selfdrive/controls/lib/latcontrol_angle.py @@ -7,8 +7,8 @@ STEER_ANGLE_SATURATION_THRESHOLD = 2.5 # Degrees class LatControlAngle(LatControl): - def __init__(self, CP, CI): - super().__init__(CP, CI) + def __init__(self, CP, CP_SP, CI): + super().__init__(CP, CP_SP, CI) self.sat_check_min_speed = 5. def update(self, active, CS, VM, params, steer_limited_by_controls, desired_curvature, calibrated_pose, curvature_limited): diff --git a/selfdrive/controls/lib/latcontrol_pid.py b/selfdrive/controls/lib/latcontrol_pid.py index 1f1199565..eb8a1ed5f 100644 --- a/selfdrive/controls/lib/latcontrol_pid.py +++ b/selfdrive/controls/lib/latcontrol_pid.py @@ -6,8 +6,8 @@ from openpilot.common.pid import PIDController class LatControlPID(LatControl): - def __init__(self, CP, CI): - super().__init__(CP, CI) + def __init__(self, CP, CP_SP, CI): + super().__init__(CP, CP_SP, CI) self.pid = PIDController((CP.lateralTuning.pid.kpBP, CP.lateralTuning.pid.kpV), (CP.lateralTuning.pid.kiBP, CP.lateralTuning.pid.kiV), k_f=CP.lateralTuning.pid.kf, pos_limit=self.steer_max, neg_limit=-self.steer_max) diff --git a/selfdrive/controls/lib/latcontrol_torque.py b/selfdrive/controls/lib/latcontrol_torque.py index 3aef57bac..613824a4e 100644 --- a/selfdrive/controls/lib/latcontrol_torque.py +++ b/selfdrive/controls/lib/latcontrol_torque.py @@ -7,6 +7,8 @@ from opendbc.car.vehicle_model import ACCELERATION_DUE_TO_GRAVITY from openpilot.selfdrive.controls.lib.latcontrol import LatControl from openpilot.common.pid import PIDController +from openpilot.sunnypilot.selfdrive.controls.lib.latcontrol_torque_ext import LatControlTorqueExt + # At higher speeds (25+mph) we can assume: # Lateral acceleration achieved by a specific car correlates to # torque applied to the steering rack. It does not correlate to @@ -23,8 +25,8 @@ LOW_SPEED_Y = [15, 13, 10, 5] class LatControlTorque(LatControl): - def __init__(self, CP, CI): - super().__init__(CP, CI) + def __init__(self, CP, CP_SP, CI): + super().__init__(CP, CP_SP, CI) self.torque_params = CP.lateralTuning.torque.as_builder() self.pid = PIDController(self.torque_params.kp, self.torque_params.ki, k_f=self.torque_params.kf, pos_limit=self.steer_max, neg_limit=-self.steer_max) @@ -32,6 +34,8 @@ class LatControlTorque(LatControl): self.use_steering_angle = self.torque_params.useSteeringAngle self.steering_angle_deadzone_deg = self.torque_params.steeringAngleDeadzoneDeg + self.extension = LatControlTorqueExt(self, CP, CP_SP) + def update_live_torque_params(self, latAccelFactor, latAccelOffset, friction): self.torque_params.latAccelFactor = latAccelFactor self.torque_params.latAccelOffset = latAccelOffset @@ -73,6 +77,11 @@ class LatControlTorque(LatControl): desired_lateral_accel - actual_lateral_accel, lateral_accel_deadzone, friction_compensation=True, gravity_adjusted=True) + # Lateral acceleration torque controller extension updates + # Overrides stock ff and pid_log.error + ff, pid_log = self.extension.update(CS, VM, params, ff, pid_log, setpoint, measurement, calibrated_pose, roll_compensation, + desired_lateral_accel, actual_lateral_accel, lateral_accel_deadzone, gravity_adjusted_lateral_accel) + freeze_integrator = steer_limited_by_controls or CS.steeringPressed or CS.vEgo < 5 output_torque = self.pid.update(pid_log.error, feedforward=ff, diff --git a/selfdrive/controls/lib/tests/test_latcontrol.py b/selfdrive/controls/lib/tests/test_latcontrol.py index 299652ff1..aee020c02 100644 --- a/selfdrive/controls/lib/tests/test_latcontrol.py +++ b/selfdrive/controls/lib/tests/test_latcontrol.py @@ -6,6 +6,7 @@ from opendbc.car.honda.values import CAR as HONDA from opendbc.car.toyota.values import CAR as TOYOTA from opendbc.car.nissan.values import CAR as NISSAN from opendbc.car.vehicle_model import VehicleModel +from openpilot.selfdrive.car.helpers import convert_to_capnp from openpilot.selfdrive.controls.lib.latcontrol_pid import LatControlPID from openpilot.selfdrive.controls.lib.latcontrol_torque import LatControlTorque from openpilot.selfdrive.controls.lib.latcontrol_angle import LatControlAngle @@ -21,9 +22,10 @@ class TestLatControl: CP = CarInterface.get_non_essential_params(car_name) CP_SP = CarInterface.get_non_essential_params_sp(CP, car_name) CI = CarInterface(CP, CP_SP) + CP_SP = convert_to_capnp(CP_SP) VM = VehicleModel(CP) - controller = controller(CP.as_reader(), CI) + controller = controller(CP.as_reader(), CP_SP.as_reader(), CI) CS = car.CarState.new_message() CS.vEgo = 30 diff --git a/sunnypilot/selfdrive/controls/lib/latcontrol_torque_ext.py b/sunnypilot/selfdrive/controls/lib/latcontrol_torque_ext.py new file mode 100644 index 000000000..1166cb32d --- /dev/null +++ b/sunnypilot/selfdrive/controls/lib/latcontrol_torque_ext.py @@ -0,0 +1,26 @@ +""" +Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors. + +This file is part of sunnypilot and is licensed under the MIT License. +See the LICENSE.md file in the root directory for more details. +""" +from openpilot.sunnypilot.selfdrive.controls.lib.latcontrol_torque_ext_base import LatControlTorqueExtBase + + +class LatControlTorqueExt(LatControlTorqueExtBase): + def __init__(self, lac_torque, CP, CP_SP): + super().__init__(lac_torque, CP, CP_SP) + + def update(self, CS, VM, params, ff, pid_log, setpoint, measurement, calibrated_pose, roll_compensation, + desired_lateral_accel, actual_lateral_accel, lateral_accel_deadzone, gravity_adjusted_lateral_accel): + self._ff = ff + self._pid_log = pid_log + self._setpoint = setpoint + self._measurement = measurement + self._lateral_accel_deadzone = lateral_accel_deadzone + self._desired_lateral_accel = desired_lateral_accel + self._actual_lateral_accel = actual_lateral_accel + + self.update_calculations(CS, VM, desired_lateral_accel) + + return self._ff, self._pid_log diff --git a/sunnypilot/selfdrive/controls/lib/latcontrol_torque_ext_base.py b/sunnypilot/selfdrive/controls/lib/latcontrol_torque_ext_base.py new file mode 100644 index 000000000..48d98501b --- /dev/null +++ b/sunnypilot/selfdrive/controls/lib/latcontrol_torque_ext_base.py @@ -0,0 +1,121 @@ +""" +Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors. + +This file is part of sunnypilot and is licensed under the MIT License. +See the LICENSE.md file in the root directory for more details. +""" +import math +import numpy as np + +from openpilot.selfdrive.controls.lib.drive_helpers import CONTROL_N +from openpilot.selfdrive.modeld.constants import ModelConstants + +LAT_PLAN_MIN_IDX = 5 + + +def get_predicted_lateral_jerk(lat_accels, t_diffs): + # compute finite difference between subsequent model_v2.acceleration.y values + # this is just two calls of np.diff followed by an element-wise division + lat_accel_diffs = np.diff(lat_accels) + lat_jerk = lat_accel_diffs / t_diffs + # return as python list + return lat_jerk.tolist() + + +def sign(x): + return 1.0 if x > 0.0 else (-1.0 if x < 0.0 else 0.0) + + +def get_lookahead_value(future_vals, current_val): + if len(future_vals) == 0: + return current_val + + same_sign_vals = [v for v in future_vals if sign(v) == sign(current_val)] + + # if any future val has opposite sign of current val, return 0 + if len(same_sign_vals) < len(future_vals): + return 0.0 + + # otherwise return the value with minimum absolute value + min_val = min(same_sign_vals + [current_val], key=lambda x: abs(x)) + return min_val + + +class LatControlTorqueExtBase: + def __init__(self, lac_torque, CP, CP_SP): + self.model_v2 = None + self.model_valid = False + self.use_steering_angle = lac_torque.use_steering_angle + + self.actual_lateral_jerk: float = 0.0 + self.lateral_jerk_setpoint: float = 0.0 + self.lateral_jerk_measurement: float = 0.0 + self.lookahead_lateral_jerk: float = 0.0 + + self.torque_from_lateral_accel = lac_torque.torque_from_lateral_accel + self.torque_params = lac_torque.torque_params + + self._ff = 0.0 + self._pid_log = None + self._setpoint = 0.0 + self._measurement = 0.0 + self._lateral_accel_deadzone = 0.0 + self._desired_lateral_accel = 0.0 + self._actual_lateral_accel = 0.0 + + # twilsonco's Lateral Neural Network Feedforward + # Instantaneous lateral jerk changes very rapidly, making it not useful on its own, + # however, we can "look ahead" to the future planned lateral jerk in order to gauge + # whether the current desired lateral jerk will persist into the future, i.e. + # whether it's "deliberate" or not. This allows us to simply ignore short-lived jerk. + # Note that LAT_PLAN_MIN_IDX is defined above and is used in order to prevent + # using a "future" value that is actually planned to occur before the "current" desired + # value, which is offset by the steerActuatorDelay. + # TODO-SP: Reevaluate lookahead v values that determines how low a desired lateral jerk signal needs to + # persist in order to be used. + self.friction_look_ahead_v = [1.4, 2.0] # how many seconds in the future to look ahead in [0, ~2.1] in 0.1 increments + self.friction_look_ahead_bp = [9.0, 30.0] # corresponding speeds in m/s in [0, ~40] in 1.0 increments + + # Scaling the lateral acceleration "friction response" could be helpful for some. + # Increase for a stronger response, decrease for a weaker response. + self.lat_jerk_friction_factor = 0.4 + self.lat_accel_friction_factor = 0.7 # in [0, 3], in 0.05 increments. 3 is arbitrary safety limit + + # precompute time differences between ModelConstants.T_IDXS + self.t_diffs = np.diff(ModelConstants.T_IDXS) + self.desired_lat_jerk_time = CP.steerActuatorDelay + 0.3 + + def update_model_v2(self, model_v2): + self.model_v2 = model_v2 + self.model_valid = self.model_v2 is not None and len(self.model_v2.orientation.x) >= CONTROL_N + + def update_friction_input(self, val_1, val_2): + _error = val_1 - val_2 + _value = self.lat_accel_friction_factor * _error + self.lat_jerk_friction_factor * self.lookahead_lateral_jerk + + return _value + + def update_calculations(self, CS, VM, desired_lateral_accel): + self.actual_lateral_jerk = 0.0 + self.lateral_jerk_setpoint = 0.0 + self.lateral_jerk_measurement = 0.0 + self.lookahead_lateral_jerk = 0.0 + + if self.use_steering_angle: + actual_curvature_rate = -VM.calc_curvature(math.radians(CS.steeringRateDeg), CS.vEgo, 0.0) + self.actual_lateral_jerk = actual_curvature_rate * CS.vEgo ** 2 + + if self.model_valid: + # prepare "look-ahead" desired lateral jerk + lookahead = np.interp(CS.vEgo, self.friction_look_ahead_bp, self.friction_look_ahead_v) + friction_upper_idx = next((i for i, val in enumerate(ModelConstants.T_IDXS) if val > lookahead), 16) + predicted_lateral_jerk = get_predicted_lateral_jerk(self.model_v2.acceleration.y, self.t_diffs) + desired_lateral_jerk = (np.interp(self.desired_lat_jerk_time, ModelConstants.T_IDXS, + self.model_v2.acceleration.y) - desired_lateral_accel) / self.desired_lat_jerk_time + self.lookahead_lateral_jerk = get_lookahead_value(predicted_lateral_jerk[LAT_PLAN_MIN_IDX:friction_upper_idx], desired_lateral_jerk) + if not self.use_steering_angle or self.lookahead_lateral_jerk == 0.0: + self.lookahead_lateral_jerk = 0.0 + self.actual_lateral_jerk = 0.0 + self.lat_accel_friction_factor = 1.0 + self.lateral_jerk_setpoint = self.lat_jerk_friction_factor * self.lookahead_lateral_jerk + self.lateral_jerk_measurement = self.lat_jerk_friction_factor * self.actual_lateral_jerk