diff --git a/docs/ford_c0_softening_v22.md b/docs/ford_c0_softening_v22.md new file mode 100644 index 0000000000..b5fd6017c0 --- /dev/null +++ b/docs/ford_c0_softening_v22.md @@ -0,0 +1,85 @@ +# Action-mode C0 feedback softening + +Route 166 used v21 and improved normal driving, but the first two user bookmarks +showed roughly 2 Hz wheel oscillations around a smoother requested angle at +17–31 mph. The first occurred with uninterrupted feedback, small I and no +PSCM limitReached. Both C0 P and C1 P followed the error, so these recordings +do not isolate either channel's contribution to physical oscillation. + +This trial changes C0 P only. With the curvature error converted into `x` metres +of C0 before applying the existing gain, the new correction is: + +```text +C0 P = gain * (x - 0.125 * tanh(x / 0.25)) +``` + +Near zero, the slope is half the old gain. It increases smoothly toward the old +gain without exceeding it. The response stays symmetric, monotonic and nonzero +for nonzero error; it introduces no time filter, deadband, cutoff or additional +state in the control law. Zero error removes P immediately, and opposite error +commands opposite P in the same cycle. + +Large corrections lose at most 0.125 m times the existing C0 gain. This is a +bound on the difference from the old correction, **not a C0 command cap**. +The field bounds remain C0 ±5.11 m and C1 ±0.5 rad. The scale and minimum slope +are explicit drive-trial choices, not an identified stable PSCM calibration. +Scales 0.15, 0.25, 0.4 and 0.5 m were compared on route 166; 0.25 m preserves +about 96% of the good left's peak entry command while reducing the repeated +C0 command component in both wobbles. + +The base action mapping, selected desired curvature, C1 P/I, delayed reference, +integral cadence and anti-windup, driver/PSCM arbitration, CAN cadence and +upstream limits are unchanged. The softening applies at all speeds in action +mode: the bookmarked wobbles were above the proposed 11.2 mph freeze cutoff. +Direct-path feedback retains its linear law; when it falls back to an action +reference, the action softening applies. The master toggle still selects +upstream Ford control when disabled. No new toggle is added. + +Diagnostics identify v22 and include `offset_proportional_linear` alongside +the actual `offset_proportional`, so a new log can show exactly what softening +removed. The extra stored scalar is diagnostic only and resets with P. + +## Validation + +Five native-time route replays cover 646,178 cycles and 64,620 CAN encode/decode +checks: routes 149, 151, 157, 162 and 166. The v21 baseline matches its archived +commands and integral exactly. C1 commands, C1 P/I, reference, validity, +arbitration and base mapping remain identical on every cycle. C2/C3 remain +zero. The largest raw C0 P change is 0.125 m; after CAN quantization the +command difference is at most 0.13 m. + +| Route-166 event | Repeating total C0 component, before → after | Reduction | +| --- | --- | --- | +| First wobble | 0.146 → 0.076 m peak-to-peak | 48% | +| Second wobble | 0.222 → 0.150 m peak-to-peak | 32% | +| Second wobble exit | 0.335 → 0.199 m peak-to-peak | 41% | + +These use a same-frequency sine fit with quadratic trend removal. They measure +the **command**, not a predicted reduction in wheel oscillation. The good left's +peak entry C0 changes from -2.93 to -2.81 m; its mid-turn unwind peak changes +from +1.44 to +1.31 m. C1 is identical. Entry and unwind are both softened; +neither physical response is proven better by a frozen-motion replay. + +Regression tests first failed against v21, then passed with v22. They exercise +small nonzero corrections, preserved large-error authority, monotonic bounded +incremental gain, immediate reversal/release, unaffected C1 and base mapping, +and the direct-path exception. Existing controls-to-CAN integration, model +selection, driver override, request-history timing and default-upstream tests +also pass. Exact counts and provenance are in +`ford_c0_softening_v22_validation.json`. + +Reproduce a route comparison with the built Python dependencies: + +```sh +PYTHONPATH=.:opendbc_repo:.cache/ford_geometry_deps python \ + tools/ford_pscm_lab/c0_softening_replay.py \ + --source .cache/ford_route166/full \ + --delay-intake .cache/ford_route166/intake.npz \ + --archive .cache/ford_feedback_delay_v21/166/commands.npz \ + --output .cache/ford_c0_softening_v22/166 +``` + +For the next drive, the discriminating observations are whether small +left-right corrections settle sooner, whether centering becomes too loose, +and whether strong entry and prompt release remain. Do not interpret this +offline validation as a demonstrated physical stability fix. diff --git a/docs/ford_c0_softening_v22_validation.json b/docs/ford_c0_softening_v22_validation.json new file mode 100644 index 0000000000..857d207d12 --- /dev/null +++ b/docs/ford_c0_softening_v22_validation.json @@ -0,0 +1,415 @@ +{ + "baseline_commit": "ed9c44f57584b5f033f79db6331e21b9b3e0ad55", + "method": "Native-time production-controller replay using recorded wheel motion. Tests command behavior, not changed physical tracking.", + "cycles": 646178, + "wire_checks": 64620, + "tests": { + "ford_controller_and_integration": 708, + "subtests": 2, + "ford_can": 11, + "ruff": "passed", + "diff_check": "passed", + "ford_can_subtests": 23 + }, + "selection": { + "0.15": { + "wobble1": { + "P_periodic_reduction_pct": 43.471957098342315, + "P_before": 0.1502831327602382, + "P_after": 0.08495211376066261 + }, + "wobble2": { + "P_periodic_reduction_pct": 20.901758921591217, + "P_before": 0.22856483271408398, + "P_after": 0.1807907624006479 + }, + "wobble2_exit": { + "P_periodic_reduction_pct": 31.59900962734852, + "P_before": 0.3407619177930598, + "P_after": 0.2330845265832934 + }, + "left_entry_peak": { + "t": 557.084304569, + "before": -2.9300000000000006, + "after_unpacked": -2.8568021983086886, + "retained_percent": 97.50178151224192 + } + }, + "0.25": { + "wobble1": { + "P_periodic_reduction_pct": 47.14718986588794, + "P_before": 0.1502831327602382, + "P_after": 0.07942885882136426 + }, + "wobble2": { + "P_periodic_reduction_pct": 31.648695347911747, + "P_before": 0.22856483271408398, + "P_after": 0.1562270451359394 + }, + "wobble2_exit": { + "P_periodic_reduction_pct": 40.10137585312036, + "P_before": 0.3407619177930598, + "P_after": 0.2041117003745639 + }, + "left_entry_peak": { + "t": 557.084304569, + "before": -2.9300000000000006, + "after_unpacked": -2.8068022480277577, + "retained_percent": 95.79529856750024 + } + }, + "0.4": { + "wobble1": { + "P_periodic_reduction_pct": 48.78814118850091, + "P_before": 0.1502831327602382, + "P_after": 0.07696278576667091 + }, + "wobble2": { + "P_periodic_reduction_pct": 40.16673505205207, + "P_before": 0.22856483271408398, + "P_after": 0.13675780193565182 + }, + "wobble2_exit": { + "P_periodic_reduction_pct": 45.26729702228269, + "P_before": 0.3407619177930598, + "P_after": 0.18650820832684864 + }, + "left_entry_peak": { + "t": 557.084304569, + "before": -2.9300000000000006, + "after_unpacked": -2.7318281193111016, + "retained_percent": 93.23645458399662 + } + }, + "0.5": { + "wobble1": { + "P_periodic_reduction_pct": 49.20782335256252, + "P_before": 0.1502831327602382, + "P_after": 0.07633207426288317 + }, + "wobble2": { + "P_periodic_reduction_pct": 43.09149572817541, + "P_before": 0.22856483271408398, + "P_after": 0.1300728275889832 + }, + "wobble2_exit": { + "P_periodic_reduction_pct": 46.79902581397264, + "P_before": 0.3407619177930598, + "P_after": 0.18128865992089754 + }, + "left_entry_peak": { + "t": 557.084304569, + "before": -2.9300000000000006, + "after_unpacked": -2.682025078944252, + "retained_percent": 91.53669211413828 + } + } + }, + "bookmarks": { + "wobble1": { + "window": [ + 420.8, + 424.0 + ], + "p": { + "old_peak_to_peak": 0.1502831327602382, + "new_peak_to_peak": 0.07942885882136426, + "reduction_percent": 47.14718986588794 + }, + "c0": { + "old_peak_to_peak": 0.1463643509329174, + "new_peak_to_peak": 0.07619232987207852, + "reduction_percent": 47.943382807061084 + } + }, + "wobble2": { + "window": [ + 514.3, + 521.4 + ], + "p": { + "old_peak_to_peak": 0.22856483271408398, + "new_peak_to_peak": 0.15622704513593932, + "reduction_percent": 31.648695347911794 + }, + "c0": { + "old_peak_to_peak": 0.22189262078364064, + "new_peak_to_peak": 0.15013115100291197, + "reduction_percent": 32.3406292319656 + } + }, + "wobble2_exit": { + "window": [ + 523.7, + 526.8 + ], + "p": { + "old_peak_to_peak": 0.3407619177930598, + "new_peak_to_peak": 0.2041117003745639, + "reduction_percent": 40.10137585312036 + }, + "c0": { + "old_peak_to_peak": 0.3347422828817533, + "new_peak_to_peak": 0.19869356211391034, + "reduction_percent": 40.642825159886286 + } + } + }, + "good_left_points": [ + { + "meaning": "entry", + "t": 557.084304569, + "target_angle": 167.83670043945312, + "actual_angle": 76.5999984741211, + "old_c0": -2.9300000000000006, + "new_c0": -2.8100000000000005, + "old_p": -1.9288202876424623, + "new_p": -1.8038203373625472, + "c1": -0.4565, + "i": -0.058477505092137665 + }, + { + "meaning": "midturn_unwind", + "t": 558.284113233, + "target_angle": 109.08927154541016, + "actual_angle": 183.6999969482422, + "old_c0": 1.44, + "new_c0": 1.31, + "old_p": 2.0905105735349747, + "new_p": 1.9655105871733405, + "c1": -0.11499999999999999, + "i": -0.05363273646512535 + } + ], + "routes": { + "166": { + "cycles": 62579, + "wire_checks": 6258, + "baseline_matches_archived_v21_exactly": true, + "c1_integral_reference_arbitration_equal_every_cycle": true, + "P_reduction_p50_p95_p99_max_m": [ + 0.003636227080371235, + 0.10752626130792256, + 0.12490297403763004, + 0.12499999961974328 + ], + "C0_change_p50_p95_p99_max_m": [ + 0.0, + 0.10999999999999943, + 0.1299999999999999, + 0.13000000000000078 + ], + "variants": { + "old": { + "c0_step_p95_p99_max": [ + 0.020000000000000018, + 0.050000000000000266, + 1.8900000000000006 + ], + "c0_bound_seconds": 0.0 + }, + "new": { + "c0_step_p95_p99_max": [ + 0.010000000000000675, + 0.040000000000000036, + 1.7700000000000005 + ], + "c0_bound_seconds": 0.0 + } + }, + "baseline_commit": "ed9c44f57584b5f033f79db6331e21b9b3e0ad55", + "method": "Replay C0 softening against v21 on frozen recorded motion, not a PSCM model.", + "controller_sha256": "b0aff6d70ad8ae7693c69068dbe646767e4d9fc20a23af70cbddcf6ebf849f48", + "source_sha256": { + ".cache/ford_route166/full/route.npz": "0ce273e4a12725c4481b11dab38b304950d364455b89f216b929a9c61b7feb1c", + ".cache/ford_route166/full/model_paths.npz": "720fe9ba9865a05937989449b0b0bd3304baf5f1730042da6b61adc2941c0f5a", + ".cache/ford_route166/intake.npz": "660b5e7f6789d931660ad71b10f7d8ce2b63436e7573b615ccc8e5da1f2a14d0", + ".cache/ford_feedback_delay_v21/166/commands.npz": "bfe51abd9e914edea505efc4fb42fe66950cfa5835e4f929d922992e90c88b5b" + } + }, + "162": { + "cycles": 49003, + "wire_checks": 4901, + "baseline_matches_archived_v21_exactly": true, + "c1_integral_reference_arbitration_equal_every_cycle": true, + "P_reduction_p50_p95_p99_max_m": [ + 0.01864462197101791, + 0.12428797325001033, + 0.12499916992638829, + 0.12499999992682431 + ], + "C0_change_p50_p95_p99_max_m": [ + 0.019999999999999574, + 0.1200000000000001, + 0.1299999999999999, + 0.13000000000000078 + ], + "variants": { + "old": { + "c0_step_p95_p99_max": [ + 0.03000000000000025, + 0.09999999999999964, + 2.7299999999999995 + ], + "c0_bound_seconds": 0.0 + }, + "new": { + "c0_step_p95_p99_max": [ + 0.03000000000000025, + 0.08999999999999986, + 2.5999999999999996 + ], + "c0_bound_seconds": 0.0 + } + }, + "baseline_commit": "ed9c44f57584b5f033f79db6331e21b9b3e0ad55", + "method": "Replay C0 softening against v21 on frozen recorded motion, not a PSCM model.", + "controller_sha256": "b0aff6d70ad8ae7693c69068dbe646767e4d9fc20a23af70cbddcf6ebf849f48", + "source_sha256": { + ".cache/ford_route162/full/route.npz": "a725f9ca309b90a2d750d51a9b9d5b459d99119a54ab048631bc0fce07348af8", + ".cache/ford_route162/full/model_paths.npz": "cffe310b122d9ae26a53ee504608156b4242a6bba5f681898d5b661951ac5ce5", + ".cache/ford_route162/intake.npz": "9455b01e1d1d3dcba08276b382a8c6cd54ff6fb19cb417b72d0ac89cf193034b", + ".cache/ford_feedback_delay_v21/162/commands.npz": "6c70518fb9b7232b689740662c4bd5c0c24cabee4e743a2b4f3b2313bcce1912" + } + }, + "157": { + "cycles": 76554, + "wire_checks": 7656, + "baseline_matches_archived_v21_exactly": true, + "c1_integral_reference_arbitration_equal_every_cycle": true, + "P_reduction_p50_p95_p99_max_m": [ + 0.00530841320802854, + 0.12371750128642126, + 0.12499999318487974, + 0.125 + ], + "C0_change_p50_p95_p99_max_m": [ + 0.009999999999999787, + 0.1200000000000001, + 0.1299999999999999, + 0.13000000000000078 + ], + "variants": { + "old": { + "c0_step_p95_p99_max": [ + 0.03000000000000025, + 0.09000000000000014, + 5.6000000000000005 + ], + "c0_bound_seconds": 0.2999506119999751 + }, + "new": { + "c0_step_p95_p99_max": [ + 0.02999999999999936, + 0.08000000000000007, + 5.6000000000000005 + ], + "c0_bound_seconds": 0.15561390599987135 + } + }, + "baseline_commit": "ed9c44f57584b5f033f79db6331e21b9b3e0ad55", + "method": "Replay C0 softening against v21 on frozen recorded motion, not a PSCM model.", + "controller_sha256": "b0aff6d70ad8ae7693c69068dbe646767e4d9fc20a23af70cbddcf6ebf849f48", + "source_sha256": { + ".cache/ford_route157/full/route.npz": "e2e2573f904aa11ee9e10450e7f5b965d475657b61127e827a67eadcd6b857fb", + ".cache/ford_route157/full/model_paths.npz": "25fc2526e092fde5ceb5aab63a5aa4f73a2c989a3263cedb0a26faf8da61c47b", + ".cache/ford_route157/intake.npz": "5dffd86fde68197727d6c6b9c5eae82320a456fa1206c4cf18e5e9c69419dde4", + ".cache/ford_feedback_delay_v21/157/commands.npz": "7d7659c54662e7f56015313e634b959be29dae524522b425f4c2adccc391fc9d" + } + }, + "151": { + "cycles": 325708, + "wire_checks": 32571, + "baseline_matches_archived_v21_exactly": true, + "c1_integral_reference_arbitration_equal_every_cycle": true, + "P_reduction_p50_p95_p99_max_m": [ + 0.004569097471431923, + 0.07115894622455567, + 0.12458615764509068, + 0.12499999999990274 + ], + "C0_change_p50_p95_p99_max_m": [ + 0.009999999999999787, + 0.07000000000000028, + 0.1200000000000001, + 0.13000000000000078 + ], + "variants": { + "old": { + "c0_step_p95_p99_max": [ + 0.010000000000000675, + 0.040000000000000036, + 3.45 + ], + "c0_bound_seconds": 0.0 + }, + "new": { + "c0_step_p95_p99_max": [ + 0.010000000000000231, + 0.03000000000000025, + 3.33 + ], + "c0_bound_seconds": 0.0 + } + }, + "baseline_commit": "ed9c44f57584b5f033f79db6331e21b9b3e0ad55", + "method": "Replay C0 softening against v21 on frozen recorded motion, not a PSCM model.", + "controller_sha256": "b0aff6d70ad8ae7693c69068dbe646767e4d9fc20a23af70cbddcf6ebf849f48", + "source_sha256": { + ".cache/ford_route151/full/route.npz": "41a5b8bd388cf5a3d553f784542376ac9355fcdc5be4f427053d0504537babe1", + ".cache/ford_route151/full/model_paths.npz": "980b3843cec04254280d744a5801cee1bf0f8ef371052398327ff245f9eee01b", + ".cache/ford_route151/intake.npz": "18bbefb7738cebd0071dc987e90f74469a0c8ed456f9412324030592cafd68c9", + ".cache/ford_feedback_delay_v21/151/commands.npz": "4ab25e4c31fcddf08184c396efc3db78953d5ce60da7c653964bc2fa7e3184c9" + } + }, + "149": { + "cycles": 132334, + "wire_checks": 13234, + "baseline_matches_archived_v21_exactly": true, + "c1_integral_reference_arbitration_equal_every_cycle": true, + "P_reduction_p50_p95_p99_max_m": [ + 0.010345416404636773, + 0.12405821661525897, + 0.12499991437968122, + 0.12499999999999556 + ], + "C0_change_p50_p95_p99_max_m": [ + 0.009999999999999787, + 0.1200000000000001, + 0.1299999999999999, + 0.13000000000000078 + ], + "variants": { + "old": { + "c0_step_p95_p99_max": [ + 0.03000000000000025, + 0.08999999999999986, + 3.87 + ], + "c0_bound_seconds": 0.0 + }, + "new": { + "c0_step_p95_p99_max": [ + 0.020000000000000462, + 0.08000000000000007, + 3.750000000000001 + ], + "c0_bound_seconds": 0.0 + } + }, + "baseline_commit": "ed9c44f57584b5f033f79db6331e21b9b3e0ad55", + "method": "Replay C0 softening against v21 on frozen recorded motion, not a PSCM model.", + "controller_sha256": "b0aff6d70ad8ae7693c69068dbe646767e4d9fc20a23af70cbddcf6ebf849f48", + "source_sha256": { + ".cache/ford_route149/full/route.npz": "aa5902877343cd033ee286b3668d91a85336ffbf740861849fb0b76b0ca24ade", + ".cache/ford_route149/full/model_paths.npz": "19827800b8fb5983f3d6b72fcfaf36e35a40170bb17cd7c1e49374744fb449fb", + ".cache/ford_route149/intake.npz": "503919c4f1566ef850007c59f16ce074697ba57732f21beb7b158c1ca7be3a9d", + ".cache/ford_feedback_delay_v21/149/commands.npz": "15c4ad68338a921cd6b2cd22d0b3d6b5b8637614dacbe61f954a3a6d7cb8d8d9" + } + } + }, + "code_sha256": { + "openpilot/selfdrive/controls/lib/ford_model_action.py": "b0aff6d70ad8ae7693c69068dbe646767e4d9fc20a23af70cbddcf6ebf849f48", + "tools/ford_pscm_lab/c0_softening_replay.py": "43e185871e5b9875cee5288160d344fe4d606c4bdf63dd719fb6a6cd9404307e" + } +} diff --git a/openpilot/selfdrive/controls/lib/ford_model_action.py b/openpilot/selfdrive/controls/lib/ford_model_action.py index c86b85261c..fd9271ff3e 100644 --- a/openpilot/selfdrive/controls/lib/ford_model_action.py +++ b/openpilot/selfdrive/controls/lib/ford_model_action.py @@ -1,7 +1,8 @@ """Opt-in Ford C2-free model mapping with measured-curvature PI feedback. C0 samples a desired-curvature arc at 7 m, optionally max(7 m, v*1s), -including base-heading overflow, plus proportional tracking correction. C1 +including base-heading overflow, plus proportional tracking correction softened +near zero error without filtering or a deadband. C1 combines the selected curvature's heading with proportional and integrated tracking error. Reference distance and gains are explicit trial choices. Commands use the current bounded request without an additional C0/C1 slew. @@ -26,6 +27,10 @@ C1_PROPORTIONAL_GAIN = 0.75 # Drive-trial gains, not a learned calibration. C1_INTEGRAL_GAIN = 1.0 C0_PROPORTIONAL_GAIN = 1.0 # Action trial: stronger immediate correction for the same tracking error. DIRECT_PATH_C0_PROPORTIONAL_GAIN = 0.5 +# Action-only trial: soften the error expressed as metres of C0 before gain. +# The slope grows smoothly from 0.5 to 1; correction reduction is at most 0.125 m * gain. +C0_SMALL_ERROR_GAIN = 0.5 +C0_SMALL_ERROR_SCALE_M = 0.25 # Offline Lightning fit: geometric curvature per metre of C0, with v in m/s. C0_RESPONSE_CONSTANT = 0.010717679293424373 C0_RESPONSE_INVERSE_SPEED_SQUARED = 0.018122981795212647 @@ -96,7 +101,7 @@ class ModelActionController: Freshness, measurement cadence and driver/PSCM arbitration belong to the caller. """ __slots__ = ('c0', 'c1', 'correction', 'proportional_gain', 'integral_gain', 'proportional', 'feedback_curvature', 'c0_time_based', - 'c0_proportional_gain', 'offset_proportional', 'offset_reference') + 'c0_proportional_gain', 'offset_proportional', 'offset_proportional_linear', 'offset_reference') def __init__(self, proportional_gain=C1_PROPORTIONAL_GAIN, integral_gain=C1_INTEGRAL_GAIN, *, c0_time_based=False, c0_proportional_gain=C0_PROPORTIONAL_GAIN): @@ -109,7 +114,7 @@ class ModelActionController: def reset(self): self.c0 = self.c1 = self.correction = self.proportional = self.feedback_curvature = 0. - self.offset_proportional = 0. + self.offset_proportional = self.offset_proportional_linear = 0. self.offset_reference = None def update(self, model, desired_curvature, *, current_curvature, speed, dt, active=True, valid=True, @@ -147,10 +152,17 @@ class ModelActionController: # Road-curvature error -> geometric steering error -> metres of C0. # This is proportional only: nothing is accumulated or carried into a release. response = C0_RESPONSE_CONSTANT+C0_RESPONSE_INVERSE_SPEED_SQUARED/max(speed, 1.34)**2 - self.offset_proportional = self.c0_proportional_gain*error*curvature_scale/response if feedback_enabled else 0. + offset_error = error*curvature_scale/response + self.offset_proportional_linear = self.c0_proportional_gain*error*curvature_scale/response if feedback_enabled else 0. + self.offset_proportional = self.offset_proportional_linear if not _finite(self.proportional, self.offset_proportional): self.reset() return FordPath() + if feedback_enabled and not direct_path: + # Unlike blending gains back to 1, this never introduces a slope above the + # original gain. Large corrections lose only a bounded amount, not a fraction. + self.offset_proportional -= self.c0_proportional_gain*(1.-C0_SMALL_ERROR_GAIN)*C0_SMALL_ERROR_SCALE_M*math.tanh( + offset_error/C0_SMALL_ERROR_SCALE_M) base = float(np.clip(target.path_angle, -.5, .5)) offset = float(np.clip(target.path_offset+OFFSET_STATION_M*(target.path_angle-base), -5.11, 5.11)) self.c0 = float(np.clip(offset+self.offset_proportional, -5.11, 5.11)) @@ -198,8 +210,8 @@ class FordModelActionController: self.direct_path = bool(direct_path) self.request_buffer_size = int(CURVATURE_REQUEST_BUFFER_SECONDS / DT_CTRL) self.request_buffer = deque([0.] * self.request_buffer_size, maxlen=self.request_buffer_size) - self.hypothesis = ('model-path-direct-feedback-v21-delayed-feedback' if self.direct_path else - 'model-action-curvature-c0-feedback-v21-delayed-feedback') + self.hypothesis = ('model-path-direct-feedback-v22-soft-c0' if self.direct_path else + 'model-action-curvature-c0-feedback-v22-soft-c0') self.reset() def path_curvature(self, model, speed): @@ -288,6 +300,7 @@ class FordModelActionController: 'heading_feedforward': base_heading, 'offset_overflow': OFFSET_STATION_M*(raw_heading-base_heading), 'offset_proportional': self.core.offset_proportional, 'c0_proportional_gain': self.core.c0_proportional_gain, + 'offset_proportional_linear': self.core.offset_proportional_linear, 'curvature_scale': curvature_scale, 'heading_correction': self.core.correction, 'feedback_enabled': feedback_enabled, 'heading_proportional': self.core.proportional, 'proportional_gain': self.core.proportional_gain, diff --git a/openpilot/selfdrive/controls/tests/test_ford_c0_softening.py b/openpilot/selfdrive/controls/tests/test_ford_c0_softening.py new file mode 100644 index 0000000000..0a7b14139a --- /dev/null +++ b/openpilot/selfdrive/controls/tests/test_ford_c0_softening.py @@ -0,0 +1,83 @@ +"""C0 trial command properties; these tests do not simulate PSCM stability.""" +import math + +import numpy as np +import pytest + +from openpilot.selfdrive.controls.lib.ford_model_action import ( + C0_RESPONSE_CONSTANT, C0_RESPONSE_INVERSE_SPEED_SQUARED, ModelActionController, +) +from openpilot.selfdrive.controls.tests.test_ford_model_action import straight + + +def correction_for_linear_request(controller, request, **overrides): + # Choose a curvature mismatch which previously produced `request` metres of P. + response = C0_RESPONSE_CONSTANT+C0_RESPONSE_INVERSE_SPEED_SQUARED/100. + controller.update(straight(), 0., current_curvature=-request*response, speed=10., dt=.01, feedback_dt=0., **overrides) + return controller.offset_proportional + + +@pytest.mark.parametrize('sign', [-1., 1.]) +def test_small_c0_correction_is_gentler_but_has_no_deadband(sign): + core = ModelActionController() + for magnitude in [1e-9, .001, .01]: + correction = correction_for_linear_request(core, sign*magnitude) + assert .4999*magnitude <= sign*correction <= .501*magnitude + assert correction_for_linear_request(core, 0.) == 0. + + +@pytest.mark.parametrize('sign', [-1., 1.]) +def test_large_entry_and_release_corrections_keep_strength(sign): + core = ModelActionController() + for magnitude in [1., 2., 5.]: + correction = correction_for_linear_request(core, sign*magnitude) + assert 0. < magnitude-sign*correction <= .125000001 + if magnitude >= 2.: + assert sign*correction >= .9375*magnitude + # No filter history or threshold state delays a reversal or a release. + correction_for_linear_request(core, sign*2.) + assert correction_for_linear_request(core, -sign*.01)*sign < 0. + assert correction_for_linear_request(core, 0.) == 0. + + +def test_softening_never_adds_a_transition_with_higher_incremental_gain(): + core = ModelActionController() + inputs = np.linspace(-2., 2., 2001) + values = np.array([correction_for_linear_request(core, value) for value in inputs]) + slopes = np.diff(values)/np.diff(inputs) + assert .499999 <= slopes.min() < .501 + assert slopes.max() <= 1.000001 + np.testing.assert_allclose(values, -values[::-1], atol=1e-12) + assert np.all(abs(values) <= abs(inputs)+1e-12) + + +def test_shaping_c0_does_not_change_c1_or_integration(): + core = ModelActionController() + without_c0 = ModelActionController(c0_proportional_gain=0.) + model = straight() + for i in range(600): + desired = .04*math.sin(i*.017) + args = {'current_curvature': .025*math.sin((i-16)*.017), 'speed': 10., 'dt': .01, + 'feedback_enabled': i % 29 != 0, 'pscm_limited': i % 17 == 0} + first, second = [c.update(model, desired, **args) for c in (core, without_c0)] + assert first.path_angle == second.path_angle + assert core.correction == without_c0.correction + assert core.proportional == without_c0.proportional + assert first.curvature == first.curvature_rate == 0. + + +@pytest.mark.parametrize('direct_path', [False, True]) +def test_softening_does_not_modify_the_base_or_override_behavior(direct_path): + core = ModelActionController() + without_c0 = ModelActionController(c0_proportional_gain=0.) + for feedback_enabled, desired, measured in [(True, .01, .01), (False, .01, 0.), (True, 0., 0.)]: + args = {'current_curvature': measured, 'speed': 10., 'dt': .01, + 'feedback_enabled': feedback_enabled, 'direct_path': direct_path} + assert core.update(straight(), desired, **args) == without_c0.update(straight(), desired, **args) + assert core.offset_proportional == 0. + + +def test_direct_path_trial_keeps_its_existing_linear_feedback(): + core = ModelActionController() + for request in [-2., -.01, 0., .01, 2.]: + assert correction_for_linear_request(core, request, direct_path=True) == pytest.approx(request) diff --git a/openpilot/selfdrive/controls/tests/test_ford_controlsd_logging.py b/openpilot/selfdrive/controls/tests/test_ford_controlsd_logging.py index dcfc95dfe3..fab39025a8 100644 --- a/openpilot/selfdrive/controls/tests/test_ford_controlsd_logging.py +++ b/openpilot/selfdrive/controls/tests/test_ford_controlsd_logging.py @@ -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-feedback-v21-delayed-feedback') + self.assertEqual(record['hypothesis'], 'model-action-curvature-c0-feedback-v22-soft-c0') self.assertIs(record['calibration_approved'], False) self.assertEqual(record['command'][2:], [0., 0.]) self.assertEqual(record['status'], controller.diagnostics['status']) diff --git a/openpilot/selfdrive/controls/tests/test_ford_model_action_adapter.py b/openpilot/selfdrive/controls/tests/test_ford_model_action_adapter.py index 4338909df8..6c3a04547e 100644 --- a/openpilot/selfdrive/controls/tests/test_ford_model_action_adapter.py +++ b/openpilot/selfdrive/controls/tests/test_ford_model_action_adapter.py @@ -196,7 +196,7 @@ def test_actual_controlsd_selection_limiting_publication_and_downstream_can(pipe assert controller.core.proportional == pytest.approx(.75*20.*expected_curvature) assert controller.core.correction == 0. # First measurement has no elapsed feedback time. assert controls.ford_path.path_angle == pytest.approx((-1 if maneuver else 1)*.0045) - assert controls.ford_path.path_offset == pytest.approx((-1 if maneuver else 1)*.02) + assert controls.ford_path.path_offset == pytest.approx((-1 if maneuver else 1)*.01) assert controller.core.offset_proportional*expected_curvature > 0. assert cc.latActive and cc.actuators.curvature == 0. assert controller.diagnostics['reference_age'] == pytest.approx(.01 if maneuver else .02) @@ -412,7 +412,7 @@ def test_continuous_pi_reversal_through_selected_limited_request_and_actual_can( assert wire['LatCtlPath_No_Cs'] == calculate_lat_ctl2_checksum(2, frame % 16, packet[1]) if frame == 199: assert sign*core.correction < 0. if same_turn else sign*core.correction > 0. - assert controls.ford_path_controller.diagnostics['hypothesis'] == 'model-action-curvature-c0-feedback-v21-delayed-feedback' + assert controls.ford_path_controller.diagnostics['hypothesis'] == 'model-action-curvature-c0-feedback-v22-soft-c0' if same_turn: assert controls.desired_curvature == pytest.approx(sign*.01) assert sign*controls.ford_path.path_angle >= speed*.01 # No old unwind correction left below the new base. diff --git a/openpilot/selfdrive/controls/tests/test_ford_model_action_c0_feedback.py b/openpilot/selfdrive/controls/tests/test_ford_model_action_c0_feedback.py index a5f9e16411..0612fc1c07 100644 --- a/openpilot/selfdrive/controls/tests/test_ford_model_action_c0_feedback.py +++ b/openpilot/selfdrive/controls/tests/test_ford_model_action_c0_feedback.py @@ -90,7 +90,8 @@ def test_c0_response_units_and_explicit_gain(): core.update(straight(), .01, current_curvature=0., speed=5., dt=.01, curvature_scale=1.2) # Fitted C0 gain is geometric curvature per metre of command, not road curvature. response = .010717679293424373+.018122981795212647/25. - assert core.offset_proportional == pytest.approx(.5*.01*1.2/response) + assert core.offset_proportional_linear == pytest.approx(.5*.01*1.2/response) + assert core.offset_proportional_linear-.0625 < core.offset_proportional < core.offset_proportional_linear @pytest.mark.parametrize('speed', [1., 5., 20., 40.]) diff --git a/openpilot/selfdrive/controls/tests/test_ford_model_action_selection.py b/openpilot/selfdrive/controls/tests/test_ford_model_action_selection.py index 9c7d5d127c..b8dac21265 100644 --- a/openpilot/selfdrive/controls/tests/test_ford_model_action_selection.py +++ b/openpilot/selfdrive/controls/tests/test_ford_model_action_selection.py @@ -53,7 +53,7 @@ def test_actual_startup_priority(candidate, observer, fingerprint): assert selected.ford_path_controller.core.proportional_gain == C1_PROPORTIONAL_GAIN == .75 assert selected.ford_path_controller.core.integral_gain == C1_INTEGRAL_GAIN == 1. assert selected.ford_path_controller.core.c0_proportional_gain == 1. - assert selected.ford_path_controller.diagnostics['hypothesis'] == 'model-action-curvature-c0-feedback-v21-delayed-feedback' + assert selected.ford_path_controller.diagnostics['hypothesis'] == 'model-action-curvature-c0-feedback-v22-soft-c0' else: assert selected.ford_path_controller is None assert selected.ford_model_action == candidate diff --git a/tools/ford_pscm_lab/c0_softening_replay.py b/tools/ford_pscm_lab/c0_softening_replay.py new file mode 100644 index 0000000000..2b1dd8fea4 --- /dev/null +++ b/tools/ford_pscm_lab/c0_softening_replay.py @@ -0,0 +1,115 @@ +"""Replay C0 softening against v21 on frozen recorded motion, not a PSCM model.""" +import argparse +import hashlib +import json +from pathlib import Path +import subprocess +from types import SimpleNamespace + +import numpy as np + +from opendbc.car.vehicle_model import VehicleModel +from openpilot.selfdrive.controls.lib.ford_model_action import FordModelActionController +from tools.ford_pscm_lab.model_action_replay import WireCheck, sample, table + + +BASELINE = 'ed9c44f57584b5f033f79db6331e21b9b3e0ad55' +CONTROLLER = 'openpilot/selfdrive/controls/lib/ford_model_action.py' + + +def replay(source, intake, archive, destination): + 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')} + with np.load(source/'model_paths.npz') as z: + paths, path_ns = z['paths'], z['ns'] + with np.load(intake) as z: + delay = z['delay'] + with np.load(archive) as z: + archived = dict(zip(z['names'], z['rows'].T, strict=True)) + baseline_source = subprocess.check_output(['git', 'show', f'{BASELINE}:{CONTROLLER}'], text=True) + namespace = {'__name__': 'v21_ford_controller'} + exec(compile(baseline_source, '', 'exec'), namespace) + old, new = namespace['FordModelActionController'](), FordModelActionController() + c, cp = r['controls'], meta['car'][0] + q = c['t'] + cs, pa, ps = [sample(r[k], q) for k in ('cs', 'params', 'pscm')] + cc, sent = [sample(r[k], q, nearest=True) for k in ('cc', 'path')] + mi = np.clip(np.searchsorted(path_ns, c['model_ns']), 0, len(path_ns)-1) + exact = path_ns[mi] == c['model_ns'] + models = [SimpleNamespace(position=SimpleNamespace(x=p[0], y=p[1]), orientation=SimpleNamespace(z=p[2])) for p in paths] + delays = sample({'t': delay[:, 0], 'value': delay[:, 3]}, q)['value'] + delays[q < delay[0, 0]] = 0. + params = SimpleNamespace(**{k: cp[k] for k in ('mass', 'wheelbase', 'centerToFront', 'steerRatioRear', 'tireStiffnessFront', 'tireStiffnessRear')}, + steerRatio=cp['steer_ratio'], rotationalInertia=0.) + vm = VehicleModel(params) + wire = WireCheck() + names = ['t', 'valid', 'speed', 'desired', 'measured', 'reference', 'target_angle', 'actual_angle', + 'old_c0', 'new_c0', 'old_p', 'new_p', 'c1', 'c1_p', 'i', 'feedback', 'pressed', 'torque', 'pscm_limit', + 'recorded_c0', 'recorded_c1'] + rows = np.zeros((len(q), len(names))) + for i, now in enumerate(q): + 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.)/(params.steerRatio*params.wheelbase) + error = c['desired'][i]-c['measured'][i] + if abs(error) > 1e-5: + scale = -np.radians(c['desired_angle'][i]-c['actual_angle'][i])/error/(params.steerRatio*params.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])) + args = {'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(cc['active'][i]), 'valid': bool(c['valid'][i] and cs['valid'][i] and cs['can_valid'][i] and pa['valid'][i] and exact[i]), + 'driver_pressed': bool(cs['pressed'][i]), 'driver_torque': cs['torque'][i], 'pscm_status': status, + 'curvature_scale': scale, 'lat_delay': delays[i]} + before, after = [core.update(models[mi[i]], c['desired'][i], **args) for core in (old, new)] + assert before.valid == after.valid + assert before.path_angle == after.path_angle and old.core.correction == new.core.correction + assert old.core.proportional == new.core.proportional and old.core.feedback_curvature == new.core.feedback_curvature + assert old.core.offset_proportional == new.core.offset_proportional_linear + assert abs(new.core.offset_proportional) <= abs(old.core.offset_proportional)+1e-12 + assert new.core.offset_proportional*old.core.offset_proportional >= 0. + assert abs(new.core.offset_proportional-old.core.offset_proportional) <= .125000001 + assert abs(before.path_offset-after.path_offset) <= .130000001 + assert abs(after.path_offset) <= 5.110000001 and abs(after.path_angle) <= .500000001 + assert after.curvature == after.curvature_rate == 0. + for key in ('feedback_enabled', 'driver_override', 'pscm_limited', 'heading_feedforward', 'offset_overflow', 'feedback_delay'): + assert old.diagnostics.get(key) == new.diagnostics.get(key) + assert before.path_offset == archived['new_c0'][i] and before.path_angle == archived['new_c1'][i] + assert old.core.correction == archived['new_i'][i] + if not new.diagnostics.get('feedback_enabled', False): + assert new.core.offset_proportional == new.core.proportional == new.core.correction == 0. + assert before == after + if i % 10 == 0: + wire.check(after) + rows[i] = [now-meta['t0'], after.valid, cs['speed'][i], c['desired'][i], c['measured'][i], new.core.feedback_curvature, + c['desired_angle'][i], c['actual_angle'][i], before.path_offset, after.path_offset, + old.core.offset_proportional, new.core.offset_proportional, after.path_angle, new.core.proportional, new.core.correction, + new.diagnostics.get('feedback_enabled', False), cs['pressed'][i], cs['torque'][i], ps['limit'][i], sent['c0'][i], sent['c1'][i]] + a = dict(zip(names, rows.T, strict=True)) + valid = a['valid'].astype(bool) + dt = np.minimum(np.diff(a['t'], append=a['t'][-1]+.01), .03) + paired = valid[1:] & valid[:-1] & (np.diff(a['t']) < .03) + metrics = {'cycles': len(q), 'wire_checks': wire.count, 'baseline_matches_archived_v21_exactly': True, + 'c1_integral_reference_arbitration_equal_every_cycle': True, + 'P_reduction_p50_p95_p99_max_m': np.quantile(abs(a['old_p'][valid]-a['new_p'][valid]), [.5, .95, .99, 1]).tolist(), + 'C0_change_p50_p95_p99_max_m': np.quantile(abs(a['old_c0'][valid]-a['new_c0'][valid]), [.5, .95, .99, 1]).tolist(), + 'variants': {}, 'baseline_commit': BASELINE, 'method': __doc__} + for prefix in ('old', 'new'): + metrics['variants'][prefix] = {'c0_step_p95_p99_max': np.quantile(abs(np.diff(a[prefix+'_c0'])[paired]), [.95, .99, 1]).tolist(), + 'c0_bound_seconds': float(dt[valid & (abs(a[prefix+'_c0']) >= 5.105)].sum())} + metrics['controller_sha256'] = hashlib.sha256(Path(CONTROLLER).read_bytes()).hexdigest() + metrics['source_sha256'] = {str(p): hashlib.sha256(p.read_bytes()).hexdigest() for p in [source/'route.npz', source/'model_paths.npz', intake, archive]} + destination.mkdir(parents=True, exist_ok=True) + np.savez_compressed(destination/'commands.npz', names=names, rows=rows) + (destination/'report.json').write_text(json.dumps(metrics, indent=2)+'\n') + return {k: v for k, v in metrics.items() if k != 'source_sha256'} + + +if __name__ == '__main__': + parser = argparse.ArgumentParser(description=__doc__) + parser.add_argument('--source', type=Path, required=True) + parser.add_argument('--delay-intake', type=Path, required=True) + parser.add_argument('--archive', type=Path, required=True, help='Archived v21 commands.npz for an exact baseline check') + parser.add_argument('--output', type=Path, required=True) + args = parser.parse_args() + print(json.dumps(replay(args.source, args.delay_intake, args.archive, args.output), indent=2))