feat: add solver-neutral QP contracts

ClumsyPilot/tests/EMPlannerVerificationHost/OptimizationChecks.cs

@@ -0,0 +1,142 @@
+using System;
+using System.Collections.Generic;
+
+namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
+
+internal static class OptimizationChecks
+{
+    public static void Run()
+    {
+        VerifyCanonicalCscAssembly();
+        VerifyInvalidTripletsAreRejected();
+        VerifyUpperTriangularHessianStorage();
+        VerifyQuadraticProgramValidation();
+        VerifyQuadraticProgramDefensivelyCopiesInputs();
+    }
+
+    private static void VerifyCanonicalCscAssembly()
+    {
+        var builder = new SparseTripletBuilder(3, 2);
+        builder.Add(2, 1, 1.5d);
+        builder.Add(0, 0, 2d);
+        builder.Add(1, 1, 3d);
+        builder.Add(2, 1, 0.5d);
+        builder.Add(1, 0, -2d);
+        builder.Add(0, 1, 7d);
+        builder.Add(0, 1, -7d);
+
+        SparseCscMatrix matrix = builder.Build();
+
+        EMPlannerVerificationHost.Verification.Equal(3, matrix.ColumnPointers.Count, "CSC column-pointer length");
+        EMPlannerVerificationHost.Verification.Equal(0, matrix.ColumnPointers[0], "CSC first pointer");
+        EMPlannerVerificationHost.Verification.Equal(2, matrix.ColumnPointers[1], "CSC second pointer");
+        EMPlannerVerificationHost.Verification.Equal(4, matrix.ColumnPointers[2], "CSC final pointer");
+        EMPlannerVerificationHost.Verification.Equal(4, matrix.Values.Count, "CSC nonzero count");
+        EMPlannerVerificationHost.Verification.Equal(0, matrix.RowIndices[0], "CSC first row");
+        EMPlannerVerificationHost.Verification.Equal(1, matrix.RowIndices[1], "CSC second row");
+        EMPlannerVerificationHost.Verification.Equal(1, matrix.RowIndices[2], "CSC third row");
+        EMPlannerVerificationHost.Verification.Equal(2, matrix.RowIndices[3], "CSC fourth row");
+        EMPlannerVerificationHost.Verification.NearlyEqual(2d, matrix.Values[0], "CSC first value");
+        EMPlannerVerificationHost.Verification.NearlyEqual(-2d, matrix.Values[1], "CSC second value");
+        EMPlannerVerificationHost.Verification.NearlyEqual(3d, matrix.Values[2], "CSC third value");
+        EMPlannerVerificationHost.Verification.NearlyEqual(2d, matrix.Values[3], "CSC duplicate sum");
+    }
+
+    private static void VerifyInvalidTripletsAreRejected()
+    {
+        var builder = new SparseTripletBuilder(2, 2);
+
+        AssertArgumentOutOfRange(() => builder.Add(-1, 0, 1d), "negative row");
+        AssertArgumentOutOfRange(() => builder.Add(0, -1, 1d), "negative column");
+        AssertArgumentOutOfRange(() => builder.Add(2, 0, 1d), "row outside matrix");
+        AssertArgumentOutOfRange(() => builder.Add(0, 2, 1d), "column outside matrix");
+        AssertArgumentOutOfRange(() => builder.Add(0, 0, double.NaN), "NaN triplet");
+        AssertArgumentOutOfRange(() => builder.Add(0, 0, double.PositiveInfinity), "infinite triplet");
+    }
+
+    private static void VerifyUpperTriangularHessianStorage()
+    {
+        var upperBuilder = new SparseTripletBuilder(2, 2, true);
+        upperBuilder.Add(0, 0, 1d);
+        upperBuilder.Add(0, 1, 2d);
+        AssertArgumentOutOfRange(() => upperBuilder.Add(1, 0, 3d), "lower-triangular Hessian entry");
+
+        SparseCscMatrix upperHessian = upperBuilder.Build();
+        var constraints = new SparseTripletBuilder(1, 2);
+        constraints.Add(0, 0, 1d);
+        constraints.Add(0, 1, 1d);
+
+        var lowerTriangleBuilder = new SparseTripletBuilder(2, 2);
+        lowerTriangleBuilder.Add(0, 0, 1d);
+        lowerTriangleBuilder.Add(1, 0, 3d);
+        AssertArgumentOutOfRange(
+            () => new QuadraticProgram(lowerTriangleBuilder.Build(), new[] { 0d, 0d }, constraints.Build(), new[] { 0d }, new[] { 1d }),
+            "lower-triangular quadratic-program Hessian");
+
+        EMPlannerVerificationHost.Verification.Equal(2, upperHessian.ColumnCount, "upper Hessian column count");
+    }
+
+    private static void VerifyQuadraticProgramValidation()
+    {
+        var nonSquareHessian = new SparseCscMatrix(1, 2, new double[0], new int[0], new[] { 0, 0, 0 });
+        var oneVariableHessian = new SparseTripletBuilder(1, 1, true);
+        oneVariableHessian.Add(0, 0, 1d);
+        var oneConstraint = new SparseTripletBuilder(1, 1);
+        oneConstraint.Add(0, 0, 1d);
+
+        AssertArgumentOutOfRange(
+            () => new QuadraticProgram(nonSquareHessian, new[] { 0d, 0d }, oneConstraint.Build(), new[] { 0d }, new[] { 1d }),
+            "non-square Hessian");
+        AssertArgumentOutOfRange(
+            () => new QuadraticProgram(oneVariableHessian.Build(), new[] { 0d }, oneConstraint.Build(), new[] { 2d }, new[] { 1d }),
+            "inverted constraint bounds");
+        AssertArgumentOutOfRange(
+            () => new QuadraticProgram(oneVariableHessian.Build(), new[] { double.NaN }, oneConstraint.Build(), new[] { 0d }, new[] { 1d }),
+            "non-finite linear cost");
+        AssertArgumentOutOfRange(
+            () => new QuadraticProgram(oneVariableHessian.Build(), new[] { 0d }, oneConstraint.Build(), new[] { -1e30d }, new[] { 2e30d }),
+            "out-of-range finite bound");
+    }
+
+    private static void VerifyQuadraticProgramDefensivelyCopiesInputs()
+    {
+        var hessianBuilder = new SparseTripletBuilder(1, 1, true);
+        hessianBuilder.Add(0, 0, 1d);
+        var constraintBuilder = new SparseTripletBuilder(1, 1);
+        constraintBuilder.Add(0, 0, 1d);
+
+        var linearCost = new List<double> { -2d };
+        var lowerBounds = new List<double> { 0d };
+        var upperBounds = new List<double> { 1d };
+        QuadraticProgram problem = new QuadraticProgram(
+            hessianBuilder.Build(),
+            linearCost,
+            constraintBuilder.Build(),
+            lowerBounds,
+            upperBounds);
+
+        linearCost[0] = 100d;
+        lowerBounds[0] = -100d;
+        upperBounds[0] = 100d;
+
+        EMPlannerVerificationHost.Verification.Equal(1, problem.VariableCount, "micro problem variable count");
+        EMPlannerVerificationHost.Verification.Equal(1, problem.ConstraintCount, "micro problem constraint count");
+        EMPlannerVerificationHost.Verification.NearlyEqual(-2d, problem.LinearCost[0], "copied linear cost");
+        EMPlannerVerificationHost.Verification.NearlyEqual(0d, problem.LowerBounds[0], "copied lower bound");
+        EMPlannerVerificationHost.Verification.NearlyEqual(1d, problem.UpperBounds[0], "copied upper bound");
+    }
+
+    private static void AssertArgumentOutOfRange(Action action, string name)
+    {
+        try
+        {
+            action();
+        }
+        catch (ArgumentOutOfRangeException)
+        {
+            return;
+        }
+
+        throw new InvalidOperationException(name + " did not throw ArgumentOutOfRangeException.");
+    }
+}

ClumsyPilot/tests/EMPlannerVerificationHost/Program.cs

@@ -7,9 +7,9 @@ internal static class Program
     private static int Main(string[] args)
     {
         if (args.Length != 1 || (args[0] != "foundation" && args[0] != "segmentation" && args[0] != "frenet" &&
-            args[0] != "corridor" && args[0] != "all-foundation"))
+            args[0] != "corridor" && args[0] != "optimization" && args[0] != "all-foundation"))
         {
-            Console.Error.WriteLine("Usage: EMPlannerVerificationHost foundation|segmentation|frenet|corridor|all-foundation");
+            Console.Error.WriteLine("Usage: EMPlannerVerificationHost foundation|segmentation|frenet|corridor|optimization|all-foundation");
             return 2;
         }
 
@@ -35,6 +35,11 @@ internal static class Program
                 MultiWheelC.TrajectoryPlanning.EMPlanner.CorridorChecks.Run();
                 Console.WriteLine("PASS corridor");
             }
+            if (args[0] == "optimization")
+            {
+                MultiWheelC.TrajectoryPlanning.EMPlanner.OptimizationChecks.Run();
+                Console.WriteLine("PASS optimization");
+            }
             return 0;
         }
         catch (Exception exception)