From f40f7f9ecee7a51dcc9f07b9337247e9bc2592c0 Mon Sep 17 00:00:00 2001 From: Maxime Desroches Date: Tue, 26 Aug 2025 21:45:49 -0700 Subject: [PATCH] Revert "torqued: apply offset (#36005)" This reverts commit 1d74a97ba65b9b9ea074ef870ab70907be33918c. --- selfdrive/controls/lib/latcontrol_torque.py | 6 +- .../tests/test_torqued_lat_accel_offset.py | 71 ------------------- selfdrive/test/process_replay/ref_commit | 2 +- 3 files changed, 4 insertions(+), 75 deletions(-) delete mode 100644 selfdrive/controls/tests/test_torqued_lat_accel_offset.py diff --git a/selfdrive/controls/lib/latcontrol_torque.py b/selfdrive/controls/lib/latcontrol_torque.py index 5a2814e089..dffe85c473 100644 --- a/selfdrive/controls/lib/latcontrol_torque.py +++ b/selfdrive/controls/lib/latcontrol_torque.py @@ -52,8 +52,10 @@ class LatControlTorque(LatControl): actual_curvature = -VM.calc_curvature(math.radians(CS.steeringAngleDeg - params.angleOffsetDeg), CS.vEgo, params.roll) roll_compensation = params.roll * ACCELERATION_DUE_TO_GRAVITY curvature_deadzone = abs(VM.calc_curvature(math.radians(self.steering_angle_deadzone_deg), CS.vEgo, 0.0)) - desired_lateral_accel = desired_curvature * CS.vEgo ** 2 + + # desired rate is the desired rate of change in the setpoint, not the absolute desired curvature + # desired_lateral_jerk = desired_curvature_rate * CS.vEgo ** 2 actual_lateral_accel = actual_curvature * CS.vEgo ** 2 lateral_accel_deadzone = curvature_deadzone * CS.vEgo ** 2 @@ -65,8 +67,6 @@ class LatControlTorque(LatControl): # do error correction in lateral acceleration space, convert at end to handle non-linear torque responses correctly pid_log.error = float(setpoint - measurement) ff = gravity_adjusted_lateral_accel - # latAccelOffset corrects roll compensation bias from device roll misalignment relative to car roll - ff -= self.torque_params.latAccelOffset ff += get_friction(desired_lateral_accel - actual_lateral_accel, lateral_accel_deadzone, FRICTION_THRESHOLD, self.torque_params) freeze_integrator = steer_limited_by_safety or CS.steeringPressed or CS.vEgo < 5 diff --git a/selfdrive/controls/tests/test_torqued_lat_accel_offset.py b/selfdrive/controls/tests/test_torqued_lat_accel_offset.py deleted file mode 100644 index 5ba9980020..0000000000 --- a/selfdrive/controls/tests/test_torqued_lat_accel_offset.py +++ /dev/null @@ -1,71 +0,0 @@ -import numpy as np -from cereal import car, messaging -from opendbc.car import ACCELERATION_DUE_TO_GRAVITY -from opendbc.car import structs -from opendbc.car.lateral import get_friction, FRICTION_THRESHOLD -from openpilot.common.realtime import DT_MDL -from openpilot.selfdrive.locationd.torqued import TorqueEstimator, MIN_BUCKET_POINTS, POINTS_PER_BUCKET, STEER_BUCKET_BOUNDS - -np.random.seed(0) - -LA_ERR_STD = 1.0 -INPUT_NOISE_STD = 0.1 -V_EGO = 30.0 - -WARMUP_BUCKET_POINTS = (1.5*MIN_BUCKET_POINTS).astype(int) -STRAIGHT_ROAD_LA_BOUNDS = (0.02, 0.03) - -ROLL_BIAS_DEG = 1.0 -ROLL_COMPENSATION_BIAS = ACCELERATION_DUE_TO_GRAVITY*float(np.sin(np.deg2rad(ROLL_BIAS_DEG))) -TORQUE_TUNE = structs.CarParams.LateralTorqueTuning(latAccelFactor=2.0, latAccelOffset=0.0, friction=0.2) -TORQUE_TUNE_BIASED = structs.CarParams.LateralTorqueTuning(latAccelFactor=2.0, latAccelOffset=-ROLL_COMPENSATION_BIAS, friction=0.2) - - -def generate_inputs(torque_tune, la_err_std, input_noise_std=None): - rng = np.random.default_rng(0) - steer_torques = np.concat([rng.uniform(bnd[0], bnd[1], pts) for bnd, pts in zip(STEER_BUCKET_BOUNDS, WARMUP_BUCKET_POINTS, strict=True)]) - la_errs = rng.normal(scale=la_err_std, size=steer_torques.size) - frictions = np.array([get_friction(la_err, 0.0, FRICTION_THRESHOLD, torque_tune) for la_err in la_errs]) - lat_accels = torque_tune.latAccelFactor*steer_torques + torque_tune.latAccelOffset + frictions - if input_noise_std is not None: - steer_torques += rng.normal(scale=input_noise_std, size=steer_torques.size) - lat_accels += rng.normal(scale=input_noise_std, size=steer_torques.size) - return steer_torques, lat_accels - -def get_warmed_up_estimator(steer_torques, lat_accels): - est = TorqueEstimator(car.CarParams()) - for steer_torque, lat_accel in zip(steer_torques, lat_accels, strict=True): - est.filtered_points.add_point(steer_torque, lat_accel) - return est - -def simulate_straight_road_msgs(est): - carControl = messaging.new_message('carControl').carControl - carOutput = messaging.new_message('carOutput').carOutput - carState = messaging.new_message('carState').carState - livePose = messaging.new_message('livePose').livePose - carControl.latActive = True - carState.vEgo = V_EGO - carState.steeringPressed = False - ts = DT_MDL*np.arange(2*POINTS_PER_BUCKET) - steer_torques = np.concat((np.linspace(-0.03, -0.02, POINTS_PER_BUCKET), np.linspace(0.02, 0.03, POINTS_PER_BUCKET))) - lat_accels = TORQUE_TUNE.latAccelFactor * steer_torques - for t, steer_torque, lat_accel in zip(ts, steer_torques, lat_accels, strict=True): - carOutput.actuatorsOutput.torque = float(-steer_torque) - livePose.orientationNED.x = float(np.deg2rad(ROLL_BIAS_DEG)) - livePose.angularVelocityDevice.z = float(lat_accel / V_EGO) - for which, msg in (('carControl', carControl), ('carOutput', carOutput), ('carState', carState), ('livePose', livePose)): - est.handle_log(t, which, msg) - -def test_estimated_offset(): - steer_torques, lat_accels = generate_inputs(TORQUE_TUNE_BIASED, la_err_std=LA_ERR_STD, input_noise_std=INPUT_NOISE_STD) - est = get_warmed_up_estimator(steer_torques, lat_accels) - msg = est.get_msg() - # TODO add lataccelfactor and friction check when we have more accurate estimates - assert abs(msg.liveTorqueParameters.latAccelOffsetRaw - TORQUE_TUNE_BIASED.latAccelOffset) < 0.03 - -def test_straight_road_roll_bias(): - steer_torques, lat_accels = generate_inputs(TORQUE_TUNE, la_err_std=LA_ERR_STD, input_noise_std=INPUT_NOISE_STD) - est = get_warmed_up_estimator(steer_torques, lat_accels) - simulate_straight_road_msgs(est) - msg = est.get_msg() - assert (msg.liveTorqueParameters.latAccelOffsetRaw < -0.05) and np.isfinite(msg.liveTorqueParameters.latAccelOffsetRaw) diff --git a/selfdrive/test/process_replay/ref_commit b/selfdrive/test/process_replay/ref_commit index 7d21526144..8e4ff1e2a8 100644 --- a/selfdrive/test/process_replay/ref_commit +++ b/selfdrive/test/process_replay/ref_commit @@ -1 +1 @@ -866f2cb1f0e49b2cb7115aa8131164b3a75fb2c5 \ No newline at end of file +209b47bea61e145cf2d27eb3ab650c97bcd1d33f \ No newline at end of file