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