ParkingRobot/docs/superpowers/plans/2026-08-10-em-closed-loop-movement-test.md

EM Closed-Loop Movement Test Implementation Plan

For agentic workers: REQUIRED SUB-SKILL: Use superpowers:subagent-driven-development (recommended) or superpowers:executing-plans to implement this plan task-by-task. Steps use checkbox (- [ ]) syntax for tracking.

Goal: Add an EM闭环测试 MovementTest that plans one complete EM direction segment once, freezes it, adapts it to Trajectory2D, and tracks it with the imported Stanley/PID/GCP controller until that segment stops.

Architecture: Keep the planning and controller models separate. Restore the reference controller's smallest portable runtime dependency set inside ClumsyPilot, add a pure EmTrajectory-to-Trajectory2D adapter, then make a session runner reuse the existing trajectory-observation bootstrap and one TrajectoryObservationController.StartCycle call before handing the frozen trajectory to TrajectoryTrackingMovement.

Tech Stack: C# 10, .NET Standard 2.0 plugin, .NET 10 Windows verification host, Clumsy MovementTest/DriveTask, existing EM planner and OSQP runtime.

Global Constraints

• The MovementTest registration name is exactly EM闭环测试.
• Plan exactly once in EmPlanningScope.FullDirectionSegment; do not replan or replace the trajectory while driving.
• Execute only the first direction segment. A gear-switch terminal stops and ends this test.
• Shared/control boundaries use SI units: m, m/s, rad, rad/s; body X is forward, body Y is left, counter-clockwise is positive.
• Preserve signed longitudinal velocity: forward positive and reverse negative.
• Do not change velocity/steering signs, CAN IDs, remote mappings, mechanical limits, or mode-switch policy.
• Do not overwrite unrelated dirty files. Stage only the exact files named by each task.
• Real-vehicle validation must remain low-speed, short-distance, in an open area, with hardware emergency stop available.

---

File Map

Existing user files retained and brought under version control

• ClumsyPilot/Control/**/*.cs — Stanley/PID/GCP controller implementation already copied from the reference project.
• ClumsyPilot/Trajectory/**/*.cs — controller-side trajectory and projection implementation already copied from the reference project.

Portable runtime dependencies copied from the reference project

• ClumsyPilot/Shared/Mathematics/AngleMath.cs — angle normalization and conversion.
• ClumsyPilot/Shared/Mathematics/InterpolationMath.cs — scalar interpolation.
• ClumsyPilot/Shared/Mathematics/FrameTransform2D.cs — world/body transforms.
• ClumsyPilot/Shared/Models/ChassisCommand.cs — Pose2D, Twist2D, and chassis command value types.
• ClumsyPilot/Shared/Chassis/MultiWheelChassisAdapter.cs — SI/GCP boundary over the existing MultiWheelChassis.
• ClumsyPilot/StateEstimation/IVehicleStateProvider.cs — controller state-source boundary.
• ClumsyPilot/StateEstimation/VehicleState.cs — immutable controller state.
• ClumsyPilot/StateEstimation/FirstOrderLowPassFilter.cs — velocity filter primitive.
• ClumsyPilot/StateEstimation/VelocityEstimator2D.cs — Detour velocity estimation.
• ClumsyPilot/StateEstimation/DetourVehicleStateProvider.cs — live Detour state source.
• ClumsyPilot/Movements/TrajectoryTrackingMovement.cs — DriveTask movement wrapper around the geometric controller.

New EM closed-loop files

• ClumsyPilot/ParkrobTrajplanner/tarjplanner_movementtest/EmControlTrajectoryAdapter.cs — pure model adapter.
• ClumsyPilot/ParkrobTrajplanner/tarjplanner_movementtest/MovementTest.EmClosedLoopTest.cs — UI input, session ownership, one-shot planning, controller handoff, and stop behavior.
• ClumsyPilot/tests/EMPlannerVerificationHost/EmControlTrajectoryAdapterChecks.cs — adapter behavior checks.
• ClumsyPilot/tests/verify_em_closed_loop_movement.ps1 — MovementTest registration and one-shot lifecycle checks.
• ClumsyPilot/tests/EMPlannerVerificationHost/Program.cs — add the em-control-adapter verification command.
• ClumsyPilot/ParkrobTrajplanner/tarjplanner_movementtest/README.md — operator procedure and test boundary.

