NNLC: use safe_exp to prevent overflow in sigmoid (#836)

* test

* prevent overflowing

* unused

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):

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