From ecb4026269b793c6c1b03f61aaff8a3eeaa2939d Mon Sep 17 00:00:00 2001 From: Jason Wen Date: Fri, 21 Mar 2025 15:38:31 -0400 Subject: [PATCH] Controls: Neural Network Lateral Control (NNLC) for Torque Lateral Accel Control (#667) * 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 * split Enhanced Lat Accel --------- Co-authored-by: DevTekVE --- .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, 439 insertions(+), 5 deletions(-) create mode 100644 selfdrive/ui/sunnypilot/qt/offroad/settings/lateral/neural_network_lateral_control.cc create mode 100644 selfdrive/ui/sunnypilot/qt/offroad/settings/lateral/neural_network_lateral_control.h create mode 160000 sunnypilot/neural_network_data create mode 100644 sunnypilot/selfdrive/controls/lib/nnlc/__init__.py create mode 100644 sunnypilot/selfdrive/controls/lib/nnlc/helpers.py create mode 100644 sunnypilot/selfdrive/controls/lib/nnlc/model.py create mode 100644 sunnypilot/selfdrive/controls/lib/nnlc/nnlc.py diff --git a/.gitmodules b/.gitmodules index f1f7c400db..b9f0336a0e 100644 --- a/.gitmodules +++ b/.gitmodules @@ -16,3 +16,6 @@ [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 524ecdd657..4d245c006b 100644 --- a/cereal/custom.capnp +++ b/cereal/custom.capnp @@ -138,6 +138,18 @@ 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 d88189d432..f95539963d 100644 --- a/common/params_keys.h +++ b/common/params_keys.h @@ -141,6 +141,9 @@ 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 a26ea1e3ee..c10bc5bd85 160000 --- a/opendbc_repo +++ b/opendbc_repo @@ -1 +1 @@ -Subproject commit a26ea1e3ee982a4b2c377f5f75fdde63cba6a007 +Subproject commit c10bc5bd85a66b61e9f917a56154b6e3e2414288 diff --git a/selfdrive/ui/sunnypilot/SConscript b/selfdrive/ui/sunnypilot/SConscript index 6866bfb7d8..24c98c9868 100644 --- a/selfdrive/ui/sunnypilot/SConscript +++ b/selfdrive/ui/sunnypilot/SConscript @@ -36,6 +36,7 @@ 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 new file mode 100644 index 0000000000..d3bc0fffa7 --- /dev/null +++ b/selfdrive/ui/sunnypilot/qt/offroad/settings/lateral/neural_network_lateral_control.cc @@ -0,0 +1,63 @@ +/** + * 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 new file mode 100644 index 0000000000..18a3fac187 --- /dev/null +++ b/selfdrive/ui/sunnypilot/qt/offroad/settings/lateral/neural_network_lateral_control.h @@ -0,0 +1,64 @@ +/** + * 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 e604b03ba9..750e26c118 100644 --- a/selfdrive/ui/sunnypilot/qt/offroad/settings/lateral_panel.cc +++ b/selfdrive/ui/sunnypilot/qt/offroad/settings/lateral_panel.cc @@ -42,8 +42,21 @@ 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, + madsToggle, nnlcToggle, }; 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 7cc4b6f11d..0857e6ebd5 100644 --- a/selfdrive/ui/sunnypilot/qt/offroad/settings/lateral_panel.h +++ b/selfdrive/ui/sunnypilot/qt/offroad/settings/lateral_panel.h @@ -12,6 +12,7 @@ #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" @@ -37,4 +38,5 @@ private: ParamControl *madsToggle; PushButtonSP *madsSettingsButton; MadsSettings *madsWidget = nullptr; + NeuralNetworkLateralControl *nnlcToggle = nullptr; }; diff --git a/sunnypilot/neural_network_data b/sunnypilot/neural_network_data new file mode 160000 index 0000000000..1acc8f75fb --- /dev/null +++ b/sunnypilot/neural_network_data @@ -0,0 +1 @@ +Subproject commit 1acc8f75fbdf2ea38e0234e8f7410f85778fe487 diff --git a/sunnypilot/selfdrive/car/interfaces.py b/sunnypilot/selfdrive/car/interfaces.py index 375ce7fa7f..0f36d34573 100644 --- a/sunnypilot/selfdrive/car/interfaces.py +++ b/sunnypilot/selfdrive/car/interfaces.py @@ -8,9 +8,12 @@ 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 @@ -22,6 +25,25 @@ 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: @@ -32,6 +54,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): diff --git a/sunnypilot/selfdrive/controls/lib/latcontrol_torque_ext.py b/sunnypilot/selfdrive/controls/lib/latcontrol_torque_ext.py index 1166cb32d6..ee8d64dcd0 100644 --- a/sunnypilot/selfdrive/controls/lib/latcontrol_torque_ext.py +++ b/sunnypilot/selfdrive/controls/lib/latcontrol_torque_ext.py @@ -4,10 +4,11 @@ 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 + +from openpilot.sunnypilot.selfdrive.controls.lib.nnlc.nnlc import NeuralNetworkLateralControl -class LatControlTorqueExt(LatControlTorqueExtBase): +class LatControlTorqueExt(NeuralNetworkLateralControl): def __init__(self, lac_torque, CP, CP_SP): super().__init__(lac_torque, CP, CP_SP) @@ -22,5 +23,6 @@ class LatControlTorqueExt(LatControlTorqueExtBase): 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 new file mode 100644 index 0000000000..e69de29bb2 diff --git a/sunnypilot/selfdrive/controls/lib/nnlc/helpers.py b/sunnypilot/selfdrive/controls/lib/nnlc/helpers.py new file mode 100644 index 0000000000..5f68a0a28e --- /dev/null +++ b/sunnypilot/selfdrive/controls/lib/nnlc/helpers.py @@ -0,0 +1,58 @@ +""" +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 new file mode 100644 index 0000000000..1e527c7370 --- /dev/null +++ b/sunnypilot/selfdrive/controls/lib/nnlc/model.py @@ -0,0 +1,81 @@ +""" +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 new file mode 100644 index 0000000000..e1208ab829 --- /dev/null +++ b/sunnypilot/selfdrive/controls/lib/nnlc/nnlc.py @@ -0,0 +1,106 @@ +""" +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 d4595ec327..9266a62aae 100755 --- a/system/manager/manager.py +++ b/system/manager/manager.py @@ -49,7 +49,8 @@ def manager_init() -> None: ("MadsSteeringMode", "0"), ("MadsUnifiedEngagementMode", "1"), ("ModelManager_LastSyncTime", "0"), - ("ModelManager_ModelsCache", "") + ("ModelManager_ModelsCache", ""), + ("NeuralNetworkLateralControl", "0"), ] if params.get_bool("RecordFrontLock"):