feat: solve QPs through OSQP

ClumsyPilot/ParkrobTrajplanner/EMPlanner/Optimization/Osqp/OsqpNativeSolver.cs

@@ -0,0 +1,296 @@
+using System;
+using System.Collections.Generic;
+using System.Runtime.InteropServices;
+using System.Text;
+using System.Threading;
+using MultiWheelC.TrajectoryPlanning.Utils;
+
+namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
+
+public sealed class OsqpNativeSolver : IQpSolver
+{
+    public QpSolveResult Solve(
+        QuadraticProgram problem,
+        QpSolverSettings settings,
+        IReadOnlyList<double> warmStart,
+        CancellationToken cancellationToken)
+    {
+        if (cancellationToken.IsCancellationRequested)
+            return CreateFailure(QpSolveStatus.Cancelled, "OSQP solve was cancelled before native setup.");
+        if (problem == null)
+            return CreateFailure(QpSolveStatus.InvalidProblem, "A quadratic program is required.");
+        if (settings == null)
+            return CreateFailure(QpSolveStatus.InvalidProblem, "Solver settings are required.");
+
+        double[] copiedWarmStart;
+        string warmStartDiagnostic;
+        if (!TryCopyWarmStart(warmStart, problem.VariableCount, out copiedWarmStart, out warmStartDiagnostic))
+            return CreateFailure(QpSolveStatus.InvalidProblem, warmStartDiagnostic);
+
+        OsqpNativeApi api;
+        OsqpNativeLoadResult loadResult;
+        if (!OsqpNativeLoader.TryGetLoadedApi(out api, out loadResult))
+            return CreateFailure(loadResult.Status, loadResult.Diagnostic, loadResult.Version);
+
+        var pinnedArrays = new OsqpPinnedArrays();
+        OsqpNativeSetupMemory setupMemory = null;
+        IntPtr solver = IntPtr.Zero;
+        try
+        {
+            IntPtr pValues = pinnedArrays.Pin(Copy(problem.UpperTriangularP.Values));
+            IntPtr pRows = pinnedArrays.Pin(Copy(problem.UpperTriangularP.RowIndices));
+            IntPtr pColumns = pinnedArrays.Pin(Copy(problem.UpperTriangularP.ColumnPointers));
+            IntPtr q = pinnedArrays.Pin(Copy(problem.LinearCost));
+            IntPtr aValues = pinnedArrays.Pin(Copy(problem.ConstraintMatrix.Values));
+            IntPtr aRows = pinnedArrays.Pin(Copy(problem.ConstraintMatrix.RowIndices));
+            IntPtr aColumns = pinnedArrays.Pin(Copy(problem.ConstraintMatrix.ColumnPointers));
+            IntPtr lowerBounds = pinnedArrays.Pin(Copy(problem.LowerBounds));
+            IntPtr upperBounds = pinnedArrays.Pin(Copy(problem.UpperBounds));
+            IntPtr warmStartPointer = pinnedArrays.Pin(copiedWarmStart);
+
+            setupMemory = OsqpNativeStructures.AllocateSetupMemory(api.SetDefaultSettings);
+            Marshal.StructureToPtr(
+                CreateCsc(problem.UpperTriangularP, pColumns, pRows, pValues),
+                setupMemory.UpperTriangularP,
+                false);
+            Marshal.StructureToPtr(
+                CreateCsc(problem.ConstraintMatrix, aColumns, aRows, aValues),
+                setupMemory.Constraints,
+                false);
+
+            var nativeSettings = (OsqpSettings)Marshal.PtrToStructure(setupMemory.Settings, typeof(OsqpSettings));
+            nativeSettings.Verbose = 0;
+            nativeSettings.WarmStarting = settings.EnableWarmStart ? 1 : 0;
+            nativeSettings.Polishing = settings.EnablePolishing ? 1 : 0;
+            nativeSettings.MaximumIterations = settings.MaximumIterations;
+            nativeSettings.AbsoluteTolerance = settings.AbsoluteTolerance;
+            nativeSettings.RelativeTolerance = settings.RelativeTolerance;
+            nativeSettings.TimeLimit = settings.TimeLimit.TotalSeconds;
+            Marshal.StructureToPtr(nativeSettings, setupMemory.Settings, false);
+
+            int setupStatus = api.Setup(
+                out solver,
+                setupMemory.UpperTriangularP,
+                q,
+                setupMemory.Constraints,
+                lowerBounds,
+                upperBounds,
+                problem.ConstraintCount,
+                problem.VariableCount,
+                setupMemory.Settings);
+            if (setupStatus != 0 || solver == IntPtr.Zero)
+                return CreateFailure(QpSolveStatus.NativeError, "OSQP setup failed with code " + setupStatus + ".", "setup=" + setupStatus);
+
+            if (settings.EnableWarmStart && copiedWarmStart != null)
+            {
+                int warmStartStatus = api.WarmStart(solver, warmStartPointer, IntPtr.Zero);
+                if (warmStartStatus != 0)
+                    return CreateFailure(QpSolveStatus.NativeError, "OSQP warm start failed with code " + warmStartStatus + ".", "warm_start=" + warmStartStatus);
+            }
+
+            int solveStatus = api.Solve(solver);
+            if (solveStatus != 0)
+                return CreateFailure(QpSolveStatus.NativeError, "OSQP solve failed with code " + solveStatus + ".", "solve=" + solveStatus);
+            if (cancellationToken.IsCancellationRequested)
+                return CreateFailure(QpSolveStatus.Cancelled, "OSQP solve was cancelled after native completion.");
+
+            return ReadSolveResult(solver, problem.VariableCount);
+        }
+        catch (Exception exception)
+        {
+            return CreateFailure(QpSolveStatus.NativeError, "OSQP native solve failed: " + exception.Message);
+        }
+        finally
+        {
+            if (solver != IntPtr.Zero)
+            {
+                try
+                {
+                    api.Cleanup(solver);
+                }
+                catch
+                {
+                }
+            }
+
+            if (setupMemory != null)
+                setupMemory.Dispose();
+            pinnedArrays.Dispose();
+        }
+    }
+
+    private static QpSolveResult ReadSolveResult(IntPtr solver, int variableCount)
+    {
+        var solverPrefix = (OsqpSolverPrefix)Marshal.PtrToStructure(solver, typeof(OsqpSolverPrefix));
+        if (solverPrefix.Info == IntPtr.Zero)
+            return CreateFailure(QpSolveStatus.NativeError, "OSQP solve returned no information block.");
+
+        var info = (OsqpInfo)Marshal.PtrToStructure(solverPrefix.Info, typeof(OsqpInfo));
+        string nativeStatus = ReadNativeStatus(info);
+        QpSolveStatus status = OsqpStatusMapper.Map(info.StatusValue);
+        double[] primal;
+        string primalDiagnostic;
+        if (!TryReadPrimal(solverPrefix.Solution, variableCount, out primal, out primalDiagnostic))
+            return CreateFailure(QpSolveStatus.NativeError, primalDiagnostic, nativeStatus);
+
+        string diagnostic = string.Empty;
+        double objective = ToFiniteMetric(info.ObjectiveValue, "objective", ref diagnostic);
+        double primalResidual = ToFiniteMetric(info.PrimalResidual, "primal residual", ref diagnostic);
+        double dualResidual = ToFiniteMetric(info.DualResidual, "dual residual", ref diagnostic);
+        return new QpSolveResult(
+            status,
+            primal,
+            objective,
+            primalResidual,
+            dualResidual,
+            Math.Max(0, info.Iterations),
+            ToSolveTime(info.SolveTime),
+            nativeStatus,
+            diagnostic);
+    }
+
+    private static bool TryReadPrimal(IntPtr solutionPointer, int variableCount, out double[] primal, out string diagnostic)
+    {
+        primal = new double[0];
+        diagnostic = string.Empty;
+        if (solutionPointer == IntPtr.Zero)
+        {
+            diagnostic = "OSQP solve returned no solution block.";
+            return false;
+        }
+
+        var solution = (OsqpSolution)Marshal.PtrToStructure(solutionPointer, typeof(OsqpSolution));
+        if (variableCount == 0)
+            return true;
+        if (solution.Primal == IntPtr.Zero)
+        {
+            diagnostic = "OSQP solve returned no primal vector.";
+            return false;
+        }
+
+        primal = new double[variableCount];
+        Marshal.Copy(solution.Primal, primal, 0, primal.Length);
+        for (int index = 0; index < primal.Length; index++)
+        {
+            if (!NumericGuard.IsFinite(primal[index]))
+            {
+                diagnostic = "OSQP solve returned a non-finite primal value.";
+                return false;
+            }
+        }
+
+        return true;
+    }
+
+    private static OsqpCscMatrix CreateCsc(SparseCscMatrix matrix, IntPtr columnPointers, IntPtr rowIndices, IntPtr values)
+    {
+        return new OsqpCscMatrix
+        {
+            RowCount = matrix.RowCount,
+            ColumnCount = matrix.ColumnCount,
+            ColumnPointers = columnPointers,
+            RowIndices = rowIndices,
+            Values = values,
+            MaximumNonZeroCount = matrix.Values.Count,
+            NonZeroCount = -1,
+            OwnsData = 0,
+        };
+    }
+
+    private static bool TryCopyWarmStart(
+        IReadOnlyList<double> warmStart,
+        int variableCount,
+        out double[] copiedWarmStart,
+        out string diagnostic)
+    {
+        copiedWarmStart = null;
+        diagnostic = string.Empty;
+        if (warmStart == null)
+            return true;
+        if (warmStart.Count != variableCount)
+        {
+            diagnostic = "OSQP warm start length must match the quadratic-program variable count.";
+            return false;
+        }
+
+        copiedWarmStart = Copy(warmStart);
+        for (int index = 0; index < copiedWarmStart.Length; index++)
+        {
+            if (!NumericGuard.IsFinite(copiedWarmStart[index]))
+            {
+                diagnostic = "OSQP warm start values must be finite.";
+                return false;
+            }
+        }
+
+        return true;
+    }
+
+    private static T[] Copy<T>(IReadOnlyList<T> values)
+    {
+        var copied = new T[values.Count];
+        for (int index = 0; index < copied.Length; index++)
+            copied[index] = values[index];
+        return copied;
+    }
+
+    private static string ReadNativeStatus(OsqpInfo info)
+    {
+        byte[] bytes = info.Status ?? new byte[0];
+        int length = 0;
+        while (length < bytes.Length && bytes[length] != 0)
+            length++;
+        string nativeStatus = Encoding.ASCII.GetString(bytes, 0, length).Trim();
+        return string.IsNullOrEmpty(nativeStatus) ? "status=" + info.StatusValue : nativeStatus;
+    }
+
+    private static double ToFiniteMetric(double value, string metricName, ref string diagnostic)
+    {
+        if (NumericGuard.IsFinite(value))
+            return value;
+
+        diagnostic = string.IsNullOrEmpty(diagnostic)
+            ? "OSQP returned a non-finite " + metricName + "; the planner-neutral diagnostic was normalized to zero."
+            : diagnostic;
+        return 0d;
+    }
+
+    private static TimeSpan ToSolveTime(double seconds)
+    {
+        if (!NumericGuard.IsFinite(seconds) || seconds <= 0d)
+            return TimeSpan.Zero;
+        if (seconds >= TimeSpan.MaxValue.TotalSeconds)
+            return TimeSpan.MaxValue;
+        return TimeSpan.FromSeconds(seconds);
+    }
+
+    private static QpSolveResult CreateFailure(QpSolveStatus status, string diagnostic, string nativeStatus = "")
+    {
+        return new QpSolveResult(status, new double[0], 0d, 0d, 0d, 0, TimeSpan.Zero, nativeStatus, diagnostic);
+    }
+
+    private sealed class OsqpPinnedArrays : IDisposable
+    {
+        private readonly List<GCHandle> handles = new List<GCHandle>();
+
+        public IntPtr Pin<T>(T[] values) where T : struct
+        {
+            if (values == null || values.Length == 0)
+                return IntPtr.Zero;
+
+            GCHandle handle = GCHandle.Alloc(values, GCHandleType.Pinned);
+            handles.Add(handle);
+            return handle.AddrOfPinnedObject();
+        }
+
+        public void Dispose()
+        {
+            for (int index = handles.Count - 1; index >= 0; index--)
+            {
+                if (handles[index].IsAllocated)
+                    handles[index].Free();
+            }
+            handles.Clear();
+        }
+    }
+}

