add TODOs

* nnlc make lat jerk adjustable rather than on/off toggle

* update lat jerk factor live + bugfix to reset lat accel friction factor after lat jerk has been zero for one frame

* bugfix to prevent crash if `use_lateral_jerk` is enabled after starting car

* assign local variable

---------

Co-authored-by: Jason Wen <haibin.wen3@gmail.com>

CHANGELOGS.md

@@ -31,6 +31,9 @@ sunnypilot - 0.9.8.0 (2024-xx-xx)
 * UPDATED: Driving Model Selector v5
   * NEW❗: Driving Model additions
     * Notre Dame (July 01, 2024) - NDv3
+* NEW❗: Lateral Jerk with Torque Lateral Control (Alpha) thanks to twilsonco!
+  * Utilizes limited lateral jerk control for improved steering response, leveraging stock torque lateral controller capabilities
+  * Designed to mimic NNLC behavior without training models or data collection
 * UPDATED: Toyota: Continued support for Smart DSU (SDSU) and Radar CAN Filter
   * In response to the official deprecation of support for Smart DSU (SDSU) and Radar CAN Filter in the upstream ([commaai/openpilot#32777](https://github.com/commaai/openpilot/pull/32777)), sunnypilot will continue maintaining software support for Smart DSU (SDSU) and Radar CAN Filter
 * UPDATED: Continued support for Mapbox navigation

common/params.cc

@@ -320,6 +320,7 @@ std::unordered_map<std::string, uint32_t> keys = {
     {"TermsVersionSunnypilot", PERSISTENT},
     {"TorqueDeadzoneDeg", PERSISTENT | BACKUP},
     {"TorqueFriction", PERSISTENT | BACKUP},
+    {"TorqueLateralJerk", PERSISTENT | BACKUP},
     {"TorqueMaxLatAccel", PERSISTENT | BACKUP},
     {"TorquedOverride", PERSISTENT | BACKUP},
     {"ToyotaAutoLockBySpeed", PERSISTENT | BACKUP},

selfdrive/controls/lib/latcontrol_torque.py

@@ -83,30 +83,35 @@ class LatControlTorque(LatControl):
     self.torqued_override = self.param_s.get_bool("TorquedOverride")
     self._frame = 0
 
-    self.use_lateral_jerk = False  # TODO: make this a parameter in the UI
+    self.use_lateral_jerk = self.param_s.get_bool("TorqueLateralJerk")
 
     # Twilsonco's Lateral Neural Network Feedforward
     self.use_nn = CI.has_lateral_torque_nn
 
-    if self.use_nn or self.use_lateral_jerk:
-      # 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 guage
-      # whether the current desired lateral jerk will persist into the future, i.e.
-      # whether it's "deliberate" or not. This lets us 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.
-      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
+    # These are only used if use_nn or use_lateral_jerk is True,
+    # but they're defined here since use_lateral_jerk can be toggled while onroad
 
+    # 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 guage
+    # whether the current desired lateral jerk will persist into the future, i.e.
+    # whether it's "deliberate" or not. This lets us 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.
+    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
+    # precompute time differences between ModelConstants.T_IDXS
+    self.t_diffs = np.diff(ModelConstants.T_IDXS)
+    self.desired_lat_jerk_time = CP.steerActuatorDelay + 0.3
+
+    if self.use_nn or self.use_lateral_jerk:
       # 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
+      nnff_lateral_jerk_factor = 1.0  # TODO-SP: replace with ---> float(self.param_s.get("NNFFLateralJerkFactor", encoding="utf8"))
+      nnff_lateral_jerk_factor = max(0.0, min(1.0, nnff_lateral_jerk_factor))
+      self.lat_jerk_friction_factor = 0.4 * nnff_lateral_jerk_factor
+      # Increasing lat accel friction factor to account for any decrease of the lat jerk friction factor from default
+      self.lat_accel_friction_factor = 0.7 + (0.3 * (1.0 - nnff_lateral_jerk_factor))  # in [0, 3], in 0.05 increments. 3 is arbitrary safety limit
     if self.use_nn:
       self.pitch = FirstOrderFilter(0.0, 0.5, 0.01)
       # NN model takes current v_ego, lateral_accel, lat accel/jerk error, roll, and past/future/planned data
@@ -140,6 +145,14 @@ class LatControlTorque(LatControl):
     if self._frame % 250 == 0:
       self._frame = 0
       self.torqued_override = self.param_s.get_bool("TorquedOverride")
+      self.use_lateral_jerk = self.param_s.get_bool("TorqueLateralJerk")
+      if self.use_nn or self.use_lateral_jerk:
+        nnff_lateral_jerk_factor = 1.0  # TODO-SP: replace with ---> float(self.param_s.get("NNFFLateralJerkFactor", encoding="utf8"))
+        nnff_lateral_jerk_factor = max(0.0, min(1.0, nnff_lateral_jerk_factor))
+        self.lat_jerk_friction_factor = 0.4 * nnff_lateral_jerk_factor
+        # Increasing lat accel friction factor to account for any decrease of the lat jerk friction factor from default
+        self.lat_accel_friction_factor = 0.7 + (0.3 * (1.0 - nnff_lateral_jerk_factor))  # in [0, 3], in 0.05 increments. 3 is arbitrary safety limit
+
       if not self.torqued_override:
         return
 
@@ -200,12 +213,15 @@ class LatControlTorque(LatControl):
         if self.use_steering_angle or lookahead_lateral_jerk == 0.0:
           lookahead_lateral_jerk = 0.0
           actual_lateral_jerk = 0.0
-          self.lat_accel_friction_factor = 1.0
         lateral_jerk_setpoint = self.lat_jerk_friction_factor * lookahead_lateral_jerk
         lateral_jerk_measurement = self.lat_jerk_friction_factor * actual_lateral_jerk
 
+      lat_accel_friction_factor = 1.0 if self.use_steering_angle or lookahead_lateral_jerk == 0.0 else \
+                                  self.lat_accel_friction_factor
+
       if self.use_nn and model_good:
         # update past data
+        pitch = 0
         roll = params.roll
         if len(llk.calibratedOrientationNED.value) > 1:
           pitch = self.pitch.update(llk.calibratedOrientationNED.value[1])
@@ -231,11 +247,18 @@ class LatControlTorque(LatControl):
                                  + past_rolls + future_rolls
         torque_from_setpoint = self.torque_from_nn(nnff_setpoint_input)
         torque_from_measurement = self.torque_from_nn(nnff_measurement_input)
+
         pid_log.error = torque_from_setpoint - torque_from_measurement
+        error_blend_factor = interp(abs(desired_lateral_accel), [1.0, 2.0], [0.0, 1.0])
+        if error_blend_factor > 0.0:  # blend in stronger error response when in high lat accel
+          nnff_error_input = [CS.vEgo, setpoint - measurement, lateral_jerk_setpoint - lateral_jerk_measurement, 0.0]
+          torque_from_error = self.torque_from_nn(nnff_error_input)
+          if sign(pid_log.error) == sign(torque_from_error) and abs(pid_log.error) < abs(torque_from_error):
+            pid_log.error = pid_log.error * (1.0 - error_blend_factor) + torque_from_error * error_blend_factor
 
         # compute feedforward (same as nn setpoint output)
         error = setpoint - measurement
-        friction_input = self.lat_accel_friction_factor * error + self.lat_jerk_friction_factor * lookahead_lateral_jerk
+        friction_input = lat_accel_friction_factor * error + self.lat_jerk_friction_factor * lookahead_lateral_jerk
         nn_input = [CS.vEgo, desired_lateral_accel, friction_input, roll] \
                    + past_lateral_accels_desired + future_planned_lateral_accels \
                    + past_rolls + future_rolls
@@ -256,7 +279,7 @@ class LatControlTorque(LatControl):
         pid_log.error = torque_from_setpoint - torque_from_measurement
         error = desired_lateral_accel - actual_lateral_accel
         if self.use_lateral_jerk:
-          friction_input = self.lat_accel_friction_factor * error + self.lat_jerk_friction_factor * lookahead_lateral_jerk
+          friction_input = lat_accel_friction_factor * error + self.lat_jerk_friction_factor * lookahead_lateral_jerk
         else:
           friction_input = error
         ff = self.torque_from_lateral_accel(LatControlInputs(gravity_adjusted_lateral_accel, roll_compensation, CS.vEgo, CS.aEgo), self.torque_params,

selfdrive/ui/sunnypilot/qt/offroad/settings/sunnypilot_settings.cc

@@ -97,6 +97,12 @@ SunnypilotPanel::SunnypilotPanel(QWidget *parent) : QFrame(parent) {
       tr("Enable this to enforce sunnypilot to steer with Torque lateral control."),
       "../assets/offroad/icon_blank.png",
     },
+    {
+      "TorqueLateralJerk",
+      tr("Lateral Jerk with Torque Lateral Control (Alpha)"),
+      tr("Utilizes limited lateral jerk control for improved steering response, leveraging stock torque lateral controller capabilities. Designed to mimic NNLC behavior without training models or data collection."),
+      "../assets/offroad/icon_blank.png",
+    },
     {
       "LiveTorque",
       tr("Enable Self-Tune"),
@@ -345,6 +351,17 @@ SunnypilotPanel::SunnypilotPanel(QWidget *parent) : QFrame(parent) {
     }
   });
 
+  connect(toggles["EnforceTorqueLateral"], &ToggleControlSP::toggleFlipped, [=](bool state) {
+    if (state) {
+      toggles["NNFF"]->setEnabled(false);
+      params.putBool("NNFF", false);
+      toggles["NNFF"]->refresh();
+    } else {
+      toggles["NNFF"]->setEnabled(true);
+      toggles["NNFF"]->refresh();
+    }
+  });
+
   // trigger updateToggles() when toggleFlipped
   for (const auto& updateToggleName : updateTogglesNames) {
     if (toggles.find(updateToggleName) != toggles.end()) {
@@ -577,7 +594,7 @@ void SunnypilotPanel::updateToggles() {
   }
 
   // toggle names to update when EnforceTorqueLateral is flipped
-  std::vector<std::string> torqueLateralGroup{"CustomTorqueLateral", "LiveTorque", "LiveTorqueRelaxed", "TorquedOverride"};
+  std::vector<std::string> torqueLateralGroup{"CustomTorqueLateral", "TorqueLateralJerk", "LiveTorque", "LiveTorqueRelaxed", "TorquedOverride"};
   for (const auto& torqueLateralToggle : torqueLateralGroup) {
     if (toggles.find(torqueLateralToggle) != toggles.end()) {
       if (nnff_toggle->isToggled()) {

selfdrive/ui/translations/main_ar.ts

@@ -2701,6 +2701,14 @@ Reboot Required.</source>
         <source>Default is Laneless. In Auto mode, sunnnypilot dynamically chooses between Laneline or Laneless model based on lane recognition confidence level on road and certain conditions.</source>
         <translation type="unfinished"></translation>
     </message>
+    <message>
+        <source>Lateral Jerk with Torque Lateral Control (Alpha)</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>Utilizes limited lateral jerk control for improved steering response, leveraging stock torque lateral controller capabilities. Designed to mimic NNLC behavior without training models or data collection.</source>
+        <translation type="unfinished"></translation>
+    </message>
 </context>
 <context>
     <name>TermsPage</name>

selfdrive/ui/translations/main_de.ts

@@ -2685,6 +2685,14 @@ Reboot Required.</source>
         <source>Default is Laneless. In Auto mode, sunnnypilot dynamically chooses between Laneline or Laneless model based on lane recognition confidence level on road and certain conditions.</source>
         <translation type="unfinished"></translation>
     </message>
+    <message>
+        <source>Lateral Jerk with Torque Lateral Control (Alpha)</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>Utilizes limited lateral jerk control for improved steering response, leveraging stock torque lateral controller capabilities. Designed to mimic NNLC behavior without training models or data collection.</source>
+        <translation type="unfinished"></translation>
+    </message>
 </context>
 <context>
     <name>TermsPage</name>

selfdrive/ui/translations/main_es.ts

@@ -2681,6 +2681,14 @@ Reboot Required.</source>
         <source>Default is Laneless. In Auto mode, sunnnypilot dynamically chooses between Laneline or Laneless model based on lane recognition confidence level on road and certain conditions.</source>
         <translation type="unfinished"></translation>
     </message>
+    <message>
+        <source>Lateral Jerk with Torque Lateral Control (Alpha)</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>Utilizes limited lateral jerk control for improved steering response, leveraging stock torque lateral controller capabilities. Designed to mimic NNLC behavior without training models or data collection.</source>
+        <translation type="unfinished"></translation>
+    </message>
 </context>
 <context>
     <name>TermsPage</name>

selfdrive/ui/translations/main_fr.ts

@@ -2685,6 +2685,14 @@ Reboot Required.</source>
         <source>Default is Laneless. In Auto mode, sunnnypilot dynamically chooses between Laneline or Laneless model based on lane recognition confidence level on road and certain conditions.</source>
         <translation type="unfinished"></translation>
     </message>
+    <message>
+        <source>Lateral Jerk with Torque Lateral Control (Alpha)</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>Utilizes limited lateral jerk control for improved steering response, leveraging stock torque lateral controller capabilities. Designed to mimic NNLC behavior without training models or data collection.</source>
+        <translation type="unfinished"></translation>
+    </message>
 </context>
 <context>
     <name>TermsPage</name>

selfdrive/ui/translations/main_ja.ts

@@ -2679,6 +2679,14 @@ Reboot Required.</source>
         <source>Default is Laneless. In Auto mode, sunnnypilot dynamically chooses between Laneline or Laneless model based on lane recognition confidence level on road and certain conditions.</source>
         <translation type="unfinished"></translation>
     </message>
+    <message>
+        <source>Lateral Jerk with Torque Lateral Control (Alpha)</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>Utilizes limited lateral jerk control for improved steering response, leveraging stock torque lateral controller capabilities. Designed to mimic NNLC behavior without training models or data collection.</source>
+        <translation type="unfinished"></translation>
+    </message>
 </context>
 <context>
     <name>TermsPage</name>

selfdrive/ui/translations/main_ko.ts

@@ -2681,6 +2681,14 @@ Reboot Required.</source>
         <source>Default is Laneless. In Auto mode, sunnnypilot dynamically chooses between Laneline or Laneless model based on lane recognition confidence level on road and certain conditions.</source>
         <translation type="unfinished"></translation>
     </message>
+    <message>
+        <source>Lateral Jerk with Torque Lateral Control (Alpha)</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>Utilizes limited lateral jerk control for improved steering response, leveraging stock torque lateral controller capabilities. Designed to mimic NNLC behavior without training models or data collection.</source>
+        <translation type="unfinished"></translation>
+    </message>
 </context>
 <context>
     <name>TermsPage</name>

selfdrive/ui/translations/main_pt-BR.ts

@@ -2685,6 +2685,14 @@ Reboot Required.</source>
         <source>Default is Laneless. In Auto mode, sunnnypilot dynamically chooses between Laneline or Laneless model based on lane recognition confidence level on road and certain conditions.</source>
         <translation type="unfinished"></translation>
     </message>
+    <message>
+        <source>Lateral Jerk with Torque Lateral Control (Alpha)</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>Utilizes limited lateral jerk control for improved steering response, leveraging stock torque lateral controller capabilities. Designed to mimic NNLC behavior without training models or data collection.</source>
+        <translation type="unfinished"></translation>
+    </message>
 </context>
 <context>
     <name>TermsPage</name>

selfdrive/ui/translations/main_th.ts

@@ -2681,6 +2681,14 @@ Reboot Required.</source>
         <source>Default is Laneless. In Auto mode, sunnnypilot dynamically chooses between Laneline or Laneless model based on lane recognition confidence level on road and certain conditions.</source>
         <translation type="unfinished"></translation>
     </message>
+    <message>
+        <source>Lateral Jerk with Torque Lateral Control (Alpha)</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>Utilizes limited lateral jerk control for improved steering response, leveraging stock torque lateral controller capabilities. Designed to mimic NNLC behavior without training models or data collection.</source>
+        <translation type="unfinished"></translation>
+    </message>
 </context>
 <context>
     <name>TermsPage</name>

selfdrive/ui/translations/main_tr.ts

@@ -2679,6 +2679,14 @@ Reboot Required.</source>
         <source>Default is Laneless. In Auto mode, sunnnypilot dynamically chooses between Laneline or Laneless model based on lane recognition confidence level on road and certain conditions.</source>
         <translation type="unfinished"></translation>
     </message>
+    <message>
+        <source>Lateral Jerk with Torque Lateral Control (Alpha)</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>Utilizes limited lateral jerk control for improved steering response, leveraging stock torque lateral controller capabilities. Designed to mimic NNLC behavior without training models or data collection.</source>
+        <translation type="unfinished"></translation>
+    </message>
 </context>
 <context>
     <name>TermsPage</name>

selfdrive/ui/translations/main_zh-CHS.ts

@@ -2681,6 +2681,14 @@ Reboot Required.</source>
         <source>Default is Laneless. In Auto mode, sunnnypilot dynamically chooses between Laneline or Laneless model based on lane recognition confidence level on road and certain conditions.</source>
         <translation type="unfinished"></translation>
     </message>
+    <message>
+        <source>Lateral Jerk with Torque Lateral Control (Alpha)</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>Utilizes limited lateral jerk control for improved steering response, leveraging stock torque lateral controller capabilities. Designed to mimic NNLC behavior without training models or data collection.</source>
+        <translation type="unfinished"></translation>
+    </message>
 </context>
 <context>
     <name>TermsPage</name>

selfdrive/ui/translations/main_zh-CHT.ts

@@ -2681,6 +2681,14 @@ Reboot Required.</source>
         <source>Default is Laneless. In Auto mode, sunnnypilot dynamically chooses between Laneline or Laneless model based on lane recognition confidence level on road and certain conditions.</source>
         <translation type="unfinished"></translation>
     </message>
+    <message>
+        <source>Lateral Jerk with Torque Lateral Control (Alpha)</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>Utilizes limited lateral jerk control for improved steering response, leveraging stock torque lateral controller capabilities. Designed to mimic NNLC behavior without training models or data collection.</source>
+        <translation type="unfinished"></translation>
+    </message>
 </context>
 <context>
     <name>TermsPage</name>

system/manager/manager.py

@@ -99,6 +99,7 @@ def manager_init() -> None:
     ("StockLongToyota", "0"),
     ("TorqueDeadzoneDeg", "0"),
     ("TorqueFriction", "1"),
+    ("TorqueLateralJerk", "0"),
     ("TorqueMaxLatAccel", "250"),
     ("ToyotaAutoLockBySpeed", "0"),
     ("ToyotaAutoUnlockByShifter", "0"),