feat: load pinned OSQP native library
This commit is contained in:
梁薄云
@@ -0,0 +1,110 @@
|
||||
using System;
|
||||
using System.IO;
|
||||
using System.Reflection;
|
||||
using System.Runtime.InteropServices;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
|
||||
|
||||
public sealed class OsqpNativeLoadResult
|
||||
{
|
||||
internal OsqpNativeLoadResult(QpSolveStatus status, string version, IntPtr moduleHandle, string diagnostic)
|
||||
{
|
||||
Status = status;
|
||||
Version = version ?? string.Empty;
|
||||
ModuleHandle = moduleHandle;
|
||||
Diagnostic = diagnostic ?? string.Empty;
|
||||
}
|
||||
|
||||
public QpSolveStatus Status { get; }
|
||||
|
||||
public string Version { get; }
|
||||
|
||||
public IntPtr ModuleHandle { get; }
|
||||
|
||||
public string Diagnostic { get; }
|
||||
}
|
||||
|
||||
public static class OsqpNativeLoader
|
||||
{
|
||||
private static readonly object Sync = new object();
|
||||
private static OsqpNativeLoadResult cachedResult;
|
||||
private static OsqpNativeApi loadedApi;
|
||||
|
||||
public static OsqpNativeLoadResult Load()
|
||||
{
|
||||
lock (Sync)
|
||||
{
|
||||
if (cachedResult != null)
|
||||
return cachedResult;
|
||||
|
||||
if (IntPtr.Size != 8)
|
||||
return CacheFailure(QpSolveStatus.SolverUnavailable, "OSQP v1.0.0 requires a 64-bit process.");
|
||||
|
||||
try
|
||||
{
|
||||
OsqpNativeStructures.ValidatePinnedLayout();
|
||||
string nativePath = ResolveNativePath();
|
||||
if (!File.Exists(nativePath))
|
||||
return CacheFailure(QpSolveStatus.SolverUnavailable, "OSQP native library was not found at " + nativePath + ".");
|
||||
|
||||
IntPtr moduleHandle = OsqpNativeMethods.LoadLibrary(nativePath);
|
||||
if (moduleHandle == IntPtr.Zero)
|
||||
return CacheFailure(QpSolveStatus.SolverUnavailable,
|
||||
"OSQP native library could not be loaded from " + nativePath + ": " + OsqpNativeMethods.LastErrorMessage());
|
||||
|
||||
try
|
||||
{
|
||||
OsqpNativeApi api = OsqpNativeMethods.ResolveApi(moduleHandle);
|
||||
string version = Marshal.PtrToStringAnsi(api.Version()) ?? string.Empty;
|
||||
if (!string.Equals(version, "1.0.0", StringComparison.Ordinal))
|
||||
{
|
||||
OsqpNativeMethods.FreeModule(moduleHandle);
|
||||
return CacheFailure(QpSolveStatus.SolverUnavailable,
|
||||
"OSQP native library version " + version + " does not match required version 1.0.0.");
|
||||
}
|
||||
|
||||
loadedApi = api;
|
||||
cachedResult = new OsqpNativeLoadResult(QpSolveStatus.Solved, version, moduleHandle, string.Empty);
|
||||
return cachedResult;
|
||||
}
|
||||
catch (Exception exception)
|
||||
{
|
||||
OsqpNativeMethods.FreeModule(moduleHandle);
|
||||
return CacheFailure(QpSolveStatus.SolverUnavailable,
|
||||
"OSQP native library exports could not be initialized: " + exception.Message);
|
||||
}
|
||||
}
|
||||
catch (Exception exception)
|
||||
{
|
||||
return CacheFailure(QpSolveStatus.NativeError, "OSQP native loader failed: " + exception.Message);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
internal static bool TryGetLoadedApi(out OsqpNativeApi api, out OsqpNativeLoadResult result)
|
||||
{
|
||||
result = Load();
|
||||
api = loadedApi;
|
||||
return result.Status == QpSolveStatus.Solved && api != null;
|
||||
}
|
||||
|
||||
private static OsqpNativeLoadResult CacheFailure(QpSolveStatus status, string diagnostic)
|
||||
{
|
||||
cachedResult = new OsqpNativeLoadResult(status, string.Empty, IntPtr.Zero, diagnostic);
|
||||
return cachedResult;
|
||||
}
|
||||
|
||||
private static string ResolveNativePath()
|
||||
{
|
||||
Assembly assembly = typeof(OsqpNativeLoader).Assembly;
|
||||
string assemblyLocation = assembly.Location;
|
||||
if (string.IsNullOrEmpty(assemblyLocation) || !Path.IsPathRooted(assemblyLocation))
|
||||
throw new InvalidOperationException("The EM Planner assembly location must be an absolute file path.");
|
||||
|
||||
string pluginDirectory = Path.GetDirectoryName(assemblyLocation);
|
||||
if (string.IsNullOrEmpty(pluginDirectory))
|
||||
throw new InvalidOperationException("The EM Planner assembly location has no plugin directory.");
|
||||
|
||||
return Path.GetFullPath(Path.Combine(pluginDirectory, "osqp.dll"));
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,111 @@
|
||||
using System;
|
||||
using System.ComponentModel;
|
||||
using System.Runtime.InteropServices;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
|
||||
|
||||
internal static class OsqpNativeMethods
|
||||
{
|
||||
[UnmanagedFunctionPointer(CallingConvention.Cdecl)]
|
||||
internal delegate IntPtr VersionDelegate();
|
||||
|
||||
[UnmanagedFunctionPointer(CallingConvention.Cdecl)]
|
||||
internal delegate void SetDefaultSettingsDelegate(IntPtr settings);
|
||||
|
||||
[UnmanagedFunctionPointer(CallingConvention.Cdecl)]
|
||||
internal delegate int SetupDelegate(
|
||||
out IntPtr solver,
|
||||
IntPtr upperTriangularP,
|
||||
IntPtr linearCost,
|
||||
IntPtr constraints,
|
||||
IntPtr lowerBounds,
|
||||
IntPtr upperBounds,
|
||||
int constraintCount,
|
||||
int variableCount,
|
||||
IntPtr settings);
|
||||
|
||||
[UnmanagedFunctionPointer(CallingConvention.Cdecl)]
|
||||
internal delegate int WarmStartDelegate(IntPtr solver, IntPtr primal, IntPtr dual);
|
||||
|
||||
[UnmanagedFunctionPointer(CallingConvention.Cdecl)]
|
||||
internal delegate int SolveDelegate(IntPtr solver);
|
||||
|
||||
[UnmanagedFunctionPointer(CallingConvention.Cdecl)]
|
||||
internal delegate int CleanupDelegate(IntPtr solver);
|
||||
|
||||
public static IntPtr LoadLibrary(string absolutePath)
|
||||
{
|
||||
return LoadLibraryW(absolutePath);
|
||||
}
|
||||
|
||||
public static bool FreeModule(IntPtr moduleHandle)
|
||||
{
|
||||
return FreeLibrary(moduleHandle);
|
||||
}
|
||||
|
||||
public static string LastErrorMessage()
|
||||
{
|
||||
int error = Marshal.GetLastWin32Error();
|
||||
return new Win32Exception(error).Message;
|
||||
}
|
||||
|
||||
public static OsqpNativeApi ResolveApi(IntPtr moduleHandle)
|
||||
{
|
||||
return new OsqpNativeApi(
|
||||
Resolve<VersionDelegate>(moduleHandle, "osqp_version"),
|
||||
Resolve<SetDefaultSettingsDelegate>(moduleHandle, "osqp_set_default_settings"),
|
||||
Resolve<SetupDelegate>(moduleHandle, "osqp_setup"),
|
||||
Resolve<WarmStartDelegate>(moduleHandle, "osqp_warm_start"),
|
||||
Resolve<SolveDelegate>(moduleHandle, "osqp_solve"),
|
||||
Resolve<CleanupDelegate>(moduleHandle, "osqp_cleanup"));
|
||||
}
|
||||
|
||||
private static T Resolve<T>(IntPtr moduleHandle, string exportName) where T : Delegate
|
||||
{
|
||||
IntPtr procedure = GetProcAddress(moduleHandle, exportName);
|
||||
if (procedure == IntPtr.Zero)
|
||||
throw new InvalidOperationException("Missing OSQP export " + exportName + ".");
|
||||
return Marshal.GetDelegateForFunctionPointer<T>(procedure);
|
||||
}
|
||||
|
||||
[DllImport("kernel32", CharSet = CharSet.Unicode, ExactSpelling = true, SetLastError = true)]
|
||||
private static extern IntPtr LoadLibraryW(string fileName);
|
||||
|
||||
[DllImport("kernel32", ExactSpelling = true, SetLastError = true)]
|
||||
[return: MarshalAs(UnmanagedType.Bool)]
|
||||
private static extern bool FreeLibrary(IntPtr moduleHandle);
|
||||
|
||||
[DllImport("kernel32", CharSet = CharSet.Ansi, ExactSpelling = true, SetLastError = true)]
|
||||
private static extern IntPtr GetProcAddress(IntPtr moduleHandle, string procedureName);
|
||||
}
|
||||
|
||||
internal sealed class OsqpNativeApi
|
||||
{
|
||||
public OsqpNativeApi(
|
||||
OsqpNativeMethods.VersionDelegate version,
|
||||
OsqpNativeMethods.SetDefaultSettingsDelegate setDefaultSettings,
|
||||
OsqpNativeMethods.SetupDelegate setup,
|
||||
OsqpNativeMethods.WarmStartDelegate warmStart,
|
||||
OsqpNativeMethods.SolveDelegate solve,
|
||||
OsqpNativeMethods.CleanupDelegate cleanup)
|
||||
{
|
||||
Version = version;
|
||||
SetDefaultSettings = setDefaultSettings;
|
||||
Setup = setup;
|
||||
WarmStart = warmStart;
|
||||
Solve = solve;
|
||||
Cleanup = cleanup;
|
||||
}
|
||||
|
||||
public OsqpNativeMethods.VersionDelegate Version { get; }
|
||||
|
||||
public OsqpNativeMethods.SetDefaultSettingsDelegate SetDefaultSettings { get; }
|
||||
|
||||
public OsqpNativeMethods.SetupDelegate Setup { get; }
|
||||
|
||||
public OsqpNativeMethods.WarmStartDelegate WarmStart { get; }
|
||||
|
||||
public OsqpNativeMethods.SolveDelegate Solve { get; }
|
||||
|
||||
public OsqpNativeMethods.CleanupDelegate Cleanup { get; }
|
||||
}
|
||||
@@ -0,0 +1,205 @@
|
||||
using System;
|
||||
using System.Runtime.InteropServices;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
|
||||
|
||||
[StructLayout(LayoutKind.Sequential, Pack = 8)]
|
||||
internal struct OsqpCscMatrix
|
||||
{
|
||||
public int RowCount;
|
||||
public int ColumnCount;
|
||||
public IntPtr ColumnPointers;
|
||||
public IntPtr RowIndices;
|
||||
public IntPtr Values;
|
||||
public int MaximumNonZeroCount;
|
||||
public int NonZeroCount;
|
||||
public int OwnsData;
|
||||
}
|
||||
|
||||
[StructLayout(LayoutKind.Sequential, Pack = 8)]
|
||||
internal struct OsqpSettings
|
||||
{
|
||||
public int Device;
|
||||
public int LinearSystemSolver;
|
||||
public int AllocateSolution;
|
||||
public int Verbose;
|
||||
public int ProfilerLevel;
|
||||
public int WarmStarting;
|
||||
public int Scaling;
|
||||
public int Polishing;
|
||||
public double Rho;
|
||||
public int RhoIsVector;
|
||||
public double Sigma;
|
||||
public double Alpha;
|
||||
public int ConjugateGradientMaximumIterations;
|
||||
public int ConjugateGradientToleranceReduction;
|
||||
public double ConjugateGradientToleranceFraction;
|
||||
public int ConjugateGradientPreconditioner;
|
||||
public int AdaptiveRho;
|
||||
public int AdaptiveRhoInterval;
|
||||
public double AdaptiveRhoFraction;
|
||||
public double AdaptiveRhoTolerance;
|
||||
public int MaximumIterations;
|
||||
public double AbsoluteTolerance;
|
||||
public double RelativeTolerance;
|
||||
public double PrimalInfeasibilityTolerance;
|
||||
public double DualInfeasibilityTolerance;
|
||||
public int ScaledTermination;
|
||||
public int CheckTermination;
|
||||
public int CheckDualityGap;
|
||||
public double TimeLimit;
|
||||
public double PolishingDelta;
|
||||
public int PolishingRefinementIterations;
|
||||
}
|
||||
|
||||
[StructLayout(LayoutKind.Sequential, Pack = 8)]
|
||||
internal struct OsqpInfo
|
||||
{
|
||||
[MarshalAs(UnmanagedType.ByValArray, SizeConst = 32, ArraySubType = UnmanagedType.I1)]
|
||||
public byte[] Status;
|
||||
public int StatusValue;
|
||||
public int PolishingStatus;
|
||||
public double ObjectiveValue;
|
||||
public double DualObjectiveValue;
|
||||
public double PrimalResidual;
|
||||
public double DualResidual;
|
||||
public double DualityGap;
|
||||
public int Iterations;
|
||||
public int RhoUpdates;
|
||||
public double RhoEstimate;
|
||||
public double SetupTime;
|
||||
public double SolveTime;
|
||||
public double UpdateTime;
|
||||
public double PolishingTime;
|
||||
public double RunTime;
|
||||
public double PrimalDualIntegral;
|
||||
public double RelativeKktError;
|
||||
}
|
||||
|
||||
[StructLayout(LayoutKind.Sequential, Pack = 8)]
|
||||
internal struct OsqpSolution
|
||||
{
|
||||
public IntPtr Primal;
|
||||
public IntPtr Dual;
|
||||
public IntPtr PrimalInfeasibilityCertificate;
|
||||
public IntPtr DualInfeasibilityCertificate;
|
||||
}
|
||||
|
||||
[StructLayout(LayoutKind.Sequential, Pack = 8)]
|
||||
internal struct OsqpSolverPrefix
|
||||
{
|
||||
public IntPtr Settings;
|
||||
public IntPtr Solution;
|
||||
public IntPtr Info;
|
||||
public IntPtr Workspace;
|
||||
}
|
||||
|
||||
internal static class OsqpNativeStructures
|
||||
{
|
||||
public static void ValidatePinnedLayout()
|
||||
{
|
||||
AssertSize(typeof(OsqpCscMatrix), 48);
|
||||
AssertOffset(typeof(OsqpCscMatrix), nameof(OsqpCscMatrix.ColumnPointers), 8);
|
||||
AssertOffset(typeof(OsqpCscMatrix), nameof(OsqpCscMatrix.Values), 24);
|
||||
AssertOffset(typeof(OsqpCscMatrix), nameof(OsqpCscMatrix.MaximumNonZeroCount), 32);
|
||||
|
||||
AssertSize(typeof(OsqpSettings), 192);
|
||||
AssertOffset(typeof(OsqpSettings), nameof(OsqpSettings.Rho), 32);
|
||||
AssertOffset(typeof(OsqpSettings), nameof(OsqpSettings.Sigma), 48);
|
||||
AssertOffset(typeof(OsqpSettings), nameof(OsqpSettings.ConjugateGradientToleranceFraction), 72);
|
||||
AssertOffset(typeof(OsqpSettings), nameof(OsqpSettings.AdaptiveRhoFraction), 96);
|
||||
AssertOffset(typeof(OsqpSettings), nameof(OsqpSettings.AbsoluteTolerance), 120);
|
||||
AssertOffset(typeof(OsqpSettings), nameof(OsqpSettings.TimeLimit), 168);
|
||||
AssertOffset(typeof(OsqpSettings), nameof(OsqpSettings.PolishingRefinementIterations), 184);
|
||||
|
||||
AssertSize(typeof(OsqpInfo), 152);
|
||||
AssertOffset(typeof(OsqpInfo), nameof(OsqpInfo.StatusValue), 32);
|
||||
AssertOffset(typeof(OsqpInfo), nameof(OsqpInfo.ObjectiveValue), 40);
|
||||
AssertOffset(typeof(OsqpInfo), nameof(OsqpInfo.Iterations), 80);
|
||||
AssertOffset(typeof(OsqpInfo), nameof(OsqpInfo.SetupTime), 96);
|
||||
AssertOffset(typeof(OsqpInfo), nameof(OsqpInfo.RelativeKktError), 144);
|
||||
|
||||
AssertSize(typeof(OsqpSolution), 32);
|
||||
AssertSize(typeof(OsqpSolverPrefix), 32);
|
||||
AssertOffset(typeof(OsqpSolverPrefix), nameof(OsqpSolverPrefix.Workspace), 24);
|
||||
}
|
||||
|
||||
public static OsqpNativeSetupMemory AllocateSetupMemory(OsqpNativeMethods.SetDefaultSettingsDelegate setDefaultSettings)
|
||||
{
|
||||
if (setDefaultSettings == null)
|
||||
throw new ArgumentNullException(nameof(setDefaultSettings));
|
||||
|
||||
return new OsqpNativeSetupMemory(setDefaultSettings);
|
||||
}
|
||||
|
||||
private static void AssertSize(Type type, int expectedSize)
|
||||
{
|
||||
int actualSize = Marshal.SizeOf(type);
|
||||
if (actualSize != expectedSize)
|
||||
throw new InvalidOperationException(type.Name + " has native size " + actualSize + " instead of " + expectedSize + ".");
|
||||
}
|
||||
|
||||
private static void AssertOffset(Type type, string fieldName, int expectedOffset)
|
||||
{
|
||||
int actualOffset = checked((int)Marshal.OffsetOf(type, fieldName).ToInt64());
|
||||
if (actualOffset != expectedOffset)
|
||||
throw new InvalidOperationException(type.Name + "." + fieldName + " has native offset " + actualOffset + " instead of " + expectedOffset + ".");
|
||||
}
|
||||
}
|
||||
|
||||
internal sealed class OsqpNativeSetupMemory : IDisposable
|
||||
{
|
||||
private IntPtr upperTriangularP;
|
||||
private IntPtr constraints;
|
||||
private IntPtr settings;
|
||||
|
||||
public OsqpNativeSetupMemory(OsqpNativeMethods.SetDefaultSettingsDelegate setDefaultSettings)
|
||||
{
|
||||
try
|
||||
{
|
||||
upperTriangularP = Marshal.AllocHGlobal(Marshal.SizeOf(typeof(OsqpCscMatrix)));
|
||||
constraints = Marshal.AllocHGlobal(Marshal.SizeOf(typeof(OsqpCscMatrix)));
|
||||
settings = Marshal.AllocHGlobal(Marshal.SizeOf(typeof(OsqpSettings)));
|
||||
Zero(upperTriangularP, Marshal.SizeOf(typeof(OsqpCscMatrix)));
|
||||
Zero(constraints, Marshal.SizeOf(typeof(OsqpCscMatrix)));
|
||||
Zero(settings, Marshal.SizeOf(typeof(OsqpSettings)));
|
||||
setDefaultSettings(settings);
|
||||
}
|
||||
catch
|
||||
{
|
||||
Dispose();
|
||||
throw;
|
||||
}
|
||||
}
|
||||
|
||||
public IntPtr UpperTriangularP { get { return upperTriangularP; } }
|
||||
|
||||
public IntPtr Constraints { get { return constraints; } }
|
||||
|
||||
public IntPtr Settings { get { return settings; } }
|
||||
|
||||
public void Dispose()
|
||||
{
|
||||
if (settings != IntPtr.Zero)
|
||||
{
|
||||
Marshal.FreeHGlobal(settings);
|
||||
settings = IntPtr.Zero;
|
||||
}
|
||||
if (constraints != IntPtr.Zero)
|
||||
{
|
||||
Marshal.FreeHGlobal(constraints);
|
||||
constraints = IntPtr.Zero;
|
||||
}
|
||||
if (upperTriangularP != IntPtr.Zero)
|
||||
{
|
||||
Marshal.FreeHGlobal(upperTriangularP);
|
||||
upperTriangularP = IntPtr.Zero;
|
||||
}
|
||||
}
|
||||
|
||||
private static void Zero(IntPtr memory, int byteCount)
|
||||
{
|
||||
for (int index = 0; index < byteCount; index++)
|
||||
Marshal.WriteByte(memory, index, 0);
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user