diff --git a/sunnypilot/selfdrive/controls/lib/nnlc/model.py b/sunnypilot/selfdrive/controls/lib/nnlc/model.py index 1e527c737..ab6706bf3 100644 --- a/sunnypilot/selfdrive/controls/lib/nnlc/model.py +++ b/sunnypilot/selfdrive/controls/lib/nnlc/model.py @@ -7,6 +7,8 @@ See the LICENSE.md file in the root directory for more details. from json import load import numpy as np +from openpilot.selfdrive.modeld.parse_model_outputs import safe_exp + # dict used to rename activation functions whose names aren't valid python identifiers ACTIVATION_FUNCTION_NAMES = {'σ': 'sigmoid'} @@ -40,7 +42,7 @@ class NNTorqueModel: # These are called by name using the keys in the model json file @staticmethod def sigmoid(x): - return 1 / (1 + np.exp(-x)) + return 1 / (1 + safe_exp(-x)) @staticmethod def identity(x): diff --git a/sunnypilot/selfdrive/controls/lib/nnlc/tests/test_nnlc.py b/sunnypilot/selfdrive/controls/lib/nnlc/tests/test_nnlc.py index c7227c2d5..4e2649f2f 100644 --- a/sunnypilot/selfdrive/controls/lib/nnlc/tests/test_nnlc.py +++ b/sunnypilot/selfdrive/controls/lib/nnlc/tests/test_nnlc.py @@ -1,6 +1,7 @@ +import numpy as np from parameterized import parameterized -from cereal import car, log +from cereal import car, log, messaging from opendbc.car.car_helpers import interfaces from opendbc.car.honda.values import CAR as HONDA from opendbc.car.hyundai.values import CAR as HYUNDAI @@ -12,6 +13,30 @@ 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 generate_modelV2(): + model = messaging.new_message('modelV2') + position = log.XYZTData.new_message() + speed = 30 + position.x = [float(x) for x in (speed + 0.5) * np.array(ModelConstants.T_IDXS)] + model.modelV2.position = position + orientation = log.XYZTData.new_message() + curvature = 0.05 + orientation.x = [float(curvature) 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 = [float(x) for x in (speed + 0.5) * np.ones_like(ModelConstants.T_IDXS)] + velocity.x[0] = float(speed) # always start at current speed + model.modelV2.velocity = velocity + acceleration = log.XYZTData.new_message() + acceleration.x = [float(x) for x in np.zeros_like(ModelConstants.T_IDXS)] + acceleration.y = [float(y) for y in np.zeros_like(ModelConstants.T_IDXS)] + model.modelV2.acceleration = acceleration + + return model class TestNeuralNetworkLateralControl: @@ -42,15 +67,23 @@ class TestNeuralNetworkLateralControl: lp = generate_livePose() pose = Pose.from_live_pose(lp.livePose) + mdl = generate_modelV2() + sm = {'modelV2': mdl.modelV2} + model_v2 = sm['modelV2'] + controller.extension.model_v2 = model_v2 + # Saturate for curvature limited and controller limited for _ in range(1000): + controller.extension.update_model_v2(model_v2) _, _, lac_log = controller.update(True, CS, VM, params, False, 0, pose, True) assert lac_log.saturated for _ in range(1000): + controller.extension.update_model_v2(model_v2) _, _, lac_log = controller.update(True, CS, VM, params, False, 0, pose, False) assert not lac_log.saturated for _ in range(1000): + controller.extension.update_model_v2(model_v2) _, _, lac_log = controller.update(True, CS, VM, params, False, 1, pose, False) assert lac_log.saturated