From a957fda6c575d83db9498e6a17c42ad2a91ab987 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=E6=A2=81=E8=96=84=E4=BA=91?= Date: Tue, 4 Aug 2026 00:19:26 +0800 Subject: [PATCH] feat: solve QPs through OSQP --- .../Optimization/Osqp/OsqpNativeSolver.cs | 296 ++++++++++++++++++ .../Optimization/Osqp/OsqpStatusMapper.cs | 30 ++ .../EMPlannerVerificationHost/OsqpChecks.cs | 137 +++++++- 3 files changed, 461 insertions(+), 2 deletions(-) create mode 100644 ClumsyPilot/ParkrobTrajplanner/EMPlanner/Optimization/Osqp/OsqpNativeSolver.cs create mode 100644 ClumsyPilot/ParkrobTrajplanner/EMPlanner/Optimization/Osqp/OsqpStatusMapper.cs diff --git a/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Optimization/Osqp/OsqpNativeSolver.cs b/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Optimization/Osqp/OsqpNativeSolver.cs new file mode 100644 index 0000000..ce82ecb --- /dev/null +++ b/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 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 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(IReadOnlyList 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 handles = new List(); + + public IntPtr Pin(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(); + } + } +} diff --git a/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Optimization/Osqp/OsqpStatusMapper.cs b/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Optimization/Osqp/OsqpStatusMapper.cs new file mode 100644 index 0000000..120668b --- /dev/null +++ b/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; + } + } +} diff --git a/ClumsyPilot/tests/EMPlannerVerificationHost/OsqpChecks.cs b/ClumsyPilot/tests/EMPlannerVerificationHost/OsqpChecks.cs index 2333602..c878e11 100644 --- a/ClumsyPilot/tests/EMPlannerVerificationHost/OsqpChecks.cs +++ b/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; + } } }