---

Task 1: Restore the controller's portable runtime dependency chain

Files:

• Add and track: ClumsyPilot/Control/**/*.cs
• Add and track: ClumsyPilot/Trajectory/**/*.cs
• Create: ClumsyPilot/Shared/Mathematics/AngleMath.cs
• Create: ClumsyPilot/Shared/Mathematics/InterpolationMath.cs
• Create: ClumsyPilot/Shared/Mathematics/FrameTransform2D.cs
• Create: ClumsyPilot/Shared/Models/ChassisCommand.cs
• Create: ClumsyPilot/Shared/Chassis/MultiWheelChassisAdapter.cs
• Create: ClumsyPilot/StateEstimation/IVehicleStateProvider.cs
• Create: ClumsyPilot/StateEstimation/VehicleState.cs
• Create: ClumsyPilot/StateEstimation/FirstOrderLowPassFilter.cs
• Create: ClumsyPilot/StateEstimation/VelocityEstimator2D.cs
• Create: ClumsyPilot/StateEstimation/DetourVehicleStateProvider.cs
• Create: ClumsyPilot/Movements/TrajectoryTrackingMovement.cs

Interfaces:

• Produces: MyParking.Shared.Pose2D, Twist2D, AngleMath, MultiWheelChassisAdapter.

• Produces: MultiWheelC.StateEstimation.IVehicleStateProvider and DetourVehicleStateProvider.

• Produces: MultiWheelC.Trajectory.Trajectory2D and TrajectoryPoint.

• Produces: MultiWheelC.TrajectoryTrackingMovement : MovementDefinition with public Trajectory, StateProvider, CycleObserver, and safety-limit fields.

• Step 1: Re-run the failing integration build

Run:

dotnet build .\ClumsyPilot\ClumsyPilot.csproj --no-restore

Expected: FAIL with missing MyParking.Shared, MultiWheelC.StateEstimation, VehicleState, and MultiWheelChassisAdapter. This proves the partial controller import is not silently excluded from the plugin build.

• Step 2: Add the exact portable reference sources

Read each source from:

D:\Users\Desktop\项目\prakrobot\停车机器人-合并测试\parkr_shen\Shared\...
D:\Users\Desktop\项目\prakrobot\停车机器人-合并测试\parkr_shen\MultiWheelC\StateEstimation\...
D:\Users\Desktop\项目\prakrobot\停车机器人-合并测试\parkr_shen\MultiWheelC\Movements\TrajectoryTrackingMovement.cs

Add the files at the destination paths in this task without changing namespaces or logic. Do not import FleetKinematics, experiment recorders, composite movements, rotation movements, or wheel-feedback providers because TrajectoryTrackingMovement does not consume them.

• Step 3: Prove existing controller/trajectory files still match the reference implementation

Run:

$reference = 'D:\Users\Desktop\项目\prakrobot\停车机器人-合并测试\parkr_shen\MultiWheelC'
$local = (Resolve-Path '.\ClumsyPilot').Path
Get-ChildItem '.\ClumsyPilot\Control','.\ClumsyPilot\Trajectory' -File -Recurse | ForEach-Object {
    $relative = $_.FullName.Substring($local.Length + 1)
    $source = Join-Path $reference $relative
    if (-not (Test-Path $source) -or (Get-FileHash $_.FullName).Hash -ne (Get-FileHash $source).Hash) {
        throw "Reference mismatch: $relative"
    }
}

Expected: exit 0 and no mismatch.

• Step 4: Run the build to verify the dependency chain compiles

