diff --git a/common/params.cc b/common/params.cc index 382183b..fc1aeb8 100644 --- a/common/params.cc +++ b/common/params.cc @@ -411,6 +411,7 @@ std::unordered_map keys = { {"Updated", PERSISTENT}, {"UpdateSchedule", PERSISTENT}, {"UpdateTime", PERSISTENT}, + {"UseLateralJerk", PERSISTENT}, {"UseSI", PERSISTENT}, {"UseVienna", PERSISTENT}, {"WarningSoftVolume", PERSISTENT}, diff --git a/selfdrive/car/interfaces.py b/selfdrive/car/interfaces.py index 4305798..0697bee 100644 --- a/selfdrive/car/interfaces.py +++ b/selfdrive/car/interfaces.py @@ -212,6 +212,7 @@ class CarInterfaceBase(ABC): params = Params() self.has_lateral_torque_nn = self.initialize_lat_torque_nn(CP.carFingerprint, eps_firmware) and params.get_bool("NNFF") and params.get_bool("LateralTune") + self.use_lateral_jerk = params.get_bool("UseLateralJerk") and params.get_bool("LateralTune") self.belowSteerSpeed_shown = False self.disable_belowSteerSpeed = False diff --git a/selfdrive/controls/lib/latcontrol_torque.py b/selfdrive/controls/lib/latcontrol_torque.py index 7fdf2eb..ea07586 100644 --- a/selfdrive/controls/lib/latcontrol_torque.py +++ b/selfdrive/controls/lib/latcontrol_torque.py @@ -71,13 +71,33 @@ class LatControlTorque(LatControl): self.torque_from_lateral_accel = CI.torque_from_lateral_accel() self.use_steering_angle = self.torque_params.useSteeringAngle self.steering_angle_deadzone_deg = self.torque_params.steeringAngleDeadzoneDeg - self.lowspeed_factor_factor = 1.0 # in [0, 1] in 0.1 increments. # Twilsonco's Lateral Neural Network Feedforward self.use_nn = CI.has_lateral_torque_nn + self.use_lateral_jerk = CI.use_lateral_jerk + + if self.use_nn or self.use_lateral_jerk: + # 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 guage + # whether the current desired lateral jerk will persist into the future, i.e. + # whether it's "deliberate" or not. This lets us 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. + 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 + if self.use_nn: self.pitch = FirstOrderFilter(0.0, 0.5, 0.01) - self.lowspeed_factor_factor = 1.0 # NN model takes current v_ego, lateral_accel, lat accel/jerk error, roll, and past/future/planned data # of lat accel and roll # Past value is computed using previous desired lat accel and observed roll @@ -98,32 +118,6 @@ class LatControlTorque(LatControl): self.roll_deque = deque(maxlen=history_check_frames[0]) self.error_deque = deque(maxlen=history_check_frames[0]) self.past_future_len = len(self.past_times) + len(self.nn_future_times) - - # precompute time differences between ModelConstants.T_IDXS - self.t_diffs = np.diff(ModelConstants.T_IDXS) - self.desired_lat_jerk_time = CP.steerActuatorDelay + 0.3 - - # Setup adjustable parameters - - # 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 guage - # whether the current desired lateral jerk will persist into the future, i.e. - # whether it's "deliberate" or not. This lets us 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. - self.friction_look_ahead_v = [0.8, 1.8] # how many seconds in the future to look ahead in [0, ~2.1] in 0.1 increments - self.friction_look_ahead_bp = [9.0, 35.0] # corresponding speeds in m/s in [0, ~40] in 1.0 increments - # Additionally, we use a deadzone to make sure that we only put additional torque - # when the jerk is large enough to be significant. - self.lat_jerk_deadzone = 0.0 # m/s^3 in [0, ∞] in 0.05 increments - # Finally, lateral jerk error is downscaled so it doesn't dominate the friction error - # term. - self.lat_jerk_friction_factor = 0.4 # in [0, 3] in 0.05 increments - - # Scaling the lateral acceleration "friction response" could be helpful for some. - # Increase for a stronger response, decrease for a weaker response. - self.lat_accel_friction_factor = 0.7 # in [0, 3], in 0.05 increments. 3 is arbitrary safety limit def update_live_torque_params(self, latAccelFactor, latAccelOffset, friction): self.torque_params.latAccelFactor = latAccelFactor @@ -143,7 +137,7 @@ class LatControlTorque(LatControl): if self.use_steering_angle: actual_curvature = actual_curvature_vm curvature_deadzone = abs(VM.calc_curvature(math.radians(self.steering_angle_deadzone_deg), CS.vEgo, 0.0)) - if self.use_nn: + if self.use_nn or self.use_lateral_jerk: actual_curvature_rate = -VM.calc_curvature(math.radians(CS.steeringRateDeg), CS.vEgo, 0.0) actual_lateral_jerk = actual_curvature_rate * CS.vEgo ** 2 else: @@ -158,26 +152,42 @@ class LatControlTorque(LatControl): actual_lateral_accel = actual_curvature * CS.vEgo ** 2 lateral_accel_deadzone = curvature_deadzone * CS.vEgo ** 2 - low_speed_factor = (self.lowspeed_factor_factor * interp(CS.vEgo, LOW_SPEED_X, LOW_SPEED_Y if not self.use_nn else LOW_SPEED_Y_NN))**2 + low_speed_factor = interp(CS.vEgo, LOW_SPEED_X, LOW_SPEED_Y if not self.use_nn else LOW_SPEED_Y_NN)**2 setpoint = desired_lateral_accel + low_speed_factor * desired_curvature measurement = actual_lateral_accel + low_speed_factor * actual_curvature + + lateral_jerk_setpoint = 0 + lateral_jerk_measurement = 0 + + if self.use_nn or self.use_lateral_jerk: + # prepare "look-ahead" desired lateral jerk + lat_accel_friction_factor = self.lat_accel_friction_factor + if len(model_data.acceleration.y) == ModelConstants.IDX_N: + lookahead = 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(model_data.acceleration.y, self.t_diffs) + desired_lateral_jerk = (interp(self.desired_lat_jerk_time, ModelConstants.T_IDXS, model_data.acceleration.y) - actual_lateral_accel) / self.desired_lat_jerk_time + lookahead_lateral_jerk = get_lookahead_value(predicted_lateral_jerk[LAT_PLAN_MIN_IDX:friction_upper_idx], desired_lateral_jerk) + if self.use_steering_angle or lookahead_lateral_jerk == 0.0: + lookahead_lateral_jerk = 0.0 + actual_lateral_jerk = 0.0 + lat_accel_friction_factor = 1.0 + lateral_jerk_setpoint = self.lat_jerk_friction_factor * lookahead_lateral_jerk + lateral_jerk_measurement = self.lat_jerk_friction_factor * actual_lateral_jerk + else: + lateral_jerk_setpoint = 0.0 + lateral_jerk_measurement = 0.0 + lookahead_lateral_jerk = 0.0 model_good = model_data is not None and len(model_data.orientation.x) >= CONTROL_N if self.use_nn and model_good: # update past data roll = params.roll - pitch = self.pitch.update(llk.calibratedOrientationNED.value[1]) + pitch = self.pitch.update(llk.calibratedOrientationNED.value[1]) if len(llk.calibratedOrientationNED.value) > 1 else 0.0 roll = roll_pitch_adjust(roll, pitch) self.roll_deque.append(roll) self.lateral_accel_desired_deque.append(desired_lateral_accel) - # prepare "look-ahead" desired lateral jerk - lookahead = 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(model_data.acceleration.y, self.t_diffs) - desired_lateral_jerk = (interp(self.desired_lat_jerk_time, ModelConstants.T_IDXS, model_data.acceleration.y) - actual_lateral_accel) / self.desired_lat_jerk_time - lookahead_lateral_jerk = get_lookahead_value(predicted_lateral_jerk[LAT_PLAN_MIN_IDX:friction_upper_idx], desired_lateral_jerk) - # prepare past and future values # adjust future times to account for longitudinal acceleration adjusted_future_times = [t + 0.5*CS.aEgo*(t/max(CS.vEgo, 1.0)) for t in self.nn_future_times] @@ -186,13 +196,6 @@ class LatControlTorque(LatControl): past_lateral_accels_desired = [self.lateral_accel_desired_deque[min(len(self.lateral_accel_desired_deque)-1, i)] for i in self.history_frame_offsets] future_planned_lateral_accels = [interp(t, ModelConstants.T_IDXS[:CONTROL_N], model_data.acceleration.y) for t in adjusted_future_times] - # compute NN error response. - lookahead_lateral_jerk = apply_deadzone(lookahead_lateral_jerk, self.lat_jerk_deadzone) - lateral_jerk_setpoint = self.lat_jerk_friction_factor * lookahead_lateral_jerk - lateral_jerk_measurement = self.lat_jerk_friction_factor * actual_lateral_jerk - if self.use_steering_angle or lookahead_lateral_jerk == 0.0: - lateral_jerk_measurement = 0.0 - # compute NNFF error response nnff_setpoint_input = [CS.vEgo, setpoint, lateral_jerk_setpoint, roll] \ + [setpoint] * self.past_future_len \ @@ -207,7 +210,7 @@ class LatControlTorque(LatControl): # compute feedforward (same as nn setpoint output) error = setpoint - measurement - friction_input = self.lat_accel_friction_factor * error + self.lat_jerk_friction_factor * lookahead_lateral_jerk + friction_input = lat_accel_friction_factor * error + self.lat_jerk_friction_factor * lookahead_lateral_jerk nn_input = [CS.vEgo, desired_lateral_accel, friction_input, roll] \ + past_lateral_accels_desired + future_planned_lateral_accels \ + past_rolls + future_rolls @@ -215,19 +218,23 @@ class LatControlTorque(LatControl): # apply friction override for cars with low NN friction response if self.nn_friction_override: - pid_log.error += self.torque_from_lateral_accel(0.0, self.torque_params, - friction_input, - lateral_accel_deadzone, friction_compensation=True) + pid_log.error += self.torque_from_lateral_accel(LatControlInputs(0.0, 0.0, CS.vEgo, CS.aEgo), self.torque_params, + friction_input, lateral_accel_deadzone, friction_compensation=True, gravity_adjusted=False) nn_log = nn_input + nnff_setpoint_input + nnff_measurement_input else: gravity_adjusted_lateral_accel = desired_lateral_accel - roll_compensation torque_from_setpoint = self.torque_from_lateral_accel(LatControlInputs(setpoint, roll_compensation, CS.vEgo, CS.aEgo), self.torque_params, - setpoint, lateral_accel_deadzone, friction_compensation=False, gravity_adjusted=False) + lateral_jerk_setpoint, lateral_accel_deadzone, friction_compensation=self.use_lateral_jerk, gravity_adjusted=False) torque_from_measurement = self.torque_from_lateral_accel(LatControlInputs(measurement, roll_compensation, CS.vEgo, CS.aEgo), self.torque_params, - measurement, lateral_accel_deadzone, friction_compensation=False, gravity_adjusted=False) + lateral_jerk_measurement, lateral_accel_deadzone, friction_compensation=self.use_lateral_jerk, gravity_adjusted=False) pid_log.error = torque_from_setpoint - torque_from_measurement + error = desired_lateral_accel - actual_lateral_accel + if self.use_lateral_jerk: + friction_input = lat_accel_friction_factor * error + self.lat_jerk_friction_factor * lookahead_lateral_jerk + else: + friction_input = error ff = self.torque_from_lateral_accel(LatControlInputs(gravity_adjusted_lateral_accel, roll_compensation, CS.vEgo, CS.aEgo), self.torque_params, - desired_lateral_accel - actual_lateral_accel, lateral_accel_deadzone, friction_compensation=True, + friction_input, lateral_accel_deadzone, friction_compensation=True, gravity_adjusted=True) freeze_integrator = steer_limited or CS.steeringPressed or CS.vEgo < 5 diff --git a/selfdrive/frogpilot/ui/control_settings.cc b/selfdrive/frogpilot/ui/control_settings.cc index 109a062..e15ea4f 100644 --- a/selfdrive/frogpilot/ui/control_settings.cc +++ b/selfdrive/frogpilot/ui/control_settings.cc @@ -33,6 +33,7 @@ FrogPilotControlsPanel::FrogPilotControlsPanel(SettingsWindow *parent) : FrogPil {"LateralTune", "Lateral Tuning", "Modify openpilot's steering behavior.", "../frogpilot/assets/toggle_icons/icon_lateral_tune.png"}, {"ForceAutoTune", "Force Auto Tune", "Forces comma's auto lateral tuning for unsupported vehicles.", ""}, {"NNFF", "NNFF - Neural Network Feedforward", "Use Twilsonco's Neural Network Feedforward for enhanced precision in lateral control.", ""}, + {"UseLateralJerk", "Use Lateral Jerk", "Include steer torque necessary to achieve desired steer rate (lateral jerk).", ""}, {"LongitudinalTune", "Longitudinal Tuning", "Modify openpilot's acceleration and braking behavior.", "../frogpilot/assets/toggle_icons/icon_longitudinal_tune.png"}, {"AccelerationProfile", "Acceleration Profile", "Change the acceleration rate to be either sporty or eco-friendly.", ""}, @@ -485,7 +486,7 @@ FrogPilotControlsPanel::FrogPilotControlsPanel(SettingsWindow *parent) : FrogPil } }); - std::set rebootKeys = {"AlwaysOnLateral", "HigherBitrate", "NNFF"}; + std::set rebootKeys = {"AlwaysOnLateral", "HigherBitrate", "NNFF", "UseLateralJerk"}; for (const std::string &key : rebootKeys) { QObject::connect(toggles[key], &ToggleControl::toggleFlipped, [this, key]() { if (started) { diff --git a/selfdrive/frogpilot/ui/control_settings.h b/selfdrive/frogpilot/ui/control_settings.h index bb68b5c..94cde92 100644 --- a/selfdrive/frogpilot/ui/control_settings.h +++ b/selfdrive/frogpilot/ui/control_settings.h @@ -44,7 +44,7 @@ private: std::set experimentalModeActivationKeys = {"ExperimentalModeViaDistance", "ExperimentalModeViaLKAS", "ExperimentalModeViaScreen"}; std::set fireTheBabysitterKeys = {"NoLogging", "MuteOverheated", "NoUploads", "OfflineMode"}; std::set laneChangeKeys = {"LaneChangeTime", "LaneDetection", "LaneDetectionWidth", "OneLaneChange"}; - std::set lateralTuneKeys = {"ForceAutoTune", "NNFF"}; + std::set lateralTuneKeys = {"ForceAutoTune", "NNFF", "UseLateralJerk"}; std::set longitudinalTuneKeys = {"AccelerationProfile", "DecelerationProfile", "AggressiveAcceleration", "SmoothBraking", "StoppingDistance"}; std::set mtscKeys = {"DisableMTSCSmoothing", "MTSCAggressiveness", "MTSCCurvatureCheck", "MTSCLimit"}; std::set qolKeys = {"DisableOnroadUploads", "HigherBitrate", "NavChill", "PauseLateralOnSignal", "ReverseCruise", "SetSpeedOffset"};