sunnypilot v2026.002.000
version: sunnypilot v2026.002.000 (staging) date: 2026-07-21T07:44:02 master commit: 2c334ede443d7391d27575af8c854a095ba702a8
This commit is contained in:
commit
de77c89cf6
3995 files changed
+1260688
No files matched your search
Whitespace-only changes.
@@ -0,0 +1,68 @@
|
||||
"""
|
||||
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
|
||||
import tomllib
|
||||
from difflib import SequenceMatcher
|
||||
|
||||
from opendbc.car import structs
|
||||
from openpilot.common.basedir import BASEDIR
|
||||
|
||||
TORQUE_NN_MODEL_PATH = os.path.join(BASEDIR, "openpilot", "sunnypilot", "neural_network_data", "neural_network_lateral_control")
|
||||
TORQUE_NN_MODEL_SUBSTITUTE_PATH = os.path.join(BASEDIR, "opendbc_repo", "opendbc", "car", "torque_data/substitute.toml")
|
||||
MOCK_MODEL_PATH = os.path.join(TORQUE_NN_MODEL_PATH, "MOCK.json")
|
||||
|
||||
|
||||
def similarity(s1: str, s2: str) -> float:
|
||||
return SequenceMatcher(None, s1, s2).ratio()
|
||||
|
||||
|
||||
def get_nn_model_path(CP: structs.CarParams) -> tuple[str, str, bool]:
|
||||
car_fingerprint = CP.carFingerprint
|
||||
eps_fw = str(next((fw.fwVersion for fw in CP.carFw if fw.ecu == "eps"), ""))
|
||||
|
||||
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}"
|
||||
else:
|
||||
nn_candidate = car_fingerprint
|
||||
|
||||
model_path, max_similarity = check_nn_path(nn_candidate)
|
||||
exact_match = max_similarity >= 0.99
|
||||
|
||||
if car_fingerprint not in model_path or 0.0 <= max_similarity < 0.9:
|
||||
nn_candidate = car_fingerprint
|
||||
model_path, max_similarity = check_nn_path(nn_candidate)
|
||||
exact_match = max_similarity >= 0.99
|
||||
|
||||
if 0.0 <= max_similarity < 0.9:
|
||||
with open(TORQUE_NN_MODEL_SUBSTITUTE_PATH, 'rb') as f:
|
||||
sub = tomllib.load(f)
|
||||
sub_candidate = sub.get(car_fingerprint, car_fingerprint)
|
||||
|
||||
for candidate in [car_fingerprint, sub_candidate]:
|
||||
model_path, max_similarity = check_nn_path(candidate)
|
||||
|
||||
exact_match = False
|
||||
|
||||
if CP.steerControlType == structs.CarParams.SteerControlType.angle:
|
||||
model_path = MOCK_MODEL_PATH
|
||||
|
||||
model_name = os.path.splitext(os.path.basename(model_path))[0]
|
||||
|
||||
return model_path, model_name, exact_match
|
||||
@@ -0,0 +1,83 @@
|
||||
"""
|
||||
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
|
||||
|
||||
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'}
|
||||
|
||||
|
||||
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 + safe_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)
|
||||
@@ -0,0 +1,164 @@
|
||||
"""
|
||||
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.lateral import FRICTION_THRESHOLD, get_friction
|
||||
from opendbc.sunnypilot.car.interfaces import LatControlInputs
|
||||
from opendbc.sunnypilot.car.lateral_ext import get_friction as get_friction_in_torque_space
|
||||
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, sign
|
||||
from openpilot.sunnypilot.selfdrive.controls.lib.nnlc.helpers import MOCK_MODEL_PATH
|
||||
from openpilot.sunnypilot.selfdrive.controls.lib.nnlc.model import NNTorqueModel
|
||||
|
||||
LOW_SPEED_X = [0, 10, 20, 30]
|
||||
LOW_SPEED_Y = [12, 3, 1, 0]
|
||||
|
||||
|
||||
# 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, CI):
|
||||
super().__init__(lac_torque, CP, CP_SP, CI)
|
||||
self.params = Params()
|
||||
self.enabled = self.params.get_bool("NeuralNetworkLateralControl")
|
||||
self.has_nn_model = CP_SP.neuralNetworkLateralControl.model.path != MOCK_MODEL_PATH
|
||||
|
||||
# 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
|
||||
self.model = NNTorqueModel(CP_SP.neuralNetworkLateralControl.model.path)
|
||||
|
||||
self.pitch = FirstOrderFilter(0.0, 0.5, 0.01)
|
||||
self.pitch_last = 0.0
|
||||
|
||||
# setup future time offsets
|
||||
self.future_times = [0.3, 0.6, 1.0, 1.5] # seconds in the future
|
||||
self.nn_future_times = [i + self.desired_lat_jerk_time for i in self.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)
|
||||
|
||||
@property
|
||||
def _nnlc_enabled(self):
|
||||
return self.enabled and self.model_valid and self.has_nn_model
|
||||
|
||||
def update_limits(self):
|
||||
if not self._nnlc_enabled:
|
||||
return
|
||||
|
||||
self._pid.set_limits(self.lac_torque.steer_max, -self.lac_torque.steer_max)
|
||||
|
||||
def update_lateral_lag(self, lag):
|
||||
super().update_lateral_lag(lag)
|
||||
self.nn_future_times = [t + self.desired_lat_jerk_time for t in self.future_times]
|
||||
|
||||
def update_feedforward_torque_space(self, CS):
|
||||
torque_from_setpoint = self.torque_from_lateral_accel_in_torque_space(LatControlInputs(self._setpoint, self._roll_compensation, CS.vEgo, CS.aEgo),
|
||||
self.torque_params, gravity_adjusted=False)
|
||||
torque_from_measurement = self.torque_from_lateral_accel_in_torque_space(LatControlInputs(self._measurement, self._roll_compensation, CS.vEgo, CS.aEgo),
|
||||
self.torque_params, gravity_adjusted=False)
|
||||
self._pid_log.error = float(torque_from_setpoint - torque_from_measurement)
|
||||
self._ff = self.torque_from_lateral_accel_in_torque_space(LatControlInputs(self._gravity_adjusted_lateral_accel, self._roll_compensation,
|
||||
CS.vEgo, CS.aEgo), self.torque_params, gravity_adjusted=True)
|
||||
self._ff += get_friction_in_torque_space(self._desired_lateral_accel - self._actual_lateral_accel, self._lateral_accel_deadzone,
|
||||
FRICTION_THRESHOLD, self.torque_params)
|
||||
|
||||
def update_output_torque(self, CS):
|
||||
freeze_integrator = self._steer_limited_by_safety or CS.steeringPressed or CS.vEgo < 5
|
||||
self._output_torque = self._pid.update(self._pid_log.error,
|
||||
feedforward=self._ff,
|
||||
speed=CS.vEgo,
|
||||
freeze_integrator=freeze_integrator)
|
||||
|
||||
def update_neural_network_feedforward(self, CS, params, calibrated_pose) -> None:
|
||||
if not self._nnlc_enabled:
|
||||
return
|
||||
|
||||
self.update_feedforward_torque_space(CS)
|
||||
|
||||
low_speed_factor = float(np.interp(CS.vEgo, LOW_SPEED_X, LOW_SPEED_Y)) ** 2
|
||||
self._setpoint = self._desired_lateral_accel + low_speed_factor * self._desired_curvature
|
||||
self._measurement = self._actual_lateral_accel + low_speed_factor * self._actual_curvature
|
||||
|
||||
# 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
|
||||
|
||||
# The "pure" NNLC error response can be too weak for cars whose models were trained
|
||||
# with a lack of high-magnitude lateral acceleration data, for which the NNLC model
|
||||
# torque response flattens out at high lateral accelerations.
|
||||
# This workaround blends in a guaranteed stronger error response only when the
|
||||
# desired lateral acceleration is high enough to warrant it, by using the lateral acceleration
|
||||
# error as the input to the NNLC model. This is not ideal, and potentially degrades the NNLC
|
||||
# accuracy for cars that don't have this issue, but it's necessary until a better NNLC model
|
||||
# structure is used that doesn't create this issue when high-magnitude data is missing.
|
||||
error_blend_factor = float(np.interp(abs(self._desired_lateral_accel), [1.0, 2.0], [0.0, 1.0]))
|
||||
if error_blend_factor > 0.0: # blend in stronger error response when in high lat accel
|
||||
# NNFF inputs 5+ are optional, and if left out are replaced with 0.0 inside the NNFF class
|
||||
nnff_error_input = [CS.vEgo, self._setpoint - self._measurement, self.lateral_jerk_setpoint - self.lateral_jerk_measurement, 0.0]
|
||||
torque_from_error = self.model.evaluate(nnff_error_input)
|
||||
if sign(self._pid_log.error) == sign(torque_from_error) and abs(self._pid_log.error) < abs(torque_from_error):
|
||||
self._pid_log.error = self._pid_log.error * (1.0 - error_blend_factor) + torque_from_error * error_blend_factor
|
||||
|
||||
# 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 += get_friction(friction_input, self._lateral_accel_deadzone, FRICTION_THRESHOLD, self.torque_params)
|
||||
|
||||
self.update_output_torque(CS)
|
||||
Whitespace-only changes.
@@ -0,0 +1,43 @@
|
||||
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
|
||||
from opendbc.car.nissan.values import CAR as NISSAN
|
||||
from opendbc.car.toyota.values import CAR as TOYOTA
|
||||
from opendbc.car.tesla.values import CAR as TESLA
|
||||
from openpilot.common.parameterized import parameterized
|
||||
from openpilot.common.params import Params
|
||||
from openpilot.sunnypilot.selfdrive.car import interfaces as sunnypilot_interfaces
|
||||
|
||||
|
||||
FINGERPRINT_EXACT_MATCH = [HONDA.HONDA_CIVIC_BOSCH, TOYOTA.TOYOTA_RAV4_TSS2_2022, HYUNDAI.HYUNDAI_IONIQ_5]
|
||||
FINGERPRINT_FUZZY_MATCH = [HONDA.HONDA_CIVIC_BOSCH_DIESEL, HYUNDAI.GENESIS_G70_2020, HYUNDAI.HYUNDAI_IONIQ_6]
|
||||
FINGERPRINT_ANGLE_NO_MATCH = [TOYOTA.TOYOTA_RAV4_TSS2_2023, NISSAN.NISSAN_LEAF, TESLA.TESLA_MODEL_3]
|
||||
|
||||
|
||||
class TestNNLCFingerprintBase:
|
||||
|
||||
@staticmethod
|
||||
def _setup_platform(car_name):
|
||||
CarInterface = interfaces[car_name]
|
||||
CP = CarInterface.get_non_essential_params(car_name)
|
||||
CP_SP = CarInterface.get_non_essential_params_sp(CP, car_name)
|
||||
CI = CarInterface(CP, CP_SP)
|
||||
|
||||
sunnypilot_interfaces.setup_interfaces(CI, Params())
|
||||
|
||||
return CI
|
||||
|
||||
@parameterized.expand(FINGERPRINT_EXACT_MATCH)
|
||||
def test_exact_fingerprint(self, car_name):
|
||||
CI = self._setup_platform(car_name)
|
||||
assert CI.CP_SP.neuralNetworkLateralControl.model.name != "MOCK" and not CI.CP_SP.neuralNetworkLateralControl.fuzzyFingerprint
|
||||
|
||||
@parameterized.expand(FINGERPRINT_FUZZY_MATCH)
|
||||
def test_fuzzy_fingerprint(self, car_name):
|
||||
CI = self._setup_platform(car_name)
|
||||
assert CI.CP_SP.neuralNetworkLateralControl.model.name != "MOCK" and CI.CP_SP.neuralNetworkLateralControl.fuzzyFingerprint
|
||||
|
||||
@parameterized.expand(FINGERPRINT_ANGLE_NO_MATCH)
|
||||
def test_no_fingerprint(self, car_name):
|
||||
CI = self._setup_platform(car_name)
|
||||
assert CI.CP_SP.neuralNetworkLateralControl.model.name == "MOCK"
|
||||
@@ -0,0 +1,31 @@
|
||||
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
|
||||
from opendbc.car.toyota.values import CAR as TOYOTA
|
||||
from openpilot.common.parameterized import parameterized
|
||||
from openpilot.common.params import Params
|
||||
from openpilot.common.realtime import DT_CTRL
|
||||
from openpilot.selfdrive.car.helpers import convert_to_capnp
|
||||
from openpilot.selfdrive.controls.lib.latcontrol_torque import LatControlTorque
|
||||
from openpilot.sunnypilot.selfdrive.car import interfaces as sunnypilot_interfaces
|
||||
|
||||
|
||||
class TestNNTorqueModel:
|
||||
|
||||
@parameterized.expand([HONDA.HONDA_CIVIC, TOYOTA.TOYOTA_RAV4, HYUNDAI.HYUNDAI_SANTA_CRUZ_1ST_GEN])
|
||||
def test_load_model(self, car_name):
|
||||
params = Params()
|
||||
params.put_bool("NeuralNetworkLateralControl", True, block=True)
|
||||
|
||||
CarInterface = interfaces[car_name]
|
||||
CP = CarInterface.get_non_essential_params(car_name)
|
||||
CP_SP = CarInterface.get_non_essential_params_sp(CP, car_name)
|
||||
CI = CarInterface(CP, CP_SP)
|
||||
|
||||
sunnypilot_interfaces.setup_interfaces(CI, params)
|
||||
|
||||
CP_SP = convert_to_capnp(CP_SP)
|
||||
|
||||
controller = LatControlTorque(CP.as_reader(), CP_SP.as_reader(), CI, DT_CTRL)
|
||||
|
||||
assert controller.extension.has_nn_model
|
||||
@@ -0,0 +1,103 @@
|
||||
import numpy as np
|
||||
|
||||
from openpilot.cereal import log, messaging
|
||||
from opendbc.car.structs import car
|
||||
from opendbc.car.car_helpers import interfaces
|
||||
from opendbc.car.gm.values import CAR as GM
|
||||
from opendbc.car.honda.values import CAR as HONDA
|
||||
from opendbc.car.hyundai.values import CAR as HYUNDAI
|
||||
from opendbc.car.toyota.values import CAR as TOYOTA
|
||||
from opendbc.car.vehicle_model import VehicleModel
|
||||
from openpilot.common.parameterized import parameterized
|
||||
from openpilot.common.params import Params
|
||||
from openpilot.common.realtime import DT_CTRL
|
||||
from openpilot.selfdrive.car.helpers import convert_to_capnp
|
||||
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:
|
||||
|
||||
@parameterized.expand([HONDA.HONDA_CIVIC, TOYOTA.TOYOTA_RAV4, HYUNDAI.HYUNDAI_SANTA_CRUZ_1ST_GEN, GM.CHEVROLET_BOLT_EUV])
|
||||
def test_saturation(self, car_name):
|
||||
params = Params()
|
||||
params.put_bool("NeuralNetworkLateralControl", True, block=True)
|
||||
|
||||
CarInterface = interfaces[car_name]
|
||||
CP = CarInterface.get_non_essential_params(car_name)
|
||||
CP_SP = CarInterface.get_non_essential_params_sp(CP, car_name)
|
||||
CI = CarInterface(CP, CP_SP)
|
||||
|
||||
sunnypilot_interfaces.setup_interfaces(CI, params)
|
||||
|
||||
CP_SP = convert_to_capnp(CP_SP)
|
||||
VM = VehicleModel(CP)
|
||||
|
||||
controller = LatControlTorque(CP.as_reader(), CP_SP.as_reader(), CI, DT_CTRL)
|
||||
torque_params = CP.lateralTuning.torque
|
||||
|
||||
CS = car.CarState.new_message()
|
||||
CS.vEgo = 30
|
||||
CS.steeringPressed = False
|
||||
|
||||
params = log.LiveParametersData.new_message()
|
||||
|
||||
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
|
||||
test_lag = 0.3
|
||||
for _ in range(1000):
|
||||
controller.extension.update_model_v2(model_v2)
|
||||
controller.extension.update_lateral_lag(test_lag)
|
||||
controller.update_live_torque_params(torque_params.latAccelFactor, torque_params.latAccelOffset, torque_params.friction)
|
||||
controller.extension.update_limits()
|
||||
_, _, lac_log = controller.update(True, CS, VM, params, False, 0, pose, True, 0.2)
|
||||
assert lac_log.saturated
|
||||
|
||||
for _ in range(1000):
|
||||
controller.extension.update_model_v2(model_v2)
|
||||
controller.extension.update_lateral_lag(test_lag)
|
||||
controller.update_live_torque_params(torque_params.latAccelFactor, torque_params.latAccelOffset, torque_params.friction)
|
||||
controller.extension.update_limits()
|
||||
_, _, lac_log = controller.update(True, CS, VM, params, False, 0, pose, False, 0.2)
|
||||
assert not lac_log.saturated
|
||||
|
||||
for _ in range(1000):
|
||||
controller.extension.update_model_v2(model_v2)
|
||||
controller.extension.update_lateral_lag(test_lag)
|
||||
controller.update_live_torque_params(torque_params.latAccelFactor, torque_params.latAccelOffset, torque_params.friction)
|
||||
controller.extension.update_limits()
|
||||
_, _, lac_log = controller.update(True, CS, VM, params, False, 1, pose, False, 0.2)
|
||||
assert lac_log.saturated
|
||||
Reference in new issue
Block a user