698 lines
40 KiB
C#
698 lines
40 KiB
C#
using System;
|
|
using System.Collections.Generic;
|
|
using EMPlannerVerificationHost;
|
|
using MultiWheelC.TrajectoryPlanning.CoarsePath;
|
|
using MultiWheelC.TrajectoryPlanning.PathSmoothing;
|
|
|
|
namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
|
|
|
|
internal static class LongitudinalModelChecks
|
|
{
|
|
public static void Run()
|
|
{
|
|
VerifiesJerkLimitedStoppingProfileEndsAtRest();
|
|
VerifiesStoppedReachabilityUsesTheSameJerkModel();
|
|
VerifiesRollingEnvelopeDoesNotStopAtWindowEnd();
|
|
VerifiesFinitePathSIndexedSpeedEnvelope();
|
|
VerifiesStoppingEnvelopeIsRefinedOnActualPathS();
|
|
VerifiesStoppingEnvelopeUsesJerkLimitedStoppingMath();
|
|
VerifiesStoppingPrecheckOnlyAppliesToRealStopBoundaries();
|
|
VerifiesReferenceHorizonSelectionSeparatesSpaceAndTime();
|
|
VerifiesFullDirectionScopeSelectsActualSegmentBoundary();
|
|
VerifiesTimeKnotLayoutDynamicsObjectiveAndHardConstraints();
|
|
VerifiesModeSpecificSolutionValidation();
|
|
VerifiesPreviousTrajectorySeedResamplesAndProjectsMonotonically();
|
|
}
|
|
|
|
private static void VerifiesJerkLimitedStoppingProfileEndsAtRest()
|
|
{
|
|
Verification.True(JerkLimitedStoppingMath.TryCalculate(
|
|
0.20d, 0d, 0.30d, 0.50d,
|
|
out JerkLimitedStoppingProfile profile, out string failure),
|
|
"jerk-limited stop builds: " + failure);
|
|
Verification.True(profile.DistanceMeters > 0d, "stop distance is positive");
|
|
Verification.True(profile.DurationSeconds > 0d, "stop duration is positive");
|
|
Verification.NearlyEqual(0d, profile.FinalSpeedMetersPerSecond,
|
|
"stop ends at zero speed");
|
|
Verification.NearlyEqual(0d, profile.FinalAccelerationMetersPerSecondSquared,
|
|
"stop releases acceleration to zero");
|
|
|
|
Verification.True(JerkLimitedStoppingMath.TryCalculate(
|
|
0.20d, 0.20d, 0.30d, 0.50d,
|
|
out JerkLimitedStoppingProfile accelerating, out failure),
|
|
"positive-acceleration stop builds: " + failure);
|
|
Verification.True(accelerating.DistanceMeters > profile.DistanceMeters,
|
|
"positive initial acceleration needs more stopping distance");
|
|
Verification.NearlyEqual(0d, accelerating.FinalAccelerationMetersPerSecondSquared,
|
|
"positive-acceleration stop also releases acceleration");
|
|
}
|
|
|
|
private static void VerifiesStoppedReachabilityUsesTheSameJerkModel()
|
|
{
|
|
double maximumDistance = JerkLimitedStoppingMath.CalculateMaximumStoppedDistance(
|
|
0d, 0d, 0.20d, 0.20d, 0.30d, 0.50d, 2d);
|
|
Verification.True(maximumDistance > 0d && maximumDistance < 0.40d,
|
|
"two-second stopped reach is finite and below unconstrained cruise distance");
|
|
|
|
double cap = JerkLimitedStoppingMath.MaximumInitialSpeedForDistance(
|
|
maximumDistance, 0.20d, 0.30d, 0.50d, 0.20d);
|
|
Verification.True(cap >= 0d && cap <= 0.20d,
|
|
"distance inversion stays inside the direction speed range");
|
|
}
|
|
|
|
private static void VerifiesRollingEnvelopeDoesNotStopAtWindowEnd()
|
|
{
|
|
EmPlannerConfiguration configuration = EmPlannerConfiguration.CreateDefault();
|
|
LateralPath path = CreatePath(new[]
|
|
{
|
|
new PathFixture(0d, 0d, 0d, 0d),
|
|
new PathFixture(1d, 1d, 0d, 0d),
|
|
});
|
|
var rolling = new LongitudinalPlanningInput(path, TravelDirection.Forward,
|
|
0d, 0d, EmTerminalType.RollingSafetyStop,
|
|
EmLongitudinalMode.RollingContinuation, configuration,
|
|
Array.Empty<double>(), Array.Empty<double>());
|
|
|
|
EmPlanningStatus status = new PathSpeedLimitBuilder().Build(
|
|
rolling, out PathSpeedLimit envelope, out string failure);
|
|
|
|
Verification.Equal(EmPlanningStatus.Success, status, "rolling envelope: " + failure);
|
|
Verification.True(envelope.MaximumSpeedAt(rolling.PathUpperBoundS) > 0d,
|
|
"rolling window end keeps a nonzero speed allowance");
|
|
Verification.True(!envelope.HasStopBoundary, "rolling envelope has no stop boundary");
|
|
Verification.NearlyEqual(rolling.PathUpperBoundS, envelope.PathUpperBoundS,
|
|
"rolling envelope reports its PathS upper bound");
|
|
|
|
var approach = new LongitudinalPlanningInput(path, TravelDirection.Forward,
|
|
0d, 0d, EmTerminalType.Goal,
|
|
EmLongitudinalMode.ApproachStopBoundary, configuration,
|
|
Array.Empty<double>(), Array.Empty<double>());
|
|
status = new PathSpeedLimitBuilder().Build(approach, out PathSpeedLimit approachEnvelope, out failure);
|
|
Verification.Equal(EmPlanningStatus.Success, status, "approach envelope: " + failure);
|
|
Verification.True(approachEnvelope.HasStopBoundary, "approach envelope retains its real stop boundary");
|
|
Verification.NearlyEqual(0d, approachEnvelope.StoppingLimitAt(approach.StopBoundaryPathS),
|
|
"approach stop boundary has an exact zero stopping limit");
|
|
}
|
|
|
|
private static void VerifiesFinitePathSIndexedSpeedEnvelope()
|
|
{
|
|
LateralPath directionPath = CreatePath(new[]
|
|
{
|
|
new PathFixture(0d, 0d, 0d, 0d),
|
|
new PathFixture(1d, 1d, 0d, 0d),
|
|
});
|
|
var directionInput = new LongitudinalPlanningInput(directionPath, TravelDirection.Forward, 0d, 0d,
|
|
EmTerminalType.Goal, EmLongitudinalMode.ExactStopAtBoundary, EmPlannerConfiguration.CreateDefault(),
|
|
Array.Empty<double>(), Array.Empty<double>());
|
|
EmPlanningStatus directionStatus = new PathSpeedLimitBuilder().Build(directionInput,
|
|
out PathSpeedLimit directionEnvelope, out string directionFailureReason);
|
|
Verification.Equal(EmPlanningStatus.Success, directionStatus, "default direction speed limit status: " +
|
|
directionFailureReason);
|
|
Verification.NearlyEqual(1d, directionEnvelope.DirectionMaximumSpeedMetersPerSecond,
|
|
"default direction speed limit");
|
|
|
|
EmPlannerConfiguration configuration = EmPlannerConfiguration.CreateDefault();
|
|
configuration.Longitudinal.MaximumForwardSpeedMetersPerSecond = 1d;
|
|
configuration.Longitudinal.MaximumReverseSpeedMetersPerSecond = 1d;
|
|
LateralPath path = CreatePath(new[]
|
|
{
|
|
new PathFixture(10d, 0d, 0d, 0d),
|
|
new PathFixture(20d, 2d, 2d, 4d),
|
|
new PathFixture(20.5d, 4d, 20d, 0d),
|
|
new PathFixture(21d, 5d, 0d, 0d),
|
|
});
|
|
var input = new LongitudinalPlanningInput(path, TravelDirection.Forward, 0d, 0d,
|
|
EmTerminalType.Goal, EmLongitudinalMode.ExactStopAtBoundary, configuration,
|
|
Array.Empty<double>(), Array.Empty<double>());
|
|
|
|
EmPlanningStatus status = new PathSpeedLimitBuilder().Build(input, out PathSpeedLimit envelope,
|
|
out string failureReason);
|
|
Verification.Equal(EmPlanningStatus.Success, status, "speed envelope status: " + failureReason);
|
|
Verification.NearlyEqual(1d, envelope.DirectionMaximumSpeedMetersPerSecond, "overridden direction speed limit");
|
|
Verification.NearlyEqual(Math.Sqrt(0.20d / 2d), envelope.LateralAccelerationLimitAt(2d),
|
|
"curvature lateral-acceleration limit");
|
|
Verification.NearlyEqual(0.50d / 4d, envelope.CurvatureRateLimitAt(2d), "curvature-rate limit");
|
|
Verification.True(double.IsFinite(envelope.LateralAccelerationLimitAt(0d)) &&
|
|
double.IsFinite(envelope.CurvatureRateLimitAt(0d)), "zero curvature limits stay finite");
|
|
Verification.NearlyEqual(MaximumJerkLimitedStopSpeed(input, 4d), envelope.StoppingLimitAt(4d),
|
|
"stopping speed limit uses the complete jerk-limited model");
|
|
Verification.NearlyEqual(Math.Sqrt(0.20d / 20d), envelope.MaximumSpeedAt(4d),
|
|
"combined limit chooses the finite minimum");
|
|
double interpolationQueryPathS = 1.013d;
|
|
int upperStation = 1;
|
|
while (envelope.PathS[upperStation] < interpolationQueryPathS)
|
|
upperStation++;
|
|
double lowerPathS = envelope.PathS[upperStation - 1];
|
|
double upperPathS = envelope.PathS[upperStation];
|
|
double fraction = (interpolationQueryPathS - lowerPathS) / (upperPathS - lowerPathS);
|
|
double expectedInterpolatedSpeed = envelope.MaximumSpeedMetersPerSecond[upperStation - 1] +
|
|
(envelope.MaximumSpeedMetersPerSecond[upperStation] - envelope.MaximumSpeedMetersPerSecond[upperStation - 1]) *
|
|
fraction;
|
|
Verification.NearlyEqual(expectedInterpolatedSpeed, envelope.MaximumSpeedAt(interpolationQueryPathS),
|
|
"speed envelope interpolates by PathS rather than ReferenceS");
|
|
Verification.NearlyEqual(0d, envelope.MaximumSpeedAt(5d), "terminal speed is exactly zero");
|
|
}
|
|
|
|
private static void VerifiesStoppingEnvelopeIsRefinedOnActualPathS()
|
|
{
|
|
EmPlannerConfiguration configuration = EmPlannerConfiguration.CreateDefault();
|
|
configuration.Longitudinal.MaximumForwardSpeedMetersPerSecond = 1d;
|
|
configuration.Longitudinal.MaximumReverseSpeedMetersPerSecond = 1d;
|
|
LateralPath path = CreatePath(new[]
|
|
{
|
|
new PathFixture(0d, 0d, 0d, 0d),
|
|
new PathFixture(100d, 2d, 0d, 0d),
|
|
});
|
|
var input = new LongitudinalPlanningInput(path, TravelDirection.Forward, 0d, 0d,
|
|
EmTerminalType.Goal, EmLongitudinalMode.ExactStopAtBoundary, configuration,
|
|
Array.Empty<double>(), Array.Empty<double>());
|
|
|
|
EmPlanningStatus status = new PathSpeedLimitBuilder().Build(input, out PathSpeedLimit envelope,
|
|
out string failureReason);
|
|
Verification.Equal(EmPlanningStatus.Success, status, "refined stopping envelope status: " + failureReason);
|
|
Verification.True(envelope.PathS.Count > path.Points.Count, "stopping envelope inserts actual-PathS refinement stations");
|
|
Verification.NearlyEqual(MaximumJerkLimitedStopSpeed(input, 1.5d), envelope.MaximumSpeedAt(1.5d),
|
|
"refined stopping envelope uses the complete jerk-limited stopping cap");
|
|
}
|
|
|
|
private static void VerifiesStoppingEnvelopeUsesJerkLimitedStoppingMath()
|
|
{
|
|
EmPlannerConfiguration configuration = EmPlannerConfiguration.CreateDefault();
|
|
configuration.Longitudinal.MaximumForwardSpeedMetersPerSecond = 1d;
|
|
configuration.Longitudinal.MaximumReverseSpeedMetersPerSecond = 1d;
|
|
LateralPath path = CreatePath(new[]
|
|
{
|
|
new PathFixture(0d, 0d, 0d, 0d),
|
|
new PathFixture(100d, 2d, 0d, 0d),
|
|
});
|
|
var input = new LongitudinalPlanningInput(path, TravelDirection.Forward, 0d, 0d,
|
|
EmTerminalType.Goal, EmLongitudinalMode.ExactStopAtBoundary, configuration,
|
|
Array.Empty<double>(), Array.Empty<double>());
|
|
|
|
EmPlanningStatus status = new PathSpeedLimitBuilder().Build(input, out PathSpeedLimit envelope,
|
|
out string failureReason);
|
|
Verification.Equal(EmPlanningStatus.Success, status, "jerk-limited stopping-tail status: " + failureReason);
|
|
double nearBoundaryPathS = input.StopBoundaryPathS - 0.005d;
|
|
Verification.NearlyEqual(MaximumJerkLimitedStopSpeed(input, nearBoundaryPathS),
|
|
envelope.StoppingLimitAt(nearBoundaryPathS),
|
|
"near-boundary speed cap uses jerk-limited distance inversion");
|
|
Verification.NearlyEqual(0d, envelope.StoppingLimitAt(input.StopBoundaryPathS),
|
|
"real stop boundary keeps an exact zero stopping cap");
|
|
}
|
|
|
|
private static void VerifiesStoppingPrecheckOnlyAppliesToRealStopBoundaries()
|
|
{
|
|
EmPlannerConfiguration configuration = EmPlannerConfiguration.CreateDefault();
|
|
LateralPath shortPath = CreatePath(new[]
|
|
{
|
|
new PathFixture(0d, 0d, 0d, 0d),
|
|
new PathFixture(100d, 0.01d, 0d, 0d),
|
|
});
|
|
var rolling = new LongitudinalPlanningInput(shortPath, TravelDirection.Forward, 0.20d, 0.20d,
|
|
EmTerminalType.RollingSafetyStop, EmLongitudinalMode.RollingContinuation, configuration,
|
|
Array.Empty<double>(), Array.Empty<double>());
|
|
|
|
EmPlanningStatus status = new PathSpeedLimitBuilder().Build(rolling, out PathSpeedLimit envelope,
|
|
out string failureReason);
|
|
Verification.Equal(EmPlanningStatus.Success, status,
|
|
"rolling windows do not require a stop inside their local PathS extent: " + failureReason);
|
|
Verification.True(envelope != null, "rolling speed envelope is created despite the short local window");
|
|
|
|
var approach = new LongitudinalPlanningInput(shortPath, TravelDirection.Forward, 0.20d, 0.20d,
|
|
EmTerminalType.Goal, EmLongitudinalMode.ApproachStopBoundary, configuration,
|
|
Array.Empty<double>(), Array.Empty<double>());
|
|
status = new PathSpeedLimitBuilder().Build(approach, out envelope, out failureReason);
|
|
Verification.Equal(EmPlanningStatus.StoppingDistanceInsufficient, status,
|
|
"real stop-boundary jerk/deceleration stopping precheck status");
|
|
Verification.True(envelope == null, "stopping-distance failure does not create a speed envelope");
|
|
Verification.True(failureReason.Length != 0, "stopping-distance failure explains the rejection");
|
|
}
|
|
|
|
private static void VerifiesReferenceHorizonSelectionSeparatesSpaceAndTime()
|
|
{
|
|
EmPlannerConfiguration configuration = EmPlannerConfiguration.CreateDefault();
|
|
configuration.Scheduling.DistanceHorizonMeters = 1d;
|
|
configuration.Scheduling.TimeHorizonSeconds = 2d;
|
|
|
|
DirectionSegmentView longSegment = CreateSegment(4d, EmBoundaryType.Goal);
|
|
EmPlanningStatus status = new PlanningHorizonSelector().Select(
|
|
longSegment, 0d, 0d, 0d, EmPlanningScope.RollingHorizon, configuration,
|
|
out PlanningHorizonSelection rolling, out string failure);
|
|
Verification.Equal(EmPlanningStatus.Success, status, "rolling selection: " + failure);
|
|
Verification.NearlyEqual(1d, rolling.WindowEndReferenceS,
|
|
"distance horizon defines the LS window");
|
|
Verification.Equal(EmLongitudinalMode.RollingContinuation,
|
|
rolling.LongitudinalMode, "far boundary rolls");
|
|
|
|
DirectionSegmentView visibleButFar = CreateSegment(0.35d, EmBoundaryType.Goal);
|
|
status = new PlanningHorizonSelector().Select(
|
|
visibleButFar, 0d, 0d, 0d, EmPlanningScope.RollingHorizon, configuration,
|
|
out PlanningHorizonSelection approach, out failure);
|
|
Verification.Equal(EmPlanningStatus.Success, status, "approach selection: " + failure);
|
|
Verification.Equal(EmLongitudinalMode.ApproachStopBoundary,
|
|
approach.LongitudinalMode, "visible unreachable boundary approaches");
|
|
|
|
DirectionSegmentView reachableGoal = CreateSegment(0.10d, EmBoundaryType.Goal);
|
|
status = new PlanningHorizonSelector().Select(
|
|
reachableGoal, 0d, 0d, 0d, EmPlanningScope.RollingHorizon, configuration,
|
|
out PlanningHorizonSelection exact, out failure);
|
|
Verification.Equal(EmPlanningStatus.Success, status, "exact selection: " + failure);
|
|
Verification.Equal(EmLongitudinalMode.ExactStopAtBoundary,
|
|
exact.LongitudinalMode, "reachable goal stops exactly");
|
|
Verification.Equal(EmTerminalType.Goal, exact.TerminalType,
|
|
"exact stop preserves Goal identity");
|
|
|
|
IReadOnlyList<double> regularTimes = LongitudinalCandidate.CreateKnotTimes(2d, 0.1d);
|
|
Verification.Equal(19, LongitudinalTerminalSchedule.GetStabilizationStartIndex(
|
|
regularTimes, 0.1d), "regular exact stop reserves t=1.9..2.0");
|
|
Verification.Equal(1, LongitudinalTerminalSchedule.GetStabilizationStartIndex(
|
|
new[] { 0d, 0.1d, 0.2d, 0.25d }, 0.1d),
|
|
"short final interval moves the stop anchor earlier");
|
|
}
|
|
|
|
private static void VerifiesFullDirectionScopeSelectsActualSegmentBoundary()
|
|
{
|
|
EmPlannerConfiguration configuration = EmPlannerConfiguration.CreateDefault();
|
|
configuration.Scheduling.DistanceHorizonMeters = 1d;
|
|
DirectionSegmentView tenMeterGoal = CreateSegment(10d, EmBoundaryType.Goal);
|
|
var selector = new PlanningHorizonSelector();
|
|
EmPlanningStatus status = selector.Select(tenMeterGoal, 3d, 0d, 0d,
|
|
EmPlanningScope.FullDirectionSegment, configuration, out PlanningHorizonSelection full, out string failure);
|
|
Verification.Equal(EmPlanningStatus.Success, status, "full selection: " + failure);
|
|
Verification.NearlyEqual(10d, full.WindowEndReferenceS, "full selection reaches actual segment end");
|
|
Verification.Equal(EmLongitudinalMode.ExactStopAtBoundary, full.LongitudinalMode,
|
|
"full selection stops at its real boundary");
|
|
Verification.True(full.HasStopBoundary, "full selection retains the real stop boundary");
|
|
|
|
status = selector.Select(tenMeterGoal, 3d, 0d, 0d,
|
|
EmPlanningScope.RollingHorizon, configuration, out PlanningHorizonSelection rolling, out failure);
|
|
Verification.Equal(EmPlanningStatus.Success, status, "rolling selection: " + failure);
|
|
Verification.NearlyEqual(4d, rolling.WindowEndReferenceS, "rolling selection retains the one-metre window");
|
|
|
|
DirectionSegmentView gearSwitch = CreateSegment(10d, EmBoundaryType.GearSwitchApproach);
|
|
status = selector.Select(gearSwitch, 3d, 0d, 0d,
|
|
EmPlanningScope.FullDirectionSegment, configuration, out PlanningHorizonSelection gear, out failure);
|
|
Verification.Equal(EmPlanningStatus.Success, status, "gear full selection: " + failure);
|
|
Verification.Equal(EmTerminalType.GearSwitch, gear.TerminalType, "gear full selection preserves switch terminal");
|
|
Verification.NearlyEqual(10d, gear.WindowEndReferenceS, "gear full selection stops before the next segment");
|
|
}
|
|
|
|
private static void VerifiesTimeKnotLayoutDynamicsObjectiveAndHardConstraints()
|
|
{
|
|
var layout = new LongitudinalVariableLayout(5);
|
|
Verification.Equal(19, layout.VariableCount, "ST variable count");
|
|
for (int index = 0; index < 5; index++)
|
|
{
|
|
Verification.Equal(index, layout.S(index), "s index " + index);
|
|
Verification.Equal(5 + index, layout.U(index), "u index " + index);
|
|
Verification.Equal(10 + index, layout.A(index), "a index " + index);
|
|
}
|
|
for (int index = 0; index < 4; index++)
|
|
Verification.Equal(15 + index, layout.J(index), "j index " + index);
|
|
|
|
double[] times = { 0d, 0.05d, 0.10d, 0.15d, 0.20d };
|
|
double[] jerk = { 0.30d, -0.10d, 0.20d, -0.20d };
|
|
LongitudinalCandidate integrated = LongitudinalCandidate.Integrate(times, 0d, 0.10d, 0.02d, jerk);
|
|
for (int index = 0; index < jerk.Length; index++)
|
|
{
|
|
double dt = times[index + 1] - times[index];
|
|
Verification.NearlyEqual(integrated.A[index] + dt * integrated.J[index], integrated.A[index + 1],
|
|
"exact ST acceleration dynamics " + index);
|
|
Verification.NearlyEqual(integrated.U[index] + dt * integrated.A[index] + 0.5d * dt * dt * integrated.J[index],
|
|
integrated.U[index + 1], "exact ST speed dynamics " + index);
|
|
Verification.NearlyEqual(integrated.S[index] + dt * integrated.U[index] +
|
|
0.5d * dt * dt * integrated.A[index] + dt * dt * dt * integrated.J[index] / 6d,
|
|
integrated.S[index + 1], "exact ST progress dynamics " + index);
|
|
}
|
|
Verification.True(integrated.SatisfiesExactDiscreteDynamics(1e-12d), "integrated ST candidate validates dynamics");
|
|
|
|
EmPlannerConfiguration configuration = CreateUnitScaleConfiguration();
|
|
LateralPath path = CreatePath(new[]
|
|
{
|
|
new PathFixture(0d, 0d, 0d, 0d),
|
|
new PathFixture(1d, 1d, 0d, 0d),
|
|
new PathFixture(2d, 2d, 0d, 0d),
|
|
});
|
|
var input = new LongitudinalPlanningInput(path, TravelDirection.Forward, 0.10d, 0.02d,
|
|
EmTerminalType.Goal, EmLongitudinalMode.ExactStopAtBoundary, configuration,
|
|
new[] { 0d, 0.10d, 0.20d, 0.30d, 0.40d },
|
|
new[] { 0.20d, 0.20d, 0.20d, 0.20d, 0.20d });
|
|
EmPlanningStatus speedStatus = new PathSpeedLimitBuilder().Build(input, out PathSpeedLimit envelope,
|
|
out string speedFailure);
|
|
Verification.Equal(EmPlanningStatus.Success, speedStatus, "unit-scale speed envelope: " + speedFailure);
|
|
|
|
Verification.True(new LongitudinalConstraintBuilder(new LongitudinalObjectiveBuilder()).TryBuild(input, envelope,
|
|
integrated, out QuadraticProgram problem, out string failureReason), "ST QP builds: " + failureReason);
|
|
var rollingInput = new LongitudinalPlanningInput(path, TravelDirection.Forward, 0.10d, 0.02d,
|
|
EmTerminalType.RollingSafetyStop, EmLongitudinalMode.RollingContinuation, configuration,
|
|
new[] { 0d, 0.10d, 0.20d, 0.30d, 0.40d },
|
|
new[] { 0.20d, 0.20d, 0.20d, 0.20d, 0.20d });
|
|
EmPlanningStatus rollingSpeedStatus = new PathSpeedLimitBuilder().Build(rollingInput,
|
|
out PathSpeedLimit rollingEnvelope, out string rollingSpeedFailure);
|
|
Verification.Equal(EmPlanningStatus.Success, rollingSpeedStatus,
|
|
"unit-scale rolling speed envelope: " + rollingSpeedFailure);
|
|
Verification.True(new LongitudinalConstraintBuilder(new LongitudinalObjectiveBuilder()).TryBuild(
|
|
rollingInput, rollingEnvelope, integrated, out QuadraticProgram rollingProblem, out string rollingFailure),
|
|
"rolling ST QP builds: " + rollingFailure);
|
|
Verification.NearlyEqual(30d, MatrixValue(problem.UpperTriangularP, layout.U(0), layout.U(0)),
|
|
"normalized speed and previous-U P coefficient");
|
|
Verification.NearlyEqual(2d, MatrixValue(problem.UpperTriangularP, layout.A(0), layout.A(0)),
|
|
"normalized acceleration P coefficient");
|
|
Verification.NearlyEqual(20d, MatrixValue(problem.UpperTriangularP, layout.J(0), layout.J(0)),
|
|
"normalized jerk P coefficient");
|
|
Verification.NearlyEqual(2.5d, MatrixValue(problem.UpperTriangularP, layout.S(0), layout.S(0)),
|
|
"normalized previous-S P coefficient");
|
|
Verification.NearlyEqual(0d, MatrixValue(problem.UpperTriangularP, layout.S(4), layout.S(4)),
|
|
"fixed terminal S has no progress-reward coefficient");
|
|
|
|
FindSingleVariableBounds(problem, layout.S(0), out double sLower, out double sUpper);
|
|
Verification.NearlyEqual(0d, sLower, "S lower bound");
|
|
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");
|
|
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");
|
|
FindSingleVariableBounds(problem, layout.J(1), out double jLower, out double jUpper);
|
|
Verification.NearlyEqual(-1d, jLower, "jerk lower bound");
|
|
Verification.NearlyEqual(1d, jUpper, "jerk upper bound");
|
|
|
|
Verification.Equal(1, CountExactEqualityRows(problem, new Dictionary<int, double> { { layout.S(0), 1d } }, 0d),
|
|
"exact initial S");
|
|
Verification.Equal(1, CountExactEqualityRows(problem, new Dictionary<int, double> { { layout.U(0), 1d } }, 0.10d),
|
|
"exact initial U");
|
|
Verification.Equal(1, CountExactEqualityRows(problem, new Dictionary<int, double> { { layout.A(0), 1d } }, 0.02d),
|
|
"exact initial A");
|
|
Verification.Equal(0, CountExactEqualityRows(rollingProblem,
|
|
new Dictionary<int, double> { { layout.S(4), 1d } }, rollingInput.PathUpperBoundS),
|
|
"rolling has no exact terminal S");
|
|
Verification.Equal(0, CountExactEqualityRows(rollingProblem,
|
|
new Dictionary<int, double> { { layout.U(4), 1d } }, 0d),
|
|
"rolling has no exact terminal U");
|
|
Verification.Equal(0, CountExactEqualityRows(rollingProblem,
|
|
new Dictionary<int, double> { { layout.A(4), 1d } }, 0d),
|
|
"rolling has no exact terminal A");
|
|
int stabilizationStart = LongitudinalTerminalSchedule.GetStabilizationStartIndex(
|
|
integrated.KnotTimes, configuration.Scheduling.OutputTimeStepSeconds);
|
|
for (int index = stabilizationStart; index < layout.KnotCount; index++)
|
|
{
|
|
Verification.Equal(1, CountExactEqualityRows(problem,
|
|
new Dictionary<int, double> { { layout.S(index), 1d } }, input.StopBoundaryPathS),
|
|
"stop tail exact S " + index);
|
|
Verification.Equal(1, CountExactEqualityRows(problem,
|
|
new Dictionary<int, double> { { layout.U(index), 1d } }, 0d),
|
|
"stop tail exact U " + index);
|
|
Verification.Equal(1, CountExactEqualityRows(problem,
|
|
new Dictionary<int, double> { { layout.A(index), 1d } }, 0d),
|
|
"stop tail exact A " + index);
|
|
}
|
|
Verification.Equal(1, CountBoundedRow(problem, new Dictionary<int, double>
|
|
{
|
|
{ layout.S(1), 1d }, { layout.S(0), -1d },
|
|
}, 0d, QuadraticProgram.MaximumFiniteBound), "monotonic S hard constraint");
|
|
Verification.Equal(1, CountExactEqualityRows(problem, new Dictionary<int, double>
|
|
{
|
|
{ layout.A(1), 1d }, { layout.A(0), -1d }, { layout.J(0), -0.05d },
|
|
}, 0d), "exact ST acceleration equation");
|
|
Verification.Equal(1, CountExactEqualityRows(problem, new Dictionary<int, double>
|
|
{
|
|
{ layout.U(1), 1d }, { layout.U(0), -1d }, { layout.A(0), -0.05d }, { layout.J(0), -0.00125d },
|
|
}, 0d), "exact ST speed equation");
|
|
Verification.Equal(1, CountExactEqualityRows(problem, new Dictionary<int, double>
|
|
{
|
|
{ layout.S(1), 1d }, { layout.S(0), -1d }, { layout.U(0), -0.05d }, { layout.A(0), -0.00125d },
|
|
{ layout.J(0), -0.000020833333333333333d },
|
|
}, 0d), "exact ST progress equation");
|
|
}
|
|
|
|
private static void VerifiesModeSpecificSolutionValidation()
|
|
{
|
|
EmPlannerConfiguration configuration = CreateTaskFourConfiguration();
|
|
IReadOnlyList<double> exactTimes = LongitudinalCandidate.CreateKnotTimes(0.30d, 0.10d);
|
|
LongitudinalCandidate nonstationaryExact = LongitudinalCandidate.Integrate(
|
|
exactTimes, 0d, 0.10d, 0d, new[] { -4d, 0d, 0d });
|
|
LateralPath exactPath = CreateStraightPath(nonstationaryExact.S[nonstationaryExact.S.Count - 1]);
|
|
var exactInput = new LongitudinalPlanningInput(exactPath, TravelDirection.Forward, 0.10d, 0d,
|
|
EmTerminalType.Goal, EmLongitudinalMode.ExactStopAtBoundary, configuration,
|
|
Array.Empty<double>(), Array.Empty<double>());
|
|
var rollingForExact = new LongitudinalPlanningInput(exactPath, TravelDirection.Forward, 0.10d, 0d,
|
|
EmTerminalType.RollingSafetyStop, EmLongitudinalMode.RollingContinuation, configuration,
|
|
Array.Empty<double>(), Array.Empty<double>());
|
|
EmPlanningStatus speedStatus = new PathSpeedLimitBuilder().Build(rollingForExact,
|
|
out PathSpeedLimit rollingEnvelope, out string speedFailure);
|
|
Verification.Equal(EmPlanningStatus.Success, speedStatus, "exact-validation rolling envelope: " + speedFailure);
|
|
|
|
var validator = new LongitudinalSolutionValidator();
|
|
Verification.True(!validator.TryValidate(exactInput, rollingEnvelope, nonstationaryExact,
|
|
out _, out string exactFailure), "exact stops reject a nonstationary internal tail");
|
|
Verification.True(exactFailure.IndexOf("exact stabilized", StringComparison.Ordinal) >= 0,
|
|
"exact-stop failure identifies the stabilized tail: " + exactFailure);
|
|
|
|
LateralPath rollingPath = CreateStraightPath(0.10d);
|
|
var rollingInput = new LongitudinalPlanningInput(rollingPath, TravelDirection.Forward, 0.10d, 0d,
|
|
EmTerminalType.RollingSafetyStop, EmLongitudinalMode.RollingContinuation, configuration,
|
|
Array.Empty<double>(), Array.Empty<double>());
|
|
speedStatus = new PathSpeedLimitBuilder().Build(rollingInput, out PathSpeedLimit openEnvelope, out speedFailure);
|
|
Verification.Equal(EmPlanningStatus.Success, speedStatus, "rolling-validation envelope: " + speedFailure);
|
|
LongitudinalCandidate rollingCandidate = LongitudinalCandidate.Integrate(
|
|
exactTimes, 0d, 0.10d, 0d, new[] { 0d, 0d, 0d });
|
|
Verification.True(validator.TryValidate(rollingInput, openEnvelope, rollingCandidate,
|
|
out _, out string rollingFailure), "rolling nonzero terminal speed validates: " + rollingFailure);
|
|
|
|
EmPlannerConfiguration approachConfiguration = CreateTaskFourConfiguration();
|
|
approachConfiguration.Scheduling.TimeHorizonSeconds = 0.15d;
|
|
approachConfiguration.Scheduling.OutputTimeStepSeconds = 0.05d;
|
|
IReadOnlyList<double> approachTimes = LongitudinalCandidate.CreateKnotTimes(0.15d, 0.05d);
|
|
LongitudinalCandidate unstoppablyFastApproach = LongitudinalCandidate.Integrate(
|
|
approachTimes, 0d, 0.20d, 0d, new[] { 0d, -10d, 10d });
|
|
LateralPath approachPath = CreateStraightPath(0.035d);
|
|
var approachInput = new LongitudinalPlanningInput(approachPath, TravelDirection.Forward, 0.20d, 0d,
|
|
EmTerminalType.Goal, EmLongitudinalMode.ApproachStopBoundary, approachConfiguration,
|
|
Array.Empty<double>(), Array.Empty<double>());
|
|
var rollingForApproach = new LongitudinalPlanningInput(approachPath, TravelDirection.Forward, 0.20d, 0d,
|
|
EmTerminalType.RollingSafetyStop, EmLongitudinalMode.RollingContinuation, approachConfiguration,
|
|
Array.Empty<double>(), Array.Empty<double>());
|
|
speedStatus = new PathSpeedLimitBuilder().Build(rollingForApproach,
|
|
out PathSpeedLimit approachEnvelope, out speedFailure);
|
|
Verification.Equal(EmPlanningStatus.Success, speedStatus, "approach-validation rolling envelope: " + speedFailure);
|
|
Verification.True(!validator.TryValidate(approachInput, approachEnvelope, unstoppablyFastApproach,
|
|
out _, out string approachFailure), "approach candidates outside the stoppable set are rejected");
|
|
Verification.True(approachFailure.IndexOf("stoppable set", StringComparison.Ordinal) >= 0,
|
|
"approach failure identifies the jerk-limited stoppable set");
|
|
}
|
|
|
|
private static void VerifiesPreviousTrajectorySeedResamplesAndProjectsMonotonically()
|
|
{
|
|
LateralPath path = CreateStraightPath(1d);
|
|
DateTimeOffset previousEffectiveAtUtc = DateTimeOffset.UnixEpoch.AddSeconds(10d);
|
|
EmTrajectory previous = CreatePreviousTrajectory(previousEffectiveAtUtc, TravelDirection.Forward, 3);
|
|
var builder = new LongitudinalPreviousTrajectorySeedBuilder();
|
|
LongitudinalPreviousTrajectorySeed seed = builder.Build(previous, path, previousEffectiveAtUtc.AddSeconds(0.20d),
|
|
new[] { 0d, 0.10d, 0.20d }, 3, TravelDirection.Forward);
|
|
|
|
Verification.Equal(3, seed.PathS.Count, "previous seed path-S count");
|
|
Verification.Equal(3, seed.ProgressSpeedMetersPerSecond.Count, "previous seed speed count");
|
|
Verification.NearlyEqual(0.20d, seed.PathS[0], "previous seed begins at new absolute effective time");
|
|
Verification.True(seed.PathS[1] >= seed.PathS[0] && seed.PathS[2] >= seed.PathS[1],
|
|
"previous seed progress is monotone");
|
|
Verification.NearlyEqual(0.10d, seed.ProgressSpeedMetersPerSecond[0],
|
|
"previous seed uses absolute progress speed");
|
|
|
|
Verification.Equal(0, builder.Build(previous, path, previousEffectiveAtUtc.AddSeconds(0.20d),
|
|
new[] { 0d, 0.10d, 0.20d }, 3, TravelDirection.Reverse).PathS.Count,
|
|
"different direction returns an empty seed");
|
|
Verification.Equal(0, builder.Build(previous, path, previousEffectiveAtUtc.AddSeconds(0.20d),
|
|
new[] { 0d, 0.10d, 0.20d }, 4, TravelDirection.Forward).PathS.Count,
|
|
"different segment returns an empty seed");
|
|
Verification.Equal(0, builder.Build(previous, path, previousEffectiveAtUtc.AddSeconds(0.40d),
|
|
new[] { 0d, 0.10d }, 3, TravelDirection.Forward).PathS.Count,
|
|
"out-of-range absolute sampling returns an empty seed");
|
|
}
|
|
|
|
private static LateralPath CreatePath(IReadOnlyList<PathFixture> fixtures)
|
|
{
|
|
var points = new List<LateralPathPoint>(fixtures.Count);
|
|
for (int index = 0; index < fixtures.Count; index++)
|
|
{
|
|
PathFixture fixture = fixtures[index];
|
|
points.Add(new LateralPathPoint(fixture.ReferenceS, fixture.PathS, 0d, 0d, 0d, 0d, fixture.PathS, 0d,
|
|
0d, fixture.Curvature, fixture.Curvature, fixture.CurvatureDerivative));
|
|
}
|
|
return new LateralPath(points, true);
|
|
}
|
|
|
|
private static EmTrajectory CreatePreviousTrajectory(DateTimeOffset effectiveAtUtc, TravelDirection direction,
|
|
int segmentIndex)
|
|
{
|
|
var metadata = new EmTrajectoryMetadata("previous-seed", effectiveAtUtc, effectiveAtUtc, 1L,
|
|
"previous-reference", 1L, string.Empty, segmentIndex, direction, EmTerminalType.RollingSafetyStop,
|
|
EmLongitudinalMode.RollingContinuation, EmPlanningScope.RollingHorizon);
|
|
double sign = direction == TravelDirection.Forward ? 1d : -1d;
|
|
return new EmTrajectory(metadata, new[]
|
|
{
|
|
new EmTrajectoryPoint(0d, 0d, 0d, sign * 0.10d, 0d, 0d, segmentIndex, 0d, 0d,
|
|
direction, EmBoundaryType.None, 0d, 0d),
|
|
new EmTrajectoryPoint(sign * 0.10d, 0d, 0d, sign * 0.10d, 0.10d, 0d, segmentIndex, 0.10d, 0.10d,
|
|
direction, EmBoundaryType.None, 0d, 0d),
|
|
new EmTrajectoryPoint(sign * 0.20d, 0d, 0d, sign * 0.10d, 0.20d, 0d, segmentIndex, 0.20d, 0.20d,
|
|
direction, EmBoundaryType.None, 0d, 0d),
|
|
new EmTrajectoryPoint(sign * 0.30d, 0d, 0d, sign * 0.10d, 0.30d, 0d, segmentIndex, 0.30d, 0.30d,
|
|
direction, EmBoundaryType.None, 0d, 0d),
|
|
new EmTrajectoryPoint(sign * 0.40d, 0d, 0d, sign * 0.10d, 0.40d, 0d, segmentIndex, 0.40d, 0.40d,
|
|
direction, EmBoundaryType.None, 0d, 0d),
|
|
});
|
|
}
|
|
|
|
private static DirectionSegmentView CreateSegment(double length, EmBoundaryType endBoundaryType)
|
|
{
|
|
var points = new List<SmoothedPathPoint>
|
|
{
|
|
Point(0d, 0d),
|
|
Point(length, length),
|
|
};
|
|
return new DirectionSegmentView(0, TravelDirection.Forward, points,
|
|
new ReferenceBoundary(0, 0d, EmBoundaryType.None, 0d),
|
|
new ReferenceBoundary(0, length, endBoundaryType, length), 0d);
|
|
}
|
|
|
|
private static SmoothedPathPoint Point(double x, double s)
|
|
{
|
|
return new SmoothedPathPoint(x, 0d, 0d, 0d, s, TravelDirection.Forward, 0d, 0d, 0d, 1d, false,
|
|
SmoothedPathPointSource.Anchor);
|
|
}
|
|
|
|
private static EmPlannerConfiguration CreateUnitScaleConfiguration()
|
|
{
|
|
EmPlannerConfiguration configuration = EmPlannerConfiguration.CreateDefault();
|
|
configuration.Scheduling.TimeHorizonSeconds = 0.20d;
|
|
configuration.Scheduling.OutputTimeStepSeconds = 0.05d;
|
|
configuration.Longitudinal.MaximumForwardSpeedMetersPerSecond = 1d;
|
|
configuration.Longitudinal.MaximumReverseSpeedMetersPerSecond = 1d;
|
|
configuration.Longitudinal.MaximumAccelerationMetersPerSecondSquared = 1d;
|
|
configuration.Longitudinal.MaximumDecelerationMetersPerSecondSquared = 1d;
|
|
configuration.Longitudinal.MaximumJerkMetersPerSecondCubed = 1d;
|
|
configuration.Longitudinal.MaximumLateralAccelerationMetersPerSecondSquared = 1d;
|
|
configuration.Longitudinal.MaximumCurvatureRatePerMeterPerSecond = 1d;
|
|
return configuration;
|
|
}
|
|
|
|
private static EmPlannerConfiguration CreateTaskFourConfiguration()
|
|
{
|
|
EmPlannerConfiguration configuration = EmPlannerConfiguration.CreateDefault();
|
|
configuration.Scheduling.TimeHorizonSeconds = 0.30d;
|
|
configuration.Scheduling.OutputTimeStepSeconds = 0.10d;
|
|
configuration.Longitudinal.MaximumForwardSpeedMetersPerSecond = 1d;
|
|
configuration.Longitudinal.MaximumReverseSpeedMetersPerSecond = 1d;
|
|
configuration.Longitudinal.MaximumAccelerationMetersPerSecondSquared = 1e-6d;
|
|
configuration.Longitudinal.MaximumDecelerationMetersPerSecondSquared = 1d;
|
|
configuration.Longitudinal.MaximumJerkMetersPerSecondCubed = 10d;
|
|
configuration.Longitudinal.MaximumLateralAccelerationMetersPerSecondSquared = 1d;
|
|
configuration.Longitudinal.MaximumCurvatureRatePerMeterPerSecond = 1d;
|
|
return configuration;
|
|
}
|
|
|
|
private static LateralPath CreateStraightPath(double pathUpperBoundS)
|
|
{
|
|
return CreatePath(new[]
|
|
{
|
|
new PathFixture(0d, 0d, 0d, 0d),
|
|
new PathFixture(pathUpperBoundS, pathUpperBoundS, 0d, 0d),
|
|
});
|
|
}
|
|
|
|
private static double MaximumJerkLimitedStopSpeed(LongitudinalPlanningInput input, double pathS)
|
|
{
|
|
LongitudinalConfiguration limits = input.Configuration.Longitudinal;
|
|
return JerkLimitedStoppingMath.MaximumInitialSpeedForDistance(
|
|
Math.Max(0d, input.StopBoundaryPathS - pathS),
|
|
limits.MaximumAccelerationMetersPerSecondSquared,
|
|
limits.MaximumDecelerationMetersPerSecondSquared,
|
|
limits.MaximumJerkMetersPerSecondCubed,
|
|
input.DirectionMaximumSpeedMetersPerSecond);
|
|
}
|
|
|
|
private static double MatrixValue(SparseCscMatrix matrix, int row, int column)
|
|
{
|
|
for (int index = matrix.ColumnPointers[column]; index < matrix.ColumnPointers[column + 1]; index++)
|
|
{
|
|
if (matrix.RowIndices[index] == row)
|
|
return matrix.Values[index];
|
|
}
|
|
return 0d;
|
|
}
|
|
|
|
private static void FindSingleVariableBounds(QuadraticProgram problem, int variable, out double lower, out double upper)
|
|
{
|
|
for (int row = 0; row < problem.ConstraintCount; row++)
|
|
{
|
|
if (RowMatches(problem.ConstraintMatrix, row, new Dictionary<int, double> { { variable, 1d } }))
|
|
{
|
|
lower = problem.LowerBounds[row];
|
|
upper = problem.UpperBounds[row];
|
|
return;
|
|
}
|
|
}
|
|
throw new InvalidOperationException("No single-variable bounds were found for variable " + variable + ".");
|
|
}
|
|
|
|
private static int CountExactEqualityRows(QuadraticProgram problem, IReadOnlyDictionary<int, double> expected,
|
|
double bound)
|
|
{
|
|
return CountBoundedRow(problem, expected, bound, bound);
|
|
}
|
|
|
|
private static int CountBoundedRow(QuadraticProgram problem, IReadOnlyDictionary<int, double> expected,
|
|
double lower, double upper)
|
|
{
|
|
int count = 0;
|
|
for (int row = 0; row < problem.ConstraintCount; row++)
|
|
{
|
|
if (Math.Abs(problem.LowerBounds[row] - lower) <= 1e-12d &&
|
|
Math.Abs(problem.UpperBounds[row] - upper) <= 1e-12d && RowMatches(problem.ConstraintMatrix, row, expected))
|
|
{
|
|
count++;
|
|
}
|
|
}
|
|
return count;
|
|
}
|
|
|
|
private static bool RowMatches(SparseCscMatrix matrix, int targetRow, IReadOnlyDictionary<int, double> expected)
|
|
{
|
|
var actual = new Dictionary<int, double>();
|
|
for (int column = 0; column < matrix.ColumnCount; column++)
|
|
{
|
|
for (int index = matrix.ColumnPointers[column]; index < matrix.ColumnPointers[column + 1]; index++)
|
|
{
|
|
if (matrix.RowIndices[index] == targetRow)
|
|
actual[column] = matrix.Values[index];
|
|
}
|
|
}
|
|
if (actual.Count != expected.Count)
|
|
return false;
|
|
foreach (KeyValuePair<int, double> pair in expected)
|
|
{
|
|
if (!actual.TryGetValue(pair.Key, out double actualValue) || Math.Abs(actualValue - pair.Value) > 1e-12d)
|
|
return false;
|
|
}
|
|
return true;
|
|
}
|
|
|
|
private sealed class PathFixture
|
|
{
|
|
public PathFixture(double referenceS, double pathS, double curvature, double curvatureDerivative)
|
|
{
|
|
ReferenceS = referenceS;
|
|
PathS = pathS;
|
|
Curvature = curvature;
|
|
CurvatureDerivative = curvatureDerivative;
|
|
}
|
|
|
|
public double ReferenceS { get; }
|
|
public double PathS { get; }
|
|
public double Curvature { get; }
|
|
public double CurvatureDerivative { get; }
|
|
}
|
|
}
|