Controls: Lateral Jerk Torque Controller (#693)

* 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 * update regardless, it's an extension now * update name and expose toggle * ui: sunnypilot Panel -> Steering Panel * Update selfdrive/ui/sunnypilot/qt/offroad/settings/lateral_panel.h * merge * move to steering panel * no need for this * live params in a thread * no live for now * new structs * more ui * more flexible * more ui * no longer needed * another ui * cereal changes * bump opendbc * simplify checks * all in one place * just Enhanced Lat Accel only * no submodule for this * Enhanced Lateral Acceleration: fix bugs, restore NNLC, add UI/schema Fix 4 bugs in latcontrol_torque_lat_accel.py: missing CI param, wrong method name (torque_from_lateral_accel → torque_from_lateral_accel_in_torque_space), self.enabled collision with NNLC, missing output torque recomputation. Restore NNLC + ExtOverride inheritance chain with Enhanced slotted between ExtBase and NNLC. Add mutual exclusion constraints, Python UI toggle, sunnylink schema entries, and controller init/update tests. * fix lint: remove unused default params list, add ty ignore, fix test fixture * use the og name * rename * need gates * TODOs * wrong * gate them all on init --------- Co-authored-by: DevTekVE <devtekve@gmail.com>
2026-09-12 03:03:41 +08:00 · 2026-08-09 15:56:33 -04:00
parent fc4699a747
commit 91a53aa161
13 changed files with 240 additions and 5 deletions
@@ -273,6 +273,7 @@ inline static std::unordered_map<std::string, ParamKeyAttributes> keys = {
    // Torque lateral control custom params
    {"CustomTorqueParams", {PERSISTENT | BACKUP , BOOL}},
    {"EnforceTorqueControl", {PERSISTENT | BACKUP, BOOL}},
+    {"LateralJerkTorqueController", {PERSISTENT | BACKUP, BOOL, "0"}},
    {"LiveTorqueParamsToggle", {PERSISTENT | BACKUP , BOOL}},
    {"LiveTorqueParamsRelaxedToggle", {PERSISTENT | BACKUP , BOOL}},
    {"TorqueControlTune", {PERSISTENT | BACKUP, FLOAT, "0.0"}},
@@ -139,7 +139,8 @@ class SteeringLayout(Widget):
      self._nnlc_toggle.action_item.set_state(False)
      enforce_torque_enabled = False
      nnlc_enabled = False
-    self._nnlc_toggle.action_item.set_enabled(ui_state.is_offroad() and torque_allowed and not enforce_torque_enabled)
+    jerk_aware_enabled = ui_state.params.get_bool("LateralJerkTorqueController")
+    self._nnlc_toggle.action_item.set_enabled(ui_state.is_offroad() and torque_allowed and not enforce_torque_enabled and not jerk_aware_enabled)
    self._torque_control_toggle.action_item.set_enabled(ui_state.is_offroad() and torque_allowed and not nnlc_enabled)
    self._torque_customization_button.action_item.set_enabled(self._torque_control_toggle.action_item.get_state())

@@ -40,6 +40,13 @@ class TorqueSettingsLayout(Widget):
      self.cached_torque_versions = json.load(f)

  def _initialize_items(self):
+    self._jerk_aware_toggle = toggle_item_sp(
+      param="LateralJerkTorqueController",
+      title=lambda: tr("Lateral Jerk Torque Controller"),
+      description=lambda: tr("Looks ahead at planned steering to reduce sudden corrections, so the wheel moves " +
+                             "more smoothly through turns. Works with Self-Tune and custom tuning. " +
+                             "Thanks to @twilsonco for the implementation."),
+    )
    self._torque_control_versions = ListItemSP(
      title=tr("Torque Control Tune Version"),
      description="Select the version of Torque Control Tune to use.",
@@ -95,6 +102,7 @@ class TorqueSettingsLayout(Widget):
    )

    items = [
+      self._jerk_aware_toggle,
      self._torque_control_versions,
      self._self_tune_toggle,
      self._relaxed_tune_toggle,
@@ -107,6 +115,8 @@ class TorqueSettingsLayout(Widget):

  def _update_state(self):
    super()._update_state()
+    nnlc_enabled = ui_state.params.get_bool("NeuralNetworkLateralControl")
+    self._jerk_aware_toggle.action_item.set_enabled(ui_state.is_offroad() and not nnlc_enabled)
    if not ui_state.params.get_bool("LiveTorqueParamsToggle"):
      ui_state.params.remove("LiveTorqueParamsRelaxedToggle")
      self._relaxed_tune_toggle.action_item.set_state(False)
@@ -184,10 +184,15 @@ class UIStateSP:
        self.params.put_bool("EnforceTorqueControl", False, block=True)
        self.params.put_bool("NeuralNetworkLateralControl", False, block=True)

+      if self.params.get_bool("LateralJerkTorqueController") and self.params.get_bool("NeuralNetworkLateralControl"):
+        self.params.put_bool("LateralJerkTorqueController", False, block=True)
+        self.params.put_bool("NeuralNetworkLateralControl", False, block=True)
+
      # Angle steering: no torque-based lateral controls
      if CP.steerControlType == car.CarParams.SteerControlType.angle:
        self.params.remove("EnforceTorqueControl")
        self.params.remove("NeuralNetworkLateralControl")
+        self.params.remove("LateralJerkTorqueController")

      # Alpha longitudinal: clear if not available
      if not CP.alphaLongitudinalAvailable:
@@ -200,6 +205,7 @@ class UIStateSP:
      # No CarParams: clear all car-dependent params as safety default
      self.params.remove("EnforceTorqueControl")
      self.params.remove("NeuralNetworkLateralControl")
+      self.params.remove("LateralJerkTorqueController")
      self.params.remove("AlphaLongitudinalEnabled")

    # No longitudinal control: no experimental mode or DEC
@@ -73,10 +73,16 @@ def _cleanup_unsupported_params(CP: structs.CarParams, CP_SP: structs.CarParamsS
  if params is None:
    params = Params()

+  if params.get_bool("LateralJerkTorqueController") and params.get_bool("NeuralNetworkLateralControl"):
+    cloudlog.warning("LateralJerkTorqueController and NeuralNetworkLateralControl both enabled, disabling both")
+    params.put_bool("LateralJerkTorqueController", False, block=True)
+    params.put_bool("NeuralNetworkLateralControl", False, block=True)
+
  if CP.steerControlType == structs.CarParams.SteerControlType.angle:
    cloudlog.warning("SteerControlType is angle, cleaning up params")
    params.remove("NeuralNetworkLateralControl")
    params.remove("EnforceTorqueControl")
+    params.remove("LateralJerkTorqueController")

  if not CP_SP.intelligentCruiseButtonManagementAvailable or CP.openpilotLongitudinalControl:
    cloudlog.warning("ICBM not available or openpilot Longitudinal Control enabled, cleaning up params")
@@ -33,6 +33,7 @@ class LatControlTorqueExt(NeuralNetworkLateralControl, LatControlTorqueExtOverri
    self._output_torque = output_torque

    self.update_calculations(CS, VM, desired_lateral_accel)
+    self.update_jerk_aware_torque_control(CS, roll_compensation, gravity_adjusted_lateral_accel)
    self.update_neural_network_feedforward(CS, params, calibrated_pose)

    return self._pid_log, self._output_torque
@@ -0,0 +1,45 @@
+"""
+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 opendbc.car.lateral import FRICTION_THRESHOLD
+from opendbc.sunnypilot.car.interfaces import LatControlInputs
+from opendbc.sunnypilot.car.lateral_ext import get_friction as get_friction_in_torque_space
+from openpilot.common.params import Params
+
+from openpilot.sunnypilot.selfdrive.controls.lib.latcontrol_torque_ext_base import LatControlTorqueExtBase
+
+
+class LatControlTorqueJerkAware(LatControlTorqueExtBase):
+  def __init__(self, lac_torque, CP, CP_SP, CI):
+    super().__init__(lac_torque, CP, CP_SP, CI)
+    self.params = Params()
+    self._jerk_aware_enabled = self.params.get_bool("LateralJerkTorqueController")
+
+  def update_limits(self):
+    if not self._jerk_aware_enabled:
+      return
+    self._pid.set_limits(self.lac_torque.steer_max, -self.lac_torque.steer_max)
+
+  def update_jerk_aware_torque_control(self, CS, roll_compensation, gravity_adjusted_lateral_accel):
+    if not self._jerk_aware_enabled:
+      return
+
+    torque_from_setpoint = self.torque_from_lateral_accel_in_torque_space(
+      LatControlInputs(self._setpoint, roll_compensation, CS.vEgo, CS.aEgo), self.torque_params, gravity_adjusted=False
+    )
+    torque_from_measurement = self.torque_from_lateral_accel_in_torque_space(
+      LatControlInputs(self._measurement, roll_compensation, CS.vEgo, CS.aEgo), self.torque_params, gravity_adjusted=False
+    )
+
+    self._pid_log.error = float(torque_from_setpoint - torque_from_measurement)  # ty: ignore[invalid-assignment]
+    self._ff = self.torque_from_lateral_accel_in_torque_space(
+      LatControlInputs(gravity_adjusted_lateral_accel, roll_compensation, CS.vEgo, CS.aEgo), self.torque_params, gravity_adjusted=True
+    )
+
+    friction_input = self.update_friction_input(self._desired_lateral_accel, self._actual_lateral_accel)
+    self._ff += get_friction_in_torque_space(friction_input, self._lateral_accel_deadzone, FRICTION_THRESHOLD, self.torque_params)
+
+    self.update_output_torque(CS)
@@ -14,7 +14,8 @@ from opendbc.sunnypilot.car.lateral_ext import get_friction as get_friction_in_t
 from openpilot.common.filter_simple import FirstOrderFilter
 from openpilot.common.params import Params
 from openpilot.selfdrive.modeld.constants import ModelConstants
-from openpilot.sunnypilot.selfdrive.controls.lib.latcontrol_torque_ext_base import LatControlTorqueExtBase, sign
+from openpilot.sunnypilot.selfdrive.controls.lib.latcontrol_torque_ext_base import sign
+from openpilot.sunnypilot.selfdrive.controls.lib.latcontrol_torque_jerk_aware import LatControlTorqueJerkAware
 from openpilot.sunnypilot.selfdrive.controls.lib.nnlc.helpers import MOCK_MODEL_PATH
 from openpilot.sunnypilot.selfdrive.controls.lib.nnlc.model import NNTorqueModel

@@ -31,7 +32,7 @@ def roll_pitch_adjust(roll, pitch):
  return roll * math.cos(pitch)


-class NeuralNetworkLateralControl(LatControlTorqueExtBase):
+class NeuralNetworkLateralControl(LatControlTorqueJerkAware):
  def __init__(self, lac_torque, CP, CP_SP, CI):
    super().__init__(lac_torque, CP, CP_SP, CI)
    self.params = Params()
@@ -65,6 +66,7 @@ class NeuralNetworkLateralControl(LatControlTorqueExtBase):
    return self.enabled and self.model_valid and self.has_nn_model

  def update_limits(self):
+    super().update_limits()
    if not self._nnlc_enabled:
      return

@@ -0,0 +1,108 @@
+"""
+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 numpy as np
+
+from openpilot.cereal import log, messaging
+from opendbc.car.structs import car
+from opendbc.car.car_helpers import interfaces
+from opendbc.car.honda.values import CAR as HONDA
+from opendbc.car.vehicle_model import VehicleModel
+from openpilot.common.params import Params
+from openpilot.common.realtime import DT_CTRL
+from openpilot.selfdrive.car.helpers import convert_to_capnp
+from openpilot.selfdrive.controls.lib.latcontrol_torque import LatControlTorque
+from openpilot.selfdrive.locationd.helpers import Pose
+from openpilot.common.mock.generators import generate_livePose
+from openpilot.sunnypilot.selfdrive.car import interfaces as sunnypilot_interfaces
+from openpilot.selfdrive.modeld.constants import ModelConstants
+
+
+def _make_controller(enhanced=False, nnlc=False):
+  params = Params()
+  params.put_bool("EnforceTorqueControl", True, block=True)
+  params.put_bool("LateralJerkTorqueController", enhanced, block=True)
+  params.put_bool("NeuralNetworkLateralControl", nnlc, block=True)
+
+  car_name = HONDA.HONDA_CIVIC
+  CarInterface = interfaces[car_name]
+  CP = CarInterface.get_non_essential_params(car_name)
+  CP_SP = CarInterface.get_non_essential_params_sp(CP, car_name)
+  CI = CarInterface(CP, CP_SP)
+  sunnypilot_interfaces.setup_interfaces(CI, params)
+  CP_SP = convert_to_capnp(CP_SP)
+  VM = VehicleModel(CP)
+  controller = LatControlTorque(CP.as_reader(), CP_SP.as_reader(), CI, DT_CTRL)
+  return controller, VM, CP
+
+
+def _make_model_v2():
+  model = messaging.new_message('modelV2')
+  position = log.XYZTData.new_message()
+  position.x = [float(x) for x in 30.0 * np.array(ModelConstants.T_IDXS)]
+  model.modelV2.position = position
+  orientation = log.XYZTData.new_message()
+  orientation.x = [0.0 for _ in ModelConstants.T_IDXS]
+  orientation.y = [0.0 for _ in ModelConstants.T_IDXS]
+  model.modelV2.orientation = orientation
+  velocity = log.XYZTData.new_message()
+  velocity.x = [30.0 for _ in ModelConstants.T_IDXS]
+  model.modelV2.velocity = velocity
+  acceleration = log.XYZTData.new_message()
+  acceleration.x = [0.0 for _ in ModelConstants.T_IDXS]
+  acceleration.y = [0.0 for _ in ModelConstants.T_IDXS]
+  model.modelV2.acceleration = acceleration
+  return model
+
+
+def _run_update(controller, VM):
+  CS = car.CarState.new_message()
+  CS.vEgo = 30
+  CS.steeringPressed = False
+  lp = generate_livePose()
+  pose = Pose.from_live_pose(lp.livePose)
+  params = log.LiveParametersData.new_message()
+  model_v2 = _make_model_v2().modelV2
+  controller.extension.update_model_v2(model_v2)
+  controller.extension.update_lateral_lag(0.2)
+  return controller.update(True, CS, VM, params, False, 0.5, pose, False, 0.2)
+
+
+class TestLatControlTorqueExt:
+  def test_init_enhanced_only(self):
+    controller, VM, _ = _make_controller(enhanced=True, nnlc=False)
+    assert controller.extension._jerk_aware_enabled
+    assert not controller.extension.enabled  # NNLC disabled
+
+  def test_init_nnlc_only(self):
+    controller, VM, _ = _make_controller(enhanced=False, nnlc=True)
+    assert not controller.extension._jerk_aware_enabled
+    assert controller.extension.enabled
+
+  def test_init_neither(self):
+    controller, VM, _ = _make_controller(enhanced=False, nnlc=False)
+    assert not controller.extension._jerk_aware_enabled
+    assert not controller.extension.enabled
+
+  def test_init_both_no_crash(self):
+    controller, VM, _ = _make_controller(enhanced=True, nnlc=True)
+    assert not controller.extension._jerk_aware_enabled
+    assert not controller.extension.enabled
+
+  def test_update_enhanced_only(self):
+    controller, VM, _ = _make_controller(enhanced=True, nnlc=False)
+    output_torque, _, pid_log = _run_update(controller, VM)
+    assert pid_log.active
+
+  def test_update_neither(self):
+    controller, VM, _ = _make_controller(enhanced=False, nnlc=False)
+    output_torque, _, pid_log = _run_update(controller, VM)
+    assert pid_log.active
+
+  def test_update_both_no_crash(self):
+    controller, VM, _ = _make_controller(enhanced=True, nnlc=True)
+    output_torque, _, pid_log = _run_update(controller, VM)
+    assert pid_log.active
@@ -323,6 +323,32 @@
                "equals": true
              },
              "items": [
+                {
+                  "key": "LateralJerkTorqueController",
+                  "widget": "toggle",
+                  "title": "Lateral Jerk Torque Controller",
+                  "description": "Looks ahead at planned steering to reduce sudden corrections, so the wheel moves more smoothly through turns. Works with Self-Tune and custom tuning. Thanks to @twilsonco for the implementation.",
+                  "visibility": [
+                    {
+                      "type": "not",
+                      "condition": {
+                        "type": "capability",
+                        "field": "steer_control_type",
+                        "equals": "angle"
+                      }
+                    }
+                  ],
+                  "enablement": [
+                    {
+                      "type": "offroad_only"
+                    },
+                    {
+                      "type": "param",
+                      "key": "NeuralNetworkLateralControl",
+                      "equals": false
+                    }
+                  ]
+                },
                {
                  "key": "LiveTorqueParamsToggle",
                  "widget": "toggle",
@@ -2037,6 +2063,11 @@
                  "type": "param",
                  "key": "EnforceTorqueControl",
                  "equals": false
+                },
+                {
+                  "type": "param",
+                  "key": "LateralJerkTorqueController",
+                  "equals": false
                }
              ]
            }
@@ -73,6 +73,9 @@ sections:
    - type: param
      key: EnforceTorqueControl
      equals: false
+    - type: param
+      key: LateralJerkTorqueController
+      equals: false
 - id: camera
  title: Camera
  description: Camera position and calibration
@@ -127,6 +127,21 @@ sections:
      key: EnforceTorqueControl
      equals: true
    items:
+    - key: LateralJerkTorqueController
+      widget: toggle
+      title: Lateral Jerk Torque Controller
+      description: Looks ahead at planned steering to reduce sudden corrections, so the wheel moves more smoothly through turns. Works with Self-Tune and custom tuning. Thanks to @twilsonco for the implementation.
+      visibility:
+      - type: not
+        condition:
+          type: capability
+          field: steer_control_type
+          equals: angle
+      enablement:
+      - $ref: '#/macros/offroad'
+      - type: param
+        key: NeuralNetworkLateralControl
+        equals: false
    - key: LiveTorqueParamsToggle
      widget: toggle
      title: Self-Tune
@@ -257,20 +257,26 @@ class TestKnownPanels:
    assert "mads_settings" in sub_ids

  def test_mutual_exclusion_torque_nnlc(self, schema):
-    """EnforceTorqueControl and NNLC must reference each other in enablement."""
-    torque = nnlc = None
+    """EnforceTorqueControl, EnhancedLatAccel, and NNLC must reference each other in enablement."""
+    torque = nnlc = enhanced = None
    for panel in schema["panels"]:
      for item in _iter_panel_items(panel):
        if item["key"] == "EnforceTorqueControl":
          torque = item
        elif item["key"] == "NeuralNetworkLateralControl":
          nnlc = item
+        elif item["key"] == "LateralJerkTorqueController":
+          enhanced = item
    assert torque is not None, "EnforceTorqueControl item missing"
    assert nnlc is not None, "NeuralNetworkLateralControl item missing"
+    assert enhanced is not None, "LateralJerkTorqueController item missing"
    torque_enable_keys = {r.get("key") for r in torque.get("enablement", []) if r.get("type") == "param"}
    assert "NeuralNetworkLateralControl" in torque_enable_keys
    nnlc_enable_keys = {r.get("key") for r in nnlc.get("enablement", []) if r.get("type") == "param"}
    assert "EnforceTorqueControl" in nnlc_enable_keys
+    assert "LateralJerkTorqueController" in nnlc_enable_keys
+    enhanced_enable_keys = {r.get("key") for r in enhanced.get("enablement", []) if r.get("type") == "param"}
+    assert "NeuralNetworkLateralControl" in enhanced_enable_keys


 class TestKnownVehicleSettings: