608 lines
34 KiB
C#
608 lines
34 KiB
C#
using System;
|
|
using System.Collections.Generic;
|
|
using System.Threading;
|
|
using EMPlannerVerificationHost;
|
|
using MultiWheelC.TrajectoryPlanning.CoarsePath;
|
|
using MultiWheelC.TrajectoryPlanning.CoarsePath.Vehicle;
|
|
using MultiWheelC.TrajectoryPlanning.PathSmoothing;
|
|
using MultiWheelC.TrajectoryPlanning.Utils;
|
|
|
|
namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
|
|
|
|
internal static class LateralModelChecks
|
|
{
|
|
public static void Run()
|
|
{
|
|
VerifiesDeterministicVariableLayout();
|
|
VerifiesExactDiscreteDynamicsForUnequalStations();
|
|
VerifiesPlanningInputBoundariesAndDefensiveCopies();
|
|
VerifiesLateralResultPublicationContract();
|
|
VerifiesNormalizedObjectiveAndHardConstraints();
|
|
VerifiesAllNamedCostScales();
|
|
VerifiesEmptyHardBoundIntersectionFailsBeforeSolve();
|
|
VerifiesFakeSolverCapturesTheNeutralQpBoundary();
|
|
VerifiesNonlinearGeometryInBothDirections();
|
|
VerifiesIndependentGeometryValidationRejectsUnsafeOrTamperedPaths();
|
|
}
|
|
|
|
private static void VerifiesDeterministicVariableLayout()
|
|
{
|
|
LateralVariableLayout layout = CreateLayout(4);
|
|
|
|
Verification.Equal(15, layout.VariableCount, "layout variable count");
|
|
for (int index = 0; index < 4; index++)
|
|
{
|
|
Verification.Equal(index, layout.L(index), "l index " + index);
|
|
Verification.Equal(4 + index, layout.DL(index), "dl index " + index);
|
|
Verification.Equal(8 + index, layout.DDL(index), "ddl index " + index);
|
|
}
|
|
for (int index = 0; index < 3; index++)
|
|
Verification.Equal(12 + index, layout.DDDL(index), "dddl index " + index);
|
|
|
|
ExpectArgumentException(() => CreateLayout(1), "layout rejects fewer than two stations");
|
|
ExpectArgumentException(() => layout.L(4), "l index bounds check");
|
|
ExpectArgumentException(() => layout.DL(-1), "dl index bounds check");
|
|
ExpectArgumentException(() => layout.DDL(4), "ddl index bounds check");
|
|
ExpectArgumentException(() => layout.DDDL(3), "dddl index bounds check");
|
|
}
|
|
|
|
private static void VerifiesExactDiscreteDynamicsForUnequalStations()
|
|
{
|
|
double[] stations = { 0d, 0.4d, 1.25d, 2.5d };
|
|
double[] jerks = { 0.5d, -0.3d, 0.2d };
|
|
LateralCandidate candidate = LateralCandidate.Integrate(stations, 0.1d, -0.2d, 0.3d, jerks);
|
|
|
|
for (int index = 0; index < jerks.Length; index++)
|
|
{
|
|
double ds = stations[index + 1] - stations[index];
|
|
Verification.NearlyEqual(candidate.DDL[index] + ds * candidate.DDDL[index], candidate.DDL[index + 1],
|
|
"exact ddl integration " + index);
|
|
Verification.NearlyEqual(candidate.DL[index] + ds * candidate.DDL[index] + 0.5d * ds * ds * candidate.DDDL[index],
|
|
candidate.DL[index + 1], "exact dl integration " + index);
|
|
Verification.NearlyEqual(candidate.L[index] + ds * candidate.DL[index] + 0.5d * ds * ds * candidate.DDL[index] +
|
|
ds * ds * ds * candidate.DDDL[index] / 6d, candidate.L[index + 1], "exact l integration " + index);
|
|
}
|
|
Verification.True(candidate.SatisfiesExactDiscreteDynamics(1e-12d), "integrated candidate validates exact dynamics");
|
|
|
|
LateralCandidate inconsistent = new LateralCandidate(new[] { 0d, 1d }, new[] { 0d, 1d },
|
|
new[] { 0d, 0d }, new[] { 0d, 0d }, new[] { 0d });
|
|
Verification.True(!inconsistent.SatisfiesExactDiscreteDynamics(1e-12d), "candidate detects inconsistent dynamics");
|
|
ExpectArgumentException(() => new LateralCandidate(new[] { 0d, 0d }, new[] { 0d, 0d },
|
|
new[] { 0d, 0d }, new[] { 0d, 0d }, new[] { 0d }), "candidate rejects non-increasing stations");
|
|
}
|
|
|
|
private static void VerifiesPlanningInputBoundariesAndDefensiveCopies()
|
|
{
|
|
DirectionSegmentView segment = CreateStraightSegment();
|
|
var corridorStations = new[]
|
|
{
|
|
new LateralInterval(0d, -0.3d, 0.3d, 0d),
|
|
new LateralInterval(1d, -0.3d, 0.3d, 0d),
|
|
new LateralInterval(2d, -0.3d, 0.3d, 0d),
|
|
};
|
|
var seeds = new[]
|
|
{
|
|
new FrenetProjection(ReferencePathInterpolator.Interpolate(segment, 0d), 0d, 0d, 0d),
|
|
};
|
|
LateralPlanningInput input = new LateralPlanningInput(segment, new StaticCorridor(corridorStations), seeds[0],
|
|
EmTerminalType.Goal, CreateVehicle(), EmPlannerConfiguration.CreateDefault(), seeds);
|
|
corridorStations[1] = new LateralInterval(1d, -0.1d, 0.1d, 0d);
|
|
seeds[0] = new FrenetProjection(ReferencePathInterpolator.Interpolate(segment, 0d), 0.2d, 0d, 0d);
|
|
|
|
Verification.NearlyEqual(-0.3d, input.Corridor.Stations[1].MinimumL, "input copies corridor stations");
|
|
Verification.NearlyEqual(0d, input.PreviousTrajectorySeed[0].LateralOffset, "input copies seed list");
|
|
ExpectArgumentException(() => new LateralPlanningInput(segment,
|
|
new StaticCorridor(new[] { new LateralInterval(0d, -0.3d, 0.3d, 0d) }),
|
|
input.StartProjection, EmTerminalType.Goal, CreateVehicle(), EmPlannerConfiguration.CreateDefault(), Array.Empty<FrenetProjection>()),
|
|
"input rejects fewer than two stations");
|
|
ExpectArgumentException(() => new LateralPlanningInput(segment,
|
|
new StaticCorridor(new[]
|
|
{
|
|
new LateralInterval(0d, -0.3d, 0.3d, 0d),
|
|
new LateralInterval(0d, -0.3d, 0.3d, 0d),
|
|
}), input.StartProjection, EmTerminalType.Goal, CreateVehicle(), EmPlannerConfiguration.CreateDefault(), Array.Empty<FrenetProjection>()),
|
|
"input rejects non-increasing corridor stations");
|
|
ExpectArgumentException(() => new LateralPlanningInput(segment,
|
|
new StaticCorridor(new[]
|
|
{
|
|
new LateralInterval(0.1d, -0.3d, 0.3d, 0d),
|
|
new LateralInterval(2d, -0.3d, 0.3d, 0d),
|
|
}), input.StartProjection, EmTerminalType.Goal, CreateVehicle(), EmPlannerConfiguration.CreateDefault(), Array.Empty<FrenetProjection>()),
|
|
"input rejects start-corridor station mismatch");
|
|
ExpectArgumentException(() => new LateralPlanningInput(segment,
|
|
new StaticCorridor(new[]
|
|
{
|
|
new LateralInterval(0d, -0.1d, 0.1d, 0d),
|
|
new LateralInterval(2d, -0.1d, 0.1d, 0d),
|
|
}), new FrenetProjection(ReferencePathInterpolator.Interpolate(segment, 0d), 0.2d, 0d, 0d),
|
|
EmTerminalType.Goal, CreateVehicle(), EmPlannerConfiguration.CreateDefault(), Array.Empty<FrenetProjection>()),
|
|
"input rejects start projection outside first hard interval");
|
|
}
|
|
|
|
private static void VerifiesLateralResultPublicationContract()
|
|
{
|
|
LateralPath unvalidated = new LateralPath(new[] { CreatePathPoint(0d) }, false);
|
|
LateralPath validated = new LateralPath(new[] { CreatePathPoint(0d), CreatePathPoint(1d) }, true);
|
|
|
|
ExpectArgumentException(() => new LateralPlanningResult(EmPlanningStatus.Success, unvalidated, string.Empty),
|
|
"success requires an independently validated path");
|
|
ExpectArgumentException(() => new LateralPlanningResult(EmPlanningStatus.SuccessWithFallback,
|
|
new LateralPath(Array.Empty<LateralPathPoint>(), true), string.Empty),
|
|
"fallback requires a non-empty path");
|
|
ExpectArgumentException(() => new LateralPlanningResult(EmPlanningStatus.LateralInfeasible, validated, string.Empty),
|
|
"failed result has no candidate");
|
|
LateralPlanningResult result = new LateralPlanningResult(EmPlanningStatus.SuccessWithFallback, validated, "fallback");
|
|
Verification.Equal(validated, result.Path, "fallback path is preserved");
|
|
}
|
|
|
|
private static void VerifiesNormalizedObjectiveAndHardConstraints()
|
|
{
|
|
EmPlannerConfiguration configuration = CreateUnitScaleConfiguration();
|
|
LateralPlanningInput input = CreateModelInput(EmTerminalType.Goal, configuration,
|
|
new[] { 0.2d, -0.1d, 0.3d });
|
|
LateralCandidate linearization = LateralCandidate.Integrate(input.ReferenceStations, 0d, 0d, 0d,
|
|
new[] { 0d, 0d });
|
|
LateralConstraintBuilder builder = CreateConstraintBuilder();
|
|
|
|
Verification.True(builder.TryBuild(input, linearization, out QuadraticProgram problem, out string failureReason),
|
|
"unit-scale QP builds: " + failureReason);
|
|
var layout = new LateralVariableLayout(3);
|
|
Verification.NearlyEqual(30d, MatrixValue(problem.UpperTriangularP, layout.L(0), layout.L(0)),
|
|
"reference plus previous P coefficient");
|
|
Verification.NearlyEqual(20d, MatrixValue(problem.UpperTriangularP, layout.DDDL(0), layout.DDDL(0)),
|
|
"jerk P coefficient");
|
|
Verification.NearlyEqual(-2d, problem.LinearCost[layout.L(0)], "previous-seed q coefficient");
|
|
|
|
for (int interval = 0; interval < 2; interval++)
|
|
{
|
|
double ds = input.ReferenceStations[interval + 1] - input.ReferenceStations[interval];
|
|
Verification.Equal(1, CountExactEqualityRows(problem, new Dictionary<int, double>
|
|
{
|
|
{ layout.DDL(interval), -1d },
|
|
{ layout.DDL(interval + 1), 1d },
|
|
{ layout.DDDL(interval), -ds },
|
|
}), "ddl dynamics equality " + interval);
|
|
Verification.Equal(1, CountExactEqualityRows(problem, new Dictionary<int, double>
|
|
{
|
|
{ layout.DL(interval), -1d },
|
|
{ layout.DL(interval + 1), 1d },
|
|
{ layout.DDL(interval), -ds },
|
|
{ layout.DDDL(interval), -0.5d * ds * ds },
|
|
}), "dl dynamics equality " + interval);
|
|
Verification.Equal(1, CountExactEqualityRows(problem, new Dictionary<int, double>
|
|
{
|
|
{ layout.L(interval), -1d },
|
|
{ layout.L(interval + 1), 1d },
|
|
{ layout.DL(interval), -ds },
|
|
{ layout.DDL(interval), -0.5d * ds * ds },
|
|
{ layout.DDDL(interval), -ds * ds * ds / 6d },
|
|
}), "l dynamics equality " + interval);
|
|
}
|
|
|
|
for (int station = 0; station < layout.StationCount; station++)
|
|
{
|
|
Verification.True(HasFiniteNonEqualityBound(problem, layout.L(station)), "finite lateral hard bound " + station);
|
|
Verification.True(HasFiniteNonEqualityBound(problem, layout.DL(station)), "finite slope hard bound " + station);
|
|
Verification.True(HasFiniteNonEqualityBound(problem, layout.DDL(station)), "finite second-derivative hard bound " + station);
|
|
}
|
|
for (int interval = 0; interval < layout.StationCount - 1; interval++)
|
|
Verification.True(HasFiniteNonEqualityBound(problem, layout.DDDL(interval)), "finite jerk hard bound " + interval);
|
|
|
|
Verification.Equal(1, CountExactEqualityRows(problem, new Dictionary<int, double> { { layout.L(2), 1d } }),
|
|
"goal terminal l equality");
|
|
Verification.Equal(1, CountExactEqualityRows(problem, new Dictionary<int, double> { { layout.DL(2), 1d } }),
|
|
"goal terminal dl equality");
|
|
|
|
LateralPlanningInput rolling = CreateModelInput(EmTerminalType.RollingSafetyStop, configuration,
|
|
new[] { 0.2d, -0.1d, 0.3d });
|
|
Verification.True(builder.TryBuild(rolling, linearization, out QuadraticProgram rollingProblem, out string rollingReason),
|
|
"rolling QP builds: " + rollingReason);
|
|
Verification.NearlyEqual(50d, MatrixValue(rollingProblem.UpperTriangularP, layout.L(2), layout.L(2)),
|
|
"rolling terminal adds normalized objective cost");
|
|
Verification.Equal(0, CountExactEqualityRows(rollingProblem, new Dictionary<int, double> { { layout.L(2), 1d } }),
|
|
"rolling terminal has no l equality");
|
|
Verification.Equal(0, CountExactEqualityRows(rollingProblem, new Dictionary<int, double> { { layout.DL(2), 1d } }),
|
|
"rolling terminal has no dl equality");
|
|
}
|
|
|
|
private static void VerifiesAllNamedCostScales()
|
|
{
|
|
EmPlannerConfiguration configuration = EmPlannerConfiguration.CreateDefault();
|
|
configuration.Corridor.MaximumLateralOffsetMeters = 2d;
|
|
configuration.Lateral.MaximumLateralSlope = 4d;
|
|
configuration.Lateral.MaximumLateralSecondDerivativePerMeter = 5d;
|
|
configuration.Lateral.MaximumLateralThirdDerivativePerSquareMeter = 6d;
|
|
LateralPlanningInput input = CreateModelInput(EmTerminalType.RollingSafetyStop, configuration,
|
|
new[] { 0.2d, 0.2d, 0.2d }, referenceCurvatureDerivative: 4d, maximumVehicleCurvature: 7d);
|
|
LateralCandidate linearization = LateralCandidate.Integrate(input.ReferenceStations, 0d, 0d, 0d,
|
|
new[] { 0d, 0d });
|
|
LateralConstraintBuilder builder = CreateConstraintBuilder();
|
|
|
|
Verification.True(builder.TryBuild(input, linearization, out QuadraticProgram problem, out string failureReason),
|
|
"non-unit-scale QP builds: " + failureReason);
|
|
var layout = new LateralVariableLayout(3);
|
|
Verification.NearlyEqual(7.5d, MatrixValue(problem.UpperTriangularP, layout.L(0), layout.L(0)),
|
|
"reference and previous costs divide by lateral scale squared");
|
|
Verification.NearlyEqual(-0.5d, problem.LinearCost[layout.L(0)],
|
|
"previous target coefficient divides by lateral scale squared");
|
|
Verification.NearlyEqual(0.125d, MatrixValue(problem.UpperTriangularP, layout.DL(0), layout.DL(0)),
|
|
"heading cost divides by slope scale squared");
|
|
Verification.NearlyEqual(5d / 9d, MatrixValue(problem.UpperTriangularP, layout.DDDL(0), layout.DDDL(0)),
|
|
"jerk cost divides by third-derivative scale squared");
|
|
Verification.NearlyEqual(0.4d + 10d / 49d + 3.125d,
|
|
MatrixValue(problem.UpperTriangularP, layout.DDL(0), layout.DDL(0)),
|
|
"second derivative, curvature, and curvature variation use their named scales");
|
|
Verification.NearlyEqual(12.5d, MatrixValue(problem.UpperTriangularP, layout.L(2), layout.L(2)),
|
|
"rolling terminal cost divides by lateral scale squared");
|
|
|
|
EmPlannerConfiguration denominatorConfiguration = CreateUnitScaleConfiguration();
|
|
denominatorConfiguration.Lateral.MaximumLateralStepPerIterationMeters = 0.5d;
|
|
LateralPlanningInput denominatorInput = CreateModelInput(EmTerminalType.RollingSafetyStop, denominatorConfiguration,
|
|
Array.Empty<double>(), 0d, 2d);
|
|
Verification.True(builder.TryBuild(denominatorInput, linearization, out QuadraticProgram denominatorProblem,
|
|
out string denominatorReason), "denominator QP builds: " + denominatorReason);
|
|
Verification.True(HasBoundWithUpper(denominatorProblem, layout.L(0), 0.4d),
|
|
"Frenet denominator is intersected as a finite hard lateral bound");
|
|
}
|
|
|
|
private static void VerifiesEmptyHardBoundIntersectionFailsBeforeSolve()
|
|
{
|
|
EmPlannerConfiguration configuration = CreateUnitScaleConfiguration();
|
|
LateralPlanningInput input = CreateModelInput(EmTerminalType.RollingSafetyStop, configuration,
|
|
Array.Empty<double>(), 0d, 0d, 0.9d, 0.9d, 1d);
|
|
LateralCandidate linearization = LateralCandidate.Integrate(input.ReferenceStations, 0d, 0d, 0d,
|
|
new[] { 0d, 0d });
|
|
|
|
Verification.True(!CreateConstraintBuilder().TryBuild(input, linearization, out QuadraticProgram problem,
|
|
out string failureReason), "empty corridor/trust intersection is infeasible before solve");
|
|
Verification.True(problem == null && failureReason.Length > 0, "infeasible build returns no QP and a reason");
|
|
}
|
|
|
|
private static void VerifiesFakeSolverCapturesTheNeutralQpBoundary()
|
|
{
|
|
EmPlannerConfiguration configuration = CreateUnitScaleConfiguration();
|
|
LateralPlanningInput input = CreateModelInput(EmTerminalType.Goal, configuration, Array.Empty<double>());
|
|
LateralCandidate linearization = LateralCandidate.Integrate(input.ReferenceStations, 0d, 0d, 0d,
|
|
new[] { 0d, 0d });
|
|
Verification.True(CreateConstraintBuilder().TryBuild(input, linearization, out QuadraticProgram problem,
|
|
out string reason), "fake solver problem builds: " + reason);
|
|
var expected = new QpSolveResult(QpSolveStatus.Solved, new double[problem.VariableCount], 0d, 0d, 0d, 1,
|
|
TimeSpan.Zero, "fake", string.Empty);
|
|
var solver = new FakeQpSolver(expected);
|
|
var settings = new QpSolverSettings(10, 1e-5d, 1e-5d, TimeSpan.FromSeconds(1d), true, false, false);
|
|
QpSolveResult actual = solver.Solve(problem, settings, new[] { 1d, 2d }, CancellationToken.None);
|
|
|
|
Verification.Equal(expected, actual, "fake solver returns configured result");
|
|
Verification.Equal(problem, solver.LastProblem, "fake solver records QP");
|
|
Verification.Equal(settings, solver.LastSettings, "fake solver records settings");
|
|
Verification.NearlyEqual(2d, solver.LastWarmStart[1], "fake solver records a defensive warm-start copy");
|
|
}
|
|
|
|
private static void VerifiesNonlinearGeometryInBothDirections()
|
|
{
|
|
double[] stations = { 0d, 0.7d, 2d };
|
|
LateralCandidate candidate = LateralCandidate.Integrate(stations, 0.1d, 0.1d, 0.05d,
|
|
new[] { 0.02d, -0.03d });
|
|
LateralGeometryEvaluator evaluator = CreateGeometryEvaluator();
|
|
LateralSolutionValidator validator = CreateGeometryValidator();
|
|
|
|
VerifyGeometryForDirection(TravelDirection.Forward, 0d, candidate, evaluator, validator);
|
|
VerifyGeometryForDirection(TravelDirection.Reverse, 0d, candidate, evaluator, validator);
|
|
VerifyGeometryForDirection(TravelDirection.Forward, 0.2d, candidate, evaluator, validator);
|
|
VerifyGeometryForDirection(TravelDirection.Reverse, 0.2d, candidate, evaluator, validator);
|
|
}
|
|
|
|
private static void VerifiesIndependentGeometryValidationRejectsUnsafeOrTamperedPaths()
|
|
{
|
|
LateralGeometryEvaluator evaluator = CreateGeometryEvaluator();
|
|
LateralSolutionValidator validator = CreateGeometryValidator();
|
|
double[] stations = { 0d, 1d, 2d };
|
|
|
|
LateralPlanningInput singularInput = CreateGeometryInput(TravelDirection.Forward, 1d, stations, 0.81d, 0d, 10d);
|
|
LateralCandidate singular = LateralCandidate.Integrate(stations, 0.81d, 0d, 0d, new[] { 0d, 0d });
|
|
Verification.True(!evaluator.TryEvaluate(singularInput, singular, out LateralPath singularPath, out string singularReason),
|
|
"Frenet denominator below 0.20 is rejected by reconstruction");
|
|
Verification.True(singularPath == null && singularReason.Length > 0, "singular reconstruction has no path");
|
|
|
|
LateralPlanningInput curvatureInput = CreateGeometryInput(TravelDirection.Forward, 0d, stations, 0d, 0d, 1d);
|
|
LateralCandidate excessiveCurvature = LateralCandidate.Integrate(stations, 0d, 0d, 2d, new[] { 0d, 0d });
|
|
Verification.True(evaluator.TryEvaluate(curvatureInput, excessiveCurvature, out LateralPath excessivePath,
|
|
out string excessiveReason), "curvature reconstruction remains geometric: " + excessiveReason);
|
|
Verification.True(!validator.TryValidate(curvatureInput, excessiveCurvature, excessivePath,
|
|
out LateralPath rejectedCurvature, out string curvatureReason), "curvature above vehicle limit is rejected");
|
|
Verification.True(rejectedCurvature == null && curvatureReason.Length > 0, "curvature failure has no validated path");
|
|
|
|
LateralCandidate valid = LateralCandidate.Integrate(stations, 0d, 0d, 0d, new[] { 0d, 0d });
|
|
Verification.True(evaluator.TryEvaluate(curvatureInput, valid, out LateralPath rawPath, out string rawReason),
|
|
"valid path reconstructs: " + rawReason);
|
|
var tamperedPoints = new List<LateralPathPoint>(rawPath.Points);
|
|
LateralPathPoint original = tamperedPoints[1];
|
|
tamperedPoints[1] = new LateralPathPoint(original.ReferenceS, original.PathS, original.L, original.DL,
|
|
original.DDL, original.DDDL, original.X + 0.01d, original.Y, original.VehicleYaw,
|
|
original.GeometricCurvature, original.VehicleCurvature, original.VehicleCurvatureDerivative);
|
|
Verification.True(!validator.TryValidate(curvatureInput, valid, new LateralPath(tamperedPoints, false),
|
|
out LateralPath rejectedTampered, out string tamperedReason),
|
|
"validator independently rejects a world-coordinate mismatch");
|
|
Verification.True(rejectedTampered == null && tamperedReason.Length > 0, "tampered path has no validated copy");
|
|
|
|
ExpectArgumentException(() => new LateralCandidate(stations, new[] { double.NaN, 0d, 0d },
|
|
new[] { 0d, 0d, 0d }, new[] { 0d, 0d, 0d }, new[] { 0d, 0d }), "non-finite lateral values are rejected");
|
|
ExpectArgumentException(() => new LateralPathPoint(0d, 0d, 0d, 0d, 0d, 0d, double.NaN, 0d, 0d, 0d, 0d, 0d),
|
|
"non-finite world geometry is rejected");
|
|
}
|
|
|
|
private static void VerifyGeometryForDirection(TravelDirection direction, double referenceCurvature,
|
|
LateralCandidate candidate,
|
|
LateralGeometryEvaluator evaluator, LateralSolutionValidator validator)
|
|
{
|
|
LateralPlanningInput input = CreateGeometryInput(direction, referenceCurvature, candidate.ReferenceStations,
|
|
candidate.L[0], candidate.DL[0], 10d);
|
|
Verification.True(evaluator.TryEvaluate(input, candidate, out LateralPath rawPath, out string evaluationReason),
|
|
direction + " geometry reconstructs: " + evaluationReason);
|
|
Verification.True(!rawPath.IsIndependentlyValidated, direction + " evaluator does not self-validate");
|
|
Verification.True(validator.TryValidate(input, candidate, rawPath, out LateralPath validatedPath,
|
|
out string validationReason), direction + " geometry validates: " + validationReason);
|
|
Verification.True(validatedPath.IsIndependentlyValidated, direction + " validation creates a marked immutable path");
|
|
|
|
double directionSign = direction == TravelDirection.Forward ? 1d : -1d;
|
|
Verification.NearlyEqual(0d, rawPath.Points[0].PathS, direction + " PathS starts at zero");
|
|
for (int index = 0; index < rawPath.Points.Count; index++)
|
|
{
|
|
LateralPathPoint point = rawPath.Points[index];
|
|
FrenetReferencePoint reference = ReferencePathInterpolator.Interpolate(input.ReferenceSegment, point.ReferenceS);
|
|
double denominator = 1d - reference.GeometricCurvature * point.L;
|
|
double expectedX = reference.X - point.L * Math.Sin(reference.TravelYaw);
|
|
double expectedY = reference.Y + point.L * Math.Cos(reference.TravelYaw);
|
|
double expectedTravelYaw = reference.TravelYaw + Math.Atan2(point.DL, denominator);
|
|
double expectedVehicleYaw = direction == TravelDirection.Forward
|
|
? AngleMath.NormalizeRadians(expectedTravelYaw)
|
|
: AngleMath.NormalizeRadians(expectedTravelYaw + Math.PI);
|
|
double expectedGeometricCurvature = CalculateGeometricCurvature(reference, point.L, point.DL, point.DDL,
|
|
directionSign * reference.VehicleCurvatureDerivative);
|
|
Verification.NearlyEqual(expectedX, point.X, direction + " world x " + index);
|
|
Verification.NearlyEqual(expectedY, point.Y, direction + " world y " + index);
|
|
Verification.NearlyEqual(expectedVehicleYaw, point.VehicleYaw, direction + " vehicle yaw " + index);
|
|
Verification.NearlyEqual(expectedGeometricCurvature, point.GeometricCurvature,
|
|
direction + " full Frenet curvature " + index);
|
|
Verification.NearlyEqual(directionSign * point.GeometricCurvature, point.VehicleCurvature,
|
|
direction + " vehicle curvature sign " + index);
|
|
if (index > 0)
|
|
{
|
|
LateralPathPoint previous = rawPath.Points[index - 1];
|
|
double chord = Math.Sqrt((point.X - previous.X) * (point.X - previous.X) +
|
|
(point.Y - previous.Y) * (point.Y - previous.Y));
|
|
Verification.NearlyEqual(previous.PathS + chord, point.PathS, direction + " actual chord PathS " + index);
|
|
Verification.True(point.PathS > previous.PathS, direction + " PathS strictly increases " + index);
|
|
}
|
|
}
|
|
|
|
LateralPathPoint check = rawPath.Points[1];
|
|
double signedSpeed = directionSign * 0.3d;
|
|
var trajectoryPoint = new EmTrajectoryPoint(check.X, check.Y, check.VehicleYaw, signedSpeed, 0d,
|
|
check.VehicleCurvature, 0, check.ReferenceS, check.PathS, direction, EmBoundaryType.None, 0d, 0d);
|
|
Verification.NearlyEqual(signedSpeed * check.VehicleCurvature, trajectoryPoint.YawRate,
|
|
direction + " yaw-rate identity");
|
|
Verification.NearlyEqual(0.3d * check.GeometricCurvature, trajectoryPoint.YawRate,
|
|
direction + " travel curvature yaw-rate identity");
|
|
}
|
|
|
|
private static DirectionSegmentView CreateStraightSegment(double referenceCurvatureDerivative = 0d,
|
|
double referenceCurvature = 0d)
|
|
{
|
|
var points = new List<SmoothedPathPoint>
|
|
{
|
|
Point(0d, 0d, referenceCurvatureDerivative, referenceCurvature),
|
|
Point(1d, 1d, referenceCurvatureDerivative, referenceCurvature),
|
|
Point(2d, 2d, referenceCurvatureDerivative, referenceCurvature),
|
|
};
|
|
return new DirectionSegmentView(0, TravelDirection.Forward, points,
|
|
new ReferenceBoundary(0, 0d, EmBoundaryType.None, 0d),
|
|
new ReferenceBoundary(0, 2d, EmBoundaryType.Goal, 2d), 0d);
|
|
}
|
|
|
|
private static SmoothedPathPoint Point(double x, double s, double curvatureDerivative = 0d, double curvature = 0d)
|
|
{
|
|
return new SmoothedPathPoint(x, 0d, 0d, 0d, s, TravelDirection.Forward, curvature, curvature,
|
|
curvatureDerivative, 1d,
|
|
false, SmoothedPathPointSource.Anchor);
|
|
}
|
|
|
|
private static EmPlannerConfiguration CreateUnitScaleConfiguration()
|
|
{
|
|
EmPlannerConfiguration configuration = EmPlannerConfiguration.CreateDefault();
|
|
configuration.Corridor.MaximumLateralOffsetMeters = 1d;
|
|
configuration.Lateral.MaximumLateralSlope = 1d;
|
|
configuration.Lateral.MaximumLateralSecondDerivativePerMeter = 1d;
|
|
configuration.Lateral.MaximumLateralThirdDerivativePerSquareMeter = 1d;
|
|
return configuration;
|
|
}
|
|
|
|
private static LateralPlanningInput CreateModelInput(EmTerminalType terminalType, EmPlannerConfiguration configuration,
|
|
IReadOnlyList<double> previousL, double referenceCurvatureDerivative = 0d, double referenceCurvature = 0d,
|
|
double startL = 0d, double corridorMinimum = -1d, double corridorMaximum = 1d,
|
|
double maximumVehicleCurvature = 1d)
|
|
{
|
|
DirectionSegmentView segment = CreateStraightSegment(referenceCurvatureDerivative, referenceCurvature);
|
|
double corridorSeed = Math.Max(corridorMinimum, Math.Min(corridorMaximum, 0d));
|
|
var stations = new[]
|
|
{
|
|
new LateralInterval(0d, corridorMinimum, corridorMaximum, startL),
|
|
new LateralInterval(1d, corridorMinimum, corridorMaximum, corridorSeed),
|
|
new LateralInterval(2d, corridorMinimum, corridorMaximum, corridorSeed),
|
|
};
|
|
var seed = new List<FrenetProjection>();
|
|
for (int index = 0; index < previousL.Count; index++)
|
|
seed.Add(new FrenetProjection(ReferencePathInterpolator.Interpolate(segment, index), previousL[index], 0d, 0d));
|
|
return new LateralPlanningInput(segment, new StaticCorridor(stations),
|
|
new FrenetProjection(ReferencePathInterpolator.Interpolate(segment, 0d), startL, 0d, 0d), terminalType,
|
|
CreateVehicle(maximumVehicleCurvature), configuration, seed);
|
|
}
|
|
|
|
private static LateralPlanningInput CreateGeometryInput(TravelDirection direction, double referenceCurvature,
|
|
IReadOnlyList<double> stations, double startL, double startDL, double maximumVehicleCurvature)
|
|
{
|
|
var points = new List<SmoothedPathPoint>(stations.Count);
|
|
for (int index = 0; index < stations.Count; index++)
|
|
{
|
|
double s = stations[index];
|
|
double travelYaw = referenceCurvature * s;
|
|
double x = Math.Abs(referenceCurvature) <= 1e-12d ? s : Math.Sin(travelYaw) / referenceCurvature;
|
|
double y = Math.Abs(referenceCurvature) <= 1e-12d ? 0d : (1d - Math.Cos(travelYaw)) / referenceCurvature;
|
|
double vehicleYaw = direction == TravelDirection.Forward ? travelYaw : travelYaw - Math.PI;
|
|
points.Add(new SmoothedPathPoint(x, y, AngleMath.NormalizeRadians(vehicleYaw), vehicleYaw, s, direction,
|
|
referenceCurvature, direction == TravelDirection.Forward ? referenceCurvature : -referenceCurvature,
|
|
0d, 1d, false, SmoothedPathPointSource.Anchor));
|
|
}
|
|
double end = stations[stations.Count - 1];
|
|
var segment = new DirectionSegmentView(0, direction, points,
|
|
new ReferenceBoundary(0, 0d, EmBoundaryType.None, 0d),
|
|
new ReferenceBoundary(0, end, EmBoundaryType.RollingSafetyStop, end), 0d);
|
|
var corridorStations = new List<LateralInterval>(stations.Count);
|
|
for (int index = 0; index < stations.Count; index++)
|
|
corridorStations.Add(new LateralInterval(stations[index], -1d, 1d, startL));
|
|
EmPlannerConfiguration configuration = CreateUnitScaleConfiguration();
|
|
configuration.Lateral.MaximumLateralSlope = 5d;
|
|
configuration.Lateral.MaximumLateralSecondDerivativePerMeter = 5d;
|
|
configuration.Lateral.MaximumLateralThirdDerivativePerSquareMeter = 5d;
|
|
double startDenominator = 1d - referenceCurvature * startL;
|
|
return new LateralPlanningInput(segment, new StaticCorridor(corridorStations),
|
|
new FrenetProjection(ReferencePathInterpolator.Interpolate(segment, stations[0]), startL,
|
|
Math.Atan2(startDL, startDenominator), 0d), EmTerminalType.RollingSafetyStop,
|
|
CreateVehicle(maximumVehicleCurvature), configuration, Array.Empty<FrenetProjection>());
|
|
}
|
|
|
|
private static double CalculateGeometricCurvature(FrenetReferencePoint reference, double l, double dl, double ddl,
|
|
double referenceCurvatureDerivative)
|
|
{
|
|
double a = 1d - reference.GeometricCurvature * l;
|
|
return (a * a * reference.GeometricCurvature + a * ddl + referenceCurvatureDerivative * l * dl +
|
|
2d * reference.GeometricCurvature * dl * dl) / Math.Pow(a * a + dl * dl, 1.5d);
|
|
}
|
|
|
|
private static LateralGeometryEvaluator CreateGeometryEvaluator()
|
|
{
|
|
return new LateralGeometryEvaluator();
|
|
}
|
|
|
|
private static LateralSolutionValidator CreateGeometryValidator()
|
|
{
|
|
return new LateralSolutionValidator();
|
|
}
|
|
|
|
private static LateralObjectiveBuilder CreateObjectiveBuilder()
|
|
{
|
|
return new LateralObjectiveBuilder();
|
|
}
|
|
|
|
private static LateralConstraintBuilder CreateConstraintBuilder()
|
|
{
|
|
return new LateralConstraintBuilder(CreateObjectiveBuilder());
|
|
}
|
|
|
|
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 int CountExactEqualityRows(QuadraticProgram problem, IReadOnlyDictionary<int, double> expected)
|
|
{
|
|
int count = 0;
|
|
for (int row = 0; row < problem.ConstraintCount; row++)
|
|
{
|
|
if (Math.Abs(problem.LowerBounds[row] - problem.UpperBounds[row]) <= 1e-12d &&
|
|
RowMatches(problem.ConstraintMatrix, row, expected))
|
|
{
|
|
count++;
|
|
}
|
|
}
|
|
return count;
|
|
}
|
|
|
|
private static bool HasFiniteNonEqualityBound(QuadraticProgram problem, int variable)
|
|
{
|
|
for (int row = 0; row < problem.ConstraintCount; row++)
|
|
{
|
|
if (Math.Abs(problem.LowerBounds[row] - problem.UpperBounds[row]) > 1e-12d &&
|
|
RowMatches(problem.ConstraintMatrix, row, new Dictionary<int, double> { { variable, 1d } }) &&
|
|
!double.IsInfinity(problem.LowerBounds[row]) && !double.IsInfinity(problem.UpperBounds[row]))
|
|
{
|
|
return true;
|
|
}
|
|
}
|
|
return false;
|
|
}
|
|
|
|
private static bool HasBoundWithUpper(QuadraticProgram problem, int variable, double upper)
|
|
{
|
|
for (int row = 0; row < problem.ConstraintCount; row++)
|
|
{
|
|
if (RowMatches(problem.ConstraintMatrix, row, new Dictionary<int, double> { { variable, 1d } }) &&
|
|
Math.Abs(problem.UpperBounds[row] - upper) <= 1e-12d)
|
|
{
|
|
return true;
|
|
}
|
|
}
|
|
return false;
|
|
}
|
|
|
|
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> expectedEntry in expected)
|
|
{
|
|
if (!actual.TryGetValue(expectedEntry.Key, out double value) || Math.Abs(value - expectedEntry.Value) > 1e-12d)
|
|
return false;
|
|
}
|
|
return true;
|
|
}
|
|
|
|
private static VehicleParameters CreateVehicle(double maximumCurvature = 1d)
|
|
{
|
|
return new VehicleParameters
|
|
{
|
|
LengthMeters = 0.1d,
|
|
WidthMeters = 0.1d,
|
|
SafetyMarginMeters = 0d,
|
|
MaximumCurvaturePerMeter = maximumCurvature,
|
|
};
|
|
}
|
|
|
|
private static LateralPathPoint CreatePathPoint(double referenceS)
|
|
{
|
|
return new LateralPathPoint(referenceS, referenceS, 0d, 0d, 0d, 0d, referenceS, 0d, 0d, 0d, 0d, 0d);
|
|
}
|
|
|
|
private static LateralVariableLayout CreateLayout(int stationCount)
|
|
{
|
|
return new LateralVariableLayout(stationCount);
|
|
}
|
|
|
|
private static void ExpectArgumentException(Action action, string name)
|
|
{
|
|
try
|
|
{
|
|
action();
|
|
}
|
|
catch (ArgumentException)
|
|
{
|
|
return;
|
|
}
|
|
throw new InvalidOperationException(name + " did not throw ArgumentException.");
|
|
}
|
|
}
|