diff --git a/release/files_common b/release/files_common index eb3d825b08..38301cafb5 100644 --- a/release/files_common +++ b/release/files_common @@ -238,7 +238,7 @@ 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 selfdrive/controls/lib/lateral_planner.py diff --git a/selfdrive/car/tests/test_car_interfaces.py b/selfdrive/car/tests/test_car_interfaces.py index 2f4984c3d9..d97b16e682 100755 --- a/selfdrive/car/tests/test_car_interfaces.py +++ b/selfdrive/car/tests/test_car_interfaces.py @@ -38,8 +38,8 @@ 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': self.assertTrue(len(car_params.lateralTuning.indi.outerLoopGainV)) diff --git a/selfdrive/car/tests/test_models.py b/selfdrive/car/tests/test_models.py index 7e02fd2a07..b9c36db07a 100755 --- a/selfdrive/car/tests/test_models.py +++ b/selfdrive/car/tests/test_models.py @@ -115,8 +115,8 @@ class TestCarModel(unittest.TestCase): tuning = self.CP.lateralTuning.which() if tuning == 'pid': self.assertTrue(len(self.CP.lateralTuning.pid.kpV)) - elif tuning == 'lqr': - self.assertTrue(len(self.CP.lateralTuning.lqr.a)) + elif tuning == 'torque': + self.assertTrue(self.CP.lateralTuning.torque.kf > 0) elif tuning == 'indi': self.assertTrue(len(self.CP.lateralTuning.indi.outerLoopGainV)) else: diff --git a/selfdrive/car/toyota/carcontroller.py b/selfdrive/car/toyota/carcontroller.py index c0868f3288..1c01703d7e 100644 --- a/selfdrive/car/toyota/carcontroller.py +++ b/selfdrive/car/toyota/carcontroller.py @@ -14,6 +14,7 @@ VisualAlert = car.CarControl.HUDControl.VisualAlert class CarController: def __init__(self, dbc_name, CP, VM): self.CP = CP + self.torque_rate_limits = CarControllerParams(self.CP) self.frame = 0 self.last_steer = 0 self.alert_active = False @@ -50,7 +51,7 @@ class CarController: # steer torque new_steer = int(round(actuators.steer * CarControllerParams.STEER_MAX)) - apply_steer = apply_toyota_steer_torque_limits(new_steer, self.last_steer, CS.out.steeringTorqueEps, CarControllerParams) + apply_steer = apply_toyota_steer_torque_limits(new_steer, self.last_steer, CS.out.steeringTorqueEps, self.torque_rate_limits) self.steer_rate_limited = new_steer != apply_steer # Cut steering while we're in a known fault state (2s) diff --git a/selfdrive/car/toyota/interface.py b/selfdrive/car/toyota/interface.py index f7d5aba7ab..4b64b2eb20 100644 --- a/selfdrive/car/toyota/interface.py +++ b/selfdrive/car/toyota/interface.py @@ -34,7 +34,6 @@ class CarInterface(CarInterfaceBase): ret.steerRatio = 15.74 # unknown end-to-end spec tire_stiffness_factor = 0.6371 # hand-tune ret.mass = 3045. * CV.LB_TO_KG + STD_CARGO_KG - set_lat_tune(ret.lateralTuning, LatTunes.INDI_PRIUS) ret.steerActuatorDelay = 0.3 @@ -44,7 +43,7 @@ class CarInterface(CarInterfaceBase): ret.steerRatio = 17.4 tire_stiffness_factor = 0.5533 ret.mass = 4387. * CV.LB_TO_KG + STD_CARGO_KG - set_lat_tune(ret.lateralTuning, LatTunes.LQR_RAV4) + set_lat_tune(ret.lateralTuning, LatTunes.TORQUE) elif candidate in (CAR.RAV4, CAR.RAV4H): stop_and_go = True if (candidate in CAR.RAV4H) else False @@ -52,7 +51,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_TORQUE=2.5, FRICTION=0.06) elif candidate == CAR.COROLLA: ret.wheelbase = 2.70 @@ -133,7 +132,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_TORQUE=3.0, FRICTION=0.08) 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 f26fc72a09..81fd695827 100644 --- a/selfdrive/car/toyota/tunes.py +++ b/selfdrive/car/toyota/tunes.py @@ -24,6 +24,7 @@ class LatTunes(Enum): PID_L = 13 PID_M = 14 PID_N = 15 + TORQUE = 16 ###### LONG ###### @@ -49,8 +50,15 @@ def set_long_tune(tune, name): ###### LAT ###### -def set_lat_tune(tune, name): - if name == LatTunes.INDI_PRIUS: +def set_lat_tune(tune, name, MAX_TORQUE=2.5, FRICTION=.1): + if name == LatTunes.TORQUE: + tune.init('torque') + tune.torque.useSteeringAngle = True + tune.torque.kp = 2.0 / MAX_TORQUE + tune.torque.kf = 1.0 / MAX_TORQUE + tune.torque.ki = 0.5 / MAX_TORQUE + tune.torque.friction = FRICTION + elif name == LatTunes.INDI_PRIUS: tune.init('indi') tune.indi.innerLoopGainBP = [0.] tune.indi.innerLoopGainV = [4.0] @@ -60,18 +68,6 @@ def set_lat_tune(tune, name): tune.indi.timeConstantV = [1.0] tune.indi.actuatorEffectivenessBP = [0.] tune.indi.actuatorEffectivenessV = [1.0] - - 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 - elif 'PID' in str(name): tune.init('pid') tune.pid.kiBP = [0.0] diff --git a/selfdrive/car/toyota/values.py b/selfdrive/car/toyota/values.py index e080f9ba05..cc5d9a581a 100644 --- a/selfdrive/car/toyota/values.py +++ b/selfdrive/car/toyota/values.py @@ -18,10 +18,16 @@ class CarControllerParams: ACCEL_MIN = -3.5 # m/s2 STEER_MAX = 1500 - STEER_DELTA_UP = 10 # 1.5s time to peak torque - STEER_DELTA_DOWN = 25 # always lower than 45 otherwise the Rav4 faults (Prius seems ok with 50) STEER_ERROR_MAX = 350 # max delta between torque cmd and torque motor + def __init__(self, CP): + if CP.lateralTuning.which == 'torque': + self.STEER_DELTA_UP = 15 # 1.0s time to peak torque + self.STEER_DELTA_DOWN = 25 # always lower than 45 otherwise the Rav4 faults (Prius seems ok with 50) + else: + self.STEER_DELTA_UP = 10 # 1.5s time to peak torque + self.STEER_DELTA_DOWN = 25 # always lower than 45 otherwise the Rav4 faults (Prius seems ok with 50) + class ToyotaFlags(IntFlag): HYBRID = 1 diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index f098657fb9..5bd75548f0 100755 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -20,8 +20,8 @@ from selfdrive.controls.lib.drive_helpers import get_lag_adjusted_curvature from selfdrive.controls.lib.longcontrol import LongControl from selfdrive.controls.lib.latcontrol_pid import LatControlPID from selfdrive.controls.lib.latcontrol_indi import LatControlINDI -from selfdrive.controls.lib.latcontrol_lqr import LatControlLQR from selfdrive.controls.lib.latcontrol_angle import LatControlAngle +from selfdrive.controls.lib.latcontrol_torque import LatControlTorque 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 @@ -144,8 +144,8 @@ 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) self.initialized = False self.state = State.disabled @@ -566,7 +566,7 @@ class Controls: lat_plan.curvatureRates) actuators.steer, actuators.steeringAngleDeg, lac_log = self.LaC.update(CC.latActive, CS, self.CP, self.VM, params, self.last_actuators, desired_curvature, - desired_curvature_rate) + desired_curvature_rate, self.sm['liveLocationKalman']) else: lac_log = log.ControlsState.LateralDebugState.new_message() if self.sm.rcv_frame['testJoystick'] > 0: @@ -730,8 +730,8 @@ class Controls: controlsState.lateralControlState.angleState = lac_log elif lat_tuning == 'pid': controlsState.lateralControlState.pidState = lac_log - elif lat_tuning == 'lqr': - controlsState.lateralControlState.lqrState = lac_log + elif lat_tuning == 'torque': + controlsState.lateralControlState.torqueState = lac_log elif lat_tuning == 'indi': controlsState.lateralControlState.indiState = lac_log diff --git a/selfdrive/controls/lib/latcontrol.py b/selfdrive/controls/lib/latcontrol.py index 4a67715fcb..0d137df733 100644 --- a/selfdrive/controls/lib/latcontrol.py +++ b/selfdrive/controls/lib/latcontrol.py @@ -16,7 +16,7 @@ class LatControl(ABC): self.steer_max = 1.0 @abstractmethod - def update(self, active, CS, CP, VM, params, last_actuators, desired_curvature, desired_curvature_rate): + def update(self, active, CS, CP, VM, params, last_actuators, desired_curvature, desired_curvature_rate, llk): pass def reset(self): diff --git a/selfdrive/controls/lib/latcontrol_angle.py b/selfdrive/controls/lib/latcontrol_angle.py index c935356311..8d09432b6a 100644 --- a/selfdrive/controls/lib/latcontrol_angle.py +++ b/selfdrive/controls/lib/latcontrol_angle.py @@ -7,7 +7,7 @@ STEER_ANGLE_SATURATION_THRESHOLD = 2.5 # Degrees class LatControlAngle(LatControl): - def update(self, active, CS, CP, VM, params, last_actuators, desired_curvature, desired_curvature_rate): + def update(self, active, CS, CP, VM, params, last_actuators, desired_curvature, desired_curvature_rate, llk): angle_log = log.ControlsState.LateralAngleState.new_message() if CS.vEgo < MIN_STEER_SPEED or not active: diff --git a/selfdrive/controls/lib/latcontrol_indi.py b/selfdrive/controls/lib/latcontrol_indi.py index 2db2d43881..4d2a727623 100644 --- a/selfdrive/controls/lib/latcontrol_indi.py +++ b/selfdrive/controls/lib/latcontrol_indi.py @@ -63,7 +63,7 @@ class LatControlINDI(LatControl): self.steer_filter.x = 0. self.speed = 0. - def update(self, active, CS, CP, VM, params, last_actuators, desired_curvature, desired_curvature_rate): + def update(self, active, CS, CP, VM, params, last_actuators, desired_curvature, desired_curvature_rate, llk): self.speed = CS.vEgo # Update Kalman filter y = np.array([[math.radians(CS.steeringAngleDeg)], [math.radians(CS.steeringRateDeg)]]) diff --git a/selfdrive/controls/lib/latcontrol_lqr.py b/selfdrive/controls/lib/latcontrol_lqr.py deleted file mode 100644 index 583244882c..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, CP, VM, params, last_actuators, desired_curvature, desired_curvature_rate): - 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_pid.py b/selfdrive/controls/lib/latcontrol_pid.py index 67d17e088c..813ba13aba 100644 --- a/selfdrive/controls/lib/latcontrol_pid.py +++ b/selfdrive/controls/lib/latcontrol_pid.py @@ -17,7 +17,7 @@ class LatControlPID(LatControl): super().reset() self.pid.reset() - def update(self, active, CS, CP, VM, params, last_actuators, desired_curvature, desired_curvature_rate): + def update(self, active, CS, CP, VM, params, last_actuators, desired_curvature, desired_curvature_rate, llk): pid_log = log.ControlsState.LateralPIDState.new_message() pid_log.steeringAngleDeg = float(CS.steeringAngleDeg) pid_log.steeringRateDeg = float(CS.steeringRateDeg) diff --git a/selfdrive/controls/lib/latcontrol_torque.py b/selfdrive/controls/lib/latcontrol_torque.py new file mode 100644 index 0000000000..2816b20149 --- /dev/null +++ b/selfdrive/controls/lib/latcontrol_torque.py @@ -0,0 +1,79 @@ +import math +from selfdrive.controls.lib.pid import PIDController +from common.numpy_fast import interp +from selfdrive.controls.lib.latcontrol import LatControl, MIN_STEER_SPEED +from selfdrive.controls.lib.vehicle_model import ACCELERATION_DUE_TO_GRAVITY +from cereal import log + +# 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 +# wheel slip, or to speed. + +# This controller applies torque to achieve desired lateral +# accelerations. To compensate for the low speed effects we +# use a LOW_SPEED_FACTOR in the error. Additionally there is +# friction in the steering wheel that needs to be overcome to +# move it at all, this is compensated for too. + + +LOW_SPEED_FACTOR = 200 +JERK_THRESHOLD = 0.2 + + +class LatControlTorque(LatControl): + def __init__(self, CP, CI): + super().__init__(CP, CI) + self.pid = PIDController(CP.lateralTuning.torque.kp, CP.lateralTuning.torque.ki, + k_f=CP.lateralTuning.torque.kf, pos_limit=1.0, neg_limit=-1.0) + self.get_steer_feedforward = CI.get_steer_feedforward_function() + self.steer_max = 1.0 + self.pid.pos_limit = self.steer_max + self.pid.neg_limit = -self.steer_max + self.use_steering_angle = CP.lateralTuning.torque.useSteeringAngle + self.friction = CP.lateralTuning.torque.friction + + def reset(self): + super().reset() + self.pid.reset() + + def update(self, active, CS, CP, VM, params, last_actuators, desired_curvature, desired_curvature_rate, llk): + pid_log = log.ControlsState.LateralTorqueState.new_message() + + if CS.vEgo < MIN_STEER_SPEED or not active: + output_torque = 0.0 + pid_log.active = False + self.pid.reset() + else: + if self.use_steering_angle: + actual_curvature = -VM.calc_curvature(math.radians(CS.steeringAngleDeg - params.angleOffsetDeg), CS.vEgo, params.roll) + else: + actual_curvature = llk.angularVelocityCalibrated.value[2] / CS.vEgo + desired_lateral_accel = desired_curvature * CS.vEgo**2 + desired_lateral_jerk = desired_curvature_rate * CS.vEgo**2 + actual_lateral_accel = actual_curvature * CS.vEgo**2 + + setpoint = desired_lateral_accel + LOW_SPEED_FACTOR * desired_curvature + measurement = actual_lateral_accel + LOW_SPEED_FACTOR * actual_curvature + error = setpoint - measurement + pid_log.error = error + + ff = desired_lateral_accel - params.roll * ACCELERATION_DUE_TO_GRAVITY + output_torque = self.pid.update(error, + override=CS.steeringPressed, feedforward=ff, + speed=CS.vEgo, + freeze_integrator=CS.steeringRateLimited) + + friction_compensation = interp(desired_lateral_jerk, [-JERK_THRESHOLD, JERK_THRESHOLD], [-self.friction, self.friction]) + output_torque += friction_compensation + + pid_log.active = True + pid_log.p = self.pid.p + pid_log.i = self.pid.i + pid_log.d = self.pid.d + pid_log.f = self.pid.f + pid_log.output = -output_torque + pid_log.saturated = self._check_saturation(self.steer_max - abs(output_torque) < 1e-3, CS) + + #TODO left is positive in this convention + return -output_torque, 0.0, pid_log 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 098f651a2e..de9e404bdc 100644 --- a/selfdrive/test/process_replay/ref_commit +++ b/selfdrive/test/process_replay/ref_commit @@ -1 +1 @@ -22356d49a926a62c01d698d77c1f323016b68fd8 \ No newline at end of file +185f5f9c8d878ad4b98664afc7147400476208cc \ No newline at end of file