feat: derive adaptive full-segment ST schedule
This commit is contained in:
梁薄云
@@ -83,16 +83,34 @@ public sealed class EmPlanningService : IEmPlanningService
|
||||
return Failure(lateral.Status, request, lateral.FailureReason);
|
||||
EmitDebug(request, "LS optimization and validation succeeded");
|
||||
|
||||
IReadOnlyList<double> knotTimes = LongitudinalCandidate.CreateKnotTimes(
|
||||
configuration.Scheduling.TimeHorizonSeconds, configuration.Scheduling.OutputTimeStepSeconds);
|
||||
EmPlanningStatus envelopeStatus = new PathSpeedLimitBuilder().Build(lateral.Path, segment.Direction,
|
||||
initialProgressSpeed, horizon.TerminalType, configuration, out PathSpeedLimit speedLimit,
|
||||
out string envelopeReason);
|
||||
if (envelopeStatus != EmPlanningStatus.Success)
|
||||
return Failure(envelopeStatus, request, envelopeReason);
|
||||
LongitudinalKnotSchedule knotSchedule;
|
||||
if (request.PlanningScope == EmPlanningScope.FullDirectionSegment)
|
||||
{
|
||||
EmPlanningStatus scheduleStatus = new FullDirectionSegmentScheduleBuilder().TryBuild(lateral.Path, speedLimit,
|
||||
initialProgressSpeed, initialAcceleration, DesiredSpeed(configuration, segment.Direction), configuration,
|
||||
out knotSchedule, out string scheduleReason);
|
||||
if (scheduleStatus != EmPlanningStatus.Success)
|
||||
return Failure(scheduleStatus, request, scheduleReason);
|
||||
}
|
||||
else
|
||||
{
|
||||
knotSchedule = LongitudinalKnotSchedule.CreateRolling(configuration.Scheduling.TimeHorizonSeconds,
|
||||
configuration.Scheduling.OutputTimeStepSeconds);
|
||||
}
|
||||
LongitudinalPreviousTrajectorySeed previousLongitudinalSeed =
|
||||
new LongitudinalPreviousTrajectorySeedBuilder().Build(
|
||||
request.PreviousTrajectory, lateral.Path, request.EffectiveAtUtc, knotTimes,
|
||||
request.PreviousTrajectory, lateral.Path, request.EffectiveAtUtc, knotSchedule,
|
||||
segment.SegmentIndex, segment.Direction);
|
||||
var longitudinalInput = new LongitudinalPlanningInput(lateral.Path, segment.Direction, initialProgressSpeed,
|
||||
initialAcceleration, horizon.TerminalType, horizon.LongitudinalMode, configuration,
|
||||
request.PlanningScope, knotSchedule,
|
||||
previousLongitudinalSeed.PathS, previousLongitudinalSeed.ProgressSpeedMetersPerSecond);
|
||||
EmPlanningStatus envelopeStatus = new PathSpeedLimitBuilder().Build(longitudinalInput, out _, out string envelopeReason);
|
||||
envelopeStatus = new PathSpeedLimitBuilder().Build(longitudinalInput, out _, out envelopeReason);
|
||||
if (envelopeStatus != EmPlanningStatus.Success)
|
||||
return Failure(envelopeStatus, request, envelopeReason);
|
||||
EmitDebug(request, "PathS speed envelope succeeded");
|
||||
@@ -195,6 +213,13 @@ public sealed class EmPlanningService : IEmPlanningService
|
||||
: state.SignedLongitudinalSpeedMetersPerSecond < 0d;
|
||||
}
|
||||
|
||||
private static double DesiredSpeed(EmPlannerConfiguration configuration, TravelDirection direction)
|
||||
{
|
||||
return direction == TravelDirection.Forward
|
||||
? configuration.Longitudinal.DesiredForwardSpeedMetersPerSecond
|
||||
: configuration.Longitudinal.DesiredReverseSpeedMetersPerSecond;
|
||||
}
|
||||
|
||||
private static bool IsSuccess(EmPlanningStatus status)
|
||||
{
|
||||
return status == EmPlanningStatus.Success || status == EmPlanningStatus.SuccessWithFallback;
|
||||
|
||||
+203
@@ -0,0 +1,203 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
|
||||
|
||||
/// <summary>Derives bounded full-direction ST knots from the physical PathS speed and stopping envelope.</summary>
|
||||
public sealed class FullDirectionSegmentScheduleBuilder
|
||||
{
|
||||
private const double Tolerance = 1e-10d;
|
||||
|
||||
public EmPlanningStatus TryBuild(LateralPath path, PathSpeedLimit speedLimit,
|
||||
double initialProgressSpeedMetersPerSecond, double initialAccelerationMetersPerSecondSquared,
|
||||
double desiredSpeedMetersPerSecond, EmPlannerConfiguration configuration,
|
||||
out LongitudinalKnotSchedule schedule, out string failureReason)
|
||||
{
|
||||
schedule = null;
|
||||
failureReason = string.Empty;
|
||||
if (path == null || speedLimit == null || configuration == null || configuration.Scheduling == null ||
|
||||
configuration.Longitudinal == null || !path.IsIndependentlyValidated || path.Points.Count < 2 ||
|
||||
!IsFinite(initialProgressSpeedMetersPerSecond) || initialProgressSpeedMetersPerSecond < 0d ||
|
||||
!IsFinite(initialAccelerationMetersPerSecondSquared) || !IsPositiveFinite(desiredSpeedMetersPerSecond))
|
||||
{
|
||||
failureReason = "Full-direction schedule inputs are invalid.";
|
||||
return EmPlanningStatus.InvalidInput;
|
||||
}
|
||||
if (!speedLimit.HasStopBoundary || Math.Abs(speedLimit.PathUpperBoundS -
|
||||
path.Points[path.Points.Count - 1].PathS) > Tolerance)
|
||||
{
|
||||
failureReason = "A full-direction schedule requires the matching real stop-boundary speed envelope.";
|
||||
return EmPlanningStatus.InvalidInput;
|
||||
}
|
||||
|
||||
SchedulingConfiguration scheduling = configuration.Scheduling;
|
||||
LongitudinalConfiguration longitudinal = configuration.Longitudinal;
|
||||
if (!IsPositiveFinite(scheduling.MaximumOptimizationTimeStepSeconds) ||
|
||||
!IsPositiveFinite(scheduling.MaximumOptimizationSpatialStepMeters) ||
|
||||
scheduling.MaximumOptimizationKnotCount < 3 ||
|
||||
!IsPositiveFinite(longitudinal.MaximumAccelerationMetersPerSecondSquared) ||
|
||||
!IsPositiveFinite(longitudinal.MaximumDecelerationMetersPerSecondSquared) ||
|
||||
!IsPositiveFinite(longitudinal.MaximumJerkMetersPerSecondCubed))
|
||||
{
|
||||
failureReason = "Full-direction schedule limits are invalid.";
|
||||
return EmPlanningStatus.InvalidInput;
|
||||
}
|
||||
|
||||
int stationCount = speedLimit.PathS.Count;
|
||||
var speeds = new double[stationCount];
|
||||
double desired = Math.Min(desiredSpeedMetersPerSecond, speedLimit.DirectionMaximumSpeedMetersPerSecond);
|
||||
speeds[0] = Math.Min(initialProgressSpeedMetersPerSecond, Math.Min(desired,
|
||||
speedLimit.MaximumSpeedMetersPerSecond[0]));
|
||||
for (int index = 1; index < stationCount; index++)
|
||||
{
|
||||
double distance = speedLimit.PathS[index] - speedLimit.PathS[index - 1];
|
||||
double reachable = Math.Sqrt(Math.Max(0d, speeds[index - 1] * speeds[index - 1] +
|
||||
2d * longitudinal.MaximumAccelerationMetersPerSecondSquared * distance));
|
||||
speeds[index] = Math.Min(reachable, Math.Min(desired, speedLimit.MaximumSpeedMetersPerSecond[index]));
|
||||
}
|
||||
speeds[stationCount - 1] = 0d;
|
||||
for (int index = stationCount - 2; index >= 0; index--)
|
||||
{
|
||||
double remainingDistance = speedLimit.PathUpperBoundS - speedLimit.PathS[index];
|
||||
double stopCap = JerkLimitedStoppingMath.MaximumInitialSpeedForDistance(remainingDistance,
|
||||
Math.Max(0d, initialAccelerationMetersPerSecondSquared), longitudinal.MaximumDecelerationMetersPerSecondSquared,
|
||||
longitudinal.MaximumJerkMetersPerSecondCubed, speedLimit.DirectionMaximumSpeedMetersPerSecond);
|
||||
double distance = speedLimit.PathS[index + 1] - speedLimit.PathS[index];
|
||||
double decelerationCap = Math.Sqrt(Math.Max(0d, speeds[index + 1] * speeds[index + 1] +
|
||||
2d * longitudinal.MaximumDecelerationMetersPerSecondSquared * distance));
|
||||
speeds[index] = Math.Min(speeds[index], Math.Min(stopCap, decelerationCap));
|
||||
}
|
||||
|
||||
var times = new List<double> { 0d };
|
||||
var pathS = new List<double> { 0d };
|
||||
var referenceSpeeds = new List<double> { speeds[0] };
|
||||
IReadOnlyList<int> scheduleStations = SelectScheduleStations(speedLimit.PathS, speeds);
|
||||
int minimumIntervalsPerSegment = Math.Max(1,
|
||||
(3 + scheduleStations.Count - 2) / (scheduleStations.Count - 1));
|
||||
for (int stationIndex = 1; stationIndex < scheduleStations.Count; stationIndex++)
|
||||
{
|
||||
int startIndex = scheduleStations[stationIndex - 1];
|
||||
int endIndex = scheduleStations[stationIndex];
|
||||
double startS = speedLimit.PathS[startIndex];
|
||||
double endS = speedLimit.PathS[endIndex];
|
||||
double startSpeed = speeds[startIndex];
|
||||
double endSpeed = speeds[endIndex];
|
||||
double distance = endS - startS;
|
||||
double denominator = startSpeed + endSpeed;
|
||||
double duration = denominator > Tolerance ? 2d * distance / denominator :
|
||||
Math.Sqrt(2d * distance / Math.Max(Tolerance, longitudinal.MaximumAccelerationMetersPerSecondSquared));
|
||||
int subdivisionCount = Math.Max(minimumIntervalsPerSegment, Math.Max(
|
||||
checked((int)Math.Ceiling(distance / scheduling.MaximumOptimizationSpatialStepMeters)),
|
||||
checked((int)Math.Ceiling(duration / scheduling.MaximumOptimizationTimeStepSeconds))));
|
||||
for (int subdivision = 1; subdivision <= subdivisionCount; subdivision++)
|
||||
{
|
||||
double fraction = (double)subdivision / subdivisionCount;
|
||||
times.Add(times[times.Count - 1] + duration / subdivisionCount);
|
||||
pathS.Add(startS + distance * fraction);
|
||||
referenceSpeeds.Add(startSpeed + (endSpeed - startSpeed) * fraction);
|
||||
}
|
||||
}
|
||||
referenceSpeeds[referenceSpeeds.Count - 1] = 0d;
|
||||
pathS[pathS.Count - 1] = speedLimit.PathUpperBoundS;
|
||||
EnsureJerkReachableReferenceTimes(times, referenceSpeeds, longitudinal);
|
||||
EnsureMinimumExactStopDuration(times, speedLimit.PathUpperBoundS, initialProgressSpeedMetersPerSecond,
|
||||
initialAccelerationMetersPerSecondSquared, longitudinal);
|
||||
if (!IsFinite(longitudinal.ZeroSpeedHoldSeconds) || longitudinal.ZeroSpeedHoldSeconds < 0d)
|
||||
{
|
||||
failureReason = "The full-direction zero-speed hold duration is invalid.";
|
||||
return EmPlanningStatus.InvalidInput;
|
||||
}
|
||||
int terminalHoldStartIndex = times.Count - 1;
|
||||
double remainingHold = longitudinal.ZeroSpeedHoldSeconds;
|
||||
while (remainingHold > Tolerance)
|
||||
{
|
||||
double holdStep = Math.Min(remainingHold, scheduling.MaximumOptimizationTimeStepSeconds);
|
||||
times.Add(times[times.Count - 1] + holdStep);
|
||||
pathS.Add(speedLimit.PathUpperBoundS);
|
||||
referenceSpeeds.Add(0d);
|
||||
remainingHold -= holdStep;
|
||||
}
|
||||
if (times.Count > scheduling.MaximumOptimizationKnotCount)
|
||||
{
|
||||
failureReason = "Full-direction schedule required knots=" + times.Count + ", configured maximum=" +
|
||||
scheduling.MaximumOptimizationKnotCount + ".";
|
||||
return EmPlanningStatus.FullSegmentResourceLimitExceeded;
|
||||
}
|
||||
try
|
||||
{
|
||||
schedule = LongitudinalKnotSchedule.CreateAdaptive(times, pathS, referenceSpeeds,
|
||||
terminalHoldStartIndex);
|
||||
return EmPlanningStatus.Success;
|
||||
}
|
||||
catch (ArgumentException exception)
|
||||
{
|
||||
failureReason = exception.Message;
|
||||
return EmPlanningStatus.InvalidInput;
|
||||
}
|
||||
}
|
||||
|
||||
private static void EnsureMinimumExactStopDuration(IList<double> times, double stopBoundaryPathS,
|
||||
double initialSpeed, double initialAcceleration, LongitudinalConfiguration configuration)
|
||||
{
|
||||
if (initialSpeed <= Tolerance || !JerkLimitedStoppingMath.TryCalculate(initialSpeed, initialAcceleration,
|
||||
configuration.MaximumDecelerationMetersPerSecondSquared,
|
||||
configuration.MaximumJerkMetersPerSecondCubed, out JerkLimitedStoppingProfile stop, out _))
|
||||
{
|
||||
return;
|
||||
}
|
||||
double cruiseDistance = Math.Max(0d, stopBoundaryPathS - stop.DistanceMeters);
|
||||
double requiredDuration = stop.DurationSeconds + cruiseDistance / initialSpeed;
|
||||
double stopSpeedTolerance = configuration.StopSpeedToleranceMetersPerSecond;
|
||||
if (IsPositiveFinite(stopSpeedTolerance) && JerkLimitedStoppingMath.TryCalculate(stopSpeedTolerance, 0d,
|
||||
configuration.MaximumDecelerationMetersPerSecondSquared,
|
||||
configuration.MaximumJerkMetersPerSecondCubed, out JerkLimitedStoppingProfile settlingStop, out _))
|
||||
{
|
||||
double envelopeTraverseDuration = 2d * stopBoundaryPathS / (initialSpeed + stopSpeedTolerance);
|
||||
requiredDuration = Math.Max(requiredDuration, envelopeTraverseDuration + settlingStop.DurationSeconds);
|
||||
}
|
||||
double currentDuration = times[times.Count - 1];
|
||||
if (currentDuration + Tolerance >= requiredDuration)
|
||||
return;
|
||||
double scale = requiredDuration / currentDuration;
|
||||
for (int index = 1; index < times.Count; index++)
|
||||
times[index] *= scale;
|
||||
}
|
||||
|
||||
private static void EnsureJerkReachableReferenceTimes(IList<double> times, IReadOnlyList<double> referenceSpeeds,
|
||||
LongitudinalConfiguration configuration)
|
||||
{
|
||||
double adjustedTime = 0d;
|
||||
for (int index = 1; index < times.Count; index++)
|
||||
{
|
||||
double requestedDuration = times[index] - times[index - 1];
|
||||
double speedChange = Math.Abs(referenceSpeeds[index] - referenceSpeeds[index - 1]);
|
||||
double accelerationLimit = referenceSpeeds[index] >= referenceSpeeds[index - 1]
|
||||
? configuration.MaximumAccelerationMetersPerSecondSquared
|
||||
: configuration.MaximumDecelerationMetersPerSecondSquared;
|
||||
double accelerationDuration = speedChange / accelerationLimit;
|
||||
double triangularJerkDuration = speedChange <= Tolerance
|
||||
? 0d
|
||||
: 2d * Math.Sqrt(speedChange / configuration.MaximumJerkMetersPerSecondCubed);
|
||||
adjustedTime += Math.Max(requestedDuration, Math.Max(accelerationDuration, triangularJerkDuration));
|
||||
times[index] = adjustedTime;
|
||||
}
|
||||
}
|
||||
|
||||
private static IReadOnlyList<int> SelectScheduleStations(IReadOnlyList<double> pathS,
|
||||
IReadOnlyList<double> speeds)
|
||||
{
|
||||
var stations = new List<int> { 0 };
|
||||
for (int index = 1; index < pathS.Count - 1; index++)
|
||||
{
|
||||
double previousSlope = (speeds[index] - speeds[index - 1]) / (pathS[index] - pathS[index - 1]);
|
||||
double nextSlope = (speeds[index + 1] - speeds[index]) / (pathS[index + 1] - pathS[index]);
|
||||
if (previousSlope * nextSlope < 0d)
|
||||
stations.Add(index);
|
||||
}
|
||||
stations.Add(pathS.Count - 1);
|
||||
return stations;
|
||||
}
|
||||
|
||||
private static bool IsFinite(double value) => !double.IsNaN(value) && !double.IsInfinity(value);
|
||||
private static bool IsPositiveFinite(double value) => IsFinite(value) && value > 0d;
|
||||
}
|
||||
+128
-10
@@ -15,6 +15,35 @@ public sealed class LongitudinalConstraintBuilder
|
||||
|
||||
public bool TryBuild(LongitudinalPlanningInput input, PathSpeedLimit speedLimit, LongitudinalCandidate iterate,
|
||||
out QuadraticProgram problem, out string failureReason)
|
||||
{
|
||||
return TryBuildCore(input, speedLimit, iterate, false, out problem, out failureReason);
|
||||
}
|
||||
|
||||
/// <summary>Builds the bounded full-scope feasibility projection before objective optimization.</summary>
|
||||
public bool TryBuildInitialFeasibilityProjection(LongitudinalPlanningInput input, PathSpeedLimit speedLimit,
|
||||
out QuadraticProgram problem, out string failureReason)
|
||||
{
|
||||
return TryBuildInitialFeasibilityProjection(input, speedLimit,
|
||||
input == null ? null : CreateScheduleReferenceIterate(input), out problem, out failureReason);
|
||||
}
|
||||
|
||||
public bool TryBuildInitialFeasibilityProjection(LongitudinalPlanningInput input, PathSpeedLimit speedLimit,
|
||||
LongitudinalCandidate linearizationIterate, out QuadraticProgram problem, out string failureReason)
|
||||
{
|
||||
problem = null;
|
||||
failureReason = string.Empty;
|
||||
if (input == null || input.PlanningScope != EmPlanningScope.FullDirectionSegment ||
|
||||
input.Mode != EmLongitudinalMode.ExactStopAtBoundary || linearizationIterate == null)
|
||||
{
|
||||
failureReason = "Initial feasibility projection is only defined for full-direction exact-stop planning.";
|
||||
return false;
|
||||
}
|
||||
return TryBuildCore(input, speedLimit, linearizationIterate, true, out problem,
|
||||
out failureReason);
|
||||
}
|
||||
|
||||
private bool TryBuildCore(LongitudinalPlanningInput input, PathSpeedLimit speedLimit, LongitudinalCandidate iterate,
|
||||
bool useScheduleReferenceObjective, out QuadraticProgram problem, out string failureReason)
|
||||
{
|
||||
problem = null;
|
||||
failureReason = string.Empty;
|
||||
@@ -25,10 +54,9 @@ public sealed class LongitudinalConstraintBuilder
|
||||
if (Math.Abs(speedLimit.PathUpperBoundS - input.PathUpperBoundS) > 1e-12d)
|
||||
throw new ArgumentException("The speed envelope upper bound must match actual lateral PathS.");
|
||||
|
||||
IReadOnlyList<double> expectedTimes = LongitudinalCandidate.CreateKnotTimes(
|
||||
input.Configuration.Scheduling.TimeHorizonSeconds, input.Configuration.Scheduling.OutputTimeStepSeconds);
|
||||
IReadOnlyList<double> expectedTimes = input.KnotSchedule.KnotTimes;
|
||||
if (!HasMatchingTimes(iterate.KnotTimes, expectedTimes))
|
||||
throw new ArgumentException("The ST iterate time knots do not match the configured horizon.");
|
||||
throw new ArgumentException("The ST iterate time knots do not match the supplied knot schedule.");
|
||||
var layout = new LongitudinalVariableLayout(expectedTimes.Count);
|
||||
if (iterate.S.Count != layout.KnotCount || iterate.U.Count != layout.KnotCount ||
|
||||
iterate.A.Count != layout.KnotCount || iterate.J.Count != layout.KnotCount - 1)
|
||||
@@ -50,13 +78,13 @@ public sealed class LongitudinalConstraintBuilder
|
||||
|
||||
var hessian = new SparseTripletBuilder(layout.VariableCount, layout.VariableCount, true);
|
||||
var linearCost = new double[layout.VariableCount];
|
||||
_objectiveBuilder.AddTerms(input, speedLimit, layout, iterate, hessian, linearCost);
|
||||
int stabilizationStart = input.Mode == EmLongitudinalMode.ExactStopAtBoundary
|
||||
? LongitudinalTerminalSchedule.GetStabilizationStartIndex(expectedTimes,
|
||||
input.Configuration.Scheduling.OutputTimeStepSeconds)
|
||||
: layout.KnotCount;
|
||||
if (useScheduleReferenceObjective)
|
||||
AddInitialFeasibilityObjective(input, layout, hessian, linearCost);
|
||||
else
|
||||
_objectiveBuilder.AddTerms(input, speedLimit, layout, iterate, hessian, linearCost);
|
||||
int stabilizationStart = GetStabilizationStart(input, expectedTimes, layout.KnotCount);
|
||||
int stationaryKnotCount = layout.KnotCount - stabilizationStart;
|
||||
int expectedRows = 8 * layout.KnotCount - 2 + 3 * stationaryKnotCount;
|
||||
int expectedRows = 9 * layout.KnotCount - 3 + 3 * stationaryKnotCount;
|
||||
var constraints = new SparseTripletBuilder(expectedRows, layout.VariableCount);
|
||||
var lower = new List<double>(expectedRows);
|
||||
var upper = new List<double>(expectedRows);
|
||||
@@ -80,6 +108,42 @@ public sealed class LongitudinalConstraintBuilder
|
||||
}
|
||||
}
|
||||
|
||||
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 static void AddInitialFeasibilityObjective(LongitudinalPlanningInput input, LongitudinalVariableLayout layout,
|
||||
SparseTripletBuilder hessian, IList<double> linearCost)
|
||||
{
|
||||
double progressScale = 1d;
|
||||
double speedScale = 1d;
|
||||
double accelerationScale = 1d;
|
||||
double jerkScale = 1d;
|
||||
for (int index = 0; index < layout.KnotCount; index++)
|
||||
{
|
||||
AddProjectionSquaredResidual(hessian, linearCost, layout.S(index), input.KnotSchedule.ReferencePathS[index],
|
||||
1d, progressScale);
|
||||
AddProjectionSquaredResidual(hessian, linearCost, layout.U(index),
|
||||
input.KnotSchedule.ReferenceSpeedMetersPerSecond[index], 10d, speedScale);
|
||||
AddProjectionSquaredResidual(hessian, linearCost, layout.A(index), 0d, 1e-3d, accelerationScale);
|
||||
}
|
||||
for (int index = 0; index < layout.KnotCount - 1; index++)
|
||||
AddProjectionSquaredResidual(hessian, linearCost, layout.J(index), 0d, 1e-3d, jerkScale);
|
||||
}
|
||||
|
||||
private static void AddProjectionSquaredResidual(SparseTripletBuilder hessian, IList<double> linearCost,
|
||||
int variable, double reference, double weight, double scale)
|
||||
{
|
||||
if (!IsFinite(reference) || !IsFinite(weight) || weight <= 0d || !IsFinite(scale) || scale <= 0d)
|
||||
throw new ArgumentOutOfRangeException(nameof(reference));
|
||||
double coefficient = 2d * weight / (scale * scale);
|
||||
hessian.Add(variable, variable, coefficient);
|
||||
linearCost[variable] += -coefficient * reference;
|
||||
}
|
||||
|
||||
private static void AddVariableBounds(LongitudinalPlanningInput input, PathSpeedLimit speedLimit,
|
||||
LongitudinalCandidate iterate, LongitudinalVariableLayout layout, double maximumAcceleration,
|
||||
double maximumDeceleration, double maximumJerk, SparseTripletBuilder constraints, IList<double> lower,
|
||||
@@ -92,8 +156,11 @@ public sealed class LongitudinalConstraintBuilder
|
||||
AddSingleVariableRow(constraints, lower, upper, layout.S(index), 0d, input.PathUpperBoundS, ref row);
|
||||
double maximumSpeed = index == 0
|
||||
? input.DirectionMaximumSpeedMetersPerSecond
|
||||
: Math.Min(input.DirectionMaximumSpeedMetersPerSecond, speedLimit.MaximumSpeedAt(iterate.S[index]));
|
||||
: input.DirectionMaximumSpeedMetersPerSecond;
|
||||
AddSingleVariableRow(constraints, lower, upper, layout.U(index), 0d, maximumSpeed, ref row);
|
||||
if (index > 0)
|
||||
AddLinearizedSpeedEnvelopeRow(speedLimit, iterate.S[index], layout.S(index), layout.U(index),
|
||||
constraints, lower, upper, ref row);
|
||||
AddSingleVariableRow(constraints, lower, upper, layout.A(index), -maximumDeceleration, maximumAcceleration,
|
||||
ref row);
|
||||
}
|
||||
@@ -101,6 +168,34 @@ public sealed class LongitudinalConstraintBuilder
|
||||
AddSingleVariableRow(constraints, lower, upper, layout.J(index), -maximumJerk, maximumJerk, ref row);
|
||||
}
|
||||
|
||||
private static void AddLinearizedSpeedEnvelopeRow(PathSpeedLimit speedLimit, double pathS, int pathSVariable,
|
||||
int speedVariable, SparseTripletBuilder constraints, IList<double> lower, IList<double> upper, ref int row)
|
||||
{
|
||||
int segment = FindSpeedEnvelopeSegment(speedLimit, pathS);
|
||||
double startS = speedLimit.PathS[segment];
|
||||
double endS = speedLimit.PathS[segment + 1];
|
||||
double startSpeed = speedLimit.MaximumSpeedMetersPerSecond[segment];
|
||||
double endSpeed = speedLimit.MaximumSpeedMetersPerSecond[segment + 1];
|
||||
double slope = (endSpeed - startSpeed) / (endS - startS);
|
||||
double intercept = startSpeed - slope * startS;
|
||||
AddRow(constraints, lower, upper, row, new[]
|
||||
{
|
||||
new Coefficient(speedVariable, 1d), new Coefficient(pathSVariable, -slope),
|
||||
}, -QuadraticProgram.MaximumFiniteBound, intercept);
|
||||
row++;
|
||||
}
|
||||
|
||||
private static int FindSpeedEnvelopeSegment(PathSpeedLimit speedLimit, double pathS)
|
||||
{
|
||||
double clamped = Math.Max(speedLimit.PathS[0], Math.Min(speedLimit.PathUpperBoundS, pathS));
|
||||
for (int index = 0; index < speedLimit.PathS.Count - 1; index++)
|
||||
{
|
||||
if (clamped <= speedLimit.PathS[index + 1])
|
||||
return index;
|
||||
}
|
||||
return speedLimit.PathS.Count - 2;
|
||||
}
|
||||
|
||||
private static void AddMonotonicProgress(LongitudinalVariableLayout layout, SparseTripletBuilder constraints,
|
||||
IList<double> lower, IList<double> upper, ref int row)
|
||||
{
|
||||
@@ -164,6 +259,24 @@ public sealed class LongitudinalConstraintBuilder
|
||||
}
|
||||
}
|
||||
|
||||
private static int GetStabilizationStart(LongitudinalPlanningInput input, IReadOnlyList<double> times,
|
||||
int knotCount)
|
||||
{
|
||||
if (input.Mode != EmLongitudinalMode.ExactStopAtBoundary)
|
||||
return knotCount;
|
||||
if (input.PlanningScope == EmPlanningScope.FullDirectionSegment)
|
||||
{
|
||||
if (input.KnotSchedule.TerminalHoldStartIndex < 1 ||
|
||||
input.KnotSchedule.TerminalHoldStartIndex >= knotCount)
|
||||
{
|
||||
throw new ArgumentException("Full-direction exact-stop schedules require an explicit terminal hold boundary.");
|
||||
}
|
||||
return input.KnotSchedule.TerminalHoldStartIndex;
|
||||
}
|
||||
return LongitudinalTerminalSchedule.GetStabilizationStartIndex(times,
|
||||
input.Configuration.Scheduling.OutputTimeStepSeconds);
|
||||
}
|
||||
|
||||
private static void AddSingleVariableRow(SparseTripletBuilder constraints, IList<double> lower, IList<double> upper,
|
||||
int variable, double minimum, double maximum, ref int row)
|
||||
{
|
||||
@@ -192,6 +305,11 @@ public sealed class LongitudinalConstraintBuilder
|
||||
return true;
|
||||
}
|
||||
|
||||
private static bool IsFinite(double value)
|
||||
{
|
||||
return !double.IsNaN(value) && !double.IsInfinity(value);
|
||||
}
|
||||
|
||||
private readonly struct Coefficient
|
||||
{
|
||||
public Coefficient(int variable, double value)
|
||||
|
||||
@@ -0,0 +1,118 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using System.Collections.ObjectModel;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
|
||||
|
||||
/// <summary>Immutable ST optimization knots, separate from the trajectory publication cadence.</summary>
|
||||
public sealed class LongitudinalKnotSchedule
|
||||
{
|
||||
public LongitudinalKnotSchedule(IReadOnlyList<double> knotTimes, IReadOnlyList<double> referencePathS,
|
||||
IReadOnlyList<double> referenceSpeedMetersPerSecond, bool isAdaptive)
|
||||
: this(knotTimes, referencePathS, referenceSpeedMetersPerSecond, isAdaptive, -1)
|
||||
{
|
||||
}
|
||||
|
||||
public LongitudinalKnotSchedule(IReadOnlyList<double> knotTimes, IReadOnlyList<double> referencePathS,
|
||||
IReadOnlyList<double> referenceSpeedMetersPerSecond, bool isAdaptive,
|
||||
int terminalHoldStartIndex)
|
||||
{
|
||||
KnotTimes = CopyTimes(knotTimes);
|
||||
ReferencePathS = CopyNondecreasing(referencePathS, KnotTimes.Count, nameof(referencePathS));
|
||||
ReferenceSpeedMetersPerSecond = CopyNonnegative(referenceSpeedMetersPerSecond, KnotTimes.Count,
|
||||
nameof(referenceSpeedMetersPerSecond));
|
||||
if (isAdaptive && ReferenceSpeedMetersPerSecond[ReferenceSpeedMetersPerSecond.Count - 1] != 0d)
|
||||
throw new ArgumentException("An adaptive full-segment schedule must end at exact zero speed.",
|
||||
nameof(referenceSpeedMetersPerSecond));
|
||||
|
||||
IsAdaptive = isAdaptive;
|
||||
TotalDurationSeconds = KnotTimes[KnotTimes.Count - 1];
|
||||
if (terminalHoldStartIndex != -1 &&
|
||||
(!isAdaptive || terminalHoldStartIndex < 1 || terminalHoldStartIndex >= KnotTimes.Count))
|
||||
{
|
||||
throw new ArgumentOutOfRangeException(nameof(terminalHoldStartIndex));
|
||||
}
|
||||
TerminalHoldStartIndex = terminalHoldStartIndex;
|
||||
}
|
||||
|
||||
public IReadOnlyList<double> KnotTimes { get; }
|
||||
public IReadOnlyList<double> ReferencePathS { get; }
|
||||
public IReadOnlyList<double> ReferenceSpeedMetersPerSecond { get; }
|
||||
public double TotalDurationSeconds { get; }
|
||||
public bool IsAdaptive { get; }
|
||||
public int TerminalHoldStartIndex { get; }
|
||||
|
||||
internal static LongitudinalKnotSchedule CreateAdaptive(IReadOnlyList<double> knotTimes,
|
||||
IReadOnlyList<double> referencePathS, IReadOnlyList<double> referenceSpeedMetersPerSecond,
|
||||
int terminalHoldStartIndex)
|
||||
{
|
||||
return new LongitudinalKnotSchedule(knotTimes, referencePathS, referenceSpeedMetersPerSecond, true,
|
||||
terminalHoldStartIndex);
|
||||
}
|
||||
|
||||
internal LongitudinalKnotSchedule Copy()
|
||||
{
|
||||
return new LongitudinalKnotSchedule(KnotTimes, ReferencePathS, ReferenceSpeedMetersPerSecond, IsAdaptive,
|
||||
TerminalHoldStartIndex);
|
||||
}
|
||||
|
||||
public static LongitudinalKnotSchedule CreateRolling(double timeHorizonSeconds, double timeStepSeconds)
|
||||
{
|
||||
IReadOnlyList<double> times = LongitudinalCandidate.CreateKnotTimes(timeHorizonSeconds, timeStepSeconds);
|
||||
var pathS = new double[times.Count];
|
||||
var speeds = new double[times.Count];
|
||||
return new LongitudinalKnotSchedule(times, pathS, speeds, false);
|
||||
}
|
||||
|
||||
private static IReadOnlyList<double> CopyTimes(IReadOnlyList<double> source)
|
||||
{
|
||||
if (source == null || source.Count < 2)
|
||||
throw new ArgumentException("At least two time knots are required.", nameof(source));
|
||||
var copy = new List<double>(source.Count);
|
||||
double previous = double.NegativeInfinity;
|
||||
for (int index = 0; index < source.Count; index++)
|
||||
{
|
||||
if (!IsFinite(source[index]) || source[index] <= previous || (index == 0 && source[index] != 0d))
|
||||
throw new ArgumentException("Time knots must be finite, begin at exact zero, and strictly increase.",
|
||||
nameof(source));
|
||||
copy.Add(source[index]);
|
||||
previous = source[index];
|
||||
}
|
||||
return new ReadOnlyCollection<double>(copy);
|
||||
}
|
||||
|
||||
private static IReadOnlyList<double> CopyNondecreasing(IReadOnlyList<double> source, int expectedCount,
|
||||
string parameterName)
|
||||
{
|
||||
if (source == null || source.Count != expectedCount || source[0] != 0d)
|
||||
throw new ArgumentException("Reference PathS must begin at exact zero and match the knot count.", parameterName);
|
||||
var copy = new List<double>(source.Count);
|
||||
double previous = double.NegativeInfinity;
|
||||
for (int index = 0; index < source.Count; index++)
|
||||
{
|
||||
if (!IsFinite(source[index]) || source[index] < previous)
|
||||
throw new ArgumentException("Reference PathS must be finite and nondecreasing.", parameterName);
|
||||
copy.Add(source[index]);
|
||||
previous = source[index];
|
||||
}
|
||||
return new ReadOnlyCollection<double>(copy);
|
||||
}
|
||||
|
||||
private static IReadOnlyList<double> CopyNonnegative(IReadOnlyList<double> source, int expectedCount,
|
||||
string parameterName)
|
||||
{
|
||||
if (source == null || source.Count != expectedCount)
|
||||
throw new ArgumentException("Reference speeds must match the knot count.", parameterName);
|
||||
var copy = new List<double>(source.Count);
|
||||
for (int index = 0; index < source.Count; index++)
|
||||
{
|
||||
if (!IsFinite(source[index]) || source[index] < 0d)
|
||||
throw new ArgumentOutOfRangeException(parameterName);
|
||||
copy.Add(source[index]);
|
||||
}
|
||||
return new ReadOnlyCollection<double>(copy);
|
||||
}
|
||||
|
||||
private static bool IsFinite(double value) => !double.IsNaN(value) && !double.IsInfinity(value);
|
||||
|
||||
}
|
||||
@@ -14,6 +14,16 @@ public sealed class LongitudinalPlanningInput
|
||||
double initialAccelerationMetersPerSecondSquared, EmTerminalType terminalType, EmLongitudinalMode mode,
|
||||
EmPlannerConfiguration configuration,
|
||||
IReadOnlyList<double> previousPathS, IReadOnlyList<double> previousProgressSpeedMetersPerSecond)
|
||||
: this(path, direction, initialProgressSpeedMetersPerSecond, initialAccelerationMetersPerSecondSquared,
|
||||
terminalType, mode, configuration, EmPlanningScope.RollingHorizon,
|
||||
CreateRollingSchedule(configuration), previousPathS, previousProgressSpeedMetersPerSecond)
|
||||
{
|
||||
}
|
||||
|
||||
public LongitudinalPlanningInput(LateralPath path, TravelDirection direction, double initialProgressSpeedMetersPerSecond,
|
||||
double initialAccelerationMetersPerSecondSquared, EmTerminalType terminalType, EmLongitudinalMode mode,
|
||||
EmPlannerConfiguration configuration, EmPlanningScope planningScope, LongitudinalKnotSchedule knotSchedule,
|
||||
IReadOnlyList<double> previousPathS, IReadOnlyList<double> previousProgressSpeedMetersPerSecond)
|
||||
{
|
||||
if (path == null || !path.IsIndependentlyValidated || path.Points.Count < 2)
|
||||
throw new ArgumentException("Longitudinal planning requires an independently validated lateral path with at least two points.",
|
||||
@@ -34,6 +44,13 @@ public sealed class LongitudinalPlanningInput
|
||||
throw new ArgumentException("Stop-boundary modes require Goal or GearSwitch.");
|
||||
if (configuration == null)
|
||||
throw new ArgumentNullException(nameof(configuration));
|
||||
if (!Enum.IsDefined(typeof(EmPlanningScope), planningScope))
|
||||
throw new ArgumentOutOfRangeException(nameof(planningScope));
|
||||
if (knotSchedule == null)
|
||||
throw new ArgumentNullException(nameof(knotSchedule));
|
||||
if ((planningScope == EmPlanningScope.FullDirectionSegment) != knotSchedule.IsAdaptive)
|
||||
throw new ArgumentException("Full-direction planning requires an adaptive schedule and rolling planning requires a rolling schedule.",
|
||||
nameof(knotSchedule));
|
||||
|
||||
Path = CopyAndValidatePath(path);
|
||||
Direction = direction;
|
||||
@@ -42,6 +59,8 @@ public sealed class LongitudinalPlanningInput
|
||||
TerminalType = terminalType;
|
||||
Mode = mode;
|
||||
Configuration = configuration.Copy();
|
||||
PlanningScope = planningScope;
|
||||
KnotSchedule = knotSchedule.Copy();
|
||||
PreviousPathS = CopyFiniteNonnegative(previousPathS, nameof(previousPathS));
|
||||
PreviousProgressSpeedMetersPerSecond = CopyFiniteNonnegative(previousProgressSpeedMetersPerSecond,
|
||||
nameof(previousProgressSpeedMetersPerSecond));
|
||||
@@ -75,6 +94,10 @@ public sealed class LongitudinalPlanningInput
|
||||
|
||||
public EmPlannerConfiguration Configuration { get; }
|
||||
|
||||
public EmPlanningScope PlanningScope { get; }
|
||||
|
||||
public LongitudinalKnotSchedule KnotSchedule { get; }
|
||||
|
||||
public IReadOnlyList<double> PreviousPathS { get; }
|
||||
|
||||
public IReadOnlyList<double> PreviousProgressSpeedMetersPerSecond { get; }
|
||||
@@ -143,6 +166,14 @@ public sealed class LongitudinalPlanningInput
|
||||
return new ReadOnlyCollection<double>(copy);
|
||||
}
|
||||
|
||||
private static LongitudinalKnotSchedule CreateRollingSchedule(EmPlannerConfiguration configuration)
|
||||
{
|
||||
if (configuration == null || configuration.Scheduling == null)
|
||||
throw new ArgumentNullException(nameof(configuration));
|
||||
return LongitudinalKnotSchedule.CreateRolling(configuration.Scheduling.TimeHorizonSeconds,
|
||||
configuration.Scheduling.OutputTimeStepSeconds);
|
||||
}
|
||||
|
||||
private static bool IsFinite(double value)
|
||||
{
|
||||
return !double.IsNaN(value) && !double.IsInfinity(value);
|
||||
|
||||
+9
@@ -55,6 +55,15 @@ public sealed class LongitudinalPreviousTrajectorySeedBuilder
|
||||
{
|
||||
private const double ProjectionTolerance = 1e-10d;
|
||||
|
||||
public LongitudinalPreviousTrajectorySeed Build(EmTrajectory previous, LateralPath currentPath,
|
||||
DateTimeOffset newEffectiveAtUtc, LongitudinalKnotSchedule knotSchedule, int segmentIndex,
|
||||
TravelDirection direction)
|
||||
{
|
||||
if (knotSchedule == null)
|
||||
return LongitudinalPreviousTrajectorySeed.Empty;
|
||||
return Build(previous, currentPath, newEffectiveAtUtc, knotSchedule.KnotTimes, segmentIndex, direction);
|
||||
}
|
||||
|
||||
public LongitudinalPreviousTrajectorySeed Build(EmTrajectory previous, LateralPath currentPath,
|
||||
DateTimeOffset newEffectiveAtUtc, IReadOnlyList<double> newKnotTimes, int segmentIndex,
|
||||
TravelDirection direction)
|
||||
|
||||
+18
-5
@@ -23,12 +23,11 @@ public sealed class LongitudinalSolutionValidator
|
||||
{
|
||||
return false;
|
||||
}
|
||||
IReadOnlyList<double> expectedTimes = LongitudinalCandidate.CreateKnotTimes(
|
||||
input.Configuration.Scheduling.TimeHorizonSeconds, input.Configuration.Scheduling.OutputTimeStepSeconds);
|
||||
IReadOnlyList<double> expectedTimes = input.KnotSchedule.KnotTimes;
|
||||
double tolerance = RequireNonnegative(input.Configuration.Validation.KinematicTolerance, nameof(tolerance));
|
||||
if (!HasMatchingTimes(candidate.KnotTimes, expectedTimes, tolerance))
|
||||
{
|
||||
failureReason = "ST candidate knot times do not match the configured horizon.";
|
||||
failureReason = "ST candidate knot times do not match the supplied knot schedule.";
|
||||
return false;
|
||||
}
|
||||
if (candidate.S.Count != expectedTimes.Count || candidate.U.Count != expectedTimes.Count ||
|
||||
@@ -88,8 +87,7 @@ public sealed class LongitudinalSolutionValidator
|
||||
int stabilizationStart = candidate.S.Count;
|
||||
if (input.Mode == EmLongitudinalMode.ExactStopAtBoundary)
|
||||
{
|
||||
stabilizationStart = LongitudinalTerminalSchedule.GetStabilizationStartIndex(
|
||||
candidate.KnotTimes, input.Configuration.Scheduling.OutputTimeStepSeconds);
|
||||
stabilizationStart = GetStabilizationStart(input, candidate.KnotTimes);
|
||||
for (int index = stabilizationStart; index < candidate.S.Count; index++)
|
||||
{
|
||||
if (!AreClose(candidate.S[index], input.StopBoundaryPathS, tolerance) ||
|
||||
@@ -166,6 +164,21 @@ public sealed class LongitudinalSolutionValidator
|
||||
return true;
|
||||
}
|
||||
|
||||
private static int GetStabilizationStart(LongitudinalPlanningInput input, IReadOnlyList<double> times)
|
||||
{
|
||||
if (input.PlanningScope == EmPlanningScope.FullDirectionSegment)
|
||||
{
|
||||
if (input.KnotSchedule.TerminalHoldStartIndex < 1 ||
|
||||
input.KnotSchedule.TerminalHoldStartIndex >= times.Count)
|
||||
{
|
||||
throw new ArgumentException("Full-direction exact-stop schedules require an explicit terminal hold boundary.");
|
||||
}
|
||||
return input.KnotSchedule.TerminalHoldStartIndex;
|
||||
}
|
||||
return LongitudinalTerminalSchedule.GetStabilizationStartIndex(times,
|
||||
input.Configuration.Scheduling.OutputTimeStepSeconds);
|
||||
}
|
||||
|
||||
private static bool AreClose(double actual, double expected, double tolerance)
|
||||
{
|
||||
return Math.Abs(actual - expected) <= tolerance;
|
||||
|
||||
@@ -1,5 +1,6 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using MultiWheelC.TrajectoryPlanning.CoarsePath;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
|
||||
|
||||
@@ -11,67 +12,112 @@ public sealed class PathSpeedLimitBuilder
|
||||
private const double StationMergeToleranceMeters = 1e-12d;
|
||||
|
||||
public EmPlanningStatus Build(LongitudinalPlanningInput input, out PathSpeedLimit speedLimit, out string failureReason)
|
||||
{
|
||||
if (input == null)
|
||||
{
|
||||
speedLimit = null;
|
||||
failureReason = "Longitudinal planning input is required.";
|
||||
return EmPlanningStatus.InvalidInput;
|
||||
}
|
||||
return BuildCore(input.Path, input.Direction, input.InitialProgressSpeedMetersPerSecond,
|
||||
input.InitialAccelerationMetersPerSecondSquared, input.TerminalType, input.Configuration,
|
||||
out speedLimit, out failureReason);
|
||||
}
|
||||
|
||||
public EmPlanningStatus Build(LateralPath path, TravelDirection direction,
|
||||
double initialProgressSpeedMetersPerSecond, EmTerminalType terminalType,
|
||||
EmPlannerConfiguration configuration, out PathSpeedLimit speedLimit, out string failureReason)
|
||||
{
|
||||
return BuildCore(path, direction, initialProgressSpeedMetersPerSecond, 0d, terminalType, configuration,
|
||||
out speedLimit, out failureReason);
|
||||
}
|
||||
|
||||
private EmPlanningStatus BuildCore(LateralPath path, TravelDirection direction,
|
||||
double initialProgressSpeedMetersPerSecond, double initialAccelerationMetersPerSecondSquared,
|
||||
EmTerminalType terminalType, EmPlannerConfiguration configuration, out PathSpeedLimit speedLimit,
|
||||
out string failureReason)
|
||||
{
|
||||
speedLimit = null;
|
||||
failureReason = string.Empty;
|
||||
if (input == null)
|
||||
if (path == null || !path.IsIndependentlyValidated || path.Points.Count < 2 ||
|
||||
!Enum.IsDefined(typeof(TravelDirection), direction) || !Enum.IsDefined(typeof(EmTerminalType), terminalType) ||
|
||||
configuration == null || !IsFinite(initialProgressSpeedMetersPerSecond) ||
|
||||
initialProgressSpeedMetersPerSecond < 0d || !IsFinite(initialAccelerationMetersPerSecondSquared))
|
||||
{
|
||||
failureReason = "Longitudinal planning input is required.";
|
||||
return EmPlanningStatus.InvalidInput;
|
||||
}
|
||||
|
||||
if (!TryGetLimits(input, out double directionMaximum, out double maximumAcceleration, out double maximumDeceleration,
|
||||
out double maximumJerk, out double maximumLateralAcceleration, out double maximumCurvatureRate,
|
||||
out failureReason))
|
||||
if (configuration.Longitudinal == null)
|
||||
{
|
||||
failureReason = "Longitudinal configuration is required.";
|
||||
return EmPlanningStatus.InvalidInput;
|
||||
}
|
||||
if (input.InitialProgressSpeedMetersPerSecond > directionMaximum + StopDistanceToleranceMeters ||
|
||||
input.InitialAccelerationMetersPerSecondSquared < -maximumDeceleration - StopDistanceToleranceMeters ||
|
||||
input.InitialAccelerationMetersPerSecondSquared > maximumAcceleration + StopDistanceToleranceMeters)
|
||||
LongitudinalConfiguration longitudinal = configuration.Longitudinal;
|
||||
double directionMaximum = direction == TravelDirection.Forward
|
||||
? longitudinal.MaximumForwardSpeedMetersPerSecond
|
||||
: longitudinal.MaximumReverseSpeedMetersPerSecond;
|
||||
double maximumAcceleration = longitudinal.MaximumAccelerationMetersPerSecondSquared;
|
||||
double maximumDeceleration = longitudinal.MaximumDecelerationMetersPerSecondSquared;
|
||||
double maximumJerk = longitudinal.MaximumJerkMetersPerSecondCubed;
|
||||
double maximumLateralAcceleration = longitudinal.MaximumLateralAccelerationMetersPerSecondSquared;
|
||||
double maximumCurvatureRate = longitudinal.MaximumCurvatureRatePerMeterPerSecond;
|
||||
if (!IsPositiveFinite(directionMaximum) || !IsPositiveFinite(maximumAcceleration) ||
|
||||
!IsPositiveFinite(maximumDeceleration) || !IsPositiveFinite(maximumJerk) ||
|
||||
!IsPositiveFinite(maximumLateralAcceleration) || !IsPositiveFinite(maximumCurvatureRate))
|
||||
{
|
||||
failureReason = "Longitudinal limits must be positive and finite.";
|
||||
return EmPlanningStatus.InvalidInput;
|
||||
}
|
||||
if (initialProgressSpeedMetersPerSecond > directionMaximum + StopDistanceToleranceMeters ||
|
||||
initialAccelerationMetersPerSecondSquared < -maximumDeceleration - StopDistanceToleranceMeters ||
|
||||
initialAccelerationMetersPerSecondSquared > maximumAcceleration + StopDistanceToleranceMeters)
|
||||
{
|
||||
failureReason = "The initial longitudinal state violates the configured hard bounds.";
|
||||
return EmPlanningStatus.InvalidInput;
|
||||
}
|
||||
|
||||
if (input.HasStopBoundary)
|
||||
bool hasStopBoundary = terminalType != EmTerminalType.RollingSafetyStop;
|
||||
double stopBoundaryPathS = path.Points[path.Points.Count - 1].PathS;
|
||||
if (hasStopBoundary)
|
||||
{
|
||||
if (!JerkLimitedStoppingMath.TryCalculate(input.InitialProgressSpeedMetersPerSecond,
|
||||
input.InitialAccelerationMetersPerSecondSquared, maximumDeceleration, maximumJerk,
|
||||
if (!JerkLimitedStoppingMath.TryCalculate(initialProgressSpeedMetersPerSecond,
|
||||
initialAccelerationMetersPerSecondSquared, maximumDeceleration, maximumJerk,
|
||||
out JerkLimitedStoppingProfile stopProfile, out failureReason))
|
||||
{
|
||||
return EmPlanningStatus.InvalidInput;
|
||||
}
|
||||
if (stopProfile.DistanceMeters + StopDistanceToleranceMeters > input.StopBoundaryPathS)
|
||||
if (stopProfile.DistanceMeters + StopDistanceToleranceMeters > stopBoundaryPathS)
|
||||
{
|
||||
failureReason = "The available actual PathS distance is insufficient for the jerk-limited stop.";
|
||||
return EmPlanningStatus.StoppingDistanceInsufficient;
|
||||
}
|
||||
}
|
||||
if (input.Configuration.Scheduling == null || !IsPositiveFinite(input.Configuration.Scheduling.OutputTimeStepSeconds))
|
||||
if (configuration.Scheduling == null ||
|
||||
!IsPositiveFinite(configuration.Scheduling.MaximumOptimizationSpatialStepMeters))
|
||||
{
|
||||
failureReason = "The output time step required to refine the PathS speed envelope is invalid.";
|
||||
failureReason = "The optimization spatial step required to refine the PathS speed envelope is invalid.";
|
||||
return EmPlanningStatus.InvalidInput;
|
||||
}
|
||||
|
||||
double maximumStationSpacing = directionMaximum * input.Configuration.Scheduling.OutputTimeStepSeconds;
|
||||
double maximumStationSpacing = configuration.Scheduling.MaximumOptimizationSpatialStepMeters;
|
||||
var pathS = new List<double>();
|
||||
var maximum = new List<double>();
|
||||
var lateral = new List<double>();
|
||||
var curvatureRate = new List<double>();
|
||||
var stopping = new List<double>();
|
||||
for (int segmentIndex = 0; segmentIndex < input.Path.Points.Count - 1; segmentIndex++)
|
||||
for (int segmentIndex = 0; segmentIndex < path.Points.Count - 1; segmentIndex++)
|
||||
{
|
||||
LateralPathPoint lowerPoint = input.Path.Points[segmentIndex];
|
||||
LateralPathPoint upperPoint = input.Path.Points[segmentIndex + 1];
|
||||
LateralPathPoint lowerPoint = path.Points[segmentIndex];
|
||||
LateralPathPoint upperPoint = path.Points[segmentIndex + 1];
|
||||
double span = upperPoint.PathS - lowerPoint.PathS;
|
||||
int subdivisions = Math.Max(1, checked((int)Math.Ceiling(span / maximumStationSpacing)));
|
||||
var segmentStations = new List<double>(subdivisions + 16);
|
||||
for (int subdivision = segmentIndex == 0 ? 0 : 1; subdivision <= subdivisions; subdivision++)
|
||||
segmentStations.Add(Interpolate(lowerPoint.PathS, upperPoint.PathS, (double)subdivision / subdivisions));
|
||||
if (input.HasStopBoundary)
|
||||
if (hasStopBoundary)
|
||||
{
|
||||
AddJerkLimitedStoppingStations(lowerPoint.PathS, upperPoint.PathS, input.StopBoundaryPathS,
|
||||
AddJerkLimitedStoppingStations(lowerPoint.PathS, upperPoint.PathS, stopBoundaryPathS,
|
||||
directionMaximum, maximumAcceleration, maximumDeceleration, maximumJerk,
|
||||
segmentIndex == 0, segmentStations);
|
||||
}
|
||||
@@ -87,8 +133,8 @@ public sealed class PathSpeedLimitBuilder
|
||||
double curvature = Interpolate(lowerPoint.VehicleCurvature, upperPoint.VehicleCurvature, fraction);
|
||||
double curvatureDerivative = Interpolate(lowerPoint.VehicleCurvatureDerivative,
|
||||
upperPoint.VehicleCurvatureDerivative, fraction);
|
||||
AddLimitSample(samplePathS, curvature, curvatureDerivative, input.HasStopBoundary,
|
||||
input.StopBoundaryPathS, directionMaximum, maximumAcceleration, maximumDeceleration,
|
||||
AddLimitSample(samplePathS, curvature, curvatureDerivative, hasStopBoundary,
|
||||
stopBoundaryPathS, directionMaximum, maximumAcceleration, maximumDeceleration,
|
||||
maximumJerk, maximumLateralAcceleration, maximumCurvatureRate, pathS, maximum, lateral,
|
||||
curvatureRate, stopping);
|
||||
}
|
||||
@@ -97,7 +143,7 @@ public sealed class PathSpeedLimitBuilder
|
||||
try
|
||||
{
|
||||
speedLimit = new PathSpeedLimit(pathS, maximum, lateral, curvatureRate, stopping, directionMaximum,
|
||||
input.HasStopBoundary);
|
||||
hasStopBoundary);
|
||||
return EmPlanningStatus.Success;
|
||||
}
|
||||
catch (ArgumentException exception)
|
||||
|
||||
+211
-13
@@ -47,18 +47,40 @@ public sealed class SequentialLongitudinalOptimizer
|
||||
if (speedStatus != EmPlanningStatus.Success)
|
||||
return Failed(speedStatus, speedFailure);
|
||||
|
||||
LongitudinalCandidate iterate = CreateInitialIterate(input, speedLimit);
|
||||
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);
|
||||
LongitudinalCandidate lastStrictCandidate;
|
||||
if (!_solutionValidator.TryValidate(input, speedLimit, iterate, out lastStrictCandidate, out _))
|
||||
lastStrictCandidate = null;
|
||||
string lastCandidateRejection = string.Empty;
|
||||
bool hasPreviousObjective = false;
|
||||
double previousObjective = 0d;
|
||||
var stopwatch = Stopwatch.StartNew();
|
||||
|
||||
for (int iteration = 0; iteration < iterationLimit; iteration++)
|
||||
for (int iteration = 0; iteration < remainingObjectiveIterations; iteration++)
|
||||
{
|
||||
if (cancellationToken.IsCancellationRequested)
|
||||
return FallbackOrFailure(lastStrictCandidate, EmPlanningStatus.Cancelled, "Longitudinal optimization was cancelled.");
|
||||
@@ -191,10 +213,132 @@ public sealed class SequentialLongitudinalOptimizer
|
||||
}
|
||||
}
|
||||
|
||||
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 = LongitudinalCandidate.CreateKnotTimes(input.Configuration.Scheduling.TimeHorizonSeconds,
|
||||
input.Configuration.Scheduling.OutputTimeStepSeconds);
|
||||
IReadOnlyList<double> times = input.KnotSchedule.KnotTimes;
|
||||
switch (input.Mode)
|
||||
{
|
||||
case EmLongitudinalMode.RollingContinuation:
|
||||
@@ -269,6 +413,10 @@ public sealed class SequentialLongitudinalOptimizer
|
||||
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];
|
||||
@@ -364,7 +512,7 @@ public sealed class SequentialLongitudinalOptimizer
|
||||
for (int index = 0; index < motionTimes.Length; index++)
|
||||
motionTimes[index] = times[index];
|
||||
|
||||
LongitudinalCandidate baseline = CreateApproachSeed(input, motionTimes, speedLimit);
|
||||
LongitudinalCandidate baseline = CreateScheduleReferenceSeed(input, motionTimes, speedLimit);
|
||||
var influence = new double[3, intervalCount];
|
||||
for (int interval = 0; interval < intervalCount; interval++)
|
||||
{
|
||||
@@ -461,6 +609,30 @@ public sealed class SequentialLongitudinalOptimizer
|
||||
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);
|
||||
@@ -679,10 +851,12 @@ public sealed class SequentialLongitudinalOptimizer
|
||||
nextIterate = null;
|
||||
if (candidate.S.Count != previous.S.Count)
|
||||
return false;
|
||||
int stabilizationStart = input.Mode == EmLongitudinalMode.ExactStopAtBoundary
|
||||
? LongitudinalTerminalSchedule.GetStabilizationStartIndex(candidate.KnotTimes,
|
||||
input.Configuration.Scheduling.OutputTimeStepSeconds)
|
||||
: candidate.S.Count;
|
||||
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;
|
||||
@@ -737,6 +911,30 @@ public sealed class SequentialLongitudinalOptimizer
|
||||
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 &&
|
||||
|
||||
@@ -39,8 +39,9 @@ public sealed class EmTrajectoryAssembler
|
||||
throw new ArgumentNullException(nameof(metadata));
|
||||
|
||||
var interpolator = new LateralPathInterpolator(path);
|
||||
var schedule = new TrajectorySampleSchedule(longitudinal.Candidate, outputTimeStepSeconds, zeroSpeedHoldSeconds,
|
||||
metadata.LongitudinalMode);
|
||||
bool isFullDirectionSegment = metadata.PlanningScope == EmPlanningScope.FullDirectionSegment;
|
||||
var schedule = new TrajectorySampleSchedule(longitudinal.Candidate, outputTimeStepSeconds,
|
||||
isFullDirectionSegment ? 0d : zeroSpeedHoldSeconds, metadata.LongitudinalMode, isFullDirectionSegment);
|
||||
double terminalPathS = path.Points[path.Points.Count - 1].PathS;
|
||||
var points = new List<EmTrajectoryPoint>(schedule.Samples.Count);
|
||||
double directionSign = metadata.Direction == TravelDirection.Forward ? 1d : -1d;
|
||||
|
||||
@@ -9,7 +9,7 @@ internal sealed class TrajectorySampleSchedule
|
||||
private const double ZeroTolerance = 1e-12d;
|
||||
|
||||
public TrajectorySampleSchedule(LongitudinalCandidate candidate, double outputTimeStepSeconds, double holdDurationSeconds,
|
||||
EmLongitudinalMode mode)
|
||||
EmLongitudinalMode mode, bool resampleMotion)
|
||||
{
|
||||
if (candidate == null)
|
||||
throw new ArgumentNullException(nameof(candidate));
|
||||
@@ -22,16 +22,25 @@ internal sealed class TrajectorySampleSchedule
|
||||
|
||||
var samples = new List<TrajectorySample>(candidate.KnotTimes.Count + 4);
|
||||
double previousPathS = double.NegativeInfinity;
|
||||
for (int index = 0; index < candidate.KnotTimes.Count; index++)
|
||||
if (resampleMotion)
|
||||
{
|
||||
if (candidate.S[index] < previousPathS)
|
||||
throw new ArgumentException("Trajectory PathS cannot decrease.", nameof(candidate));
|
||||
if (candidate.U[index] < -ZeroTolerance)
|
||||
throw new ArgumentException("Longitudinal progress speed cannot be negative.", nameof(candidate));
|
||||
|
||||
samples.Add(new TrajectorySample(candidate.KnotTimes[index], candidate.S[index], Math.Max(0d, candidate.U[index]),
|
||||
candidate.A[index], index < candidate.J.Count ? candidate.J[index] : 0d, false));
|
||||
previousPathS = candidate.S[index];
|
||||
double finalTime = candidate.KnotTimes[candidate.KnotTimes.Count - 1];
|
||||
int sourceInterval = 0;
|
||||
for (double sampleTime = 0d; sampleTime < finalTime - ZeroTolerance;
|
||||
sampleTime += outputTimeStepSeconds)
|
||||
{
|
||||
AddSample(Interpolate(candidate, sampleTime, ref sourceInterval), samples, ref previousPathS, candidate);
|
||||
}
|
||||
AddSample(Interpolate(candidate, finalTime, ref sourceInterval), samples, ref previousPathS, candidate);
|
||||
}
|
||||
else
|
||||
{
|
||||
for (int index = 0; index < candidate.KnotTimes.Count; index++)
|
||||
{
|
||||
AddSample(new TrajectorySample(candidate.KnotTimes[index], candidate.S[index],
|
||||
Math.Max(0d, candidate.U[index]), candidate.A[index], index < candidate.J.Count ? candidate.J[index] : 0d,
|
||||
false), samples, ref previousPathS, candidate);
|
||||
}
|
||||
}
|
||||
|
||||
if (mode != EmLongitudinalMode.ExactStopAtBoundary)
|
||||
@@ -41,14 +50,22 @@ internal sealed class TrajectorySampleSchedule
|
||||
return;
|
||||
}
|
||||
|
||||
int stabilizationStart = LongitudinalTerminalSchedule.GetStabilizationStartIndex(candidate.KnotTimes,
|
||||
outputTimeStepSeconds);
|
||||
double stopPathS = candidate.S[stabilizationStart];
|
||||
for (int index = stabilizationStart; index < candidate.S.Count; index++)
|
||||
int sourceStabilizationStart = resampleMotion
|
||||
? FindTerminalStationaryTailStart(candidate)
|
||||
: LongitudinalTerminalSchedule.GetStabilizationStartIndex(candidate.KnotTimes, outputTimeStepSeconds);
|
||||
double stabilizationStartTime = candidate.KnotTimes[sourceStabilizationStart];
|
||||
int stabilizationStart = 0;
|
||||
while (stabilizationStart < samples.Count - 1 &&
|
||||
samples[stabilizationStart].TimeFromStart < stabilizationStartTime - ZeroTolerance)
|
||||
{
|
||||
if (Math.Abs(candidate.S[index] - stopPathS) > ZeroTolerance ||
|
||||
Math.Abs(candidate.U[index]) > ZeroTolerance || Math.Abs(candidate.A[index]) > ZeroTolerance ||
|
||||
(index < candidate.J.Count && Math.Abs(candidate.J[index]) > ZeroTolerance))
|
||||
stabilizationStart++;
|
||||
}
|
||||
double stopPathS = samples[stabilizationStart].PathS;
|
||||
for (int index = stabilizationStart; index < samples.Count; index++)
|
||||
{
|
||||
if (Math.Abs(samples[index].PathS - stopPathS) > ZeroTolerance ||
|
||||
Math.Abs(samples[index].ProgressSpeed) > ZeroTolerance || Math.Abs(samples[index].Acceleration) > ZeroTolerance ||
|
||||
Math.Abs(samples[index].Jerk) > ZeroTolerance)
|
||||
{
|
||||
throw new ArgumentException("An exact stop requires a stationary S/U/A/J tail.", nameof(candidate));
|
||||
}
|
||||
@@ -69,6 +86,57 @@ internal sealed class TrajectorySampleSchedule
|
||||
public IReadOnlyList<TrajectorySample> Samples { get; }
|
||||
public int TerminalAnchorSampleIndex { get; }
|
||||
|
||||
private static void AddSample(TrajectorySample sample, ICollection<TrajectorySample> samples,
|
||||
ref double previousPathS, LongitudinalCandidate candidate)
|
||||
{
|
||||
if (sample.PathS < previousPathS)
|
||||
throw new ArgumentException("Trajectory PathS cannot decrease.", nameof(candidate));
|
||||
if (sample.ProgressSpeed < -ZeroTolerance)
|
||||
throw new ArgumentException("Longitudinal progress speed cannot be negative.", nameof(candidate));
|
||||
samples.Add(sample);
|
||||
previousPathS = sample.PathS;
|
||||
}
|
||||
|
||||
private static TrajectorySample Interpolate(LongitudinalCandidate candidate, double sampleTime, ref int sourceInterval)
|
||||
{
|
||||
int lastKnot = candidate.KnotTimes.Count - 1;
|
||||
if (sampleTime >= candidate.KnotTimes[lastKnot] - ZeroTolerance)
|
||||
{
|
||||
return new TrajectorySample(candidate.KnotTimes[lastKnot], candidate.S[lastKnot],
|
||||
Math.Max(0d, candidate.U[lastKnot]), candidate.A[lastKnot], 0d, false);
|
||||
}
|
||||
while (sourceInterval < lastKnot - 1 &&
|
||||
sampleTime >= candidate.KnotTimes[sourceInterval + 1] - ZeroTolerance)
|
||||
{
|
||||
sourceInterval++;
|
||||
}
|
||||
if (Math.Abs(sampleTime - candidate.KnotTimes[sourceInterval]) <= ZeroTolerance)
|
||||
{
|
||||
return new TrajectorySample(candidate.KnotTimes[sourceInterval], candidate.S[sourceInterval],
|
||||
Math.Max(0d, candidate.U[sourceInterval]), candidate.A[sourceInterval], candidate.J[sourceInterval], false);
|
||||
}
|
||||
double dt = sampleTime - candidate.KnotTimes[sourceInterval];
|
||||
double jerk = candidate.J[sourceInterval];
|
||||
double acceleration = candidate.A[sourceInterval] + jerk * dt;
|
||||
double speed = candidate.U[sourceInterval] + candidate.A[sourceInterval] * dt + 0.5d * jerk * dt * dt;
|
||||
double pathS = candidate.S[sourceInterval] + candidate.U[sourceInterval] * dt +
|
||||
0.5d * candidate.A[sourceInterval] * dt * dt + jerk * dt * dt * dt / 6d;
|
||||
return new TrajectorySample(sampleTime, pathS, Math.Max(0d, speed), acceleration, jerk, false);
|
||||
}
|
||||
|
||||
private static int FindTerminalStationaryTailStart(LongitudinalCandidate candidate)
|
||||
{
|
||||
int start = candidate.KnotTimes.Count - 1;
|
||||
double terminalPathS = candidate.S[start];
|
||||
while (start > 0 && Math.Abs(candidate.S[start - 1] - terminalPathS) <= ZeroTolerance &&
|
||||
Math.Abs(candidate.U[start - 1]) <= ZeroTolerance && Math.Abs(candidate.A[start - 1]) <= ZeroTolerance &&
|
||||
Math.Abs(candidate.J[start - 1]) <= ZeroTolerance)
|
||||
{
|
||||
start--;
|
||||
}
|
||||
return start;
|
||||
}
|
||||
|
||||
private static bool IsFinite(double value)
|
||||
{
|
||||
return !double.IsNaN(value) && !double.IsInfinity(value);
|
||||
|
||||
@@ -109,7 +109,9 @@ internal static class EmPlanningServiceChecks
|
||||
CreateReferencePath(TravelDirection.Forward, false, 0.0075d), null,
|
||||
EmPlanningScope.FullDirectionSegment);
|
||||
ConfigureExactStopServiceScenario(request.Configuration);
|
||||
EmPlanningResult result = new EmPlanningService(new ScriptedPipelineSolver(PipelineSolverMode.Success)).Plan(
|
||||
double[] strictFullPrimal = CreateStrictFullScopePrimal(request.Configuration);
|
||||
EmPlanningResult result = new EmPlanningService(new ScriptedPipelineSolver(PipelineSolverMode.Success, null,
|
||||
strictFullPrimal)).Plan(
|
||||
request, CancellationToken.None);
|
||||
VerifySuccess(result, request, EmTerminalType.Goal, "full scope publication");
|
||||
Verification.Equal(EmPlanningScope.FullDirectionSegment, result.Trajectory.Metadata.PlanningScope,
|
||||
@@ -127,9 +129,9 @@ internal static class EmPlanningServiceChecks
|
||||
configuration.Longitudinal.MaximumForwardSpeedMetersPerSecond = 0.2d;
|
||||
}
|
||||
|
||||
private static void ConfigureExactStopServiceScenario(EmPlannerConfiguration configuration)
|
||||
{
|
||||
configuration.Scheduling.TimeHorizonSeconds = 0.40d;
|
||||
private static void ConfigureExactStopServiceScenario(EmPlannerConfiguration configuration)
|
||||
{
|
||||
configuration.Scheduling.TimeHorizonSeconds = 0.40d;
|
||||
configuration.Scheduling.OutputTimeStepSeconds = 0.10d;
|
||||
configuration.Longitudinal.MaximumForwardSpeedMetersPerSecond = 1d;
|
||||
configuration.Longitudinal.MaximumReverseSpeedMetersPerSecond = 1d;
|
||||
@@ -137,8 +139,171 @@ internal static class EmPlanningServiceChecks
|
||||
configuration.Longitudinal.MaximumDecelerationMetersPerSecondSquared = 1d;
|
||||
configuration.Longitudinal.MaximumJerkMetersPerSecondCubed = 20d;
|
||||
configuration.Longitudinal.MaximumLateralAccelerationMetersPerSecondSquared = 1d;
|
||||
configuration.Longitudinal.MaximumCurvatureRatePerMeterPerSecond = 1d;
|
||||
}
|
||||
configuration.Longitudinal.MaximumCurvatureRatePerMeterPerSecond = 1d;
|
||||
}
|
||||
|
||||
private static double[] CreateStrictFullScopePrimal(EmPlannerConfiguration configuration)
|
||||
{
|
||||
var path = new LateralPath(new[]
|
||||
{
|
||||
new LateralPathPoint(0d, 0d, 0d, 0d, 0d, 0d, 0d, 0d, 0d, 0d, 0d, 0d),
|
||||
new LateralPathPoint(0.0075d, 0.0075d, 0d, 0d, 0d, 0d, 0.0075d, 0d, 0d, 0d, 0d, 0d),
|
||||
}, true);
|
||||
EmPlanningStatus status = new PathSpeedLimitBuilder().Build(path, TravelDirection.Forward, 0.05d,
|
||||
EmTerminalType.Goal, configuration, out PathSpeedLimit speedLimit, out string failureReason);
|
||||
Verification.Equal(EmPlanningStatus.Success, status, "full scope test envelope: " + failureReason);
|
||||
status = new FullDirectionSegmentScheduleBuilder().TryBuild(path, speedLimit, 0.05d, 0d,
|
||||
configuration.Longitudinal.DesiredForwardSpeedMetersPerSecond, configuration,
|
||||
out LongitudinalKnotSchedule schedule, out failureReason);
|
||||
Verification.Equal(EmPlanningStatus.Success, status, "full scope test schedule: " + failureReason);
|
||||
var input = new LongitudinalPlanningInput(path, TravelDirection.Forward, 0.05d, 0d,
|
||||
EmTerminalType.Goal, EmLongitudinalMode.ExactStopAtBoundary, configuration,
|
||||
EmPlanningScope.FullDirectionSegment, schedule, Array.Empty<double>(), Array.Empty<double>());
|
||||
int motionIntervalCount = schedule.TerminalHoldStartIndex;
|
||||
Verification.True(motionIntervalCount >= 3, "full scope test schedule has three motion intervals");
|
||||
int terminalFirstInterval = motionIntervalCount - 3;
|
||||
var terminalTimes = new double[4];
|
||||
for (int index = 1; index < terminalTimes.Length; index++)
|
||||
terminalTimes[index] = terminalTimes[index - 1] +
|
||||
schedule.KnotTimes[terminalFirstInterval + index] -
|
||||
schedule.KnotTimes[terminalFirstInterval + index - 1];
|
||||
var motionTimes = new double[motionIntervalCount + 1];
|
||||
for (int index = 0; index < motionTimes.Length; index++)
|
||||
motionTimes[index] = schedule.KnotTimes[index];
|
||||
var influence = new double[3, 3];
|
||||
for (int interval = 0; interval < 3; interval++)
|
||||
{
|
||||
var basis = new double[3];
|
||||
basis[interval] = 1d;
|
||||
LongitudinalCandidate response = LongitudinalCandidate.Integrate(terminalTimes, 0d, 0d, 0d, basis);
|
||||
int terminalIndex = response.S.Count - 1;
|
||||
influence[0, interval] = response.A[terminalIndex];
|
||||
influence[1, interval] = response.U[terminalIndex];
|
||||
influence[2, interval] = response.S[terminalIndex];
|
||||
}
|
||||
var validator = new LongitudinalSolutionValidator();
|
||||
for (int firstJerkStep = -20; firstJerkStep <= 0; firstJerkStep++)
|
||||
{
|
||||
for (int secondJerkStep = terminalFirstInterval >= 2 ? -20 : 0;
|
||||
secondJerkStep <= (terminalFirstInterval >= 2 ? 20 : 0); secondJerkStep++)
|
||||
{
|
||||
for (int thirdJerkStep = terminalFirstInterval >= 3 ? -20 : 0;
|
||||
thirdJerkStep <= (terminalFirstInterval >= 3 ? 20 : 0); thirdJerkStep++)
|
||||
{
|
||||
var jerk = new double[motionIntervalCount];
|
||||
jerk[0] = firstJerkStep;
|
||||
if (terminalFirstInterval >= 2)
|
||||
jerk[1] = secondJerkStep;
|
||||
if (terminalFirstInterval >= 3)
|
||||
jerk[2] = thirdJerkStep;
|
||||
LongitudinalCandidate baseline = LongitudinalCandidate.Integrate(motionTimes, 0d, 0.05d,
|
||||
0d, jerk);
|
||||
double[] target =
|
||||
{
|
||||
-baseline.A[baseline.A.Count - 1],
|
||||
-baseline.U[baseline.U.Count - 1],
|
||||
0.0075d - baseline.S[baseline.S.Count - 1],
|
||||
};
|
||||
if (!TrySolveThreeByThree(influence, target, out double[] terminalJerk))
|
||||
throw new InvalidOperationException("Full scope strict candidate terminal system is singular.");
|
||||
for (int interval = 0; interval < 3; interval++)
|
||||
jerk[terminalFirstInterval + interval] = terminalJerk[interval];
|
||||
LongitudinalCandidate motion = LongitudinalCandidate.Integrate(motionTimes, 0d, 0.05d, 0d,
|
||||
jerk);
|
||||
LongitudinalCandidate candidate = AppendFullStopTail(schedule.KnotTimes, motionIntervalCount,
|
||||
motion);
|
||||
if (!validator.TryValidate(input, speedLimit, candidate, out LongitudinalCandidate strict, out _))
|
||||
continue;
|
||||
return ToPrimal(strict);
|
||||
}
|
||||
}
|
||||
}
|
||||
throw new InvalidOperationException("Unable to construct a strict full-scope test candidate: hold=" +
|
||||
motionIntervalCount + ";times=" + string.Join(",", schedule.KnotTimes));
|
||||
}
|
||||
|
||||
private static LongitudinalCandidate AppendFullStopTail(IReadOnlyList<double> times, int motionIntervalCount,
|
||||
LongitudinalCandidate motion)
|
||||
{
|
||||
var pathS = new double[times.Count];
|
||||
var speed = new double[times.Count];
|
||||
var acceleration = new double[times.Count];
|
||||
var jerk = new double[times.Count - 1];
|
||||
for (int index = 0; index <= motionIntervalCount; index++)
|
||||
{
|
||||
pathS[index] = index == motionIntervalCount ? 0.0075d : motion.S[index];
|
||||
speed[index] = index == motionIntervalCount ? 0d : motion.U[index];
|
||||
acceleration[index] = index == motionIntervalCount ? 0d : motion.A[index];
|
||||
}
|
||||
for (int index = motionIntervalCount + 1; index < times.Count; index++)
|
||||
pathS[index] = 0.0075d;
|
||||
for (int index = 0; index < motion.J.Count; index++)
|
||||
jerk[index] = motion.J[index];
|
||||
return new LongitudinalCandidate(times, pathS, speed, acceleration, jerk);
|
||||
}
|
||||
|
||||
private static double[] ToPrimal(LongitudinalCandidate candidate)
|
||||
{
|
||||
var layout = new LongitudinalVariableLayout(candidate.S.Count);
|
||||
var primal = new double[layout.VariableCount];
|
||||
for (int index = 0; index < candidate.S.Count; 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 < candidate.J.Count; index++)
|
||||
primal[layout.J(index)] = candidate.J[index];
|
||||
return primal;
|
||||
}
|
||||
|
||||
private static bool TrySolveThreeByThree(double[,] matrix, IReadOnlyList<double> rightHandSide,
|
||||
out double[] solution)
|
||||
{
|
||||
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 column = 0; column < 3; column++)
|
||||
{
|
||||
int pivot = column;
|
||||
for (int row = column + 1; row < 3; row++)
|
||||
{
|
||||
if (Math.Abs(augmented[row, column]) > Math.Abs(augmented[pivot, column]))
|
||||
pivot = row;
|
||||
}
|
||||
if (Math.Abs(augmented[pivot, column]) < 1e-12d)
|
||||
{
|
||||
solution = Array.Empty<double>();
|
||||
return false;
|
||||
}
|
||||
if (pivot != column)
|
||||
{
|
||||
for (int index = column; index < 4; index++)
|
||||
{
|
||||
double temporary = augmented[column, index];
|
||||
augmented[column, index] = augmented[pivot, index];
|
||||
augmented[pivot, index] = temporary;
|
||||
}
|
||||
}
|
||||
double divisor = augmented[column, column];
|
||||
for (int index = column; index < 4; index++)
|
||||
augmented[column, index] /= divisor;
|
||||
for (int row = 0; row < 3; row++)
|
||||
{
|
||||
if (row == column)
|
||||
continue;
|
||||
double factor = augmented[row, column];
|
||||
for (int index = column; index < 4; index++)
|
||||
augmented[row, index] -= factor * augmented[column, index];
|
||||
}
|
||||
}
|
||||
solution = new[] { augmented[0, 3], augmented[1, 3], augmented[2, 3] };
|
||||
return true;
|
||||
}
|
||||
|
||||
private static void AssertExactStopStabilization(EmTrajectory trajectory, EmBoundaryType boundaryType, string name)
|
||||
{
|
||||
@@ -452,20 +617,23 @@ internal static class EmPlanningServiceChecks
|
||||
{
|
||||
private readonly PipelineSolverMode mode;
|
||||
private readonly PlanningGridMap? mapToCorrupt;
|
||||
private readonly IReadOnlyList<double>? strictFullPrimal;
|
||||
private int longitudinalCallCount;
|
||||
|
||||
public QuadraticProgram? LastLongitudinalProblem { get; private set; }
|
||||
|
||||
public ScriptedPipelineSolver(PipelineSolverMode mode, PlanningGridMap? mapToCorrupt = null)
|
||||
public ScriptedPipelineSolver(PipelineSolverMode mode, PlanningGridMap? mapToCorrupt = null,
|
||||
IReadOnlyList<double>? strictFullPrimal = null)
|
||||
{
|
||||
this.mode = mode;
|
||||
this.mapToCorrupt = mapToCorrupt;
|
||||
this.strictFullPrimal = strictFullPrimal;
|
||||
}
|
||||
|
||||
public QpSolveResult Solve(QuadraticProgram problem, QpSolverSettings settings, IReadOnlyList<double> warmStart,
|
||||
CancellationToken cancellationToken)
|
||||
{
|
||||
bool longitudinal = problem.VariableCount > 100;
|
||||
bool longitudinal = IsLongitudinalProblem(problem);
|
||||
if (mode == PipelineSolverMode.SolverUnavailable)
|
||||
return Result(QpSolveStatus.SolverUnavailable, Array.Empty<double>());
|
||||
if (!longitudinal)
|
||||
@@ -479,12 +647,18 @@ internal static class EmPlanningServiceChecks
|
||||
LastLongitudinalProblem = problem;
|
||||
if (mode == PipelineSolverMode.LongitudinalInfeasible)
|
||||
return Result(QpSolveStatus.PrimalInfeasible, Array.Empty<double>());
|
||||
if (strictFullPrimal != null && strictFullPrimal.Count == problem.VariableCount)
|
||||
{
|
||||
longitudinalCallCount++;
|
||||
return Result(QpSolveStatus.Solved, strictFullPrimal);
|
||||
}
|
||||
if (mode == PipelineSolverMode.PublicationValidationFailure && longitudinalCallCount == 0)
|
||||
CorruptMapAtOrigin(mapToCorrupt);
|
||||
if (mode == PipelineSolverMode.TimeoutWithFallback && ++longitudinalCallCount > 1)
|
||||
return Result(QpSolveStatus.TimeLimit, Array.Empty<double>());
|
||||
longitudinalCallCount++;
|
||||
return Result(QpSolveStatus.Solved, warmStart);
|
||||
return Result(QpSolveStatus.Solved,
|
||||
TryCreateStrictExactStopPrimal(problem, out double[] strictPrimal) ? strictPrimal : warmStart);
|
||||
}
|
||||
|
||||
private static void CorruptMapAtOrigin(PlanningGridMap? map)
|
||||
@@ -502,41 +676,234 @@ internal static class EmPlanningServiceChecks
|
||||
distances[index] = 0d;
|
||||
}
|
||||
|
||||
private static double[] CreateStrictLongitudinalPrimal(QuadraticProgram problem)
|
||||
private static bool TryCreateStrictExactStopPrimal(QuadraticProgram problem, out double[] primal)
|
||||
{
|
||||
primal = Array.Empty<double>();
|
||||
int variableCount = problem.VariableCount;
|
||||
int knotCount = (variableCount + 1) / 4;
|
||||
var layout = new LongitudinalVariableLayout(knotCount);
|
||||
var jerk = new double[knotCount - 1];
|
||||
const int rampIntervals = 5;
|
||||
for (int index = 0; index < rampIntervals; index++) jerk[index] = 1d;
|
||||
for (int index = rampIntervals; index < 3 * rampIntervals; index++) jerk[index] = -1d;
|
||||
for (int index = 3 * rampIntervals; index < 4 * rampIntervals; index++) jerk[index] = 1d;
|
||||
int stabilizationStart = FindExactStopTailStart(problem, layout);
|
||||
if (stabilizationStart < 3)
|
||||
return false;
|
||||
var times = new double[knotCount];
|
||||
for (int index = 0; index < times.Length; index++) times[index] = index * 0.05d;
|
||||
LongitudinalCandidate baseCandidate = LongitudinalCandidate.Integrate(times, 0d, 0d, 0d, jerk);
|
||||
double terminalPathS = ReadFixedVariable(problem, layout.S(knotCount - 1));
|
||||
double scale = terminalPathS / baseCandidate.S[baseCandidate.S.Count - 1];
|
||||
for (int index = 0; index < jerk.Length; index++) jerk[index] *= scale;
|
||||
LongitudinalCandidate candidate = LongitudinalCandidate.Integrate(times, 0d, 0d, 0d, jerk);
|
||||
for (int index = 0; index < knotCount - 1; index++)
|
||||
{
|
||||
if (!TryReadDynamicsDuration(problem, layout, index, out double duration))
|
||||
return false;
|
||||
times[index + 1] = times[index] + duration;
|
||||
}
|
||||
|
||||
var motionTimes = new double[stabilizationStart + 1];
|
||||
Array.Copy(times, motionTimes, motionTimes.Length);
|
||||
double initialPathS = ReadFixedVariable(problem, layout.S(0));
|
||||
double initialSpeed = ReadFixedVariable(problem, layout.U(0));
|
||||
double initialAcceleration = ReadFixedVariable(problem, layout.A(0));
|
||||
double terminalPathS = ReadFixedVariable(problem, layout.S(stabilizationStart));
|
||||
var preferredJerk = new double[stabilizationStart];
|
||||
LongitudinalCandidate baseline = LongitudinalCandidate.Integrate(motionTimes, initialPathS, initialSpeed,
|
||||
initialAcceleration, preferredJerk);
|
||||
var influence = new double[3, stabilizationStart];
|
||||
for (int interval = 0; interval < stabilizationStart; interval++)
|
||||
{
|
||||
var basis = new double[stabilizationStart];
|
||||
basis[interval] = 1d;
|
||||
LongitudinalCandidate response = LongitudinalCandidate.Integrate(motionTimes, 0d, 0d, 0d, basis);
|
||||
int terminalIndex = response.S.Count - 1;
|
||||
influence[0, interval] = response.A[terminalIndex];
|
||||
influence[1, interval] = response.U[terminalIndex];
|
||||
influence[2, interval] = response.S[terminalIndex];
|
||||
}
|
||||
double[] target =
|
||||
{
|
||||
-baseline.A[baseline.A.Count - 1],
|
||||
-baseline.U[baseline.U.Count - 1],
|
||||
terminalPathS - baseline.S[baseline.S.Count - 1],
|
||||
};
|
||||
var jerk = new double[knotCount - 1];
|
||||
if (stabilizationStart >= 4)
|
||||
{
|
||||
int terminalFirstInterval = stabilizationStart - 3;
|
||||
var terminalInfluence = new double[3, 3];
|
||||
for (int row = 0; row < 3; row++)
|
||||
{
|
||||
for (int column = 0; column < 3; column++)
|
||||
terminalInfluence[row, column] = influence[row, terminalFirstInterval + column];
|
||||
}
|
||||
if (!TrySolveThreeByThree(terminalInfluence, target, out double[] terminalJerk))
|
||||
return false;
|
||||
for (int interval = 0; interval < 3; interval++)
|
||||
jerk[terminalFirstInterval + interval] = terminalJerk[interval];
|
||||
}
|
||||
else
|
||||
{
|
||||
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 < stabilizationStart; interval++)
|
||||
gram[row, column] += influence[row, interval] * influence[column, interval];
|
||||
}
|
||||
}
|
||||
if (!TrySolveThreeByThree(gram, target, out double[] multipliers))
|
||||
return false;
|
||||
for (int interval = 0; interval < stabilizationStart; interval++)
|
||||
{
|
||||
jerk[interval] = preferredJerk[interval];
|
||||
for (int row = 0; row < 3; row++)
|
||||
jerk[interval] += influence[row, interval] * multipliers[row];
|
||||
}
|
||||
}
|
||||
var motionJerk = new double[stabilizationStart];
|
||||
Array.Copy(jerk, motionJerk, motionJerk.Length);
|
||||
LongitudinalCandidate candidate = LongitudinalCandidate.Integrate(motionTimes, initialPathS, initialSpeed,
|
||||
initialAcceleration, motionJerk);
|
||||
primal = CreateExactStopPrimal(layout, knotCount, stabilizationStart, terminalPathS, candidate);
|
||||
return true;
|
||||
}
|
||||
|
||||
private static double[] CreateExactStopPrimal(LongitudinalVariableLayout layout, int knotCount,
|
||||
int stabilizationStart, double terminalPathS, LongitudinalCandidate candidate)
|
||||
{
|
||||
var primal = new double[layout.VariableCount];
|
||||
for (int index = 0; index < 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 < jerk.Length; index++) primal[layout.J(index)] = candidate.J[index];
|
||||
for (int index = 4 * rampIntervals; index < knotCount; index++)
|
||||
{
|
||||
primal[layout.S(index)] = terminalPathS;
|
||||
primal[layout.U(index)] = 0d;
|
||||
primal[layout.A(index)] = 0d;
|
||||
bool isTerminalTail = index >= stabilizationStart;
|
||||
primal[layout.S(index)] = isTerminalTail ? terminalPathS : candidate.S[index];
|
||||
primal[layout.U(index)] = isTerminalTail ? 0d : candidate.U[index];
|
||||
primal[layout.A(index)] = isTerminalTail ? 0d : candidate.A[index];
|
||||
}
|
||||
for (int index = 0; index < candidate.J.Count; index++)
|
||||
primal[layout.J(index)] = candidate.J[index];
|
||||
return primal;
|
||||
}
|
||||
|
||||
private static int FindExactStopTailStart(QuadraticProgram problem, LongitudinalVariableLayout layout)
|
||||
{
|
||||
for (int index = 1; index < layout.KnotCount; index++)
|
||||
{
|
||||
if (TryReadFixedVariable(problem, layout.S(index), out _) &&
|
||||
TryReadFixedVariable(problem, layout.U(index), out _) &&
|
||||
TryReadFixedVariable(problem, layout.A(index), out _))
|
||||
{
|
||||
return index;
|
||||
}
|
||||
}
|
||||
return -1;
|
||||
}
|
||||
|
||||
private static bool TryReadDynamicsDuration(QuadraticProgram problem, LongitudinalVariableLayout layout,
|
||||
int interval, out double duration)
|
||||
{
|
||||
duration = 0d;
|
||||
for (int row = 0; row < problem.ConstraintCount; row++)
|
||||
{
|
||||
if (Math.Abs(problem.LowerBounds[row]) > 1e-12d || Math.Abs(problem.UpperBounds[row]) > 1e-12d ||
|
||||
CountRowEntries(problem, row) != 3 ||
|
||||
Math.Abs(ReadCoefficient(problem, row, layout.A(interval + 1)) - 1d) > 1e-12d ||
|
||||
Math.Abs(ReadCoefficient(problem, row, layout.A(interval)) + 1d) > 1e-12d)
|
||||
{
|
||||
continue;
|
||||
}
|
||||
double jerkCoefficient = ReadCoefficient(problem, row, layout.J(interval));
|
||||
if (jerkCoefficient >= -1e-12d)
|
||||
continue;
|
||||
duration = -jerkCoefficient;
|
||||
return true;
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
private static int CountRowEntries(QuadraticProgram problem, int row)
|
||||
{
|
||||
int count = 0;
|
||||
for (int column = 0; column < problem.ConstraintMatrix.ColumnCount; column++)
|
||||
{
|
||||
for (int index = problem.ConstraintMatrix.ColumnPointers[column];
|
||||
index < problem.ConstraintMatrix.ColumnPointers[column + 1]; index++)
|
||||
{
|
||||
if (problem.ConstraintMatrix.RowIndices[index] == row)
|
||||
count++;
|
||||
}
|
||||
}
|
||||
return count;
|
||||
}
|
||||
|
||||
private static double ReadCoefficient(QuadraticProgram problem, int row, int column)
|
||||
{
|
||||
for (int index = problem.ConstraintMatrix.ColumnPointers[column];
|
||||
index < problem.ConstraintMatrix.ColumnPointers[column + 1]; index++)
|
||||
{
|
||||
if (problem.ConstraintMatrix.RowIndices[index] == row)
|
||||
return problem.ConstraintMatrix.Values[index];
|
||||
}
|
||||
return 0d;
|
||||
}
|
||||
|
||||
private static bool TrySolveThreeByThree(double[,] matrix, IReadOnlyList<double> rightHandSide,
|
||||
out double[] solution)
|
||||
{
|
||||
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 column = 0; column < 3; column++)
|
||||
{
|
||||
int pivot = column;
|
||||
for (int row = column + 1; row < 3; row++)
|
||||
{
|
||||
if (Math.Abs(augmented[row, column]) > Math.Abs(augmented[pivot, column]))
|
||||
pivot = row;
|
||||
}
|
||||
if (Math.Abs(augmented[pivot, column]) < 1e-12d)
|
||||
{
|
||||
solution = Array.Empty<double>();
|
||||
return false;
|
||||
}
|
||||
if (pivot != column)
|
||||
{
|
||||
for (int index = column; index < 4; index++)
|
||||
{
|
||||
double temporary = augmented[column, index];
|
||||
augmented[column, index] = augmented[pivot, index];
|
||||
augmented[pivot, index] = temporary;
|
||||
}
|
||||
}
|
||||
double divisor = augmented[column, column];
|
||||
for (int index = column; index < 4; index++)
|
||||
augmented[column, index] /= divisor;
|
||||
for (int row = 0; row < 3; row++)
|
||||
{
|
||||
if (row == column)
|
||||
continue;
|
||||
double factor = augmented[row, column];
|
||||
for (int index = column; index < 4; index++)
|
||||
augmented[row, index] -= factor * augmented[column, index];
|
||||
}
|
||||
}
|
||||
solution = new[] { augmented[0, 3], augmented[1, 3], augmented[2, 3] };
|
||||
return true;
|
||||
}
|
||||
|
||||
private static bool IsLongitudinalProblem(QuadraticProgram problem)
|
||||
{
|
||||
if (problem.VariableCount < 7 || (problem.VariableCount + 1) % 4 != 0)
|
||||
return false;
|
||||
int knotCount = (problem.VariableCount + 1) / 4;
|
||||
return problem.ConstraintCount >= 8 * knotCount - 2;
|
||||
}
|
||||
|
||||
private static double ReadFixedVariable(QuadraticProgram problem, int variable)
|
||||
{
|
||||
if (TryReadFixedVariable(problem, variable, out double value))
|
||||
return value;
|
||||
throw new InvalidOperationException("Expected a fixed ST variable constraint.");
|
||||
}
|
||||
|
||||
private static bool TryReadFixedVariable(QuadraticProgram problem, int variable, out double value)
|
||||
{
|
||||
for (int row = 0; row < problem.ConstraintCount; row++)
|
||||
{
|
||||
@@ -557,10 +924,12 @@ internal static class EmPlanningServiceChecks
|
||||
if (entryCount == 1 && Math.Abs(coefficient) > 1e-12d &&
|
||||
Math.Abs(problem.LowerBounds[row] - problem.UpperBounds[row]) <= 1e-12d)
|
||||
{
|
||||
return problem.LowerBounds[row] / coefficient;
|
||||
value = problem.LowerBounds[row] / coefficient;
|
||||
return true;
|
||||
}
|
||||
}
|
||||
throw new InvalidOperationException("Expected a fixed ST variable constraint.");
|
||||
value = 0d;
|
||||
return false;
|
||||
}
|
||||
|
||||
private static QpSolveResult Result(QpSolveStatus status, IReadOnlyList<double> primal)
|
||||
|
||||
@@ -13,6 +13,8 @@ internal static class LongitudinalIntegrationChecks
|
||||
public static void Run()
|
||||
{
|
||||
VerifiesRollingOptimizationKeepsANonzeroTerminalSpeed();
|
||||
VerifiesFullDirectionScheduleIsIndependentFromPublicationCadence();
|
||||
VerifiesFullDirectionPublicationDoesNotDuplicateItsTerminalHold();
|
||||
VerifiesExactStopIncludesAStabilizationTail();
|
||||
VerifiesLastStrictCandidateSurvivesLaterTimeout();
|
||||
VerifiesInvalidAndInaccurateCandidatesNeverBecomeFallbacks();
|
||||
@@ -58,6 +60,82 @@ internal static class LongitudinalIntegrationChecks
|
||||
"rolling ST keeps nonzero terminal speed");
|
||||
}
|
||||
|
||||
private static void VerifiesFullDirectionScheduleIsIndependentFromPublicationCadence()
|
||||
{
|
||||
EmPlannerConfiguration configuration = EmPlannerConfiguration.CreateDefault();
|
||||
configuration.Scheduling.TimeHorizonSeconds = 10d;
|
||||
configuration.Scheduling.OutputTimeStepSeconds = 0.10d;
|
||||
configuration.Scheduling.MaximumOptimizationTimeStepSeconds = 0.20d;
|
||||
configuration.Scheduling.MaximumOptimizationSpatialStepMeters = 0.10d;
|
||||
configuration.Scheduling.MaximumOptimizationKnotCount = 401;
|
||||
LateralPath path = new LateralPath(new[]
|
||||
{
|
||||
Point(0d, 0d, 0d),
|
||||
Point(1d, 1d, 0d),
|
||||
Point(2d, 2d, 0d),
|
||||
}, true);
|
||||
|
||||
EmPlanningStatus status = new PathSpeedLimitBuilder().Build(path, TravelDirection.Forward, 0.10d,
|
||||
EmTerminalType.Goal, configuration, out PathSpeedLimit speedLimit, out string failureReason);
|
||||
Verification.Equal(EmPlanningStatus.Success, status, "full schedule envelope: " + failureReason);
|
||||
status = new FullDirectionSegmentScheduleBuilder().TryBuild(path, speedLimit, 0.10d, 0d,
|
||||
configuration.Longitudinal.DesiredForwardSpeedMetersPerSecond, configuration,
|
||||
out LongitudinalKnotSchedule coarsePublication, out failureReason);
|
||||
Verification.Equal(EmPlanningStatus.Success, status, "full schedule with 0.10 s publication: " + failureReason);
|
||||
|
||||
EmPlannerConfiguration densePublicationConfiguration = configuration.Copy();
|
||||
densePublicationConfiguration.Scheduling.OutputTimeStepSeconds = 0.05d;
|
||||
status = new FullDirectionSegmentScheduleBuilder().TryBuild(path, speedLimit, 0.10d, 0d,
|
||||
densePublicationConfiguration.Longitudinal.DesiredForwardSpeedMetersPerSecond, densePublicationConfiguration,
|
||||
out LongitudinalKnotSchedule densePublication, out failureReason);
|
||||
Verification.Equal(EmPlanningStatus.Success, status, "full schedule with 0.05 s publication: " + failureReason);
|
||||
Verification.Equal(coarsePublication.KnotTimes.Count, densePublication.KnotTimes.Count,
|
||||
"publication cadence does not determine full-segment optimization knot count");
|
||||
|
||||
var publicationCandidate = new LongitudinalCandidate(new[] { 0d, 0.50d, 1d },
|
||||
new[] { 0d, 1d / 60d, 1d / 60d }, new[] { 0.10d, 0d, 0d }, new[] { -0.40d, 0d, 0d },
|
||||
new[] { 0.80d, 0d });
|
||||
var publicationResult = new LongitudinalPlanningResult(EmPlanningStatus.Success, publicationCandidate, string.Empty);
|
||||
DateTimeOffset now = DateTimeOffset.UtcNow;
|
||||
var metadata = new EmTrajectoryMetadata("publication-cadence", now, now, 1L, "publication-path", 1L,
|
||||
string.Empty, 0, TravelDirection.Forward, EmTerminalType.Goal, EmLongitudinalMode.ExactStopAtBoundary,
|
||||
EmPlanningScope.FullDirectionSegment);
|
||||
configuration.Longitudinal.ZeroSpeedHoldSeconds = 0d;
|
||||
densePublicationConfiguration.Longitudinal.ZeroSpeedHoldSeconds = 0d;
|
||||
EmTrajectory coarseTrajectory = new EmTrajectoryAssembler(configuration).Assemble(path, publicationResult, metadata);
|
||||
EmTrajectory denseTrajectory = new EmTrajectoryAssembler(densePublicationConfiguration).Assemble(path,
|
||||
publicationResult, metadata);
|
||||
Verification.Equal(2 * (coarseTrajectory.Points.Count - 1), denseTrajectory.Points.Count - 1,
|
||||
"halving publication cadence doubles emitted trajectory intervals without changing optimization knots");
|
||||
}
|
||||
|
||||
private static void VerifiesFullDirectionPublicationDoesNotDuplicateItsTerminalHold()
|
||||
{
|
||||
EmPlannerConfiguration configuration = EmPlannerConfiguration.CreateDefault();
|
||||
configuration.Scheduling.OutputTimeStepSeconds = 0.10d;
|
||||
configuration.Longitudinal.ZeroSpeedHoldSeconds = 0.20d;
|
||||
LateralPath path = new LateralPath(new[]
|
||||
{
|
||||
Point(0d, 0d, 0d),
|
||||
Point(1d, 0.0075d, 0d),
|
||||
}, true);
|
||||
var candidate = new LongitudinalCandidate(new[] { 0d, 0.10d, 0.20d, 0.40d },
|
||||
new[] { 0d, 0.005d, 0.0075d, 0.0075d }, new[] { 0.05d, 0.025d, 0d, 0d },
|
||||
new[] { 0d, -0.5d, 0d, 0d }, new[] { -5d, 5d, 0d });
|
||||
var result = new LongitudinalPlanningResult(EmPlanningStatus.Success, candidate, string.Empty);
|
||||
DateTimeOffset now = DateTimeOffset.UtcNow;
|
||||
var metadata = new EmTrajectoryMetadata("full-hold", now, now, 1L, "hold-path", 1L, string.Empty, 0,
|
||||
TravelDirection.Forward, EmTerminalType.Goal, EmLongitudinalMode.ExactStopAtBoundary,
|
||||
EmPlanningScope.FullDirectionSegment);
|
||||
|
||||
EmTrajectory trajectory = new EmTrajectoryAssembler(configuration).Assemble(path, result, metadata);
|
||||
|
||||
Verification.NearlyEqual(0.40d, trajectory.Points[trajectory.Points.Count - 1].TimeFromStart,
|
||||
"full-scope publication reuses its candidate hold instead of appending a second hold");
|
||||
Verification.Equal(3, CountStationaryTerminalPoints(trajectory),
|
||||
"full-scope publication emits the candidate's single nonzero terminal hold");
|
||||
}
|
||||
|
||||
private static void VerifiesExactStopIncludesAStabilizationTail()
|
||||
{
|
||||
EmPlannerConfiguration configuration = CreateExactStopSeedConfiguration();
|
||||
@@ -97,6 +175,22 @@ internal static class LongitudinalIntegrationChecks
|
||||
}
|
||||
}
|
||||
|
||||
private static int CountStationaryTerminalPoints(EmTrajectory trajectory)
|
||||
{
|
||||
double terminalPathS = trajectory.Points[trajectory.Points.Count - 1].PathS;
|
||||
int count = 0;
|
||||
for (int index = 0; index < trajectory.Points.Count; index++)
|
||||
{
|
||||
EmTrajectoryPoint point = trajectory.Points[index];
|
||||
if (Math.Abs(point.PathS - terminalPathS) <= 1e-12d &&
|
||||
Math.Abs(point.SignedLongitudinalVelocity) <= 1e-12d)
|
||||
{
|
||||
count++;
|
||||
}
|
||||
}
|
||||
return count;
|
||||
}
|
||||
|
||||
public static void RunRealOsqp()
|
||||
{
|
||||
foreach (LongitudinalScenario scenario in CreateRealOsqpScenarios())
|
||||
|
||||
@@ -1,5 +1,6 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using System.Threading;
|
||||
using EMPlannerVerificationHost;
|
||||
using MultiWheelC.TrajectoryPlanning.CoarsePath;
|
||||
using MultiWheelC.TrajectoryPlanning.PathSmoothing;
|
||||
@@ -19,6 +20,8 @@ internal static class LongitudinalModelChecks
|
||||
VerifiesStoppingPrecheckOnlyAppliesToRealStopBoundaries();
|
||||
VerifiesReferenceHorizonSelectionSeparatesSpaceAndTime();
|
||||
VerifiesFullDirectionScopeSelectsActualSegmentBoundary();
|
||||
VerifiesFullDirectionScheduleDerivesDurationAndAdaptiveBreakpoints();
|
||||
VerifiesFullDirectionInitialFeasibilityProjectionAndFallbackSemantics();
|
||||
VerifiesTimeKnotLayoutDynamicsObjectiveAndHardConstraints();
|
||||
VerifiesModeSpecificSolutionValidation();
|
||||
VerifiesPreviousTrajectorySeedResamplesAndProjectsMonotonically();
|
||||
@@ -297,6 +300,231 @@ internal static class LongitudinalModelChecks
|
||||
Verification.NearlyEqual(10d, gear.WindowEndReferenceS, "gear full selection stops before the next segment");
|
||||
}
|
||||
|
||||
private static void VerifiesFullDirectionScheduleDerivesDurationAndAdaptiveBreakpoints()
|
||||
{
|
||||
EmPlannerConfiguration configuration = EmPlannerConfiguration.CreateDefault();
|
||||
configuration.Scheduling.TimeHorizonSeconds = 10d;
|
||||
configuration.Scheduling.DistanceHorizonMeters = 0.25d;
|
||||
configuration.Scheduling.OutputTimeStepSeconds = 0.10d;
|
||||
configuration.Scheduling.MaximumOptimizationTimeStepSeconds = 0.20d;
|
||||
configuration.Scheduling.MaximumOptimizationSpatialStepMeters = 0.10d;
|
||||
configuration.Scheduling.MaximumOptimizationKnotCount = 401;
|
||||
configuration.Longitudinal.MaximumForwardSpeedMetersPerSecond = 1d;
|
||||
configuration.Longitudinal.DesiredForwardSpeedMetersPerSecond = 1d;
|
||||
configuration.Longitudinal.MaximumAccelerationMetersPerSecondSquared = 0.50d;
|
||||
configuration.Longitudinal.MaximumDecelerationMetersPerSecondSquared = 0.50d;
|
||||
configuration.Longitudinal.MaximumJerkMetersPerSecondCubed = 1d;
|
||||
|
||||
LateralPath shortPath = CreateStraightPath(0.50d);
|
||||
EmPlanningStatus status = new PathSpeedLimitBuilder().Build(shortPath, TravelDirection.Forward, 0.10d,
|
||||
EmTerminalType.Goal, configuration, out PathSpeedLimit shortLimit, out string failureReason);
|
||||
Verification.Equal(EmPlanningStatus.Success, status, "short full-segment envelope: " + failureReason);
|
||||
status = new FullDirectionSegmentScheduleBuilder().TryBuild(shortPath, shortLimit, 0.10d, 0d,
|
||||
configuration.Longitudinal.DesiredForwardSpeedMetersPerSecond, configuration,
|
||||
out LongitudinalKnotSchedule shortSchedule, out failureReason);
|
||||
Verification.Equal(EmPlanningStatus.Success, status, "short full-segment schedule: " + failureReason);
|
||||
Verification.True(shortSchedule.TotalDurationSeconds < 10d, "short segment derives its own T_end");
|
||||
|
||||
LateralPath longPath = CreatePath(new[]
|
||||
{
|
||||
new PathFixture(0d, 0d, 0d, 0d),
|
||||
new PathFixture(1.50d, 1.50d, 2d, 0d),
|
||||
new PathFixture(3d, 3d, 0d, 0d),
|
||||
});
|
||||
status = new PathSpeedLimitBuilder().Build(longPath, TravelDirection.Forward, 0.10d,
|
||||
EmTerminalType.Goal, configuration, out PathSpeedLimit longLimit, out failureReason);
|
||||
Verification.Equal(EmPlanningStatus.Success, status, "long full-segment envelope: " + failureReason);
|
||||
status = new FullDirectionSegmentScheduleBuilder().TryBuild(longPath, longLimit, 0.10d, 0d,
|
||||
configuration.Longitudinal.DesiredForwardSpeedMetersPerSecond, configuration,
|
||||
out LongitudinalKnotSchedule longSchedule, out failureReason);
|
||||
Verification.Equal(EmPlanningStatus.Success, status, "long full-segment schedule: " + failureReason);
|
||||
Verification.True(longSchedule.TotalDurationSeconds > shortSchedule.TotalDurationSeconds,
|
||||
"duration grows from s_end and limits");
|
||||
Verification.True(longSchedule.KnotTimes.Count <= configuration.Scheduling.MaximumOptimizationKnotCount,
|
||||
"adaptive schedule respects knot cap");
|
||||
Verification.True(longSchedule.IsAdaptive, "full segment produces an adaptive knot schedule");
|
||||
Verification.True(longSchedule.ReferencePathS.Count > longPath.Points.Count,
|
||||
"curvature and stopping envelopes add schedule breakpoints");
|
||||
Verification.NearlyEqual(longPath.Points[longPath.Points.Count - 1].PathS,
|
||||
longSchedule.ReferencePathS[longSchedule.ReferencePathS.Count - 1], "schedule reaches s_end");
|
||||
Verification.NearlyEqual(0d,
|
||||
longSchedule.ReferenceSpeedMetersPerSecond[longSchedule.ReferenceSpeedMetersPerSecond.Count - 1],
|
||||
"schedule stops at s_end");
|
||||
|
||||
EmPlannerConfiguration constrained = configuration.Copy();
|
||||
constrained.Scheduling.MaximumOptimizationKnotCount = 4;
|
||||
status = new FullDirectionSegmentScheduleBuilder().TryBuild(longPath, longLimit, 0.10d, 0d,
|
||||
constrained.Longitudinal.DesiredForwardSpeedMetersPerSecond, constrained,
|
||||
out LongitudinalKnotSchedule rejected, out failureReason);
|
||||
Verification.Equal(EmPlanningStatus.FullSegmentResourceLimitExceeded, status,
|
||||
"undersized full-segment knot cap rejects rather than truncates");
|
||||
Verification.True(rejected == null, "resource rejection produces no partial schedule");
|
||||
Verification.True(failureReason.IndexOf("required", StringComparison.OrdinalIgnoreCase) >= 0 &&
|
||||
failureReason.IndexOf("configured", StringComparison.OrdinalIgnoreCase) >= 0,
|
||||
"resource rejection reports required and configured knots");
|
||||
}
|
||||
|
||||
private static void VerifiesFullDirectionInitialFeasibilityProjectionAndFallbackSemantics()
|
||||
{
|
||||
EmPlannerConfiguration configuration = EmPlannerConfiguration.CreateDefault();
|
||||
configuration.Scheduling.MaximumOptimizationTimeStepSeconds = 0.20d;
|
||||
configuration.Scheduling.MaximumOptimizationSpatialStepMeters = 0.10d;
|
||||
configuration.Scheduling.MaximumOptimizationKnotCount = 401;
|
||||
configuration.Longitudinal.MaximumAccelerationMetersPerSecondSquared = 1e-6d;
|
||||
configuration.Longitudinal.MaximumDecelerationMetersPerSecondSquared = 1d;
|
||||
configuration.Longitudinal.MaximumJerkMetersPerSecondCubed = 20d;
|
||||
LateralPath path = CreateStraightPath(0.0075d);
|
||||
EmPlanningStatus status = new PathSpeedLimitBuilder().Build(path, TravelDirection.Forward, 0.05d,
|
||||
EmTerminalType.Goal, configuration, out PathSpeedLimit speedLimit, out string failureReason);
|
||||
Verification.Equal(EmPlanningStatus.Success, status, "feasible-reference envelope: " + failureReason);
|
||||
status = new FullDirectionSegmentScheduleBuilder().TryBuild(path, speedLimit, 0.05d, 0d,
|
||||
configuration.Longitudinal.DesiredForwardSpeedMetersPerSecond, configuration,
|
||||
out LongitudinalKnotSchedule schedule, out failureReason);
|
||||
Verification.Equal(EmPlanningStatus.Success, status, "feasible-reference schedule: " + failureReason);
|
||||
|
||||
Verification.True(typeof(LongitudinalKnotSchedule).GetProperty("ReferenceCandidate") == null,
|
||||
"adaptive schedule is only a knot/reference/hold contract");
|
||||
Verification.True(schedule.TerminalHoldStartIndex > 0 &&
|
||||
schedule.TerminalHoldStartIndex < schedule.KnotTimes.Count,
|
||||
"adaptive reference explicitly identifies its terminal hold boundary");
|
||||
Verification.True(schedule.TerminalHoldStartIndex >= 3,
|
||||
"adaptive exact-stop schedule reserves three independent motion jerk intervals");
|
||||
LongitudinalCandidate strictProjection = CreateStrictNonuniformExactStopCandidate();
|
||||
var projectionSchedule = new LongitudinalKnotSchedule(strictProjection.KnotTimes,
|
||||
new[] { 0d, 0.003d, 0.006d, 0.0075d, 0.0075d }, new[] { 0.05d, 0.025d, 0.01d, 0d, 0d }, true, 3);
|
||||
var input = new LongitudinalPlanningInput(path, TravelDirection.Forward, 0.05d, 0d,
|
||||
EmTerminalType.Goal, EmLongitudinalMode.ExactStopAtBoundary, configuration,
|
||||
EmPlanningScope.FullDirectionSegment, projectionSchedule, Array.Empty<double>(), Array.Empty<double>());
|
||||
Verification.True(new LongitudinalSolutionValidator().TryValidate(input, speedLimit, strictProjection,
|
||||
out _, out failureReason), "nonuniform strict projection fixture is physically feasible: " + failureReason);
|
||||
|
||||
var constraintBuilder = new LongitudinalConstraintBuilder(new LongitudinalObjectiveBuilder());
|
||||
Verification.True(constraintBuilder.TryBuildInitialFeasibilityProjection(input, speedLimit,
|
||||
out QuadraticProgram projectionProblem, out failureReason),
|
||||
"full exact-stop feasibility projection builds: " + failureReason);
|
||||
var layout = new LongitudinalVariableLayout(projectionSchedule.KnotTimes.Count);
|
||||
Verification.True(Math.Abs(projectionProblem.LinearCost[layout.S(1)]) > 1e-12d,
|
||||
"feasibility projection tracks scheduled PathS");
|
||||
Verification.True(Math.Abs(projectionProblem.LinearCost[layout.U(1)]) > 1e-12d,
|
||||
"feasibility projection tracks scheduled speed");
|
||||
Verification.Equal(9 * layout.KnotCount - 3 +
|
||||
3 * (layout.KnotCount - projectionSchedule.TerminalHoldStartIndex), projectionProblem.ConstraintCount,
|
||||
"feasibility projection carries a PathS-linearized speed-envelope row for each motion knot");
|
||||
|
||||
var initialTimeoutSolver = new FakeQpSolver(new QpSolveResult(QpSolveStatus.TimeLimit, Array.Empty<double>(), 0d, 0d,
|
||||
0d, 0, TimeSpan.Zero, "time limit", string.Empty));
|
||||
LongitudinalPlanningResult initialTimeout = new SequentialLongitudinalOptimizer(initialTimeoutSolver).Optimize(input,
|
||||
CancellationToken.None);
|
||||
Verification.Equal(EmPlanningStatus.SolverTimedOut, initialTimeout.Status,
|
||||
"initial feasibility timeout cannot publish a fallback");
|
||||
Verification.True(initialTimeout.Candidate == null, "initial feasibility timeout publishes no candidate");
|
||||
|
||||
var solver = new FakeQpSolver(new[]
|
||||
{
|
||||
new QpSolveResult(QpSolveStatus.Solved, ToPrimal(strictProjection), 0d, 0d, 0d, 1,
|
||||
TimeSpan.Zero, "solved", string.Empty),
|
||||
new QpSolveResult(QpSolveStatus.TimeLimit, Array.Empty<double>(), 0d, 0d, 0d, 0,
|
||||
TimeSpan.Zero, "time limit", string.Empty),
|
||||
});
|
||||
LongitudinalPlanningResult result = new SequentialLongitudinalOptimizer(solver).Optimize(input,
|
||||
CancellationToken.None);
|
||||
Verification.Equal(EmPlanningStatus.SuccessWithFallback, result.Status,
|
||||
"strict feasibility projection permits a later exact-stop fallback: " + result.FailureReason);
|
||||
Verification.Equal(2, solver.SolveCallCount,
|
||||
"full scope consumes strict feasibility projection before the objective timeout");
|
||||
Verification.True(new LongitudinalSolutionValidator().TryValidate(input, speedLimit,
|
||||
result.Candidate ?? throw new InvalidOperationException("Adaptive fallback was missing."), out _,
|
||||
out failureReason), "adaptive fallback is strict-feasible: " + failureReason);
|
||||
|
||||
EmPlannerConfiguration denserPublication = configuration.Copy();
|
||||
denserPublication.Scheduling.OutputTimeStepSeconds = 0.05d;
|
||||
status = new FullDirectionSegmentScheduleBuilder().TryBuild(path, speedLimit, 0.05d, 0d,
|
||||
denserPublication.Longitudinal.DesiredForwardSpeedMetersPerSecond, denserPublication,
|
||||
out LongitudinalKnotSchedule sameOptimizationSchedule, out failureReason);
|
||||
Verification.Equal(EmPlanningStatus.Success, status, "independent-cadence schedule: " + failureReason);
|
||||
Verification.Equal(schedule.KnotTimes.Count, sameOptimizationSchedule.KnotTimes.Count,
|
||||
"publication cadence does not change adaptive knot count");
|
||||
Verification.Equal(schedule.TerminalHoldStartIndex, sameOptimizationSchedule.TerminalHoldStartIndex,
|
||||
"publication cadence does not change the terminal hold boundary");
|
||||
}
|
||||
|
||||
private static LongitudinalCandidate CreateStrictNonuniformExactStopCandidate()
|
||||
{
|
||||
double[] times = { 0d, 0.09d, 0.19d, 0.30d, 0.50d };
|
||||
double[] motionTimes = { 0d, 0.09d, 0.19d, 0.30d };
|
||||
var influence = new double[3, 3];
|
||||
for (int interval = 0; interval < 3; interval++)
|
||||
{
|
||||
var basis = new double[3];
|
||||
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[] jerkMotion = SolveThreeByThree(influence, new[] { 0d, -0.05d, -0.0075d });
|
||||
var jerk = new[] { jerkMotion[0], jerkMotion[1], jerkMotion[2], 0d };
|
||||
LongitudinalCandidate integrated = LongitudinalCandidate.Integrate(times, 0d, 0.05d, 0d, jerk);
|
||||
var pathS = new[] { integrated.S[0], integrated.S[1], integrated.S[2], 0.0075d, 0.0075d };
|
||||
var speed = new[] { integrated.U[0], integrated.U[1], integrated.U[2], 0d, 0d };
|
||||
var acceleration = new[] { integrated.A[0], integrated.A[1], integrated.A[2], 0d, 0d };
|
||||
return new LongitudinalCandidate(times, pathS, speed, acceleration, jerk);
|
||||
}
|
||||
|
||||
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 double[] SolveThreeByThree(double[,] matrix, IReadOnlyList<double> rightHandSide)
|
||||
{
|
||||
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;
|
||||
}
|
||||
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];
|
||||
}
|
||||
}
|
||||
return new[] { augmented[0, 3], augmented[1, 3], augmented[2, 3] };
|
||||
}
|
||||
|
||||
private static void VerifiesTimeKnotLayoutDynamicsObjectiveAndHardConstraints()
|
||||
{
|
||||
var layout = new LongitudinalVariableLayout(5);
|
||||
@@ -370,8 +598,21 @@ internal static class LongitudinalModelChecks
|
||||
Verification.NearlyEqual(2d, sUpper, "S upper bound");
|
||||
FindSingleVariableBounds(problem, layout.U(1), out double uLower, out double uUpper);
|
||||
Verification.NearlyEqual(0d, uLower, "U nonnegative bound");
|
||||
Verification.NearlyEqual(envelope.MaximumSpeedAt(integrated.S[1]), uUpper,
|
||||
"U upper bound samples envelope at current S iterate");
|
||||
Verification.NearlyEqual(input.DirectionMaximumSpeedMetersPerSecond, uUpper,
|
||||
"U retains its direction hard bound alongside the PathS envelope");
|
||||
int envelopeSegment = 0;
|
||||
while (envelopeSegment < envelope.PathS.Count - 2 && integrated.S[1] > envelope.PathS[envelopeSegment + 1])
|
||||
envelopeSegment++;
|
||||
double envelopeSlope = (envelope.MaximumSpeedMetersPerSecond[envelopeSegment + 1] -
|
||||
envelope.MaximumSpeedMetersPerSecond[envelopeSegment]) /
|
||||
(envelope.PathS[envelopeSegment + 1] - envelope.PathS[envelopeSegment]);
|
||||
double envelopeIntercept = envelope.MaximumSpeedMetersPerSecond[envelopeSegment] -
|
||||
envelopeSlope * envelope.PathS[envelopeSegment];
|
||||
Verification.Equal(1, CountBoundedRow(problem, new Dictionary<int, double>
|
||||
{
|
||||
{ layout.U(1), 1d }, { layout.S(1), -envelopeSlope },
|
||||
}, -QuadraticProgram.MaximumFiniteBound, envelopeIntercept),
|
||||
"U upper bound linearly re-evaluates the actual PathS envelope");
|
||||
FindSingleVariableBounds(problem, layout.A(1), out double aLower, out double aUpper);
|
||||
Verification.NearlyEqual(-1d, aLower, "deceleration lower bound");
|
||||
Verification.NearlyEqual(1d, aUpper, "acceleration upper bound");
|
||||
|
||||
Reference in New Issue
Block a user