669 lines
37 KiB
C#
669 lines
37 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()
|
|
{
|
|
VerifiesRollingOptimizationKeepsANonzeroTerminalSpeed();
|
|
VerifiesFullDirectionScheduleIsIndependentFromPublicationCadence();
|
|
VerifiesFullDirectionPublicationDoesNotDuplicateItsTerminalHold();
|
|
VerifiesExactStopIncludesAStabilizationTail();
|
|
VerifiesLastStrictCandidateSurvivesLaterTimeout();
|
|
VerifiesInvalidAndInaccurateCandidatesNeverBecomeFallbacks();
|
|
VerifiesEnvelopeLinearizationAdvancesAfterStrictRejection();
|
|
VerifiesExactStopTailDoesNotDistortEnvelope();
|
|
VerifiesValidatedEndpointsAreCanonical();
|
|
VerifiesWarmStartAndFiveIterationLimit();
|
|
VerifiesNonzeroSpeedSeedIsStrictlyFeasible();
|
|
VerifiesCancellationInfeasibilityAndPlannerDelegation();
|
|
RunRealOsqpInCleanPluginBundle();
|
|
}
|
|
|
|
private static void VerifiesRollingOptimizationKeepsANonzeroTerminalSpeed()
|
|
{
|
|
EmPlannerConfiguration configuration = EmPlannerConfiguration.CreateDefault();
|
|
configuration.Scheduling.DistanceHorizonMeters = 5d;
|
|
configuration.Scheduling.TimeHorizonSeconds = 2d;
|
|
configuration.Scheduling.OutputTimeStepSeconds = 0.10d;
|
|
configuration.Scheduling.SolverTimeoutSeconds = 1d;
|
|
configuration.Longitudinal.MaximumForwardSpeedMetersPerSecond = 0.20d;
|
|
LateralPath path = new LateralPath(new[]
|
|
{
|
|
Point(0d, 0d, 0d),
|
|
Point(1d, 2.5d, 0d),
|
|
Point(2d, 5d, 0d),
|
|
}, true);
|
|
var input = new LongitudinalPlanningInput(path, TravelDirection.Forward, 0.10d, 0d,
|
|
EmTerminalType.RollingSafetyStop, EmLongitudinalMode.RollingContinuation, configuration,
|
|
Array.Empty<double>(), Array.Empty<double>());
|
|
var solver = new FakeQpSolver(Result(QpSolveStatus.TimeLimit, Array.Empty<double>(), 1d));
|
|
|
|
LongitudinalPlanningResult result = new SequentialLongitudinalOptimizer(solver).Optimize(input,
|
|
CancellationToken.None);
|
|
|
|
Verification.Equal(EmPlanningStatus.SuccessWithFallback, result.Status,
|
|
"rolling seed remains a strict timeout fallback: " + result.FailureReason);
|
|
LongitudinalCandidate candidate = result.Candidate ??
|
|
throw new InvalidOperationException("Rolling timeout fallback candidate was missing.");
|
|
Verification.Equal(21, candidate.KnotTimes.Count, "two-second ST emits twenty-one knots");
|
|
Verification.True(candidate.S[candidate.S.Count - 1] < input.PathUpperBoundS,
|
|
"two-second ST does not consume the five-metre LS window");
|
|
Verification.True(candidate.U[candidate.U.Count - 1] > 0.01d,
|
|
"rolling ST keeps nonzero terminal speed");
|
|
}
|
|
|
|
private static void VerifiesFullDirectionScheduleIsIndependentFromPublicationCadence()
|
|
{
|
|
EmPlannerConfiguration configuration = EmPlannerConfiguration.CreateDefault();
|
|
configuration.Scheduling.TimeHorizonSeconds = 10d;
|
|
configuration.Scheduling.OutputTimeStepSeconds = 0.10d;
|
|
configuration.Scheduling.MaximumOptimizationTimeStepSeconds = 0.20d;
|
|
configuration.Scheduling.MaximumOptimizationSpatialStepMeters = 0.10d;
|
|
configuration.Scheduling.MaximumOptimizationKnotCount = 401;
|
|
LateralPath path = new LateralPath(new[]
|
|
{
|
|
Point(0d, 0d, 0d),
|
|
Point(1d, 1d, 0d),
|
|
Point(2d, 2d, 0d),
|
|
}, true);
|
|
|
|
EmPlanningStatus status = new PathSpeedLimitBuilder().Build(path, TravelDirection.Forward, 0.10d,
|
|
EmTerminalType.Goal, configuration, out PathSpeedLimit speedLimit, out string failureReason);
|
|
Verification.Equal(EmPlanningStatus.Success, status, "full schedule envelope: " + failureReason);
|
|
status = new FullDirectionSegmentScheduleBuilder().TryBuild(path, speedLimit, 0.10d, 0d,
|
|
configuration.Longitudinal.DesiredForwardSpeedMetersPerSecond, configuration,
|
|
out LongitudinalKnotSchedule coarsePublication, out failureReason);
|
|
Verification.Equal(EmPlanningStatus.Success, status, "full schedule with 0.10 s publication: " + failureReason);
|
|
|
|
EmPlannerConfiguration densePublicationConfiguration = configuration.Copy();
|
|
densePublicationConfiguration.Scheduling.OutputTimeStepSeconds = 0.05d;
|
|
status = new FullDirectionSegmentScheduleBuilder().TryBuild(path, speedLimit, 0.10d, 0d,
|
|
densePublicationConfiguration.Longitudinal.DesiredForwardSpeedMetersPerSecond, densePublicationConfiguration,
|
|
out LongitudinalKnotSchedule densePublication, out failureReason);
|
|
Verification.Equal(EmPlanningStatus.Success, status, "full schedule with 0.05 s publication: " + failureReason);
|
|
Verification.Equal(coarsePublication.KnotTimes.Count, densePublication.KnotTimes.Count,
|
|
"publication cadence does not determine full-segment optimization knot count");
|
|
|
|
var publicationCandidate = new LongitudinalCandidate(new[] { 0d, 0.50d, 1d },
|
|
new[] { 0d, 1d / 60d, 1d / 60d }, new[] { 0.10d, 0d, 0d }, new[] { -0.40d, 0d, 0d },
|
|
new[] { 0.80d, 0d });
|
|
var publicationResult = new LongitudinalPlanningResult(EmPlanningStatus.Success, publicationCandidate, string.Empty);
|
|
DateTimeOffset now = DateTimeOffset.UtcNow;
|
|
var metadata = new EmTrajectoryMetadata("publication-cadence", now, now, 1L, "publication-path", 1L,
|
|
string.Empty, 0, TravelDirection.Forward, EmTerminalType.Goal, EmLongitudinalMode.ExactStopAtBoundary,
|
|
EmPlanningScope.FullDirectionSegment);
|
|
configuration.Longitudinal.ZeroSpeedHoldSeconds = 0d;
|
|
densePublicationConfiguration.Longitudinal.ZeroSpeedHoldSeconds = 0d;
|
|
EmTrajectory coarseTrajectory = new EmTrajectoryAssembler(configuration).Assemble(path, publicationResult, metadata);
|
|
EmTrajectory denseTrajectory = new EmTrajectoryAssembler(densePublicationConfiguration).Assemble(path,
|
|
publicationResult, metadata);
|
|
Verification.Equal(2 * (coarseTrajectory.Points.Count - 1), denseTrajectory.Points.Count - 1,
|
|
"halving publication cadence doubles emitted trajectory intervals without changing optimization knots");
|
|
}
|
|
|
|
private static void VerifiesFullDirectionPublicationDoesNotDuplicateItsTerminalHold()
|
|
{
|
|
EmPlannerConfiguration configuration = EmPlannerConfiguration.CreateDefault();
|
|
configuration.Scheduling.OutputTimeStepSeconds = 0.10d;
|
|
configuration.Longitudinal.ZeroSpeedHoldSeconds = 0.20d;
|
|
LateralPath path = new LateralPath(new[]
|
|
{
|
|
Point(0d, 0d, 0d),
|
|
Point(1d, 0.0075d, 0d),
|
|
}, true);
|
|
var candidate = new LongitudinalCandidate(new[] { 0d, 0.10d, 0.20d, 0.40d },
|
|
new[] { 0d, 0.005d, 0.0075d, 0.0075d }, new[] { 0.05d, 0.025d, 0d, 0d },
|
|
new[] { 0d, -0.5d, 0d, 0d }, new[] { -5d, 5d, 0d });
|
|
var result = new LongitudinalPlanningResult(EmPlanningStatus.Success, candidate, string.Empty);
|
|
DateTimeOffset now = DateTimeOffset.UtcNow;
|
|
var metadata = new EmTrajectoryMetadata("full-hold", now, now, 1L, "hold-path", 1L, string.Empty, 0,
|
|
TravelDirection.Forward, EmTerminalType.Goal, EmLongitudinalMode.ExactStopAtBoundary,
|
|
EmPlanningScope.FullDirectionSegment);
|
|
|
|
EmTrajectory trajectory = new EmTrajectoryAssembler(configuration).Assemble(path, result, metadata);
|
|
|
|
Verification.NearlyEqual(0.40d, trajectory.Points[trajectory.Points.Count - 1].TimeFromStart,
|
|
"full-scope publication reuses its candidate hold instead of appending a second hold");
|
|
Verification.Equal(3, CountStationaryTerminalPoints(trajectory),
|
|
"full-scope publication emits the candidate's single nonzero terminal hold");
|
|
}
|
|
|
|
private static void VerifiesExactStopIncludesAStabilizationTail()
|
|
{
|
|
EmPlannerConfiguration configuration = CreateExactStopSeedConfiguration();
|
|
LateralPath path = new LateralPath(new[]
|
|
{
|
|
Point(0d, 0d, 0d),
|
|
Point(1d, 0.00375d, 0d),
|
|
Point(2d, 0.0075d, 0d),
|
|
}, true);
|
|
var input = new LongitudinalPlanningInput(path, TravelDirection.Forward, 0.05d, 0d,
|
|
EmTerminalType.Goal, EmLongitudinalMode.ExactStopAtBoundary, configuration,
|
|
Array.Empty<double>(), Array.Empty<double>());
|
|
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, "exact-stop seed envelope: " + speedFailure);
|
|
Verification.True(new LongitudinalSolutionValidator().TryValidate(input, envelope, seed, out _,
|
|
out string validationFailure), "exact-stop seed is strictly feasible: " + validationFailure);
|
|
Verification.Equal(EmPlanningStatus.SuccessWithFallback, result.Status,
|
|
"exact-stop seed remains a strict timeout fallback");
|
|
LongitudinalCandidate candidate = result.Candidate ??
|
|
throw new InvalidOperationException("Exact-stop timeout fallback candidate was missing.");
|
|
int stabilizationStart = LongitudinalTerminalSchedule.GetStabilizationStartIndex(
|
|
candidate.KnotTimes, input.Configuration.Scheduling.OutputTimeStepSeconds);
|
|
for (int index = stabilizationStart; index < candidate.S.Count; index++)
|
|
{
|
|
Verification.NearlyEqual(input.StopBoundaryPathS, candidate.S[index], "stop-tail S " + index);
|
|
Verification.NearlyEqual(0d, candidate.U[index], "stop-tail U " + index);
|
|
Verification.NearlyEqual(0d, candidate.A[index], "stop-tail A " + index);
|
|
}
|
|
}
|
|
|
|
private static int CountStationaryTerminalPoints(EmTrajectory trajectory)
|
|
{
|
|
double terminalPathS = trajectory.Points[trajectory.Points.Count - 1].PathS;
|
|
int count = 0;
|
|
for (int index = 0; index < trajectory.Points.Count; index++)
|
|
{
|
|
EmTrajectoryPoint point = trajectory.Points[index];
|
|
if (Math.Abs(point.PathS - terminalPathS) <= 1e-12d &&
|
|
Math.Abs(point.SignedLongitudinalVelocity) <= 1e-12d)
|
|
{
|
|
count++;
|
|
}
|
|
}
|
|
return count;
|
|
}
|
|
|
|
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.SuccessWithFallback, invalidResult.Status,
|
|
"invalid solver vector cannot replace the strict initial fallback");
|
|
Verification.NearlyEqual(valid.U[1], invalidResult.Candidate?.U[1] ?? double.NaN,
|
|
"invalid solver vector does not become the fallback 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.SuccessWithFallback, inaccurateResult.Status,
|
|
"inaccurate residual candidate cannot replace the strict initial fallback");
|
|
Verification.NearlyEqual(valid.U[1], inaccurateResult.Candidate?.U[1] ?? double.NaN,
|
|
"inaccurate residual does not become the fallback 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.PathUpperBoundS, 0d),
|
|
}, true);
|
|
var input = new LongitudinalPlanningInput(curvedPath, TravelDirection.Forward,
|
|
baseline.InitialProgressSpeedMetersPerSecond, baseline.InitialAccelerationMetersPerSecondSquared,
|
|
baseline.TerminalType, baseline.Mode, 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);
|
|
Verification.True(Math.Abs(secondUpper - envelope.MaximumSpeedAt(candidate.S[1])) > 1e-12d,
|
|
"scripted rejection advances the ST PathS envelope probe");
|
|
}
|
|
|
|
private static void VerifiesExactStopTailDoesNotDistortEnvelope()
|
|
{
|
|
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.PathUpperBoundS, 0d),
|
|
}, true);
|
|
var input = new LongitudinalPlanningInput(curvedPath, TravelDirection.Forward,
|
|
baseline.InitialProgressSpeedMetersPerSecond, baseline.InitialAccelerationMetersPerSecondSquared,
|
|
baseline.TerminalType, baseline.Mode, 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.NearlyEqual(firstUpper, secondUpper,
|
|
"exact stop-tail perturbations do not distort a moving-knot 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.SuccessWithFallback, infeasible.Status,
|
|
"QP infeasibility preserves the strict mode-specific seed");
|
|
Verification.True(infeasible.Candidate != null, "QP infeasibility retains a safe fallback profile");
|
|
|
|
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 = CreateFakeInput(out _);
|
|
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);
|
|
Verification.True(solver.WarmStarts.Count > 0,
|
|
"nonzero-speed seed reaches the ST solver: " + result.Status + " " + result.FailureReason);
|
|
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.StopBoundaryPathS, 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.50d, 0d, 0.05d, 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, EmLongitudinalMode.ApproachStopBoundary,
|
|
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.True(candidate.S[candidate.S.Count - 1] <= scenario.Input.PathUpperBoundS,
|
|
scenario.Name + " remains inside the PathS window");
|
|
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 = 1e-6d;
|
|
configuration.Longitudinal.MaximumDecelerationMetersPerSecondSquared = 1d;
|
|
configuration.Longitudinal.MaximumJerkMetersPerSecondCubed = 4d;
|
|
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.8d, 0d, 0.8d, 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,
|
|
EmLongitudinalMode.ExactStopAtBoundary, configuration, Array.Empty<double>(), Array.Empty<double>());
|
|
}
|
|
|
|
private static EmPlannerConfiguration CreateExactStopSeedConfiguration()
|
|
{
|
|
EmPlannerConfiguration configuration = EmPlannerConfiguration.CreateDefault();
|
|
configuration.Scheduling.TimeHorizonSeconds = 0.40d;
|
|
configuration.Scheduling.OutputTimeStepSeconds = 0.10d;
|
|
configuration.Scheduling.SolverTimeoutSeconds = 1d;
|
|
configuration.Longitudinal.MaximumForwardSpeedMetersPerSecond = 1d;
|
|
configuration.Longitudinal.MaximumReverseSpeedMetersPerSecond = 1d;
|
|
configuration.Longitudinal.MaximumAccelerationMetersPerSecondSquared = 1e-6d;
|
|
configuration.Longitudinal.MaximumDecelerationMetersPerSecondSquared = 1d;
|
|
configuration.Longitudinal.MaximumJerkMetersPerSecondCubed = 20d;
|
|
configuration.Longitudinal.MaximumLateralAccelerationMetersPerSecondSquared = 1d;
|
|
configuration.Longitudinal.MaximumCurvatureRatePerMeterPerSecond = 1d;
|
|
return configuration;
|
|
}
|
|
|
|
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; }
|
|
}
|
|
}
|