brianhansonxyz/clearpilot
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Torque lateral jerk with stock torque controller

Browse Source 
Add toggle to use Twilsonco's  lateral jerk with stock torque controller.

Co-Authored-By: Tim Wilson <7284371+twilsonco@users.noreply.github.com>
This commit is contained in:
FrogAi
2024-02-27 16:34:47 -07:00
parent 437ecc73ca
commit a0209cf918
5 changed files with 64 additions and 54 deletions
Download Patch File Download Diff File Expand all files Collapse all files

1
common/params.cc
View File

@@ -411,6 +411,7 @@ std::unordered_map<std::string, uint32_t> keys = {
{"Updated", PERSISTENT},
{"UpdateSchedule", PERSISTENT},
{"UpdateTime", PERSISTENT},
{"UseLateralJerk", PERSISTENT},
{"UseSI", PERSISTENT},
{"UseVienna", PERSISTENT},
{"WarningSoftVolume", PERSISTENT},

1
selfdrive/car/interfaces.py
View File

@@ -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

111
selfdrive/controls/lib/latcontrol_torque.py
View File

@@ -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
@@ -99,32 +119,6 @@ class LatControlTorque(LatControl):
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
self.torque_params.latAccelOffset = latAccelOffset
@@ -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,25 +152,41 @@ 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
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])
roll = roll_pitch_adjust(roll, pitch)
self.roll_deque.append(roll)
self.lateral_accel_desired_deque.append(desired_lateral_accel)
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]) 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 past and future values
# adjust future times to account for longitudinal acceleration
@@ -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

3
selfdrive/frogpilot/ui/control_settings.cc
View File

@@ -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<std::string> rebootKeys = {"AlwaysOnLateral", "HigherBitrate", "NNFF"};
std::set<std::string> rebootKeys = {"AlwaysOnLateral", "HigherBitrate", "NNFF", "UseLateralJerk"};
for (const std::string &key : rebootKeys) {
QObject::connect(toggles[key], &ToggleControl::toggleFlipped, [this, key]() {
if (started) {

2
selfdrive/frogpilot/ui/control_settings.h
View File

@@ -44,7 +44,7 @@ private:
std::set<QString> experimentalModeActivationKeys = {"ExperimentalModeViaDistance", "ExperimentalModeViaLKAS", "ExperimentalModeViaScreen"};
std::set<QString> fireTheBabysitterKeys = {"NoLogging", "MuteOverheated", "NoUploads", "OfflineMode"};
std::set<QString> laneChangeKeys = {"LaneChangeTime", "LaneDetection", "LaneDetectionWidth", "OneLaneChange"};
std::set<QString> lateralTuneKeys = {"ForceAutoTune", "NNFF"};
std::set<QString> lateralTuneKeys = {"ForceAutoTune", "NNFF", "UseLateralJerk"};
std::set<QString> longitudinalTuneKeys = {"AccelerationProfile", "DecelerationProfile", "AggressiveAcceleration", "SmoothBraking", "StoppingDistance"};
std::set<QString> mtscKeys = {"DisableMTSCSmoothing", "MTSCAggressiveness", "MTSCCurvatureCheck", "MTSCLimit"};
std::set<QString> qolKeys = {"DisableOnroadUploads", "HigherBitrate", "NavChill", "PauseLateralOnSignal", "ReverseCruise", "SetSpeedOffset"};