feat: derive adaptive full-segment ST schedule

2026-08-06 23:54:34 +08:00
parent dad4ff4b04
commit 3269d556b6
14 changed files with 1642 additions and 108 deletions
@@ -1,5 +1,6 @@
 using System;
 using System.Collections.Generic;
+using System.Threading;
 using EMPlannerVerificationHost;
 using MultiWheelC.TrajectoryPlanning.CoarsePath;
 using MultiWheelC.TrajectoryPlanning.PathSmoothing;
@@ -19,6 +20,8 @@ internal static class LongitudinalModelChecks
        VerifiesStoppingPrecheckOnlyAppliesToRealStopBoundaries();
        VerifiesReferenceHorizonSelectionSeparatesSpaceAndTime();
        VerifiesFullDirectionScopeSelectsActualSegmentBoundary();
+        VerifiesFullDirectionScheduleDerivesDurationAndAdaptiveBreakpoints();
+        VerifiesFullDirectionInitialFeasibilityProjectionAndFallbackSemantics();
        VerifiesTimeKnotLayoutDynamicsObjectiveAndHardConstraints();
        VerifiesModeSpecificSolutionValidation();
        VerifiesPreviousTrajectorySeedResamplesAndProjectsMonotonically();
@@ -297,6 +300,231 @@ internal static class LongitudinalModelChecks
        Verification.NearlyEqual(10d, gear.WindowEndReferenceS, "gear full selection stops before the next segment");
    }

