feat: assemble lateral LS quadratic programs

2026-08-04 00:58:09 +08:00
parent c225b17d37
commit f703d418ab
4 changed files with 766 additions and 8 deletions
@@ -0,0 +1,39 @@
+using System;
+using System.Collections.Generic;
+using System.Collections.ObjectModel;
+using System.Threading;
+using MultiWheelC.TrajectoryPlanning.EMPlanner;
+
+namespace EMPlannerVerificationHost;
+
+internal sealed class FakeQpSolver : IQpSolver
+{
+    private readonly QpSolveResult _result;
+
+    public FakeQpSolver(QpSolveResult result)
+    {
+        _result = result ?? throw new ArgumentNullException(nameof(result));
+        LastWarmStart = Array.Empty<double>();
+    }
+
+    public QuadraticProgram? LastProblem { get; private set; }
+
+    public QpSolverSettings? LastSettings { get; private set; }
+
+    public IReadOnlyList<double> LastWarmStart { get; private set; }
+
+    public QpSolveResult Solve(QuadraticProgram problem, QpSolverSettings settings, IReadOnlyList<double> warmStart,
+        CancellationToken cancellationToken)
+    {
+        LastProblem = problem ?? throw new ArgumentNullException(nameof(problem));
+        LastSettings = settings ?? throw new ArgumentNullException(nameof(settings));
+        var copy = new List<double>(warmStart == null ? 0 : warmStart.Count);
+        if (warmStart != null)
+        {
+            for (int index = 0; index < warmStart.Count; index++)
+                copy.Add(warmStart[index]);
+        }
+        LastWarmStart = new ReadOnlyCollection<double>(copy);
+        return _result;
+    }
+}
@@ -1,5 +1,6 @@
 using System;
 using System.Collections.Generic;
+using System.Threading;
 using EMPlannerVerificationHost;
 using MultiWheelC.TrajectoryPlanning.CoarsePath;
 using MultiWheelC.TrajectoryPlanning.CoarsePath.Vehicle;
@@ -15,6 +16,10 @@ internal static class LateralModelChecks
        VerifiesExactDiscreteDynamicsForUnequalStations();
        VerifiesPlanningInputBoundariesAndDefensiveCopies();
        VerifiesLateralResultPublicationContract();
+        VerifiesNormalizedObjectiveAndHardConstraints();
+        VerifiesAllNamedCostScales();
+        VerifiesEmptyHardBoundIntersectionFailsBeforeSolve();
+        VerifiesFakeSolverCapturesTheNeutralQpBoundary();
    }

    private static void VerifiesDeterministicVariableLayout()
@@ -127,33 +132,291 @@ internal static class LateralModelChecks
        Verification.Equal(validated, result.Path, "fallback path is preserved");
    }

