From 0990212edc641378140b3b6ac339f4a0ae14391b Mon Sep 17 00:00:00 2001 From: Jason Wen Date: Fri, 21 Mar 2025 16:13:50 -0400 Subject: [PATCH] Revert "Controls: Neural Network Lateral Control (NNLC) for Torque Lateral Accel Control" (#694) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Revert "Controls: Neural Network Lateral Control (NNLC) for Torque Lateral Ac…" This reverts commit ecb4026269b793c6c1b03f61aaff8a3eeaa2939d. --- .gitmodules | 3 - cereal/custom.capnp | 12 -- common/params_keys.h | 3 - opendbc_repo | 2 +- selfdrive/ui/sunnypilot/SConscript | 1 - .../lateral/neural_network_lateral_control.cc | 63 ----------- .../lateral/neural_network_lateral_control.h | 64 ----------- .../qt/offroad/settings/lateral_panel.cc | 15 +-- .../qt/offroad/settings/lateral_panel.h | 2 - sunnypilot/neural_network_data | 1 - sunnypilot/selfdrive/car/interfaces.py | 24 ---- .../controls/lib/latcontrol_torque_ext.py | 6 +- .../selfdrive/controls/lib/nnlc/__init__.py | 0 .../selfdrive/controls/lib/nnlc/helpers.py | 58 ---------- .../selfdrive/controls/lib/nnlc/model.py | 81 ------------- .../selfdrive/controls/lib/nnlc/nnlc.py | 106 ------------------ system/manager/manager.py | 3 +- 17 files changed, 5 insertions(+), 439 deletions(-) delete mode 100644 selfdrive/ui/sunnypilot/qt/offroad/settings/lateral/neural_network_lateral_control.cc delete mode 100644 selfdrive/ui/sunnypilot/qt/offroad/settings/lateral/neural_network_lateral_control.h delete mode 160000 sunnypilot/neural_network_data delete mode 100644 sunnypilot/selfdrive/controls/lib/nnlc/__init__.py delete mode 100644 sunnypilot/selfdrive/controls/lib/nnlc/helpers.py delete mode 100644 sunnypilot/selfdrive/controls/lib/nnlc/model.py delete mode 100644 sunnypilot/selfdrive/controls/lib/nnlc/nnlc.py diff --git a/.gitmodules b/.gitmodules index b9f0336a0e..f1f7c400db 100644 --- a/.gitmodules +++ b/.gitmodules @@ -16,6 +16,3 @@ [submodule "tinygrad"] path = tinygrad_repo url = https://github.com/commaai/tinygrad.git -[submodule "sunnypilot/neural_network_data"] - path = sunnypilot/neural_network_data - url = https://github.com/sunnypilot/neural-network-data.git diff --git a/cereal/custom.capnp b/cereal/custom.capnp index 4d245c006b..524ecdd657 100644 --- a/cereal/custom.capnp +++ b/cereal/custom.capnp @@ -138,18 +138,6 @@ struct OnroadEventSP @0xda96579883444c35 { struct CarParamsSP @0x80ae746ee2596b11 { flags @0 :UInt32; # flags for car specific quirks in sunnypilot safetyParam @1 : Int16; # flags for sunnypilot's custom safety flags - - neuralNetworkLateralControl @2 :NeuralNetworkLateralControl; - - struct NeuralNetworkLateralControl { - model @0 :Model; - fuzzyFingerprint @1 :Bool; - - struct Model { - path @0 :Text; - name @1 :Text; - } - } } struct CarControlSP @0xa5cd762cd951a455 { diff --git a/common/params_keys.h b/common/params_keys.h index f95539963d..d88189d432 100644 --- a/common/params_keys.h +++ b/common/params_keys.h @@ -141,9 +141,6 @@ inline static std::unordered_map 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}, diff --git a/opendbc_repo b/opendbc_repo index c10bc5bd85..a26ea1e3ee 160000 --- a/opendbc_repo +++ b/opendbc_repo @@ -1 +1 @@ -Subproject commit c10bc5bd85a66b61e9f917a56154b6e3e2414288 +Subproject commit a26ea1e3ee982a4b2c377f5f75fdde63cba6a007 diff --git a/selfdrive/ui/sunnypilot/SConscript b/selfdrive/ui/sunnypilot/SConscript index 24c98c9868..6866bfb7d8 100644 --- a/selfdrive/ui/sunnypilot/SConscript +++ b/selfdrive/ui/sunnypilot/SConscript @@ -36,7 +36,6 @@ qt_src = [ lateral_panel_qt_src = [ "sunnypilot/qt/offroad/settings/lateral/mads_settings.cc", - "sunnypilot/qt/offroad/settings/lateral/neural_network_lateral_control.cc", ] network_src = [ diff --git a/selfdrive/ui/sunnypilot/qt/offroad/settings/lateral/neural_network_lateral_control.cc b/selfdrive/ui/sunnypilot/qt/offroad/settings/lateral/neural_network_lateral_control.cc deleted file mode 100644 index d3bc0fffa7..0000000000 --- a/selfdrive/ui/sunnypilot/qt/offroad/settings/lateral/neural_network_lateral_control.cc +++ /dev/null @@ -1,63 +0,0 @@ -/** - * 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. - */ - -#include "selfdrive/ui/sunnypilot/qt/offroad/settings/lateral/neural_network_lateral_control.h" - -NeuralNetworkLateralControl::NeuralNetworkLateralControl() : - ParamControl("NeuralNetworkLateralControl", tr("Neural Network Lateral Control (NNLC)"), "", "") { - setConfirmation(true, false); - updateToggle(); -} - -void NeuralNetworkLateralControl::showEvent(QShowEvent *event) { - updateToggle(); -} - -void NeuralNetworkLateralControl::updateToggle() { - QString statusInitText = "" + STATUS_CHECK_COMPATIBILITY + ""; - QString notLoadedText = "" + STATUS_NOT_LOADED + ""; - QString loadedText = "" + STATUS_LOADED + ""; - - auto cp_bytes = params.get("CarParamsPersistent"); - auto cp_sp_bytes = params.get("CarParamsSPPersistent"); - if (!cp_bytes.empty() && !cp_sp_bytes.empty()) { - AlignedBuffer aligned_buf; - AlignedBuffer aligned_buf_sp; - capnp::FlatArrayMessageReader cmsg(aligned_buf.align(cp_bytes.data(), cp_bytes.size())); - capnp::FlatArrayMessageReader cmsg_sp(aligned_buf_sp.align(cp_sp_bytes.data(), cp_sp_bytes.size())); - cereal::CarParams::Reader CP = cmsg.getRoot(); - cereal::CarParamsSP::Reader CP_SP = cmsg_sp.getRoot(); - - if (CP.getSteerControlType() == cereal::CarParams::SteerControlType::ANGLE) { - params.remove("NeuralNetworkLateralControl"); - setDescription(nnffDescriptionBuilder(STATUS_NOT_AVAILABLE)); - setEnabled(false); - } else { - QString nn_model_name = QString::fromStdString(CP_SP.getNeuralNetworkLateralControl().getModel().getName()); - QString nn_fuzzy = CP_SP.getNeuralNetworkLateralControl().getFuzzyFingerprint() ? - STATUS_MATCH_FUZZY : STATUS_MATCH_EXACT; - - if (nn_model_name.isEmpty()) { - setDescription(nnffDescriptionBuilder(statusInitText)); - } else if (nn_model_name == "MOCK") { - setDescription(nnffDescriptionBuilder( - notLoadedText + "
" + buildSupportText(SUPPORT_DONATE_LOGS) - )); - } else { - QString statusText = loadedText + " | " + STATUS_MATCH + " = " + nn_fuzzy + " | " + nn_model_name; - QString explanationText = EXPLANATION_MATCH + " " + buildSupportText(SUPPORT_ISSUES); - setDescription(nnffDescriptionBuilder(statusText + "

" + explanationText)); - } - } - } else { - setDescription(nnffDescriptionBuilder(statusInitText)); - } - - if (getDescription() != getBaseDescription()) { - showDescription(); - } -} diff --git a/selfdrive/ui/sunnypilot/qt/offroad/settings/lateral/neural_network_lateral_control.h b/selfdrive/ui/sunnypilot/qt/offroad/settings/lateral/neural_network_lateral_control.h deleted file mode 100644 index 18a3fac187..0000000000 --- a/selfdrive/ui/sunnypilot/qt/offroad/settings/lateral/neural_network_lateral_control.h +++ /dev/null @@ -1,64 +0,0 @@ -/** - * 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. - */ - -#pragma once - -#include - -#include "selfdrive/ui/sunnypilot/ui.h" -#include "selfdrive/ui/qt/util.h" -#include "selfdrive/ui/sunnypilot/qt/offroad/settings/settings.h" - -class NeuralNetworkLateralControl : public ParamControl { - Q_OBJECT - -public: - NeuralNetworkLateralControl(); - void showEvent(QShowEvent *event) override; - -public slots: - void updateToggle(); - -private: - Params params; - - void refresh(); - - // Status messages - const QString STATUS_NOT_AVAILABLE = tr("NNLC is currently not available on this platform."); - const QString STATUS_CHECK_COMPATIBILITY = tr("Start the car to check car compatibility"); - const QString STATUS_NOT_LOADED = tr("NNLC Not Loaded"); - const QString STATUS_LOADED = tr("NNLC Loaded"); - const QString STATUS_MATCH = tr("Match"); - const QString STATUS_MATCH_EXACT = tr("Exact"); - const QString STATUS_MATCH_FUZZY = tr("Fuzzy"); - - // Explanations - const QString EXPLANATION_MATCH = tr("Match: \"Exact\" is ideal, but \"Fuzzy\" is fine too."); - const QString EXPLANATION_FEATURE = tr("Formerly known as \"NNFF\", this replaces the lateral \"torque\" controller, " - "with one using a neural network trained on each car's (actually, each separate EPS firmware) driving data for increased controls accuracy."); - - // Support information - const QString SUPPORT_CHANNEL = "#tuning-nnlc"; - const QString SUPPORT_REACH_OUT = tr("Reach out to the sunnypilot team in the following channel at the sunnypilot Discord server"); - const QString SUPPORT_FEEDBACK = tr("with feedback, or to provide log data for your car if your car is currently unsupported:"); - const QString SUPPORT_ISSUES = tr("if there are any issues:"); - const QString SUPPORT_DONATE_LOGS = tr("and donate logs to get NNLC loaded for your car:"); - - // Description builders - QString buildSupportText(const QString& context) const { - return SUPPORT_REACH_OUT + " " + context + " " + SUPPORT_CHANNEL; - } - - QString nnffDescriptionBuilder(const QString &custom_description) const { - return "" + custom_description + "

