feat: add solver-neutral QP contracts

ClumsyPilot/ParkrobTrajplanner/EMPlanner/Optimization/IQpSolver.cs

@@ -0,0 +1,13 @@
+using System.Collections.Generic;
+using System.Threading;
+
+namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
+
+public interface IQpSolver
+{
+    QpSolveResult Solve(
+        QuadraticProgram problem,
+        QpSolverSettings settings,
+        IReadOnlyList<double> warmStart,
+        CancellationToken cancellationToken);
+}

ClumsyPilot/ParkrobTrajplanner/EMPlanner/Optimization/QpSolveResult.cs

@@ -0,0 +1,68 @@
+using System;
+using System.Collections.Generic;
+using System.Collections.ObjectModel;
+using MultiWheelC.TrajectoryPlanning.Utils;
+
+namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
+
+public sealed class QpSolveResult
+{
+    public QpSolveResult(
+        QpSolveStatus status,
+        IReadOnlyList<double> primal,
+        double objective,
+        double primalResidual,
+        double dualResidual,
+        int iterations,
+        TimeSpan solveTime,
+        string nativeStatus,
+        string diagnostic)
+    {
+        if (!Enum.IsDefined(typeof(QpSolveStatus), status))
+            throw new ArgumentOutOfRangeException(nameof(status));
+        if (primal == null)
+            throw new ArgumentNullException(nameof(primal));
+        if (iterations < 0)
+            throw new ArgumentOutOfRangeException(nameof(iterations));
+        if (solveTime < TimeSpan.Zero)
+            throw new ArgumentOutOfRangeException(nameof(solveTime));
+        if (!NumericGuard.IsFinite(objective) || !NumericGuard.IsFinite(primalResidual) || !NumericGuard.IsFinite(dualResidual))
+            throw new ArgumentOutOfRangeException(nameof(objective), "Solver metrics must be finite.");
+
+        var copiedPrimal = new List<double>(primal.Count);
+        for (int index = 0; index < primal.Count; index++)
+        {
+            if (!NumericGuard.IsFinite(primal[index]))
+                throw new ArgumentOutOfRangeException(nameof(primal), "Primal values must be finite.");
+            copiedPrimal.Add(primal[index]);
+        }
+
+        Status = status;
+        Primal = new ReadOnlyCollection<double>(copiedPrimal);
+        Objective = objective;
+        PrimalResidual = primalResidual;
+        DualResidual = dualResidual;
+        Iterations = iterations;
+        SolveTime = solveTime;
+        NativeStatus = nativeStatus ?? string.Empty;
+        Diagnostic = diagnostic ?? string.Empty;
+    }
+
+    public QpSolveStatus Status { get; }
+
+    public IReadOnlyList<double> Primal { get; }
+
+    public double Objective { get; }
+
+    public double PrimalResidual { get; }
+
+    public double DualResidual { get; }
+
+    public int Iterations { get; }
+
+    public TimeSpan SolveTime { get; }
+
+    public string NativeStatus { get; }
+
+    public string Diagnostic { get; }
+}

ClumsyPilot/ParkrobTrajplanner/EMPlanner/Optimization/QpSolveStatus.cs

@@ -0,0 +1,15 @@
+namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
+
+public enum QpSolveStatus
+{
+    Solved,
+    SolvedInaccurate,
+    PrimalInfeasible,
+    DualInfeasible,
+    MaximumIterations,
+    TimeLimit,
+    Cancelled,
+    SolverUnavailable,
+    InvalidProblem,
+    NativeError,
+}

ClumsyPilot/ParkrobTrajplanner/EMPlanner/Optimization/QpSolverSettings.cs

