ParkingRobot/docs/superpowers/plans/2026-08-05-em-observation-planning-diagnostics.md

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.

Preflight: Preserve manual MovementTest parameter tuning

TrajectoryObservationMovementTest exposes fields precisely so an operator can tune a test session. The verification host must not require those source literals to equal the defaults of TrajectoryObservationSettings; it must continue to verify that the fields exist, are copied into settings, and that TrajectoryObservationSettings itself retains its independently tested defaults.

• Run dotnet run --project ClumsyPilot/tests/EMPlannerVerificationHost/EMPlannerVerificationHost.csproj -- trajectory-observation and observe the current source-literal failure.
• In TrajectoryObservationChecks.VerifiesMovementTestVehicleInputsAndSettingsSnapshot, replace only the four source-text checks that require 0.50d, 12000, 6d, and 0.10d with checks for public fields named SolverTimeoutSeconds, MaximumOsqpIterations, TimeHorizonSeconds, and OutputTimeStepSeconds. Preserve the separate new TrajectoryObservationSettings() default assertions and all field-to-settings propagation checks.
• Re-run the same command and require exit code 0 before adding the Task 1 regression test.

---

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 -- trajectory

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 -- trajectory

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 -- trajectory-observation

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 -- trajectory-observation

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 -- trajectory-observation

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 -- trajectory-observation
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 -- trajectory dotnet run --project ClumsyPilot/tests/EMPlannerVerificationHost/EMPlannerVerificationHost.csproj -- trajectory-observation 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.