+    private static void VerifiesFullDirectionScheduleDerivesDurationAndAdaptiveBreakpoints()
+    {
+        EmPlannerConfiguration configuration = EmPlannerConfiguration.CreateDefault();
+        configuration.Scheduling.TimeHorizonSeconds = 10d;
+        configuration.Scheduling.DistanceHorizonMeters = 0.25d;
+        configuration.Scheduling.OutputTimeStepSeconds = 0.10d;
+        configuration.Scheduling.MaximumOptimizationTimeStepSeconds = 0.20d;
+        configuration.Scheduling.MaximumOptimizationSpatialStepMeters = 0.10d;
+        configuration.Scheduling.MaximumOptimizationKnotCount = 401;
+        configuration.Longitudinal.MaximumForwardSpeedMetersPerSecond = 1d;
+        configuration.Longitudinal.DesiredForwardSpeedMetersPerSecond = 1d;
+        configuration.Longitudinal.MaximumAccelerationMetersPerSecondSquared = 0.50d;
+        configuration.Longitudinal.MaximumDecelerationMetersPerSecondSquared = 0.50d;
+        configuration.Longitudinal.MaximumJerkMetersPerSecondCubed = 1d;
+
+        LateralPath shortPath = CreateStraightPath(0.50d);
+        EmPlanningStatus status = new PathSpeedLimitBuilder().Build(shortPath, TravelDirection.Forward, 0.10d,
+            EmTerminalType.Goal, configuration, out PathSpeedLimit shortLimit, out string failureReason);
+        Verification.Equal(EmPlanningStatus.Success, status, "short full-segment envelope: " + failureReason);
+        status = new FullDirectionSegmentScheduleBuilder().TryBuild(shortPath, shortLimit, 0.10d, 0d,
+            configuration.Longitudinal.DesiredForwardSpeedMetersPerSecond, configuration,
+            out LongitudinalKnotSchedule shortSchedule, out failureReason);
+        Verification.Equal(EmPlanningStatus.Success, status, "short full-segment schedule: " + failureReason);
+        Verification.True(shortSchedule.TotalDurationSeconds < 10d, "short segment derives its own T_end");
+
+        LateralPath longPath = CreatePath(new[]
+        {
+            new PathFixture(0d, 0d, 0d, 0d),
+            new PathFixture(1.50d, 1.50d, 2d, 0d),
+            new PathFixture(3d, 3d, 0d, 0d),
+        });
+        status = new PathSpeedLimitBuilder().Build(longPath, TravelDirection.Forward, 0.10d,
+            EmTerminalType.Goal, configuration, out PathSpeedLimit longLimit, out failureReason);
+        Verification.Equal(EmPlanningStatus.Success, status, "long full-segment envelope: " + failureReason);
+        status = new FullDirectionSegmentScheduleBuilder().TryBuild(longPath, longLimit, 0.10d, 0d,
+            configuration.Longitudinal.DesiredForwardSpeedMetersPerSecond, configuration,
+            out LongitudinalKnotSchedule longSchedule, out failureReason);
+        Verification.Equal(EmPlanningStatus.Success, status, "long full-segment schedule: " + failureReason);
+        Verification.True(longSchedule.TotalDurationSeconds > shortSchedule.TotalDurationSeconds,
+            "duration grows from s_end and limits");
+        Verification.True(longSchedule.KnotTimes.Count <= configuration.Scheduling.MaximumOptimizationKnotCount,
+            "adaptive schedule respects knot cap");
+        Verification.True(longSchedule.IsAdaptive, "full segment produces an adaptive knot schedule");
+        Verification.True(longSchedule.ReferencePathS.Count > longPath.Points.Count,
+            "curvature and stopping envelopes add schedule breakpoints");
+        Verification.NearlyEqual(longPath.Points[longPath.Points.Count - 1].PathS,
+            longSchedule.ReferencePathS[longSchedule.ReferencePathS.Count - 1], "schedule reaches s_end");
+        Verification.NearlyEqual(0d,
+            longSchedule.ReferenceSpeedMetersPerSecond[longSchedule.ReferenceSpeedMetersPerSecond.Count - 1],
+            "schedule stops at s_end");
+
+        EmPlannerConfiguration constrained = configuration.Copy();
+        constrained.Scheduling.MaximumOptimizationKnotCount = 4;
+        status = new FullDirectionSegmentScheduleBuilder().TryBuild(longPath, longLimit, 0.10d, 0d,
+            constrained.Longitudinal.DesiredForwardSpeedMetersPerSecond, constrained,
+            out LongitudinalKnotSchedule rejected, out failureReason);
+        Verification.Equal(EmPlanningStatus.FullSegmentResourceLimitExceeded, status,
+            "undersized full-segment knot cap rejects rather than truncates");
+        Verification.True(rejected == null, "resource rejection produces no partial schedule");
+        Verification.True(failureReason.IndexOf("required", StringComparison.OrdinalIgnoreCase) >= 0 &&
+            failureReason.IndexOf("configured", StringComparison.OrdinalIgnoreCase) >= 0,
+            "resource rejection reports required and configured knots");
+    }
+
+    private static void VerifiesFullDirectionInitialFeasibilityProjectionAndFallbackSemantics()
+    {
+        EmPlannerConfiguration configuration = EmPlannerConfiguration.CreateDefault();
+        configuration.Scheduling.MaximumOptimizationTimeStepSeconds = 0.20d;
+        configuration.Scheduling.MaximumOptimizationSpatialStepMeters = 0.10d;
+        configuration.Scheduling.MaximumOptimizationKnotCount = 401;
+        configuration.Longitudinal.MaximumAccelerationMetersPerSecondSquared = 1e-6d;
+        configuration.Longitudinal.MaximumDecelerationMetersPerSecondSquared = 1d;
+        configuration.Longitudinal.MaximumJerkMetersPerSecondCubed = 20d;
+        LateralPath path = CreateStraightPath(0.0075d);
+        EmPlanningStatus status = new PathSpeedLimitBuilder().Build(path, TravelDirection.Forward, 0.05d,
+            EmTerminalType.Goal, configuration, out PathSpeedLimit speedLimit, out string failureReason);
+        Verification.Equal(EmPlanningStatus.Success, status, "feasible-reference envelope: " + failureReason);
+        status = new FullDirectionSegmentScheduleBuilder().TryBuild(path, speedLimit, 0.05d, 0d,
+            configuration.Longitudinal.DesiredForwardSpeedMetersPerSecond, configuration,
+            out LongitudinalKnotSchedule schedule, out failureReason);
+        Verification.Equal(EmPlanningStatus.Success, status, "feasible-reference schedule: " + failureReason);
+
+        Verification.True(typeof(LongitudinalKnotSchedule).GetProperty("ReferenceCandidate") == null,
+            "adaptive schedule is only a knot/reference/hold contract");
+        Verification.True(schedule.TerminalHoldStartIndex > 0 &&
+            schedule.TerminalHoldStartIndex < schedule.KnotTimes.Count,
+            "adaptive reference explicitly identifies its terminal hold boundary");
+        Verification.True(schedule.TerminalHoldStartIndex >= 3,
+            "adaptive exact-stop schedule reserves three independent motion jerk intervals");
+        LongitudinalCandidate strictProjection = CreateStrictNonuniformExactStopCandidate();
+        var projectionSchedule = new LongitudinalKnotSchedule(strictProjection.KnotTimes,
+            new[] { 0d, 0.003d, 0.006d, 0.0075d, 0.0075d }, new[] { 0.05d, 0.025d, 0.01d, 0d, 0d }, true, 3);
+        var input = new LongitudinalPlanningInput(path, TravelDirection.Forward, 0.05d, 0d,
+            EmTerminalType.Goal, EmLongitudinalMode.ExactStopAtBoundary, configuration,
+            EmPlanningScope.FullDirectionSegment, projectionSchedule, Array.Empty<double>(), Array.Empty<double>());
+        Verification.True(new LongitudinalSolutionValidator().TryValidate(input, speedLimit, strictProjection,
+            out _, out failureReason), "nonuniform strict projection fixture is physically feasible: " + failureReason);
+
+        var constraintBuilder = new LongitudinalConstraintBuilder(new LongitudinalObjectiveBuilder());
+        Verification.True(constraintBuilder.TryBuildInitialFeasibilityProjection(input, speedLimit,
+            out QuadraticProgram projectionProblem, out failureReason),
+            "full exact-stop feasibility projection builds: " + failureReason);
+        var layout = new LongitudinalVariableLayout(projectionSchedule.KnotTimes.Count);
+        Verification.True(Math.Abs(projectionProblem.LinearCost[layout.S(1)]) > 1e-12d,
+            "feasibility projection tracks scheduled PathS");
+        Verification.True(Math.Abs(projectionProblem.LinearCost[layout.U(1)]) > 1e-12d,
+            "feasibility projection tracks scheduled speed");
+        Verification.Equal(9 * layout.KnotCount - 3 +
+            3 * (layout.KnotCount - projectionSchedule.TerminalHoldStartIndex), projectionProblem.ConstraintCount,
+            "feasibility projection carries a PathS-linearized speed-envelope row for each motion knot");
+
+        var initialTimeoutSolver = new FakeQpSolver(new QpSolveResult(QpSolveStatus.TimeLimit, Array.Empty<double>(), 0d, 0d,
+            0d, 0, TimeSpan.Zero, "time limit", string.Empty));
+        LongitudinalPlanningResult initialTimeout = new SequentialLongitudinalOptimizer(initialTimeoutSolver).Optimize(input,
+            CancellationToken.None);
+        Verification.Equal(EmPlanningStatus.SolverTimedOut, initialTimeout.Status,
+            "initial feasibility timeout cannot publish a fallback");
+        Verification.True(initialTimeout.Candidate == null, "initial feasibility timeout publishes no candidate");
+
+        var solver = new FakeQpSolver(new[]
+        {
+            new QpSolveResult(QpSolveStatus.Solved, ToPrimal(strictProjection), 0d, 0d, 0d, 1,
+                TimeSpan.Zero, "solved", string.Empty),
+            new QpSolveResult(QpSolveStatus.TimeLimit, Array.Empty<double>(), 0d, 0d, 0d, 0,
+                TimeSpan.Zero, "time limit", string.Empty),
+        });
+        LongitudinalPlanningResult result = new SequentialLongitudinalOptimizer(solver).Optimize(input,
+            CancellationToken.None);
+        Verification.Equal(EmPlanningStatus.SuccessWithFallback, result.Status,
+            "strict feasibility projection permits a later exact-stop fallback: " + result.FailureReason);
+        Verification.Equal(2, solver.SolveCallCount,
+            "full scope consumes strict feasibility projection before the objective timeout");
+        Verification.True(new LongitudinalSolutionValidator().TryValidate(input, speedLimit,
+            result.Candidate ?? throw new InvalidOperationException("Adaptive fallback was missing."), out _,
+            out failureReason), "adaptive fallback is strict-feasible: " + failureReason);
+
+        EmPlannerConfiguration denserPublication = configuration.Copy();
+        denserPublication.Scheduling.OutputTimeStepSeconds = 0.05d;
+        status = new FullDirectionSegmentScheduleBuilder().TryBuild(path, speedLimit, 0.05d, 0d,
+            denserPublication.Longitudinal.DesiredForwardSpeedMetersPerSecond, denserPublication,
+            out LongitudinalKnotSchedule sameOptimizationSchedule, out failureReason);
+        Verification.Equal(EmPlanningStatus.Success, status, "independent-cadence schedule: " + failureReason);
+        Verification.Equal(schedule.KnotTimes.Count, sameOptimizationSchedule.KnotTimes.Count,
+            "publication cadence does not change adaptive knot count");
+        Verification.Equal(schedule.TerminalHoldStartIndex, sameOptimizationSchedule.TerminalHoldStartIndex,
+            "publication cadence does not change the terminal hold boundary");
+    }
+
+    private static LongitudinalCandidate CreateStrictNonuniformExactStopCandidate()
+    {
+        double[] times = { 0d, 0.09d, 0.19d, 0.30d, 0.50d };
+        double[] motionTimes = { 0d, 0.09d, 0.19d, 0.30d };
+        var influence = new double[3, 3];
+        for (int interval = 0; interval < 3; interval++)
+        {
+            var basis = new double[3];
+            basis[interval] = 1d;
+            LongitudinalCandidate response = LongitudinalCandidate.Integrate(motionTimes, 0d, 0d, 0d, basis);
+            int last = response.S.Count - 1;
+            influence[0, interval] = response.A[last];
+            influence[1, interval] = response.U[last];
+            influence[2, interval] = response.S[last];
+        }
+        double[] jerkMotion = SolveThreeByThree(influence, new[] { 0d, -0.05d, -0.0075d });
+        var jerk = new[] { jerkMotion[0], jerkMotion[1], jerkMotion[2], 0d };
+        LongitudinalCandidate integrated = LongitudinalCandidate.Integrate(times, 0d, 0.05d, 0d, jerk);
+        var pathS = new[] { integrated.S[0], integrated.S[1], integrated.S[2], 0.0075d, 0.0075d };
+        var speed = new[] { integrated.U[0], integrated.U[1], integrated.U[2], 0d, 0d };
+        var acceleration = new[] { integrated.A[0], integrated.A[1], integrated.A[2], 0d, 0d };
+        return new LongitudinalCandidate(times, pathS, speed, acceleration, jerk);
+    }
+
+    private static double[] ToPrimal(LongitudinalCandidate candidate)
+    {
+        var layout = new LongitudinalVariableLayout(candidate.KnotTimes.Count);
+        var primal = new double[layout.VariableCount];
+        for (int index = 0; index < layout.KnotCount; index++)
+        {
+            primal[layout.S(index)] = candidate.S[index];
+            primal[layout.U(index)] = candidate.U[index];
+            primal[layout.A(index)] = candidate.A[index];
+        }
+        for (int index = 0; index < layout.KnotCount - 1; index++)
+            primal[layout.J(index)] = candidate.J[index];
+        return primal;
+    }
+
+    private static double[] SolveThreeByThree(double[,] matrix, IReadOnlyList<double> rightHandSide)
+    {
+        var augmented = new double[3, 4];
+        for (int row = 0; row < 3; row++)
+        {
+            for (int column = 0; column < 3; column++)
+                augmented[row, column] = matrix[row, column];
+            augmented[row, 3] = rightHandSide[row];
+        }
+        for (int pivot = 0; pivot < 3; pivot++)
+        {
+            int bestRow = pivot;
+            for (int row = pivot + 1; row < 3; row++)
+            {
+                if (Math.Abs(augmented[row, pivot]) > Math.Abs(augmented[bestRow, pivot]))
+                    bestRow = row;
+            }
+            for (int column = pivot; column < 4; column++)
+            {
+                double temporary = augmented[pivot, column];
+                augmented[pivot, column] = augmented[bestRow, column];
+                augmented[bestRow, column] = temporary;
+            }
+            double divisor = augmented[pivot, pivot];
+            for (int column = pivot; column < 4; column++)
+                augmented[pivot, column] /= divisor;
+            for (int row = 0; row < 3; row++)
+            {
+                if (row == pivot)
+                    continue;
+                double factor = augmented[row, pivot];
+                for (int column = pivot; column < 4; column++)
+                    augmented[row, column] -= factor * augmented[pivot, column];
+            }
+        }
+        return new[] { augmented[0, 3], augmented[1, 3], augmented[2, 3] };
+    }
+
    private static void VerifiesTimeKnotLayoutDynamicsObjectiveAndHardConstraints()
    {
        var layout = new LongitudinalVariableLayout(5);
@@ -370,8 +598,21 @@ internal static class LongitudinalModelChecks
        Verification.NearlyEqual(2d, sUpper, "S upper bound");
        FindSingleVariableBounds(problem, layout.U(1), out double uLower, out double uUpper);
        Verification.NearlyEqual(0d, uLower, "U nonnegative bound");
-        Verification.NearlyEqual(envelope.MaximumSpeedAt(integrated.S[1]), uUpper,
-            "U upper bound samples envelope at current S iterate");
+        Verification.NearlyEqual(input.DirectionMaximumSpeedMetersPerSecond, uUpper,
+            "U retains its direction hard bound alongside the PathS envelope");
+        int envelopeSegment = 0;
+        while (envelopeSegment < envelope.PathS.Count - 2 && integrated.S[1] > envelope.PathS[envelopeSegment + 1])
+            envelopeSegment++;
+        double envelopeSlope = (envelope.MaximumSpeedMetersPerSecond[envelopeSegment + 1] -
+            envelope.MaximumSpeedMetersPerSecond[envelopeSegment]) /
+            (envelope.PathS[envelopeSegment + 1] - envelope.PathS[envelopeSegment]);
+        double envelopeIntercept = envelope.MaximumSpeedMetersPerSecond[envelopeSegment] -
+            envelopeSlope * envelope.PathS[envelopeSegment];
+        Verification.Equal(1, CountBoundedRow(problem, new Dictionary<int, double>
+        {
+            { layout.U(1), 1d }, { layout.S(1), -envelopeSlope },
+        }, -QuadraticProgram.MaximumFiniteBound, envelopeIntercept),
+            "U upper bound linearly re-evaluates the actual PathS envelope");
        FindSingleVariableBounds(problem, layout.A(1), out double aLower, out double aUpper);
        Verification.NearlyEqual(-1d, aLower, "deceleration lower bound");
        Verification.NearlyEqual(1d, aUpper, "acceleration upper bound");