modeld_v2: planplus model tuning (#1620)

* modeld: planplus model tuning

* little more

* final

---------

Co-authored-by: Jason Wen <haibin.wen3@gmail.com>

common/params_keys.h

@@ -224,6 +224,7 @@ inline static std::unordered_map<std::string, ParamKeyAttributes> keys = {
     {"LagdValueCache", {PERSISTENT, FLOAT, "0.2"}},
     {"LaneTurnDesire", {PERSISTENT | BACKUP, BOOL, "0"}},
     {"LaneTurnValue", {PERSISTENT | BACKUP, FLOAT, "19.0"}},
+    {"PlanplusControl", {PERSISTENT | BACKUP, FLOAT, "1.0"}},
 
     // mapd
     {"MapAdvisorySpeedLimit", {CLEAR_ON_ONROAD_TRANSITION, FLOAT}},

sunnypilot/modeld_v2/modeld.py

@@ -28,7 +28,6 @@ from openpilot.sunnypilot.models.helpers import get_active_bundle
 from openpilot.sunnypilot.models.runners.helpers import get_model_runner
 
 PROCESS_NAME = "selfdrive.modeld.modeld_tinygrad"
-RECOVERY_POWER = 1.0 # The higher this number the more aggressively the model will recover to lanecenter, too high and it will ping-pong
 
 
 class FrameMeta:
@@ -63,6 +62,7 @@ class ModelState(ModelStateBase):
     self.LAT_SMOOTH_SECONDS = float(overrides.get('lat', ".0"))
     self.LONG_SMOOTH_SECONDS = float(overrides.get('long', ".0"))
     self.MIN_LAT_CONTROL_SPEED = 0.3
+    self.PLANPLUS_CONTROL: float = 1.0
 
     buffer_length = 5 if self.model_runner.is_20hz else 2
     self.frames = {name: DrivingModelFrame(context, buffer_length) for name in self.model_runner.vision_input_names}
@@ -158,7 +158,8 @@ class ModelState(ModelStateBase):
                             lat_action_t: float, long_action_t: float, v_ego: float) -> log.ModelDataV2.Action:
     plan = model_output['plan'][0]
     if 'planplus' in model_output:
-      plan = plan + RECOVERY_POWER*model_output['planplus'][0]
+      recovery_power = self.PLANPLUS_CONTROL * (0.75 if v_ego > 20.0 else 1.0)
+      plan = plan + recovery_power * model_output['planplus'][0]
     desired_accel, should_stop = get_accel_from_plan(plan[:, Plan.VELOCITY][:, 0], plan[:, Plan.ACCELERATION][:, 0], self.constants.T_IDXS,
                                                      action_t=long_action_t)
     desired_accel = smooth_value(desired_accel, prev_action.desiredAcceleration, self.LONG_SMOOTH_SECONDS)
@@ -283,6 +284,7 @@ def main(demo=False):
     v_ego = max(sm["carState"].vEgo, 0.)
     if sm.frame % 60 == 0:
       model.lat_delay = get_lat_delay(params, sm["liveDelay"].lateralDelay)
+      model.PLANPLUS_CONTROL = params.get("PlanplusControl", return_default=True)
     lat_delay = model.lat_delay + model.LAT_SMOOTH_SECONDS
     if sm.updated["liveCalibration"] and sm.seen['roadCameraState'] and sm.seen['deviceState']:
       device_from_calib_euler = np.array(sm["liveCalibration"].rpyCalib, dtype=np.float32)

sunnypilot/modeld_v2/tests/test_recovery_power.py

@@ -0,0 +1,61 @@
+import numpy as np
+
+from cereal import log
+
+from openpilot.sunnypilot.modeld_v2.constants import Plan
+from openpilot.sunnypilot.modeld_v2.modeld import ModelState
+import openpilot.sunnypilot.modeld_v2.modeld as modeld
+
+
+class MockStruct:
+  def __init__(self, **kwargs):
+    for k, v in kwargs.items():
+      setattr(self, k, v)
+
+
+def test_recovery_power_scaling():
+  state = MockStruct(
+    PLANPLUS_CONTROL=1.0,
+    LONG_SMOOTH_SECONDS=0.3,
+    LAT_SMOOTH_SECONDS=0.1,
+    MIN_LAT_CONTROL_SPEED=0.3,
+    mlsim=True,
+    generation=12,
+    constants=MockStruct(T_IDXS=np.arange(100), DESIRE_LEN=8)
+  )
+  prev_action = log.ModelDataV2.Action()
+  recorded_vel: list = []
+
+  def mock_accel(plan_vel, plan_accel, t_idxs, action_t=0.0):
+    recorded_vel.append(plan_vel.copy())
+    return 0.0, False
+
+  modeld.get_accel_from_plan = mock_accel
+  modeld.get_curvature_from_output = lambda *args: 0.0
+  plan = np.random.rand(1, 100, 15).astype(np.float32)
+  planplus = np.random.rand(1, 100, 15).astype(np.float32)
+
+  model_output: dict = {
+    'plan': plan.copy(),
+    'planplus': planplus.copy()
+  }
+
+  test_cases: list = [
+    # (control, v_ego, expected_factor)
+    (0.55, 20.0, 1.0),
+    (1.0, 25.0, .75),
+    (1.5, 25.1, 0.75),
+    (2.0, 20.0, 1.0),
+    (0.75, 19.0, 1.0),
+    (0.8, 25.1, 0.75),
+  ]
+
+  for control, v_ego, factor in test_cases:
+    state.PLANPLUS_CONTROL = control
+    recorded_vel.clear()
+    ModelState.get_action_from_model(state, model_output, prev_action, 0.0, 0.0, v_ego)
+
+    expected_recovery_power = control * factor
+    expected_plan_vel = plan[0, :, Plan.VELOCITY][:, 0] + expected_recovery_power * planplus[0, :, Plan.VELOCITY][:, 0]
+
+    np.testing.assert_allclose(recorded_vel[0], expected_plan_vel, rtol=1e-5, atol=1e-6)

sunnypilot/sunnylink/params_metadata.json

@@ -826,6 +826,13 @@
     "title": "Panda Som Reset Triggered",
     "description": ""
   },
+  "PlanplusControl": {
+    "title": "Plan Plus Controls",
+    "description": "Adjust planplus model recentering strength. The higher this number the more aggressively the model will recover to lanecenter, too high and it will ping-pong",
+    "min": 0.0,
+    "max": 2.0,
+    "step": 0.1
+  },
   "PrimeType": {
     "title": "Prime Type",
     "description": ""