ClumsyPilot/ParkrobTrajplanner/EMPlanner/Optimization/Osqp/OsqpStatusMapper.cs

@@ -0,0 +1,30 @@
+namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
+
+internal static class OsqpStatusMapper
+{
+    public static QpSolveStatus Map(int nativeStatusValue)
+    {
+        switch (nativeStatusValue)
+        {
+            case 1:
+                return QpSolveStatus.Solved;
+            case 2:
+                return QpSolveStatus.SolvedInaccurate;
+            case 3:
+            case 4:
+                return QpSolveStatus.PrimalInfeasible;
+            case 5:
+            case 6:
+                return QpSolveStatus.DualInfeasible;
+            case 7:
+                return QpSolveStatus.MaximumIterations;
+            case 8:
+                return QpSolveStatus.TimeLimit;
+            case 9:
+            case 10:
+            case 11:
+            default:
+                return QpSolveStatus.NativeError;
+        }
+    }
+}

ClumsyPilot/tests/EMPlannerVerificationHost/OsqpChecks.cs

@@ -2,6 +2,7 @@ using System;
 using System.Collections.Generic;
 using System.Diagnostics;
 using System.IO;
+using System.Threading;
 using System.Threading.Tasks;
 
 namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
@@ -38,6 +39,8 @@ internal static class OsqpChecks
             EMPlannerVerificationHost.Verification.Equal("1.0.0", realNative.Version, "real native version");
             EMPlannerVerificationHost.Verification.True(realNative.ModuleHandle != IntPtr.Zero, "real native module handle");
             EMPlannerVerificationHost.Verification.Equal(true, realNative.ConcurrentHandleStable, "concurrent native module handle");