@@ -0,0 +1,48 @@
+using System;
+using MultiWheelC.TrajectoryPlanning.Utils;
+
+namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
+
+public sealed class QpSolverSettings
+{
+    public QpSolverSettings(
+        int maximumIterations,
+        double absoluteTolerance,
+        double relativeTolerance,
+        TimeSpan timeLimit,
+        bool enableWarmStart,
+        bool enablePolishing,
+        bool enableNativeVerboseOutput)
+    {
+        if (maximumIterations <= 0)
+            throw new ArgumentOutOfRangeException(nameof(maximumIterations));
+        if (!NumericGuard.IsPositiveFinite(absoluteTolerance))
+            throw new ArgumentOutOfRangeException(nameof(absoluteTolerance));
+        if (!NumericGuard.IsPositiveFinite(relativeTolerance))
+            throw new ArgumentOutOfRangeException(nameof(relativeTolerance));
+        if (timeLimit <= TimeSpan.Zero)
+            throw new ArgumentOutOfRangeException(nameof(timeLimit));
+
+        MaximumIterations = maximumIterations;
+        AbsoluteTolerance = absoluteTolerance;
+        RelativeTolerance = relativeTolerance;
+        TimeLimit = timeLimit;
+        EnableWarmStart = enableWarmStart;
+        EnablePolishing = enablePolishing;
+        EnableNativeVerboseOutput = enableNativeVerboseOutput;
+    }
+
+    public int MaximumIterations { get; }
+
+    public double AbsoluteTolerance { get; }
+
+    public double RelativeTolerance { get; }
+
+    public TimeSpan TimeLimit { get; }
+
+    public bool EnableWarmStart { get; }
+
+    public bool EnablePolishing { get; }
+
+    public bool EnableNativeVerboseOutput { get; }
+}

ClumsyPilot/ParkrobTrajplanner/EMPlanner/Optimization/QuadraticProgram.cs

@@ -0,0 +1,113 @@
+using System;
+using System.Collections.Generic;
+using System.Collections.ObjectModel;
+using MultiWheelC.TrajectoryPlanning.Utils;
+
+namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
+
+public sealed class QuadraticProgram
+{
+    public const double MaximumFiniteBound = 1e30d;
+
+    public QuadraticProgram(
+        SparseCscMatrix upperTriangularP,
+        IReadOnlyList<double> q,
+        SparseCscMatrix a,
+        IReadOnlyList<double> lowerBounds,
+        IReadOnlyList<double> upperBounds)
+    {
+        if (upperTriangularP == null)
+            throw new ArgumentNullException(nameof(upperTriangularP));
+        if (q == null)
+            throw new ArgumentNullException(nameof(q));
+        if (a == null)
+            throw new ArgumentNullException(nameof(a));
+        if (lowerBounds == null)
+            throw new ArgumentNullException(nameof(lowerBounds));
+        if (upperBounds == null)
+            throw new ArgumentNullException(nameof(upperBounds));
+        if (upperTriangularP.RowCount != upperTriangularP.ColumnCount)
+            throw new ArgumentOutOfRangeException(nameof(upperTriangularP), "The quadratic Hessian must be square.");
+        if (q.Count != upperTriangularP.ColumnCount)
+            throw new ArgumentOutOfRangeException(nameof(q), "The linear cost length must match the variable count.");
+        if (a.ColumnCount != upperTriangularP.ColumnCount)
+            throw new ArgumentOutOfRangeException(nameof(a), "Constraint columns must match the variable count.");
+        if (lowerBounds.Count != a.RowCount || upperBounds.Count != a.RowCount)
+            throw new ArgumentOutOfRangeException(nameof(lowerBounds), "Constraint bounds must match the constraint count.");
+
+        ValidateUpperTriangle(upperTriangularP);
+        ValidateFinite(q, nameof(q));
+        ValidateBounds(lowerBounds, upperBounds);
+
+        UpperTriangularP = CopyMatrix(upperTriangularP);
+        LinearCost = Copy(q);
+        ConstraintMatrix = CopyMatrix(a);
+        LowerBounds = Copy(lowerBounds);
+        UpperBounds = Copy(upperBounds);
+        VariableCount = UpperTriangularP.ColumnCount;
+        ConstraintCount = ConstraintMatrix.RowCount;
+    }
+
+    public SparseCscMatrix UpperTriangularP { get; }
+
+    public IReadOnlyList<double> LinearCost { get; }
+
+    public SparseCscMatrix ConstraintMatrix { get; }
+
+    public IReadOnlyList<double> LowerBounds { get; }
+
+    public IReadOnlyList<double> UpperBounds { get; }
+
+    public int VariableCount { get; }
+
+    public int ConstraintCount { get; }
+
+    private static void ValidateUpperTriangle(SparseCscMatrix matrix)
+    {
+        for (int column = 0; column < matrix.ColumnCount; column++)
+        {
+            for (int index = matrix.ColumnPointers[column]; index < matrix.ColumnPointers[column + 1]; index++)
+            {
+                if (matrix.RowIndices[index] > column)
+                    throw new ArgumentOutOfRangeException(nameof(matrix), "The quadratic Hessian must store only its upper triangle.");
+            }
+        }
+    }
+
+    private static void ValidateFinite(IReadOnlyList<double> values, string parameterName)
+    {
+        for (int index = 0; index < values.Count; index++)
+        {
+            if (!NumericGuard.IsFinite(values[index]))
+                throw new ArgumentOutOfRangeException(parameterName, "Values must be finite.");
+        }
+    }
+
+    private static void ValidateBounds(IReadOnlyList<double> lowerBounds, IReadOnlyList<double> upperBounds)
+    {
+        for (int index = 0; index < lowerBounds.Count; index++)
+        {
+            double lower = lowerBounds[index];
+            double upper = upperBounds[index];
+            if (!NumericGuard.IsFinite(lower) || !NumericGuard.IsFinite(upper) ||
+                lower < -MaximumFiniteBound || upper > MaximumFiniteBound || lower > upper)
+            {
+                throw new ArgumentOutOfRangeException(nameof(lowerBounds),
+                    "Constraint bounds must be finite, within the supported range, and ordered.");
+            }
+        }
+    }
+
+    private static SparseCscMatrix CopyMatrix(SparseCscMatrix source)
+    {
+        return new SparseCscMatrix(source.RowCount, source.ColumnCount, source.Values, source.RowIndices, source.ColumnPointers);
+    }
+
+    private static IReadOnlyList<T> Copy<T>(IReadOnlyList<T> source)
+    {
+        var copy = new List<T>(source.Count);
+        for (int index = 0; index < source.Count; index++)
+            copy.Add(source[index]);
+        return new ReadOnlyCollection<T>(copy);
+    }
+}