Run:

dotnet build .\ClumsyPilot\ClumsyPilot.csproj --no-restore

Expected: PASS with zero errors. Record warnings verbatim; do not hide them.

• Step 5: Commit only the controller runtime integration

git add -- ClumsyPilot/Control ClumsyPilot/Trajectory ClumsyPilot/Shared ClumsyPilot/StateEstimation ClumsyPilot/Movements/TrajectoryTrackingMovement.cs
git diff --cached --check
git commit -m "feat: integrate trajectory tracking controller runtime"

---

Task 2: Adapt a frozen EM trajectory to Trajectory2D

Files:

• Create: ClumsyPilot/ParkrobTrajplanner/tarjplanner_movementtest/EmControlTrajectoryAdapter.cs
• Create: ClumsyPilot/tests/EMPlannerVerificationHost/EmControlTrajectoryAdapterChecks.cs
• Modify: ClumsyPilot/tests/EMPlannerVerificationHost/Program.cs

Interfaces:

• Consumes: EmTrajectory with validated SI-unit EmTrajectoryPoint values.

• Produces: public sealed class EmControlTrajectoryAdapter.

• Produces: public Trajectory2D Create(EmTrajectory trajectory).

• Duplicate-position threshold: 1e-6 m, matching Trajectory2D's minimum valid projection segment.

• Step 1: Register the adapter check before implementing the adapter

Extend Program.cs argument validation with em-control-adapter, and add:

if (args[0] == "em-control-adapter" || args[0] == "em-all")
{
    EmControlTrajectoryAdapterChecks.Run();
    Console.WriteLine("PASS em-control-adapter");
}

Create EmControlTrajectoryAdapterChecks.cs with checks that build real EmTrajectory objects and assert:

internal static void Run()
{
    MapsFieldsAndRebuildsGeometricArcLength();
    KeepsReverseVelocityNegative();
    CollapsesTerminalHoldAndKeepsItsZeroSpeed();
    RejectsFewerThanTwoDistinctPositions();
}

The forward mapping check must assert all of these exact relations:

Trajectory2D actual = new EmControlTrajectoryAdapter().Create(source);
Require(actual.Count == 3, "adapter point count");
RequireClose(actual[0].ArcLengthMeters, 0d, "first arc");
RequireClose(actual[1].ArcLengthMeters, 0.5d, "second arc");
RequireClose(actual[2].ArcLengthMeters, 1.0d, "third arc");
RequireClose(actual[1].PoseInWorld.XMeters, source.Points[1].X, "x");
RequireClose(actual[1].PoseInWorld.YMeters, source.Points[1].Y, "y");
RequireClose(actual[1].PoseInWorld.YawRadians, source.Points[1].Yaw, "yaw");
RequireClose(actual[1].CurvaturePerMeter, source.Points[1].VehicleCurvature, "curvature");
RequireClose(actual[1].ReferenceSpeedMetersPerSecond,
    source.Points[1].SignedLongitudinalVelocity, "signed speed");

For the terminal-hold case, provide two final EM samples at the same pose where the later sample has zero speed, then assert that the output contains one terminal position and EndPoint.ReferenceSpeedMetersPerSecond == 0d.

• Step 2: Run the new check and verify RED

Run:

dotnet run --project .\ClumsyPilot\tests\EMPlannerVerificationHost\EMPlannerVerificationHost.csproj -- em-control-adapter

Expected: FAIL to compile because EmControlTrajectoryAdapter does not exist.

• Step 3: Implement the minimum adapter

Implement this public surface and algorithm:

public sealed class EmControlTrajectoryAdapter
{
    private const double MinimumSegmentLengthMeters = 1e-6;

