1026 lines
51 KiB
C#
1026 lines
51 KiB
C#
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);
|
|
|
|
var stopwatch = Stopwatch.StartNew();
|
|
LongitudinalCandidate iterate;
|
|
LongitudinalCandidate lastStrictCandidate = null;
|
|
int remainingObjectiveIterations = iterationLimit;
|
|
if (input.PlanningScope == EmPlanningScope.FullDirectionSegment &&
|
|
input.Mode == EmLongitudinalMode.ExactStopAtBoundary)
|
|
{
|
|
if (!TryCreateInitialFeasibleCandidate(input, speedLimit, settings, totalBudget, convergenceTolerance,
|
|
iterationLimit, stopwatch, cancellationToken, out iterate, out int projectionSolveCount,
|
|
out EmPlanningStatus projectionStatus, out string projectionFailure))
|
|
{
|
|
return Failed(projectionStatus, projectionFailure);
|
|
}
|
|
lastStrictCandidate = CopyCandidate(iterate);
|
|
remainingObjectiveIterations -= projectionSolveCount;
|
|
if (remainingObjectiveIterations <= 0)
|
|
{
|
|
return new LongitudinalPlanningResult(EmPlanningStatus.SuccessWithFallback, lastStrictCandidate,
|
|
"The strict initial feasibility projection consumed the configured outer-iteration budget.");
|
|
}
|
|
}
|
|
else
|
|
{
|
|
iterate = CreateInitialIterate(input, speedLimit);
|
|
if (!_solutionValidator.TryValidate(input, speedLimit, iterate, out lastStrictCandidate, out _))
|
|
lastStrictCandidate = null;
|
|
}
|
|
double[] warmStart = ToPrimal(iterate);
|
|
bool hasDynamicsConsistentInitialWarmStart = iterate.SatisfiesExactDiscreteDynamics(1e-12d);
|
|
string lastCandidateRejection = string.Empty;
|
|
bool hasPreviousObjective = false;
|
|
double previousObjective = 0d;
|
|
|
|
for (int iteration = 0; iteration < remainingObjectiveIterations; 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 bool TryCreateInitialFeasibleCandidate(LongitudinalPlanningInput input, PathSpeedLimit speedLimit,
|
|
QpSolverSettings settings, TimeSpan totalBudget, double convergenceTolerance, int iterationLimit,
|
|
Stopwatch stopwatch, CancellationToken cancellationToken, out LongitudinalCandidate candidate,
|
|
out int projectionSolveCount, out EmPlanningStatus failureStatus, out string failureReason)
|
|
{
|
|
candidate = null;
|
|
projectionSolveCount = 0;
|
|
failureStatus = EmPlanningStatus.LongitudinalInfeasible;
|
|
failureReason = string.Empty;
|
|
LongitudinalCandidate linearizationIterate = CreateScheduleReferenceIterate(input);
|
|
string lastRejection = string.Empty;
|
|
for (int iteration = 0; iteration < iterationLimit; iteration++)
|
|
{
|
|
if (cancellationToken.IsCancellationRequested)
|
|
{
|
|
failureStatus = EmPlanningStatus.Cancelled;
|
|
failureReason = "Initial full-direction feasibility projection was cancelled.";
|
|
return false;
|
|
}
|
|
TimeSpan remainingBudget = totalBudget - stopwatch.Elapsed;
|
|
if (remainingBudget <= TimeSpan.Zero)
|
|
{
|
|
failureStatus = EmPlanningStatus.SolverTimedOut;
|
|
failureReason = "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 = "Initial full-direction feasibility constraints are infeasible: " + buildFailure;
|
|
return false;
|
|
}
|
|
|
|
double projectionTolerance = Math.Min(settings.AbsoluteTolerance,
|
|
input.Configuration.Validation.KinematicTolerance * 0.1d);
|
|
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);
|
|
projectionSolveCount++;
|
|
if (solved == null)
|
|
{
|
|
failureStatus = EmPlanningStatus.Failed;
|
|
failureReason = "The initial full-direction feasibility solver returned no result.";
|
|
return false;
|
|
}
|
|
if (solved.Status == QpSolveStatus.TimeLimit || solved.Status == QpSolveStatus.MaximumIterations)
|
|
{
|
|
failureStatus = EmPlanningStatus.SolverTimedOut;
|
|
failureReason = "Initial full-direction feasibility projection timed out (status=" + solved.NativeStatus +
|
|
", iterations=" + solved.Iterations + ", primal=" + solved.PrimalResidual + ", dual=" +
|
|
solved.DualResidual + "): " + solved.Diagnostic;
|
|
return false;
|
|
}
|
|
if (solved.Status == QpSolveStatus.Cancelled)
|
|
{
|
|
failureStatus = EmPlanningStatus.Cancelled;
|
|
failureReason = "Initial full-direction feasibility projection was cancelled: " + solved.Diagnostic;
|
|
return false;
|
|
}
|
|
if (solved.Status == QpSolveStatus.PrimalInfeasible || solved.Status == QpSolveStatus.DualInfeasible)
|
|
{
|
|
failureStatus = EmPlanningStatus.LongitudinalInfeasible;
|
|
failureReason = "Initial full-direction feasibility projection is infeasible: " + solved.Diagnostic;
|
|
return false;
|
|
}
|
|
if (solved.Status == QpSolveStatus.SolverUnavailable)
|
|
{
|
|
failureStatus = EmPlanningStatus.SolverUnavailable;
|
|
failureReason = "Initial full-direction feasibility solver is unavailable: " + solved.Diagnostic;
|
|
return false;
|
|
}
|
|
if (solved.Status != QpSolveStatus.Solved && solved.Status != QpSolveStatus.SolvedInaccurate)
|
|
{
|
|
failureStatus = EmPlanningStatus.Failed;
|
|
failureReason = "Initial full-direction feasibility solver failed: " + solved.Diagnostic;
|
|
return false;
|
|
}
|
|
if (!TryCreateCandidate(input.KnotSchedule.KnotTimes, solved.Primal, out LongitudinalCandidate projected))
|
|
{
|
|
failureStatus = EmPlanningStatus.LongitudinalInfeasible;
|
|
failureReason = "Initial full-direction feasibility solver primal does not match the ST layout.";
|
|
return false;
|
|
}
|
|
if (solved.Status == QpSolveStatus.Solved || HasStrictResiduals(solved, convergenceTolerance))
|
|
{
|
|
if (_solutionValidator.TryValidate(input, speedLimit, projected, out LongitudinalCandidate strict,
|
|
out string validationFailure))
|
|
{
|
|
candidate = strict;
|
|
return true;
|
|
}
|
|
lastRejection = validationFailure;
|
|
}
|
|
|
|
if (!TryCreateFeasibilityEnvelopeIterate(input, projected,
|
|
out LongitudinalCandidate nextLinearization))
|
|
{
|
|
failureStatus = EmPlanningStatus.LongitudinalInfeasible;
|
|
failureReason = "Initial full-direction feasibility candidate could not be relinearized against the PathS envelope.";
|
|
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.";
|
|
}
|
|
failureStatus = EmPlanningStatus.LongitudinalInfeasible;
|
|
failureReason = "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);
|
|
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 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)
|
|
{
|
|
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++)
|
|
{
|
|
double[] direction = CreateEndpointNullspaceDirection(influence, gram, basisIndex);
|
|
if (direction == null)
|
|
continue;
|
|
|
|
double[] bestJerk = jerk;
|
|
double bestViolation = currentViolation;
|
|
for (int sample = -256; sample <= 256; sample++)
|
|
{
|
|
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 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 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 TryCreateEnvelopeIterate(LongitudinalPlanningInput input, LongitudinalCandidate previous,
|
|
LongitudinalCandidate candidate, out LongitudinalCandidate nextIterate)
|
|
{
|
|
nextIterate = null;
|
|
if (candidate.S.Count != previous.S.Count)
|
|
return false;
|
|
int stabilizationStart = input.Mode != EmLongitudinalMode.ExactStopAtBoundary
|
|
? candidate.S.Count
|
|
: input.PlanningScope == EmPlanningScope.FullDirectionSegment
|
|
? input.KnotSchedule.TerminalHoldStartIndex
|
|
: LongitudinalTerminalSchedule.GetStabilizationStartIndex(candidate.KnotTimes,
|
|
input.Configuration.Scheduling.OutputTimeStepSeconds);
|
|
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.PathUpperBoundS ||
|
|
previousProgress < priorPreviousProgress)
|
|
{
|
|
return false;
|
|
}
|
|
if (!IsFinite(candidateProgress))
|
|
{
|
|
candidateProgress = previousProgress;
|
|
}
|
|
candidateProgress = Math.Max(0d, Math.Min(input.PathUpperBoundS, candidateProgress));
|
|
if (input.Mode == EmLongitudinalMode.ExactStopAtBoundary && index >= stabilizationStart)
|
|
candidateProgress = input.StopBoundaryPathS;
|
|
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 || (input.Mode == EmLongitudinalMode.ExactStopAtBoundary &&
|
|
index >= stabilizationStart) || candidateProgress >= input.PathUpperBoundS)
|
|
{
|
|
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.PathUpperBoundS - 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 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 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.PathUpperBoundS, 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);
|
|
}
|
|
|
|
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;
|
|
}
|
|
}
|