485 lines
26 KiB
C#
485 lines
26 KiB
C#
using System;
|
|
using System.Collections.Generic;
|
|
using System.Diagnostics;
|
|
using System.IO;
|
|
using System.Threading;
|
|
using EMPlannerVerificationHost;
|
|
using MultiWheelC.TrajectoryPlanning.CoarsePath;
|
|
|
|
namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
|
|
|
|
internal static class LongitudinalIntegrationChecks
|
|
{
|
|
public static void Run()
|
|
{
|
|
VerifiesLastStrictCandidateSurvivesLaterTimeout();
|
|
VerifiesInvalidAndInaccurateCandidatesNeverBecomeFallbacks();
|
|
VerifiesEnvelopeLinearizationAdvancesAfterStrictRejection();
|
|
VerifiesRejectedTerminalPathSStillUpdatesEnvelope();
|
|
VerifiesValidatedEndpointsAreCanonical();
|
|
VerifiesWarmStartAndFiveIterationLimit();
|
|
VerifiesNonzeroSpeedSeedIsStrictlyFeasible();
|
|
VerifiesCancellationInfeasibilityAndPlannerDelegation();
|
|
RunRealOsqpInCleanPluginBundle();
|
|
}
|
|
|
|
public static void RunRealOsqp()
|
|
{
|
|
foreach (LongitudinalScenario scenario in CreateRealOsqpScenarios())
|
|
{
|
|
LongitudinalPlanningResult first = new LongitudinalPlanner(new OsqpNativeSolver()).Plan(scenario.Input,
|
|
CancellationToken.None);
|
|
LongitudinalPlanningResult second = new LongitudinalPlanner(new OsqpNativeSolver()).Plan(scenario.Input,
|
|
CancellationToken.None);
|
|
VerifyRealScenario(scenario, first);
|
|
VerifyRealScenario(scenario, second);
|
|
VerifyDeterministicResult(scenario.Name, first, second);
|
|
}
|
|
}
|
|
|
|
private static void RunRealOsqpInCleanPluginBundle()
|
|
{
|
|
string pluginDirectory = Path.Combine(Path.GetTempPath(), "em-planner-longitudinal-real-" + Guid.NewGuid().ToString("N"));
|
|
try
|
|
{
|
|
Directory.CreateDirectory(pluginDirectory);
|
|
foreach (string sourcePath in Directory.GetFiles(AppContext.BaseDirectory))
|
|
File.Copy(sourcePath, Path.Combine(pluginDirectory, Path.GetFileName(sourcePath)), false);
|
|
string nativeSource = Path.GetFullPath(Path.Combine(AppContext.BaseDirectory, "..", "..", "..", "..", "..",
|
|
"ThirdParty", "OSQP", "win-x64", "osqp.dll"));
|
|
Verification.True(File.Exists(nativeSource), "pinned OSQP DLL is available for the real longitudinal bundle");
|
|
File.Copy(nativeSource, Path.Combine(pluginDirectory, "osqp.dll"), true);
|
|
|
|
var startInfo = new ProcessStartInfo
|
|
{
|
|
FileName = Path.Combine(pluginDirectory, "EMPlannerVerificationHost.exe"),
|
|
Arguments = "longitudinal-real-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 || standardOutput.IndexOf("PASS longitudinal-real-osqp", StringComparison.Ordinal) < 0)
|
|
{
|
|
throw new InvalidOperationException("Real longitudinal OSQP clean-plugin probe exited " + process.ExitCode + ": " +
|
|
standardError + standardOutput);
|
|
}
|
|
}
|
|
}
|
|
finally
|
|
{
|
|
if (Directory.Exists(pluginDirectory))
|
|
Directory.Delete(pluginDirectory, true);
|
|
}
|
|
}
|
|
|
|
private static void VerifiesLastStrictCandidateSurvivesLaterTimeout()
|
|
{
|
|
LongitudinalPlanningInput input = CreateFakeInput(out LongitudinalCandidate valid);
|
|
var solver = new FakeQpSolver(new[]
|
|
{
|
|
Result(QpSolveStatus.Solved, ToPrimal(valid), 10d),
|
|
Result(QpSolveStatus.TimeLimit, Array.Empty<double>(), 10d),
|
|
});
|
|
|
|
LongitudinalPlanningResult result = new SequentialLongitudinalOptimizer(solver).Optimize(input,
|
|
CancellationToken.None);
|
|
Verification.Equal(EmPlanningStatus.SuccessWithFallback, result.Status,
|
|
"timeout after strict candidate returns fallback success");
|
|
LongitudinalCandidate fallback = result.Candidate ?? throw new InvalidOperationException("Fallback candidate was missing.");
|
|
Verification.NearlyEqual(valid.S[1], fallback.S[1], "last strict candidate remains the fallback");
|
|
}
|
|
|
|
private static void VerifiesInvalidAndInaccurateCandidatesNeverBecomeFallbacks()
|
|
{
|
|
LongitudinalPlanningInput input = CreateFakeInput(out LongitudinalCandidate valid);
|
|
double[] invalid = ToPrimal(valid);
|
|
var layout = new LongitudinalVariableLayout(valid.KnotTimes.Count);
|
|
invalid[layout.U(1)] = 10d;
|
|
var invalidSolver = new FakeQpSolver(new[]
|
|
{
|
|
Result(QpSolveStatus.Solved, invalid, 1d),
|
|
Result(QpSolveStatus.TimeLimit, Array.Empty<double>(), 1d),
|
|
});
|
|
LongitudinalPlanningResult invalidResult = new SequentialLongitudinalOptimizer(invalidSolver).Optimize(input,
|
|
CancellationToken.None);
|
|
Verification.Equal(EmPlanningStatus.SolverTimedOut, invalidResult.Status,
|
|
"invalid solver vector cannot become fallback");
|
|
Verification.True(invalidResult.Candidate == null, "invalid solver vector publishes no candidate");
|
|
|
|
var inaccurateSolver = new FakeQpSolver(new[]
|
|
{
|
|
Result(QpSolveStatus.SolvedInaccurate, ToPrimal(valid), 1d, 2e-5d, 0d),
|
|
Result(QpSolveStatus.TimeLimit, Array.Empty<double>(), 1d),
|
|
});
|
|
LongitudinalPlanningResult inaccurateResult = new SequentialLongitudinalOptimizer(inaccurateSolver).Optimize(input,
|
|
CancellationToken.None);
|
|
Verification.Equal(EmPlanningStatus.SolverTimedOut, inaccurateResult.Status,
|
|
"inaccurate residual candidate cannot become fallback");
|
|
Verification.True(inaccurateResult.Candidate == null, "inaccurate residual publishes no candidate");
|
|
double[] inaccuratePrimal = ToPrimal(valid);
|
|
for (int index = 0; index < inaccuratePrimal.Length; index++)
|
|
{
|
|
Verification.NearlyEqual(inaccuratePrimal[index], inaccurateSolver.WarmStarts[1][index],
|
|
"inaccurate finite primal only warms the next ST QP " + index);
|
|
}
|
|
}
|
|
|
|
private static void VerifiesWarmStartAndFiveIterationLimit()
|
|
{
|
|
LongitudinalPlanningInput input = CreateFakeInput(out LongitudinalCandidate valid);
|
|
var firstSolveOnly = new FakeQpSolver(Result(QpSolveStatus.TimeLimit, Array.Empty<double>(), 1d));
|
|
new SequentialLongitudinalOptimizer(firstSolveOnly).Optimize(input, CancellationToken.None);
|
|
Verification.Equal(true, firstSolveOnly.LastSettings != null && firstSolveOnly.LastSettings.EnableWarmStart,
|
|
"first ST solve enables a dynamics-consistent native warm start");
|
|
Verification.True(FromPrimal(valid.KnotTimes, firstSolveOnly.WarmStarts[0]).SatisfiesExactDiscreteDynamics(1e-12d),
|
|
"first ST warm start satisfies exact constant-jerk dynamics");
|
|
|
|
var results = new List<QpSolveResult>();
|
|
for (int index = 0; index < 5; index++)
|
|
results.Add(Result(QpSolveStatus.Solved, ToPrimal(valid), 100d - 10d * index));
|
|
var solver = new FakeQpSolver(results);
|
|
|
|
LongitudinalPlanningResult result = new SequentialLongitudinalOptimizer(solver).Optimize(input,
|
|
CancellationToken.None);
|
|
Verification.Equal(EmPlanningStatus.Success, result.Status, "five solved iterations publish success");
|
|
Verification.Equal(5, solver.SolveCallCount, "ST has a hard five-envelope-iteration maximum");
|
|
Verification.Equal(new LongitudinalVariableLayout(valid.KnotTimes.Count).VariableCount, solver.WarmStarts[0].Count,
|
|
"first ST linearization seed remains a complete primal vector");
|
|
Verification.Equal(true, solver.LastSettings != null && solver.LastSettings.EnableWarmStart,
|
|
"later ST solves enable native warm start");
|
|
for (int index = 0; index < solver.WarmStarts[1].Count; index++)
|
|
Verification.NearlyEqual(ToPrimal(valid)[index], solver.WarmStarts[1][index], "strict candidate warms the next QP " + index);
|
|
}
|
|
|
|
private static void VerifiesEnvelopeLinearizationAdvancesAfterStrictRejection()
|
|
{
|
|
LongitudinalPlanningInput baseline = CreateFakeInput(out LongitudinalCandidate candidate);
|
|
var curvedPath = new LateralPath(new[]
|
|
{
|
|
Point(0d, 0d, 0d),
|
|
Point(1d, candidate.S[1], 10000d),
|
|
Point(2d, baseline.TerminalPathS, 0d),
|
|
}, true);
|
|
var input = new LongitudinalPlanningInput(curvedPath, TravelDirection.Forward,
|
|
baseline.InitialProgressSpeedMetersPerSecond, baseline.InitialAccelerationMetersPerSecondSquared,
|
|
baseline.TerminalType, baseline.Configuration, Array.Empty<double>(), Array.Empty<double>());
|
|
EmPlanningStatus speedStatus = new PathSpeedLimitBuilder().Build(input, out PathSpeedLimit envelope,
|
|
out string speedFailure);
|
|
Verification.Equal(EmPlanningStatus.Success, speedStatus, "curved-envelope setup: " + speedFailure);
|
|
Verification.True(candidate.U[1] > envelope.MaximumSpeedAt(candidate.S[1]),
|
|
"scripted candidate violates its own curvature speed envelope");
|
|
var solver = new FakeQpSolver(new[]
|
|
{
|
|
Result(QpSolveStatus.Solved, ToPrimal(candidate), 2d),
|
|
Result(QpSolveStatus.TimeLimit, Array.Empty<double>(), 2d),
|
|
});
|
|
|
|
new SequentialLongitudinalOptimizer(solver).Optimize(input, CancellationToken.None);
|
|
Verification.Equal(2, solver.SolveCallCount, "rejected candidate reaches the next envelope iteration");
|
|
var layout = new LongitudinalVariableLayout(candidate.KnotTimes.Count);
|
|
FindSingleVariableBounds(solver.Problems[1], layout.U(1), out _, out double secondUpper);
|
|
double initialProgress = solver.WarmStarts[0][layout.S(1)];
|
|
double timeStep = candidate.KnotTimes[2] - candidate.KnotTimes[1];
|
|
double expectedAdvance = Math.Max(Math.Max(0d, candidate.S[1] - initialProgress), candidate.U[1] * timeStep);
|
|
double expectedProgress = candidate.S[1] + Math.Min(expectedAdvance,
|
|
0.5d * (input.TerminalPathS - candidate.S[1]));
|
|
Verification.True(expectedProgress > candidate.S[1], "scripted rejection advances the ST PathS envelope probe");
|
|
Verification.NearlyEqual(envelope.MaximumSpeedAt(expectedProgress), secondUpper,
|
|
"next ST QP samples the bounded forward-extrapolated PathS envelope");
|
|
}
|
|
|
|
private static void VerifiesRejectedTerminalPathSStillUpdatesEnvelope()
|
|
{
|
|
LongitudinalPlanningInput baseline = CreateFakeInput(out LongitudinalCandidate valid);
|
|
double[] perturbedProgress = new double[valid.S.Count];
|
|
for (int index = 0; index < perturbedProgress.Length; index++)
|
|
perturbedProgress[index] = valid.S[index];
|
|
perturbedProgress[perturbedProgress.Length - 1] += 0.05d;
|
|
var toleranceCandidate = new LongitudinalCandidate(valid.KnotTimes, perturbedProgress, valid.U, valid.A, valid.J);
|
|
var curvedPath = new LateralPath(new[]
|
|
{
|
|
Point(0d, 0d, 0d),
|
|
Point(1d, valid.S[1], 10000d),
|
|
Point(2d, baseline.TerminalPathS, 0d),
|
|
}, true);
|
|
var input = new LongitudinalPlanningInput(curvedPath, TravelDirection.Forward,
|
|
baseline.InitialProgressSpeedMetersPerSecond, baseline.InitialAccelerationMetersPerSecondSquared,
|
|
baseline.TerminalType, baseline.Configuration, Array.Empty<double>(), Array.Empty<double>());
|
|
var solver = new FakeQpSolver(new[]
|
|
{
|
|
Result(QpSolveStatus.Solved, ToPrimal(toleranceCandidate), 2d),
|
|
Result(QpSolveStatus.TimeLimit, Array.Empty<double>(), 2d),
|
|
});
|
|
|
|
new SequentialLongitudinalOptimizer(solver).Optimize(input, CancellationToken.None);
|
|
Verification.Equal(2, solver.SolveCallCount, "rejected terminal PathS still reaches a new envelope iteration");
|
|
var layout = new LongitudinalVariableLayout(valid.KnotTimes.Count);
|
|
FindSingleVariableBounds(solver.Problems[0], layout.U(1), out _, out double firstUpper);
|
|
FindSingleVariableBounds(solver.Problems[1], layout.U(1), out _, out double secondUpper);
|
|
Verification.True(Math.Abs(firstUpper - secondUpper) > 1e-12d,
|
|
"rejected terminal PathS is projected before the next envelope sample");
|
|
}
|
|
|
|
private static void VerifiesCancellationInfeasibilityAndPlannerDelegation()
|
|
{
|
|
LongitudinalPlanningInput input = CreateFakeInput(out LongitudinalCandidate valid);
|
|
using (var cancellation = new CancellationTokenSource())
|
|
{
|
|
cancellation.Cancel();
|
|
var cancellationSolver = new FakeQpSolver(Result(QpSolveStatus.Solved, ToPrimal(valid), 1d));
|
|
LongitudinalPlanningResult cancelled = new SequentialLongitudinalOptimizer(cancellationSolver).Optimize(input,
|
|
cancellation.Token);
|
|
Verification.Equal(EmPlanningStatus.Cancelled, cancelled.Status, "cancellation before QP solve");
|
|
Verification.Equal(0, cancellationSolver.SolveCallCount, "cancelled ST does not call the QP solver");
|
|
}
|
|
|
|
var infeasibleSolver = new FakeQpSolver(Result(QpSolveStatus.PrimalInfeasible, Array.Empty<double>(), 1d));
|
|
LongitudinalPlanningResult infeasible = new SequentialLongitudinalOptimizer(infeasibleSolver).Optimize(input,
|
|
CancellationToken.None);
|
|
Verification.Equal(EmPlanningStatus.LongitudinalInfeasible, infeasible.Status, "QP infeasibility is longitudinal");
|
|
|
|
var plannerSolver = new FakeQpSolver(new[]
|
|
{
|
|
Result(QpSolveStatus.Solved, ToPrimal(valid), 2d),
|
|
Result(QpSolveStatus.Solved, ToPrimal(valid), 1d),
|
|
Result(QpSolveStatus.Solved, ToPrimal(valid), 1d),
|
|
});
|
|
LongitudinalPlanningResult delegated = new LongitudinalPlanner(plannerSolver).Plan(input, CancellationToken.None);
|
|
Verification.Equal(EmPlanningStatus.Success, delegated.Status, "LongitudinalPlanner delegates to the ST optimizer");
|
|
}
|
|
|
|
private static void VerifiesNonzeroSpeedSeedIsStrictlyFeasible()
|
|
{
|
|
LongitudinalPlanningInput input = CreateRealScenario("seed", TravelDirection.Forward, 0.20d, 0d, 0.20d, 0d).Input;
|
|
var solver = new FakeQpSolver(Result(QpSolveStatus.TimeLimit, Array.Empty<double>(), 1d));
|
|
LongitudinalPlanningResult result = new SequentialLongitudinalOptimizer(solver).Optimize(input,
|
|
CancellationToken.None);
|
|
IReadOnlyList<double> times = LongitudinalCandidate.CreateKnotTimes(
|
|
input.Configuration.Scheduling.TimeHorizonSeconds, input.Configuration.Scheduling.OutputTimeStepSeconds);
|
|
LongitudinalCandidate seed = FromPrimal(times, solver.WarmStarts[0]);
|
|
EmPlanningStatus speedStatus = new PathSpeedLimitBuilder().Build(input, out PathSpeedLimit envelope,
|
|
out string speedFailure);
|
|
Verification.Equal(EmPlanningStatus.Success, speedStatus, "nonzero-speed seed envelope: " + speedFailure);
|
|
Verification.True(new LongitudinalSolutionValidator().TryValidate(input, envelope, seed, out _,
|
|
out string validationFailure), "nonzero-speed seed is strictly feasible: " + validationFailure);
|
|
Verification.Equal(EmPlanningStatus.SuccessWithFallback, result.Status,
|
|
"strictly validated initial seed survives an immediate solver timeout");
|
|
Verification.True(result.Candidate != null,
|
|
"strictly validated initial seed is retained as the timeout fallback");
|
|
}
|
|
|
|
private static void VerifiesValidatedEndpointsAreCanonical()
|
|
{
|
|
LongitudinalPlanningInput input = CreateFakeInput(out LongitudinalCandidate valid);
|
|
double toleranceOffset = 0.5d * input.Configuration.Validation.KinematicTolerance;
|
|
double[] progress = new double[valid.S.Count];
|
|
double[] speed = new double[valid.U.Count];
|
|
for (int index = 0; index < progress.Length; index++)
|
|
{
|
|
progress[index] = valid.S[index];
|
|
speed[index] = valid.U[index];
|
|
}
|
|
progress[progress.Length - 1] += toleranceOffset;
|
|
speed[speed.Length - 1] += toleranceOffset;
|
|
var toleranceCandidate = new LongitudinalCandidate(valid.KnotTimes, progress, speed, valid.A, valid.J);
|
|
var solver = new FakeQpSolver(new[]
|
|
{
|
|
Result(QpSolveStatus.Solved, ToPrimal(toleranceCandidate), 2d),
|
|
Result(QpSolveStatus.TimeLimit, Array.Empty<double>(), 2d),
|
|
});
|
|
|
|
LongitudinalPlanningResult result = new SequentialLongitudinalOptimizer(solver).Optimize(input,
|
|
CancellationToken.None);
|
|
|
|
Verification.Equal(EmPlanningStatus.SuccessWithFallback, result.Status,
|
|
"canonical strict candidate remains the timeout fallback");
|
|
LongitudinalCandidate canonical = result.Candidate ??
|
|
throw new InvalidOperationException("Canonical fallback candidate was missing.");
|
|
Verification.Equal(input.TerminalPathS, canonical.S[canonical.S.Count - 1],
|
|
"validated terminal PathS is canonicalized exactly");
|
|
Verification.Equal(0d, canonical.U[canonical.U.Count - 1],
|
|
"validated terminal speed is canonicalized exactly");
|
|
}
|
|
|
|
private static IReadOnlyList<LongitudinalScenario> CreateRealOsqpScenarios()
|
|
{
|
|
return new[]
|
|
{
|
|
CreateRealScenario("forward", TravelDirection.Forward, 0.50d, 0d, 0d, 0d),
|
|
CreateRealScenario("reverse", TravelDirection.Reverse, 0.50d, 0d, 0d, 0d),
|
|
CreateRealScenario("curvature-limited", TravelDirection.Forward, 0.35d, 20d, 0d, 0d),
|
|
CreateRealScenario("jerk-limited-stop", TravelDirection.Forward, 0.20d, 0d, 0.20d, 0d),
|
|
CreateRealScenario("short-segment", TravelDirection.Forward, 0.05d, 0d, 0d, 0d),
|
|
CreateRealScenario("zero-start-speed", TravelDirection.Forward, 0.50d, 0d, 0d, 0d),
|
|
};
|
|
}
|
|
|
|
private static LongitudinalScenario CreateRealScenario(string name, TravelDirection direction, double terminalPathS,
|
|
double middleCurvature, double initialSpeed, double initialAcceleration)
|
|
{
|
|
EmPlannerConfiguration configuration = EmPlannerConfiguration.CreateDefault();
|
|
configuration.Scheduling.SolverTimeoutSeconds = 1d;
|
|
configuration.Validation.KinematicTolerance = 1e-5d;
|
|
var points = new[]
|
|
{
|
|
Point(0d, 0d, 0d),
|
|
Point(1d, terminalPathS * 0.5d, middleCurvature),
|
|
Point(2d, terminalPathS, 0d),
|
|
};
|
|
return new LongitudinalScenario(name, new LongitudinalPlanningInput(new LateralPath(points, true), direction,
|
|
initialSpeed, initialAcceleration, EmTerminalType.Goal, configuration, Array.Empty<double>(), Array.Empty<double>()));
|
|
}
|
|
|
|
private static void VerifyRealScenario(LongitudinalScenario scenario, LongitudinalPlanningResult result)
|
|
{
|
|
Verification.True(result.Status == EmPlanningStatus.Success || result.Status == EmPlanningStatus.SuccessWithFallback,
|
|
scenario.Name + " returns a strict profile: " + result.FailureReason);
|
|
LongitudinalCandidate candidate = result.Candidate ?? throw new InvalidOperationException(scenario.Name + " candidate missing.");
|
|
Verification.NearlyEqual(scenario.Input.TerminalPathS, candidate.S[candidate.S.Count - 1],
|
|
scenario.Name + " exact terminal PathS");
|
|
Verification.NearlyEqual(0d, candidate.U[candidate.U.Count - 1], scenario.Name + " exact terminal speed");
|
|
PathSpeedLimitBuilder builder = new PathSpeedLimitBuilder();
|
|
EmPlanningStatus speedStatus = builder.Build(scenario.Input, out PathSpeedLimit envelope, out string speedFailure);
|
|
Verification.Equal(EmPlanningStatus.Success, speedStatus, scenario.Name + " envelope: " + speedFailure);
|
|
Verification.True(new LongitudinalSolutionValidator().TryValidate(scenario.Input, envelope, candidate,
|
|
out _, out string validationFailure), scenario.Name + " strict physical validation: " + validationFailure);
|
|
}
|
|
|
|
private static void VerifyDeterministicResult(string name, LongitudinalPlanningResult first, LongitudinalPlanningResult second)
|
|
{
|
|
Verification.Equal(first.Status, second.Status, name + " deterministic status");
|
|
LongitudinalCandidate left = first.Candidate ?? throw new InvalidOperationException(name + " first candidate missing.");
|
|
LongitudinalCandidate right = second.Candidate ?? throw new InvalidOperationException(name + " second candidate missing.");
|
|
for (int index = 0; index < left.S.Count; index++)
|
|
{
|
|
Verification.NearlyEqual(left.S[index], right.S[index], name + " deterministic S " + index);
|
|
Verification.NearlyEqual(left.U[index], right.U[index], name + " deterministic U " + index);
|
|
Verification.NearlyEqual(left.A[index], right.A[index], name + " deterministic A " + index);
|
|
}
|
|
for (int index = 0; index < left.J.Count; index++)
|
|
Verification.NearlyEqual(left.J[index], right.J[index], name + " deterministic J " + index);
|
|
}
|
|
|
|
private static LongitudinalPlanningInput CreateFakeInput(out LongitudinalCandidate valid)
|
|
{
|
|
EmPlannerConfiguration configuration = EmPlannerConfiguration.CreateDefault();
|
|
configuration.Scheduling.TimeHorizonSeconds = 1d;
|
|
configuration.Scheduling.OutputTimeStepSeconds = 0.25d;
|
|
configuration.Scheduling.SolverTimeoutSeconds = 1d;
|
|
configuration.Longitudinal.MaximumForwardSpeedMetersPerSecond = 1d;
|
|
configuration.Longitudinal.MaximumReverseSpeedMetersPerSecond = 1d;
|
|
configuration.Longitudinal.MaximumAccelerationMetersPerSecondSquared = 1d;
|
|
configuration.Longitudinal.MaximumDecelerationMetersPerSecondSquared = 1d;
|
|
configuration.Longitudinal.MaximumJerkMetersPerSecondCubed = 1d;
|
|
configuration.Longitudinal.MaximumLateralAccelerationMetersPerSecondSquared = 1d;
|
|
configuration.Longitudinal.MaximumCurvatureRatePerMeterPerSecond = 1d;
|
|
IReadOnlyList<double> times = LongitudinalCandidate.CreateKnotTimes(1d, 0.25d);
|
|
valid = LongitudinalCandidate.Integrate(times, 0d, 0.10d, 0d,
|
|
new[] { -0.45714285714285714d, 0d, 0d, 0d });
|
|
double terminalPathS = valid.S[valid.S.Count - 1];
|
|
var path = new LateralPath(new[]
|
|
{
|
|
Point(0d, 0d, 0d),
|
|
Point(1d, terminalPathS * 0.5d, 0d),
|
|
Point(2d, terminalPathS, 0d),
|
|
}, true);
|
|
return new LongitudinalPlanningInput(path, TravelDirection.Forward, 0.10d, 0d, EmTerminalType.Goal,
|
|
configuration, Array.Empty<double>(), Array.Empty<double>());
|
|
}
|
|
|
|
private static LateralPathPoint Point(double referenceS, double pathS, double curvature)
|
|
{
|
|
return new LateralPathPoint(referenceS, pathS, 0d, 0d, 0d, 0d, pathS, 0d, 0d, curvature, curvature, 0d);
|
|
}
|
|
|
|
private static QpSolveResult Result(QpSolveStatus status, IReadOnlyList<double> primal, double objective,
|
|
double primalResidual = 0d, double dualResidual = 0d)
|
|
{
|
|
return new QpSolveResult(status, primal, objective, primalResidual, dualResidual, 1, TimeSpan.Zero,
|
|
status.ToString(), string.Empty);
|
|
}
|
|
|
|
private static double[] ToPrimal(LongitudinalCandidate candidate)
|
|
{
|
|
var layout = new LongitudinalVariableLayout(candidate.KnotTimes.Count);
|
|
var primal = new double[layout.VariableCount];
|
|
for (int index = 0; index < layout.KnotCount; index++)
|
|
{
|
|
primal[layout.S(index)] = candidate.S[index];
|
|
primal[layout.U(index)] = candidate.U[index];
|
|
primal[layout.A(index)] = candidate.A[index];
|
|
}
|
|
for (int index = 0; index < layout.KnotCount - 1; index++)
|
|
primal[layout.J(index)] = candidate.J[index];
|
|
return primal;
|
|
}
|
|
|
|
private static LongitudinalCandidate FromPrimal(IReadOnlyList<double> times, IReadOnlyList<double> primal)
|
|
{
|
|
var layout = new LongitudinalVariableLayout(times.Count);
|
|
var s = new double[layout.KnotCount];
|
|
var u = new double[layout.KnotCount];
|
|
var a = new double[layout.KnotCount];
|
|
var j = new double[layout.KnotCount - 1];
|
|
for (int index = 0; index < layout.KnotCount; index++)
|
|
{
|
|
s[index] = primal[layout.S(index)];
|
|
u[index] = primal[layout.U(index)];
|
|
a[index] = primal[layout.A(index)];
|
|
}
|
|
for (int index = 0; index < j.Length; index++)
|
|
j[index] = primal[layout.J(index)];
|
|
return new LongitudinalCandidate(times, s, u, a, j);
|
|
}
|
|
|
|
private static void FindSingleVariableBounds(QuadraticProgram problem, int variable, out double lower, out double upper)
|
|
{
|
|
for (int row = 0; row < problem.ConstraintCount; row++)
|
|
{
|
|
int found = 0;
|
|
double coefficient = 0d;
|
|
for (int column = 0; column < problem.ConstraintMatrix.ColumnCount; column++)
|
|
{
|
|
for (int index = problem.ConstraintMatrix.ColumnPointers[column];
|
|
index < problem.ConstraintMatrix.ColumnPointers[column + 1]; index++)
|
|
{
|
|
if (problem.ConstraintMatrix.RowIndices[index] == row)
|
|
{
|
|
found++;
|
|
if (column == variable)
|
|
coefficient = problem.ConstraintMatrix.Values[index];
|
|
}
|
|
}
|
|
}
|
|
if (found == 1 && Math.Abs(coefficient - 1d) <= 1e-12d)
|
|
{
|
|
lower = problem.LowerBounds[row];
|
|
upper = problem.UpperBounds[row];
|
|
return;
|
|
}
|
|
}
|
|
throw new InvalidOperationException("No single-variable bounds were found for ST variable " + variable + ".");
|
|
}
|
|
|
|
private sealed class LongitudinalScenario
|
|
{
|
|
public LongitudinalScenario(string name, LongitudinalPlanningInput input)
|
|
{
|
|
Name = name;
|
|
Input = input;
|
|
}
|
|
|
|
public string Name { get; }
|
|
|
|
public LongitudinalPlanningInput Input { get; }
|
|
}
|
|
}
|