From 0eeda2d8249877034aec5a06d8d97f6a2585b1ad Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=E6=A2=81=E8=96=84=E4=BA=91?= Date: Wed, 5 Aug 2026 15:19:04 +0800 Subject: [PATCH] docs: plan EM observation planning diagnostics --- ...-05-em-observation-planning-diagnostics.md | 287 ++++++++++++++++++ 1 file changed, 287 insertions(+) create mode 100644 docs/superpowers/plans/2026-08-05-em-observation-planning-diagnostics.md diff --git a/docs/superpowers/plans/2026-08-05-em-observation-planning-diagnostics.md b/docs/superpowers/plans/2026-08-05-em-observation-planning-diagnostics.md new file mode 100644 index 0000000..92bed1d --- /dev/null +++ b/docs/superpowers/plans/2026-08-05-em-observation-planning-diagnostics.md @@ -0,0 +1,287 @@ +# EM Observation Planning Diagnostics Implementation Plan + +> **For agentic workers:** REQUIRED SUB-SKILL: Use executing-plans to implement this plan task-by-task. Steps use checkbox syntax for tracking. + +**Goal:** Print one effective EM planning configuration snapshot at observation-session startup and make trajectory validation failures and successes numerically diagnosable without changing planning behavior. + +**Architecture:** EmTrajectoryValidator enriches its existing jerk rejection reason with the values used in the decision. TrajectoryObservationDiagnostics owns pure invariant-culture formatters for configuration and successful trajectory metrics. The MovementTest runner prints the configuration formatter once after it creates the session-local controller. + +**Tech Stack:** C#/.NET, existing EMPlannerVerificationHost, OSQP-backed EM planner. + +## Global Constraints + +- Keep the MovementTest OBSERVE_ONLY; do not add actuator calls. +- Do not change solver configuration, motion limits, QP formulation, horizon selection, or acceptance rules. +- Print configuration once per session; do not put it in the recurring UI diagnostic or every planning cycle. +- Format values with CultureInfo.InvariantCulture. +- Failed publications keep returning no EmTrajectory; their numeric evidence belongs in the failure reason. + +--- + +### Task 1: Detail a jerk-limit rejection with computed evidence + +**Files:** + +- Modify: ClumsyPilot/ParkrobTrajplanner/EMPlanner/Validation/EmTrajectoryValidator.cs:132-199 +- Modify: ClumsyPilot/tests/EMPlannerVerificationHost/TrajectoryChecks.cs:58-110 + +**Interfaces:** + +- Consumes: the existing EmTrajectoryValidator.Validate(...) method. +- Produces: an unchanged JerkLimitExceeded result whose Message contains time, dt, previousAcceleration, acceleration, jerk, limit, excess, storedPreviousJerk, and storedCurrentJerk. + +- [ ] **Step 1: Write the failing test** + +In TrajectoryChecks.VerifiesWorldSpacePublicationMutationsAreRejected, retain the current jerkMutated trajectory and add: + + EmTrajectoryValidationResult jerkResult = new EmTrajectoryValidator().Validate( + jerkMutated, context.EmptyMap, context.Vehicle, context.Configuration, 2, 0.0055d, EmBoundaryType.Goal); + Verification.Equal(EmTrajectoryValidationFailure.JerkLimitExceeded, jerkResult.Failure, + "jerk diagnostic failure code"); + Verification.Equal(2, jerkResult.PointIndex, "jerk diagnostic point index"); + foreach (string field in new[] + { + "time=", "dt=", "previousAcceleration=", "acceleration=", "jerk=", "limit=", "excess=", + "storedPreviousJerk=", "storedCurrentJerk=", + }) + Verification.True(jerkResult.Message.Contains(field), "jerk diagnostic includes " + field); + +- [ ] **Step 2: Run the test to confirm RED** + +Run: + + dotnet run --project ClumsyPilot/tests/EMPlannerVerificationHost/EMPlannerVerificationHost.csproj + +Expected: the new assertion fails because the current message contains only Trajectory finite-difference jerk exceeds its limit. + +- [ ] **Step 3: Implement the message enrichment** + +Add using System.Globalization; to EmTrajectoryValidator.cs. Calculate finiteDifferenceJerk before the condition and preserve the existing inequality: + + double finiteDifferenceJerk = (acceleration - previousAcceleration) / dt; + if (hasPreviousAcceleration && Math.Abs(finiteDifferenceJerk) > limits.MaximumJerk + limits.KinematicTolerance) + { + double storedPreviousJerk = index >= 2 ? trajectory.Points[index - 2].LongitudinalJerk : 0d; + double storedCurrentJerk = previous.LongitudinalJerk; + return Reject(EmTrajectoryValidationFailure.JerkLimitExceeded, index, + "Trajectory finite-difference jerk exceeds its limit" + + " (time=" + Format(point.TimeFromStart) + "s, dt=" + Format(dt) + "s" + + ", previousAcceleration=" + Format(previousAcceleration) + "m/s2" + + ", acceleration=" + Format(acceleration) + "m/s2" + + ", jerk=" + Format(finiteDifferenceJerk) + "m/s3" + + ", limit=" + Format(limits.MaximumJerk) + "m/s3" + + ", excess=" + Format(Math.Abs(finiteDifferenceJerk) - limits.MaximumJerk) + "m/s3" + + ", storedPreviousJerk=" + Format(storedPreviousJerk) + "m/s3" + + ", storedCurrentJerk=" + Format(storedCurrentJerk) + "m/s3)."); + } + +Add this private method to the same class: + + private static string Format(double value) + { + return value.ToString("G17", CultureInfo.InvariantCulture); + } + +storedPreviousJerk and storedCurrentJerk are evidence only and do not influence the validation decision. + +- [ ] **Step 4: Confirm GREEN and commit** + +Run: + + dotnet run --project ClumsyPilot/tests/EMPlannerVerificationHost/EMPlannerVerificationHost.csproj + +Expected: host exit code 0. + +Run: + + git add -- ClumsyPilot/ParkrobTrajplanner/EMPlanner/Validation/EmTrajectoryValidator.cs ClumsyPilot/tests/EMPlannerVerificationHost/TrajectoryChecks.cs + git commit -m "feat: detail trajectory jerk validation failures" + +### Task 2: Format and print the effective session configuration once + +**Files:** + +- Modify: ClumsyPilot/ParkrobTrajplanner/tarjplanner_movementtest/TrajectoryObservationDiagnostics.cs:17-39 +- Modify: ClumsyPilot/ParkrobTrajplanner/tarjplanner_movementtest/TrajectoryObservationPipeline.cs:188-241 +- Modify: ClumsyPilot/ParkrobTrajplanner/tarjplanner_movementtest/MovementTest.TrajectoryObservationTest.cs:269-305 +- Modify: ClumsyPilot/tests/EMPlannerVerificationHost/TrajectoryObservationChecks.cs:18-39,96-191 + +**Interfaces:** + +- Consumes: the effective EmPlannerConfiguration held privately by TrajectoryObservationController. +- Produces: TrajectoryObservationDiagnostics.CreateConfiguration(EmPlannerConfiguration) and TrajectoryObservationController.CreateConfigurationDiagnostic(). +- Produces: one terminal planning configuration block before the observation loop begins. + +- [ ] **Step 1: Write failing configuration tests** + +Add VerifiesPlanningConfigurationDiagnosticUsesEffectiveConfiguration() to TrajectoryObservationChecks.Run(). Bootstrap a controller with: + + var settings = new TrajectoryObservationSettings + { + ReplanPeriodSeconds = 0.25d, + SolverTimeoutSeconds = 1.25d, + MaximumOsqpIterations = 54321, + TimeHorizonSeconds = 3.5d, + OutputTimeStepSeconds = 0.10d, + }; + var controller = new TrajectoryObservationController(bootstrap, settings, + new FixedTrajectoryPlanningService(CreatePublishedTrajectory(effectiveAt)), "config-diagnostic"); + string text = controller.CreateConfigurationDiagnostic().Text; + foreach (string expected in new[] + { + "planning configuration:", "timeHorizon=3.50s", "distanceHorizon=5.00m", "outputTimeStep=0.10s", + "outputFrequency=10.00Hz", "trajectoryKnots=36", "maximumOsqpIterations=54321", + "solverTimeout=1.25s", "replanPeriod=0.25s", + }) + Verification.True(text.Contains(expected), "configuration diagnostic includes " + expected); + +Source-read MovementTest.TrajectoryObservationTest.cs and assert the runner call is singular: + + int firstCall = runnerSource.IndexOf("CreateConfigurationDiagnostic()", StringComparison.Ordinal); + Verification.True(firstCall >= 0, "runner prints configuration diagnostic"); + Verification.Equal(-1, runnerSource.IndexOf("CreateConfigurationDiagnostic()", firstCall + 1, + StringComparison.Ordinal), "runner prints configuration diagnostic once"); + +- [ ] **Step 2: Run the test to confirm RED** + +Run: + + dotnet run --project ClumsyPilot/tests/EMPlannerVerificationHost/EMPlannerVerificationHost.csproj + +Expected: compilation fails because CreateConfigurationDiagnostic does not exist. + +- [ ] **Step 3: Implement pure formatting and the one runner call** + +Add to TrajectoryObservationDiagnostics: + + public static TrajectoryObservationDiagnostic CreateConfiguration(EmPlannerConfiguration configuration) + { + if (configuration == null || configuration.Scheduling == null || configuration.Solver == null) + throw new ArgumentNullException(nameof(configuration)); + double horizon = configuration.Scheduling.TimeHorizonSeconds; + double step = configuration.Scheduling.OutputTimeStepSeconds; + int knots = checked((int)Math.Ceiling(horizon / step) + 1); + return new TrajectoryObservationDiagnostic("planning configuration:\n" + + "timeHorizon=" + Format(horizon, "F2") + "s\n" + + "distanceHorizon=" + Format(configuration.Scheduling.DistanceHorizonMeters, "F2") + "m\n" + + "outputTimeStep=" + Format(step, "F2") + "s\n" + + "outputFrequency=" + Format(1d / step, "F2") + "Hz\n" + + "trajectoryKnots=" + knots.ToString(CultureInfo.InvariantCulture) + "\n" + + "maximumOsqpIterations=" + configuration.Solver.MaximumOsqpIterations.ToString(CultureInfo.InvariantCulture) + "\n" + + "solverTimeout=" + Format(configuration.Scheduling.SolverTimeoutSeconds, "F2") + "s\n" + + "replanPeriod=" + Format(configuration.Scheduling.ReplanPeriodSeconds, "F2") + "s"); + } + + private static string Format(double value, string format) + { + return value.ToString(format, CultureInfo.InvariantCulture); + } + +Add to TrajectoryObservationController: + + public TrajectoryObservationDiagnostic CreateConfigurationDiagnostic() + { + return TrajectoryObservationDiagnostics.CreateConfiguration(configuration); + } + +Immediately after constructing controller in RunSessionAsync, before new TrajectoryObservationLoop(controller), add exactly: + + LogIfCurrent(sessionId, controller.CreateConfigurationDiagnostic().Text); + +Do not call this method from the while loop or the UI drawing methods. + +- [ ] **Step 4: Confirm GREEN and commit** + +Run: + + dotnet run --project ClumsyPilot/tests/EMPlannerVerificationHost/EMPlannerVerificationHost.csproj + +Expected: host exit code 0. + +Run: + + git add -- ClumsyPilot/ParkrobTrajplanner/tarjplanner_movementtest/TrajectoryObservationDiagnostics.cs ClumsyPilot/ParkrobTrajplanner/tarjplanner_movementtest/TrajectoryObservationPipeline.cs ClumsyPilot/ParkrobTrajplanner/tarjplanner_movementtest/MovementTest.TrajectoryObservationTest.cs ClumsyPilot/tests/EMPlannerVerificationHost/TrajectoryObservationChecks.cs + git commit -m "feat: log EM observation session planning configuration" + +### Task 3: Summarize each successfully published trajectory + +**Files:** + +- Modify: ClumsyPilot/ParkrobTrajplanner/tarjplanner_movementtest/TrajectoryObservationDiagnostics.cs:17-39 +- Modify: ClumsyPilot/tests/EMPlannerVerificationHost/TrajectoryObservationChecks.cs:538-561 + +**Interfaces:** + +- Consumes: latestCycle.Result.Trajectory only when latestCycle.Published is true. +- Produces: a trajectory summary with ID, point count, duration, PathS length, max speed, max finite-difference acceleration, and max finite-difference jerk. + +- [ ] **Step 1: Write the failing summary test** + +Add VerifiesPublishedPlanningDiagnosticsIncludeTrajectorySummary() to TrajectoryObservationChecks.Run(): + + EmTrajectory trajectory = CreatePublishedTrajectory(effectiveAt); + var succeeded = new PlanningCycleResult(9L, + new PlanningCycleIdentity(1L, "summary-reference", 2L, string.Empty, 0), + new EmPlanningResult(EmPlanningStatus.Success, trajectory, string.Empty), true, string.Empty); + string text = TrajectoryObservationDiagnostics.Create( + succeeded, TimeSpan.FromMilliseconds(12d), false, trajectory).Text; + foreach (string expected in new[] + { + "trajectory summary:", "trajectoryId=observer-published", "points=2", "duration=1.000s", + "pathLength=1.000m", "maxSpeed=0.400m/s", "maxAcceleration=0.200m/s2", "maxJerk=0.000m/s3", + }) + Verification.True(text.Contains(expected), "published diagnostic includes " + expected); + +Also add this assertion to the existing failed-cycle test: + + Verification.True(!diagnostic.Text.Contains("trajectory summary:"), "failed diagnostic has no stale trajectory summary"); + +- [ ] **Step 2: Run the test to confirm RED** + +Run: + + dotnet run --project ClumsyPilot/tests/EMPlannerVerificationHost/EMPlannerVerificationHost.csproj + +Expected: summary assertions fail because the formatter only writes trajectory=. + +- [ ] **Step 3: Implement summary formatting** + +Replace the publishedTrajectory != null branch in TrajectoryObservationDiagnostics.Create(...) with a branch requiring latestCycle.Published && latestCycle.Result.Trajectory != null. Add CreateTrajectorySummary(EmTrajectory trajectory), which: + +- reads immutable trajectory points; +- calculates maximum absolute signed speed; +- calculates acceleration and jerk by the same direction-aware finite-difference convention as EmTrajectoryValidator; +- ignores a nonpositive or non-finite dt only in logging; +- reports first and last PathS difference as actual pathLength; +- uses Format(value, "F3") and CultureInfo.InvariantCulture; +- returns exactly these two lines: + + trajectory summary: + trajectoryId=, points=, duration=s, pathLength=m + maxSpeed=m/s, maxAcceleration=m/s2, maxJerk=m/s3 + +Do not call this helper for an unsuccessful cycle, even when an older published trajectory is still observed. + +- [ ] **Step 4: Confirm GREEN, inspect, and commit** + +Run: + + dotnet run --project ClumsyPilot/tests/EMPlannerVerificationHost/EMPlannerVerificationHost.csproj + git diff --check + +Expected: host exit code 0 and no whitespace errors. + +Run: + + git add -- ClumsyPilot/ParkrobTrajplanner/tarjplanner_movementtest/TrajectoryObservationDiagnostics.cs ClumsyPilot/tests/EMPlannerVerificationHost/TrajectoryObservationChecks.cs + git commit -m "feat: summarize published EM observation trajectories" + +## Final verification + +- [ ] Run: + + dotnet run --project ClumsyPilot/tests/EMPlannerVerificationHost/EMPlannerVerificationHost.csproj + git diff --check + git log -3 --oneline + +Expected: verification exits 0, no whitespace errors exist, and three focused diagnostics commits follow the already committed design document.