-    private static DirectionSegmentView CreateStraightSegment()
+    private static void VerifiesNormalizedObjectiveAndHardConstraints()
+    {
+        EmPlannerConfiguration configuration = CreateUnitScaleConfiguration();
+        LateralPlanningInput input = CreateModelInput(EmTerminalType.Goal, configuration,
+            new[] { 0.2d, -0.1d, 0.3d });
+        LateralCandidate linearization = LateralCandidate.Integrate(input.ReferenceStations, 0d, 0d, 0d,
+            new[] { 0d, 0d });
+        LateralConstraintBuilder builder = CreateConstraintBuilder();
+
+        Verification.True(builder.TryBuild(input, linearization, out QuadraticProgram problem, out string failureReason),
+            "unit-scale QP builds: " + failureReason);
+        var layout = new LateralVariableLayout(3);
+        Verification.NearlyEqual(30d, MatrixValue(problem.UpperTriangularP, layout.L(0), layout.L(0)),
+            "reference plus previous P coefficient");
+        Verification.NearlyEqual(20d, MatrixValue(problem.UpperTriangularP, layout.DDDL(0), layout.DDDL(0)),
+            "jerk P coefficient");
+        Verification.NearlyEqual(-2d, problem.LinearCost[layout.L(0)], "previous-seed q coefficient");
+
+        for (int interval = 0; interval < 2; interval++)
+        {
+            double ds = input.ReferenceStations[interval + 1] - input.ReferenceStations[interval];
+            Verification.Equal(1, CountExactEqualityRows(problem, new Dictionary<int, double>
+            {
+                { layout.DDL(interval), -1d },
+                { layout.DDL(interval + 1), 1d },
+                { layout.DDDL(interval), -ds },
+            }), "ddl dynamics equality " + interval);
+            Verification.Equal(1, CountExactEqualityRows(problem, new Dictionary<int, double>
+            {
+                { layout.DL(interval), -1d },
+                { layout.DL(interval + 1), 1d },
+                { layout.DDL(interval), -ds },
+                { layout.DDDL(interval), -0.5d * ds * ds },
+            }), "dl dynamics equality " + interval);
+            Verification.Equal(1, CountExactEqualityRows(problem, new Dictionary<int, double>
+            {
+                { layout.L(interval), -1d },
+                { layout.L(interval + 1), 1d },
+                { layout.DL(interval), -ds },
+                { layout.DDL(interval), -0.5d * ds * ds },
+                { layout.DDDL(interval), -ds * ds * ds / 6d },
+            }), "l dynamics equality " + interval);
+        }
+
+        for (int station = 0; station < layout.StationCount; station++)
+        {
+            Verification.True(HasFiniteNonEqualityBound(problem, layout.L(station)), "finite lateral hard bound " + station);
+            Verification.True(HasFiniteNonEqualityBound(problem, layout.DL(station)), "finite slope hard bound " + station);
+            Verification.True(HasFiniteNonEqualityBound(problem, layout.DDL(station)), "finite second-derivative hard bound " + station);
+        }
+        for (int interval = 0; interval < layout.StationCount - 1; interval++)
+            Verification.True(HasFiniteNonEqualityBound(problem, layout.DDDL(interval)), "finite jerk hard bound " + interval);
+
+        Verification.Equal(1, CountExactEqualityRows(problem, new Dictionary<int, double> { { layout.L(2), 1d } }),
+            "goal terminal l equality");
+        Verification.Equal(1, CountExactEqualityRows(problem, new Dictionary<int, double> { { layout.DL(2), 1d } }),
+            "goal terminal dl equality");
+
+        LateralPlanningInput rolling = CreateModelInput(EmTerminalType.RollingSafetyStop, configuration,
+            new[] { 0.2d, -0.1d, 0.3d });
+        Verification.True(builder.TryBuild(rolling, linearization, out QuadraticProgram rollingProblem, out string rollingReason),
+            "rolling QP builds: " + rollingReason);
+        Verification.NearlyEqual(50d, MatrixValue(rollingProblem.UpperTriangularP, layout.L(2), layout.L(2)),
+            "rolling terminal adds normalized objective cost");
+        Verification.Equal(0, CountExactEqualityRows(rollingProblem, new Dictionary<int, double> { { layout.L(2), 1d } }),
+            "rolling terminal has no l equality");
+        Verification.Equal(0, CountExactEqualityRows(rollingProblem, new Dictionary<int, double> { { layout.DL(2), 1d } }),
+            "rolling terminal has no dl equality");
+    }
+
+    private static void VerifiesAllNamedCostScales()
+    {
+        EmPlannerConfiguration configuration = EmPlannerConfiguration.CreateDefault();
+        configuration.Corridor.MaximumLateralOffsetMeters = 2d;
+        configuration.Lateral.MaximumLateralSlope = 4d;
+        configuration.Lateral.MaximumLateralSecondDerivativePerMeter = 5d;
+        configuration.Lateral.MaximumLateralThirdDerivativePerSquareMeter = 6d;
+        LateralPlanningInput input = CreateModelInput(EmTerminalType.RollingSafetyStop, configuration,
+            new[] { 0.2d, 0.2d, 0.2d }, referenceCurvatureDerivative: 4d, maximumVehicleCurvature: 7d);
+        LateralCandidate linearization = LateralCandidate.Integrate(input.ReferenceStations, 0d, 0d, 0d,
+            new[] { 0d, 0d });
+        LateralConstraintBuilder builder = CreateConstraintBuilder();
+
+        Verification.True(builder.TryBuild(input, linearization, out QuadraticProgram problem, out string failureReason),
+            "non-unit-scale QP builds: " + failureReason);
+        var layout = new LateralVariableLayout(3);
+        Verification.NearlyEqual(7.5d, MatrixValue(problem.UpperTriangularP, layout.L(0), layout.L(0)),
+            "reference and previous costs divide by lateral scale squared");
+        Verification.NearlyEqual(-0.5d, problem.LinearCost[layout.L(0)],
+            "previous target coefficient divides by lateral scale squared");
+        Verification.NearlyEqual(0.125d, MatrixValue(problem.UpperTriangularP, layout.DL(0), layout.DL(0)),
+            "heading cost divides by slope scale squared");
+        Verification.NearlyEqual(5d / 9d, MatrixValue(problem.UpperTriangularP, layout.DDDL(0), layout.DDDL(0)),
+            "jerk cost divides by third-derivative scale squared");
+        Verification.NearlyEqual(0.4d + 10d / 49d + 3.125d,
+            MatrixValue(problem.UpperTriangularP, layout.DDL(0), layout.DDL(0)),
+            "second derivative, curvature, and curvature variation use their named scales");
+        Verification.NearlyEqual(12.5d, MatrixValue(problem.UpperTriangularP, layout.L(2), layout.L(2)),
+            "rolling terminal cost divides by lateral scale squared");
+
+        EmPlannerConfiguration denominatorConfiguration = CreateUnitScaleConfiguration();
+        denominatorConfiguration.Lateral.MaximumLateralStepPerIterationMeters = 0.5d;
+        LateralPlanningInput denominatorInput = CreateModelInput(EmTerminalType.RollingSafetyStop, denominatorConfiguration,
+            Array.Empty<double>(), 0d, 2d);
+        Verification.True(builder.TryBuild(denominatorInput, linearization, out QuadraticProgram denominatorProblem,
+            out string denominatorReason), "denominator QP builds: " + denominatorReason);
+        Verification.True(HasBoundWithUpper(denominatorProblem, layout.L(0), 0.4d),
+            "Frenet denominator is intersected as a finite hard lateral bound");
+    }
+
+    private static void VerifiesEmptyHardBoundIntersectionFailsBeforeSolve()
+    {
+        EmPlannerConfiguration configuration = CreateUnitScaleConfiguration();
+        LateralPlanningInput input = CreateModelInput(EmTerminalType.RollingSafetyStop, configuration,
+            Array.Empty<double>(), 0d, 0d, 0.9d, 0.9d, 1d);
+        LateralCandidate linearization = LateralCandidate.Integrate(input.ReferenceStations, 0d, 0d, 0d,
+            new[] { 0d, 0d });
+
+        Verification.True(!CreateConstraintBuilder().TryBuild(input, linearization, out QuadraticProgram problem,
+            out string failureReason), "empty corridor/trust intersection is infeasible before solve");
+        Verification.True(problem == null && failureReason.Length > 0, "infeasible build returns no QP and a reason");
+    }
+
+    private static void VerifiesFakeSolverCapturesTheNeutralQpBoundary()
+    {
+        EmPlannerConfiguration configuration = CreateUnitScaleConfiguration();
+        LateralPlanningInput input = CreateModelInput(EmTerminalType.Goal, configuration, Array.Empty<double>());
+        LateralCandidate linearization = LateralCandidate.Integrate(input.ReferenceStations, 0d, 0d, 0d,
+            new[] { 0d, 0d });
+        Verification.True(CreateConstraintBuilder().TryBuild(input, linearization, out QuadraticProgram problem,
+            out string reason), "fake solver problem builds: " + reason);
+        var expected = new QpSolveResult(QpSolveStatus.Solved, new double[problem.VariableCount], 0d, 0d, 0d, 1,
+            TimeSpan.Zero, "fake", string.Empty);
+        var solver = new FakeQpSolver(expected);
+        var settings = new QpSolverSettings(10, 1e-5d, 1e-5d, TimeSpan.FromSeconds(1d), true, false, false);
+        QpSolveResult actual = solver.Solve(problem, settings, new[] { 1d, 2d }, CancellationToken.None);
+
+        Verification.Equal(expected, actual, "fake solver returns configured result");
+        Verification.Equal(problem, solver.LastProblem, "fake solver records QP");
+        Verification.Equal(settings, solver.LastSettings, "fake solver records settings");
+        Verification.NearlyEqual(2d, solver.LastWarmStart[1], "fake solver records a defensive warm-start copy");
+    }
+
+    private static DirectionSegmentView CreateStraightSegment(double referenceCurvatureDerivative = 0d,
+        double referenceCurvature = 0d)
    {
        var points = new List<SmoothedPathPoint>
        {
-            Point(0d, 0d),
-            Point(1d, 1d),
-            Point(2d, 2d),
+            Point(0d, 0d, referenceCurvatureDerivative, referenceCurvature),
+            Point(1d, 1d, referenceCurvatureDerivative, referenceCurvature),
+            Point(2d, 2d, referenceCurvatureDerivative, referenceCurvature),
        };
        return new DirectionSegmentView(0, TravelDirection.Forward, points,
            new ReferenceBoundary(0, 0d, EmBoundaryType.None, 0d),
            new ReferenceBoundary(0, 2d, EmBoundaryType.Goal, 2d), 0d);
    }

