From 6891b795c49070a8916946454a85fae0fddb84a6 Mon Sep 17 00:00:00 2001 From: Shane Smiskol Date: Mon, 3 Mar 2025 21:47:52 -0600 Subject: [PATCH] controls: limit max curvature from lateral acceleration (#34651) * limit max curvature with lateral accel too * not a guideline * roll compensation in curv clip * improve clipping and alerting * typo * clean up * no float * get ready * good idea * good * redundant * TODO * test * do max curvature clip last * flip --------- Co-authored-by: Bruce Wayne --- selfdrive/controls/controlsd.py | 8 ++-- selfdrive/controls/lib/drive_helpers.py | 38 +++++++++++++------ selfdrive/controls/lib/latcontrol.py | 7 ++-- selfdrive/controls/lib/latcontrol_angle.py | 4 +- selfdrive/controls/lib/latcontrol_pid.py | 4 +- selfdrive/controls/lib/latcontrol_torque.py | 4 +- .../controls/lib/tests/test_latcontrol.py | 10 ++++- selfdrive/selfdrived/selfdrived.py | 2 +- 8 files changed, 50 insertions(+), 27 deletions(-) diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index 14fd37040e..dcc1c74794 100755 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -109,11 +109,11 @@ class Controls: actuators.accel = float(self.LoC.update(CC.longActive, CS, long_plan.aTarget, long_plan.shouldStop, pid_accel_limits)) # Steering PID loop and lateral MPC - self.desired_curvature = clip_curvature(CS.vEgo, self.desired_curvature, model_v2.action.desiredCurvature) - actuators.curvature = float(self.desired_curvature) + self.desired_curvature, curvature_limited = clip_curvature(CS.vEgo, self.desired_curvature, model_v2.action.desiredCurvature, lp.roll) + actuators.curvature = self.desired_curvature steer, steeringAngleDeg, lac_log = self.LaC.update(CC.latActive, CS, self.VM, lp, self.steer_limited_by_controls, self.desired_curvature, - self.calibrated_pose) # TODO what if not available + self.calibrated_pose, curvature_limited) # TODO what if not available actuators.torque = float(steer) actuators.steeringAngleDeg = float(steeringAngleDeg) # Ensure no NaNs/Infs @@ -180,7 +180,7 @@ class Controls: cs.longitudinalPlanMonoTime = self.sm.logMonoTime['longitudinalPlan'] cs.lateralPlanMonoTime = self.sm.logMonoTime['modelV2'] - cs.desiredCurvature = float(self.desired_curvature) + cs.desiredCurvature = self.desired_curvature cs.longControlState = self.LoC.long_control_state cs.upAccelCmd = float(self.LoC.pid.p) cs.uiAccelCmd = float(self.LoC.pid.i) diff --git a/selfdrive/controls/lib/drive_helpers.py b/selfdrive/controls/lib/drive_helpers.py index dcb0093a18..41384abae8 100644 --- a/selfdrive/controls/lib/drive_helpers.py +++ b/selfdrive/controls/lib/drive_helpers.py @@ -1,5 +1,6 @@ import numpy as np from cereal import log +from opendbc.car.vehicle_model import ACCELERATION_DUE_TO_GRAVITY from openpilot.common.realtime import DT_CTRL MIN_SPEED = 1.0 @@ -7,20 +8,33 @@ CONTROL_N = 17 CAR_ROTATION_RADIUS = 0.0 # This is a turn radius smaller than most cars can achieve MAX_CURVATURE = 0.2 +MAX_VEL_ERR = 5.0 # m/s # EU guidelines -MAX_LATERAL_JERK = 5.0 -MAX_VEL_ERR = 5.0 - -def clip_curvature(v_ego, prev_curvature, new_curvature): - new_curvature = np.clip(new_curvature, -MAX_CURVATURE, MAX_CURVATURE) - v_ego = max(MIN_SPEED, v_ego) - max_curvature_rate = MAX_LATERAL_JERK / (v_ego**2) # inexact calculation, check https://github.com/commaai/openpilot/pull/24755 - safe_desired_curvature = np.clip(new_curvature, - prev_curvature - max_curvature_rate * DT_CTRL, - prev_curvature + max_curvature_rate * DT_CTRL) - - return safe_desired_curvature +MAX_LATERAL_JERK = 5.0 # m/s^3 +MAX_LATERAL_ACCEL_NO_ROLL = 3.0 # m/s^2 + + +def clamp(val, min_val, max_val): + clamped_val = float(np.clip(val, min_val, max_val)) + return clamped_val, clamped_val != val + + +def clip_curvature(v_ego, prev_curvature, new_curvature, roll): + # This function respects ISO lateral jerk and acceleration limits + a max curvature + v_ego = max(v_ego, MIN_SPEED) + max_curvature_rate = MAX_LATERAL_JERK / (v_ego ** 2) # inexact calculation, check https://github.com/commaai/openpilot/pull/24755 + new_curvature = np.clip(new_curvature, + prev_curvature - max_curvature_rate * DT_CTRL, + prev_curvature + max_curvature_rate * DT_CTRL) + + roll_compensation = roll * ACCELERATION_DUE_TO_GRAVITY + max_lat_accel = MAX_LATERAL_ACCEL_NO_ROLL + roll_compensation + min_lat_accel = -MAX_LATERAL_ACCEL_NO_ROLL + roll_compensation + new_curvature, limited_accel = clamp(new_curvature, min_lat_accel / v_ego ** 2, max_lat_accel / v_ego ** 2) + + new_curvature, limited_max_curv = clamp(new_curvature, -MAX_CURVATURE, MAX_CURVATURE) + return float(new_curvature), limited_accel or limited_max_curv def get_speed_error(modelV2: log.ModelDataV2, v_ego: float) -> float: diff --git a/selfdrive/controls/lib/latcontrol.py b/selfdrive/controls/lib/latcontrol.py index f0121f73bc..dcf0003430 100644 --- a/selfdrive/controls/lib/latcontrol.py +++ b/selfdrive/controls/lib/latcontrol.py @@ -17,14 +17,15 @@ class LatControl(ABC): self.steer_max = 1.0 @abstractmethod - def update(self, active, CS, VM, params, steer_limited_by_controls, desired_curvature, calibrated_pose): + def update(self, active, CS, VM, params, steer_limited_by_controls, desired_curvature, calibrated_pose, curvature_limited): pass def reset(self): self.sat_count = 0. - def _check_saturation(self, saturated, CS, steer_limited_by_controls): - if saturated and CS.vEgo > self.sat_check_min_speed and not steer_limited_by_controls and not CS.steeringPressed: + def _check_saturation(self, saturated, CS, steer_limited_by_controls, curvature_limited): + # Saturated only if control output is not being limited by car torque/angle rate limits + if (saturated or curvature_limited) and CS.vEgo > self.sat_check_min_speed and not steer_limited_by_controls and not CS.steeringPressed: self.sat_count += self.sat_count_rate else: self.sat_count -= self.sat_count_rate diff --git a/selfdrive/controls/lib/latcontrol_angle.py b/selfdrive/controls/lib/latcontrol_angle.py index de07423fd4..1b249a3d1a 100644 --- a/selfdrive/controls/lib/latcontrol_angle.py +++ b/selfdrive/controls/lib/latcontrol_angle.py @@ -11,7 +11,7 @@ class LatControlAngle(LatControl): super().__init__(CP, CI) self.sat_check_min_speed = 5. - def update(self, active, CS, VM, params, steer_limited_by_controls, desired_curvature, calibrated_pose): + def update(self, active, CS, VM, params, steer_limited_by_controls, desired_curvature, calibrated_pose, curvature_limited): angle_log = log.ControlsState.LateralAngleState.new_message() if not active: @@ -23,7 +23,7 @@ class LatControlAngle(LatControl): angle_steers_des += params.angleOffsetDeg angle_control_saturated = abs(angle_steers_des - CS.steeringAngleDeg) > STEER_ANGLE_SATURATION_THRESHOLD - angle_log.saturated = bool(self._check_saturation(angle_control_saturated, CS, False)) + angle_log.saturated = bool(self._check_saturation(angle_control_saturated, CS, False, curvature_limited)) angle_log.steeringAngleDeg = float(CS.steeringAngleDeg) angle_log.steeringAngleDesiredDeg = angle_steers_des return 0, float(angle_steers_des), angle_log diff --git a/selfdrive/controls/lib/latcontrol_pid.py b/selfdrive/controls/lib/latcontrol_pid.py index cf54125a77..1f1199565e 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, VM, params, steer_limited_by_controls, desired_curvature, calibrated_pose): + def update(self, active, CS, VM, params, steer_limited_by_controls, desired_curvature, calibrated_pose, curvature_limited): pid_log = log.ControlsState.LateralPIDState.new_message() pid_log.steeringAngleDeg = float(CS.steeringAngleDeg) pid_log.steeringRateDeg = float(CS.steeringRateDeg) @@ -43,6 +43,6 @@ class LatControlPID(LatControl): pid_log.i = float(self.pid.i) pid_log.f = float(self.pid.f) pid_log.output = float(output_steer) - pid_log.saturated = bool(self._check_saturation(self.steer_max - abs(output_steer) < 1e-3, CS, steer_limited_by_controls)) + pid_log.saturated = bool(self._check_saturation(self.steer_max - abs(output_steer) < 1e-3, CS, steer_limited_by_controls, curvature_limited)) return output_steer, angle_steers_des, pid_log diff --git a/selfdrive/controls/lib/latcontrol_torque.py b/selfdrive/controls/lib/latcontrol_torque.py index 7b00cce56b..3aef57baca 100644 --- a/selfdrive/controls/lib/latcontrol_torque.py +++ b/selfdrive/controls/lib/latcontrol_torque.py @@ -37,7 +37,7 @@ class LatControlTorque(LatControl): self.torque_params.latAccelOffset = latAccelOffset self.torque_params.friction = friction - def update(self, active, CS, VM, params, steer_limited_by_controls, desired_curvature, calibrated_pose): + def update(self, active, CS, VM, params, steer_limited_by_controls, desired_curvature, calibrated_pose, curvature_limited): pid_log = log.ControlsState.LateralTorqueState.new_message() if not active: output_torque = 0.0 @@ -87,7 +87,7 @@ class LatControlTorque(LatControl): pid_log.output = float(-output_torque) pid_log.actualLateralAccel = float(actual_lateral_accel) pid_log.desiredLateralAccel = float(desired_lateral_accel) - pid_log.saturated = bool(self._check_saturation(self.steer_max - abs(output_torque) < 1e-3, CS, steer_limited_by_controls)) + pid_log.saturated = bool(self._check_saturation(self.steer_max - abs(output_torque) < 1e-3, CS, steer_limited_by_controls, curvature_limited)) # 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 c7038dd581..564c93be0d 100644 --- a/selfdrive/controls/lib/tests/test_latcontrol.py +++ b/selfdrive/controls/lib/tests/test_latcontrol.py @@ -33,7 +33,15 @@ class TestLatControl: lp = generate_livePose() pose = Pose.from_live_pose(lp.livePose) + # Saturate for curvature limited and controller limited for _ in range(1000): - _, _, lac_log = controller.update(True, CS, VM, params, False, 1, pose) + _, _, lac_log = controller.update(True, CS, VM, params, False, 0, pose, True) + assert lac_log.saturated + + for _ in range(1000): + _, _, lac_log = controller.update(True, CS, VM, params, False, 0, pose, False) + assert not lac_log.saturated + for _ in range(1000): + _, _, lac_log = controller.update(True, CS, VM, params, False, 1, pose, False) assert lac_log.saturated diff --git a/selfdrive/selfdrived/selfdrived.py b/selfdrive/selfdrived/selfdrived.py index 68716c1f64..8b7db19c9a 100755 --- a/selfdrive/selfdrived/selfdrived.py +++ b/selfdrive/selfdrived/selfdrived.py @@ -328,7 +328,7 @@ class SelfdriveD: if lac.active and not recent_steer_pressed and not self.CP.notCar: clipped_speed = max(CS.vEgo, MIN_LATERAL_CONTROL_SPEED) actual_lateral_accel = controlstate.curvature * (clipped_speed**2) - desired_lateral_accel = controlstate.desiredCurvature * (clipped_speed**2) + desired_lateral_accel = self.sm['modelV2'].action.desiredCurvature * (clipped_speed**2) undershooting = abs(desired_lateral_accel) / abs(1e-3 + actual_lateral_accel) > 1.2 turning = abs(desired_lateral_accel) > 1.0 # TODO: lac.saturated includes speed and other checks, should be pulled out