feat: add lateral optimization model
This commit is contained in:
梁薄云
@@ -0,0 +1,182 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using EMPlannerVerificationHost;
|
||||
using MultiWheelC.TrajectoryPlanning.CoarsePath;
|
||||
using MultiWheelC.TrajectoryPlanning.CoarsePath.Vehicle;
|
||||
using MultiWheelC.TrajectoryPlanning.PathSmoothing;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
|
||||
|
||||
internal static class LateralModelChecks
|
||||
{
|
||||
public static void Run()
|
||||
{
|
||||
VerifiesDeterministicVariableLayout();
|
||||
VerifiesExactDiscreteDynamicsForUnequalStations();
|
||||
VerifiesPlanningInputBoundariesAndDefensiveCopies();
|
||||
VerifiesLateralResultPublicationContract();
|
||||
}
|
||||
|
||||
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 DirectionSegmentView CreateStraightSegment()
|
||||
{
|
||||
var points = new List<SmoothedPathPoint>
|
||||
{
|
||||
Point(0d, 0d),
|
||||
Point(1d, 1d),
|
||||
Point(2d, 2d),
|
||||
};
|
||||
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)
|
||||
{
|
||||
return new SmoothedPathPoint(x, 0d, 0d, 0d, s, TravelDirection.Forward, 0d, 0d, 0d, 1d,
|
||||
false, SmoothedPathPointSource.Anchor);
|
||||
}
|
||||
|
||||
private static VehicleParameters CreateVehicle()
|
||||
{
|
||||
return new VehicleParameters
|
||||
{
|
||||
LengthMeters = 0.1d,
|
||||
WidthMeters = 0.1d,
|
||||
SafetyMarginMeters = 0d,
|
||||
MaximumCurvaturePerMeter = 1d,
|
||||
};
|
||||
}
|
||||
|
||||
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.");
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user