-    private static SmoothedPathPoint Point(double x, double s)
+    private static SmoothedPathPoint Point(double x, double s, double curvatureDerivative = 0d, double curvature = 0d)
    {
-        return new SmoothedPathPoint(x, 0d, 0d, 0d, s, TravelDirection.Forward, 0d, 0d, 0d, 1d,
+        return new SmoothedPathPoint(x, 0d, 0d, 0d, s, TravelDirection.Forward, curvature, curvature,
+            curvatureDerivative, 1d,
            false, SmoothedPathPointSource.Anchor);
    }

-    private static VehicleParameters CreateVehicle()
+    private static EmPlannerConfiguration CreateUnitScaleConfiguration()
+    {
+        EmPlannerConfiguration configuration = EmPlannerConfiguration.CreateDefault();
+        configuration.Corridor.MaximumLateralOffsetMeters = 1d;
+        configuration.Lateral.MaximumLateralSlope = 1d;
+        configuration.Lateral.MaximumLateralSecondDerivativePerMeter = 1d;
+        configuration.Lateral.MaximumLateralThirdDerivativePerSquareMeter = 1d;
+        return configuration;
+    }
+
+    private static LateralPlanningInput CreateModelInput(EmTerminalType terminalType, EmPlannerConfiguration configuration,
+        IReadOnlyList<double> previousL, double referenceCurvatureDerivative = 0d, double referenceCurvature = 0d,
+        double startL = 0d, double corridorMinimum = -1d, double corridorMaximum = 1d,
+        double maximumVehicleCurvature = 1d)
+    {
+        DirectionSegmentView segment = CreateStraightSegment(referenceCurvatureDerivative, referenceCurvature);
+        double corridorSeed = Math.Max(corridorMinimum, Math.Min(corridorMaximum, 0d));
+        var stations = new[]
+        {
+            new LateralInterval(0d, corridorMinimum, corridorMaximum, startL),
+            new LateralInterval(1d, corridorMinimum, corridorMaximum, corridorSeed),
+            new LateralInterval(2d, corridorMinimum, corridorMaximum, corridorSeed),
+        };
+        var seed = new List<FrenetProjection>();
+        for (int index = 0; index < previousL.Count; index++)
+            seed.Add(new FrenetProjection(ReferencePathInterpolator.Interpolate(segment, index), previousL[index], 0d, 0d));
+        return new LateralPlanningInput(segment, new StaticCorridor(stations),
+            new FrenetProjection(ReferencePathInterpolator.Interpolate(segment, 0d), startL, 0d, 0d), terminalType,
+            CreateVehicle(maximumVehicleCurvature), configuration, seed);
+    }
+
+    private static LateralObjectiveBuilder CreateObjectiveBuilder()
+    {
+        return new LateralObjectiveBuilder();
+    }
+
+    private static LateralConstraintBuilder CreateConstraintBuilder()
+    {
+        return new LateralConstraintBuilder(CreateObjectiveBuilder());
+    }
+
+    private static double MatrixValue(SparseCscMatrix matrix, int row, int column)
+    {
+        for (int index = matrix.ColumnPointers[column]; index < matrix.ColumnPointers[column + 1]; index++)
+        {
+            if (matrix.RowIndices[index] == row)
+                return matrix.Values[index];
+        }
+        return 0d;
+    }
+
+    private static int CountExactEqualityRows(QuadraticProgram problem, IReadOnlyDictionary<int, double> expected)
+    {
+        int count = 0;
+        for (int row = 0; row < problem.ConstraintCount; row++)
+        {
+            if (Math.Abs(problem.LowerBounds[row] - problem.UpperBounds[row]) <= 1e-12d &&
+                RowMatches(problem.ConstraintMatrix, row, expected))
+            {
+                count++;
+            }
+        }
+        return count;
+    }
+
+    private static bool HasFiniteNonEqualityBound(QuadraticProgram problem, int variable)
+    {
+        for (int row = 0; row < problem.ConstraintCount; row++)
+        {
+            if (Math.Abs(problem.LowerBounds[row] - problem.UpperBounds[row]) > 1e-12d &&
+                RowMatches(problem.ConstraintMatrix, row, new Dictionary<int, double> { { variable, 1d } }) &&
+                !double.IsInfinity(problem.LowerBounds[row]) && !double.IsInfinity(problem.UpperBounds[row]))
+            {
+                return true;
+            }
+        }
+        return false;
+    }
+
+    private static bool HasBoundWithUpper(QuadraticProgram problem, int variable, double upper)
+    {
+        for (int row = 0; row < problem.ConstraintCount; row++)
+        {
+            if (RowMatches(problem.ConstraintMatrix, row, new Dictionary<int, double> { { variable, 1d } }) &&
+                Math.Abs(problem.UpperBounds[row] - upper) <= 1e-12d)
+            {
+                return true;
+            }
+        }
+        return false;
+    }
+
+    private static bool RowMatches(SparseCscMatrix matrix, int targetRow, IReadOnlyDictionary<int, double> expected)
+    {
+        var actual = new Dictionary<int, double>();
+        for (int column = 0; column < matrix.ColumnCount; column++)
+        {
+            for (int index = matrix.ColumnPointers[column]; index < matrix.ColumnPointers[column + 1]; index++)
+            {
+                if (matrix.RowIndices[index] == targetRow)
+                    actual[column] = matrix.Values[index];
+            }
+        }
+        if (actual.Count != expected.Count)
+            return false;
+        foreach (KeyValuePair<int, double> expectedEntry in expected)
+        {
+            if (!actual.TryGetValue(expectedEntry.Key, out double value) || Math.Abs(value - expectedEntry.Value) > 1e-12d)
+                return false;
+        }
+        return true;
+    }
+
+    private static VehicleParameters CreateVehicle(double maximumCurvature = 1d)
    {
        return new VehicleParameters
        {
            LengthMeters = 0.1d,
            WidthMeters = 0.1d,
            SafetyMarginMeters = 0d,
-            MaximumCurvaturePerMeter = 1d,
+            MaximumCurvaturePerMeter = maximumCurvature,
        };
    }