    public Trajectory2D Create(EmTrajectory trajectory)
    {
        if (trajectory == null)
            throw new ArgumentNullException(nameof(trajectory));

        var points = new List<TrajectoryPoint>(trajectory.Points.Count);
        double arcLength = 0d;
        for (int index = 0; index < trajectory.Points.Count; index++)
        {
            EmTrajectoryPoint source = trajectory.Points[index];
            var pose = new MyParking.Shared.Pose2D(source.X, source.Y, source.Yaw);
            var converted = new TrajectoryPoint(
                arcLength, pose, source.VehicleCurvature, source.SignedLongitudinalVelocity);

            if (points.Count == 0)
            {
                points.Add(converted);
                continue;
            }

            TrajectoryPoint previous = points[points.Count - 1];
            double dx = pose.XMeters - previous.PoseInWorld.XMeters;
            double dy = pose.YMeters - previous.PoseInWorld.YMeters;
            double distance = Math.Sqrt(dx * dx + dy * dy);
            if (distance < MinimumSegmentLengthMeters)
            {
                points[points.Count - 1] = new TrajectoryPoint(
                    previous.ArcLengthMeters, pose, source.VehicleCurvature,
                    source.SignedLongitudinalVelocity);
                continue;
            }

            arcLength += distance;
            points.Add(new TrajectoryPoint(
                arcLength, pose, source.VehicleCurvature,
                source.SignedLongitudinalVelocity));
        }

        if (points.Count < 2)
            throw new ArgumentException(
                "EM轨迹至少需要包含两个不同位置的有效控制点。", nameof(trajectory));

        return new Trajectory2D(points);
    }
}

Include /// <summary> comments on the class and public method, explicitly stating the unit and signed-speed boundary.

• Step 4: Run adapter checks and the existing EM suite

Run:

dotnet run --project .\ClumsyPilot\tests\EMPlannerVerificationHost\EMPlannerVerificationHost.csproj -- em-control-adapter
dotnet run --project .\ClumsyPilot\tests\EMPlannerVerificationHost\EMPlannerVerificationHost.csproj -- trajectory-observation

Expected: both commands PASS.

• Step 5: Commit the adapter and checks

git add -- ClumsyPilot/ParkrobTrajplanner/tarjplanner_movementtest/EmControlTrajectoryAdapter.cs ClumsyPilot/tests/EMPlannerVerificationHost/EmControlTrajectoryAdapterChecks.cs ClumsyPilot/tests/EMPlannerVerificationHost/Program.cs
git diff --cached --check
git commit -m "feat: adapt EM trajectory for geometric controller"

---

Task 3: Add the one-shot, single-direction EM闭环测试

Files:

• Create: ClumsyPilot/tests/verify_em_closed_loop_movement.ps1
• Create: ClumsyPilot/ParkrobTrajplanner/tarjplanner_movementtest/MovementTest.EmClosedLoopTest.cs

Interfaces:

• Consumes: TrajectoryObservationSetupFactory.CreateBootstrapJob(...).

• Consumes: TrajectoryObservationBootstrapper.Bootstrap(...).

• Consumes exactly one call to TrajectoryObservationController.StartCycle(...).

• Consumes: EmControlTrajectoryAdapter.Create(...).

• Produces: [MovementTest(name = "EM闭环测试")] public sealed class EmClosedLoopMovementTest : MovementTest.

• Produces: Test() for session start and TestStop() for idempotent cancellation/stop.

• Step 1: Write the structural/lifecycle verification first

Create verify_em_closed_loop_movement.ps1 so it reads the new source file and fails unless all conditions hold:

$sourcePath = Join-Path $PSScriptRoot '..\ParkrobTrajplanner\tarjplanner_movementtest\MovementTest.EmClosedLoopTest.cs'
if (-not (Test-Path -LiteralPath $sourcePath)) { throw 'EM closed-loop MovementTest source is missing.' }
$source = Get-Content -Raw -Encoding UTF8 -LiteralPath $sourcePath