ClumsyPilot/ParkrobTrajplanner/EMPlanner/Optimization/SparseCscMatrix.cs

@@ -0,0 +1,87 @@
+using System;
+using System.Collections.Generic;
+using System.Collections.ObjectModel;
+using MultiWheelC.TrajectoryPlanning.Utils;
+
+namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
+
+public sealed class SparseCscMatrix
+{
+    public SparseCscMatrix(
+        int rowCount,
+        int columnCount,
+        IReadOnlyList<double> values,
+        IReadOnlyList<int> rowIndices,
+        IReadOnlyList<int> columnPointers)
+    {
+        if (rowCount < 0)
+            throw new ArgumentOutOfRangeException(nameof(rowCount));
+        if (columnCount < 0)
+            throw new ArgumentOutOfRangeException(nameof(columnCount));
+        if (values == null)
+            throw new ArgumentNullException(nameof(values));
+        if (rowIndices == null)
+            throw new ArgumentNullException(nameof(rowIndices));
+        if (columnPointers == null)
+            throw new ArgumentNullException(nameof(columnPointers));
+        if (values.Count != rowIndices.Count)
+            throw new ArgumentException("CSC values and row indices must have the same length.", nameof(rowIndices));
+        if (columnPointers.Count != columnCount + 1)
+            throw new ArgumentException("CSC column-pointer count must equal column count plus one.", nameof(columnPointers));
+        if (columnPointers[0] != 0)
+            throw new ArgumentOutOfRangeException(nameof(columnPointers), "The first CSC column pointer must be zero.");
+        if (columnPointers[columnCount] != values.Count)
+            throw new ArgumentOutOfRangeException(nameof(columnPointers), "The final CSC column pointer must equal the nonzero count.");
+
+        for (int column = 0; column < columnCount; column++)
+        {
+            if (columnPointers[column] > columnPointers[column + 1])
+                throw new ArgumentOutOfRangeException(nameof(columnPointers), "CSC column pointers must be monotonic.");
+            if (columnPointers[column] < 0 || columnPointers[column + 1] > values.Count)
+                throw new ArgumentOutOfRangeException(nameof(columnPointers), "CSC column pointers must stay within the nonzero count.");
+        }
+
+        for (int index = 0; index < values.Count; index++)
+        {
+            if (!NumericGuard.IsFinite(values[index]))
+                throw new ArgumentOutOfRangeException(nameof(values), "CSC values must be finite.");
+            if (rowIndices[index] < 0 || rowIndices[index] >= rowCount)
+                throw new ArgumentOutOfRangeException(nameof(rowIndices), "CSC row indices must be within matrix bounds.");
+        }
+
+        for (int column = 0; column < columnCount; column++)
+        {
+            int previousRow = -1;
+            for (int index = columnPointers[column]; index < columnPointers[column + 1]; index++)
+            {
+                if (rowIndices[index] <= previousRow)
+                    throw new ArgumentException("CSC row indices must be strictly ascending in each column.", nameof(rowIndices));
+                previousRow = rowIndices[index];
+            }
+        }
+
+        RowCount = rowCount;
+        ColumnCount = columnCount;
+        Values = Copy(values);
+        RowIndices = Copy(rowIndices);
+        ColumnPointers = Copy(columnPointers);
+    }
+
+    public int RowCount { get; }
+
+    public int ColumnCount { get; }
+
+    public IReadOnlyList<double> Values { get; }
+
+    public IReadOnlyList<int> RowIndices { get; }
+
+    public IReadOnlyList<int> ColumnPointers { get; }
+
+    private static IReadOnlyList<T> Copy<T>(IReadOnlyList<T> source)
+    {
+        var copy = new List<T>(source.Count);
+        for (int index = 0; index < source.Count; index++)
+            copy.Add(source[index]);
+        return new ReadOnlyCollection<T>(copy);
+    }
+}