" + getBaseDescription(); - } - - QString getBaseDescription() const { - return EXPLANATION_FEATURE + "

" + buildSupportText(SUPPORT_FEEDBACK); - } -}; diff --git a/selfdrive/ui/sunnypilot/qt/offroad/settings/lateral_panel.cc b/selfdrive/ui/sunnypilot/qt/offroad/settings/lateral_panel.cc index 750e26c118..e604b03ba9 100644 --- a/selfdrive/ui/sunnypilot/qt/offroad/settings/lateral_panel.cc +++ b/selfdrive/ui/sunnypilot/qt/offroad/settings/lateral_panel.cc @@ -42,21 +42,8 @@ LateralPanel::LateralPanel(SettingsWindowSP *parent) : QFrame(parent) { }); list->addItem(madsSettingsButton); - nnlcToggle = new NeuralNetworkLateralControl(); - list->addItem(nnlcToggle); - - QObject::connect(nnlcToggle, &ParamControl::toggleFlipped, [=](bool state) { - if (state) { - nnlcToggle->showDescription(); - } else { - nnlcToggle->hideDescription(); - } - - nnlcToggle->updateToggle(); - }); - toggleOffroadOnly = { - madsToggle, nnlcToggle, + madsToggle, }; QObject::connect(uiState(), &UIState::offroadTransition, this, &LateralPanel::updateToggles); diff --git a/selfdrive/ui/sunnypilot/qt/offroad/settings/lateral_panel.h b/selfdrive/ui/sunnypilot/qt/offroad/settings/lateral_panel.h index 0857e6ebd5..7cc4b6f11d 100644 --- a/selfdrive/ui/sunnypilot/qt/offroad/settings/lateral_panel.h +++ b/selfdrive/ui/sunnypilot/qt/offroad/settings/lateral_panel.h @@ -12,7 +12,6 @@ #include "selfdrive/ui/sunnypilot/ui.h" #include "selfdrive/ui/sunnypilot/qt/offroad/settings/lateral/mads_settings.h" -#include "selfdrive/ui/sunnypilot/qt/offroad/settings/lateral/neural_network_lateral_control.h" #include "selfdrive/ui/qt/util.h" #include "selfdrive/ui/sunnypilot/qt/offroad/settings/settings.h" #include "selfdrive/ui/sunnypilot/qt/widgets/scrollview.h" @@ -38,5 +37,4 @@ private: ParamControl *madsToggle; PushButtonSP *madsSettingsButton; MadsSettings *madsWidget = nullptr; - NeuralNetworkLateralControl *nnlcToggle = nullptr; }; diff --git a/sunnypilot/neural_network_data b/sunnypilot/neural_network_data deleted file mode 160000 index 1acc8f75fb..0000000000 --- a/sunnypilot/neural_network_data +++ /dev/null @@ -1 +0,0 @@ -Subproject commit 1acc8f75fbdf2ea38e0234e8f7410f85778fe487 diff --git a/sunnypilot/selfdrive/car/interfaces.py b/sunnypilot/selfdrive/car/interfaces.py index 0f36d34573..375ce7fa7f 100644 --- a/sunnypilot/selfdrive/car/interfaces.py +++ b/sunnypilot/selfdrive/car/interfaces.py @@ -8,12 +8,9 @@ See the LICENSE.md file in the root directory for more details. 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 @@ -25,25 +22,6 @@ 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, enabled: bool = False) -> None: - nnlc_model_path, nnlc_model_name, exact_match = 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" - nnlc_model_name = "MOCK" - - if nnlc_model_path != "MOCK" and CP.steerControlType != structs.CarParams.SteerControlType.angle: - enabled = params.get_bool("NeuralNetworkLateralControl") - - if enabled: - CarInterfaceBase.configure_torque_tune(CP.carFingerprint, CP.lateralTuning) - - CP_SP.neuralNetworkLateralControl.model.path = nnlc_model_path - CP_SP.neuralNetworkLateralControl.model.name = nnlc_model_name - CP_SP.neuralNetworkLateralControl.fuzzyFingerprint = not exact_match - - 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: @@ -54,8 +32,6 @@ 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): diff --git a/sunnypilot/selfdrive/controls/lib/latcontrol_torque_ext.py b/sunnypilot/selfdrive/controls/lib/latcontrol_torque_ext.py index ee8d64dcd0..1166cb32d6 100644 --- a/sunnypilot/selfdrive/controls/lib/latcontrol_torque_ext.py +++ b/sunnypilot/selfdrive/controls/lib/latcontrol_torque_ext.py @@ -4,11 +4,10 @@ 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.nnlc.nnlc import NeuralNetworkLateralControl +from openpilot.sunnypilot.selfdrive.controls.lib.latcontrol_torque_ext_base import LatControlTorqueExtBase -class LatControlTorqueExt(NeuralNetworkLateralControl): +class LatControlTorqueExt(LatControlTorqueExtBase): def __init__(self, lac_torque, CP, CP_SP): super().__init__(lac_torque, CP, CP_SP) @@ -23,6 +22,5 @@ class LatControlTorqueExt(NeuralNetworkLateralControl): self._actual_lateral_accel = actual_lateral_accel self.update_calculations(CS, VM, desired_lateral_accel) - self.update_neural_network_feedforward(CS, params, calibrated_pose) return self._ff, self._pid_log diff --git a/sunnypilot/selfdrive/controls/lib/nnlc/__init__.py b/sunnypilot/selfdrive/controls/lib/nnlc/__init__.py deleted file mode 100644 index e69de29bb2..0000000000 diff --git a/sunnypilot/selfdrive/controls/lib/nnlc/helpers.py b/sunnypilot/selfdrive/controls/lib/nnlc/helpers.py deleted file mode 100644 index 5f68a0a28e..0000000000 --- a/sunnypilot/selfdrive/controls/lib/nnlc/helpers.py +++ /dev/null @@ -1,58 +0,0 @@ -""" -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 os -from difflib import SequenceMatcher - -from opendbc.car import structs -from openpilot.common.basedir import BASEDIR - -TORQUE_NN_MODEL_PATH = os.path.join(BASEDIR, "sunnypilot", "neural_network_data", "neural_network_lateral_control") - - -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]: - exact_match = True - car_fingerprint = CP.carFingerprint - eps_fw = str(next((fw.fwVersion for fw in CP.carFw if fw.ecu == "eps"), "")) - model_name = "" - - def check_nn_path(nn_candidate): - _model_path = None - _max_similarity = -1.0 - for f in os.listdir(TORQUE_NN_MODEL_PATH): - if f.endswith(".json"): - model = os.path.splitext(f)[0] - similarity_score = similarity(model, nn_candidate) - 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("\\", "") - nn_candidate = f"{car_fingerprint} {eps_fw}" - model_path, max_similarity = check_nn_path(nn_candidate) - - if model_path is not None and car_fingerprint in model_path and max_similarity >= 0.9: - model_name = os.path.splitext(os.path.basename(model_path))[0] - exact_match = max_similarity >= 0.99 - return model_path, model_name, exact_match - - nn_candidate = car_fingerprint - model_path, max_similarity = check_nn_path(nn_candidate) - - if model_path is None or car_fingerprint not in model_path or max_similarity < 0.9: - model_path = None - - if model_path is not None: - model_name = os.path.splitext(os.path.basename(model_path))[0] - exact_match = max_similarity >= 0.99 - - return model_path, model_name, exact_match diff --git a/sunnypilot/selfdrive/controls/lib/nnlc/model.py b/sunnypilot/selfdrive/controls/lib/nnlc/model.py deleted file mode 100644 index 1e527c7370..0000000000 --- a/sunnypilot/selfdrive/controls/lib/nnlc/model.py +++ /dev/null @@ -1,81 +0,0 @@ -""" -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 json import load -import numpy as np - -# dict used to rename activation functions whose names aren't valid python identifiers -ACTIVATION_FUNCTION_NAMES = {'σ': 'sigmoid'} - - -class NNTorqueModel: - def __init__(self, params_file, zero_bias=False): - with open(params_file) 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 _, _, 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) diff --git a/sunnypilot/selfdrive/controls/lib/nnlc/nnlc.py b/sunnypilot/selfdrive/controls/lib/nnlc/nnlc.py deleted file mode 100644 index e1208ab829..0000000000 --- a/sunnypilot/selfdrive/controls/lib/nnlc/nnlc.py +++ /dev/null @@ -1,106 +0,0 @@ -""" -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 collections import deque -import math -import numpy as np - -from opendbc.car.interfaces import LatControlInputs -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 -from openpilot.sunnypilot.selfdrive.controls.lib.nnlc.model import NNTorqueModel - - -# At a given roll, if pitch magnitude increases, the -# gravitational acceleration component starts pointing -# in the longitudinal direction, decreasing the lateral -# acceleration component. Here we do the same thing -# to the roll value itself, then passed to nnff. -def roll_pitch_adjust(roll, pitch): - return roll * math.cos(pitch) - - -class NeuralNetworkLateralControl(LatControlTorqueExtBase): - def __init__(self, lac_torque, CP, CP_SP): - super().__init__(lac_torque, CP, CP_SP) - self.params = Params() - self.enabled = self.params.get_bool("NeuralNetworkLateralControl") - - # NN model takes current v_ego, lateral_accel, lat accel/jerk error, roll, and past/future/planned data - # of lat accel and roll - # Past value is computed using previous desired lat accel and observed roll - # Only initialize NNTorqueModel if enabled - self.model = NNTorqueModel(CP_SP.neuralNetworkLateralControl.model.path) if self.enabled else None - - self.pitch = FirstOrderFilter(0.0, 0.5, 0.01) - self.pitch_last = 0.0 - - # setup future time offsets - self.nn_time_offset = CP.steerActuatorDelay + 0.2 - future_times = [0.3, 0.6, 1.0, 1.5] # seconds in the future - self.nn_future_times = [i + self.nn_time_offset for i in future_times] - - # setup past time offsets - self.past_times = [-0.3, -0.2, -0.1] - history_check_frames = [int(abs(i)*100) for i in self.past_times] - self.history_frame_offsets = [history_check_frames[0] - i for i in history_check_frames] - self.lateral_accel_desired_deque = deque(maxlen=history_check_frames[0]) - self.roll_deque = deque(maxlen=history_check_frames[0]) - self.error_deque = deque(maxlen=history_check_frames[0]) - self.past_future_len = len(self.past_times) + len(self.nn_future_times) - - def update_neural_network_feedforward(self, CS, params, calibrated_pose): - if not self.enabled or not self.model_valid: - return - - # update past data - roll = params.roll - if calibrated_pose is not None: - pitch = self.pitch.update(calibrated_pose.orientation.pitch) - roll = roll_pitch_adjust(roll, pitch) - self.pitch_last = pitch - self.roll_deque.append(roll) - self.lateral_accel_desired_deque.append(self._desired_lateral_accel) - - # prepare past and future values - # adjust future times to account for longitudinal acceleration - adjusted_future_times = [t + 0.5 * CS.aEgo * (t / max(CS.vEgo, 1.0)) for t in self.nn_future_times] - past_rolls = [self.roll_deque[min(len(self.roll_deque) - 1, i)] for i in self.history_frame_offsets] - future_rolls = [roll_pitch_adjust(np.interp(t, ModelConstants.T_IDXS, self.model_v2.orientation.x) + roll, - np.interp(t, ModelConstants.T_IDXS, self.model_v2.orientation.y) + self.pitch_last) for t in - adjusted_future_times] - past_lateral_accels_desired = [self.lateral_accel_desired_deque[min(len(self.lateral_accel_desired_deque) - 1, i)] - for i in self.history_frame_offsets] - future_planned_lateral_accels = [np.interp(t, ModelConstants.T_IDXS, self.model_v2.acceleration.y) for t in - adjusted_future_times] - - # compute NNFF error response - nnff_setpoint_input = [CS.vEgo, self._setpoint, self.lateral_jerk_setpoint, roll] \ - + [self._setpoint] * self.past_future_len \ - + past_rolls + future_rolls - # past lateral accel error shouldn't count, so use past desired like the setpoint input - nnff_measurement_input = [CS.vEgo, self._measurement, self.lateral_jerk_measurement, roll] \ - + [self._measurement] * self.past_future_len \ - + past_rolls + future_rolls - torque_from_setpoint = self.model.evaluate(nnff_setpoint_input) - torque_from_measurement = self.model.evaluate(nnff_measurement_input) - self._pid_log.error = torque_from_setpoint - torque_from_measurement - - # compute feedforward (same as nn setpoint output) - friction_input = self.update_friction_input(self._setpoint, self._measurement) - nn_input = [CS.vEgo, self._desired_lateral_accel, friction_input, roll] \ - + past_lateral_accels_desired + future_planned_lateral_accels \ - + past_rolls + future_rolls - self._ff = self.model.evaluate(nn_input) - - # apply friction override for cars with low NN friction response - if self.model.friction_override: - self._pid_log.error += self.torque_from_lateral_accel(LatControlInputs(0.0, 0.0, CS.vEgo, CS.aEgo), self.torque_params, - friction_input, - self._lateral_accel_deadzone, friction_compensation=True, - gravity_adjusted=False) diff --git a/system/manager/manager.py b/system/manager/manager.py index 9266a62aae..d4595ec327 100755 --- a/system/manager/manager.py +++ b/system/manager/manager.py @@ -49,8 +49,7 @@ def manager_init() -> None: ("MadsSteeringMode", "0"), ("MadsUnifiedEngagementMode", "1"), ("ModelManager_LastSyncTime", "0"), - ("ModelManager_ModelsCache", ""), - ("NeuralNetworkLateralControl", "0"), + ("ModelManager_ModelsCache", "") ] if params.get_bool("RecordFrontLock"):