$required = @(
    '\[MovementTest\(name = "EM闭环测试"\)\]',
    'EmPlanningScope\.FullDirectionSegment',
    'TrajectoryObservationBootstrapper',
    '\.StartCycle\(',
    'EmControlTrajectoryAdapter',
    'TrajectoryTrackingMovement',
    'public override void TestStop\(\)',
    '\.Cancel\(\)',
    '\.Stop\(\)'
)
foreach ($pattern in $required) {
    if ($source -notmatch $pattern) { throw "Missing required pattern: $pattern" }
}

if ([regex]::Matches($source, '\.StartCycle\(').Count -ne 1) {
    throw 'EM closed-loop test must contain exactly one planning-cycle call site.'
}
if ($source -match 'TrajectoryObservationLoop|ReplanPeriod|while\s*\(true\).*StartCycle') {
    throw 'EM closed-loop test must not contain a replanning loop.'
}

Write-Output 'EM closed-loop MovementTest checks passed.'

• Step 2: Run the verification and confirm RED

Run:

powershell -NoProfile -ExecutionPolicy Bypass -File .\ClumsyPilot\tests\verify_em_closed_loop_movement.ps1

Expected: FAIL with EM closed-loop MovementTest source is missing.

• Step 3: Implement the MovementTest input surface

Add EmClosedLoopMovementTest with the same goal, map, obstacle, vehicle, solver, and output-step defaults used by TrajectoryObservationMovementTest. Set control defaults explicitly:

public double MaximumCommandSpeedMetersPerSecond = 0.20d;
public double MaximumDistanceToTrajectoryMeters = 0.30d;
public double ExecutionTimeoutSeconds = 120d;
public float WheelAlignmentToleranceDegrees = 2f;

Test() must validate inputs, create a validated settings snapshot with:

PlanningScope = EmPlanningScope.FullDirectionSegment

and start one session through EmClosedLoopMovementTestRunner.Start(...). TestStop() must call only:

EmClosedLoopMovementTestRunner.Stop();

Use the existing observation test's culture-aware finite-number and bounded-obstacle input behavior. Keep those helpers private to the new test so the user's modified observation file is not edited.

• Step 4: Implement idempotent session ownership and one-shot planning

Use a static runner guarded by one lock. Store one CancellationTokenSource, one background Task, one active DriveTask, and a monotonically increasing session ID.

The planning body must follow this exact sequence:

VehicleMotionState initialState = ReadPlanningState();
CoarsePathPlanningJob job = TrajectoryObservationSetupFactory.CreateBootstrapJob(
    initialState.Pose, goal, settings, obstacles, obstacleSnapshotVersion);
TrajectoryObservationBootstrapResult bootstrap =
    new TrajectoryObservationBootstrapper().Bootstrap(job, token);
if (!bootstrap.Succeeded)
    throw new InvalidOperationException(bootstrap.FailureReason);

var controller = new TrajectoryObservationController(
    bootstrap, settings, new EmPlanningService(new OsqpNativeSolver()),
    "em-closed-loop-" + sessionId.ToString(CultureInfo.InvariantCulture));
PlanningCycleResult cycle = await controller.StartCycle(
    initialState.CapturedAtUtc, initialState, token).ConfigureAwait(false);
if (!cycle.Published || controller.PublishedTrajectory == null)
    throw new InvalidOperationException(
        string.IsNullOrWhiteSpace(cycle.Result.FailureReason)
            ? cycle.Diagnostic
            : cycle.Result.FailureReason);

Trajectory2D trajectory =
    new EmControlTrajectoryAdapter().Create(controller.PublishedTrajectory);

There must be no observation loop and no second StartCycle call. Log the EM point count, controller point count, geometric length, segment index, direction, and terminal type before control starts.

Create EmClosedLoopWheelSafety.AreWheelsForward(float toleranceDegrees) in the same file using MultiWheelChassisAdapter.AreParallelWheelsAligned(0d, toleranceRadians). If this check fails, throw before constructing DriveTask.

Create and run the controller movement as:

var movement = new TrajectoryTrackingMovement
{
    Trajectory = trajectory,
    MaximumCommandSpeedMetersPerSecond = maximumCommandSpeedMetersPerSecond,
    MaximumDistanceToTrajectoryMeters = maximumDistanceToTrajectoryMeters,
    ExecutionTimeoutSeconds = executionTimeoutSeconds
};
var driveTask = new DriveTask(movement.Get());

Publish driveTask under the session lock only if the session is still current and not cancelled. Otherwise stop it immediately. For a current session call driveTask.Wait() on the background worker, then report that the first direction segment ended. A GearSwitch terminal is reported as “stopped at gear-switch boundary; next segment was not started.”

Stop() must atomically clear the active references, call CancellationTokenSource.Cancel(), and call DriveTask.Stop(). The completion continuation must clear only references that still belong to its own session.

• Step 5: Run structural verification and compile

Run:

powershell -NoProfile -ExecutionPolicy Bypass -File .\ClumsyPilot\tests\verify_em_closed_loop_movement.ps1
dotnet build .\ClumsyPilot\ClumsyPilot.csproj --no-restore

Expected: structural check PASS; build PASS with zero errors.

• Step 6: Commit the MovementTest

git add -- ClumsyPilot/tests/verify_em_closed_loop_movement.ps1 ClumsyPilot/ParkrobTrajplanner/tarjplanner_movementtest/MovementTest.EmClosedLoopTest.cs
git diff --cached --check
git commit -m "feat: add one-shot EM closed-loop movement test"

---

Task 4: Document operation and run the complete non-hardware verification

Files:

• Modify: ClumsyPilot/ParkrobTrajplanner/tarjplanner_movementtest/README.md

Interfaces:

• Documents: test selection, inputs, one-shot/single-direction boundary, TestStop, low-speed first-run procedure, and limits of automated validation.

• Step 1: Add the operator-facing section

Add a section named EM闭环测试 containing these exact operational facts:

- 启动时只规划一次，并冻结首次成功的完整方向段轨迹。
- 控制执行期间不重规划、不切换轨迹。
- 到达目标或换向边界后停车；本测试不启动下一方向段。
- 默认控制速度上限为 0.20 m/s。
- 规划、适配或车轮方向检查失败时不会启动底盘控制。
- TestStop 会同时取消规划并停止活动控制任务。
- 首次实车测试使用空旷环境、短距离单方向目标，并确保硬件急停可用。

• Step 2: Run all targeted checks

Run:

dotnet run --project .\ClumsyPilot\tests\EMPlannerVerificationHost\EMPlannerVerificationHost.csproj -- em-control-adapter
dotnet run --project .\ClumsyPilot\tests\EMPlannerVerificationHost\EMPlannerVerificationHost.csproj -- trajectory-observation
powershell -NoProfile -ExecutionPolicy Bypass -File .\ClumsyPilot\tests\verify_em_closed_loop_movement.ps1
dotnet build .\ClumsyPilot\ClumsyPilot.csproj --no-restore

Expected: every command exits 0. Report warning counts from the build.

• Step 3: Verify scope and source hygiene

Run:

git diff --check
git status --short
git diff --no-ext-diff -- ClumsyPilot/Control ClumsyPilot/Trajectory ClumsyPilot/Shared ClumsyPilot/StateEstimation ClumsyPilot/Movements ClumsyPilot/ParkrobTrajplanner/tarjplanner_movementtest ClumsyPilot/tests

Expected: no whitespace errors; only files named by this plan are staged or changed by this work. Existing unrelated modifications remain untouched.

• Step 4: Commit documentation

git add -- ClumsyPilot/ParkrobTrajplanner/tarjplanner_movementtest/README.md
git diff --cached --check
git commit -m "docs: explain EM closed-loop test operation"

• Step 5: Hand off real-vehicle validation without claiming it passed

Report separately:

Automated status: adapter checks, observation regressions, structural lifecycle check, and plugin build.
Hardware status: not run by automated verification; requires a low-speed, short, single-direction field test with emergency stop available.