more init

cereal/custom.capnp

@@ -143,6 +143,9 @@ struct CarParamsSP @0x80ae746ee2596b11 {
 
   struct NeuralNetworkLateralControl {
     enabled @0 :Bool;
+    modelPath @1 :Text;
+    modelName @2 :Text;
+    fuzzyFingerprint @3 :Bool;
   }
 }
 

common/params_keys.h

@@ -141,6 +141,9 @@ inline static std::unordered_map<std::string, uint32_t> keys = {
     {"ModelManager_LastSyncTime", CLEAR_ON_MANAGER_START | CLEAR_ON_OFFROAD_TRANSITION},
     {"ModelManager_ModelsCache", PERSISTENT | BACKUP},
 
+    // Neural Network Lateral Control
+    {"NeuralNetworkLateralControl", PERSISTENT | BACKUP},
+
     // sunnylink params
     {"EnableSunnylinkUploader", PERSISTENT | BACKUP},
     {"LastSunnylinkPingTime", CLEAR_ON_MANAGER_START},

sunnypilot/selfdrive/car/interfaces.py

@@ -5,12 +5,17 @@ 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 os
+
 from opendbc.car import Bus, structs
 from opendbc.car.can_definitions import CanRecvCallable, CanSendCallable
 from opendbc.car.car_helpers import can_fingerprint
+from opendbc.car.interfaces import CarInterfaceBase
 from opendbc.car.hyundai.radar_interface import RADAR_START_ADDR
 from opendbc.car.hyundai.values import HyundaiFlags, DBC as HYUNDAI_DBC
 from opendbc.sunnypilot.car.hyundai.values import HyundaiFlagsSP
+from openpilot.common.swaglog import cloudlog
+from openpilot.sunnypilot.selfdrive.controls.lib.nnlc.helpers import get_nn_model_path
 
 import openpilot.system.sentry as sentry
 
@@ -22,6 +27,24 @@ def log_fingerprint(CP: structs.CarParams) -> None:
     sentry.capture_fingerprint(CP.carFingerprint, CP.brand)
 
 
+def initialize_neural_network_lateral_control(CP: structs.CarParams, CP_SP: structs.CarParamsSP, params) -> None:
+  nnlc_model_path, nnlc_model_name, fuzzy_fingerprint = get_nn_model_path(CP)
+
+  if nnlc_model_path is None:
+    cloudlog.error({"nnlc event": "car doesn't match any Neural Network model"})
+    nnlc_model_path = "MOCK"
+
+  if nnlc_model_path != "MOCK" and CP.steerControlType != structs.CarParams.SteerControlType.angle:
+    CP_SP.neuralNetworkLateralControl.enabled = params.get_bool("NeuralNetworkLateralControl")
+
+  if CP_SP.neuralNetworkLateralControl.enabled:
+    CarInterfaceBase.configure_torque_tune(CP.carFingerprint, CP.lateralTuning)
+
+  CP_SP.neuralNetworkLateralControl.modelPath = os.path.splitext(os.path.basename(nnlc_model_path))[0]
+  CP_SP.neuralNetworkLateralControl.modelName = nnlc_model_name
+  CP_SP.neuralNetworkLateralControl.fuzzyFingerprint = fuzzy_fingerprint
+
+
 def setup_car_interface_sp(CP: structs.CarParams, CP_SP: structs.CarParamsSP, params):
   if CP.brand == 'hyundai':
     if CP.flags & HyundaiFlags.MANDO_RADAR and CP.radarUnavailable:
@@ -32,6 +55,8 @@ def setup_car_interface_sp(CP: structs.CarParams, CP_SP: structs.CarParamsSP, pa
         if params.get_bool("HyundaiRadarTracks"):
           CP.radarUnavailable = False
 
+  initialize_neural_network_lateral_control(CP, CP_SP, params)
+
 
 def initialize_car_interface_sp(CP: structs.CarParams, CP_SP: structs.CarParamsSP, params, can_recv: CanRecvCallable,
                                 can_send: CanSendCallable):

sunnypilot/selfdrive/controls/lib/nnlc/flux_model.py

@@ -0,0 +1,100 @@
+"""
+The MIT License
+
+Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
+
+Permission is hereby granted, free of charge, to any person obtaining a copy
+of this software and associated documentation files (the "Software"), to deal
+in the Software without restriction, including without limitation the rights
+to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+copies of the Software, and to permit persons to whom the Software is
+furnished to do so, subject to the following conditions:
+
+The above copyright notice and this permission notice shall be included in
+all copies or substantial portions of the Software.
+
+THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+THE SOFTWARE.
+
+Last updated: July 29, 2024
+"""
+
+import numpy as np
+from json import load
+
+from openpilot.sunnypilot.selfdrive.car.nnlc.helpers import ACTIVATION_FUNCTION_NAMES
+
+
+class FluxModel:
+  def __init__(self, params_file, zero_bias=False):
+    with open(params_file, "r") as f:
+      params = load(f)
+
+    self.input_size = params["input_size"]
+    self.output_size = params["output_size"]
+    self.input_mean = np.array(params["input_mean"], dtype=np.float32).T
+    self.input_std = np.array(params["input_std"], dtype=np.float32).T
+    self.layers = []
+    self.friction_override = False
+
+    for layer_params in params["layers"]:
+      W = np.array(layer_params[next(key for key in layer_params.keys() if key.endswith('_W'))], dtype=np.float32).T
+      b = np.array(layer_params[next(key for key in layer_params.keys() if key.endswith('_b'))], dtype=np.float32).T
+      if zero_bias:
+        b = np.zeros_like(b)
+      activation = layer_params["activation"]
+      for k, v in ACTIVATION_FUNCTION_NAMES.items():
+        activation = activation.replace(k, v)
+      self.layers.append((W, b, activation))
+
+    self.validate_layers()
+    self.check_for_friction_override()
+
+  # Begin activation functions.
+  # These are called by name using the keys in the model json file
+  @staticmethod
+  def sigmoid(x):
+    return 1 / (1 + np.exp(-x))
+
+  @staticmethod
+  def identity(x):
+    return x
+  # End activation functions
+
+  def forward(self, x):
+    for W, b, activation in self.layers:
+      x = getattr(self, activation)(x.dot(W) + b)
+    return x
+
+  def evaluate(self, input_array):
+    in_len = len(input_array)
+    if in_len != self.input_size:
+      # If the input is length 2-4, then it's a simplified evaluation.
+      # In that case, need to add on zeros to fill out the input array to match the correct length.
+      if 2 <= in_len:
+        input_array = input_array + [0] * (self.input_size - in_len)
+      else:
+        raise ValueError(f"Input array length {len(input_array)} must be length 2 or greater")
+
+    input_array = np.array(input_array, dtype=np.float32)
+
+    # Rescale the input array using the input_mean and input_std
+    input_array = (input_array - self.input_mean) / self.input_std
+
+    output_array = self.forward(input_array)
+
+    return float(output_array[0, 0])
+
+  def validate_layers(self):
+    for W, b, activation in self.layers:
+      if not hasattr(self, activation):
+        raise ValueError(f"Unknown activation: {activation}")
+
+  def check_for_friction_override(self):
+    y = self.evaluate([10.0, 0.0, 0.2])
+    self.friction_override = (y < 0.1)

sunnypilot/selfdrive/controls/lib/nnlc/helpers.py

@@ -0,0 +1,74 @@
+"""
+The MIT License
+
+Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
+
+Permission is hereby granted, free of charge, to any person obtaining a copy
+of this software and associated documentation files (the "Software"), to deal
+in the Software without restriction, including without limitation the rights
+to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+copies of the Software, and to permit persons to whom the Software is
+furnished to do so, subject to the following conditions:
+
+The above copyright notice and this permission notice shall be included in
+all copies or substantial portions of the Software.
+
+THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+THE SOFTWARE.
+
+Last updated: July 29, 2024
+"""
+import os
+from difflib import SequenceMatcher
+
+from opendbc.car import structs
+from openpilot.common.basedir import BASEDIR
+
+# dict used to rename activation functions whose names aren't valid python identifiers
+ACTIVATION_FUNCTION_NAMES = {'σ': 'sigmoid'}
+
+TORQUE_NN_MODEL_PATH = os.path.join(BASEDIR, 'lat_models')
+
+
+def similarity(s1: str, s2: str) -> float:
+  return SequenceMatcher(None, s1, s2).ratio()
+
+
+def get_nn_model_path(CP: structs.CarParams) -> tuple[str | None, str, bool]:
+  _car = CP.carFingerprint
+  _eps_fw = str(next((fw.fwVersion for fw in CP.carFw if fw.ecu == "eps"), ""))
+  _model_name = ""
+
+  def check_nn_path(_check_model):
+    _model_path = None
+    _max_similarity = -1.0
+    for f in os.listdir(TORQUE_NN_MODEL_PATH):
+      if f.endswith(".json"):
+        model = f.replace(".json", "").replace(f"{TORQUE_NN_MODEL_PATH}/", "")
+        similarity_score = similarity(model, _check_model)
+        if similarity_score > _max_similarity:
+          _max_similarity = similarity_score
+          _model_path = os.path.join(TORQUE_NN_MODEL_PATH, f)
+    return _model_path, _max_similarity
+
+  if len(_eps_fw) > 3:
+    _eps_fw = _eps_fw.replace("\\", "")
+    check_model = f"{_car} {_eps_fw}"
+  else:
+    check_model = _car
+  model_path, max_similarity = check_nn_path(check_model)
+  if 0.0 <= max_similarity < 0.9:
+    check_model = _car
+    model_path, max_similarity = check_nn_path(check_model)
+    if 0.0 <= max_similarity < 0.9:
+      model_path = None
+
+  _model_name = os.path.splitext(os.path.basename(model_path))[0] if model_path else "MOCK"
+
+  fuzzy_fingerprint = max_similarity < 0.99
+  return model_path, _model_name, fuzzy_fingerprint