Ford: blend geometry assistance into the action reference

2026-09-17 10:33:44 +08:00 · 2026-09-16 12:59:09 -04:00
parent 01f5d54292
commit 9d34f77c86
21 changed files with 1129 additions and 32 deletions
@@ -1,5 +1,8 @@
 # Direct model-path C0/C1 trial

+Historical trial at `01f5d5429`. The existing geometry toggle now selects the
+[geometry-assisted action reference](ford_geometry_action_hybrid_offline.md).
+
 This trial takes priority over the untested filtered-driver change. Driver
 arbitration is restored to the last driven baseline, `18ded0380`: a raw torque
 crossing above 1 Nm, filtered driver input, or fresh PSCM driver override still
@@ -0,0 +1,162 @@
+# Geometry-assisted action reference: offline experiment
+
+Status: the revised selector is now wired behind `FordGeometryReference`, labelled
+**Model Geometry Assist (Experimental)** in Sunnylink. This supersedes the direct
+path trial at `01f5d5429`. The initial offline experiment below preceded wiring.
+The user authorized deployment after reviewing the numeric comparison.
+
+The requested behavior is geometry's earlier/stronger turn request, with the
+original action's ordinary driving and release behavior. This experiment uses
+the earlier smoothed orientation-derived curvature as geometry, not the newest
+direct-path C0/C1 mapping. That direct mapping is a separate baseline.
+
+## Proposed selection rule
+
+`openpilot/selfdrive/controls/lib/ford_geometry_action.py` returns a single curvature reference.
+Both C0 and C1 then use the existing scalar controller, including measured-wheel
+feedback. No P/I gains, driver gates, upstream curvature limits, or CAN field
+bounds change.
+
+For same-sign action `A` and geometry `G`, with `|G| > |A|`:
+
+```
+H = A + smoothstep((|G| - start) / (full - start))
+        * (|A| / max(start, action_peak))**power * (G - A)
+```
+
+The smoothstep argument is clamped to [0, 1]. Otherwise the result is `A`.
+Defaults: `start=0.010/m`, `full=0.020/m`, `power=2`.
+These are experimental tuning choices, not Ford specifications. This is not a
+claim of a gainless controller: the existing feedback gains remain, and the
+reference selector introduces a curvature band and release exponent.
+
+`action_peak` is the largest same-direction action magnitude in the current
+geometry excursion. It resets when inactive, geometry becomes invalid, geometry
+changes direction, or geometry drops below the start threshold. Repeated control
+cycles using the same model frame do not accumulate extra assistance.
+
+The denominator's `start` floor is essential. The first version normalized by
+`action_peak` alone. On route 15b, geometry briefly fell below its gate and then
+rebounded. A tiny action rebound rearmed full geometry: at 46.73 s, action's 3.2°
+wheel-equivalent reference became 103°. The revised version produces 4.3°, with
+C1 -0.0095 rad instead of -0.1805 rad. A minimized regression test failed in both
+directions before the fix and passed afterward.
+
+## Replay scope and results
+
+Five routes: 149, 151, 157, 15a, 15b. 578,741 unique control cycles, approximately
+55.7 minutes of active valid input. The 86 existing marked windows overlap;
+they are not 86 independent turns or statistically independent observations.
+
+Compare original action, earlier geometry, newest direct path, stronger
+same-direction source, 50/50 blend, and the proposed hybrid. All use identical
+recorded motion, driver input and PSCM status. Feedback is recalculated at native
+control cadence; new vehicle motion is not simulated.
+
+- On 15b's large left, hybrid/geometry/action peak references are 359°/361°/218°.
+  Hybrid C0 peaks at 4.33 m versus geometry's 4.41 m and action's 1.27 m.
+  Hybrid and geometry both reach C1's 0.50 rad field bound.
+- The hybrid reference crosses below 25° at the action's time, 0.85 s before
+  geometry. At approximately 173 s, hybrid and action C0/C1 are both
+  -0.26 m / -0.0745 rad. These are commands, not measured unwind times.
+- Across 76 eligible marked entry windows, median retained geometry extra demand
+  is 91%, with a 10th percentile of 51%. Eligibility includes same-direction
+  samples before the marked action peak, geometry above 50° wheel-equivalent and
+  more than 5° stronger than action. It excludes conflicting-direction preview.
+- In 83 windows with paired sustained release crossings, median added reference
+  delay versus action is 0 s; maximum is 0.102 s. Route 157 event 5 disengages
+  before a sustained hybrid release can be observed. First threshold crossings
+  do not rule out later rebounds; final crossings and complete traces were also
+  inspected. Some windows include another turn or driver intervention.
+- For gentle raw sources (both below 0.010/m), the selector returns action.
+  Limiter and integrator history can still differ. Pooled 95th-percentile
+  C0/C1 differences are zero, but worst C1 difference is 0.0755 rad; on 15a the
+  C1 95th-percentile difference is 0.002 rad.
+
+## Limits of the design
+
+1. A temporary action dip looks like an exit to the selector. In route 151,
+   before the action later recovers, action/geometry/hybrid are approximately
+   26°/122°/30°. This rule cannot prove that reducing assistance there is correct.
+2. Conflicting-direction preview is discarded. On 15b event 4, the hybrid retains
+   only about 5% of eligible extra geometry demand before the marked action peak.
+3. Large geometry requests can be inherited, including approximately 807° on an
+   older route. Similar command numbers do not establish correct path following.
+4. Rapid low-speed commands remain. Across these routes, at below 15 mph,
+   hybrid has 1,054 adjacent C0 changes over 0.25 m and 583 C1 changes over
+   0.05 rad, versus geometry's 972/558 and action's 658/478. These include recorded
+   driver-gate transitions and are not a physical comfort measurement.
+5. The selector has peak memory. Closely spaced same-direction turns can inherit
+   the earlier action peak until geometry crosses the reset condition.
+
+This is a promising numeric division of work, not evidence of improved physical
+tracking. No PSCM model, neural-model rerun, or camera replay was used. The recent
+routes log action and geometry separately and join by exact model timestamp.
+Older geometry is reconstructed with recorded delay/model smoothing; speed is
+approximated by the first control sample consuming the model frame.
+
+## Validation and artifacts
+
+Production integration carries the geometry reference inside the same `modelV2`
+message as the original action. Neither `modelV2.action` nor
+`drivingModelData.action` is overwritten. controlsd selects the hybrid at control
+cadence before the existing curvature limiter; desired curvature/steering angle,
+C0/C1 and measured-wheel feedback all use that combined reference. Missing,
+invalid or mismatched geometry falls back to action and clears selector memory.
+Inactive/invalid controller inputs and lateral-maneuver priority also clear it.
+Diagnostics identify `geometry-assisted-action-feedback-v18` and include geometry
+assistance weight and the stored action peak. Geometry telemetry's selected field
+now describes the smoothed geometry source, not the final controlsd target.
+
+Keep **Selected-Action Path Tracking** and **Model Geometry Assist** enabled for
+the hybrid. The stored geometry key is unchanged, so an enabled direct-path trial
+becomes this hybrid after updating and restarting. Geometry assist off selects
+action-only; the main controller toggle off selects upstream Ford control even
+if the geometry key remains enabled. Settings apply after an offroad/onroad cycle.
+
+The production adapter was replayed over all five routes again. Every value in
+every output column exactly matches the approved offline candidate, including
+reference, C0/C1, correction state and validity. See
+`ford_geometry_action_hybrid_validation.json` for counts, source hashes and
+the equality check. Real serialized messages are additionally tested through
+the actual controlsd selection/limiter/feedback branch and real CAN packing.
+
+- 20 prototype tests, including finite inputs/configuration, repeated frames,
+  direction changes, causality and the real exit-rebound regression.
+- Six-variant replay checked finite outputs, field bounds, zero C2/C3 and driver
+  feedback suppression; real CAN packing/unpacking every tenth control cycle
+  supplied 347,256 checks. The revised hybrid added 57,876 checks.
+- Unchanged baseline columns can be reused when iterating the selector. A complete
+  fresh revised 15a run exactly matched every output in the cached-baseline run.
+- Twelve reference settings on each recent route: start 0.006/0.010/0.015 per m,
+  full twice start, release power 1/2/4/8. These alternatives check references
+  only; the selected configuration also has complete C0/C1 replay. Larger powers
+  sacrifice more entry demand and can increase command discontinuities.
+
+Outputs are under `.cache/ford_hybrid` (first version),
+`.cache/ford_hybrid_guarded` (revised), and `.cache/ford_hybrid_verify/15a`
+(fresh replay equivalence check). Per-route JSON records runtime source hashes;
+older first-version hashes intentionally differ from the revised source. HTML
+and six plots are in the existing report server directory, at
+`http://127.0.0.1:52447/geometry-action-hybrid.html`.
+
+Reproduce using the configured Python environment and existing cached route data:
+
+```sh
+PYTHONPATH=.:opendbc_repo python -m pytest -q -p no:cacheprovider \
+  openpilot/selfdrive/controls/tests/test_ford_geometry_action.py
+
+PYTHONPATH=.:opendbc_repo python tools/ford_pscm_lab/hybrid_reference_replay.py \
+  --route 15a --output .cache/ford_hybrid_fresh
+
+PYTHONPATH=.:opendbc_repo python tools/ford_pscm_lab/hybrid_reference_sweep.py \
+  --route 15a --output .cache/ford_hybrid_fresh
+
+PYTHONPATH=.:opendbc_repo python tools/ford_pscm_lab/hybrid_reference_report.py \
+  --root .cache/ford_hybrid_guarded --initial-root .cache/ford_hybrid \
+  --output /path/to/report/directory
+```
+
+Repeat full replay for the other route labels and sensitivity for 15b. The report
+includes both favorable and unfavorable examples; it does not tune to a single
+turn or claim the replayed feedback trajectory would survive changed actuation.
@@ -0,0 +1,118 @@
+{
+  "baseline_commit": "01f5d54292056a64aafa7750f30d2a7871304d04",
+  "hypothesis": "geometry-assisted-action-feedback-v18",
+  "settings": {
+    "controller": "FordModelActionController",
+    "geometry_assist": "FordGeometryReference",
+    "start_curvature": 0.01,
+    "full_curvature": 0.02,
+    "release_power": 2.0
+  },
+  "regression_tests_passed": 678,
+  "regression_subtests_passed": 2,
+  "hybrid_unit_and_integration_tests": 35,
+  "ruff": "passed",
+  "sunnylink_schema_compile_check": "passed",
+  "validation_scope": "Frozen-motion replay and code/message/CAN tests; physical steering response is not simulated.",
+  "routes": {
+    "149": {
+      "cycles": 132334,
+      "wire_checks": 13234,
+      "every_output_column_exactly_matches_offline_candidate": true,
+      "active_valid_seconds": 1247.577060015,
+      "geometry_source": "Reconstructed geometry with recorded delay/model smoothing; speed approximated by first consuming control sample",
+      "input_and_runtime_sha256": {
+        ".cache/ford_route149/full/route.npz": "aa5902877343cd033ee286b3668d91a85336ffbf740861849fb0b76b0ca24ade",
+        ".cache/ford_route149/full/metadata.json": "624fff03c25eb298661cb7b25f3dbe6d214d863f799d93635c0f4f05fc0d2b32",
+        ".cache/ford_route149/intake.npz": "503919c4f1566ef850007c59f16ce074697ba57732f21beb7b158c1ca7be3a9d",
+        ".cache/ford_route149/intake.json": "80f8c4ba69b4259ed308e0db110123063c6917357d26b49c9b6585695c9b24ae",
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/tools/ford_pscm_lab/hybrid_reference_replay.py": "068c746a1a6c5642a3430d6efb44b43d1ae6951cc4ad0e65721f1275dae31add",
+        "openpilot/selfdrive/controls/lib/ford_geometry_action.py": "f92ee4a952049be980229bbb2b4fb3f6d61d8d5876c6df8e7bace418b1fac80f",
+        "openpilot/selfdrive/controls/lib/ford_model_action.py": "5d90f819c03b109877bb8aa7ed44db30083518da24e61d9612d13540e7ace9ad",
+        ".cache/ford_hybrid_guarded/149/commands.npz": "bf40a870d33edbc0dec0d90206b7f082fee3a54f807bd61dd87e56102a571ba8",
+        ".cache/ford_hybrid_guarded/149/report.json": "963ed7e2c7ffe647ad5ea7d9c4530df54fa6f555423532efc89710aa20cc054a"
+      }
+    },
+    "151": {
+      "cycles": 325708,
+      "wire_checks": 32571,
+      "every_output_column_exactly_matches_offline_candidate": true,
+      "active_valid_seconds": 1355.493226190003,
+      "geometry_source": "Reconstructed geometry with recorded delay/model smoothing; speed approximated by first consuming control sample",
+      "input_and_runtime_sha256": {
+        ".cache/ford_route151/full/route.npz": "41a5b8bd388cf5a3d553f784542376ac9355fcdc5be4f427053d0504537babe1",
+        ".cache/ford_route151/full/metadata.json": "937825317a0edd470c54647240b922be8f79dda5b3365ffdd61281f0aca877a1",
+        ".cache/ford_route151/intake.npz": "18bbefb7738cebd0071dc987e90f74469a0c8ed456f9412324030592cafd68c9",
+        ".cache/ford_route151/intake.json": "0cfe327b5889f2603d902d2a01e915bf9a27b7a312137111d892a5e8540e03aa",
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/tools/ford_pscm_lab/hybrid_reference_replay.py": "068c746a1a6c5642a3430d6efb44b43d1ae6951cc4ad0e65721f1275dae31add",
+        "openpilot/selfdrive/controls/lib/ford_geometry_action.py": "f92ee4a952049be980229bbb2b4fb3f6d61d8d5876c6df8e7bace418b1fac80f",
+        "openpilot/selfdrive/controls/lib/ford_model_action.py": "5d90f819c03b109877bb8aa7ed44db30083518da24e61d9612d13540e7ace9ad",
+        ".cache/ford_hybrid_guarded/151/commands.npz": "b01843e65718f51695a0e7642bb90c894a74e576e15e3e28b9101528b9052953",
+        ".cache/ford_hybrid_guarded/151/report.json": "8849332e600372ef405d098cb0f4eeeb4dc87100041af3ecddfe0d24b88ebd20"
+      }
+    },
+    "157": {
+      "cycles": 76554,
+      "wire_checks": 7656,
+      "every_output_column_exactly_matches_offline_candidate": true,
+      "active_valid_seconds": 527.940320743,
+      "geometry_source": "Reconstructed geometry with recorded delay/model smoothing; speed approximated by first consuming control sample",
+      "input_and_runtime_sha256": {
+        ".cache/ford_route157/full/route.npz": "e2e2573f904aa11ee9e10450e7f5b965d475657b61127e827a67eadcd6b857fb",
+        ".cache/ford_route157/full/metadata.json": "f2d72c9a877ed6694e4da6831841113dc0c05987e2c76cca51f358d12411b235",
+        ".cache/ford_route157/intake.npz": "5dffd86fde68197727d6c6b9c5eae82320a456fa1206c4cf18e5e9c69419dde4",
+        ".cache/ford_route157/intake.json": "1d82c7f87d42229b96771bce3dd43c6b1651aa4fb8bb0ca3b1886d5a1a1c1a3c",
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/tools/ford_pscm_lab/hybrid_reference_replay.py": "068c746a1a6c5642a3430d6efb44b43d1ae6951cc4ad0e65721f1275dae31add",
+        "openpilot/selfdrive/controls/lib/ford_geometry_action.py": "f92ee4a952049be980229bbb2b4fb3f6d61d8d5876c6df8e7bace418b1fac80f",
+        "openpilot/selfdrive/controls/lib/ford_model_action.py": "5d90f819c03b109877bb8aa7ed44db30083518da24e61d9612d13540e7ace9ad",
+        ".cache/ford_hybrid_guarded/157/commands.npz": "49f02f7471d6fbb548f7f46a5615153a4079c2d29bc33cedf63987089eb9fb4b",
+        ".cache/ford_hybrid_guarded/157/report.json": "5aca689ca41bc98796f081f211212799d5a58ca90af0f94232fcc841d8af9e43"
+      }
+    },
+    "15a": {
+      "cycles": 21867,
+      "wire_checks": 2187,
+      "every_output_column_exactly_matches_offline_candidate": true,
+      "active_valid_seconds": 65.37018141000004,
+      "geometry_source": "Logged original action and selected geometry; exact model timestamp joins",
+      "input_and_runtime_sha256": {
+        ".cache/ford_route15a/rlog_full/route.npz": "057ce69ce028b2c9c1b42ab8110f35c888a6c398ddcc4156e74afc98c83d119d",
+        ".cache/ford_route15a/rlog_full/metadata.json": "bea47c7f0c0b583964ced3484fe814ec9a7a24abbfe6272e70f9dde12580a756",
+        ".cache/ford_route15a/rlog_full/model_paths.npz": "ef820d9df14afa308bf46f95463b636a599ff24fd08bffaff1ae671b3932c24a",
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/tools/ford_pscm_lab/hybrid_reference_replay.py": "068c746a1a6c5642a3430d6efb44b43d1ae6951cc4ad0e65721f1275dae31add",
+        "openpilot/selfdrive/controls/lib/ford_geometry_action.py": "f92ee4a952049be980229bbb2b4fb3f6d61d8d5876c6df8e7bace418b1fac80f",
+        "openpilot/selfdrive/controls/lib/ford_model_action.py": "5d90f819c03b109877bb8aa7ed44db30083518da24e61d9612d13540e7ace9ad",
+        ".cache/ford_hybrid_guarded/15a/commands.npz": "b2648c067840336ddf49ec732459a9325e7d02e15215b514438364df6733c0c7",
+        ".cache/ford_hybrid_guarded/15a/report.json": "9ac33ad43dc8e395402a89ef0fcee44875428e4e7e39c7cc31c4e3d8e8b1153f"
+      }
+    },
+    "15b": {
+      "cycles": 22278,
+      "wire_checks": 2228,
+      "every_output_column_exactly_matches_offline_candidate": true,
+      "active_valid_seconds": 148.29076710100003,
+      "geometry_source": "Logged original action and selected geometry; exact model timestamp joins",
+      "input_and_runtime_sha256": {
+        ".cache/ford_route15b/rlog_full/route.npz": "f95e1c16a4677fee0ab57002d714e22716bf11873c308280553312fa9d18aab0",
+        ".cache/ford_route15b/rlog_full/metadata.json": "545ef8d6740e57237f35b2ca684111fc2d774e83aa1d6c8a5a0fff1b71270e85",
+        ".cache/ford_route15b/rlog_full/model_paths.npz": "a90be55943ce0009afbf38ef2035b3fff4de8ab00ea266636247bcc5a5012eae",
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/tools/ford_pscm_lab/hybrid_reference_replay.py": "068c746a1a6c5642a3430d6efb44b43d1ae6951cc4ad0e65721f1275dae31add",
+        "openpilot/selfdrive/controls/lib/ford_geometry_action.py": "f92ee4a952049be980229bbb2b4fb3f6d61d8d5876c6df8e7bace418b1fac80f",
+        "openpilot/selfdrive/controls/lib/ford_model_action.py": "5d90f819c03b109877bb8aa7ed44db30083518da24e61d9612d13540e7ace9ad",
+        ".cache/ford_hybrid_guarded/15b/commands.npz": "11c35c5c35680d60471aeb50c14479226d7a5494c9edb509e8bfb790509d44cf",
+        ".cache/ford_hybrid_guarded/15b/report.json": "245fb9d5ae00391d3c5a0a0acbbc01726b02fe33cea5ed6196911b660ca36b33"
+      }
+    }
+  },
+  "source_sha256": {
+    "openpilot/cereal/log.capnp": "836fe271f91ce39603889bcbca37439ca62c62271a7ab91579a4a9dd4ccb3b65",
+    "openpilot/cereal/custom.capnp": "854840fae9bb3b7dda26a3532004ffc3450235dab7cdb2e735cbeeb41d0bf728",
+    "openpilot/selfdrive/controls/controlsd.py": "d54d5838499a7f151f8475a2ea45dc7fd624fba69d49fdf6edc9525a3ee9bd12",
+    "openpilot/selfdrive/controls/lib/ford_geometry_action.py": "f92ee4a952049be980229bbb2b4fb3f6d61d8d5876c6df8e7bace418b1fac80f",
+    "openpilot/selfdrive/controls/lib/ford_model_action.py": "5d90f819c03b109877bb8aa7ed44db30083518da24e61d9612d13540e7ace9ad",
+    "openpilot/sunnypilot/modeld_v2/ford_geometry.py": "55d144886dfa7cb8c300f7cbfed435d9bd0072d95dcfa295107b227ef63702b9",
+    "openpilot/selfdrive/controls/tests/test_ford_geometry_action.py": "1454e068faf7a603e6cbb61d07d85a02f1022e7f384e2a468707090f004d3698"
+  },
+  "unique_control_cycles": 578741,
+  "production_adapter_wire_checks": 57876
+}
@@ -1,5 +1,8 @@
 # Ford model geometry reference trial