+            EMPlannerVerificationHost.Verification.Equal(true, realNative.SolveChecksPassed, "native solve checks");
+            Console.WriteLine("PASS osqp-solve");
         }
         finally
         {
@@ -66,6 +69,12 @@ internal static class OsqpChecks
         Console.WriteLine("HANDLE=" + first.ModuleHandle.ToInt64());
         Console.WriteLine("DIAGNOSTIC=" + first.Diagnostic.Replace('\r', ' ').Replace('\n', ' '));
         Console.WriteLine("CONCURRENT_HANDLE_STABLE=" + sameHandle);
+
+        if (first.Status == QpSolveStatus.Solved)
+        {
+            VerifyNativeSolveLifecycle();
+            Console.WriteLine("SOLVE_CHECKS=PASS");
+        }
     }
 
     private static string CreatePluginDirectory()
@@ -85,6 +94,113 @@ internal static class OsqpChecks
         return destinationDirectory;
     }
 
+    private static void VerifyNativeSolveLifecycle()
+    {
+        var solver = new OsqpNativeSolver();
+
+        QpSolveResult bounded = solver.Solve(
+            CreateBoundedOptimumProblem(),
+            CreateSettings(TimeSpan.FromMilliseconds(100)),
+            new[] { 0d },
+            CancellationToken.None);
+        EMPlannerVerificationHost.Verification.Equal(QpSolveStatus.Solved, bounded.Status, "bounded optimum status");
+        AssertPopulatedResult(bounded, "bounded optimum");
+        EMPlannerVerificationHost.Verification.Equal(1, bounded.Primal.Count, "bounded optimum primal count");
+        AssertClose(1d, bounded.Primal[0], 1e-5d, "bounded optimum primal");
+        AssertClose(-1.5d, bounded.Objective, 1e-5d, "bounded optimum objective");
+        EMPlannerVerificationHost.Verification.True(bounded.PrimalResidual <= 1e-5d, "bounded optimum primal residual");
+        EMPlannerVerificationHost.Verification.True(bounded.DualResidual <= 1e-5d, "bounded optimum dual residual");
+
+        QpSolveResult equality = solver.Solve(
+            CreateEqualityOptimumProblem(),
+            CreateSettings(TimeSpan.FromMilliseconds(100)),
+            new[] { 0.5d, 0.5d },
+            CancellationToken.None);
+        EMPlannerVerificationHost.Verification.Equal(QpSolveStatus.Solved, equality.Status, "equality optimum status");
+        AssertPopulatedResult(equality, "equality optimum");
+        EMPlannerVerificationHost.Verification.Equal(2, equality.Primal.Count, "equality optimum primal count");
+        AssertClose(0.5d, equality.Primal[0], 1e-5d, "equality optimum first primal");
+        AssertClose(0.5d, equality.Primal[1], 1e-5d, "equality optimum second primal");
+        AssertClose(0.5d, equality.Objective, 1e-5d, "equality optimum objective");
+        EMPlannerVerificationHost.Verification.True(equality.PrimalResidual <= 1e-5d, "equality optimum primal residual");
+        EMPlannerVerificationHost.Verification.True(equality.DualResidual <= 1e-5d, "equality optimum dual residual");
+
+        QpSolveResult infeasible = solver.Solve(
+            CreateInfeasibleProblem(),
+            CreateSettings(TimeSpan.FromMilliseconds(100)),
+            null,
+            CancellationToken.None);
+        EMPlannerVerificationHost.Verification.Equal(QpSolveStatus.PrimalInfeasible, infeasible.Status, "infeasible status");
+        AssertPopulatedResult(infeasible, "infeasible");
+
+        QpSolveResult tinyTimeLimit = solver.Solve(
+            CreateEqualityOptimumProblem(),
+            CreateSettings(TimeSpan.FromTicks(1)),
+            null,
+            CancellationToken.None);
+        EMPlannerVerificationHost.Verification.True(
+            tinyTimeLimit.Status == QpSolveStatus.TimeLimit ||
+            tinyTimeLimit.Status == QpSolveStatus.Solved ||
+            tinyTimeLimit.Status == QpSolveStatus.SolvedInaccurate,
+            "tiny time-limit status maps to a time limit or solved state");
+        AssertPopulatedResult(tinyTimeLimit, "tiny time-limit");
+    }
+
+    private static QuadraticProgram CreateBoundedOptimumProblem()
+    {
+        var hessian = new SparseTripletBuilder(1, 1, true);
+        hessian.Add(0, 0, 1d);
+        var constraints = new SparseTripletBuilder(1, 1);
+        constraints.Add(0, 0, 1d);
+        return new QuadraticProgram(hessian.Build(), new[] { -2d }, constraints.Build(), new[] { 0d }, new[] { 1d });
+    }
+
+    private static QuadraticProgram CreateEqualityOptimumProblem()
+    {
+        var hessian = new SparseTripletBuilder(2, 2, true);
+        hessian.Add(0, 0, 2d);
+        hessian.Add(1, 1, 2d);
+        var constraints = new SparseTripletBuilder(1, 2);
+        constraints.Add(0, 0, 1d);
+        constraints.Add(0, 1, 1d);
+        return new QuadraticProgram(hessian.Build(), new[] { 0d, 0d }, constraints.Build(), new[] { 1d }, new[] { 1d });
+    }
+
+    private static QuadraticProgram CreateInfeasibleProblem()
+    {
+        var hessian = new SparseTripletBuilder(1, 1, true);
+        hessian.Add(0, 0, 1d);
+        var constraints = new SparseTripletBuilder(2, 1);
+        constraints.Add(0, 0, 1d);
+        constraints.Add(1, 0, 1d);
+        return new QuadraticProgram(
+            hessian.Build(),
+            new[] { 0d },
+            constraints.Build(),
+            new[] { 1d, -QuadraticProgram.MaximumFiniteBound },
+            new[] { QuadraticProgram.MaximumFiniteBound, 0d });
+    }
+
+    private static QpSolverSettings CreateSettings(TimeSpan timeLimit)
+    {
+        return new QpSolverSettings(4000, 1e-6d, 1e-6d, timeLimit, true, true, false);
+    }
+
+    private static void AssertPopulatedResult(QpSolveResult result, string name)
+    {
+        EMPlannerVerificationHost.Verification.True(result.Iterations >= 0, name + " iterations");
+        EMPlannerVerificationHost.Verification.True(result.SolveTime >= TimeSpan.Zero, name + " solve time");
+        EMPlannerVerificationHost.Verification.True(!double.IsNaN(result.Objective) && !double.IsInfinity(result.Objective), name + " objective");
+        EMPlannerVerificationHost.Verification.True(!double.IsNaN(result.PrimalResidual) && !double.IsInfinity(result.PrimalResidual), name + " primal residual");
+        EMPlannerVerificationHost.Verification.True(!double.IsNaN(result.DualResidual) && !double.IsInfinity(result.DualResidual), name + " dual residual");
+        EMPlannerVerificationHost.Verification.True(!string.IsNullOrWhiteSpace(result.NativeStatus), name + " native status");
+    }
+
+    private static void AssertClose(double expected, double actual, double tolerance, string name)
+    {
+        EMPlannerVerificationHost.Verification.True(Math.Abs(expected - actual) <= tolerance, name + " expected " + expected + " but was " + actual);
+    }
+
     private static ProbeResult RunProbe(string pluginDirectory)
     {
         var startInfo = new ProcessStartInfo
@@ -112,13 +228,14 @@ internal static class OsqpChecks
 
     private sealed class ProbeResult
     {
-        private ProbeResult(QpSolveStatus status, string version, IntPtr moduleHandle, string diagnostic, bool concurrentHandleStable, string rawOutput)
+        private ProbeResult(QpSolveStatus status, string version, IntPtr moduleHandle, string diagnostic, bool concurrentHandleStable, bool solveChecksPassed, string rawOutput)
         {
             Status = status;
             Version = version;
             ModuleHandle = moduleHandle;
             Diagnostic = diagnostic;
             ConcurrentHandleStable = concurrentHandleStable;
+            SolveChecksPassed = solveChecksPassed;
             RawOutput = rawOutput;
         }
 
@@ -132,6 +249,8 @@ internal static class OsqpChecks
 
         public bool ConcurrentHandleStable { get; }
 
+        public bool SolveChecksPassed { get; }
+
         public string RawOutput { get; }
 
         public static ProbeResult Parse(string output)
@@ -141,6 +260,7 @@ internal static class OsqpChecks
             string handleText = ReadValue(output, "HANDLE=");
             string diagnostic = ReadValue(output, "DIAGNOSTIC=");
             string stableText = ReadValue(output, "CONCURRENT_HANDLE_STABLE=");
+            string solveChecksText = ReadOptionalValue(output, "SOLVE_CHECKS=");
 
             QpSolveStatus status;
             if (!Enum.TryParse(statusText, out status))
@@ -152,7 +272,8 @@ internal static class OsqpChecks
             if (!bool.TryParse(stableText, out concurrentHandleStable))
                 throw new InvalidOperationException("Loader probe reported an invalid concurrency flag: " + stableText);
 
-            return new ProbeResult(status, version, new IntPtr(handleValue), diagnostic, concurrentHandleStable, output);
+            return new ProbeResult(status, version, new IntPtr(handleValue), diagnostic, concurrentHandleStable,
+                string.Equals(solveChecksText, "PASS", StringComparison.Ordinal), output);
         }
 
         private static string ReadValue(string output, string prefix)
@@ -166,5 +287,17 @@ internal static class OsqpChecks
 
             throw new InvalidOperationException("Loader probe did not report " + prefix + ". Output: " + output);
         }
+
+        private static string ReadOptionalValue(string output, string prefix)
+        {
+            string[] lines = output.Replace("\r", string.Empty).Split('\n');
+            for (int index = 0; index < lines.Length; index++)
+            {
+                if (lines[index].StartsWith(prefix, StringComparison.Ordinal))
+                    return lines[index].Substring(prefix.Length);
+            }
+
+            return string.Empty;
+        }
     }
 }