1514 lines
74 KiB
C#
1514 lines
74 KiB
C#
using System;
|
|
using System.Collections.Generic;
|
|
using System.Diagnostics;
|
|
using System.Globalization;
|
|
using System.Threading;
|
|
using MultiWheelC.TrajectoryPlanning.CoarsePath;
|
|
|
|
namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
|
|
|
|
/// <summary>Bounded ST envelope iteration retaining only independently validated physical candidates.</summary>
|
|
public sealed class SequentialLongitudinalOptimizer
|
|
{
|
|
private const int MaximumAcceptedAnchorUpdates = 5;
|
|
private const int MaximumQpSolveCalls = 12;
|
|
private const double MinimumTrustRegionWidthMeters = 0.001d;
|
|
private const double ObjectiveAcceptanceRelativeTolerance = 1e-9d;
|
|
private const double HighPrecisionRetryTolerance = 1e-7d;
|
|
private const double StaticStartSeedBudgetFraction = 0.10d;
|
|
private static readonly TimeSpan MaximumStaticStartSeedBudget = TimeSpan.FromMilliseconds(250d);
|
|
private static readonly TimeSpan PublicationReserve = TimeSpan.FromMilliseconds(250d);
|
|
private static readonly double[] TrustRegionScales = { 1d, 0.5d, 0.25d, 0.125d };
|
|
private readonly IQpSolver _qpSolver;
|
|
private readonly PathSpeedLimitBuilder _speedLimitBuilder;
|
|
private readonly LongitudinalConstraintBuilder _constraintBuilder;
|
|
private readonly LongitudinalSolutionValidator _solutionValidator;
|
|
private readonly LongitudinalEnvelopeTrustRegionBuilder _trustRegionBuilder =
|
|
new LongitudinalEnvelopeTrustRegionBuilder();
|
|
|
|
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 (input.PlanningScope == EmPlanningScope.FullDirectionSegment &&
|
|
input.Mode == EmLongitudinalMode.ExactStopAtBoundary)
|
|
{
|
|
double staticStartSpeedTolerance = Math.Max(input.Configuration.Validation.SpatialToleranceMeters,
|
|
input.Configuration.Longitudinal.StopSpeedToleranceMetersPerSecond);
|
|
if (input.InitialProgressSpeedMetersPerSecond <= staticStartSpeedTolerance &&
|
|
Math.Abs(input.InitialAccelerationMetersPerSecondSquared) <=
|
|
input.Configuration.Validation.KinematicTolerance)
|
|
{
|
|
input = new LongitudinalPlanningInput(input.Path, input.Direction, 0d,
|
|
input.InitialAccelerationMetersPerSecondSquared, input.TerminalType, input.Mode,
|
|
input.Configuration, input.PlanningScope, input.KnotSchedule,
|
|
input.PreviousPathS, input.PreviousProgressSpeedMetersPerSecond);
|
|
}
|
|
}
|
|
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);
|
|
|
|
var stopwatch = Stopwatch.StartNew();
|
|
var solveTrace = new LongitudinalSolveTrace();
|
|
LongitudinalCandidate initialCandidate;
|
|
int projectionSolveCount = 0;
|
|
if (input.PlanningScope == EmPlanningScope.FullDirectionSegment &&
|
|
input.Mode == EmLongitudinalMode.ExactStopAtBoundary)
|
|
{
|
|
if (!TryCreateInitialFeasibleCandidate(input, speedLimit, settings, totalBudget, convergenceTolerance,
|
|
iterationLimit, stopwatch, solveTrace, cancellationToken, out initialCandidate,
|
|
out int usedProjectionSolveCount, out EmPlanningStatus projectionStatus,
|
|
out string projectionFailure))
|
|
{
|
|
string projectionTrace = solveTrace.Format();
|
|
return Failed(projectionStatus, projectionFailure +
|
|
(string.IsNullOrEmpty(projectionTrace) ? string.Empty : ";solveTrace=" + projectionTrace));
|
|
}
|
|
projectionSolveCount = usedProjectionSolveCount;
|
|
}
|
|
else
|
|
{
|
|
LongitudinalCandidate seed = CreateInitialIterate(input, speedLimit);
|
|
if (!_solutionValidator.TryValidate(input, speedLimit, seed, out initialCandidate,
|
|
out EmPlanningStatus initializationStatus, out string initializationFailure))
|
|
{
|
|
return Failed(initializationStatus, "No strictly validated longitudinal candidate was found. " +
|
|
initializationFailure);
|
|
}
|
|
}
|
|
|
|
LongitudinalCandidate anchor = CopyCandidate(initialCandidate);
|
|
int stabilizationStart = GetStabilizationStart(input);
|
|
int qpSolveCount = projectionSolveCount;
|
|
int trustShrinkCount = 0;
|
|
int acceptedAnchorCount = 0;
|
|
int acceptedUpdateLimit = Math.Min(MaximumAcceptedAnchorUpdates, iterationLimit);
|
|
double finalTrustScale = 1d;
|
|
string lastRejection = string.Empty;
|
|
|
|
while (acceptedAnchorCount < acceptedUpdateLimit && qpSolveCount < MaximumQpSolveCalls)
|
|
{
|
|
bool promoted = false;
|
|
for (int scaleIndex = 0; scaleIndex < TrustRegionScales.Length; scaleIndex++)
|
|
{
|
|
double scale = TrustRegionScales[scaleIndex];
|
|
finalTrustScale = scale;
|
|
if (cancellationToken.IsCancellationRequested || totalBudget - stopwatch.Elapsed <= TimeSpan.Zero)
|
|
{
|
|
return FinishFromAnchor(anchor, acceptedAnchorCount, qpSolveCount, trustShrinkCount,
|
|
finalTrustScale, cancellationToken.IsCancellationRequested, lastRejection, solveTrace);
|
|
}
|
|
if (!_trustRegionBuilder.TryBuild(speedLimit, anchor, input.KnotSchedule.ReferencePathS,
|
|
stabilizationStart, scale, MinimumTrustRegionWidthMeters,
|
|
out LongitudinalEnvelopeTrustRegion region, out string regionFailure))
|
|
{
|
|
lastRejection = regionFailure;
|
|
break;
|
|
}
|
|
if (!_constraintBuilder.TryBuildTrusted(input, speedLimit, anchor, region,
|
|
convergenceTolerance, out QuadraticProgram problem, out string buildFailure))
|
|
{
|
|
lastRejection = buildFailure;
|
|
break;
|
|
}
|
|
TrustedSolveAttempt attempt = SolveTrustedProblem(problem, input, speedLimit, anchor, settings,
|
|
totalBudget, stopwatch, convergenceTolerance, stabilizationStart,
|
|
MaximumQpSolveCalls - qpSolveCount, solveTrace, qpSolveCount, acceptedAnchorCount,
|
|
scale, acceptedAnchorCount > 0, cancellationToken);
|
|
qpSolveCount += attempt.SolveCount;
|
|
lastRejection = attempt.FailureReason;
|
|
if (attempt.Status == EmPlanningStatus.Cancelled)
|
|
{
|
|
return Failed(EmPlanningStatus.Cancelled, CreateRunDiagnostic(qpSolveCount,
|
|
trustShrinkCount, acceptedAnchorCount, finalTrustScale, lastRejection, solveTrace));
|
|
}
|
|
if (attempt.Accepted)
|
|
{
|
|
anchor = CopyCandidate(attempt.Candidate);
|
|
acceptedAnchorCount++;
|
|
promoted = true;
|
|
break;
|
|
}
|
|
if (attempt.Status != EmPlanningStatus.SuccessWithFallback)
|
|
{
|
|
return FinishFromAnchor(anchor, acceptedAnchorCount, qpSolveCount, trustShrinkCount,
|
|
finalTrustScale, false, lastRejection, solveTrace);
|
|
}
|
|
if (scaleIndex + 1 < TrustRegionScales.Length)
|
|
{
|
|
double nextScale = TrustRegionScales[scaleIndex + 1];
|
|
if (!region.CanShrinkTo(nextScale, MinimumTrustRegionWidthMeters, out string shrinkFailure))
|
|
{
|
|
lastRejection = shrinkFailure;
|
|
break;
|
|
}
|
|
trustShrinkCount++;
|
|
}
|
|
}
|
|
if (!promoted)
|
|
break;
|
|
}
|
|
|
|
return FinishFromAnchor(anchor, acceptedAnchorCount, qpSolveCount, trustShrinkCount,
|
|
finalTrustScale, cancellationToken.IsCancellationRequested, lastRejection, solveTrace);
|
|
}
|
|
|
|
private TrustedSolveAttempt SolveTrustedProblem(QuadraticProgram problem,
|
|
LongitudinalPlanningInput input, PathSpeedLimit speedLimit, LongitudinalCandidate anchor,
|
|
QpSolverSettings settings, TimeSpan totalBudget, Stopwatch stopwatch, double strictTolerance,
|
|
int stabilizationStart, int remainingCallCount, LongitudinalSolveTrace solveTrace,
|
|
int solveOrdinalOffset, int anchorUpdateIndex, double trustScale,
|
|
bool optionalImprovement, CancellationToken cancellationToken)
|
|
{
|
|
int solveCount = 0;
|
|
double anchorObjective = EvaluateObjective(problem, ToPrimal(anchor));
|
|
if (!IsFinite(anchorObjective))
|
|
{
|
|
return new TrustedSolveAttempt(EmPlanningStatus.LongitudinalInfeasible, null, solveCount, false,
|
|
"The strict anchor objective is non-finite for the trusted QP.");
|
|
}
|
|
|
|
IReadOnlyList<double> warmStart = ToPrimal(anchor);
|
|
string lastFailure = string.Empty;
|
|
for (int attemptIndex = 0; attemptIndex < 2; attemptIndex++)
|
|
{
|
|
if (solveCount >= remainingCallCount)
|
|
{
|
|
return new TrustedSolveAttempt(EmPlanningStatus.SolverTimedOut, null, solveCount, false,
|
|
string.IsNullOrWhiteSpace(lastFailure) ? "The longitudinal QP solve-call cap was reached." : lastFailure);
|
|
}
|
|
if (cancellationToken.IsCancellationRequested)
|
|
{
|
|
return new TrustedSolveAttempt(EmPlanningStatus.Cancelled, null, solveCount, false,
|
|
"Longitudinal optimization was cancelled before the trusted QP solve.");
|
|
}
|
|
TimeSpan remainingBudget = totalBudget - stopwatch.Elapsed;
|
|
if (remainingBudget <= TimeSpan.Zero)
|
|
{
|
|
return new TrustedSolveAttempt(EmPlanningStatus.SolverTimedOut, null, solveCount, false,
|
|
"Longitudinal optimization exhausted its shared solve budget.");
|
|
}
|
|
bool hasOptionalSolveBudget = TryGetOptionalSolveBudget(remainingBudget,
|
|
out TimeSpan remainingAfterReserve);
|
|
if (optionalImprovement && !hasOptionalSolveBudget)
|
|
{
|
|
return new TrustedSolveAttempt(EmPlanningStatus.SolverTimedOut, null, solveCount, false,
|
|
CreatePublicationReserveSkipDiagnostic(remainingAfterReserve));
|
|
}
|
|
TimeSpan solveBudget = optionalImprovement ? remainingAfterReserve : remainingBudget;
|
|
|
|
bool highPrecision = attemptIndex == 1;
|
|
double absoluteTolerance = highPrecision
|
|
? Math.Min(settings.AbsoluteTolerance, HighPrecisionRetryTolerance)
|
|
: settings.AbsoluteTolerance;
|
|
double relativeTolerance = highPrecision
|
|
? Math.Min(settings.AbsoluteTolerance, HighPrecisionRetryTolerance)
|
|
: settings.RelativeTolerance;
|
|
var solveStopwatch = Stopwatch.StartNew();
|
|
QpSolveResult solved = _qpSolver.Solve(problem,
|
|
new QpSolverSettings(settings.MaximumIterations, absoluteTolerance, relativeTolerance,
|
|
solveBudget, settings.EnableWarmStart, settings.EnablePolishing,
|
|
settings.EnableNativeVerboseOutput), warmStart, cancellationToken);
|
|
solveStopwatch.Stop();
|
|
solveCount++;
|
|
double candidateObjective = double.NaN;
|
|
void AddSolveTrace(string rejection)
|
|
{
|
|
solveTrace.Add(attemptIndex == 0 ? "normal" : "retry",
|
|
solveOrdinalOffset + solveCount, anchorUpdateIndex, trustScale,
|
|
solveBudget, remainingAfterReserve, solveStopwatch.Elapsed, solved, anchorObjective,
|
|
candidateObjective, rejection);
|
|
}
|
|
|
|
if (cancellationToken.IsCancellationRequested)
|
|
{
|
|
const string rejection = "Longitudinal optimization was cancelled after the trusted QP solve.";
|
|
AddSolveTrace(rejection);
|
|
return new TrustedSolveAttempt(EmPlanningStatus.Cancelled, null, solveCount, false,
|
|
rejection);
|
|
}
|
|
if (solved == null)
|
|
{
|
|
const string rejection = "The longitudinal QP solver returned no result.";
|
|
AddSolveTrace(rejection);
|
|
return new TrustedSolveAttempt(EmPlanningStatus.Failed, null, solveCount, false,
|
|
rejection);
|
|
}
|
|
if (solved.Status == QpSolveStatus.Cancelled)
|
|
{
|
|
string rejection = "The longitudinal QP solver was cancelled: " + solved.Diagnostic;
|
|
AddSolveTrace(rejection);
|
|
return new TrustedSolveAttempt(EmPlanningStatus.Cancelled, null, solveCount, false,
|
|
rejection);
|
|
}
|
|
if (solved.Status == QpSolveStatus.TimeLimit || solved.Status == QpSolveStatus.MaximumIterations)
|
|
{
|
|
string rejection = "The longitudinal QP solver timed out (status=" + solved.NativeStatus +
|
|
", iterations=" + solved.Iterations + ", primal=" + solved.PrimalResidual +
|
|
", dual=" + solved.DualResidual + "): " + solved.Diagnostic;
|
|
AddSolveTrace(rejection);
|
|
return new TrustedSolveAttempt(EmPlanningStatus.SolverTimedOut, null, solveCount, false,
|
|
rejection);
|
|
}
|
|
if (solved.Status == QpSolveStatus.PrimalInfeasible || solved.Status == QpSolveStatus.DualInfeasible)
|
|
{
|
|
string rejection = "The preflight-feasible longitudinal QP solver reported infeasibility;" +
|
|
"solverNumericalAnomaly=true;status=" + solved.NativeStatus + ": " + solved.Diagnostic;
|
|
AddSolveTrace(rejection);
|
|
return new TrustedSolveAttempt(EmPlanningStatus.LongitudinalInfeasible, null, solveCount, false,
|
|
rejection);
|
|
}
|
|
if (solved.Status == QpSolveStatus.SolverUnavailable)
|
|
{
|
|
string rejection = "The longitudinal QP solver is unavailable: " + solved.Diagnostic;
|
|
AddSolveTrace(rejection);
|
|
return new TrustedSolveAttempt(EmPlanningStatus.SolverUnavailable, null, solveCount, false,
|
|
rejection);
|
|
}
|
|
if (solved.Status != QpSolveStatus.Solved && solved.Status != QpSolveStatus.SolvedInaccurate)
|
|
{
|
|
string rejection = "The longitudinal QP solver failed (status=" + solved.NativeStatus + "): " +
|
|
solved.Diagnostic;
|
|
AddSolveTrace(rejection);
|
|
return new TrustedSolveAttempt(EmPlanningStatus.Failed, null, solveCount, false,
|
|
rejection);
|
|
}
|
|
|
|
if (!TryCreateCandidate(anchor.KnotTimes, solved.Primal, out LongitudinalCandidate candidate))
|
|
{
|
|
const string rejection =
|
|
"The solver primal does not match the ST variable layout or contains non-finite values.";
|
|
AddSolveTrace(rejection);
|
|
return new TrustedSolveAttempt(EmPlanningStatus.SuccessWithFallback, null, solveCount, false,
|
|
rejection);
|
|
}
|
|
|
|
bool accepted = true;
|
|
if (solved.Status == QpSolveStatus.SolvedInaccurate && !HasStrictResiduals(solved, strictTolerance))
|
|
{
|
|
accepted = false;
|
|
lastFailure = "SolvedInaccurate residuals exceed the strict acceptance tolerance" +
|
|
" (primal=" + solved.PrimalResidual + ", dual=" + solved.DualResidual + ").";
|
|
}
|
|
if (accepted && !TryFastValidateCandidate(problem, candidate, strictTolerance,
|
|
stabilizationStart, out lastFailure))
|
|
{
|
|
accepted = false;
|
|
}
|
|
LongitudinalCandidate validated = null;
|
|
if (accepted && !_solutionValidator.TryValidate(input, speedLimit, candidate, out validated,
|
|
out string validationFailure))
|
|
{
|
|
accepted = false;
|
|
lastFailure = validationFailure;
|
|
}
|
|
if (accepted)
|
|
{
|
|
candidateObjective = EvaluateObjective(problem, ToPrimal(validated));
|
|
if (!IsObjectiveAccepted(anchorObjective, candidateObjective))
|
|
{
|
|
accepted = false;
|
|
lastFailure = "The strictly valid candidate worsens the current trusted-QP objective" +
|
|
" (anchor=" + anchorObjective.ToString("R", CultureInfo.InvariantCulture) +
|
|
", candidate=" + candidateObjective.ToString("R", CultureInfo.InvariantCulture) + ").";
|
|
}
|
|
}
|
|
if (accepted && cancellationToken.IsCancellationRequested)
|
|
{
|
|
const string rejection = "Longitudinal optimization was cancelled before strict-anchor promotion.";
|
|
AddSolveTrace(rejection);
|
|
return new TrustedSolveAttempt(EmPlanningStatus.Cancelled, null, solveCount, false,
|
|
rejection);
|
|
}
|
|
if (accepted && totalBudget - stopwatch.Elapsed <= TimeSpan.Zero)
|
|
{
|
|
const string rejection =
|
|
"Longitudinal optimization exhausted its shared solve budget before strict-anchor promotion.";
|
|
AddSolveTrace(rejection);
|
|
return new TrustedSolveAttempt(EmPlanningStatus.SolverTimedOut, null, solveCount, false,
|
|
rejection);
|
|
}
|
|
if (accepted)
|
|
{
|
|
AddSolveTrace(string.Empty);
|
|
return new TrustedSolveAttempt(EmPlanningStatus.Success, validated, solveCount, true, string.Empty);
|
|
}
|
|
if (highPrecision)
|
|
{
|
|
AddSolveTrace(lastFailure);
|
|
return new TrustedSolveAttempt(EmPlanningStatus.SuccessWithFallback, null, solveCount, false,
|
|
lastFailure);
|
|
}
|
|
|
|
AddSolveTrace(lastFailure);
|
|
warmStart = ToPrimal(candidate);
|
|
}
|
|
|
|
return new TrustedSolveAttempt(EmPlanningStatus.SuccessWithFallback, null, solveCount, false,
|
|
lastFailure);
|
|
}
|
|
|
|
private static double EvaluateObjective(QuadraticProgram problem, IReadOnlyList<double> primal)
|
|
{
|
|
if (problem == null || primal == null || primal.Count != problem.VariableCount)
|
|
return double.NaN;
|
|
double objective = 0d;
|
|
SparseCscMatrix hessian = problem.UpperTriangularP;
|
|
for (int column = 0; column < hessian.ColumnCount; column++)
|
|
{
|
|
double columnValue = primal[column];
|
|
if (!IsFinite(columnValue))
|
|
return double.NaN;
|
|
for (int entry = hessian.ColumnPointers[column]; entry < hessian.ColumnPointers[column + 1]; entry++)
|
|
{
|
|
int row = hessian.RowIndices[entry];
|
|
double term = hessian.Values[entry] * primal[row] * columnValue;
|
|
objective += row == column ? 0.5d * term : term;
|
|
if (!IsFinite(objective))
|
|
return double.NaN;
|
|
}
|
|
objective += problem.LinearCost[column] * columnValue;
|
|
if (!IsFinite(objective))
|
|
return double.NaN;
|
|
}
|
|
return objective;
|
|
}
|
|
|
|
private static bool IsObjectiveAccepted(double anchorObjective, double candidateObjective)
|
|
{
|
|
if (!IsFinite(anchorObjective) || !IsFinite(candidateObjective))
|
|
return false;
|
|
double tolerance = ObjectiveAcceptanceRelativeTolerance * Math.Max(1d, Math.Abs(anchorObjective));
|
|
return candidateObjective <= anchorObjective + tolerance;
|
|
}
|
|
|
|
private static int GetStabilizationStart(LongitudinalPlanningInput input)
|
|
{
|
|
if (input.Mode != EmLongitudinalMode.ExactStopAtBoundary)
|
|
return input.KnotSchedule.KnotTimes.Count;
|
|
if (input.PlanningScope == EmPlanningScope.FullDirectionSegment)
|
|
return input.KnotSchedule.TerminalHoldStartIndex;
|
|
return LongitudinalTerminalSchedule.GetStabilizationStartIndex(input.KnotSchedule.KnotTimes,
|
|
input.Configuration.Scheduling.OutputTimeStepSeconds);
|
|
}
|
|
|
|
private static bool TryFastValidateCandidate(QuadraticProgram problem, LongitudinalCandidate candidate,
|
|
double strictTolerance, int stabilizationStart, out string failureReason)
|
|
{
|
|
failureReason = string.Empty;
|
|
var layout = new LongitudinalVariableLayout(candidate.KnotTimes.Count);
|
|
LongitudinalQpAuditResult audit;
|
|
try
|
|
{
|
|
audit = LongitudinalQpFeasibilityAudit.Evaluate(problem, candidate, strictTolerance,
|
|
layout, stabilizationStart);
|
|
}
|
|
catch (ArgumentException exception)
|
|
{
|
|
failureReason = "The candidate cannot be audited against the current trusted QP: " + exception.Message;
|
|
return false;
|
|
}
|
|
if (audit.IsFeasible)
|
|
return true;
|
|
failureReason = "The candidate violates the current trusted QP" +
|
|
";row=" + audit.WorstRow + ";category=" + audit.Category +
|
|
";residual=" + audit.MaximumResidual.ToString("R", CultureInfo.InvariantCulture) + audit.Unit +
|
|
";tolerance=" + strictTolerance.ToString("R", CultureInfo.InvariantCulture);
|
|
return false;
|
|
}
|
|
|
|
private static string CreateRunDiagnostic(int qpSolveCount, int trustShrinkCount,
|
|
int acceptedAnchorCount, double finalTrustScale, string lastRejection,
|
|
LongitudinalSolveTrace solveTrace)
|
|
{
|
|
return "qpSolves=" + qpSolveCount +
|
|
",trustShrinks=" + trustShrinkCount +
|
|
",acceptedAnchors=" + acceptedAnchorCount +
|
|
",trustScale=" + finalTrustScale.ToString("R", CultureInfo.InvariantCulture) +
|
|
(string.IsNullOrWhiteSpace(lastRejection) ? string.Empty : ";lastRejection=" + lastRejection) +
|
|
(string.IsNullOrEmpty(solveTrace.Format()) ? string.Empty : ";solveTrace=" + solveTrace.Format());
|
|
}
|
|
|
|
internal static bool TryGetOptionalSolveBudget(TimeSpan remaining, out TimeSpan solveBudget)
|
|
{
|
|
solveBudget = remaining - PublicationReserve;
|
|
if (solveBudget <= TimeSpan.Zero)
|
|
{
|
|
solveBudget = TimeSpan.Zero;
|
|
return false;
|
|
}
|
|
return true;
|
|
}
|
|
|
|
private static string CreatePublicationReserveSkipDiagnostic(TimeSpan remainingAfterReserve)
|
|
{
|
|
return "remainingAfterReserveMs=" + remainingAfterReserve.TotalMilliseconds.ToString(
|
|
"F3", CultureInfo.InvariantCulture) +
|
|
";publicationReserveMs=" + PublicationReserve.TotalMilliseconds.ToString(
|
|
"F0", CultureInfo.InvariantCulture) +
|
|
";optionalImprovement=skipped";
|
|
}
|
|
|
|
private static LongitudinalPlanningResult FinishFromAnchor(LongitudinalCandidate anchor,
|
|
int acceptedAnchorCount, int qpSolveCount, int trustShrinkCount, double finalTrustScale,
|
|
bool cancelled, string lastRejection, LongitudinalSolveTrace solveTrace)
|
|
{
|
|
string diagnostic = CreateRunDiagnostic(qpSolveCount, trustShrinkCount, acceptedAnchorCount,
|
|
finalTrustScale, lastRejection, solveTrace);
|
|
if (cancelled)
|
|
return Failed(EmPlanningStatus.Cancelled, diagnostic);
|
|
EmPlanningStatus status = acceptedAnchorCount > 0
|
|
? EmPlanningStatus.Success
|
|
: EmPlanningStatus.SuccessWithFallback;
|
|
return new LongitudinalPlanningResult(status, CopyCandidate(anchor), diagnostic);
|
|
}
|
|
|
|
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(MaximumAcceptedAnchorUpdates, solver.MaximumOuterIterations);
|
|
return true;
|
|
}
|
|
catch (ArgumentException exception)
|
|
{
|
|
failureReason = exception.Message;
|
|
return false;
|
|
}
|
|
}
|
|
|
|
private bool TryCreateInitialFeasibleCandidate(LongitudinalPlanningInput input, PathSpeedLimit speedLimit,
|
|
QpSolverSettings settings, TimeSpan totalBudget, double convergenceTolerance, int iterationLimit,
|
|
Stopwatch stopwatch, LongitudinalSolveTrace solveTrace, CancellationToken cancellationToken,
|
|
out LongitudinalCandidate candidate, out int projectionSolveCount,
|
|
out EmPlanningStatus failureStatus, out string failureReason)
|
|
{
|
|
candidate = null;
|
|
projectionSolveCount = 0;
|
|
failureStatus = EmPlanningStatus.LongitudinalInfeasible;
|
|
failureReason = string.Empty;
|
|
double staticStartSpeedTolerance = Math.Max(input.Configuration.Validation.SpatialToleranceMeters,
|
|
input.Configuration.Longitudinal.StopSpeedToleranceMetersPerSecond);
|
|
bool staticStartEligible = input.InitialProgressSpeedMetersPerSecond <= staticStartSpeedTolerance &&
|
|
Math.Abs(input.InitialAccelerationMetersPerSecondSquared) <=
|
|
input.Configuration.Validation.KinematicTolerance;
|
|
bool staticStartSeedUsed = false;
|
|
string staticStartSeedFailure = string.Empty;
|
|
TimeSpan staticStartSeedDeadline = stopwatch.Elapsed + GetStaticStartSeedBudget(totalBudget);
|
|
if (staticStartEligible && TryCreateStaticStartSeed(input, speedLimit, stopwatch, staticStartSeedDeadline,
|
|
out LongitudinalCandidate staticStartSeed, out staticStartSeedFailure))
|
|
{
|
|
staticStartSeedUsed = true;
|
|
candidate = staticStartSeed;
|
|
return true;
|
|
}
|
|
string WithStaticSeedDiagnostic(string reason)
|
|
{
|
|
return staticStartEligible && !staticStartSeedUsed
|
|
? "staticStartSeed=failed (" + staticStartSeedFailure + "); " + reason
|
|
: reason;
|
|
}
|
|
|
|
LongitudinalCandidate linearizationIterate = CreateScheduleReferenceIterate(input);
|
|
string lastRejection = string.Empty;
|
|
for (int iteration = 0; iteration < iterationLimit; iteration++)
|
|
{
|
|
if (cancellationToken.IsCancellationRequested)
|
|
{
|
|
failureStatus = EmPlanningStatus.Cancelled;
|
|
failureReason = WithStaticSeedDiagnostic("Initial full-direction feasibility projection was cancelled.");
|
|
return false;
|
|
}
|
|
TimeSpan remainingBudget = totalBudget - stopwatch.Elapsed;
|
|
if (remainingBudget <= TimeSpan.Zero)
|
|
{
|
|
failureStatus = EmPlanningStatus.SolverTimedOut;
|
|
failureReason = WithStaticSeedDiagnostic("Initial full-direction feasibility projection exhausted the shared solve budget.");
|
|
return false;
|
|
}
|
|
if (!_constraintBuilder.TryBuildInitialFeasibilityProjection(input, speedLimit, linearizationIterate,
|
|
out QuadraticProgram problem, out string buildFailure))
|
|
{
|
|
failureStatus = EmPlanningStatus.LongitudinalInfeasible;
|
|
failureReason = WithStaticSeedDiagnostic("Initial full-direction feasibility constraints are infeasible: " + buildFailure);
|
|
return false;
|
|
}
|
|
|
|
double projectionTolerance = Math.Min(settings.AbsoluteTolerance,
|
|
input.Configuration.Validation.KinematicTolerance * 0.1d);
|
|
double anchorObjective = double.NaN;
|
|
var solveStopwatch = Stopwatch.StartNew();
|
|
QpSolveResult solved = _qpSolver.Solve(problem,
|
|
new QpSolverSettings(settings.MaximumIterations, projectionTolerance, projectionTolerance,
|
|
remainingBudget, settings.EnableWarmStart && linearizationIterate.SatisfiesExactDiscreteDynamics(1e-12d),
|
|
settings.EnablePolishing, settings.EnableNativeVerboseOutput),
|
|
ToPrimal(linearizationIterate), cancellationToken);
|
|
solveStopwatch.Stop();
|
|
projectionSolveCount++;
|
|
int projectionCallOrdinal = projectionSolveCount;
|
|
double candidateObjective = double.NaN;
|
|
void AddProjectionTrace(string rejection)
|
|
{
|
|
TryGetOptionalSolveBudget(remainingBudget, out TimeSpan remainingAfterReserve);
|
|
solveTrace.Add("projection", projectionCallOrdinal, 0, 1d, remainingBudget,
|
|
remainingAfterReserve, solveStopwatch.Elapsed, solved, anchorObjective,
|
|
candidateObjective, rejection);
|
|
}
|
|
if (cancellationToken.IsCancellationRequested)
|
|
{
|
|
const string rejection =
|
|
"Initial full-direction feasibility projection was cancelled after the QP solve.";
|
|
AddProjectionTrace(rejection);
|
|
failureStatus = EmPlanningStatus.Cancelled;
|
|
failureReason = WithStaticSeedDiagnostic(rejection);
|
|
return false;
|
|
}
|
|
if (solved == null)
|
|
{
|
|
const string rejection = "The initial full-direction feasibility solver returned no result.";
|
|
AddProjectionTrace(rejection);
|
|
failureStatus = EmPlanningStatus.Failed;
|
|
failureReason = WithStaticSeedDiagnostic(rejection);
|
|
return false;
|
|
}
|
|
if (solved.Status == QpSolveStatus.TimeLimit || solved.Status == QpSolveStatus.MaximumIterations)
|
|
{
|
|
string rejection = "Initial full-direction feasibility projection timed out (status=" + solved.NativeStatus +
|
|
", iterations=" + solved.Iterations + ", primal=" + solved.PrimalResidual + ", dual=" +
|
|
solved.DualResidual + "): " + solved.Diagnostic;
|
|
AddProjectionTrace(rejection);
|
|
failureStatus = EmPlanningStatus.SolverTimedOut;
|
|
failureReason = WithStaticSeedDiagnostic(rejection);
|
|
return false;
|
|
}
|
|
if (solved.Status == QpSolveStatus.Cancelled)
|
|
{
|
|
string rejection =
|
|
"Initial full-direction feasibility projection was cancelled: " + solved.Diagnostic;
|
|
AddProjectionTrace(rejection);
|
|
failureStatus = EmPlanningStatus.Cancelled;
|
|
failureReason = WithStaticSeedDiagnostic(rejection);
|
|
return false;
|
|
}
|
|
if (solved.Status == QpSolveStatus.PrimalInfeasible || solved.Status == QpSolveStatus.DualInfeasible)
|
|
{
|
|
string rejection =
|
|
"Initial full-direction feasibility projection is infeasible: " + solved.Diagnostic;
|
|
AddProjectionTrace(rejection);
|
|
failureStatus = EmPlanningStatus.LongitudinalInfeasible;
|
|
failureReason = WithStaticSeedDiagnostic(rejection);
|
|
return false;
|
|
}
|
|
if (solved.Status == QpSolveStatus.SolverUnavailable)
|
|
{
|
|
string rejection =
|
|
"Initial full-direction feasibility solver is unavailable: " + solved.Diagnostic;
|
|
AddProjectionTrace(rejection);
|
|
failureStatus = EmPlanningStatus.SolverUnavailable;
|
|
failureReason = WithStaticSeedDiagnostic(rejection);
|
|
return false;
|
|
}
|
|
if (solved.Status != QpSolveStatus.Solved && solved.Status != QpSolveStatus.SolvedInaccurate)
|
|
{
|
|
string rejection = "Initial full-direction feasibility solver failed: " + solved.Diagnostic;
|
|
AddProjectionTrace(rejection);
|
|
failureStatus = EmPlanningStatus.Failed;
|
|
failureReason = WithStaticSeedDiagnostic(rejection);
|
|
return false;
|
|
}
|
|
if (!TryCreateCandidate(input.KnotSchedule.KnotTimes, solved.Primal, out LongitudinalCandidate projected))
|
|
{
|
|
const string rejection =
|
|
"Initial full-direction feasibility solver primal does not match the ST layout.";
|
|
AddProjectionTrace(rejection);
|
|
failureStatus = EmPlanningStatus.LongitudinalInfeasible;
|
|
failureReason = WithStaticSeedDiagnostic(rejection);
|
|
return false;
|
|
}
|
|
if (solved.Status == QpSolveStatus.Solved || HasStrictResiduals(solved, convergenceTolerance))
|
|
{
|
|
if (_solutionValidator.TryValidate(input, speedLimit, projected, out LongitudinalCandidate strict,
|
|
out EmPlanningStatus validationStatus, out string validationFailure))
|
|
{
|
|
AddProjectionTrace(string.Empty);
|
|
candidate = strict;
|
|
return true;
|
|
}
|
|
if (validationStatus == EmPlanningStatus.NoProgress)
|
|
{
|
|
AddProjectionTrace(validationFailure);
|
|
failureStatus = validationStatus;
|
|
failureReason = WithStaticSeedDiagnostic(validationFailure);
|
|
return false;
|
|
}
|
|
lastRejection = validationFailure;
|
|
}
|
|
|
|
if (!TryCreateFeasibilityEnvelopeIterate(input, projected,
|
|
out LongitudinalCandidate nextLinearization))
|
|
{
|
|
const string rejection =
|
|
"Initial full-direction feasibility candidate could not be relinearized against the PathS envelope.";
|
|
AddProjectionTrace(rejection);
|
|
failureStatus = EmPlanningStatus.LongitudinalInfeasible;
|
|
failureReason = WithStaticSeedDiagnostic(rejection);
|
|
return false;
|
|
}
|
|
linearizationIterate = nextLinearization;
|
|
if (solved.Status == QpSolveStatus.SolvedInaccurate)
|
|
lastRejection = "Initial feasibility projection residuals exceed the strict acceptance tolerance.";
|
|
else if (string.IsNullOrEmpty(lastRejection))
|
|
lastRejection = "Initial feasibility projection violated the strict physical validator.";
|
|
AddProjectionTrace(lastRejection);
|
|
}
|
|
failureStatus = EmPlanningStatus.LongitudinalInfeasible;
|
|
failureReason = WithStaticSeedDiagnostic("Initial full-direction feasibility projection exhausted the configured outer iterations. " +
|
|
lastRejection);
|
|
return false;
|
|
}
|
|
|
|
private static LongitudinalCandidate CreateScheduleReferenceIterate(LongitudinalPlanningInput input)
|
|
{
|
|
int knotCount = input.KnotSchedule.KnotTimes.Count;
|
|
return new LongitudinalCandidate(input.KnotSchedule.KnotTimes, input.KnotSchedule.ReferencePathS,
|
|
input.KnotSchedule.ReferenceSpeedMetersPerSecond, new double[knotCount], new double[knotCount - 1]);
|
|
}
|
|
|
|
private LongitudinalCandidate CreateInitialIterate(LongitudinalPlanningInput input, PathSpeedLimit speedLimit)
|
|
{
|
|
IReadOnlyList<double> times = input.KnotSchedule.KnotTimes;
|
|
switch (input.Mode)
|
|
{
|
|
case EmLongitudinalMode.RollingContinuation:
|
|
return CreateRollingSeed(input, times, speedLimit);
|
|
case EmLongitudinalMode.ApproachStopBoundary:
|
|
return CreateApproachSeed(input, times, speedLimit);
|
|
case EmLongitudinalMode.ExactStopAtBoundary:
|
|
return CreateExactStopSeed(input, times, speedLimit);
|
|
default:
|
|
throw new ArgumentOutOfRangeException(nameof(input.Mode));
|
|
}
|
|
}
|
|
|
|
private static LongitudinalCandidate CreateRollingSeed(LongitudinalPlanningInput input,
|
|
IReadOnlyList<double> times, PathSpeedLimit speedLimit)
|
|
{
|
|
return CreateEnvelopeSeed(input, times, speedLimit);
|
|
}
|
|
|
|
private static LongitudinalCandidate CreateApproachSeed(LongitudinalPlanningInput input,
|
|
IReadOnlyList<double> times, PathSpeedLimit speedLimit)
|
|
{
|
|
return CreateEnvelopeSeed(input, times, speedLimit);
|
|
}
|
|
|
|
private static LongitudinalCandidate CreateEnvelopeSeed(LongitudinalPlanningInput input,
|
|
IReadOnlyList<double> times, PathSpeedLimit speedLimit)
|
|
{
|
|
LongitudinalConfiguration configuration = input.Configuration.Longitudinal;
|
|
var jerk = new double[times.Count - 1];
|
|
double s = 0d;
|
|
double u = input.InitialProgressSpeedMetersPerSecond;
|
|
double a = input.InitialAccelerationMetersPerSecondSquared;
|
|
for (int index = 0; index < jerk.Length; index++)
|
|
{
|
|
double dt = times[index + 1] - times[index];
|
|
double speedLimitAtS = speedLimit.MaximumSpeedAt(Math.Max(0d, Math.Min(input.PathUpperBoundS, s)));
|
|
double targetSpeed = Math.Min(input.InitialProgressSpeedMetersPerSecond, speedLimitAtS);
|
|
if (input.PlanningScope == EmPlanningScope.FullDirectionSegment)
|
|
{
|
|
double desiredSpeed = input.Direction == TravelDirection.Forward
|
|
? configuration.DesiredForwardSpeedMetersPerSecond
|
|
: configuration.DesiredReverseSpeedMetersPerSecond;
|
|
double scheduleSpeed = input.KnotSchedule.ReferenceSpeedMetersPerSecond[index];
|
|
targetSpeed = Math.Min(desiredSpeed, Math.Min(scheduleSpeed, speedLimitAtS));
|
|
}
|
|
double lowerJerk = Math.Max(-configuration.MaximumJerkMetersPerSecondCubed,
|
|
(-configuration.MaximumDecelerationMetersPerSecondSquared - a) / dt);
|
|
lowerJerk = Math.Max(lowerJerk, -2d * (u + a * dt) / (dt * dt));
|
|
double upperJerk = Math.Min(configuration.MaximumJerkMetersPerSecondCubed,
|
|
(configuration.MaximumAccelerationMetersPerSecondSquared - a) / dt);
|
|
double requestedJerk = 2d * (targetSpeed - u - a * dt) / (dt * dt);
|
|
double selectedJerk = Clamp(requestedJerk, lowerJerk, upperJerk);
|
|
IntegrateStep(s, u, a, selectedJerk, dt, out double nextS, out double nextU, out double nextA);
|
|
if (nextU > speedLimit.MaximumSpeedAt(Math.Max(0d, Math.Min(input.PathUpperBoundS, nextS))) + 1e-12d)
|
|
{
|
|
double lower = lowerJerk;
|
|
double upper = selectedJerk;
|
|
for (int iteration = 0; iteration < 48; iteration++)
|
|
{
|
|
double midpoint = 0.5d * (lower + upper);
|
|
IntegrateStep(s, u, a, midpoint, dt, out double probeS, out double probeU, out _);
|
|
if (probeU <= speedLimit.MaximumSpeedAt(Math.Max(0d, Math.Min(input.PathUpperBoundS, probeS))))
|
|
lower = midpoint;
|
|
else
|
|
upper = midpoint;
|
|
}
|
|
selectedJerk = lower;
|
|
IntegrateStep(s, u, a, selectedJerk, dt, out nextS, out nextU, out nextA);
|
|
}
|
|
jerk[index] = selectedJerk;
|
|
s = nextS;
|
|
u = nextU;
|
|
a = nextA;
|
|
}
|
|
return LongitudinalCandidate.Integrate(times, 0d, input.InitialProgressSpeedMetersPerSecond,
|
|
input.InitialAccelerationMetersPerSecondSquared, jerk);
|
|
}
|
|
|
|
private LongitudinalCandidate CreateExactStopSeed(LongitudinalPlanningInput input,
|
|
IReadOnlyList<double> times, PathSpeedLimit speedLimit)
|
|
{
|
|
if (input.PlanningScope == EmPlanningScope.FullDirectionSegment)
|
|
{
|
|
throw new InvalidOperationException("Full-direction exact-stop planning requires the initial feasibility projection.");
|
|
}
|
|
int stabilizationStart = LongitudinalTerminalSchedule.GetStabilizationStartIndex(times,
|
|
input.Configuration.Scheduling.OutputTimeStepSeconds);
|
|
var motionTimes = new double[stabilizationStart + 1];
|
|
for (int index = 0; index < motionTimes.Length; index++)
|
|
motionTimes[index] = times[index];
|
|
|
|
if (TryCreateCruiseThenBrakeSeed(input, motionTimes, input.StopBoundaryPathS,
|
|
out LongitudinalCandidate cruiseThenBrake))
|
|
{
|
|
LongitudinalCandidate candidate = AppendExactStopTail(times, stabilizationStart,
|
|
input.StopBoundaryPathS, cruiseThenBrake);
|
|
if (_solutionValidator.TryValidate(input, speedLimit, candidate,
|
|
out LongitudinalCandidate validated, out _))
|
|
{
|
|
return validated;
|
|
}
|
|
}
|
|
|
|
if (TryCreateExactJerkSeed(input, times, stabilizationStart, speedLimit,
|
|
out LongitudinalCandidate exactSeed))
|
|
return exactSeed;
|
|
|
|
return CreateApproachSeed(input, times, speedLimit);
|
|
}
|
|
|
|
private bool TryCreateStaticStartSeed(LongitudinalPlanningInput input, PathSpeedLimit speedLimit,
|
|
Stopwatch stopwatch, TimeSpan deadline, out LongitudinalCandidate candidate, out string failureReason)
|
|
{
|
|
candidate = null;
|
|
failureReason = "unknown";
|
|
int stabilizationStart = input.KnotSchedule.TerminalHoldStartIndex;
|
|
if (stabilizationStart < 5)
|
|
{
|
|
failureReason = "terminalHoldStartIndex=" + stabilizationStart.ToString(CultureInfo.InvariantCulture);
|
|
return false;
|
|
}
|
|
|
|
IReadOnlyList<double> times = input.KnotSchedule.KnotTimes;
|
|
if (TryCreateStaticStartScurveSeed(input, times, stabilizationStart, speedLimit, stopwatch, deadline, out candidate))
|
|
return true;
|
|
if (TryCreateExactJerkSeed(input, times, stabilizationStart, speedLimit, stopwatch, deadline, out candidate))
|
|
return true;
|
|
failureReason = "scurveSeed=failed; exactJerkSeed=failed";
|
|
double firstDuration = times[1] - times[0];
|
|
double secondDuration = times[2] - times[1];
|
|
LongitudinalConfiguration configuration = input.Configuration.Longitudinal;
|
|
double maximumFirstJerk = Math.Min(configuration.MaximumJerkMetersPerSecondCubed,
|
|
Math.Min(configuration.MaximumAccelerationMetersPerSecondSquared / firstDuration,
|
|
configuration.MaximumJerkMetersPerSecondCubed * secondDuration / firstDuration));
|
|
for (int sample = -256; sample <= 256; sample++)
|
|
{
|
|
if (HasReachedDeadline(stopwatch, deadline))
|
|
return false;
|
|
if (sample == 0)
|
|
continue;
|
|
double firstJerk = maximumFirstJerk * sample / 256d;
|
|
var jerk = new double[stabilizationStart];
|
|
jerk[0] = firstJerk;
|
|
jerk[1] = -firstJerk * firstDuration / secondDuration;
|
|
if (!TryCloseExactStopEndpoint(input, times, stabilizationStart, jerk,
|
|
out LongitudinalCandidate probe))
|
|
{
|
|
continue;
|
|
}
|
|
if (_solutionValidator.TryValidate(input, speedLimit, probe,
|
|
out LongitudinalCandidate strict, out _))
|
|
{
|
|
candidate = strict;
|
|
failureReason = string.Empty;
|
|
return true;
|
|
}
|
|
}
|
|
failureReason = "scurveSeed=failed; exactJerkSeed=failed; sampledSeeds=failed";
|
|
return false;
|
|
}
|
|
|
|
private bool TryCreateStaticStartScurveSeed(LongitudinalPlanningInput input,
|
|
IReadOnlyList<double> times, int stabilizationStart, PathSpeedLimit speedLimit,
|
|
Stopwatch stopwatch, TimeSpan deadline, out LongitudinalCandidate candidate)
|
|
{
|
|
candidate = null;
|
|
int intervalCount = stabilizationStart;
|
|
int maximumRamp = Math.Max(1, intervalCount / 6);
|
|
var motionTimes = new double[intervalCount + 1];
|
|
for (int index = 0; index <= intervalCount; index++)
|
|
motionTimes[index] = times[index];
|
|
|
|
for (int ramp = 1; ramp <= maximumRamp; ramp++)
|
|
{
|
|
for (int plateau = 0; 4 * ramp + 2 * plateau <= intervalCount; plateau++)
|
|
{
|
|
if (HasReachedDeadline(stopwatch, deadline))
|
|
return false;
|
|
int cruise = intervalCount - 4 * ramp - 2 * plateau;
|
|
var basisAccel = new double[intervalCount];
|
|
var basisBrake = new double[intervalCount];
|
|
var basisOffset = new double[intervalCount];
|
|
int cursor = 0;
|
|
for (int index = 0; index < ramp; index++)
|
|
basisAccel[cursor++] = 1d;
|
|
for (int index = 0; index < plateau; index++)
|
|
cursor++;
|
|
for (int index = 0; index < ramp; index++)
|
|
basisAccel[cursor++] = -1d;
|
|
for (int index = 0; index < cruise; index++)
|
|
cursor++;
|
|
for (int index = 0; index < ramp; index++)
|
|
basisBrake[cursor++] = -1d;
|
|
for (int index = 0; index < plateau; index++)
|
|
cursor++;
|
|
for (int index = 0; index < ramp; index++)
|
|
basisBrake[cursor++] = 1d;
|
|
if (cursor != intervalCount)
|
|
continue;
|
|
for (int index = 0; index < intervalCount; index++)
|
|
basisOffset[index] = 1d;
|
|
|
|
LongitudinalCandidate accelResponse = LongitudinalCandidate.Integrate(
|
|
motionTimes, 0d, 0d, 0d, basisAccel);
|
|
LongitudinalCandidate brakeResponse = LongitudinalCandidate.Integrate(
|
|
motionTimes, 0d, 0d, 0d, basisBrake);
|
|
LongitudinalCandidate offsetResponse = LongitudinalCandidate.Integrate(
|
|
motionTimes, 0d, 0d, 0d, basisOffset);
|
|
var influence = new double[3, 3];
|
|
influence[0, 0] = accelResponse.A[intervalCount];
|
|
influence[1, 0] = accelResponse.U[intervalCount];
|
|
influence[2, 0] = accelResponse.S[intervalCount];
|
|
influence[0, 1] = brakeResponse.A[intervalCount];
|
|
influence[1, 1] = brakeResponse.U[intervalCount];
|
|
influence[2, 1] = brakeResponse.S[intervalCount];
|
|
influence[0, 2] = offsetResponse.A[intervalCount];
|
|
influence[1, 2] = offsetResponse.U[intervalCount];
|
|
influence[2, 2] = offsetResponse.S[intervalCount];
|
|
|
|
double[] target = { 0d, 0d, input.StopBoundaryPathS };
|
|
if (!TrySolveThreeByThree(influence, target, out double[] multipliers))
|
|
continue;
|
|
|
|
var jerk = new double[intervalCount];
|
|
for (int index = 0; index < intervalCount; index++)
|
|
{
|
|
jerk[index] = multipliers[0] * basisAccel[index] +
|
|
multipliers[1] * basisBrake[index] +
|
|
multipliers[2] * basisOffset[index];
|
|
}
|
|
if (TryValidateExactSeed(input, times, stabilizationStart, speedLimit, jerk, out candidate))
|
|
return true;
|
|
}
|
|
}
|
|
return false;
|
|
}
|
|
|
|
private static bool TryCreateCruiseThenBrakeSeed(LongitudinalPlanningInput input, IReadOnlyList<double> times,
|
|
double stopBoundaryPathS, 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 > stopBoundaryPathS + 1e-12d)
|
|
continue;
|
|
int maximumCruiseIntervals = intervalCount - brakingIntervals;
|
|
int cruiseIntervals = Math.Min(maximumCruiseIntervals, Math.Max(0, checked((int)Math.Floor(
|
|
(stopBoundaryPathS - 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);
|
|
int lastIndex = integrated.S.Count - 1;
|
|
if (Math.Abs(integrated.S[lastIndex] - stopBoundaryPathS) <= 1e-10d &&
|
|
Math.Abs(integrated.U[lastIndex]) <= 1e-10d && Math.Abs(integrated.A[lastIndex]) <= 1e-10d)
|
|
{
|
|
candidate = integrated;
|
|
return true;
|
|
}
|
|
}
|
|
}
|
|
return false;
|
|
}
|
|
|
|
private bool TryCreateExactJerkSeed(LongitudinalPlanningInput input, IReadOnlyList<double> times,
|
|
int stabilizationStart, PathSpeedLimit speedLimit, out LongitudinalCandidate candidate)
|
|
{
|
|
return TryCreateExactJerkSeed(input, times, stabilizationStart, speedLimit, Stopwatch.StartNew(),
|
|
TimeSpan.MaxValue, out candidate);
|
|
}
|
|
|
|
private bool TryCreateExactJerkSeed(LongitudinalPlanningInput input, IReadOnlyList<double> times,
|
|
int stabilizationStart, PathSpeedLimit speedLimit, Stopwatch stopwatch, TimeSpan deadline,
|
|
out LongitudinalCandidate candidate)
|
|
{
|
|
candidate = null;
|
|
int intervalCount = stabilizationStart;
|
|
if (intervalCount < 3)
|
|
return false;
|
|
|
|
var motionTimes = new double[intervalCount + 1];
|
|
for (int index = 0; index < motionTimes.Length; index++)
|
|
motionTimes[index] = times[index];
|
|
|
|
LongitudinalCandidate baseline = CreateScheduleReferenceSeed(input, motionTimes, speedLimit);
|
|
var influence = new double[3, intervalCount];
|
|
for (int interval = 0; interval < intervalCount; interval++)
|
|
{
|
|
var basis = new double[intervalCount];
|
|
basis[interval] = 1d;
|
|
LongitudinalCandidate response = LongitudinalCandidate.Integrate(motionTimes, 0d, 0d, 0d, basis);
|
|
int last = response.S.Count - 1;
|
|
influence[0, interval] = response.A[last];
|
|
influence[1, interval] = response.U[last];
|
|
influence[2, interval] = response.S[last];
|
|
}
|
|
|
|
double[] target =
|
|
{
|
|
-baseline.A[baseline.A.Count - 1],
|
|
-baseline.U[baseline.U.Count - 1],
|
|
input.StopBoundaryPathS - baseline.S[baseline.S.Count - 1],
|
|
};
|
|
var gram = new double[3, 3];
|
|
for (int row = 0; row < 3; row++)
|
|
{
|
|
for (int column = 0; column < 3; column++)
|
|
{
|
|
for (int interval = 0; interval < intervalCount; interval++)
|
|
gram[row, column] += influence[row, interval] * influence[column, interval];
|
|
}
|
|
}
|
|
if (!TrySolveThreeByThree(gram, target, out double[] multipliers))
|
|
return false;
|
|
|
|
var jerk = new double[intervalCount];
|
|
for (int interval = 0; interval < intervalCount; interval++)
|
|
{
|
|
jerk[interval] = baseline.J[interval];
|
|
for (int row = 0; row < 3; row++)
|
|
jerk[interval] += influence[row, interval] * multipliers[row];
|
|
}
|
|
|
|
if (TryValidateExactSeed(input, times, stabilizationStart, speedLimit, jerk, out candidate))
|
|
return true;
|
|
|
|
double currentViolation = CalculateExactSeedViolation(input, speedLimit,
|
|
CreateExactCandidate(input, times, stabilizationStart, jerk));
|
|
double maximumJerk = input.Configuration.Longitudinal.MaximumJerkMetersPerSecondCubed;
|
|
for (int pass = 0; pass < 4; pass++)
|
|
{
|
|
for (int basisIndex = 0; basisIndex < intervalCount; basisIndex++)
|
|
{
|
|
if (HasReachedDeadline(stopwatch, deadline))
|
|
return false;
|
|
double[] direction = CreateEndpointNullspaceDirection(influence, gram, basisIndex);
|
|
if (direction == null)
|
|
continue;
|
|
|
|
double[] bestJerk = jerk;
|
|
double bestViolation = currentViolation;
|
|
for (int sample = -256; sample <= 256; sample++)
|
|
{
|
|
if (HasReachedDeadline(stopwatch, deadline))
|
|
return false;
|
|
double scale = maximumJerk * sample / 256d;
|
|
var probeJerk = new double[intervalCount];
|
|
for (int interval = 0; interval < intervalCount; interval++)
|
|
probeJerk[interval] = jerk[interval] + scale * direction[interval];
|
|
LongitudinalCandidate probe = CreateExactCandidate(input, times, stabilizationStart, probeJerk);
|
|
double violation = CalculateExactSeedViolation(input, speedLimit, probe);
|
|
if (violation < bestViolation)
|
|
{
|
|
bestViolation = violation;
|
|
bestJerk = probeJerk;
|
|
}
|
|
}
|
|
jerk = bestJerk;
|
|
currentViolation = bestViolation;
|
|
if (TryValidateExactSeed(input, times, stabilizationStart, speedLimit, jerk, out candidate))
|
|
return true;
|
|
}
|
|
}
|
|
return false;
|
|
}
|
|
|
|
private static TimeSpan GetStaticStartSeedBudget(TimeSpan totalBudget)
|
|
{
|
|
double milliseconds = Math.Min(MaximumStaticStartSeedBudget.TotalMilliseconds,
|
|
Math.Max(1d, totalBudget.TotalMilliseconds * StaticStartSeedBudgetFraction));
|
|
return TimeSpan.FromMilliseconds(milliseconds);
|
|
}
|
|
|
|
private static bool HasReachedDeadline(Stopwatch stopwatch, TimeSpan deadline)
|
|
{
|
|
return stopwatch.Elapsed >= deadline;
|
|
}
|
|
|
|
private bool TryValidateExactSeed(LongitudinalPlanningInput input, IReadOnlyList<double> times,
|
|
int stabilizationStart, PathSpeedLimit speedLimit, IReadOnlyList<double> jerk,
|
|
out LongitudinalCandidate candidate)
|
|
{
|
|
LongitudinalCandidate probe = CreateExactCandidate(input, times, stabilizationStart, jerk);
|
|
return _solutionValidator.TryValidate(input, speedLimit, probe, out candidate, out _);
|
|
}
|
|
|
|
private static LongitudinalCandidate CreateExactCandidate(LongitudinalPlanningInput input,
|
|
IReadOnlyList<double> times, int stabilizationStart, IReadOnlyList<double> jerk)
|
|
{
|
|
var motionTimes = new double[stabilizationStart + 1];
|
|
for (int index = 0; index < motionTimes.Length; index++)
|
|
motionTimes[index] = times[index];
|
|
LongitudinalCandidate motion = LongitudinalCandidate.Integrate(motionTimes, 0d,
|
|
input.InitialProgressSpeedMetersPerSecond, input.InitialAccelerationMetersPerSecondSquared, jerk);
|
|
return AppendExactStopTail(times, stabilizationStart, input.StopBoundaryPathS, motion);
|
|
}
|
|
|
|
private static bool TryCloseExactStopEndpoint(LongitudinalPlanningInput input, IReadOnlyList<double> times,
|
|
int stabilizationStart, double[] jerk, out LongitudinalCandidate candidate)
|
|
{
|
|
candidate = null;
|
|
var motionTimes = new double[stabilizationStart + 1];
|
|
for (int index = 0; index < motionTimes.Length; index++)
|
|
motionTimes[index] = times[index];
|
|
LongitudinalCandidate motion = LongitudinalCandidate.Integrate(motionTimes, 0d,
|
|
input.InitialProgressSpeedMetersPerSecond, input.InitialAccelerationMetersPerSecondSquared, jerk);
|
|
int terminalIndex = motion.S.Count - 1;
|
|
double[] correction =
|
|
{
|
|
-motion.A[terminalIndex],
|
|
-motion.U[terminalIndex],
|
|
input.StopBoundaryPathS - motion.S[terminalIndex],
|
|
};
|
|
var influence = new double[3, 3];
|
|
for (int basisIndex = 0; basisIndex < 3; basisIndex++)
|
|
{
|
|
var basis = new double[stabilizationStart];
|
|
basis[stabilizationStart - 3 + basisIndex] = 1d;
|
|
LongitudinalCandidate response = LongitudinalCandidate.Integrate(motionTimes, 0d, 0d, 0d, basis);
|
|
influence[0, basisIndex] = response.A[terminalIndex];
|
|
influence[1, basisIndex] = response.U[terminalIndex];
|
|
influence[2, basisIndex] = response.S[terminalIndex];
|
|
}
|
|
if (!TrySolveThreeByThree(influence, correction, out double[] adjustment))
|
|
return false;
|
|
for (int index = 0; index < adjustment.Length; index++)
|
|
jerk[stabilizationStart - 3 + index] += adjustment[index];
|
|
candidate = CreateExactCandidate(input, times, stabilizationStart, jerk);
|
|
return true;
|
|
}
|
|
|
|
private static LongitudinalCandidate CreateScheduleReferenceSeed(LongitudinalPlanningInput input,
|
|
IReadOnlyList<double> times, PathSpeedLimit speedLimit)
|
|
{
|
|
LongitudinalConfiguration configuration = input.Configuration.Longitudinal;
|
|
var jerk = new double[times.Count - 1];
|
|
double speed = input.InitialProgressSpeedMetersPerSecond;
|
|
double acceleration = input.InitialAccelerationMetersPerSecondSquared;
|
|
for (int index = 0; index < jerk.Length; index++)
|
|
{
|
|
double dt = times[index + 1] - times[index];
|
|
double targetSpeed = input.KnotSchedule.ReferenceSpeedMetersPerSecond[index + 1];
|
|
double lowerJerk = Math.Max(-configuration.MaximumJerkMetersPerSecondCubed,
|
|
(-configuration.MaximumDecelerationMetersPerSecondSquared - acceleration) / dt);
|
|
double upperJerk = Math.Min(configuration.MaximumJerkMetersPerSecondCubed,
|
|
(configuration.MaximumAccelerationMetersPerSecondSquared - acceleration) / dt);
|
|
double requestedJerk = 2d * (targetSpeed - speed - acceleration * dt) / (dt * dt);
|
|
double selectedJerk = Clamp(requestedJerk, lowerJerk, upperJerk);
|
|
jerk[index] = selectedJerk;
|
|
IntegrateStep(0d, speed, acceleration, selectedJerk, dt, out _, out speed, out acceleration);
|
|
}
|
|
return LongitudinalCandidate.Integrate(times, 0d, input.InitialProgressSpeedMetersPerSecond,
|
|
input.InitialAccelerationMetersPerSecondSquared, jerk);
|
|
}
|
|
|
|
private static double[] CreateEndpointNullspaceDirection(double[,] influence, double[,] gram, int basisIndex)
|
|
{
|
|
int intervalCount = influence.GetLength(1);
|
|
double[] rightHandSide = { influence[0, basisIndex], influence[1, basisIndex], influence[2, basisIndex] };
|
|
if (!TrySolveThreeByThree(gram, rightHandSide, out double[] multipliers))
|
|
return null;
|
|
var direction = new double[intervalCount];
|
|
double magnitude = 0d;
|
|
for (int interval = 0; interval < intervalCount; interval++)
|
|
{
|
|
direction[interval] = interval == basisIndex ? 1d : 0d;
|
|
for (int row = 0; row < 3; row++)
|
|
direction[interval] -= influence[row, interval] * multipliers[row];
|
|
magnitude = Math.Max(magnitude, Math.Abs(direction[interval]));
|
|
}
|
|
if (magnitude <= 1e-12d)
|
|
return null;
|
|
for (int interval = 0; interval < intervalCount; interval++)
|
|
direction[interval] /= magnitude;
|
|
return direction;
|
|
}
|
|
|
|
private static double CalculateExactSeedViolation(LongitudinalPlanningInput input, PathSpeedLimit speedLimit,
|
|
LongitudinalCandidate candidate)
|
|
{
|
|
double tolerance = input.Configuration.Validation.KinematicTolerance;
|
|
double maximumAcceleration = input.Configuration.Longitudinal.MaximumAccelerationMetersPerSecondSquared;
|
|
double maximumDeceleration = input.Configuration.Longitudinal.MaximumDecelerationMetersPerSecondSquared;
|
|
double maximumJerk = input.Configuration.Longitudinal.MaximumJerkMetersPerSecondCubed;
|
|
double violation = 0d;
|
|
double previousS = double.NegativeInfinity;
|
|
for (int index = 0; index < candidate.S.Count; index++)
|
|
{
|
|
double s = candidate.S[index];
|
|
double u = candidate.U[index];
|
|
double a = candidate.A[index];
|
|
if (!IsFinite(s) || !IsFinite(u) || !IsFinite(a))
|
|
return double.PositiveInfinity;
|
|
violation += SquaredExcess(-s, tolerance);
|
|
violation += SquaredExcess(s - input.PathUpperBoundS, tolerance);
|
|
violation += SquaredExcess(previousS - s, tolerance);
|
|
violation += SquaredExcess(-u, tolerance);
|
|
violation += SquaredExcess(a - maximumAcceleration, tolerance);
|
|
violation += SquaredExcess(-maximumDeceleration - a, tolerance);
|
|
double speedLimitAtS = speedLimit.MaximumSpeedAt(Math.Max(0d, Math.Min(input.PathUpperBoundS, s)));
|
|
violation += SquaredExcess(u - speedLimitAtS, tolerance);
|
|
if (!JerkLimitedStoppingMath.TryCalculate(u, a, maximumDeceleration, maximumJerk,
|
|
out JerkLimitedStoppingProfile stop, out _))
|
|
{
|
|
return double.PositiveInfinity;
|
|
}
|
|
violation += SquaredExcess(s + stop.DistanceMeters - input.StopBoundaryPathS, tolerance);
|
|
previousS = s;
|
|
}
|
|
for (int index = 0; index < candidate.J.Count; index++)
|
|
{
|
|
if (!IsFinite(candidate.J[index]))
|
|
return double.PositiveInfinity;
|
|
violation += SquaredExcess(Math.Abs(candidate.J[index]) - maximumJerk, tolerance);
|
|
}
|
|
return violation;
|
|
}
|
|
|
|
private static double SquaredExcess(double actual, double tolerance)
|
|
{
|
|
double excess = Math.Max(0d, actual - tolerance);
|
|
return excess * excess;
|
|
}
|
|
|
|
private static LongitudinalCandidate AppendExactStopTail(IReadOnlyList<double> times, int stabilizationStart,
|
|
double stopBoundaryPathS, LongitudinalCandidate motion)
|
|
{
|
|
var s = new double[times.Count];
|
|
var u = new double[times.Count];
|
|
var a = new double[times.Count];
|
|
var jerk = new double[times.Count - 1];
|
|
int motionCount = Math.Min(stabilizationStart + 1, motion.S.Count);
|
|
for (int index = 0; index < motionCount; index++)
|
|
{
|
|
s[index] = motion.S[index];
|
|
u[index] = motion.U[index];
|
|
a[index] = motion.A[index];
|
|
}
|
|
for (int index = 0; index < Math.Min(stabilizationStart, motion.J.Count); index++)
|
|
jerk[index] = motion.J[index];
|
|
for (int index = stabilizationStart; index < times.Count; index++)
|
|
{
|
|
s[index] = stopBoundaryPathS;
|
|
u[index] = 0d;
|
|
a[index] = 0d;
|
|
}
|
|
return new LongitudinalCandidate(times, s, u, a, jerk);
|
|
}
|
|
|
|
private static bool SatisfiesLongitudinalBounds(LongitudinalPlanningInput input, LongitudinalCandidate candidate)
|
|
{
|
|
LongitudinalConfiguration configuration = input.Configuration.Longitudinal;
|
|
double previousS = double.NegativeInfinity;
|
|
for (int index = 0; index < candidate.S.Count; index++)
|
|
{
|
|
if (candidate.S[index] < -1e-10d || candidate.S[index] > input.StopBoundaryPathS + 1e-10d ||
|
|
candidate.S[index] < previousS - 1e-10d || candidate.U[index] < -1e-10d ||
|
|
candidate.U[index] > input.DirectionMaximumSpeedMetersPerSecond + 1e-10d ||
|
|
candidate.A[index] < -configuration.MaximumDecelerationMetersPerSecondSquared - 1e-10d ||
|
|
candidate.A[index] > configuration.MaximumAccelerationMetersPerSecondSquared + 1e-10d)
|
|
{
|
|
return false;
|
|
}
|
|
previousS = candidate.S[index];
|
|
}
|
|
for (int index = 0; index < candidate.J.Count; index++)
|
|
{
|
|
if (Math.Abs(candidate.J[index]) > configuration.MaximumJerkMetersPerSecondCubed + 1e-10d)
|
|
return false;
|
|
}
|
|
return true;
|
|
}
|
|
|
|
private static bool TrySolveThreeByThree(double[,] matrix, IReadOnlyList<double> rightHandSide,
|
|
out double[] solution)
|
|
{
|
|
solution = new double[3];
|
|
var augmented = new double[3, 4];
|
|
for (int row = 0; row < 3; row++)
|
|
{
|
|
for (int column = 0; column < 3; column++)
|
|
augmented[row, column] = matrix[row, column];
|
|
augmented[row, 3] = rightHandSide[row];
|
|
}
|
|
for (int pivot = 0; pivot < 3; pivot++)
|
|
{
|
|
int bestRow = pivot;
|
|
for (int row = pivot + 1; row < 3; row++)
|
|
{
|
|
if (Math.Abs(augmented[row, pivot]) > Math.Abs(augmented[bestRow, pivot]))
|
|
bestRow = row;
|
|
}
|
|
if (Math.Abs(augmented[bestRow, pivot]) <= 1e-14d)
|
|
return false;
|
|
if (bestRow != pivot)
|
|
{
|
|
for (int column = pivot; column < 4; column++)
|
|
{
|
|
double temporary = augmented[pivot, column];
|
|
augmented[pivot, column] = augmented[bestRow, column];
|
|
augmented[bestRow, column] = temporary;
|
|
}
|
|
}
|
|
double divisor = augmented[pivot, pivot];
|
|
for (int column = pivot; column < 4; column++)
|
|
augmented[pivot, column] /= divisor;
|
|
for (int row = 0; row < 3; row++)
|
|
{
|
|
if (row == pivot)
|
|
continue;
|
|
double factor = augmented[row, pivot];
|
|
for (int column = pivot; column < 4; column++)
|
|
augmented[row, column] -= factor * augmented[pivot, column];
|
|
}
|
|
}
|
|
for (int row = 0; row < 3; row++)
|
|
solution[row] = augmented[row, 3];
|
|
return true;
|
|
}
|
|
|
|
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 TryCreateFeasibilityEnvelopeIterate(LongitudinalPlanningInput input,
|
|
LongitudinalCandidate candidate, out LongitudinalCandidate nextIterate)
|
|
{
|
|
nextIterate = null;
|
|
int stabilizationStart = input.KnotSchedule.TerminalHoldStartIndex;
|
|
var pathS = new double[candidate.S.Count];
|
|
double previousPathS = double.NegativeInfinity;
|
|
double tolerance = input.Configuration.Validation.KinematicTolerance;
|
|
for (int index = 0; index < pathS.Length; index++)
|
|
{
|
|
double value = candidate.S[index];
|
|
if (!IsFinite(value) || value < -tolerance || value > input.PathUpperBoundS + tolerance ||
|
|
value < previousPathS - tolerance)
|
|
{
|
|
return false;
|
|
}
|
|
value = Math.Max(0d, Math.Min(input.PathUpperBoundS, value));
|
|
pathS[index] = index >= stabilizationStart ? input.StopBoundaryPathS : Math.Max(previousPathS, value);
|
|
previousPathS = pathS[index];
|
|
}
|
|
nextIterate = new LongitudinalCandidate(candidate.KnotTimes, pathS, candidate.U, candidate.A, candidate.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 readonly struct TrustedSolveAttempt
|
|
{
|
|
internal TrustedSolveAttempt(EmPlanningStatus status, LongitudinalCandidate candidate,
|
|
int solveCount, bool accepted, string failureReason)
|
|
{
|
|
Status = status;
|
|
Candidate = candidate;
|
|
SolveCount = solveCount;
|
|
Accepted = accepted;
|
|
FailureReason = failureReason ?? string.Empty;
|
|
}
|
|
|
|
internal EmPlanningStatus Status { get; }
|
|
|
|
internal LongitudinalCandidate Candidate { get; }
|
|
|
|
internal int SolveCount { get; }
|
|
|
|
internal bool Accepted { get; }
|
|
|
|
internal string FailureReason { get; }
|
|
}
|
|
|
|
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);
|
|
}
|
|
|
|
private static double Clamp(double value, double minimum, double maximum)
|
|
{
|
|
return Math.Max(minimum, Math.Min(maximum, value));
|
|
}
|
|
|
|
private static void IntegrateStep(double s, double u, double a, double jerk, double duration,
|
|
out double nextS, out double nextU, out double nextA)
|
|
{
|
|
nextS = s + u * duration + 0.5d * a * duration * duration +
|
|
jerk * duration * duration * duration / 6d;
|
|
nextU = u + a * duration + 0.5d * jerk * duration * duration;
|
|
nextA = a + jerk * duration;
|
|
}
|
|
}
|