mirror of
https://github.com/infiniteCable2/openpilot.git
synced 2026-08-06 00:36:29 +08:00
Controls: Lateral Accel Torque Control Extension (#690)
* init * more init * keep it alive * fixes * more fixes * more fix * new submodule for nn data * bump submodule * update path to submodule * spacing??? * update submodule path * update submodule path * bump * dump * bump * introduce params * Add Neural Network Lateral Control toggle to developer panel This introduces a new toggle for enabling Neural Network Lateral Control (NNLC), providing detailed descriptions of its functionality and compatibility. It includes UI integration, car compatibility checks, and feedback links for unsupported vehicles. * decouple even more * static * codespell * remove debug * in structs * fix import * convert to capnp * fixes * debug * only initialize if NNLC is enabled or allow to enable * oops * fix initialization * only allow engage if nnlc is off * fix toggle param * fix tests * lint * fix more test * capnp test * try this out * validate if it's not None * make it 33 to match * align * share the same friction input calculation * return stock values if not enabled * unused * split base and child * space * rename * NeuralNetworkFeedForwardModel * less * just use file name * try this * more explicit * rename * move it * child class for additional controllers * rename * time to split out custom lateral acceleration * move around * space * fix * TODO-SP * TODO-SP * split nnlc and custom lat accel * more * not yet * comment * fix --------- Co-authored-by: DevTekVE <devtekve@gmail.com>
This commit is contained in:
Jason Wen
@@ -42,7 +42,7 @@ class TestCarInterfaces:
|
||||
car_params = CarInterface.get_params(car_name, args['fingerprints'], args['car_fw'],
|
||||
experimental_long=args['experimental_long'], docs=False)
|
||||
car_params_sp = CarInterface.get_params_sp(car_params, car_name, args['fingerprints'], args['car_fw'],
|
||||
experimental_long=args['experimental_long'], docs=False)
|
||||
experimental_long=args['experimental_long'], docs=False)
|
||||
car_params = car_params.as_reader()
|
||||
car_interface = CarInterface(car_params, car_params_sp)
|
||||
assert car_params
|
||||
@@ -100,8 +100,8 @@ class TestCarInterfaces:
|
||||
# hypothesis also slows down significantly with just one more message draw
|
||||
LongControl(car_params)
|
||||
if car_params.steerControlType == CarParams.SteerControlType.angle:
|
||||
LatControlAngle(car_params, car_interface)
|
||||
LatControlAngle(car_params, car_params_sp, car_interface)
|
||||
elif car_params.lateralTuning.which() == 'pid':
|
||||
LatControlPID(car_params, car_interface)
|
||||
LatControlPID(car_params, car_params_sp, car_interface)
|
||||
elif car_params.lateralTuning.which() == 'torque':
|
||||
LatControlTorque(car_params, car_interface)
|
||||
LatControlTorque(car_params, car_params_sp, car_interface)
|
||||
|
||||
@@ -55,11 +55,11 @@ class Controls:
|
||||
self.VM = VehicleModel(self.CP)
|
||||
self.LaC: LatControl
|
||||
if self.CP.steerControlType == car.CarParams.SteerControlType.angle:
|
||||
self.LaC = LatControlAngle(self.CP, self.CI)
|
||||
self.LaC = LatControlAngle(self.CP, self.CP_SP, self.CI)
|
||||
elif self.CP.lateralTuning.which() == 'pid':
|
||||
self.LaC = LatControlPID(self.CP, self.CI)
|
||||
self.LaC = LatControlPID(self.CP, self.CP_SP, self.CI)
|
||||
elif self.CP.lateralTuning.which() == 'torque':
|
||||
self.LaC = LatControlTorque(self.CP, self.CI)
|
||||
self.LaC = LatControlTorque(self.CP, self.CP_SP, self.CI)
|
||||
|
||||
def update(self):
|
||||
self.sm.update(15)
|
||||
@@ -85,6 +85,8 @@ class Controls:
|
||||
self.LaC.update_live_torque_params(torque_params.latAccelFactorFiltered, torque_params.latAccelOffsetFiltered,
|
||||
torque_params.frictionCoefficientFiltered)
|
||||
|
||||
self.LaC.extension.update_model_v2(self.sm['modelV2'])
|
||||
|
||||
long_plan = self.sm['longitudinalPlan']
|
||||
model_v2 = self.sm['modelV2']
|
||||
|
||||
|
||||
@@ -7,7 +7,7 @@ MIN_LATERAL_CONTROL_SPEED = 0.3 # m/s
|
||||
|
||||
|
||||
class LatControl(ABC):
|
||||
def __init__(self, CP, CI):
|
||||
def __init__(self, CP, CP_SP, CI):
|
||||
self.sat_count_rate = 1.0 * DT_CTRL
|
||||
self.sat_limit = CP.steerLimitTimer
|
||||
self.sat_count = 0.
|
||||
|
||||
@@ -7,8 +7,8 @@ STEER_ANGLE_SATURATION_THRESHOLD = 2.5 # Degrees
|
||||
|
||||
|
||||
class LatControlAngle(LatControl):
|
||||
def __init__(self, CP, CI):
|
||||
super().__init__(CP, CI)
|
||||
def __init__(self, CP, CP_SP, CI):
|
||||
super().__init__(CP, CP_SP, CI)
|
||||
self.sat_check_min_speed = 5.
|
||||
|
||||
def update(self, active, CS, VM, params, steer_limited_by_controls, desired_curvature, calibrated_pose, curvature_limited):
|
||||
|
||||
@@ -6,8 +6,8 @@ from openpilot.common.pid import PIDController
|
||||
|
||||
|
||||
class LatControlPID(LatControl):
|
||||
def __init__(self, CP, CI):
|
||||
super().__init__(CP, CI)
|
||||
def __init__(self, CP, CP_SP, CI):
|
||||
super().__init__(CP, CP_SP, CI)
|
||||
self.pid = PIDController((CP.lateralTuning.pid.kpBP, CP.lateralTuning.pid.kpV),
|
||||
(CP.lateralTuning.pid.kiBP, CP.lateralTuning.pid.kiV),
|
||||
k_f=CP.lateralTuning.pid.kf, pos_limit=self.steer_max, neg_limit=-self.steer_max)
|
||||
|
||||
@@ -7,6 +7,8 @@ from opendbc.car.vehicle_model import ACCELERATION_DUE_TO_GRAVITY
|
||||
from openpilot.selfdrive.controls.lib.latcontrol import LatControl
|
||||
from openpilot.common.pid import PIDController
|
||||
|
||||
from openpilot.sunnypilot.selfdrive.controls.lib.latcontrol_torque_ext import LatControlTorqueExt
|
||||
|
||||
# At higher speeds (25+mph) we can assume:
|
||||
# Lateral acceleration achieved by a specific car correlates to
|
||||
# torque applied to the steering rack. It does not correlate to
|
||||
@@ -23,8 +25,8 @@ LOW_SPEED_Y = [15, 13, 10, 5]
|
||||
|
||||
|
||||
class LatControlTorque(LatControl):
|
||||
def __init__(self, CP, CI):
|
||||
super().__init__(CP, CI)
|
||||
def __init__(self, CP, CP_SP, CI):
|
||||
super().__init__(CP, CP_SP, CI)
|
||||
self.torque_params = CP.lateralTuning.torque.as_builder()
|
||||
self.pid = PIDController(self.torque_params.kp, self.torque_params.ki,
|
||||
k_f=self.torque_params.kf, pos_limit=self.steer_max, neg_limit=-self.steer_max)
|
||||
@@ -32,6 +34,8 @@ class LatControlTorque(LatControl):
|
||||
self.use_steering_angle = self.torque_params.useSteeringAngle
|
||||
self.steering_angle_deadzone_deg = self.torque_params.steeringAngleDeadzoneDeg
|
||||
|
||||
self.extension = LatControlTorqueExt(self, CP, CP_SP)
|
||||
|
||||
def update_live_torque_params(self, latAccelFactor, latAccelOffset, friction):
|
||||
self.torque_params.latAccelFactor = latAccelFactor
|
||||
self.torque_params.latAccelOffset = latAccelOffset
|
||||
@@ -73,6 +77,11 @@ class LatControlTorque(LatControl):
|
||||
desired_lateral_accel - actual_lateral_accel, lateral_accel_deadzone, friction_compensation=True,
|
||||
gravity_adjusted=True)
|
||||
|
||||
# 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)
|
||||
|
||||
freeze_integrator = steer_limited_by_controls or CS.steeringPressed or CS.vEgo < 5
|
||||
output_torque = self.pid.update(pid_log.error,
|
||||
feedforward=ff,
|
||||
|
||||
@@ -6,6 +6,7 @@ from opendbc.car.honda.values import CAR as HONDA
|
||||
from opendbc.car.toyota.values import CAR as TOYOTA
|
||||
from opendbc.car.nissan.values import CAR as NISSAN
|
||||
from opendbc.car.vehicle_model import VehicleModel
|
||||
from openpilot.selfdrive.car.helpers import convert_to_capnp
|
||||
from openpilot.selfdrive.controls.lib.latcontrol_pid import LatControlPID
|
||||
from openpilot.selfdrive.controls.lib.latcontrol_torque import LatControlTorque
|
||||
from openpilot.selfdrive.controls.lib.latcontrol_angle import LatControlAngle
|
||||
@@ -21,9 +22,10 @@ class TestLatControl:
|
||||
CP = CarInterface.get_non_essential_params(car_name)
|
||||
CP_SP = CarInterface.get_non_essential_params_sp(CP, car_name)
|
||||
CI = CarInterface(CP, CP_SP)
|
||||
CP_SP = convert_to_capnp(CP_SP)
|
||||
VM = VehicleModel(CP)
|
||||
|
||||
controller = controller(CP.as_reader(), CI)
|
||||
controller = controller(CP.as_reader(), CP_SP.as_reader(), CI)
|
||||
|
||||
CS = car.CarState.new_message()
|
||||
CS.vEgo = 30
|
||||
|
||||
@@ -0,0 +1,26 @@
|
||||
"""
|
||||
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 openpilot.sunnypilot.selfdrive.controls.lib.latcontrol_torque_ext_base import LatControlTorqueExtBase
|
||||
|
||||
|
||||
class LatControlTorqueExt(LatControlTorqueExtBase):
|
||||
def __init__(self, lac_torque, CP, CP_SP):
|
||||
super().__init__(lac_torque, CP, CP_SP)
|
||||
|
||||
def update(self, 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):
|
||||
self._ff = ff
|
||||
self._pid_log = pid_log
|
||||
self._setpoint = setpoint
|
||||
self._measurement = measurement
|
||||
self._lateral_accel_deadzone = lateral_accel_deadzone
|
||||
self._desired_lateral_accel = desired_lateral_accel
|
||||
self._actual_lateral_accel = actual_lateral_accel
|
||||
|
||||
self.update_calculations(CS, VM, desired_lateral_accel)
|
||||
|
||||
return self._ff, self._pid_log
|
||||
@@ -0,0 +1,121 @@
|
||||
"""
|
||||
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 math
|
||||
import numpy as np
|
||||
|
||||
from openpilot.selfdrive.controls.lib.drive_helpers import CONTROL_N
|
||||
from openpilot.selfdrive.modeld.constants import ModelConstants
|
||||
|
||||
LAT_PLAN_MIN_IDX = 5
|
||||
|
||||
|
||||
def get_predicted_lateral_jerk(lat_accels, t_diffs):
|
||||
# compute finite difference between subsequent model_v2.acceleration.y values
|
||||
# this is just two calls of np.diff followed by an element-wise division
|
||||
lat_accel_diffs = np.diff(lat_accels)
|
||||
lat_jerk = lat_accel_diffs / t_diffs
|
||||
# return as python list
|
||||
return lat_jerk.tolist()
|
||||
|
||||
|
||||
def sign(x):
|
||||
return 1.0 if x > 0.0 else (-1.0 if x < 0.0 else 0.0)
|
||||
|
||||
|
||||
def get_lookahead_value(future_vals, current_val):
|
||||
if len(future_vals) == 0:
|
||||
return current_val
|
||||
|
||||
same_sign_vals = [v for v in future_vals if sign(v) == sign(current_val)]
|
||||
|
||||
# if any future val has opposite sign of current val, return 0
|
||||
if len(same_sign_vals) < len(future_vals):
|
||||
return 0.0
|
||||
|
||||
# otherwise return the value with minimum absolute value
|
||||
min_val = min(same_sign_vals + [current_val], key=lambda x: abs(x))
|
||||
return min_val
|
||||
|
||||
|
||||
class LatControlTorqueExtBase:
|
||||
def __init__(self, lac_torque, CP, CP_SP):
|
||||
self.model_v2 = None
|
||||
self.model_valid = False
|
||||
self.use_steering_angle = lac_torque.use_steering_angle
|
||||
|
||||
self.actual_lateral_jerk: float = 0.0
|
||||
self.lateral_jerk_setpoint: float = 0.0
|
||||
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_params = lac_torque.torque_params
|
||||
|
||||
self._ff = 0.0
|
||||
self._pid_log = None
|
||||
self._setpoint = 0.0
|
||||
self._measurement = 0.0
|
||||
self._lateral_accel_deadzone = 0.0
|
||||
self._desired_lateral_accel = 0.0
|
||||
self._actual_lateral_accel = 0.0
|
||||
|
||||
# twilsonco's Lateral Neural Network Feedforward
|
||||
# Instantaneous lateral jerk changes very rapidly, making it not useful on its own,
|
||||
# however, we can "look ahead" to the future planned lateral jerk in order to gauge
|
||||
# whether the current desired lateral jerk will persist into the future, i.e.
|
||||
# whether it's "deliberate" or not. This allows us to simply ignore short-lived jerk.
|
||||
# Note that LAT_PLAN_MIN_IDX is defined above and is used in order to prevent
|
||||
# using a "future" value that is actually planned to occur before the "current" desired
|
||||
# value, which is offset by the steerActuatorDelay.
|
||||
# TODO-SP: Reevaluate lookahead v values that determines how low a desired lateral jerk signal needs to
|
||||
# persist in order to be used.
|
||||
self.friction_look_ahead_v = [1.4, 2.0] # how many seconds in the future to look ahead in [0, ~2.1] in 0.1 increments
|
||||
self.friction_look_ahead_bp = [9.0, 30.0] # corresponding speeds in m/s in [0, ~40] in 1.0 increments
|
||||
|
||||
# Scaling the lateral acceleration "friction response" could be helpful for some.
|
||||
# Increase for a stronger response, decrease for a weaker response.
|
||||
self.lat_jerk_friction_factor = 0.4
|
||||
self.lat_accel_friction_factor = 0.7 # in [0, 3], in 0.05 increments. 3 is arbitrary safety limit
|
||||
|
||||
# precompute time differences between ModelConstants.T_IDXS
|
||||
self.t_diffs = np.diff(ModelConstants.T_IDXS)
|
||||
self.desired_lat_jerk_time = CP.steerActuatorDelay + 0.3
|
||||
|
||||
def update_model_v2(self, model_v2):
|
||||
self.model_v2 = model_v2
|
||||
self.model_valid = self.model_v2 is not None and len(self.model_v2.orientation.x) >= CONTROL_N
|
||||
|
||||
def update_friction_input(self, val_1, val_2):
|
||||
_error = val_1 - val_2
|
||||
_value = self.lat_accel_friction_factor * _error + self.lat_jerk_friction_factor * self.lookahead_lateral_jerk
|
||||
|
||||
return _value
|
||||
|
||||
def update_calculations(self, CS, VM, desired_lateral_accel):
|
||||
self.actual_lateral_jerk = 0.0
|
||||
self.lateral_jerk_setpoint = 0.0
|
||||
self.lateral_jerk_measurement = 0.0
|
||||
self.lookahead_lateral_jerk = 0.0
|
||||
|
||||
if self.use_steering_angle:
|
||||
actual_curvature_rate = -VM.calc_curvature(math.radians(CS.steeringRateDeg), CS.vEgo, 0.0)
|
||||
self.actual_lateral_jerk = actual_curvature_rate * CS.vEgo ** 2
|
||||
|
||||
if self.model_valid:
|
||||
# prepare "look-ahead" desired lateral jerk
|
||||
lookahead = np.interp(CS.vEgo, self.friction_look_ahead_bp, self.friction_look_ahead_v)
|
||||
friction_upper_idx = next((i for i, val in enumerate(ModelConstants.T_IDXS) if val > lookahead), 16)
|
||||
predicted_lateral_jerk = get_predicted_lateral_jerk(self.model_v2.acceleration.y, self.t_diffs)
|
||||
desired_lateral_jerk = (np.interp(self.desired_lat_jerk_time, ModelConstants.T_IDXS,
|
||||
self.model_v2.acceleration.y) - desired_lateral_accel) / self.desired_lat_jerk_time
|
||||
self.lookahead_lateral_jerk = get_lookahead_value(predicted_lateral_jerk[LAT_PLAN_MIN_IDX:friction_upper_idx], desired_lateral_jerk)
|
||||
if not self.use_steering_angle or self.lookahead_lateral_jerk == 0.0:
|
||||
self.lookahead_lateral_jerk = 0.0
|
||||
self.actual_lateral_jerk = 0.0
|
||||
self.lat_accel_friction_factor = 1.0
|
||||
self.lateral_jerk_setpoint = self.lat_jerk_friction_factor * self.lookahead_lateral_jerk
|
||||
self.lateral_jerk_measurement = self.lat_jerk_friction_factor * self.actual_lateral_jerk
|
||||
Reference in New Issue
Block a user