feat: optimize longitudinal ST profiles
This commit is contained in:
梁薄云
@@ -0,0 +1,20 @@
|
||||
using System;
|
||||
using System.Threading;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
|
||||
|
||||
/// <summary>Pure longitudinal planning facade that depends only on the solver-neutral IQpSolver boundary.</summary>
|
||||
public sealed class LongitudinalPlanner
|
||||
{
|
||||
private readonly SequentialLongitudinalOptimizer _optimizer;
|
||||
|
||||
public LongitudinalPlanner(IQpSolver qpSolver)
|
||||
{
|
||||
_optimizer = new SequentialLongitudinalOptimizer(qpSolver ?? throw new ArgumentNullException(nameof(qpSolver)));
|
||||
}
|
||||
|
||||
public LongitudinalPlanningResult Plan(LongitudinalPlanningInput input, CancellationToken cancellationToken)
|
||||
{
|
||||
return _optimizer.Optimize(input, cancellationToken);
|
||||
}
|
||||
}
|
||||
+153
@@ -0,0 +1,153 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
|
||||
|
||||
/// <summary>Independently validates ST candidates directly in physical units before they may become fallbacks.</summary>
|
||||
public sealed class LongitudinalSolutionValidator
|
||||
{
|
||||
public bool TryValidate(LongitudinalPlanningInput input, PathSpeedLimit speedLimit, LongitudinalCandidate candidate,
|
||||
out LongitudinalCandidate validatedCandidate, out string failureReason)
|
||||
{
|
||||
validatedCandidate = null;
|
||||
failureReason = string.Empty;
|
||||
if (input == null || speedLimit == null || candidate == null)
|
||||
{
|
||||
failureReason = "ST input, speed envelope, and candidate are required.";
|
||||
return false;
|
||||
}
|
||||
try
|
||||
{
|
||||
if (!PathSpeedLimitBuilder.TryGetLimits(input, out _, out double maximumAcceleration,
|
||||
out double maximumDeceleration, out double maximumJerk, out _, out _, out failureReason))
|
||||
{
|
||||
return false;
|
||||
}
|
||||
IReadOnlyList<double> expectedTimes = LongitudinalCandidate.CreateKnotTimes(
|
||||
input.Configuration.Scheduling.TimeHorizonSeconds, input.Configuration.Scheduling.OutputTimeStepSeconds);
|
||||
double tolerance = RequireNonnegative(input.Configuration.Validation.KinematicTolerance, nameof(tolerance));
|
||||
if (!HasMatchingTimes(candidate.KnotTimes, expectedTimes, tolerance))
|
||||
{
|
||||
failureReason = "ST candidate knot times do not match the configured horizon.";
|
||||
return false;
|
||||
}
|
||||
if (candidate.S.Count != expectedTimes.Count || candidate.U.Count != expectedTimes.Count ||
|
||||
candidate.A.Count != expectedTimes.Count || candidate.J.Count != expectedTimes.Count - 1)
|
||||
{
|
||||
failureReason = "ST candidate value counts do not match its time knots.";
|
||||
return false;
|
||||
}
|
||||
if (!candidate.SatisfiesExactDiscreteDynamics(tolerance))
|
||||
{
|
||||
failureReason = "ST candidate violates exact constant-jerk dynamics.";
|
||||
return false;
|
||||
}
|
||||
if (!AreClose(candidate.S[0], 0d, tolerance) ||
|
||||
!AreClose(candidate.U[0], input.InitialProgressSpeedMetersPerSecond, tolerance) ||
|
||||
!AreClose(candidate.A[0], input.InitialAccelerationMetersPerSecondSquared, tolerance))
|
||||
{
|
||||
failureReason = "ST candidate does not satisfy the exact initial state.";
|
||||
return false;
|
||||
}
|
||||
|
||||
for (int index = 0; index < candidate.S.Count; index++)
|
||||
{
|
||||
double progress = candidate.S[index];
|
||||
double speed = candidate.U[index];
|
||||
double acceleration = candidate.A[index];
|
||||
if (!IsFinite(progress) || !IsFinite(speed) || !IsFinite(acceleration) || progress < -tolerance ||
|
||||
progress > input.TerminalPathS + tolerance || speed < -tolerance ||
|
||||
acceleration < -maximumDeceleration - tolerance || acceleration > maximumAcceleration + tolerance)
|
||||
{
|
||||
failureReason = "ST candidate violates physical bounds at knot " + index + ".";
|
||||
return false;
|
||||
}
|
||||
double speedLimitAtProgress = speedLimit.MaximumSpeedAt(Math.Max(0d, Math.Min(input.TerminalPathS, progress)));
|
||||
if (speed > speedLimitAtProgress + tolerance)
|
||||
{
|
||||
failureReason = "ST candidate violates the actual-PathS speed envelope at knot " + index +
|
||||
" (S=" + progress + ", U=" + speed + ", limit=" + speedLimitAtProgress + ").";
|
||||
return false;
|
||||
}
|
||||
if (index > 0 && progress < candidate.S[index - 1] - tolerance)
|
||||
{
|
||||
failureReason = "ST candidate PathS decreases at knot " + index + ".";
|
||||
return false;
|
||||
}
|
||||
}
|
||||
for (int index = 0; index < candidate.J.Count; index++)
|
||||
{
|
||||
if (!IsFinite(candidate.J[index]) || Math.Abs(candidate.J[index]) > maximumJerk + tolerance)
|
||||
{
|
||||
failureReason = "ST candidate violates the jerk bound at interval " + index + ".";
|
||||
return false;
|
||||
}
|
||||
}
|
||||
int terminalIndex = candidate.S.Count - 1;
|
||||
if (!AreClose(candidate.S[terminalIndex], input.TerminalPathS, tolerance) ||
|
||||
!AreClose(candidate.U[terminalIndex], 0d, tolerance))
|
||||
{
|
||||
failureReason = "ST candidate does not satisfy the exact zero-speed terminal.";
|
||||
return false;
|
||||
}
|
||||
|
||||
var canonicalS = new double[candidate.S.Count];
|
||||
var canonicalU = new double[candidate.U.Count];
|
||||
var canonicalA = new double[candidate.A.Count];
|
||||
for (int index = 0; index < candidate.S.Count; index++)
|
||||
{
|
||||
canonicalS[index] = candidate.S[index];
|
||||
canonicalU[index] = candidate.U[index];
|
||||
canonicalA[index] = candidate.A[index];
|
||||
}
|
||||
canonicalS[0] = 0d;
|
||||
canonicalU[0] = input.InitialProgressSpeedMetersPerSecond;
|
||||
canonicalA[0] = input.InitialAccelerationMetersPerSecondSquared;
|
||||
canonicalS[terminalIndex] = input.TerminalPathS;
|
||||
canonicalU[terminalIndex] = 0d;
|
||||
var canonicalCandidate = new LongitudinalCandidate(candidate.KnotTimes, canonicalS, canonicalU, canonicalA,
|
||||
candidate.J);
|
||||
if (!canonicalCandidate.SatisfiesExactDiscreteDynamics(tolerance))
|
||||
{
|
||||
failureReason = "Canonical ST hard-boundary values exceed the dynamics tolerance.";
|
||||
return false;
|
||||
}
|
||||
validatedCandidate = canonicalCandidate;
|
||||
return true;
|
||||
}
|
||||
catch (ArgumentException exception)
|
||||
{
|
||||
failureReason = exception.Message;
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
private static bool HasMatchingTimes(IReadOnlyList<double> actual, IReadOnlyList<double> expected, double tolerance)
|
||||
{
|
||||
if (actual.Count != expected.Count)
|
||||
return false;
|
||||
for (int index = 0; index < expected.Count; index++)
|
||||
{
|
||||
if (!AreClose(actual[index], expected[index], tolerance))
|
||||
return false;
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
private static bool AreClose(double actual, double expected, double tolerance)
|
||||
{
|
||||
return Math.Abs(actual - expected) <= tolerance;
|
||||
}
|
||||
|
||||
private static double RequireNonnegative(double value, string parameterName)
|
||||
{
|
||||
if (!IsFinite(value) || value < 0d)
|
||||
throw new ArgumentOutOfRangeException(parameterName);
|
||||
return value;
|
||||
}
|
||||
|
||||
private static bool IsFinite(double value)
|
||||
{
|
||||
return !double.IsNaN(value) && !double.IsInfinity(value);
|
||||
}
|
||||
}
|
||||
+461
@@ -0,0 +1,461 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using System.Diagnostics;
|
||||
using System.Threading;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
|
||||
|
||||
/// <summary>Bounded ST envelope iteration retaining only independently validated physical candidates.</summary>
|
||||
public sealed class SequentialLongitudinalOptimizer
|
||||
{
|
||||
private const int MaximumEnvelopeIterations = 5;
|
||||
private const double OrdinaryEnvelopeProbeLookaheadSteps = 1d;
|
||||
private const double OrdinaryTerminalProbeFraction = 0.5d;
|
||||
private readonly IQpSolver _qpSolver;
|
||||
private readonly PathSpeedLimitBuilder _speedLimitBuilder;
|
||||
private readonly LongitudinalConstraintBuilder _constraintBuilder;
|
||||
private readonly LongitudinalSolutionValidator _solutionValidator;
|
||||
|
||||
public SequentialLongitudinalOptimizer(IQpSolver qpSolver)
|
||||
: this(qpSolver, new PathSpeedLimitBuilder(), new LongitudinalConstraintBuilder(new LongitudinalObjectiveBuilder()),
|
||||
new LongitudinalSolutionValidator())
|
||||
{
|
||||
}
|
||||
|
||||
internal SequentialLongitudinalOptimizer(IQpSolver qpSolver, PathSpeedLimitBuilder speedLimitBuilder,
|
||||
LongitudinalConstraintBuilder constraintBuilder, LongitudinalSolutionValidator solutionValidator)
|
||||
{
|
||||
_qpSolver = qpSolver ?? throw new ArgumentNullException(nameof(qpSolver));
|
||||
_speedLimitBuilder = speedLimitBuilder ?? throw new ArgumentNullException(nameof(speedLimitBuilder));
|
||||
_constraintBuilder = constraintBuilder ?? throw new ArgumentNullException(nameof(constraintBuilder));
|
||||
_solutionValidator = solutionValidator ?? throw new ArgumentNullException(nameof(solutionValidator));
|
||||
}
|
||||
|
||||
public LongitudinalPlanningResult Optimize(LongitudinalPlanningInput input, CancellationToken cancellationToken)
|
||||
{
|
||||
if (input == null)
|
||||
return Failed(EmPlanningStatus.InvalidInput, "Longitudinal planning input is required.");
|
||||
if (cancellationToken.IsCancellationRequested)
|
||||
return Failed(EmPlanningStatus.Cancelled, "Longitudinal optimization was cancelled.");
|
||||
if (!TryCreateSettings(input, out QpSolverSettings settings, out TimeSpan totalBudget, out double convergenceTolerance,
|
||||
out int iterationLimit, out string configurationFailure))
|
||||
{
|
||||
return Failed(EmPlanningStatus.InvalidInput, configurationFailure);
|
||||
}
|
||||
|
||||
EmPlanningStatus speedStatus = _speedLimitBuilder.Build(input, out PathSpeedLimit speedLimit, out string speedFailure);
|
||||
if (speedStatus != EmPlanningStatus.Success)
|
||||
return Failed(speedStatus, speedFailure);
|
||||
|
||||
LongitudinalCandidate iterate = CreateInitialIterate(input);
|
||||
double[] warmStart = ToPrimal(iterate);
|
||||
bool hasDynamicsConsistentInitialWarmStart = iterate.SatisfiesExactDiscreteDynamics(1e-12d);
|
||||
LongitudinalCandidate lastStrictCandidate;
|
||||
if (!_solutionValidator.TryValidate(input, speedLimit, iterate, out lastStrictCandidate, out _))
|
||||
lastStrictCandidate = null;
|
||||
string lastCandidateRejection = string.Empty;
|
||||
bool hasPreviousObjective = false;
|
||||
double previousObjective = 0d;
|
||||
var stopwatch = Stopwatch.StartNew();
|
||||
|
||||
for (int iteration = 0; iteration < iterationLimit; iteration++)
|
||||
{
|
||||
if (cancellationToken.IsCancellationRequested)
|
||||
return FallbackOrFailure(lastStrictCandidate, EmPlanningStatus.Cancelled, "Longitudinal optimization was cancelled.");
|
||||
TimeSpan remainingBudget = totalBudget - stopwatch.Elapsed;
|
||||
if (remainingBudget <= TimeSpan.Zero)
|
||||
{
|
||||
return FallbackOrFailure(lastStrictCandidate, EmPlanningStatus.SolverTimedOut,
|
||||
"Longitudinal optimization exhausted its solve budget.");
|
||||
}
|
||||
if (!_constraintBuilder.TryBuild(input, speedLimit, iterate, out QuadraticProgram problem, out string buildFailure))
|
||||
{
|
||||
return FallbackOrFailure(lastStrictCandidate, EmPlanningStatus.LongitudinalInfeasible,
|
||||
"Longitudinal constraints are infeasible: " + buildFailure);
|
||||
}
|
||||
|
||||
QpSolveResult solved = _qpSolver.Solve(problem,
|
||||
new QpSolverSettings(settings.MaximumIterations, settings.AbsoluteTolerance, settings.RelativeTolerance,
|
||||
remainingBudget, settings.EnableWarmStart && (iteration > 0 || hasDynamicsConsistentInitialWarmStart),
|
||||
settings.EnablePolishing,
|
||||
settings.EnableNativeVerboseOutput),
|
||||
warmStart, cancellationToken);
|
||||
if (solved == null)
|
||||
return FallbackOrFailure(lastStrictCandidate, EmPlanningStatus.Failed, "The longitudinal QP solver returned no result.");
|
||||
if (solved.Status == QpSolveStatus.TimeLimit || solved.Status == QpSolveStatus.MaximumIterations)
|
||||
{
|
||||
return FallbackOrFailure(lastStrictCandidate, EmPlanningStatus.SolverTimedOut,
|
||||
"The longitudinal QP solver timed out (status=" + solved.NativeStatus +
|
||||
", iterations=" + solved.Iterations + ", primal=" + solved.PrimalResidual +
|
||||
", dual=" + solved.DualResidual + "): " + solved.Diagnostic);
|
||||
}
|
||||
if (solved.Status == QpSolveStatus.Cancelled)
|
||||
return FallbackOrFailure(lastStrictCandidate, EmPlanningStatus.Cancelled,
|
||||
"The longitudinal QP solver was cancelled: " + solved.Diagnostic);
|
||||
if (solved.Status == QpSolveStatus.PrimalInfeasible || solved.Status == QpSolveStatus.DualInfeasible)
|
||||
{
|
||||
return FallbackOrFailure(lastStrictCandidate, EmPlanningStatus.LongitudinalInfeasible,
|
||||
"The longitudinal QP solver reported infeasibility: " + solved.Diagnostic);
|
||||
}
|
||||
if (solved.Status == QpSolveStatus.SolverUnavailable)
|
||||
return FallbackOrFailure(lastStrictCandidate, EmPlanningStatus.SolverUnavailable,
|
||||
"The longitudinal QP solver is unavailable: " + solved.Diagnostic);
|
||||
if (solved.Status != QpSolveStatus.Solved && solved.Status != QpSolveStatus.SolvedInaccurate)
|
||||
{
|
||||
return FallbackOrFailure(lastStrictCandidate, EmPlanningStatus.Failed,
|
||||
"The longitudinal QP solver failed: " + solved.Diagnostic);
|
||||
}
|
||||
if (solved.Status == QpSolveStatus.SolvedInaccurate && !HasStrictResiduals(solved, convergenceTolerance))
|
||||
{
|
||||
lastCandidateRejection = "SolvedInaccurate residuals exceed the strict acceptance tolerance" +
|
||||
" (primal=" + solved.PrimalResidual + ", dual=" + solved.DualResidual + ").";
|
||||
if (TryCreateCandidate(iterate.KnotTimes, solved.Primal, out LongitudinalCandidate inaccurateCandidate))
|
||||
warmStart = ToPrimal(inaccurateCandidate);
|
||||
continue;
|
||||
}
|
||||
if (!TryCreateCandidate(iterate.KnotTimes, solved.Primal, out LongitudinalCandidate candidate))
|
||||
{
|
||||
lastCandidateRejection = "The solver primal does not match the ST variable layout.";
|
||||
continue;
|
||||
}
|
||||
if (!_solutionValidator.TryValidate(input, speedLimit, candidate, out LongitudinalCandidate validated,
|
||||
out string validationFailure))
|
||||
{
|
||||
string rejection = validationFailure + CreateEnvelopeDiagnostic(speedLimit, iterate, candidate,
|
||||
iteration + 1);
|
||||
lastCandidateRejection = string.IsNullOrEmpty(lastCandidateRejection)
|
||||
? rejection
|
||||
: lastCandidateRejection + " | " + rejection;
|
||||
if (TryCreateEnvelopeIterate(input, iterate, candidate, out LongitudinalCandidate nextIterate))
|
||||
{
|
||||
iterate = nextIterate;
|
||||
warmStart = ToPrimal(candidate);
|
||||
}
|
||||
continue;
|
||||
}
|
||||
|
||||
double maximumChange = MaximumProgressOrSpeedChange(iterate, validated);
|
||||
double relativeObjectiveImprovement = hasPreviousObjective
|
||||
? RelativeObjectiveImprovement(previousObjective, solved.Objective)
|
||||
: double.PositiveInfinity;
|
||||
lastStrictCandidate = CopyCandidate(validated);
|
||||
iterate = validated;
|
||||
warmStart = ToPrimal(validated);
|
||||
previousObjective = solved.Objective;
|
||||
hasPreviousObjective = true;
|
||||
if (maximumChange <= convergenceTolerance && relativeObjectiveImprovement <= convergenceTolerance)
|
||||
return new LongitudinalPlanningResult(EmPlanningStatus.Success, lastStrictCandidate, string.Empty);
|
||||
}
|
||||
|
||||
return lastStrictCandidate == null
|
||||
? Failed(EmPlanningStatus.LongitudinalInfeasible, "No strictly validated longitudinal candidate was found. " +
|
||||
lastCandidateRejection)
|
||||
: new LongitudinalPlanningResult(EmPlanningStatus.Success, lastStrictCandidate, string.Empty);
|
||||
}
|
||||
|
||||
private static bool TryCreateSettings(LongitudinalPlanningInput input, out QpSolverSettings settings,
|
||||
out TimeSpan totalBudget, out double convergenceTolerance, out int iterationLimit, out string failureReason)
|
||||
{
|
||||
settings = null;
|
||||
totalBudget = TimeSpan.Zero;
|
||||
convergenceTolerance = 0d;
|
||||
iterationLimit = 0;
|
||||
failureReason = string.Empty;
|
||||
if (input.Configuration == null || input.Configuration.Solver == null || input.Configuration.Scheduling == null)
|
||||
{
|
||||
failureReason = "Longitudinal solver configuration is required.";
|
||||
return false;
|
||||
}
|
||||
SolverConfiguration solver = input.Configuration.Solver;
|
||||
SchedulingConfiguration scheduling = input.Configuration.Scheduling;
|
||||
if (solver.MaximumOuterIterations <= 0 || solver.MaximumOsqpIterations <= 0 ||
|
||||
!IsPositiveFinite(solver.AbsoluteTolerance) || !IsPositiveFinite(solver.RelativeTolerance) ||
|
||||
!IsPositiveFinite(solver.StrictResidualTolerance) || !IsPositiveFinite(scheduling.SolverTimeoutSeconds))
|
||||
{
|
||||
failureReason = "Longitudinal solver configuration is invalid.";
|
||||
return false;
|
||||
}
|
||||
try
|
||||
{
|
||||
totalBudget = TimeSpan.FromSeconds(scheduling.SolverTimeoutSeconds);
|
||||
settings = new QpSolverSettings(solver.MaximumOsqpIterations, solver.AbsoluteTolerance, solver.RelativeTolerance,
|
||||
totalBudget, solver.WarmStart, solver.Polish, solver.NativeVerbose);
|
||||
convergenceTolerance = solver.StrictResidualTolerance;
|
||||
iterationLimit = Math.Min(MaximumEnvelopeIterations, solver.MaximumOuterIterations);
|
||||
return true;
|
||||
}
|
||||
catch (ArgumentException exception)
|
||||
{
|
||||
failureReason = exception.Message;
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
private static LongitudinalCandidate CreateInitialIterate(LongitudinalPlanningInput input)
|
||||
{
|
||||
IReadOnlyList<double> times = LongitudinalCandidate.CreateKnotTimes(input.Configuration.Scheduling.TimeHorizonSeconds,
|
||||
input.Configuration.Scheduling.OutputTimeStepSeconds);
|
||||
if (TryCreateCruiseThenBrakeSeed(input, times, out LongitudinalCandidate brakingSeed))
|
||||
return brakingSeed;
|
||||
int knotCount = times.Count;
|
||||
var s = new double[knotCount];
|
||||
var u = new double[knotCount];
|
||||
var a = new double[knotCount];
|
||||
var j = new double[knotCount - 1];
|
||||
double horizon = times[knotCount - 1];
|
||||
double requestedSpeed = Math.Min(input.DirectionMaximumSpeedMetersPerSecond,
|
||||
Math.Max(0d, input.TerminalPathS / horizon));
|
||||
LongitudinalStoppingProfile terminalStop = LongitudinalStoppingMath.Calculate(requestedSpeed, 0d,
|
||||
input.Configuration.Longitudinal.MaximumDecelerationMetersPerSecondSquared,
|
||||
input.Configuration.Longitudinal.MaximumJerkMetersPerSecondCubed);
|
||||
double cruiseDistance = Math.Max(0d, input.TerminalPathS - terminalStop.DistanceMeters);
|
||||
for (int index = 0; index < knotCount; index++)
|
||||
{
|
||||
double fraction = (double)index / (knotCount - 1);
|
||||
s[index] = Math.Min(input.TerminalPathS, cruiseDistance * fraction + terminalStop.DistanceMeters * fraction * fraction);
|
||||
u[index] = index == 0 ? input.InitialProgressSpeedMetersPerSecond : requestedSpeed;
|
||||
a[index] = index == 0 ? input.InitialAccelerationMetersPerSecondSquared : 0d;
|
||||
}
|
||||
s[0] = 0d;
|
||||
s[knotCount - 1] = input.TerminalPathS;
|
||||
u[knotCount - 1] = 0d;
|
||||
a[knotCount - 1] = 0d;
|
||||
return new LongitudinalCandidate(times, s, u, a, j);
|
||||
}
|
||||
|
||||
private static bool TryCreateCruiseThenBrakeSeed(LongitudinalPlanningInput input, IReadOnlyList<double> times,
|
||||
out LongitudinalCandidate candidate)
|
||||
{
|
||||
candidate = null;
|
||||
double initialSpeed = input.InitialProgressSpeedMetersPerSecond;
|
||||
double initialAcceleration = input.InitialAccelerationMetersPerSecondSquared;
|
||||
if (initialSpeed <= 0d || Math.Abs(initialAcceleration) > 1e-12d)
|
||||
return false;
|
||||
double timeStep = times[1] - times[0];
|
||||
for (int index = 1; index < times.Count - 1; index++)
|
||||
{
|
||||
if (Math.Abs((times[index + 1] - times[index]) - timeStep) > 1e-12d)
|
||||
return false;
|
||||
}
|
||||
LongitudinalConfiguration configuration = input.Configuration.Longitudinal;
|
||||
int intervalCount = times.Count - 1;
|
||||
int maximumRampIntervals = Math.Min(intervalCount / 2, checked((int)Math.Floor(
|
||||
configuration.MaximumDecelerationMetersPerSecondSquared /
|
||||
(configuration.MaximumJerkMetersPerSecondCubed * timeStep))));
|
||||
for (int rampIntervals = maximumRampIntervals; rampIntervals >= 1; rampIntervals--)
|
||||
{
|
||||
for (int plateauIntervals = 0; 2 * rampIntervals + plateauIntervals <= intervalCount; plateauIntervals++)
|
||||
{
|
||||
double jerkMagnitude = initialSpeed / (rampIntervals * (rampIntervals + plateauIntervals) *
|
||||
timeStep * timeStep);
|
||||
double peakDeceleration = jerkMagnitude * rampIntervals * timeStep;
|
||||
if (jerkMagnitude > configuration.MaximumJerkMetersPerSecondCubed + 1e-12d ||
|
||||
peakDeceleration > configuration.MaximumDecelerationMetersPerSecondSquared + 1e-12d)
|
||||
{
|
||||
continue;
|
||||
}
|
||||
int brakingIntervals = 2 * rampIntervals + plateauIntervals;
|
||||
double brakingDistance = 0.5d * initialSpeed * brakingIntervals * timeStep;
|
||||
if (brakingDistance > input.TerminalPathS + 1e-12d)
|
||||
continue;
|
||||
int maximumCruiseIntervals = intervalCount - brakingIntervals;
|
||||
int cruiseIntervals = Math.Min(maximumCruiseIntervals, Math.Max(0, checked((int)Math.Floor(
|
||||
(input.TerminalPathS - brakingDistance) / (initialSpeed * timeStep) + 1e-12d))));
|
||||
var jerk = new double[intervalCount];
|
||||
int cursor = cruiseIntervals;
|
||||
for (int index = 0; index < rampIntervals; index++)
|
||||
jerk[cursor++] = -jerkMagnitude;
|
||||
cursor += plateauIntervals;
|
||||
for (int index = 0; index < rampIntervals; index++)
|
||||
jerk[cursor++] = jerkMagnitude;
|
||||
LongitudinalCandidate integrated = LongitudinalCandidate.Integrate(times, 0d, initialSpeed, 0d, jerk);
|
||||
if (integrated.S[integrated.S.Count - 1] <= input.TerminalPathS + 1e-12d)
|
||||
{
|
||||
candidate = integrated;
|
||||
return true;
|
||||
}
|
||||
}
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
private static bool TryCreateCandidate(IReadOnlyList<double> times, IReadOnlyList<double> primal,
|
||||
out LongitudinalCandidate candidate)
|
||||
{
|
||||
candidate = null;
|
||||
if (primal == null)
|
||||
return false;
|
||||
try
|
||||
{
|
||||
var layout = new LongitudinalVariableLayout(times.Count);
|
||||
if (primal.Count != layout.VariableCount)
|
||||
return false;
|
||||
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 < layout.KnotCount - 1; index++)
|
||||
j[index] = primal[layout.J(index)];
|
||||
candidate = new LongitudinalCandidate(times, s, u, a, j);
|
||||
return true;
|
||||
}
|
||||
catch (ArgumentException)
|
||||
{
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
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 bool TryCreateEnvelopeIterate(LongitudinalPlanningInput input, LongitudinalCandidate previous,
|
||||
LongitudinalCandidate candidate, out LongitudinalCandidate nextIterate)
|
||||
{
|
||||
nextIterate = null;
|
||||
if (candidate.S.Count != previous.S.Count)
|
||||
return false;
|
||||
var candidateProgressSamples = new double[candidate.S.Count];
|
||||
double priorProgress = double.NegativeInfinity;
|
||||
double priorPreviousProgress = double.NegativeInfinity;
|
||||
for (int index = 0; index < candidate.S.Count; index++)
|
||||
{
|
||||
double candidateProgress = candidate.S[index];
|
||||
double previousProgress = previous.S[index];
|
||||
if (!IsFinite(previousProgress) || previousProgress < 0d || previousProgress > input.TerminalPathS ||
|
||||
previousProgress < priorPreviousProgress)
|
||||
{
|
||||
return false;
|
||||
}
|
||||
if (!IsFinite(candidateProgress))
|
||||
{
|
||||
candidateProgress = previousProgress;
|
||||
}
|
||||
candidateProgress = Math.Max(0d, Math.Min(input.TerminalPathS, candidateProgress));
|
||||
if (index == candidate.S.Count - 1)
|
||||
candidateProgress = input.TerminalPathS;
|
||||
candidateProgress = Math.Max(priorProgress, candidateProgress);
|
||||
candidateProgressSamples[index] = candidateProgress;
|
||||
priorProgress = candidateProgress;
|
||||
priorPreviousProgress = previousProgress;
|
||||
}
|
||||
var progress = new double[candidate.S.Count];
|
||||
double previousNextProgress = 0d;
|
||||
for (int index = 0; index < progress.Length; index++)
|
||||
{
|
||||
double candidateProgress = candidateProgressSamples[index];
|
||||
if (index == 0 || index == progress.Length - 1 || candidateProgress >= input.TerminalPathS)
|
||||
{
|
||||
progress[index] = candidateProgress;
|
||||
}
|
||||
else
|
||||
{
|
||||
double timeStep = candidate.KnotTimes[index + 1] - candidate.KnotTimes[index];
|
||||
double iterationAdvance = Math.Max(0d, candidateProgress - previous.S[index]);
|
||||
double candidateSpeed = IsFinite(candidate.U[index]) ? Math.Max(0d, candidate.U[index]) : 0d;
|
||||
double lookaheadAdvance = IsFinite(candidate.U[index])
|
||||
? OrdinaryEnvelopeProbeLookaheadSteps * candidateSpeed * timeStep
|
||||
: 0d;
|
||||
double terminalLimitedAdvance = OrdinaryTerminalProbeFraction *
|
||||
(input.TerminalPathS - candidateProgress);
|
||||
double advance = Math.Min(Math.Max(iterationAdvance, lookaheadAdvance), terminalLimitedAdvance);
|
||||
progress[index] = candidateProgress + advance;
|
||||
}
|
||||
progress[index] = Math.Max(previousNextProgress, progress[index]);
|
||||
previousNextProgress = progress[index];
|
||||
}
|
||||
nextIterate = new LongitudinalCandidate(candidate.KnotTimes, progress, previous.U, previous.A, previous.J);
|
||||
return true;
|
||||
}
|
||||
|
||||
private static bool HasStrictResiduals(QpSolveResult result, double tolerance)
|
||||
{
|
||||
return IsPositiveFinite(tolerance) && result.PrimalResidual >= 0d && result.DualResidual >= 0d &&
|
||||
result.PrimalResidual <= tolerance && result.DualResidual <= tolerance;
|
||||
}
|
||||
|
||||
private static string CreateEnvelopeDiagnostic(PathSpeedLimit speedLimit, LongitudinalCandidate iterate,
|
||||
LongitudinalCandidate candidate, int iteration)
|
||||
{
|
||||
int worstIndex = -1;
|
||||
double worstExcess = double.NegativeInfinity;
|
||||
for (int index = 0; index < candidate.S.Count; index++)
|
||||
{
|
||||
if (!IsFinite(candidate.S[index]) || !IsFinite(candidate.U[index]))
|
||||
continue;
|
||||
double candidateProgress = Math.Max(0d, Math.Min(speedLimit.TerminalPathS, candidate.S[index]));
|
||||
double limit = speedLimit.MaximumSpeedAt(candidateProgress);
|
||||
double excess = candidate.U[index] - limit;
|
||||
if (excess > worstExcess)
|
||||
{
|
||||
worstExcess = excess;
|
||||
worstIndex = index;
|
||||
}
|
||||
}
|
||||
if (worstIndex < 0)
|
||||
return "";
|
||||
return " Envelope iteration " + iteration + " used PathS=" + iterate.S[worstIndex] +
|
||||
" and produced PathS=" + candidate.S[worstIndex] + " at its largest speed-envelope excess.";
|
||||
}
|
||||
|
||||
private static double MaximumProgressOrSpeedChange(LongitudinalCandidate previous, LongitudinalCandidate current)
|
||||
{
|
||||
double maximum = 0d;
|
||||
for (int index = 0; index < previous.S.Count; index++)
|
||||
{
|
||||
maximum = Math.Max(maximum, Math.Abs(current.S[index] - previous.S[index]));
|
||||
maximum = Math.Max(maximum, Math.Abs(current.U[index] - previous.U[index]));
|
||||
}
|
||||
return maximum;
|
||||
}
|
||||
|
||||
private static double RelativeObjectiveImprovement(double previous, double current)
|
||||
{
|
||||
return Math.Abs(previous - current) / Math.Max(1d, Math.Abs(previous));
|
||||
}
|
||||
|
||||
private static LongitudinalPlanningResult FallbackOrFailure(LongitudinalCandidate candidate,
|
||||
EmPlanningStatus failureStatus, string failureReason)
|
||||
{
|
||||
return candidate == null
|
||||
? Failed(failureStatus, failureReason)
|
||||
: new LongitudinalPlanningResult(EmPlanningStatus.SuccessWithFallback, candidate, failureReason);
|
||||
}
|
||||
|
||||
private static LongitudinalPlanningResult Failed(EmPlanningStatus status, string reason)
|
||||
{
|
||||
return new LongitudinalPlanningResult(status, null, reason);
|
||||
}
|
||||
|
||||
private static LongitudinalCandidate CopyCandidate(LongitudinalCandidate source)
|
||||
{
|
||||
return new LongitudinalCandidate(source.KnotTimes, source.S, source.U, source.A, source.J);
|
||||
}
|
||||
|
||||
private static bool IsPositiveFinite(double value)
|
||||
{
|
||||
return IsFinite(value) && value > 0d;
|
||||
}
|
||||
|
||||
private static bool IsFinite(double value)
|
||||
{
|
||||
return !double.IsNaN(value) && !double.IsInfinity(value);
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,484 @@
|
||||
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; }
|
||||
}
|
||||
}
|
||||
@@ -10,9 +10,10 @@ internal static class Program
|
||||
args[0] != "corridor" && args[0] != "optimization" && args[0] != "osqp" && args[0] != "osqp-loader" && args[0] != "osqp-probe" &&
|
||||
args[0] != "all-foundation" && args[0] != "lateral-model" && args[0] != "lateral-integration" &&
|
||||
args[0] != "lateral-real-osqp" && args[0] != "lateral-real-osqp-probe" && args[0] != "lateral-all" &&
|
||||
args[0] != "longitudinal-model"))
|
||||
args[0] != "longitudinal-model" && args[0] != "longitudinal-integration" &&
|
||||
args[0] != "longitudinal-real-osqp-probe"))
|
||||
{
|
||||
Console.Error.WriteLine("Usage: EMPlannerVerificationHost foundation|segmentation|frenet|corridor|optimization|osqp|osqp-loader|all-foundation|lateral-model|lateral-integration|lateral-real-osqp|lateral-all");
|
||||
Console.Error.WriteLine("Usage: EMPlannerVerificationHost foundation|segmentation|frenet|corridor|optimization|osqp|osqp-loader|all-foundation|lateral-model|lateral-integration|lateral-real-osqp|lateral-all|longitudinal-model|longitudinal-integration");
|
||||
return 2;
|
||||
}
|
||||
|
||||
@@ -77,6 +78,16 @@ internal static class Program
|
||||
MultiWheelC.TrajectoryPlanning.EMPlanner.LongitudinalModelChecks.Run();
|
||||
Console.WriteLine("PASS longitudinal-model");
|
||||
}
|
||||
if (args[0] == "longitudinal-integration")
|
||||
{
|
||||
MultiWheelC.TrajectoryPlanning.EMPlanner.LongitudinalIntegrationChecks.Run();
|
||||
Console.WriteLine("PASS longitudinal-integration");
|
||||
}
|
||||
if (args[0] == "longitudinal-real-osqp-probe")
|
||||
{
|
||||
MultiWheelC.TrajectoryPlanning.EMPlanner.LongitudinalIntegrationChecks.RunRealOsqp();
|
||||
Console.WriteLine("PASS longitudinal-real-osqp");
|
||||
}
|
||||
return 0;
|
||||
}
|
||||
catch (Exception exception)
|
||||
|
||||
Reference in New Issue
Block a user