First_Dev_Dept/ParkingRobot
10
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

feat: solve QPs through OSQP

Browse Source
This commit is contained in:
梁薄云
2026-08-04 00:19:26 +08:00
parent 2051827416
commit a957fda6c5
3 changed files with 461 additions and 2 deletions
Download Patch File Download Diff File Expand all files Collapse all files
ClumsyPilot/tests/EMPlannerVerificationHost/OsqpChecks.cs
+135 -2
View File
@@ -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;
}
}
}