+Current behavior: [geometry-assisted action reference](ford_geometry_action_hybrid_offline.md).
+The descriptions below document the earlier geometry-only trial.
+
 This documents the initial curvature-reference trial at `18ded0380`. The
 subsequent [direct model-path trial](ford_direct_path.md) reuses the geometry
 toggle and changes controlsd's reference and C0/C1 mapping. modeld telemetry
@@ -490,7 +490,7 @@ struct ModelDataV2SP @0xa1680744031fdb2d {
    modelMonoTime @2 :UInt64;
    actionDesiredCurvature @3 :Float32;  # Original model action, with its own unchanged smoothing history.
    rawCurvature @4 :Float32;
-    selectedCurvature @5 :Float32;  # Published modelV2.action, before controlsd's normal limits/maneuver override.
+    selectedCurvature @5 :Float32;  # Smoothed geometry; controlsd combines it with the original action before normal limits.
    previewSeconds @6 :Float32;
    smoothSeconds @7 :Float32;
  }
@@ -1076,6 +1076,8 @@ struct ModelDataV2 {

  # e2e lateral planner
  action @26: Action;
+  # Atomic action/geometry pair for the opt-in Ford reference selector.
+  fordGeometryReference @28 :Custom.ModelDataV2SP.FordGeometryReference;

  lateralPlannerSolutionDEPRECATED @25: Deprecated.LateralPlannerSolution;
  leadsDEPRECATED @11 :List(LeadDataV2DEPRECATED);
@@ -57,7 +57,7 @@ class Controls(ControlsExt):
    self.desired_curvature = 0.0
    self.ford_path_controller = select_model_action_controller(self.CP, self.params.get_bool("FordModelActionController"),
                                                              c0_time_based=self.params.get_bool("FordC0TimeBased"),
-                                                              direct_path=self.params.get_bool("FordGeometryReference"))
+                                                              geometry_assist=self.params.get_bool("FordGeometryReference"))
    self.ford_model_action = isinstance(self.ford_path_controller, FordModelActionController)
    if self.CP.brand == "ford":
      cloudlog.event("Ford path controller selected",
@@ -151,9 +151,14 @@ class Controls(ControlsExt):
    # Steering PID loop and lateral MPC
    # Reset desired curvature to current to avoid violating the limits on engage
    if self.sm.valid['lateralManeuverPlan']:
+      if self.ford_model_action and self.ford_path_controller.geometry_assist is not None:
+        self.ford_path_controller.geometry_assist.reset()
      new_desired_curvature = self.sm['lateralManeuverPlan'].desiredCurvature if CC.latActive else self.curvature
-    elif self.ford_model_action and self.ford_path_controller.direct_path:
-      new_desired_curvature = self.ford_path_controller.path_curvature(model_v2, CS.vEgo) if CC.latActive else self.curvature
+    elif self.ford_model_action and self.ford_path_controller.geometry_assist is not None:
+      reference = self.ford_path_controller.select_reference(
+        model_v2, model_mono_time=self.sm.logMonoTime['modelV2'], active=CC.latActive,
+        valid=CS.canValid and self.sm.all_checks(['carState', 'vehicleParameters', 'modelV2']))
+      new_desired_curvature = reference if CC.latActive else self.curvature
    else:
      new_desired_curvature = model_v2.action.desiredCurvature if CC.latActive else self.curvature
    self.desired_curvature, curvature_limited = clip_curvature(CS.vEgo, self.desired_curvature, new_desired_curvature, lp.roll)
@@ -0,0 +1,41 @@
+"""Opt-in action baseline with geometry assistance for stronger turn requests.
+
+The curvature band and release exponent are experimental choices. No PSCM
+dynamics or physical tracking are modeled by this reference selector.
+"""
+import math
+
+
+class GeometryActionHybrid:
+  def __init__(self, start_curvature=.01, full_curvature=.02, release_power=2.):
+    if not all(math.isfinite(x) for x in (start_curvature, full_curvature, release_power)) or not 0. < start_curvature < full_curvature or release_power <= 0:
+      raise ValueError('Expected positive curvature band and release exponent')
+    self.start, self.full, self.release_power = start_curvature, full_curvature, release_power
+    self.reset()
+
+  def reset(self):
+    self.direction = self.action_peak = self.weight = 0.
+
+  def update(self, action, geometry, *, active=True):
+    if not math.isfinite(action):
+      self.reset()
+      return 0.
+    if not active or not math.isfinite(geometry) or abs(geometry) <= self.start:
+      self.reset()
+      return action
+    direction = math.copysign(1., geometry)
+    if direction != self.direction:
+      self.reset()
+      self.direction = direction
+    self.weight = 0.
+    if action*geometry <= 0:
+      return action
+    self.action_peak = max(self.action_peak, abs(action))
+    if abs(geometry) <= abs(action):
+      return action
+    progress = min((abs(geometry)-self.start)/(self.full-self.start), 1.)
+    turn_weight = progress*progress*(3.-2.*progress)
+    # A tiny action rebound after a geometry reset must not authorize full boost.
+    release_weight = (abs(action)/max(self.start, self.action_peak))**self.release_power
+    self.weight = turn_weight*release_weight
+    return action+self.weight*(geometry-action)
@@ -15,6 +15,7 @@ import numpy as np

 from opendbc.car.ford.values import CarControllerParams, FordFlags
 from openpilot.selfdrive.controls.lib.drive_helpers import clip_curvature
+from openpilot.selfdrive.controls.lib.ford_geometry_action import GeometryActionHybrid
 from openpilot.selfdrive.controls.lib.ford_path import FordPath, _model_path


@@ -184,13 +185,28 @@ class FordModelActionController:
  neither a limit nor a repeated measurement freezes the model request.
  """
  def __init__(self, proportional_gain=C1_PROPORTIONAL_GAIN, integral_gain=C1_INTEGRAL_GAIN, *, c0_time_based=False,
-               c0_proportional_gain=C0_PROPORTIONAL_GAIN, direct_path=False):
+               c0_proportional_gain=C0_PROPORTIONAL_GAIN, direct_path=False, geometry_assist=False):
    self.core = ModelActionController(proportional_gain=proportional_gain, integral_gain=integral_gain, c0_time_based=c0_time_based,
                                      c0_proportional_gain=c0_proportional_gain)
    self.direct_path = bool(direct_path)
+    self.geometry_assist = GeometryActionHybrid() if geometry_assist else None
    self.hypothesis = 'model-path-direct-feedback-v17' if self.direct_path else 'model-action-curvature-c0-feedback-v15'
+    if self.geometry_assist is not None:
+      if self.direct_path:
+        raise ValueError('Geometry assistance uses the scalar C0/C1 mapper')
+      self.hypothesis = 'geometry-assisted-action-feedback-v18'
    self.reset()

+  def select_reference(self, model, *, model_mono_time, active, valid):
+    """Combine only an atomic, matching geometry/action pair; missing geometry uses action."""
+    action = float(model.action.desiredCurvature)
+    if self.geometry_assist is None:
+      return action
+    ref = getattr(model, 'fordGeometryReference', None)
+    matched = ref is not None and ref.enabled and ref.valid and ref.modelMonoTime == model_mono_time
+    geometry = float(ref.selectedCurvature) if matched else math.nan
+    return self.geometry_assist.update(action, geometry, active=active and valid)
+
  def path_curvature(self, model, speed):
    """Heading-equivalent feedback target; controlsd limits and logs this value."""
    target = encode_model_path(model, speed, c0_time_based=self.core.c0_time_based)
@@ -206,6 +222,8 @@ class FordModelActionController:

  def reset(self, status='inactive'):
    self.core.reset()
+    if self.geometry_assist is not None:
+      self.geometry_assist.reset()
    self.last_time = self.last_measurement_time = self.last_model_time = None
    self.diagnostics = {'status': status, 'hypothesis': self.hypothesis,
                        'c0_time_based': self.core.c0_time_based,
@@ -271,13 +289,15 @@ class FordModelActionController:
                        'feedback_error': self.core.feedback_curvature-current_curvature,
                        'driver_override': driver_override, 'pscm_limited': pscm_limited, 'pscm_status_fresh': bool(status_fresh),
                        'command': (command.path_offset, command.path_angle, 0., 0.)}
+    if self.geometry_assist is not None:
+      self.diagnostics.update(geometry_assistance=self.geometry_assist.weight, action_peak=self.geometry_assist.action_peak)
    return command


-def select_model_action_controller(CP, enabled, *, c0_time_based=False, direct_path=False):
+def select_model_action_controller(CP, enabled, *, c0_time_based=False, direct_path=False, geometry_assist=False):
  """Only opt-in Ford CAN FD vehicles override upstream curvature control."""
  compatible = CP.brand == 'ford' and CP.flags & FordFlags.CANFD
  if enabled and compatible:
    return FordModelActionController(proportional_gain=C1_PROPORTIONAL_GAIN, integral_gain=C1_INTEGRAL_GAIN,
-                                     c0_time_based=c0_time_based, direct_path=direct_path)
+                                     c0_time_based=c0_time_based, direct_path=direct_path, geometry_assist=geometry_assist)
  return None
@@ -53,7 +53,7 @@ class TestFordControlsLogging(unittest.TestCase):
      controls = SimpleNamespace(ford_path_controller=controller, desired_curvature=.03, curvature=.015,
                                 sm=SimpleNamespace(logMonoTime={'modelV2': 123456789, 'carState': 123450000}))
      record = self.emit_controls_event('Ford C2-free path tracking', controls)
-      self.assertEqual(record['hypothesis'], 'model-action-curvature-c0-distance-pi-v14')
+      self.assertEqual(record['hypothesis'], 'model-action-curvature-c0-feedback-v15')
      self.assertIs(record['calibration_approved'], False)
      self.assertEqual(record['command'][2:], [0., 0.])
      self.assertEqual(record['status'], controller.diagnostics['status'])
@@ -107,22 +107,22 @@ def test_feedback_tracks_path_heading_and_raw_torque_override_is_still_the_drive

@pytest.mark.parametrize('geometry', [False, True])
@pytest.mark.parametrize('maneuver', [False, True])
-def test_actual_controlsd_path_selection_feedback_logging_publication_and_can(pipeline, geometry, maneuver):  # noqa: F811
+def test_missing_geometry_reference_falls_back_to_action_through_controlsd_and_can(pipeline, geometry, maneuver):  # noqa: F811
  settings = {'FordModelActionController': True, 'FordGeometryReference': geometry}
  controls = startup(params=SimpleNamespace(get_bool=lambda key: settings.get(key, False)))
  controls.sm, controls.desired_curvature, controls.curvature = Subscriptions(maneuver), 0., 0.
-  assert controls.ford_path_controller.direct_path == geometry
+  assert not controls.ford_path_controller.direct_path
+  assert (controls.ford_path_controller.geometry_assist is not None) == geometry
  model = circle(.01)
  model.action = SimpleNamespace(desiredCurvature=-.03)  # Deliberately opposite to the model path.
  cc = structs.CarControl(latActive=True)
  cs = SimpleNamespace(vEgo=5., yawRate=0., canValid=True, steeringPressed=False, steeringTorque=0.)
  exec(pipeline[0], {'self': controls, 'CS': cs, 'CC': cc, 'actuators': cc.actuators, 'model_v2': model,
                    'lp': SimpleNamespace(roll=0.), 'clip_curvature': clip_curvature, 'time': SimpleNamespace(monotonic=lambda: 1.)})
-  direct = geometry and not maneuver
-  expected = .002 if direct else -.002
+  expected = -.002
  assert controls.desired_curvature == pytest.approx(expected)
  assert controls.ford_path_controller.core.feedback_curvature == controls.desired_curvature
-  assert controls.ford_path_controller.diagnostics['direct_path'] == direct
+  assert not controls.ford_path_controller.diagnostics['direct_path']
  assert controls.ford_path.path_angle*expected > 0.
  msg = custom.CarControlSP.new_message()
  exec(pipeline[1], {'self': controls, 'CC_SP': msg})
@@ -0,0 +1,185 @@
+import math
+from types import SimpleNamespace
+
+import pytest
+
+from openpilot.selfdrive.controls.lib.ford_geometry_action import GeometryActionHybrid
+from openpilot.selfdrive.controls.lib.ford_model_action import FordModelActionController
+from openpilot.selfdrive.controls.tests.test_ford_model_action_adapter import Subscriptions, pipeline  # noqa: F401
+from openpilot.selfdrive.controls.tests.test_ford_model_action_selection import startup
+
+
+@pytest.mark.parametrize('sign', [-1., 1.])
+def test_build_and_hold_keep_geometry_strength_then_release_follows_action(sign):
+  h = GeometryActionHybrid()
+  assert h.update(sign*.005, sign*.04) == pytest.approx(sign*(.005+.25*.035))
+  for action in [.01, .02]+[.02]*100:
+    assert h.update(sign*action, sign*.04) == pytest.approx(sign*.04)
+  # Half the action peak leaves a quarter of the geometry excess.
+  assert h.update(sign*.01, sign*.04) == pytest.approx(sign*(.01+.25*.03))
+  assert h.update(0., sign*.04) == 0.
+  assert h.update(-sign*.005, sign*.04) == pytest.approx(-sign*.005)
+
+
+def test_small_action_dip_does_not_switch_all_assistance_off():
+  h = GeometryActionHybrid()
+  h.update(.02, .04)
+  assert .035 < h.update(.019, .04) < .04
+  assert h.update(.02, .04) == pytest.approx(.04)
+
+
+@pytest.mark.parametrize('sign', [-1., 1.])
+def test_tiny_action_cannot_restart_full_assistance_after_geometry_dips(sign):
+  h = GeometryActionHybrid()
+  h.update(sign*.03, sign*.04)
+  h.update(-sign*.001, sign*.003)
+  # Route 15b's exit: geometry rebounds after falling below the turn band.
+  # A tiny same-direction action must not authorize the entire geometry request.
+  assert abs(h.update(sign*.0008, sign*.025)) < .0012
+
+
+@pytest.mark.parametrize('action,geometry', [(0., .04), (-.01, .04), (.02, .01), (.003, .006), (-.003, -.009)])
+def test_conflicting_weaker_and_gentle_geometry_preserve_action(action, geometry):
+  assert GeometryActionHybrid().update(action, geometry) == action
+
+
+def test_exit_cannot_rearm_from_small_action_rebound_until_the_geometry_excursion_ends():
+  h = GeometryActionHybrid()
+  h.update(.03, .04)
+  h.update(0., .04)
+  assert h.update(.003, .04) < .004
+  h.update(0., 0.)
+  assert h.update(.003, .04) == pytest.approx(.003+.09*.037)
+
+
+def test_repeated_model_frames_are_idempotent_and_inactive_invalid_inputs_reset():
+  h = GeometryActionHybrid()
+  h.update(.02, .04)
+  expected = h.update(.01, .04)
+  for _ in range(100):
+    assert h.update(.01, .04) == expected
+  assert h.update(.01, .04, active=False) == .01 and h.action_peak == 0.
+  assert h.update(.01, math.nan) == .01 and h.action_peak == 0.
+  assert h.update(math.nan, .04) == 0. and h.action_peak == 0.
+
+
+def test_sign_change_starts_a_fresh_excursion():
+  h = GeometryActionHybrid()
+  h.update(.02, .04)
+  assert h.update(-.003, -.04) == pytest.approx(-(.003+.09*.037))
+
+
+def test_causal_output_stays_between_sources_for_both_directions():
+  h = GeometryActionHybrid()
+  for i in range(10000):
+    action, geometry = .05*math.sin(i*.012), .08*math.sin(i*.008)
+    result = h.update(action, geometry)
+    assert min(action, geometry)-1e-12 <= result <= max(action, geometry)+1e-12
+    assert 0. <= h.weight <= 1.
+
+
+@pytest.mark.parametrize('values', [(0., .02, 2.), (.02, .01, 2.), (.01, .02, 0.), (.01, .02, math.nan), (math.nan, .02, 2.)])
+def test_invalid_configuration_is_rejected(values):
+  with pytest.raises(ValueError):
+    GeometryActionHybrid(*values)
+
+
+def test_future_predictions_cannot_change_earlier_outputs():
+  prefix = [(.003+i*.001, .04) for i in range(20)]
+  results = []
+  for future in [[(.005, .04)]*20, [(-.005, -.04)]*20]:
+    h = GeometryActionHybrid()
+    results.append([h.update(a, g) for a, g in prefix+future][:len(prefix)])
+  assert results[0] == results[1]
+
+
+@pytest.mark.parametrize('controller_enabled', [False, True])
+@pytest.mark.parametrize('geometry_enabled', [False, True])
+def test_actual_startup_selects_upstream_action_or_hybrid(controller_enabled, geometry_enabled):
+  settings = {'FordModelActionController': controller_enabled, 'FordGeometryReference': geometry_enabled}
+  controls = startup(params=SimpleNamespace(get_bool=lambda key: settings.get(key, False)))
+  if not controller_enabled:
+    assert controls.ford_path_controller is None
+    return
+  assert not controls.ford_path_controller.direct_path
+  assert (controls.ford_path_controller.geometry_assist is not None) == geometry_enabled
+  expected = 'geometry-assisted-action-feedback-v18' if geometry_enabled else 'model-action-curvature-c0-feedback-v15'
+  assert controls.ford_path_controller.hypothesis == expected
+
+
+@pytest.mark.parametrize('invalid', ['missing', 'disabled', 'invalid', 'older', 'future', 'nonfinite', 'inactive', 'service'])
+def test_adapter_rejects_missing_mismatched_or_invalid_geometry_and_clears_peak(invalid):
+  controller = FordModelActionController(geometry_assist=True)
+  ref = SimpleNamespace(enabled=True, valid=True, modelMonoTime=1_000_000_000, selectedCurvature=.04)
+  model = SimpleNamespace(action=SimpleNamespace(desiredCurvature=.02), fordGeometryReference=ref)
+  kwargs = {'model_mono_time': 1_000_000_000, 'active': True, 'valid': True}
+  assert controller.select_reference(model, **kwargs) == .04
+  model.action.desiredCurvature = .003
+  if invalid == 'missing':
+    del model.fordGeometryReference
+  elif invalid == 'disabled':
+    ref.enabled = False
+  elif invalid == 'invalid':
+    ref.valid = False
+  elif invalid in ('older', 'future'):
+    ref.modelMonoTime += -1 if invalid == 'older' else 1
+  elif invalid == 'nonfinite':
+    ref.selectedCurvature = math.nan
+  else:
+    kwargs['active' if invalid == 'inactive' else 'valid'] = False
+  assert controller.select_reference(model, **kwargs) == .003
+  assert controller.geometry_assist.action_peak == controller.geometry_assist.weight == 0.
+
+
+def test_incompatible_direct_and_hybrid_modes_are_rejected():
+  with pytest.raises(ValueError):
+    FordModelActionController(direct_path=True, geometry_assist=True)
+
+
+@pytest.mark.parametrize('maneuver', [False, True])
+def test_atomic_model_publication_through_controlsd_limiter_feedback_and_can(pipeline, maneuver):  # noqa: F811
+  from opendbc.car import structs
+  from openpilot.cereal import log
+  from openpilot.selfdrive.controls.lib.drive_helpers import clip_curvature
+  from openpilot.sunnypilot.modeld_v2.tests.test_ford_geometry import messages, apply
+  from openpilot.sunnypilot.modeld_v2.ford_geometry import FordGeometryReference
+  from tools.ford_pscm_lab.model_action_replay import WireCheck
+
+  controls = startup(params=SimpleNamespace(get_bool=lambda key: key in ('FordModelActionController', 'FordGeometryReference')))
+  controls.sm, controls.desired_curvature, controls.curvature = Subscriptions(maneuver), 0., 0.
+  expected_selector = GeometryActionHybrid()
+  baseline = FordModelActionController()
+  publisher = FordGeometryReference()
+  previous = 0.
+  wire = WireCheck()
+  # Build, hold, release, tiny rebound and direction change. Each model frame is
+  # consumed five times at control cadence, with a real Cap'n Proto round trip.
+  for i, action in enumerate([.015]*20+[.008, .004, .001, 0., .0008, -.005, -.015]):
+    m, d, sp, _ = messages()
+    now_model = .98+i*.05
+    m.logMonoTime = round(now_model*1e9)
+    m.modelV2.action.desiredCurvature = d.drivingModelData.action.desiredCurvature = action
+    apply(publisher, m, d, sp)
+    with log.Event.from_bytes(m.to_bytes()) as decoded:
+      model = decoded.modelV2
+      for j in range(5):
+        now = 1.+i*.05+j*.01
+        controls.sm.logMonoTime.update(modelV2=m.logMonoTime, carState=round(now*1e9), lateralManeuverPlan=round(now*1e9))
+        cc = structs.CarControl(latActive=True)
+        cs = SimpleNamespace(vEgo=5., yawRate=0., canValid=True, steeringPressed=False, steeringTorque=0.)
+        target = -.1 if maneuver else expected_selector.update(model.action.desiredCurvature, model.fordGeometryReference.selectedCurvature)
+        previous, _ = clip_curvature(5., previous, target, 0.)
+        expected = baseline.update(model, previous, current_curvature=0., speed=5., yaw_rate=0., now=now,
+                                   measurement_time=now, model_time=now_model, reference_time=now if maneuver else now_model,
+                                   active=True, curvature_scale=controls.VM.get_steer_from_curvature(1., 5., 0.) /
+                                   (controls.CP.steerRatio*controls.CP.wheelbase),
+                                   reference_source='lateralManeuverPlan' if maneuver else 'modelV2')
+        exec(pipeline[0], {'self': controls, 'CS': cs, 'CC': cc, 'actuators': cc.actuators, 'model_v2': model,
+                          'lp': SimpleNamespace(roll=0.), 'clip_curvature': clip_curvature, 'time': SimpleNamespace(monotonic=lambda now=now: now)})
+        assert controls.desired_curvature == pytest.approx(previous)
+        assert controls.ford_path == expected
+        assert controls.ford_path_controller.core.feedback_curvature == pytest.approx(previous)
+        assert controls.ford_path_controller.diagnostics['hypothesis'] == 'geometry-assisted-action-feedback-v18'
+        if maneuver:
+          assert controls.ford_path_controller.geometry_assist.action_peak == 0.
+        wire.check(controls.ford_path)
@@ -35,10 +35,10 @@ class FordGeometryReference:
    return raw, selected

  def apply(self, model_msg, driving_msg, sp_msg, *, speed, preview, smooth_seconds, smoothing_enabled):
-    """Keep the original action separately; both published actions use the selected reference.
+    """Publish geometry alongside the untouched action for controlsd's selector.

-    The caller retains its original prev_action, so this experiment cannot feed
-    geometry into the learned action's smoothing or change longitudinal control.
+    Embed the pair in modelV2 as well as diagnostic telemetry so controlsd never
+    has to join independently delivered publications from different frames.
    """
    model = model_msg.modelV2
    ref = sp_msg.modelDataV2SP.fordGeometryReference
@@ -51,6 +51,7 @@ class FordGeometryReference:
                         speed=speed, preview=preview, smooth_seconds=smooth_seconds, smoothing_enabled=smoothing_enabled)
    ref.valid = result is not None
    if result is not None:
-      ref.rawCurvature, model.action.desiredCurvature = result
-      driving_msg.drivingModelData.action.desiredCurvature = model.action.desiredCurvature
-    ref.selectedCurvature = model.action.desiredCurvature
+      ref.rawCurvature, ref.selectedCurvature = result
+    else:
+      ref.selectedCurvature = model.action.desiredCurvature
+    model.fordGeometryReference = ref
@@ -418,7 +418,7 @@ def main(demo=False):
  ford_model_action = params.get_bool("FordModelActionController")
  ford_geometry = select_ford_geometry_reference(CP, ford_model_action, params.get_bool("FordGeometryReference"))
  if CP.brand == 'ford':
-    cloudlog.event('Ford model reference selected', source='geometry' if ford_geometry is not None else 'action')
+    cloudlog.event('Ford model reference selected', source='geometry_assist_inputs' if ford_geometry is not None else 'action')

  # TODO Move smooth seconds to action function
  long_delay = CP.longitudinalActuatorDelay + model.LONG_SMOOTH_SECONDS
@@ -102,6 +102,7 @@ def test_publication_preserves_original_history_and_longitudinal_and_serializes_
  apply(FordGeometryReference(), m, d, sp)
  assert original.to_dict() == before  # prev_action held by modeld must remain the learned action.
  assert m.modelV2.action.to_dict() == d.drivingModelData.action.to_dict()
