306 lines
15 KiB
C#
306 lines
15 KiB
C#
using System;
|
|
using System.Collections.Generic;
|
|
using System.Diagnostics;
|
|
using System.IO;
|
|
using System.Threading;
|
|
using System.Threading.Tasks;
|
|
|
|
namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
|
|
|
|
internal static class OsqpChecks
|
|
{
|
|
public static void Run()
|
|
{
|
|
var pluginDirectories = new List<string>();
|
|
try
|
|
{
|
|
string missingNativeDirectory = CreatePluginDirectory();
|
|
pluginDirectories.Add(missingNativeDirectory);
|
|
ProbeResult missingNative = RunProbe(missingNativeDirectory);
|
|
EMPlannerVerificationHost.Verification.Equal(QpSolveStatus.SolverUnavailable, missingNative.Status, "missing native status");
|
|
EMPlannerVerificationHost.Verification.True(!string.IsNullOrEmpty(missingNative.Diagnostic), "missing native diagnostic");
|
|
|
|
string corruptNativeDirectory = CreatePluginDirectory();
|
|
pluginDirectories.Add(corruptNativeDirectory);
|
|
File.WriteAllText(Path.Combine(corruptNativeDirectory, "osqp.dll"), "not a native library");
|
|
ProbeResult corruptNative = RunProbe(corruptNativeDirectory);
|
|
EMPlannerVerificationHost.Verification.Equal(QpSolveStatus.SolverUnavailable, corruptNative.Status, "corrupt native status");
|
|
EMPlannerVerificationHost.Verification.True(!string.IsNullOrEmpty(corruptNative.Diagnostic), "corrupt native diagnostic");
|
|
EMPlannerVerificationHost.Verification.True(corruptNative.RawOutput.IndexOf("BadImageFormatException", StringComparison.Ordinal) < 0,
|
|
"corrupt native does not escape BadImageFormatException");
|
|
|
|
string realNativeDirectory = CreatePluginDirectory();
|
|
pluginDirectories.Add(realNativeDirectory);
|
|
string realNativePath = Path.GetFullPath(Path.Combine(Directory.GetCurrentDirectory(), "ClumsyPilot", "ThirdParty", "OSQP", "win-x64", "osqp.dll"));
|
|
EMPlannerVerificationHost.Verification.True(File.Exists(realNativePath), "pinned native package exists");
|
|
File.Copy(realNativePath, Path.Combine(realNativeDirectory, "osqp.dll"), false);
|
|
ProbeResult realNative = RunProbe(realNativeDirectory);
|
|
EMPlannerVerificationHost.Verification.Equal(QpSolveStatus.Solved, realNative.Status, "real native status");
|
|
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
|
|
{
|
|
for (int index = 0; index < pluginDirectories.Count; index++)
|
|
{
|
|
if (Directory.Exists(pluginDirectories[index]))
|
|
Directory.Delete(pluginDirectories[index], true);
|
|
}
|
|
}
|
|
}
|
|
|
|
public static void RunProbe()
|
|
{
|
|
var results = new OsqpNativeLoadResult[16];
|
|
Parallel.For(0, results.Length, index => results[index] = OsqpNativeLoader.Load());
|
|
|
|
OsqpNativeLoadResult first = results[0];
|
|
bool sameHandle = true;
|
|
for (int index = 1; index < results.Length; index++)
|
|
{
|
|
sameHandle = sameHandle && results[index].Status == first.Status && results[index].ModuleHandle == first.ModuleHandle;
|
|
}
|
|
|
|
Console.WriteLine("STATUS=" + first.Status);
|
|
Console.WriteLine("VERSION=" + first.Version);
|
|
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()
|
|
{
|
|
string sourceDirectory = AppContext.BaseDirectory;
|
|
string destinationDirectory = Path.Combine(Path.GetTempPath(), "em-planner-osqp-loader-" + Guid.NewGuid().ToString("N"));
|
|
Directory.CreateDirectory(destinationDirectory);
|
|
|
|
string[] hostFiles = Directory.GetFiles(sourceDirectory);
|
|
for (int index = 0; index < hostFiles.Length; index++)
|
|
{
|
|
if (string.Equals(Path.GetFileName(hostFiles[index]), "osqp.dll", StringComparison.OrdinalIgnoreCase))
|
|
continue;
|
|
string destinationPath = Path.Combine(destinationDirectory, Path.GetFileName(hostFiles[index]));
|
|
File.Copy(hostFiles[index], destinationPath, false);
|
|
}
|
|
|
|
EMPlannerVerificationHost.Verification.True(File.Exists(Path.Combine(destinationDirectory, "ClumsyPilot.dll")), "copied plugin ClumsyPilot.dll");
|
|
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
|
|
{
|
|
FileName = Path.Combine(pluginDirectory, "EMPlannerVerificationHost.exe"),
|
|
Arguments = "osqp-probe",
|
|
WorkingDirectory = pluginDirectory,
|
|
UseShellExecute = false,
|
|
CreateNoWindow = true,
|
|
RedirectStandardOutput = true,
|
|
RedirectStandardError = true,
|
|
};
|
|
|
|
using (var process = new Process { StartInfo = startInfo })
|
|
{
|
|
process.Start();
|
|
string standardOutput = process.StandardOutput.ReadToEnd();
|
|
string standardError = process.StandardError.ReadToEnd();
|
|
process.WaitForExit();
|
|
if (process.ExitCode != 0)
|
|
throw new InvalidOperationException("OSQP loader probe exited " + process.ExitCode + ": " + standardError + standardOutput);
|
|
return ProbeResult.Parse(standardOutput + standardError);
|
|
}
|
|
}
|
|
|
|
private sealed class ProbeResult
|
|
{
|
|
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;
|
|
}
|
|
|
|
public QpSolveStatus Status { get; }
|
|
|
|
public string Version { get; }
|
|
|
|
public IntPtr ModuleHandle { get; }
|
|
|
|
public string Diagnostic { get; }
|
|
|
|
public bool ConcurrentHandleStable { get; }
|
|
|
|
public bool SolveChecksPassed { get; }
|
|
|
|
public string RawOutput { get; }
|
|
|
|
public static ProbeResult Parse(string output)
|
|
{
|
|
string statusText = ReadValue(output, "STATUS=");
|
|
string version = ReadValue(output, "VERSION=");
|
|
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))
|
|
throw new InvalidOperationException("Loader probe reported an invalid status: " + statusText);
|
|
long handleValue;
|
|
if (!long.TryParse(handleText, out handleValue))
|
|
throw new InvalidOperationException("Loader probe reported an invalid handle: " + handleText);
|
|
bool concurrentHandleStable;
|
|
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,
|
|
string.Equals(solveChecksText, "PASS", StringComparison.Ordinal), output);
|
|
}
|
|
|
|
private static string ReadValue(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);
|
|
}
|
|
|
|
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;
|
|
}
|
|
}
|
|
}
|