ClumsyPilot/ParkrobTrajplanner/EMPlanner/Optimization/SparseTripletBuilder.cs

@@ -0,0 +1,104 @@
+using System;
+using System.Collections.Generic;
+using MultiWheelC.TrajectoryPlanning.Utils;
+
+namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
+
+public sealed class SparseTripletBuilder
+{
+    private readonly int rowCount;
+    private readonly int columnCount;
+    private readonly bool upperTriangleOnly;
+    private readonly List<Triplet> triplets = new List<Triplet>();
+
+    public SparseTripletBuilder(int rowCount, int columnCount)
+        : this(rowCount, columnCount, false)
+    {
+    }
+
+    public SparseTripletBuilder(int rowCount, int columnCount, bool upperTriangleOnly)
+    {
+        if (rowCount < 0)
+            throw new ArgumentOutOfRangeException(nameof(rowCount));
+        if (columnCount < 0)
+            throw new ArgumentOutOfRangeException(nameof(columnCount));
+
+        this.rowCount = rowCount;
+        this.columnCount = columnCount;
+        this.upperTriangleOnly = upperTriangleOnly;
+    }
+
+    public void Add(int row, int column, double value)
+    {
+        if (row < 0 || row >= rowCount)
+            throw new ArgumentOutOfRangeException(nameof(row));
+        if (column < 0 || column >= columnCount)
+            throw new ArgumentOutOfRangeException(nameof(column));
+        if (!NumericGuard.IsFinite(value))
+            throw new ArgumentOutOfRangeException(nameof(value), "Sparse triplet values must be finite.");
+        if (upperTriangleOnly && row > column)
+            throw new ArgumentOutOfRangeException(nameof(row), "Upper-triangular storage does not accept lower-triangular entries.");
+
+        triplets.Add(new Triplet(row, column, value));
+    }
+
+    public SparseCscMatrix Build()
+    {
+        var ordered = new List<Triplet>(triplets);
+        ordered.Sort(CompareTriplets);
+
+        var values = new List<double>();
+        var rows = new List<int>();
+        var pointers = new List<int>(columnCount + 1) { 0 };
+        int nextTriplet = 0;
+
+        for (int column = 0; column < columnCount; column++)
+        {
+            while (nextTriplet < ordered.Count && ordered[nextTriplet].Column == column)
+            {
+                int row = ordered[nextTriplet].Row;
+                double sum = 0d;
+                do
+                {
+                    sum += ordered[nextTriplet].Value;
+                    nextTriplet++;
+                }
+                while (nextTriplet < ordered.Count && ordered[nextTriplet].Column == column && ordered[nextTriplet].Row == row);
+
+                if (!NumericGuard.IsFinite(sum))
+                    throw new ArgumentOutOfRangeException(nameof(triplets), "Duplicate sparse triplets must sum to a finite value.");
+                if (sum != 0d)
+                {
+                    rows.Add(row);
+                    values.Add(sum);
+                }
+            }
+
+            pointers.Add(values.Count);
+        }
+
+        return new SparseCscMatrix(rowCount, columnCount, values, rows, pointers);
+    }
+
+    private static int CompareTriplets(Triplet left, Triplet right)
+    {
+        int columnComparison = left.Column.CompareTo(right.Column);
+        return columnComparison != 0 ? columnComparison : left.Row.CompareTo(right.Row);
+    }
+
+    private sealed class Triplet
+    {
+        public Triplet(int row, int column, double value)
+        {
+            Row = row;
+            Column = column;
+            Value = value;
+        }
+
+        public int Row { get; }
+
+        public int Column { get; }
+
+        public double Value { get; }
+    }
+}

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)