mawei/openpilot-evo
1
0
Fork 0
mirror of https://github.com/infiniteCable2/openpilot.git synced 2026-07-25 19:32:03 +08:00
Code Issues Packages Projects Releases Wiki Activity

NNLC: compute error in torque space (#1185)

Browse Source 
* NNLC: compute error in torque space

* bump

* sp happy too

* bump

* lint

* update path

* oops

* test entire loop

* bump

* test gm

* bump

* bump
This commit is contained in:
Jason Wen
2025-08-29 16:39:25 -04:00
committed by GitHub
parent 54174d1ef0
commit ba1da60c25
7 changed files with 75 additions and 19 deletions
Download Patch File Download Diff File Expand all files Collapse all files
opendbc_repo
+1 -1
Submodule opendbc_repo updated: ee25c18829...004fa8df07
selfdrive/controls/controlsd.py
+2
View File
@@ -95,6 +95,8 @@ class Controls(ControlsExt, ModelStateBase):
self.LaC.update_live_torque_params(torque_params.latAccelFactorFiltered, torque_params.latAccelOffsetFiltered,
torque_params.frictionCoefficientFiltered)
self.LaC.extension.update_limits()
self.LaC.extension.update_model_v2(self.sm['modelV2'])
self.lat_delay = get_lat_delay(self.params, self.sm["liveDelay"].lateralDelay)
selfdrive/controls/lib/latcontrol_torque.py
+7 -7
View File
@@ -35,7 +35,7 @@ class LatControlTorque(LatControl):
self.update_limits()
self.steering_angle_deadzone_deg = self.torque_params.steeringAngleDeadzoneDeg
self.extension = LatControlTorqueExt(self, CP, CP_SP)
self.extension = LatControlTorqueExt(self, CP, CP_SP, CI)
def update_live_torque_params(self, latAccelFactor, latAccelOffset, friction):
self.torque_params.latAccelFactor = latAccelFactor
@@ -73,12 +73,6 @@ class LatControlTorque(LatControl):
ff = gravity_adjusted_lateral_accel
ff += get_friction(desired_lateral_accel - actual_lateral_accel, lateral_accel_deadzone, FRICTION_THRESHOLD, self.torque_params)
# Lateral acceleration torque controller extension updates
# Overrides stock ff and pid_log.error
ff, pid_log = self.extension.update(CS, VM, params, ff, pid_log, setpoint, measurement, calibrated_pose, roll_compensation,
desired_lateral_accel, actual_lateral_accel, lateral_accel_deadzone, gravity_adjusted_lateral_accel,
desired_curvature, actual_curvature)
freeze_integrator = steer_limited_by_safety or CS.steeringPressed or CS.vEgo < 5
output_lataccel = self.pid.update(pid_log.error,
feedforward=ff,
@@ -86,6 +80,12 @@ class LatControlTorque(LatControl):
freeze_integrator=freeze_integrator)
output_torque = self.torque_from_lateral_accel(output_lataccel, self.torque_params)
# Lateral acceleration torque controller extension updates
# Overrides pid_log.error and output_torque
pid_log, output_torque = self.extension.update(CS, VM, self.pid, params, ff, pid_log, setpoint, measurement, calibrated_pose, roll_compensation,
desired_lateral_accel, actual_lateral_accel, lateral_accel_deadzone, gravity_adjusted_lateral_accel,
desired_curvature, actual_curvature, steer_limited_by_safety, output_torque)
pid_log.active = True
pid_log.p = float(self.pid.p)
pid_log.i = float(self.pid.i)
sunnypilot/selfdrive/controls/lib/latcontrol_torque_ext.py
+10 -5
View File
@@ -9,23 +9,28 @@ from openpilot.sunnypilot.selfdrive.controls.lib.nnlc.nnlc import NeuralNetworkL
class LatControlTorqueExt(NeuralNetworkLateralControl):
def __init__(self, lac_torque, CP, CP_SP):
super().__init__(lac_torque, CP, CP_SP)
def __init__(self, lac_torque, CP, CP_SP, CI):
super().__init__(lac_torque, CP, CP_SP, CI)
def update(self, CS, VM, params, ff, pid_log, setpoint, measurement, calibrated_pose, roll_compensation,
def update(self, CS, VM, pid, params, ff, pid_log, setpoint, measurement, calibrated_pose, roll_compensation,
desired_lateral_accel, actual_lateral_accel, lateral_accel_deadzone, gravity_adjusted_lateral_accel,
desired_curvature, actual_curvature):
desired_curvature, actual_curvature, steer_limited_by_safety, output_torque):
self._ff = ff
self._pid = pid
self._pid_log = pid_log
self._setpoint = setpoint
self._measurement = measurement
self._roll_compensation = roll_compensation
self._lateral_accel_deadzone = lateral_accel_deadzone
self._desired_lateral_accel = desired_lateral_accel
self._actual_lateral_accel = actual_lateral_accel
self._desired_curvature = desired_curvature
self._actual_curvature = actual_curvature
self._gravity_adjusted_lateral_accel = gravity_adjusted_lateral_accel
self._steer_limited_by_safety = steer_limited_by_safety
self._output_torque = output_torque
self.update_calculations(CS, VM, desired_lateral_accel)
self.update_neural_network_feedforward(CS, params, calibrated_pose)
return self._ff, self._pid_log
return self._pid_log, self._output_torque
sunnypilot/selfdrive/controls/lib/latcontrol_torque_ext_base.py
+9 -2
View File
@@ -7,6 +7,7 @@ See the LICENSE.md file in the root directory for more details.
import math
import numpy as np
from openpilot.common.pid import PIDController
from openpilot.selfdrive.controls.lib.drive_helpers import CONTROL_N
from openpilot.selfdrive.modeld.constants import ModelConstants
@@ -43,9 +44,10 @@ def get_lookahead_value(future_vals, current_val):
class LatControlTorqueExtBase:
def __init__(self, lac_torque, CP, CP_SP):
def __init__(self, lac_torque, CP, CP_SP, CI):
self.model_v2 = None
self.model_valid = False
self.lac_torque = lac_torque
self.torque_params = lac_torque.torque_params
self.actual_lateral_jerk: float = 0.0
@@ -53,17 +55,22 @@ class LatControlTorqueExtBase:
self.lateral_jerk_measurement: float = 0.0
self.lookahead_lateral_jerk: float = 0.0
self.torque_from_lateral_accel = lac_torque.torque_from_lateral_accel
self.torque_from_lateral_accel_in_torque_space = CI.torque_from_lateral_accel_in_torque_space()
self._ff = 0.0
self._pid = PIDController(0.0, 0.0, k_f=0.0)
self._pid_log = None
self._setpoint = 0.0
self._measurement = 0.0
self._roll_compensation = 0.0
self._lateral_accel_deadzone = 0.0
self._desired_lateral_accel = 0.0
self._actual_lateral_accel = 0.0
self._desired_curvature = 0.0
self._actual_curvature = 0.0
self._gravity_adjusted_lateral_accel = 0.0
self._steer_limited_by_safety = False
self._output_torque = 0.0
# twilsonco's Lateral Neural Network Feedforward
# Instantaneous lateral jerk changes very rapidly, making it not useful on its own,
sunnypilot/selfdrive/controls/lib/nnlc/nnlc.py
+37 -3
View File
@@ -9,6 +9,8 @@ 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
@@ -30,8 +32,8 @@ def roll_pitch_adjust(roll, pitch):
class NeuralNetworkLateralControl(LatControlTorqueExtBase):
def __init__(self, lac_torque, CP, CP_SP):
super().__init__(lac_torque, CP, CP_SP)
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
@@ -57,14 +59,44 @@ class NeuralNetworkLateralControl(LatControlTorqueExtBase):
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.enabled or not self.model_valid or not self.has_nn_model:
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
@@ -128,3 +160,5 @@ class NeuralNetworkLateralControl(LatControlTorqueExtBase):
# 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)
sunnypilot/selfdrive/controls/lib/nnlc/tests/test_nnlc.py
+9 -1
View File
@@ -3,6 +3,7 @@ from parameterized import parameterized
from cereal import car, log, messaging
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
@@ -41,7 +42,7 @@ def generate_modelV2():
class TestNeuralNetworkLateralControl:
@parameterized.expand([HONDA.HONDA_CIVIC, TOYOTA.TOYOTA_RAV4, HYUNDAI.HYUNDAI_SANTA_CRUZ_1ST_GEN])
@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)
@@ -57,6 +58,7 @@ class TestNeuralNetworkLateralControl:
VM = VehicleModel(CP)
controller = LatControlTorque(CP.as_reader(), CP_SP.as_reader(), CI)
torque_params = CP.lateralTuning.torque
CS = car.CarState.new_message()
CS.vEgo = 30
@@ -77,17 +79,23 @@ class TestNeuralNetworkLateralControl:
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)
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)
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)
assert lac_log.saturated