Customizable personality profiles

Added toggles to customize the personality profiles.

common/params.cc

@@ -212,6 +212,8 @@ std::unordered_map<std::string, uint32_t> keys = {
     {"AccelerationPath", PERSISTENT},
     {"AccelerationProfile", PERSISTENT},
     {"AggressiveAcceleration", PERSISTENT},
+    {"AggressiveFollow", PERSISTENT},
+    {"AggressiveJerk", PERSISTENT},
     {"AlertVolumeControl", PERSISTENT},
     {"AlwaysOnLateral", PERSISTENT},
     {"AlwaysOnLateralMain", PERSISTENT},
@@ -235,6 +237,7 @@ std::unordered_map<std::string, uint32_t> keys = {
     {"CustomAlerts", PERSISTENT},
     {"CustomColors", PERSISTENT},
     {"CustomIcons", PERSISTENT},
+    {"CustomPersonalities", PERSISTENT},
     {"CustomUI", PERSISTENT},
     {"CustomSignals", PERSISTENT},
     {"CustomSounds", PERSISTENT},
@@ -259,8 +262,12 @@ std::unordered_map<std::string, uint32_t> keys = {
     {"QOLControls", PERSISTENT},
     {"QOLVisuals", PERSISTENT},
     {"RefuseVolume", PERSISTENT},
+    {"RelaxedFollow", PERSISTENT},
+    {"RelaxedJerk", PERSISTENT},
     {"RoadEdgesWidth", PERSISTENT},
     {"SilentMode", PERSISTENT},
+    {"StandardFollow", PERSISTENT},
+    {"StandardJerk", PERSISTENT},
     {"StockTune", PERSISTENT},
     {"UnlimitedLength", PERSISTENT},
     {"UpdateSchedule", PERSISTENT},

selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py

@@ -56,26 +56,46 @@ T_DIFFS = np.diff(T_IDXS, prepend=[0.])
 COMFORT_BRAKE = 2.5
 STOP_DISTANCE = 6.0
 
-def get_jerk_factor(personality=log.LongitudinalPersonality.standard):
+def get_jerk_factor(custom_personalities=False, aggressive_jerk=0.5, standard_jerk=1.0, relaxed_jerk=1.0, personality=log.LongitudinalPersonality.standard):
-  if personality==log.LongitudinalPersonality.relaxed:
+  if custom_personalities:
-    return 1.0
+    if personality==log.LongitudinalPersonality.relaxed:
-  elif personality==log.LongitudinalPersonality.standard:
+      return relaxed_jerk
-    return 1.0
+    elif personality==log.LongitudinalPersonality.standard:
-  elif personality==log.LongitudinalPersonality.aggressive:
+      return standard_jerk
-    return 0.5
+    elif personality==log.LongitudinalPersonality.aggressive:
+      return aggressive_jerk
+    else:
+      raise NotImplementedError("Longitudinal personality not supported")
   else:
-    raise NotImplementedError("Longitudinal personality not supported")
+    if personality==log.LongitudinalPersonality.relaxed:
+      return 1.0
+    elif personality==log.LongitudinalPersonality.standard:
+      return 1.0
+    elif personality==log.LongitudinalPersonality.aggressive:
+      return 0.5
+    else:
+      raise NotImplementedError("Longitudinal personality not supported")
 
 
-def get_T_FOLLOW(personality=log.LongitudinalPersonality.standard):
+def get_T_FOLLOW(custom_personalities=False, aggressive_follow=1.25, standard_follow=1.45, relaxed_follow=1.75, personality=log.LongitudinalPersonality.standard):
-  if personality==log.LongitudinalPersonality.relaxed:
+  if custom_personalities:
-    return 1.75
+    if personality==log.LongitudinalPersonality.relaxed:
-  elif personality==log.LongitudinalPersonality.standard:
+      return relaxed_follow
-    return 1.45
+    elif personality==log.LongitudinalPersonality.standard:
-  elif personality==log.LongitudinalPersonality.aggressive:
+      return standard_follow
-    return 1.25
+    elif personality==log.LongitudinalPersonality.aggressive:
+      return aggressive_follow
+    else:
+      raise NotImplementedError("Longitudinal personality not supported")
   else:
-    raise NotImplementedError("Longitudinal personality not supported")
+    if personality==log.LongitudinalPersonality.relaxed:
+      return 1.75
+    elif personality==log.LongitudinalPersonality.standard:
+      return 1.45
+    elif personality==log.LongitudinalPersonality.aggressive:
+      return 1.25
+    else:
+      raise NotImplementedError("Longitudinal personality not supported")
 
 def get_stopped_equivalence_factor(v_lead):
   return (v_lead**2) / (2 * COMFORT_BRAKE)
@@ -274,8 +294,8 @@ class LongitudinalMpc:
     for i in range(N):
       self.solver.cost_set(i, 'Zl', Zl)
 
-  def set_weights(self, prev_accel_constraint=True, personality=log.LongitudinalPersonality.standard):
+  def set_weights(self, prev_accel_constraint=True, custom_personalities=False, aggressive_jerk=0.5, standard_jerk=1.0, relaxed_jerk=1.0, personality=log.LongitudinalPersonality.standard):
-    jerk_factor = get_jerk_factor(personality)
+    jerk_factor = get_jerk_factor(custom_personalities, aggressive_jerk, standard_jerk, relaxed_jerk, personality)
     jerk_factor /= np.mean(self.acceleration_offset)
 
     if self.mode == 'acc':
@@ -336,7 +356,7 @@ class LongitudinalMpc:
     self.max_a = max_a
 
   def update(self, radarstate, v_cruise, x, v, a, j, frogpilot_planner, personality=log.LongitudinalPersonality.standard):
-    t_follow = get_T_FOLLOW(personality)
+    t_follow = get_T_FOLLOW(frogpilot_planner.custom_personalities, frogpilot_planner.aggressive_follow, frogpilot_planner.standard_follow, frogpilot_planner.relaxed_follow, personality)
     self.t_follow = t_follow
     v_ego = self.x0[1]
     self.status = radarstate.leadOne.status or radarstate.leadTwo.status

selfdrive/controls/lib/longitudinal_planner.py

@@ -136,7 +136,9 @@ class LongitudinalPlanner:
     accel_limits_turns[0] = min(accel_limits_turns[0], self.a_desired + 0.05)
     accel_limits_turns[1] = max(accel_limits_turns[1], self.a_desired - 0.05)
 
-    self.mpc.set_weights(prev_accel_constraint, personality=self.personality)
+    self.mpc.set_weights(prev_accel_constraint,
+                         frogpilot_planner.custom_personalities, frogpilot_planner.aggressive_jerk, frogpilot_planner.standard_jerk, frogpilot_planner.relaxed_jerk,
+                         personality=self.personality)
     self.mpc.set_accel_limits(accel_limits_turns[0], accel_limits_turns[1])
     self.mpc.set_cur_state(self.v_desired_filter.x, self.a_desired)
     x, v, a, j = self.parse_model(sm['modelV2'], self.v_model_error)

selfdrive/frogpilot/functions/frogpilot_planner.py

@@ -107,6 +107,14 @@ class FrogPilotPlanner:
       self.cem.update_frogpilot_params(self.is_metric, params)
       params.put_bool("ExperimentalMode", True)
 
+    self.custom_personalities = params.get_bool("CustomPersonalities")
+    self.aggressive_follow = params.get_float("AggressiveFollow")
+    self.standard_follow = params.get_float("StandardFollow")
+    self.relaxed_follow = params.get_float("RelaxedFollow")
+    self.aggressive_jerk = params.get_float("AggressiveJerk")
+    self.standard_jerk = params.get_float("StandardJerk")
+    self.relaxed_jerk = params.get_float("RelaxedJerk")
+
     custom_ui = params.get_bool("CustomUI")
     self.blind_spot_path = custom_ui and params.get_bool("BlindSpotPath")
 

selfdrive/frogpilot/ui/control_settings.cc

@@ -11,6 +11,8 @@ FrogPilotControlsPanel::FrogPilotControlsPanel(SettingsWindow *parent) : FrogPil
     {"CEStopLights", "Stop Lights and Stop Signs", "Switch to 'Experimental Mode' when a stop light or stop sign is detected.", ""},
     {"CESignal", "Turn Signal When Below Highway Speeds", "Switch to 'Experimental Mode' when using turn signals below highway speeds to help assit with turns.", ""},
 
+    {"CustomPersonalities", "Custom Driving Personalities", "Customize the driving personality profiles to your driving style.", "../frogpilot/assets/toggle_icons/icon_custom.png"},
+
     {"LateralTune", "Lateral Tuning", "Modify openpilot's steering behavior.", "../frogpilot/assets/toggle_icons/icon_lateral_tune.png"},
 
     {"LongitudinalTune", "Longitudinal Tuning", "Modify openpilot's acceleration and braking behavior.", "../frogpilot/assets/toggle_icons/icon_longitudinal_tune.png"},
@@ -79,6 +81,58 @@ FrogPilotControlsPanel::FrogPilotControlsPanel(SettingsWindow *parent) : FrogPil
       std::vector<QString> stopLightToggleNames{tr("With Lead")};
       toggle = new FrogPilotParamToggleControl(param, title, desc, icon, stopLightToggles, stopLightToggleNames);
 
+    } else if (param == "CustomPersonalities") {
+      FrogPilotParamManageControl *customPersonalitiesToggle = new FrogPilotParamManageControl(param, title, desc, icon, this);
+      QObject::connect(customPersonalitiesToggle, &FrogPilotParamManageControl::manageButtonClicked, this, [this]() {
+        parentToggleClicked();
+        for (auto &[key, toggle] : toggles) {
+          toggle->setVisible(false);
+        }
+        aggressiveProfile->setVisible(true);
+        standardProfile->setVisible(true);
+        relaxedProfile->setVisible(true);
+      });
+      toggle = customPersonalitiesToggle;
+
+      FrogPilotParamValueControl *aggressiveFollow = new FrogPilotParamValueControl("AggressiveFollow", "Follow",
+                                                                                    "Set the 'Aggressive' personality' following distance. "
+                                                                                    "Represents seconds to follow behind the lead vehicle.\n\nStock: 1.25 seconds.",
+                                                                                    "../frogpilot/assets/other_images/aggressive.png",
+                                                                                    1, 5, std::map<int, QString>(), this, false, " sec", 1, 0.01);
+      FrogPilotParamValueControl *aggressiveJerk = new FrogPilotParamValueControl("AggressiveJerk", " Jerk",
+                                                                                  "Configure brake/gas pedal responsiveness for the 'Aggressive' personality. "
+                                                                                  "Higher jerk value = smoother rides.\nLower jerk value = faster response.\n\nStock: 0.5.",
+                                                                                  "",
+                                                                                  0.01, 5, std::map<int, QString>(), this, false, "", 1, 0.01);
+      aggressiveProfile = new FrogPilotDualParamControl(aggressiveFollow, aggressiveJerk, this, true);
+      addItem(aggressiveProfile);
+
+      FrogPilotParamValueControl *standardFollow = new FrogPilotParamValueControl("StandardFollow", "Follow",
+                                                                                  "Set the 'Standard' personality following distance. "
+                                                                                  "Represents seconds to follow behind the lead vehicle.\n\nStock: 1.45 seconds.",
+                                                                                  "../frogpilot/assets/other_images/standard.png",
+                                                                                  1, 5, std::map<int, QString>(), this, false, " sec", 1, 0.01);
+      FrogPilotParamValueControl *standardJerk = new FrogPilotParamValueControl("StandardJerk", " Jerk",
+                                                                                "Adjust brake/gas pedal responsiveness for the 'Standard' personality. "
+                                                                                "Higher jerk value = smoother rides.\nLower jerk value = faster response.\n\nStock: 1.0.",
+                                                                                "",
+                                                                                0.01, 5, std::map<int, QString>(), this, false, "", 1, 0.01);
+      standardProfile = new FrogPilotDualParamControl(standardFollow, standardJerk, this, true);
+      addItem(standardProfile);
+
+      FrogPilotParamValueControl *relaxedFollow = new FrogPilotParamValueControl("RelaxedFollow", "Follow",
+                                                                                 "Set the 'Relaxed' personality following distance. "
+                                                                                 "Represents seconds to follow behind the lead vehicle.\n\nStock: 1.75 seconds.",
+                                                                                 "../frogpilot/assets/other_images/relaxed.png",
+                                                                                 1, 5, std::map<int, QString>(), this, false, " sec", 1, 0.01);
+      FrogPilotParamValueControl *relaxedJerk = new FrogPilotParamValueControl("RelaxedJerk", " Jerk",
+                                                                               "Set brake/gas pedal responsiveness for the 'Relaxed' personality. "
+                                                                               "Higher jerk value = smoother rides.\nLower jerk value = faster response.\n\nStock: 1.0.",
+                                                                               "",
+                                                                               0.01, 5, std::map<int, QString>(), this, false, "", 1, 0.01);
+      relaxedProfile = new FrogPilotDualParamControl(relaxedFollow, relaxedJerk, this, true);
+      addItem(relaxedProfile);
+
     } else if (param == "LateralTune") {
       FrogPilotParamManageControl *lateralTuneToggle = new FrogPilotParamManageControl(param, title, desc, icon, this);
       QObject::connect(lateralTuneToggle, &FrogPilotParamManageControl::manageButtonClicked, this, [this]() {
@@ -208,8 +262,11 @@ void FrogPilotControlsPanel::updateMetric() {
 }
 
 void FrogPilotControlsPanel::parentToggleClicked() {
+  aggressiveProfile->setVisible(false);
   conditionalSpeedsImperial->setVisible(false);
   conditionalSpeedsMetric->setVisible(false);
+  standardProfile->setVisible(false);
+  relaxedProfile->setVisible(false);
 
   openParentToggle();
 }
@@ -219,8 +276,11 @@ void FrogPilotControlsPanel::subParentToggleClicked() {
 }
 
 void FrogPilotControlsPanel::hideSubToggles() {
+  aggressiveProfile->setVisible(false);
   conditionalSpeedsImperial->setVisible(false);
   conditionalSpeedsMetric->setVisible(false);
+  standardProfile->setVisible(false);
+  relaxedProfile->setVisible(false);
 
   for (auto &[key, toggle] : toggles) {
     bool subToggles = conditionalExperimentalKeys.find(key.c_str()) != conditionalExperimentalKeys.end() ||

selfdrive/frogpilot/ui/control_settings.h

@@ -29,8 +29,11 @@ private:
   void updateState(const UIState &s);
   void updateToggles();
 
+  FrogPilotDualParamControl *aggressiveProfile;
   FrogPilotDualParamControl *conditionalSpeedsImperial;
   FrogPilotDualParamControl *conditionalSpeedsMetric;
+  FrogPilotDualParamControl *standardProfile;
+  FrogPilotDualParamControl *relaxedProfile;
 
   std::set<QString> conditionalExperimentalKeys = {"CECurves", "CECurvesLead", "CESlowerLead", "CENavigation", "CEStopLights", "CESignal"};
   std::set<QString> fireTheBabysitterKeys = {};