diff --git a/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Optimization/IQpSolver.cs b/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Optimization/IQpSolver.cs new file mode 100644 index 0000000..e160826 --- /dev/null +++ b/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 warmStart, + CancellationToken cancellationToken); +} diff --git a/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Optimization/QpSolveResult.cs b/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Optimization/QpSolveResult.cs new file mode 100644 index 0000000..6dda032 --- /dev/null +++ b/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 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(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(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 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; } +} diff --git a/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Optimization/QpSolveStatus.cs b/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Optimization/QpSolveStatus.cs new file mode 100644 index 0000000..114fb86 --- /dev/null +++ b/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, +} diff --git a/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Optimization/QpSolverSettings.cs b/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Optimization/QpSolverSettings.cs new file mode 100644 index 0000000..9302c78 --- /dev/null +++ b/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; } +} diff --git a/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Optimization/QuadraticProgram.cs b/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Optimization/QuadraticProgram.cs new file mode 100644 index 0000000..1b391ee --- /dev/null +++ b/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 q, + SparseCscMatrix a, + IReadOnlyList lowerBounds, + IReadOnlyList 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 LinearCost { get; } + + public SparseCscMatrix ConstraintMatrix { get; } + + public IReadOnlyList LowerBounds { get; } + + public IReadOnlyList 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 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 lowerBounds, IReadOnlyList 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 Copy(IReadOnlyList source) + { + var copy = new List(source.Count); + for (int index = 0; index < source.Count; index++) + copy.Add(source[index]); + return new ReadOnlyCollection(copy); + } +} diff --git a/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Optimization/SparseCscMatrix.cs b/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Optimization/SparseCscMatrix.cs new file mode 100644 index 0000000..ca31187 --- /dev/null +++ b/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 values, + IReadOnlyList rowIndices, + IReadOnlyList 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 Values { get; } + + public IReadOnlyList RowIndices { get; } + + public IReadOnlyList ColumnPointers { get; } + + private static IReadOnlyList Copy(IReadOnlyList source) + { + var copy = new List(source.Count); + for (int index = 0; index < source.Count; index++) + copy.Add(source[index]); + return new ReadOnlyCollection(copy); + } +} diff --git a/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Optimization/SparseTripletBuilder.cs b/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Optimization/SparseTripletBuilder.cs new file mode 100644 index 0000000..c4db4c6 --- /dev/null +++ b/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 triplets = new List(); + + 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(triplets); + ordered.Sort(CompareTriplets); + + var values = new List(); + var rows = new List(); + var pointers = new List(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; } + } +} diff --git a/ClumsyPilot/tests/EMPlannerVerificationHost/OptimizationChecks.cs b/ClumsyPilot/tests/EMPlannerVerificationHost/OptimizationChecks.cs new file mode 100644 index 0000000..3dcc75a --- /dev/null +++ b/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 { -2d }; + var lowerBounds = new List { 0d }; + var upperBounds = new List { 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."); + } +} diff --git a/ClumsyPilot/tests/EMPlannerVerificationHost/Program.cs b/ClumsyPilot/tests/EMPlannerVerificationHost/Program.cs index 2d07268..68e49f6 100644 --- a/ClumsyPilot/tests/EMPlannerVerificationHost/Program.cs +++ b/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)