+  assert m.modelV2.action.to_dict() == before
  assert m.modelV2.action.desiredAcceleration == original.desiredAcceleration
  assert m.modelV2.action.shouldStop == original.shouldStop
  with log.Event.from_bytes(sp.to_bytes()) as decoded:
@@ -109,7 +110,7 @@ def test_publication_preserves_original_history_and_longitudinal_and_serializes_
    assert ref.enabled and ref.valid == (not invalid)
    assert ref.modelMonoTime == m.logMonoTime
    assert ref.actionDesiredCurvature == original.desiredCurvature
-    assert ref.selectedCurvature == m.modelV2.action.desiredCurvature
+    assert ref.to_dict() == m.modelV2.fordGeometryReference.to_dict()
    if invalid:
      assert ref.selectedCurvature == original.desiredCurvature
    else:
@@ -121,14 +122,15 @@ def test_published_geometry_flows_through_actual_controlsd_selection_feedback_an
  from opendbc.car import structs

  m, d, sp, _ = messages()
+  m.modelV2.action.desiredCurvature = d.drivingModelData.action.desiredCurvature = .015
  apply(FordGeometryReference(), m, d, sp)
-  controls = startup()
+  controls = startup(params=SimpleNamespace(get_bool=lambda k: k in ('FordModelActionController', 'FordGeometryReference')))
  controls.sm, controls.desired_curvature, controls.curvature = Subscriptions(maneuver), 0., 0.
  cc = structs.CarControl(latActive=True)
  cs = SimpleNamespace(vEgo=5., yawRate=0., canValid=True, steeringPressed=False, steeringTorque=0., steeringAngleDeg=0.)
  exec(pipeline[0], {'self': controls, 'CS': cs, 'CC': cc, 'actuators': cc.actuators, 'model_v2': m.modelV2,
                    'lp': SimpleNamespace(roll=0.), 'clip_curvature': clip_curvature, 'time': SimpleNamespace(monotonic=lambda: 1.)})
-  # Geometry asks right; original action and maneuver both ask left. The unchanged
+  # Both sources ask right; maneuver asks left and retains priority. The unchanged
  # upstream jerk limit permits 0.002 curvature in this first 10 ms step.
  expected = -.002 if maneuver else .002
  assert controls.desired_curvature == pytest.approx(expected)
@@ -167,5 +169,7 @@ def test_actual_modeld_hook_uses_exact_timing_after_original_action_history_is_s
    return
  expected = smooth_value(get_curvature_from_plan(list(m.modelV2.orientation.z), list(m.modelV2.orientationRate.z),
                                                 TIMES, 5., .743946), 0., .1)
-  assert m.modelV2.action.desiredCurvature == pytest.approx(expected)
+  assert m.modelV2.action.desiredCurvature == pytest.approx(-.003)
+  assert sp.modelDataV2SP.fordGeometryReference.selectedCurvature == pytest.approx(expected)
+  assert m.modelV2.fordGeometryReference.to_dict() == sp.modelDataV2SP.fordGeometryReference.to_dict()
  assert sp.modelDataV2SP.fordGeometryReference.previewSeconds == pytest.approx(.743946)
@@ -2184,7 +2184,7 @@
          "needs_onroad_cycle": true,
          "title": "Selected-Action Path Tracking (Experimental)",
          "description": "Follow the selected model reference using path-offset and heading commands with measured steering feedback on any Ford CAN FD vehicle.",
