From 39886246c96454663d2720f39c66a539023b90a1 Mon Sep 17 00:00:00 2001 From: HaraldSchafer Date: Tue, 24 May 2022 17:52:33 -0700 Subject: [PATCH] Rerevert torque control (#24649) * Revert "Revert torque control (#24565)" This reverts commit 93f434d047a92366c4ccefeece86fe101ed98d02. * Move tune out of car specific stuff * Update ref commit old-commit-hash: d9289721503976cd99ac90216c502a841988c1a6 --- release/files_common | 1 - selfdrive/car/hyundai/interface.py | 10 +-- selfdrive/car/tests/test_car_interfaces.py | 2 - selfdrive/car/tests/test_models.py | 2 - selfdrive/car/toyota/interface.py | 11 ++- selfdrive/car/toyota/tunes.py | 19 +---- selfdrive/controls/controlsd.py | 5 -- selfdrive/controls/lib/latcontrol_lqr.py | 84 ------------------- selfdrive/controls/lib/latcontrol_torque.py | 9 ++ .../controls/lib/tests/test_latcontrol.py | 4 +- selfdrive/test/process_replay/ref_commit | 2 +- 11 files changed, 24 insertions(+), 125 deletions(-) delete mode 100644 selfdrive/controls/lib/latcontrol_lqr.py diff --git a/release/files_common b/release/files_common index 1b75fb9a11..e254abec66 100644 --- a/release/files_common +++ b/release/files_common @@ -168,7 +168,6 @@ selfdrive/controls/lib/events.py selfdrive/controls/lib/lane_planner.py selfdrive/controls/lib/latcontrol_angle.py selfdrive/controls/lib/latcontrol_indi.py -selfdrive/controls/lib/latcontrol_lqr.py selfdrive/controls/lib/latcontrol_torque.py selfdrive/controls/lib/latcontrol_pid.py selfdrive/controls/lib/latcontrol.py diff --git a/selfdrive/car/hyundai/interface.py b/selfdrive/car/hyundai/interface.py index 452e782ee4..b4dc26c87a 100644 --- a/selfdrive/car/hyundai/interface.py +++ b/selfdrive/car/hyundai/interface.py @@ -7,6 +7,7 @@ from selfdrive.car.hyundai.radar_interface import RADAR_START_ADDR from selfdrive.car import STD_CARGO_KG, scale_rot_inertia, scale_tire_stiffness, gen_empty_fingerprint, get_safety_config from selfdrive.car.interfaces import CarInterfaceBase from selfdrive.car.disable_ecu import disable_ecu +from selfdrive.controls.lib.latcontrol_torque import set_torque_tune ButtonType = car.CarState.ButtonEvent.Type EventName = car.CarEvent.EventName @@ -48,7 +49,7 @@ class CarInterface(CarInterfaceBase): ret.longitudinalTuning.kiV = [0.0] ret.stopAccel = 0.0 - ret.longitudinalActuatorDelayUpperBound = 1.0 # s + ret.longitudinalActuatorDelayUpperBound = 1.0 # s if candidate in (CAR.SANTA_FE, CAR.SANTA_FE_2022, CAR.SANTA_FE_HEV_2022, CAR.SANTA_FE_PHEV_2022): ret.lateralTuning.pid.kf = 0.00005 @@ -252,12 +253,7 @@ class CarInterface(CarInterfaceBase): tire_stiffness_factor = 0.65 max_lat_accel = 2. - ret.lateralTuning.init('torque') - ret.lateralTuning.torque.useSteeringAngle = True - ret.lateralTuning.torque.kp = 1.0 / max_lat_accel - ret.lateralTuning.torque.kf = 1.0 / max_lat_accel - ret.lateralTuning.torque.ki = 0.1 / max_lat_accel - ret.lateralTuning.torque.friction = 0.01 + set_torque_tune(ret.lateralTuning, max_lat_accel, 0.01) # Genesis elif candidate == CAR.GENESIS_G70: diff --git a/selfdrive/car/tests/test_car_interfaces.py b/selfdrive/car/tests/test_car_interfaces.py index 18bf9c1f01..92024ab0c2 100755 --- a/selfdrive/car/tests/test_car_interfaces.py +++ b/selfdrive/car/tests/test_car_interfaces.py @@ -38,8 +38,6 @@ class TestCarInterfaces(unittest.TestCase): tuning = car_params.lateralTuning.which() if tuning == 'pid': self.assertTrue(len(car_params.lateralTuning.pid.kpV)) - elif tuning == 'lqr': - self.assertTrue(len(car_params.lateralTuning.lqr.a)) elif tuning == 'torque': self.assertTrue(car_params.lateralTuning.torque.kf > 0) elif tuning == 'indi': diff --git a/selfdrive/car/tests/test_models.py b/selfdrive/car/tests/test_models.py index 27669fd331..d2867b9f4f 100755 --- a/selfdrive/car/tests/test_models.py +++ b/selfdrive/car/tests/test_models.py @@ -118,8 +118,6 @@ class TestCarModel(unittest.TestCase): self.assertTrue(len(self.CP.lateralTuning.pid.kpV)) elif tuning == 'torque': self.assertTrue(self.CP.lateralTuning.torque.kf > 0) - elif tuning == 'lqr': - self.assertTrue(len(self.CP.lateralTuning.lqr.a)) elif tuning == 'indi': self.assertTrue(len(self.CP.lateralTuning.indi.outerLoopGainV)) else: diff --git a/selfdrive/car/toyota/interface.py b/selfdrive/car/toyota/interface.py index fd3bd15cf1..d8e642ba61 100644 --- a/selfdrive/car/toyota/interface.py +++ b/selfdrive/car/toyota/interface.py @@ -47,7 +47,7 @@ class CarInterface(CarInterfaceBase): ret.steerRatio = 17.4 tire_stiffness_factor = 0.5533 ret.mass = 3340. * CV.LB_TO_KG + STD_CARGO_KG - set_lat_tune(ret.lateralTuning, LatTunes.LQR_RAV4) + set_lat_tune(ret.lateralTuning, LatTunes.TORQUE, MAX_LAT_ACCEL=1.8, FRICTION=0.06) elif candidate in (CAR.RAV4, CAR.RAV4H): stop_and_go = True if (candidate in CAR.RAV4H) else False @@ -55,7 +55,7 @@ class CarInterface(CarInterfaceBase): ret.steerRatio = 16.88 # 14.5 is spec end-to-end tire_stiffness_factor = 0.5533 ret.mass = 3650. * CV.LB_TO_KG + STD_CARGO_KG # mean between normal and hybrid - set_lat_tune(ret.lateralTuning, LatTunes.LQR_RAV4) + set_lat_tune(ret.lateralTuning, LatTunes.TORQUE, MAX_LAT_ACCEL=1.8, FRICTION=0.06) elif candidate == CAR.COROLLA: ret.wheelbase = 2.70 @@ -87,7 +87,10 @@ class CarInterface(CarInterfaceBase): ret.steerRatio = 13.7 tire_stiffness_factor = 0.7933 ret.mass = 3400. * CV.LB_TO_KG + STD_CARGO_KG # mean between normal and hybrid - set_lat_tune(ret.lateralTuning, LatTunes.PID_C) + if candidate in (CAR.CAMRY_TSS2, CAR.CAMRYH_TSS2): + set_lat_tune(ret.lateralTuning, LatTunes.TORQUE, MAX_LAT_ACCEL=2.4, FRICTION=0.05) + else: + set_lat_tune(ret.lateralTuning, LatTunes.PID_C) elif candidate in (CAR.HIGHLANDER_TSS2, CAR.HIGHLANDERH_TSS2): stop_and_go = True @@ -136,7 +139,7 @@ class CarInterface(CarInterfaceBase): ret.steerRatio = 13.9 tire_stiffness_factor = 0.444 # not optimized yet ret.mass = 3060. * CV.LB_TO_KG + STD_CARGO_KG - set_lat_tune(ret.lateralTuning, LatTunes.PID_D) + set_lat_tune(ret.lateralTuning, LatTunes.TORQUE, MAX_LAT_ACCEL=2.0, FRICTION=0.07) elif candidate in (CAR.LEXUS_ES_TSS2, CAR.LEXUS_ESH_TSS2, CAR.LEXUS_ESH): stop_and_go = True diff --git a/selfdrive/car/toyota/tunes.py b/selfdrive/car/toyota/tunes.py index c200e39c0c..2cfdd31644 100644 --- a/selfdrive/car/toyota/tunes.py +++ b/selfdrive/car/toyota/tunes.py @@ -1,6 +1,6 @@ #!/usr/bin/env python3 from enum import Enum - +from selfdrive.controls.lib.latcontrol_torque import set_torque_tune class LongTunes(Enum): PEDAL = 0 @@ -52,22 +52,7 @@ def set_long_tune(tune, name): ###### LAT ###### def set_lat_tune(tune, name, MAX_LAT_ACCEL=2.5, FRICTION=.1): if name == LatTunes.TORQUE: - tune.init('torque') - tune.torque.useSteeringAngle = True - tune.torque.kp = 1.0 / MAX_LAT_ACCEL - tune.torque.kf = 1.0 / MAX_LAT_ACCEL - tune.torque.ki = 0.1 / MAX_LAT_ACCEL - tune.torque.friction = FRICTION - elif name == LatTunes.LQR_RAV4: - tune.init('lqr') - tune.lqr.scale = 1500.0 - tune.lqr.ki = 0.05 - tune.lqr.a = [0., 1., -0.22619643, 1.21822268] - tune.lqr.b = [-1.92006585e-04, 3.95603032e-05] - tune.lqr.c = [1., 0.] - tune.lqr.k = [-110.73572306, 451.22718255] - tune.lqr.l = [0.3233671, 0.3185757] - tune.lqr.dcGain = 0.002237852961363602 + set_torque_tune(tune, MAX_LAT_ACCEL, FRICTION) elif name == LatTunes.INDI_PRIUS: tune.init('indi') tune.indi.innerLoopGainBP = [0.] diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index 5cb6874ca7..fc0f289e39 100755 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -23,7 +23,6 @@ from selfdrive.controls.lib.latcontrol_pid import LatControlPID from selfdrive.controls.lib.latcontrol_indi import LatControlINDI from selfdrive.controls.lib.latcontrol_angle import LatControlAngle from selfdrive.controls.lib.latcontrol_torque import LatControlTorque -from selfdrive.controls.lib.latcontrol_lqr import LatControlLQR from selfdrive.controls.lib.events import Events, ET from selfdrive.controls.lib.alertmanager import AlertManager, set_offroad_alert from selfdrive.controls.lib.vehicle_model import VehicleModel @@ -146,8 +145,6 @@ class Controls: self.LaC = LatControlPID(self.CP, self.CI) elif self.CP.lateralTuning.which() == 'indi': self.LaC = LatControlINDI(self.CP, self.CI) - elif self.CP.lateralTuning.which() == 'lqr': - self.LaC = LatControlLQR(self.CP, self.CI) elif self.CP.lateralTuning.which() == 'torque': self.LaC = LatControlTorque(self.CP, self.CI) @@ -752,8 +749,6 @@ class Controls: controlsState.lateralControlState.pidState = lac_log elif lat_tuning == 'torque': controlsState.lateralControlState.torqueState = lac_log - elif lat_tuning == 'lqr': - controlsState.lateralControlState.lqrState = lac_log elif lat_tuning == 'indi': controlsState.lateralControlState.indiState = lac_log diff --git a/selfdrive/controls/lib/latcontrol_lqr.py b/selfdrive/controls/lib/latcontrol_lqr.py deleted file mode 100644 index 5b4f65a03c..0000000000 --- a/selfdrive/controls/lib/latcontrol_lqr.py +++ /dev/null @@ -1,84 +0,0 @@ -import math -import numpy as np - -from common.numpy_fast import clip -from common.realtime import DT_CTRL -from cereal import log -from selfdrive.controls.lib.latcontrol import LatControl, MIN_STEER_SPEED - - -class LatControlLQR(LatControl): - def __init__(self, CP, CI): - super().__init__(CP, CI) - self.scale = CP.lateralTuning.lqr.scale - self.ki = CP.lateralTuning.lqr.ki - - self.A = np.array(CP.lateralTuning.lqr.a).reshape((2, 2)) - self.B = np.array(CP.lateralTuning.lqr.b).reshape((2, 1)) - self.C = np.array(CP.lateralTuning.lqr.c).reshape((1, 2)) - self.K = np.array(CP.lateralTuning.lqr.k).reshape((1, 2)) - self.L = np.array(CP.lateralTuning.lqr.l).reshape((2, 1)) - self.dc_gain = CP.lateralTuning.lqr.dcGain - - self.x_hat = np.array([[0], [0]]) - self.i_unwind_rate = 0.3 * DT_CTRL - self.i_rate = 1.0 * DT_CTRL - - self.reset() - - def reset(self): - super().reset() - self.i_lqr = 0.0 - - def update(self, active, CS, VM, params, last_actuators, desired_curvature, desired_curvature_rate, llk): - lqr_log = log.ControlsState.LateralLQRState.new_message() - - torque_scale = (0.45 + CS.vEgo / 60.0)**2 # Scale actuator model with speed - - # Subtract offset. Zero angle should correspond to zero torque - steering_angle_no_offset = CS.steeringAngleDeg - params.angleOffsetAverageDeg - - desired_angle = math.degrees(VM.get_steer_from_curvature(-desired_curvature, CS.vEgo, params.roll)) - - instant_offset = params.angleOffsetDeg - params.angleOffsetAverageDeg - desired_angle += instant_offset # Only add offset that originates from vehicle model errors - lqr_log.steeringAngleDesiredDeg = desired_angle - - # Update Kalman filter - angle_steers_k = float(self.C.dot(self.x_hat)) - e = steering_angle_no_offset - angle_steers_k - self.x_hat = self.A.dot(self.x_hat) + self.B.dot(CS.steeringTorqueEps / torque_scale) + self.L.dot(e) - - if CS.vEgo < MIN_STEER_SPEED or not active: - lqr_log.active = False - lqr_output = 0. - output_steer = 0. - self.reset() - else: - lqr_log.active = True - - # LQR - u_lqr = float(desired_angle / self.dc_gain - self.K.dot(self.x_hat)) - lqr_output = torque_scale * u_lqr / self.scale - - # Integrator - if CS.steeringPressed: - self.i_lqr -= self.i_unwind_rate * float(np.sign(self.i_lqr)) - else: - error = desired_angle - angle_steers_k - i = self.i_lqr + self.ki * self.i_rate * error - control = lqr_output + i - - if (error >= 0 and (control <= self.steer_max or i < 0.0)) or \ - (error <= 0 and (control >= -self.steer_max or i > 0.0)): - self.i_lqr = i - - output_steer = lqr_output + self.i_lqr - output_steer = clip(output_steer, -self.steer_max, self.steer_max) - - lqr_log.steeringAngleDeg = angle_steers_k - lqr_log.i = self.i_lqr - lqr_log.output = output_steer - lqr_log.lqrOutput = lqr_output - lqr_log.saturated = self._check_saturation(self.steer_max - abs(output_steer) < 1e-3, CS) - return output_steer, desired_angle, lqr_log diff --git a/selfdrive/controls/lib/latcontrol_torque.py b/selfdrive/controls/lib/latcontrol_torque.py index e3dbe373b6..502d0abf0e 100644 --- a/selfdrive/controls/lib/latcontrol_torque.py +++ b/selfdrive/controls/lib/latcontrol_torque.py @@ -22,6 +22,15 @@ LOW_SPEED_FACTOR = 200 JERK_THRESHOLD = 0.2 +def set_torque_tune(tune, MAX_LAT_ACCEL=2.5, FRICTION=.1): + tune.init('torque') + tune.torque.useSteeringAngle = True + tune.torque.kp = 1.0 / MAX_LAT_ACCEL + tune.torque.kf = 1.0 / MAX_LAT_ACCEL + tune.torque.ki = 0.1 / MAX_LAT_ACCEL + tune.torque.friction = FRICTION + + class LatControlTorque(LatControl): def __init__(self, CP, CI): super().__init__(CP, CI) diff --git a/selfdrive/controls/lib/tests/test_latcontrol.py b/selfdrive/controls/lib/tests/test_latcontrol.py index 8345840eca..503eaaa6a4 100755 --- a/selfdrive/controls/lib/tests/test_latcontrol.py +++ b/selfdrive/controls/lib/tests/test_latcontrol.py @@ -9,7 +9,7 @@ from selfdrive.car.honda.values import CAR as HONDA from selfdrive.car.toyota.values import CAR as TOYOTA from selfdrive.car.nissan.values import CAR as NISSAN from selfdrive.controls.lib.latcontrol_pid import LatControlPID -from selfdrive.controls.lib.latcontrol_lqr import LatControlLQR +from selfdrive.controls.lib.latcontrol_torque import LatControlTorque from selfdrive.controls.lib.latcontrol_indi import LatControlINDI from selfdrive.controls.lib.latcontrol_angle import LatControlAngle from selfdrive.controls.lib.vehicle_model import VehicleModel @@ -17,7 +17,7 @@ from selfdrive.controls.lib.vehicle_model import VehicleModel class TestLatControl(unittest.TestCase): - @parameterized.expand([(HONDA.CIVIC, LatControlPID), (TOYOTA.RAV4, LatControlLQR), (TOYOTA.PRIUS, LatControlINDI), (NISSAN.LEAF, LatControlAngle)]) + @parameterized.expand([(HONDA.CIVIC, LatControlPID), (TOYOTA.RAV4, LatControlTorque), (TOYOTA.PRIUS, LatControlINDI), (NISSAN.LEAF, LatControlAngle)]) def test_saturation(self, car_name, controller): CarInterface, CarController, CarState = interfaces[car_name] CP = CarInterface.get_params(car_name) diff --git a/selfdrive/test/process_replay/ref_commit b/selfdrive/test/process_replay/ref_commit index 276905a432..55ce9832cc 100644 --- a/selfdrive/test/process_replay/ref_commit +++ b/selfdrive/test/process_replay/ref_commit @@ -1 +1 @@ -b8c35486e8354713221d4237e97e5abced6f5228 +336d77ad17b90af17b7eb24cc832e80b62d05a24