-          "details": "Uses the selected desired curvature, or the model path when Model Geometry Reference is enabled, with correction based on requested versus measured steering. Uses remaining path-offset range when the base heading request reaches its limit. The correction holds when steering matches and clears on driver override. Default off; physical tracking and turn-exit behavior are not road-validated. Enable only for controlled testing. Turning it off restores upstream Ford curvature control, regardless of any previously stored experimental settings. Only Ford CAN FD vehicles can use this experiment. Changes apply after a real offroad-to-onroad cycle, not immediately or on disengagement alone.",
+          "details": "Uses the selected desired curvature, with geometry assistance when Model Geometry Assist is enabled, with correction based on requested versus measured steering. Uses remaining path-offset range when the base heading request reaches its limit. The correction holds when steering matches and clears on driver override. Default off; physical tracking and turn-exit behavior are not road-validated. Enable only for controlled testing. Turning it off restores upstream Ford curvature control, regardless of any previously stored experimental settings. Only Ford CAN FD vehicles can use this experiment. Changes apply after a real offroad-to-onroad cycle, not immediately or on disengagement alone.",
          "enablement": [
            {
              "type": "offroad_only"
@@ -2195,9 +2195,9 @@
          "key": "FordGeometryReference",
          "widget": "toggle",
          "needs_onroad_cycle": true,
-          "title": "Model Geometry Reference (Experimental)",
-          "description": "Sample model lateral position for C0 and model heading for C1, with measured steering feedback.",
-          "details": "On uses direct model-path commands. C0 samples at 7 metres unless the C0 distance option is enabled; C1 samples at the larger of 7 metres or one second of travel. Feedback follows the heading from that same path. Command bounds and request limits remain active. Off restores the original model-action mapping. Requires Selected-Action Path Tracking; otherwise upstream Ford control remains selected. This changes the steering reference and has not been road-validated. Default off. Changes apply after an offroad-to-onroad cycle.",
+          "title": "Model Geometry Assist (Experimental)",
+          "description": "Add model geometry for stronger turn requests, then fade that assistance as the model action unwinds.",
+          "details": "Uses the model action for gentle requests. Adds stronger geometry demand when both sources agree on turn direction, and reduces the extra demand as the action falls from its peak. Both path commands follow the combined reference with measured steering feedback. Temporary action dips can also reduce assistance. Command bounds and request limits remain active. Off restores action-only control. Requires Selected-Action Path Tracking; turning that main toggle off restores upstream Ford control. Experimental and default off. Changes apply after an offroad-to-onroad cycle.",
          "enablement": [
            {
              "type": "offroad_only"
@@ -15,15 +15,15 @@ sections:
    needs_onroad_cycle: true
    title: Selected-Action Path Tracking (Experimental)
    description: Follow the selected model reference using path-offset and heading commands with measured steering feedback on any Ford CAN FD vehicle.
-    details: Uses the selected desired curvature, or the model path when Model Geometry Reference is enabled, with correction based on requested versus measured steering. Uses remaining path-offset range when the base heading request reaches its limit. The correction holds when steering matches and clears on driver override. Default off; physical tracking and turn-exit behavior are not road-validated. Enable only for controlled testing. Turning it off restores upstream Ford curvature control, regardless of any previously stored experimental settings. Only Ford CAN FD vehicles can use this experiment. Changes apply after a real offroad-to-onroad cycle, not immediately or on disengagement alone.
+    details: Uses the selected desired curvature, with geometry assistance when Model Geometry Assist is enabled, with correction based on requested versus measured steering. Uses remaining path-offset range when the base heading request reaches its limit. The correction holds when steering matches and clears on driver override. Default off; physical tracking and turn-exit behavior are not road-validated. Enable only for controlled testing. Turning it off restores upstream Ford curvature control, regardless of any previously stored experimental settings. Only Ford CAN FD vehicles can use this experiment. Changes apply after a real offroad-to-onroad cycle, not immediately or on disengagement alone.
    enablement:
    - $ref: '#/macros/offroad'
  - key: FordGeometryReference
    widget: toggle
    needs_onroad_cycle: true
-    title: Model Geometry Reference (Experimental)
-    description: Sample model lateral position for C0 and model heading for C1, with measured steering feedback.
-    details: On uses direct model-path commands. C0 samples at 7 metres unless the C0 distance option is enabled; C1 samples at the larger of 7 metres or one second of travel. Feedback follows the heading from that same path. Command bounds and request limits remain active. Off restores the original model-action mapping. Requires Selected-Action Path Tracking; otherwise upstream Ford control remains selected. This changes the steering reference and has not been road-validated. Default off. Changes apply after an offroad-to-onroad cycle.
+    title: Model Geometry Assist (Experimental)
+    description: Add model geometry for stronger turn requests, then fade that assistance as the model action unwinds.
+    details: Uses the model action for gentle requests. Adds stronger geometry demand when both sources agree on turn direction, and reduces the extra demand as the action falls from its peak. Both path commands follow the combined reference with measured steering feedback. Temporary action dips can also reduce assistance. Command bounds and request limits remain active. Off restores action-only control. Requires Selected-Action Path Tracking; turning that main toggle off restores upstream Ford control. Experimental and default off. Changes apply after an offroad-to-onroad cycle.
    enablement:
    - $ref: '#/macros/offroad'
    - type: param
@@ -0,0 +1,2 @@
+"""Compatibility import for the offline replay; production owns the tested selector."""
+from openpilot.selfdrive.controls.lib.ford_geometry_action import GeometryActionHybrid as GeometryActionHybrid
@@ -0,0 +1,240 @@
+"""Offline action/geometry reference comparison on frozen recorded vehicle motion.
+
+All variants share the current gains, driver gates and PSCM status. The direct
+baseline keeps its independent C0 path mapping; scalar variants use the driven
+curvature-to-C0/C1 mapper. This predicts commands, never the resulting trajectory.
+"""
+import argparse
+import hashlib
+import json
+from pathlib import Path
+from types import SimpleNamespace
+
+import numpy as np
+
+from opendbc.car.vehicle_model import VehicleModel
+from openpilot.selfdrive.controls.lib.drive_helpers import clip_curvature
+from openpilot.selfdrive.controls.lib.ford_model_action import FordModelActionController
+from openpilot.sunnypilot.modeld_v2.constants import ModelConstants
+from openpilot.sunnypilot.modeld_v2.ford_geometry import FordGeometryReference
+from tools.ford_pscm_lab.model_action_replay import WireCheck, sample, table
+
+
+def load_route(label):
+  root = Path('.cache')/f'ford_route{label}'
+  recent = label in ('15a', '15b')
+  source = root/('rlog_full' if recent else 'full')
+  meta = json.loads((source/'metadata.json').read_text())
+  with np.load(source/'route.npz') as z:
+    r = {k: table(z, k) for k in ('controls', 'cs', 'cc', 'path', 'params', 'pscm', 'model')}
+    if recent:
+      r['geometry'] = table(z, 'geometry')
+  c = r['controls']
+  t = c['t']
+  cs, pa, ps = [sample(r[k], t) for k in ('cs', 'params', 'pscm')]
+  cc, sent = [sample(r[k], t, nearest=True) for k in ('cc', 'path')]
+  sources = [source/'route.npz', source/'metadata.json']
+  if recent:
+    with np.load(source/'model_paths.npz') as z:
+      paths, ns = z['paths'], z['ns']
+    g = r['geometry']
+    gi = np.clip(np.searchsorted(g['model_ns'], ns), 0, len(g['t'])-1)
+    assert np.array_equal(g['model_ns'][gi], ns)
+    action = g['action'][gi]
+    geometry = np.where(g['valid'][gi]*g['reference_valid'][gi] > 0, g['selected'][gi], action)
+    geometry_method = 'Logged original action and selected geometry; exact model timestamp joins'
+    sources.append(source/'model_paths.npz')
+  else:
+    with np.load(root/'intake.npz') as z:
+      m, paths, delay = z['model'], z['plans'], z['delay']
+    intake = json.loads((root/'intake.json').read_text())
+    settings = intake['settings'][0]['params']
+    bundle = next(b for b in intake['bundles'].values() if b['internalName'] == settings['ModelManager_ActiveBundleChestnut'])
+    tau = float(next(o['value'] for o in bundle['overrides'] if o['key'] == 'lat'))
+    ns, action = m[:, 3], m[:, 7]
+    ci = np.clip(np.searchsorted(c['model_ns'], ns), 0, len(t)-1)
+    speed = np.maximum(cs['speed'][ci], 0.)
+    learned_delay = delay[np.clip(np.searchsorted(delay[:, 0], m[:, 0], side='right')-1, 0, len(delay)-1), 3]
+    preview = learned_delay+tau+.075+.4*np.clip((30-speed/.44704)/15, 0, 1)
+    reference = FordGeometryReference()
+    geometry = np.empty(len(m))
+    for j, path in enumerate(paths):
+      result = reference.update(path[2], path[3], ModelConstants.T_IDXS, speed=speed[j], preview=preview[j],
+                                smooth_seconds=tau, smoothing_enabled=bundle['generation'] >= 10)
+      geometry[j] = action[j] if result is None else np.float32(result[1])
+    geometry_method = 'Reconstructed geometry with recorded delay/model smoothing; speed approximated by first consuming control sample'
+    sources += [root/'intake.npz', root/'intake.json']
+  mi = np.clip(np.searchsorted(ns, c['model_ns']), 0, len(ns)-1)
+  exact = ns[mi] == c['model_ns']
+  models = [SimpleNamespace(position=SimpleNamespace(x=p[0], y=p[1]), orientation=SimpleNamespace(z=p[2]),
+                            action=SimpleNamespace(desiredCurvature=float(action[j])),
+                            fordGeometryReference=SimpleNamespace(enabled=True, valid=True, modelMonoTime=int(ns[j]),
+                                                                  selectedCurvature=float(geometry[j]))) for j, p in enumerate(paths)]
+  active = cc['active'].astype(bool)
+  valid = c['valid']*cs['valid']*cs['can_valid']*pa['valid'] > 0
+  valid &= exact & (abs(t-cs['t']) <= .15) & (t-c['model_ns']*1e-9 >= -.005) & (t-c['model_ns']*1e-9 <= .15)
+  valid &= (cs['speed'] >= .3) & (cs['speed'] <= 55.)
+  car = meta['car'][0]
+  cp = SimpleNamespace(**{k: car[k] for k in ('mass', 'wheelbase', 'centerToFront', 'steerRatioRear', 'tireStiffnessFront', 'tireStiffnessRear')},
+                       steerRatio=car['steer_ratio'], rotationalInertia=0.)
+  summary = (Path('.cache/ford_routes158_159')/f'{label}_summary.json') if recent else root/'summary.json'
+  events = json.loads(summary.read_text())['events']
+  return SimpleNamespace(label=label, source=source, meta=meta, c=c, t=t, cs=cs, pa=pa, ps=ps, sent=sent,
+                         active=active, valid=valid, models=models, mi=mi, action=action[mi], geometry=geometry[mi], cp=cp,
+                         events=events, sources=sources, geometry_method=geometry_method)
+
+
+def sustained(t, mask, seconds=.2):
+  indices = np.flatnonzero(mask)
+  if not len(indices):
+    return None
+  breaks = np.r_[0, np.flatnonzero((np.diff(indices) > 1) | (np.diff(t[indices]) > .03))+1, len(indices)]
+  for start, end in zip(breaks[:-1], breaks[1:], strict=True):
+    if t[indices[end-1]]-t[indices[start]] >= seconds:
+      return float(t[indices[start]])
+  return None
+
+
+def summarize(d, a, names):
+  t = a['t']
+  live = a['valid'].astype(bool)
+  dt = np.minimum(np.diff(t, append=t[-1]+.01), .03)
+  consecutive = live[1:] & live[:-1] & (np.diff(t) < .03)
+  low = consecutive & (a['speed'][1:] < 15*.44704)
+  ordinary = live & (abs(d.action) < .01) & (abs(d.geometry) < .01)
+  results = {'cycles': len(t), 'active_valid_s': float(dt[live].sum()), 'geometry_source': d.geometry_method,
+             'ordinary_definition': 'Both action and geometry below 0.01 1/m; includes state carried out of earlier turns',
+             'variants': {}, 'events': []}
+  for name in names:
+    results['variants'][name] = {
+      'c0_bound_s': float(dt[live & (abs(a[name+'_c0']) >= 5.105)].sum()),
+      'c1_bound_s': float(dt[live & (abs(a[name+'_c1']) >= .49975)].sum()),
+      'low_speed_c0_steps_over_025m': int((abs(np.diff(a[name+'_c0'])[low]) > .250001).sum()),
+      'low_speed_c1_steps_over_005rad': int((abs(np.diff(a[name+'_c1'])[low]) > .050001).sum()),
+      'ordinary_difference_from_action_p50_p95_max': {
+        k: np.quantile(abs(a[name+'_'+k][ordinary]-a['action_'+k][ordinary]), [.5, .95, 1]).tolist() for k in ('angle', 'c0', 'c1')},
+    }
+  for e in d.events:
+    lo, hi = e['window']
+    window = live & (t >= lo) & (t <= hi)
+    if not window.any():
+      continue
+    sign = 1 if e['direction'] == 'left' else -1
+    action_peak_i = np.flatnonzero(window)[np.argmax(sign*a['action_angle'][window])]
+    action_peak_t = t[action_peak_i]
+    build = window & (t <= action_peak_t)
+    release = window & (t >= action_peak_t)
+    item = {'id': e['id'], 'direction': e['direction'], 'window': [lo, hi], 'action_peak_t': float(action_peak_t), 'variants': {}}
+    extra = sign*(a['geometry_angle']-a['action_angle'])
+    assist = build & (extra > 5.) & (sign*a['action_angle'] > 0.) & (sign*a['geometry_angle'] > 50.)
+    for name in names:
+      angle = sign*a[name+'_angle']
+      last_above = np.flatnonzero(release & (angle >= 25.))
+      last_c1 = np.flatnonzero(release & (-sign*a[name+'_c1'] >= .05))
+      item['variants'][name] = {
+        'peak_angle_deg': float(max(angle[window])),
+        'entry_50_s': sustained(t, build & (angle >= 50.)),
+        'release_25_s': sustained(t, release & (angle < 25.)),
+        'last_above_25_s': float(t[last_above[-1]]) if len(last_above) else None,
+        'peak_abs_c0': float(max(abs(a[name+'_c0'][window]))), 'peak_abs_c1': float(max(abs(a[name+'_c1'][window]))),
+        'c1_release_005_s': sustained(t, release & (-sign*a[name+'_c1'] < .05)),
+        'c1_last_above_005_s': float(t[last_c1[-1]]) if len(last_c1) else None,
+        'entry_geometry_extra_retained_fraction': (float(np.sum(sign*(a[name+'_angle']-a['action_angle'])[assist]*dt[assist]) /
+                                                            np.sum(extra[assist]*dt[assist])) if assist.any() else None),
+      }
+    results['events'].append(item)
+  return results
+
+
+def run(label, output, baseline=None):
+  d = load_route(label)
+  names = ['action', 'geometry', 'direct', 'stronger', 'half', 'hybrid']
+  evaluated = ['hybrid'] if baseline else names
+  cores = [FordModelActionController(direct_path=name == 'direct', geometry_assist=name == 'hybrid') for name in evaluated]
+  wires = [WireCheck() for _ in evaluated]
+  hybrid_controller = cores[evaluated.index('hybrid')]
+  hybrid = hybrid_controller.geometry_assist
+  selected = np.zeros(len(evaluated))
+  vm = VehicleModel(d.cp)
+  columns = ['t', 'valid', 'active', 'speed', 'driver', 'wheel', 'weight', 'action_peak', 'action_raw', 'geometry_raw']
+  for name in names:
+    columns.extend(name+'_'+k for k in ('reference', 'angle', 'c0', 'c1', 'i'))
+  rows = np.empty((len(d.t), len(columns)))
+  saved_columns = list(range(10))+[columns.index(name+'_'+k) for name in evaluated for k in ('reference', 'angle', 'c0', 'c1', 'i')]
+  if baseline:
+    with np.load(baseline/label/'commands.npz') as z:
+      assert z['names'].tolist() == columns
+      rows[:] = z['rows']
+    np.testing.assert_allclose(rows[:, 0], d.t-d.meta['t0'], atol=1e-9)
+  for i, now in enumerate(d.t):
+    cs, pa, ps, c = d.cs, d.pa, d.ps, d.c
+    vm.update_params(max(pa['stiffness'][i], .1), max(pa['steer_ratio'][i], .1))
+    scale = vm.get_steer_from_curvature(1., cs['speed'][i], 0.)/(d.cp.steerRatio*d.cp.wheelbase)
+    error = c['desired'][i]-c['measured'][i]
+    angle_scale = -vm.sR/vm.curvature_factor(cs['speed'][i])*180./np.pi
+    if abs(error) > 1e-5:
+      angle_scale = (c['desired_angle'][i]-c['actual_angle'][i])/error
+      scale = -np.radians(angle_scale)/(d.cp.steerRatio*d.cp.wheelbase)
+    status = SimpleNamespace(valid=bool(ps['valid'][i] and ps['status_valid'][i]), canMonoTime=round(ps['stamp'][i]*1e9),
+                             limit=int(ps['limit'][i]), lateralState=int(ps['lateral_state'][i]), denied=bool(ps['denied'][i]))
+    action, geometry = float(d.action[i]), float(d.geometry[i])
+    maximum = geometry if action*geometry > 0 and abs(geometry) > abs(action) else action
+    combined = hybrid_controller.select_reference(d.models[d.mi[i]], model_mono_time=int(c['model_ns'][i]),
+                                                  active=bool(d.active[i]), valid=bool(d.valid[i]))
+    targets = {'action': action, 'geometry': geometry, 'stronger': maximum, 'half': .5*(action+geometry), 'hybrid': combined}
+    if 'direct' in evaluated:
+      targets['direct'] = cores[evaluated.index('direct')].path_curvature(d.models[d.mi[i]], cs['speed'][i])
+    row = [now-d.meta['t0'], False, d.active[i], cs['speed'][i],
+           cs['pressed'][i] or abs(cs['torque'][i]) > 1. or status.limit == 3, c['actual_angle'][i],
+           hybrid.weight, hybrid.action_peak, action, geometry]
+    validity = []
+    for j, (core, name) in enumerate(zip(cores, evaluated, strict=True)):
+      target = targets[name]
+      selected[j], _ = clip_curvature(cs['speed'][i], selected[j], target if d.active[i] else c['measured'][i], pa['roll'][i])
+      command = core.update(d.models[d.mi[i]], selected[j], current_curvature=c['measured'][i],
+                            speed=cs['speed'][i], yaw_rate=cs['yaw'][i], now=now, measurement_time=cs['t'][i],
+                            model_time=c['model_ns'][i]*1e-9, reference_time=c['model_ns'][i]*1e-9,
+                            active=bool(d.active[i]), valid=bool(d.valid[i]),
+                            driver_pressed=bool(cs['pressed'][i]), driver_torque=cs['torque'][i],
+                            pscm_status=status, curvature_scale=scale, roll=pa['roll'][i])
+      assert np.isfinite([command.path_offset, command.path_angle]).all()
+      assert abs(command.path_offset) <= 5.1100001 and abs(command.path_angle) <= .5000001
+      assert command.curvature == command.curvature_rate == 0.
+      if command.valid and row[4] and (status.limit != 3 or (status.valid and -.005 <= now-ps['stamp'][i] <= .15)):
+        assert not core.diagnostics['feedback_enabled']
+        assert core.core.correction == core.core.proportional == core.core.offset_proportional == 0.
+      if i % 10 == 0:
+        wires[j].check(command)
+      angle = c['desired_angle'][i]+angle_scale*(selected[j]-c['desired'][i])
+      row.extend([selected[j], angle, command.path_offset, command.path_angle, core.core.correction])
+      validity.append(command.valid)
+    assert all(v == validity[0] for v in validity)
+    row[1] = validity[0]
+    if baseline:
+      assert rows[i, 1] == row[1]
+    rows[i, saved_columns] = row
+  a = dict(zip(columns, rows.T, strict=True))
+  results = summarize(d, a, names)
+  results['wire_checks'] = sum(w.count for w in wires)
+  results['variants_evaluated_this_run'] = evaluated
+  results['cached_baseline'] = str(baseline/label) if baseline else None
+  results['reference_design'] = {'start_curvature': hybrid.start, 'full_curvature': hybrid.full, 'release_power': hybrid.release_power}
+  files = d.sources+[Path(__file__), Path('openpilot/selfdrive/controls/lib/ford_geometry_action.py'),
+                     Path('openpilot/selfdrive/controls/lib/ford_model_action.py')]
+  if baseline:
+    files += [baseline/label/'commands.npz', baseline/label/'report.json']
+  results['sources_sha256'] = {str(f): hashlib.sha256(f.read_bytes()).hexdigest() for f in files}
+  dest = output/label
+  dest.mkdir(parents=True, exist_ok=True)
+  np.savez_compressed(dest/'commands.npz', names=columns, rows=rows)
+  (dest/'report.json').write_text(json.dumps(results, indent=2, allow_nan=False)+'\n')
+  print(json.dumps({'route': label, 'cycles': results['cycles'], 'wire_checks': results['wire_checks'], 'events': len(results['events'])}), flush=True)
+
+
+if __name__ == '__main__':
+  parser = argparse.ArgumentParser(description=__doc__)
+  parser.add_argument('--route', required=True)
+  parser.add_argument('--output', type=Path, required=True)
+  parser.add_argument('--baseline', type=Path, help='Reuse unchanged baseline columns and replay the hybrid only')
+  args = parser.parse_args()
+  run(args.route, args.output, args.baseline)
@@ -0,0 +1,222 @@
+"""Build an offline command-comparison report; no simulated PSCM response."""
+# ruff: noqa: E501
+import argparse
+import hashlib
+import json
+from pathlib import Path
+
+import matplotlib
+matplotlib.use('Agg')
+import matplotlib.pyplot as plt
+import numpy as np
+
+
+LABELS = ['149', '151', '157', '15a', '15b']
+VARIANTS = ['action', 'geometry', 'direct', 'stronger', 'half', 'hybrid']
+TITLES = {'action': 'Action only', 'geometry': 'Geometry only', 'direct': 'Direct path',
+          'stronger': 'Stronger source', 'half': '50/50 blend', 'hybrid': 'Hybrid'}
+COLORS = {'action': '#1971c2', 'geometry': '#d18a00', 'hybrid': '#087f5b', 'initial': '#bd3545'}
+
+
+def load(path):
+  with np.load(path) as z:
+    return dict(zip(z['names'], z['rows'].T, strict=True))
+
+
+def cells(values):
+  return '<tr>'+''.join('<td>'+str(v)+'</td>' for v in values)+'</tr>'
+
+
+def table(headers, rows):
+  return '<div class="scroll"><table><thead><tr>'+''.join('<th>'+x+'</th>' for x in headers)+'</tr></thead><tbody>'+''.join(rows)+'</tbody></table></div>'
+
+
+def plot(a, dest, title, window, direction, initial=None):
+  t = a['t']
+  mask = (t >= window[0]) & (t <= window[1])
+  live = a['valid'][mask].astype(bool)
+  x = t[mask]
+  sign = 1 if direction == 'left' else -1
+  fig, axes = plt.subplots(4, 1, figsize=(11.4, 9.2), sharex=True, gridspec_kw={'height_ratios': [2, 1.35, 1.35, .8]})
+  fig.suptitle(title, x=.09, ha='left', fontsize=17, fontweight='bold')
+  labels = ['Reference\n(wheel-equivalent °)', 'C0 command\n(m)', 'C1 command\n(rad)']
+  for ax, key, ylabel in zip(axes, ['angle', 'c0', 'c1'], labels, strict=False):
+    for name in ['geometry', 'action', 'hybrid']:
+      y = a[name+'_'+key][mask]*sign*(1 if key == 'angle' else -1)
+      ax.plot(x, np.where(live, y, np.nan), label=TITLES[name], color=COLORS[name], lw=2.1 if name == 'hybrid' else 1.6)
+    if initial is not None:
+      y = initial['hybrid_'+key][mask]*sign*(1 if key == 'angle' else -1)
+      ax.plot(x, np.where(live, y, np.nan), label='Rejected first hybrid', color=COLORS['initial'], ls='--', lw=1.5)
+    ax.set_ylabel(ylabel)
+  axes[0].legend(loc='best', fontsize=9, framealpha=.95, ncol=2)
+  axes[-1].plot(x, a['weight'][mask]*100, color=COLORS['hybrid'], lw=1.5)
+  axes[-1].set_ylabel('Geometry\nassistance (%)')
+  axes[-1].set_ylim(-5, 105)
+  axes[-1].set_yticks([0, 50, 100])
+  axes[-1].set_xlabel('Seconds from route start · positive values point into the named turn')
+  for ax in axes:
+    ax.grid(alpha=.2)
+    ax.axhline(0, color='#868e96', lw=.7)
+    ax.fill_between(x, 0, 1, where=(a['driver'][mask] > 0) & live, color='#89939e', alpha=.10,
+                    transform=ax.get_xaxis_transform(), linewidth=0)
+    ax.spines[['top', 'right']].set_visible(False)
+    ax.margins(x=0)
+  fig.text(.09, .018, 'Gray shading: recorded driver/override indication. These are replayed requests, not predicted wheel motion.',
+           fontsize=9, color='#495057')
+  fig.subplots_adjust(left=.10, right=.98, top=.92, bottom=.075, hspace=.12)
+  fig.savefig(dest, dpi=150, facecolor='white')
+  plt.close(fig)
+
+
+def run(root, initial_root, dest):
+  dest.mkdir(parents=True, exist_ok=True)
+  arrays = {s: load(root/s/'commands.npz') for s in LABELS}
+  reports = {s: json.loads((root/s/'report.json').read_text()) for s in LABELS}
+  initial = {s: load(initial_root/s/'commands.npz') for s in ['15a', '15b']}
+  all_a = {k: np.concatenate([a[k] for a in arrays.values()]) for k in arrays['15a']}
+  live = all_a['valid'].astype(bool)
+  ordinary = live & (abs(all_a['action_raw']) < .01) & (abs(all_a['geometry_raw']) < .01)
+  events = [(s, e) for s, r in reports.items() for e in r['events']]
+  retained, release_delta, entry_lead = [], [], []
+  censored = []
+  for s, e in events:
+    a, h = [e['variants'][v] for v in ['action', 'hybrid']]
+    if h['entry_geometry_extra_retained_fraction'] is not None:
+      retained.append(h['entry_geometry_extra_retained_fraction'])
+    if None not in (a['release_25_s'], h['release_25_s']):
+      release_delta.append(h['release_25_s']-a['release_25_s'])
+    elif a['release_25_s'] is not None:
+      censored.append([s, e['id']])
+    if None not in (a['entry_50_s'], h['entry_50_s']):
+      entry_lead.append(a['entry_50_s']-h['entry_50_s'])
+  metrics = {
+    'route_labels': LABELS, 'unique_control_cycles': len(live), 'active_valid_cycles': int(live.sum()),
+    'active_valid_seconds': sum(r['active_valid_s'] for r in reports.values()),
+    'marked_windows_including_overlaps': len(events), 'entry_extra_eligible_windows': len(retained),
+    'entry_geometry_extra_retained_p10_p50_p90': np.quantile(retained, [.1, .5, .9]).tolist(),
+    'paired_reference_releases': len(release_delta), 'reference_release_delay_p50_p90_max': np.quantile(release_delta, [.5, .9, 1]).tolist(),
+    'hybrid_release_not_observed_before_disengagement': censored,
+    'paired_reference_entries': len(entry_lead), 'reference_entry_lead_p50_p90_max': np.quantile(entry_lead, [.5, .9, 1]).tolist(),
+    'ordinary_cycles': int(ordinary.sum()), 'ordinary_p95_difference_from_action': {}, 'totals': {},
+    'comparison_baseline_commit': '01f5d5429', 'report_scope': 'offline_command_comparison',
+  }
+  ordinary_rows, totals_rows, route_rows = [], [], []
+  for v in VARIANTS:
+    ordinary_diff = {k: np.quantile(abs(all_a[v+'_'+k]-all_a['action_'+k])[ordinary], [.5, .95, 1]).tolist() for k in ['angle', 'c0', 'c1']}
+    metrics['ordinary_p95_difference_from_action'][v] = ordinary_diff
+    ordinary_rows.append(cells([TITLES[v], *[f'{ordinary_diff[k][1]:.4f}' for k in ['angle', 'c0', 'c1']]]))
+    sums = {k: sum(r['variants'][v][k] for r in reports.values()) for k in [
+      'c0_bound_s', 'c1_bound_s', 'low_speed_c0_steps_over_025m', 'low_speed_c1_steps_over_005rad']}
+    metrics['totals'][v] = sums
+    totals_rows.append(cells([TITLES[v], f"{sums['c0_bound_s']:.1f}", f"{sums['c1_bound_s']:.1f}",
+                             sums['low_speed_c0_steps_over_025m'], sums['low_speed_c1_steps_over_005rad']]))
+  for s, r in reports.items():
+    route_rows.append(cells([s, f"{r['cycles']:,}", f"{r['active_valid_s']/60:.2f}", len(r['events']),
+                            'Logged separately' if s in ['15a', '15b'] else 'Reconstructed from logged plans']))
+  sweep_rows = []
+  for s in ['15a', '15b']:
+    for row in json.loads((root/s/'sweep.json').read_text()):
+      sweep_rows.append(cells([s, f"{row['start']:.3f}–{row['full']:.3f}", f"{row['power']:.0f}",
+                               f"{row['geometry_entry_extra_retained_p10_p50_p95_max'][1]*100:.1f}%",
+                               f"{row['release_later_than_action_s_p10_p50_p95_max'][-1]:.3f}", row['target_steps_over_20deg']]))
+  event_rows = []
+  for s, id_ in [('15a', 1), ('15a', 5), ('15b', 1), ('15b', 4), ('15b', 7)]:
+    e = next(e for e in reports[s]['events'] if e['id'] == id_)
+    a, g, h = [e['variants'][v] for v in ['action', 'geometry', 'hybrid']]
+    event_rows.append(cells([f'{s} / {id_} {e["direction"]}',
+                            f"{a['peak_angle_deg']:.0f} / {g['peak_angle_deg']:.0f} / <b>{h['peak_angle_deg']:.0f}</b>",
+                            f"{a['peak_abs_c0']:.2f} / {g['peak_abs_c0']:.2f} / <b>{h['peak_abs_c0']:.2f}</b>",
+                            f"{a['peak_abs_c1']:.3f} / {g['peak_abs_c1']:.3f} / <b>{h['peak_abs_c1']:.3f}</b>",
+                            f"{h['release_25_s']-a['release_25_s']:+.3f}"]))
+  plots = [
+    ('15b', 'large-left', '15b · large left: geometry strength, action-led release', [164., 176.], 'left', False),
+    ('15a', 'exit', '15a · left turn: geometry rebound during the exit', [188., 198.], 'left', False),
+    ('15b', 'rearm', '15b · exit rebound caught and fixed offline', [43., 48.5], 'left', True),
+    ('15b', 'conflict', '15b · source disagreement: the hybrid follows the action', [79., 85.], 'left', False),
+    ('151', 'dip', '151 · a temporary action dip also reduces geometry assistance', [2590., 2603.], 'right', False),
+    ('157', 'large-geometry', '157 · stronger requests do not establish that the path is correct', [166., 180.], 'right', False),
+  ]
+  for s, slug, title, window, direction, show_initial in plots:
+    plot(arrays[s], dest/f'hybrid-{slug}.png', title, window, direction, initial.get(s) if show_initial else None)
+  def get_image(slug):
+    return f'<img src="hybrid-{slug}.png" alt="Reference, C0, C1 and geometry assistance over time">'
+  report = f'''<!doctype html><html><head><meta charset="utf-8"><meta name="viewport" content="width=device-width, initial-scale=1">
+<title>Ford action + geometry · offline hybrid</title><style>
+:root{{color-scheme:light}}*{{box-sizing:border-box}}body{{font-family:system-ui,-apple-system,sans-serif;color:#17232e;background:#f4f6f8;margin:0;line-height:1.55}}
+main{{max-width:1190px;margin:auto;padding:35px 25px 70px}}h1{{font-size:38px;letter-spacing:-1.2px;line-height:1.15;margin:12px 0 20px}}h2{{font-size:24px;margin:30px 0 12px}}h3{{font-size:19px}}
+p{{max-width:1000px}}.eyebrow{{color:#556373;font-size:13px;letter-spacing:1px;text-transform:uppercase}}.lead{{font-size:21px;max-width:950px}}.card{{background:white;padding:24px;border:1px solid #dce3e9;border-radius:12px;margin:20px 0}}
+.stats{{display:grid;grid-template-columns:repeat(3,1fr);gap:12px}}.stats div{{background:#e4f3ec;padding:20px;border-radius:10px}}.stats strong{{font-size:30px;display:block;color:#087f5b}}.note{{color:#526171;font-size:14px}}.caution{{background:#fff4dc;border-left:4px solid #d18a00;padding:16px 20px}}
+img{{max-width:100%;display:block;border:1px solid #e0e6eb;border-radius:8px;margin:16px 0}}table{{border-collapse:collapse;width:100%;font-size:14px}}th,td{{padding:11px 12px;border-bottom:1px solid #e1e7ed;text-align:left;white-space:nowrap}}th{{background:#edf2f6;font-size:13px}}.scroll{{overflow:auto}}code{{background:#eaf0f4;padding:2px 5px;border-radius:4px}}pre{{white-space:pre-wrap;background:#eef3f7;padding:16px;border-radius:8px}}summary{{cursor:pointer;font-weight:600;padding:12px 0}}a{{color:#1263a0}}li{{margin:8px 0}}
+@media(max-width:700px){{main{{padding:20px 12px}}h1{{font-size:30px}}.stats{{grid-template-columns:1fr}}.card{{padding:16px}}}}
+</style></head><body><main>
+<div class="eyebrow">Ford Lightning · offline experiment · 16 September 2026</div>
+<h1>Geometry for turn strength.<br>Action to lead the unwind.</h1>
+<p class="lead">The revised hybrid produces the intended command pattern in many turns: requests close to geometry at the peak, then close to the action at the exit. It preserves the action reference for gentle bends. It does not resolve which source is right when they disagree.</p>
+<div class="stats"><div><strong>4.33 vs 4.41 m</strong>Hybrid vs geometry peak C0<br>15b, large left</div><div><strong>0.50 vs 0.50 rad</strong>Same peak C1 on that turn<br>Both reach the field bound</div><div><strong>0.85 s earlier</strong>Hybrid reference releases before geometry<br>Matches action’s 25° crossing</div></div>
+<p class="note">These are replayed requests using recorded vehicle motion. They are not predictions of the wheel response, path accuracy, comfort, or safety. This report evaluates requests offline; the repository deployment notes describe the current integration.</p>
+<section class="card"><h2>The proposed rule</h2>
+<ol><li><b>Start with the original model action.</b> Gentle requests stay with the action.</li>
+<li><b>Add geometry’s extra demand</b> when geometry describes a tighter turn, both sources agree on direction, and the action supports meaningful demand.</li>
+<li><b>Fade that extra demand as the action falls from its peak.</b> Half the peak action leaves at most a quarter of the extra geometry request. Zero or opposite action removes the boost.</li></ol>
+<p>Both C0 and C1 receive <b>one combined curvature reference</b> through the existing scalar mapper and feedback. C0 uses the existing 7 m circular-arc calculation. C1 uses the existing heading calculation. This does not give one source to C0 and a conflicting source to C1.</p>
+<p>Geometry here is the earlier, smoothed orientation-derived curvature reference from the first geometry drives. The latest direct-path controller is included as a separate comparison.</p>
+<details><summary>Formula and tuning choices</summary><pre>H = A + gate(|G|) × (|A| / max(0.01, A_peak))² × (G − A)
+Apply assistance only when A and G have the same sign and |G| &gt; |A|.
+gate = smoothstep from 0 at |G| = 0.01/m to 1 at |G| = 0.02/m.
+A_peak = largest same-direction action magnitude during the current geometry excursion.
+Reset at inactive/invalid geometry, geometry sign change, or |G| ≤ 0.01/m.</pre>
+<p>The 0.01/0.02 band and squared fade are explicit experimental tuning choices. They are not Ford constants. P/I gains, ordinary controller gates, upstream curvature limits and CAN field bounds are unchanged. The selector has peak memory; it is not a stateless average.</p></details></section>
+<section class="card"><h2>Turn strength really reaches C0/C1</h2>
+<p>On the large left, the hybrid reaches a 359° wheel-equivalent reference versus geometry’s 361° and action’s 218°. Around 170 s, its C0/C1 are −4.04 m / −0.50 rad, close to geometry’s −4.09 m / −0.50 rad. Around 173 s, hybrid and action both request −0.26 m / −0.0745 rad.</p>
+{get_image('large-left')}
+<p class="note">Raw CAN command signs above are retained. The chart flips signs so positive means into this left turn. C1’s 0.50 rad bound is a message-field bound; this replay cannot identify the PSCM’s physical limit.</p>
+<p>Each cell below lists <b>action / geometry / hybrid</b>. Peaks need not occur at the same instant.</p>
+{table(['Window','Peak reference °','Peak |C0| m','Peak |C1| rad','Hybrid − action release, s'],event_rows)}
+<p class="note">Release is the first reference below 25° in the turn direction, sustained for 0.2 s after the action peak. It is not a measured wheel unwind time.</p></section>
+<section class="card"><h2>Exit behavior: useful, but not a perfect phase detector</h2>
+<p>In this 15a exit, the action-led fade removes geometry’s later extra demand. The hybrid reaches the reference release threshold with the action, roughly 0.50 s before geometry.</p>{get_image('exit')}
+<p>The first prototype had a real flaw: after a geometry dip reset its memory, a tiny action rebound could authorize full geometry again. At 46.73 s on 15b, action requested 3.2°, but that prototype requested 103°. The revised denominator limits it to <b>4.3°</b>; C1 becomes −0.0095 rad rather than −0.1805 rad.</p>{get_image('rearm')}
+<p class="caution"><b>The remaining ambiguity:</b> a temporary dip in the action also reduces assistance. The selector cannot know from that dip alone whether the road is ending the turn or the action is briefly weak. When sources point opposite ways, it follows the action and can lose geometry’s early entry.</p>
+<details><summary>See the unfavorable cases</summary>
+<p>15b: the action and geometry disagree on turn phase/direction. This rule keeps only about 5% of geometry’s eligible extra demand before the marked action peak. That is a deliberate source choice, not evidence the road needed less steering.</p>{get_image('conflict')}
+<p>151: before a later action peak, the action briefly falls to a 26° request while geometry still requests 122°. Hybrid falls to 30°. This is the unresolved tradeoff in using action reductions to lead release.</p>{get_image('dip')}
+<p>157: some geometry requests are far larger than the action (about 807° versus 266° peak in this window). The hybrid can inherit that demand and encounter the existing field bounds. Matching previous geometry numbers does not show that the model path is correct.</p>{get_image('large-geometry')}</details></section>
+<section class="card"><h2>Across five routes</h2>
+<p><b>{len(live):,} unique control cycles</b>, {metrics['active_valid_seconds']/60:.1f} minutes of active valid input, and {len(events)} previously marked windows, some overlapping. Median retained geometry extra demand during eligible entries is <b>{np.median(retained)*100:.0f}%</b>; the 10th percentile is {np.quantile(retained,.1)*100:.0f}%. This metric includes only same-direction entry samples where geometry already requests at least 50° and exceeds the action by more than 5°.</p>
+<p>For {len(release_delta)} paired reference-release crossings, median added delay versus action is <b>{np.median(release_delta):.2f} s</b>, with a maximum of {max(release_delta):.2f} s. One additional window (157 / 5) disengages before hybrid’s 0.2 s release crossing can be observed. First crossings do not rule out a later rebound; the plots and final-crossing checks matter.</p>
+{table(['Route suffix','Control cycles','Active valid minutes','Marked windows','Geometry source'],route_rows)}
+<h3>Gentle requests stay near action</h3>
+<p>For {int(ordinary.sum()):,} active valid samples where both source curvatures are below 0.01/m, these are the 95th-percentile absolute differences from action. Earlier integrator state and curvature-limiter history can still differ: hybrid’s worst C1 difference in this cohort is {metrics['ordinary_p95_difference_from_action']['hybrid']['c1'][2]:.4f} rad.</p>
+{table(['Candidate','Reference difference °','C0 difference m','C1 difference rad'],ordinary_rows)}
+<h3>Sharp low-speed changes remain</h3>
+<p>At below 15 mph, count adjacent valid command changes greater than 0.25 m C0 or 0.05 rad C1. These counts include source changes and recorded driver-gate transitions; they are a diagnostic proxy, not a comfort score. The hybrid is <b>not a general fix for rapid actuation</b>.</p>
+{table(['Candidate','C0 at field bound, s','C1 at field bound, s','C0 jumps','C1 jumps'],totals_rows)}
+<p>The always-stronger rule retains geometry on exits. A 50/50 blend weakens peaks and changes gentle driving. The hybrid more closely matches the requested division of work, but requires an explicit compromise when source demand drops and then recovers.</p></section>
+<section class="card"><h2>Sensitivity and validation</h2>
+<p>Twelve settings were checked on each recent route. Smaller thresholds admit more preview, including more small-action assistance. Larger release exponents clear the extra request faster but discard more entry demand and often increase abrupt target changes. The selected 0.010–0.020/m band with power 2 is a compromise, not an optimized or vehicle-validated calibration.</p>
+<details><summary>All 24 sensitivity results</summary>
+{table(['Route','Curvature band /m','Fade power','Median entry extra retained','Max release delay vs action, s','Target jumps &gt;20°'],sweep_rows)}
+<p class="note">Sensitivity alternatives replay references through the upstream curvature limiter. Full C0/C1/controller replay was run for the selected configuration, both initial and revised, and all five comparison baselines.</p></details>
+<ul><li>20 prototype tests pass, including the exit-rebound regression, direction changes, input validity, causality and repeated model frames.</li>
+<li>Native control-cycle replay checks finite C0/C1, field bounds, zero C2/C3, driver feedback suppression and identical validity gates. Real CAN packing/unpacking is checked every tenth cycle.</li>
+<li>The first six-variant replay performed 347,256 CAN checks; the revised hybrid added 57,876. Reusing unchanged baseline columns was checked against a complete fresh 15a run: every output value matched exactly.</li>
+<li>15a/15b join separately logged original action and geometry by exact model timestamp. Older geometry is reconstructed with recorded delay/model smoothing; speed uses the first consuming control sample.</li>
+<li>All candidates see the same recorded wheel motion, road inputs and driver interventions. Their feedback states are recomputed, but the vehicle never responds to the hypothetical new commands. No new camera/model inference or PSCM dynamics are simulated.</li></ul>
+<p><b>Recommendation:</b> keep this as the candidate hybrid design. It achieves the requested numeric pattern on several major turns and catches an exit flaw before deployment. Retain the action-dip and source-disagreement cases as explicit unresolved limitations. Comparison baseline: <code>01f5d5429</code>. The installed hybrid identifies itself as <code>geometry-assisted-action-feedback-v18</code>.</p>
+<p><a href="geometry-action-hybrid-validation.json">Validation numbers and source fingerprints</a></p></section>
+</main></body></html>'''
+  (dest/'geometry-action-hybrid.html').write_text(report)
+  metrics['sources_sha256'] = {str(p): hashlib.sha256(p.read_bytes()).hexdigest() for p in [
+    Path(__file__), Path('tools/ford_pscm_lab/geometry_action_hybrid.py'),
+    *[root/s/'report.json' for s in LABELS], *[root/s/'sweep.json' for s in ['15a', '15b']]]}
+  (dest/'geometry-action-hybrid-validation.json').write_text(json.dumps(metrics, indent=2, allow_nan=False)+'\n')
+  print(json.dumps({'report': str(dest/'geometry-action-hybrid.html'), 'metrics': metrics}, indent=2))
+
+
+if __name__ == '__main__':
+  parser = argparse.ArgumentParser(description=__doc__)
+  parser.add_argument('--output', type=Path, required=True)
+  parser.add_argument('--root', type=Path, default=Path('.cache/ford_hybrid_guarded'))
+  parser.add_argument('--initial-root', type=Path, default=Path('.cache/ford_hybrid'))
+  args = parser.parse_args()
+  run(args.root, args.initial_root, args.output)
@@ -0,0 +1,89 @@
+"""Reference-only sensitivity check; the selected default also has full command replay."""
+import argparse
+import json
+from pathlib import Path
+
+import numpy as np
+
+from openpilot.selfdrive.controls.lib.drive_helpers import clip_curvature
+from tools.ford_pscm_lab.geometry_action_hybrid import GeometryActionHybrid
+from tools.ford_pscm_lab.hybrid_reference_replay import load_route, sustained
+
+
+def quantiles(values):
+  return np.quantile(values, [.1, .5, .95, 1.]).tolist() if values else None
+
+
+def run(label, root):
+  d = load_route(label)
+  with np.load(root/label/'commands.npz') as z:
+    a = dict(zip(z['names'], z['rows'].T, strict=True))
+  with np.load(Path('.cache/ford_routes158_159')/f'{label}_fullrate_aligned.npz') as z:
+    scale = z['angle_scale']
+    np.testing.assert_allclose(z['t'], a['t'])
+  events = json.loads((root/label/'report.json').read_text())['events']
+  t, live = a['t'], a['valid'].astype(bool)
+  consecutive = live[1:] & live[:-1] & (np.diff(t) < .03)
+  results = []
+  curves = {'t': t}
+  for start in [.006, .01, .015]:
+    for power in [1., 2., 4., 8.]:
+      h = GeometryActionHybrid(start, 2*start, power)
+      selected = 0.
+      curve = np.zeros(len(t))
+      weights = np.zeros(len(t))
+      for i in range(len(t)):
+        target = h.update(float(d.action[i]), float(d.geometry[i]), active=bool(d.active[i] and d.valid[i]))
+        selected, _ = clip_curvature(d.cs['speed'][i], selected, target if d.active[i] else d.c['measured'][i], d.pa['roll'][i])
+        curve[i] = a['action_angle'][i]+scale[i]*(selected-a['action_reference'][i])
+        weights[i] = h.weight
+      if start == .01 and power == 2.:
+        np.testing.assert_allclose(curve[live], a['hybrid_angle'][live], atol=1e-5)
+      retained, entry_leads, exit_delays = [], [], []
+      unavailable_release = unavailable_entry = 0
+      details = []
+      for e in events:
+        sign = 1 if e['direction'] == 'left' else -1
+        window = live & (t >= e['window'][0]) & (t <= e['window'][1])
+        build = window & (t <= e['action_peak_t'])
+        release = window & (t >= e['action_peak_t'])
+        extra = sign*(a['geometry_angle']-a['action_angle'])
+        assist = build & (extra > 5.) & (sign*a['action_angle'] > 0.) & (sign*a['geometry_angle'] > 50.)
+        keep = float(np.sum(sign*(curve-a['action_angle'])[assist])/np.sum(extra[assist])) if assist.any() else None
+        if keep is not None:
+          retained.append(keep)
+        entry = sustained(t, build & (sign*curve >= 50.))
+        end = sustained(t, release & (sign*curve < 25.))
+        action_entry, action_exit = [e['variants']['action'][k] for k in ('entry_50_s', 'release_25_s')]
+        if action_entry is not None:
+          if entry is None:
+            unavailable_entry += 1
+          else:
+            entry_leads.append(action_entry-entry)
+        if action_exit is not None:
+          if end is None:
+            unavailable_release += 1
+          else:
+            exit_delays.append(end-action_exit)
+        details.append({'id': e['id'], 'retained': keep, 'entry': entry, 'release': end})
+      key = f'{start:.3f}_p{power:.0f}'
+      curves[key] = curve
+      results.append({'start': start, 'full': 2*start, 'power': power,
+                      'geometry_entry_extra_retained_p10_p50_p95_max': quantiles(retained),
+                      'entry_lead_vs_action_s_p10_p50_p95_max': quantiles(entry_leads),
+                      'release_later_than_action_s_p10_p50_p95_max': quantiles(exit_delays),
+                      'unavailable_entry_when_action_has_one': unavailable_entry,
+                      'unavailable_release_when_action_has_one': unavailable_release,
+                      'target_steps_over_20deg': int((abs(np.diff(curve)[consecutive]) > 20.).sum()),
+                      'events': details})
+  np.savez_compressed(root/label/'sweep_curves.npz', **curves)
+  (root/label/'sweep.json').write_text(json.dumps(results, indent=2, allow_nan=False)+'\n')
+  print(json.dumps({'route': label, 'settings': len(results)}), flush=True)
+
+
+if __name__ == '__main__':
+  parser = argparse.ArgumentParser(description=__doc__)
+  parser.add_argument('--route', choices=['15a', '15b'], required=True)
+  parser.add_argument('--output', type=Path, default=Path('.cache/ford_hybrid'))
+  args = parser.parse_args()
+  run(args.route, args.output)