From f703d418abce2ec07201929b46c84a71b2705fb9 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=E6=A2=81=E8=96=84=E4=BA=91?= Date: Tue, 4 Aug 2026 00:58:09 +0800 Subject: [PATCH] feat: assemble lateral LS quadratic programs --- .../Lateral/LateralConstraintBuilder.cs | 208 +++++++++++++ .../Lateral/LateralObjectiveBuilder.cs | 248 ++++++++++++++++ .../EMPlannerVerificationHost/FakeQpSolver.cs | 39 +++ .../LateralModelChecks.cs | 279 +++++++++++++++++- 4 files changed, 766 insertions(+), 8 deletions(-) create mode 100644 ClumsyPilot/ParkrobTrajplanner/EMPlanner/Lateral/LateralConstraintBuilder.cs create mode 100644 ClumsyPilot/ParkrobTrajplanner/EMPlanner/Lateral/LateralObjectiveBuilder.cs create mode 100644 ClumsyPilot/tests/EMPlannerVerificationHost/FakeQpSolver.cs diff --git a/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Lateral/LateralConstraintBuilder.cs b/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Lateral/LateralConstraintBuilder.cs new file mode 100644 index 0000000..2b5aef7 --- /dev/null +++ b/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Lateral/LateralConstraintBuilder.cs @@ -0,0 +1,208 @@ +using System; +using System.Collections.Generic; + +namespace MultiWheelC.TrajectoryPlanning.EMPlanner; + +/// Assembles one lateral SQP QP with exact dynamics and finite hard bounds. +public sealed class LateralConstraintBuilder +{ + private const double Epsilon = 1e-12d; + private readonly LateralObjectiveBuilder _objectiveBuilder; + + public LateralConstraintBuilder(LateralObjectiveBuilder objectiveBuilder) + { + _objectiveBuilder = objectiveBuilder ?? throw new ArgumentNullException(nameof(objectiveBuilder)); + } + + public bool TryBuild(LateralPlanningInput input, LateralCandidate linearization, out QuadraticProgram problem, + out string failureReason) + { + problem = null; + failureReason = string.Empty; + if (input == null || linearization == null) + { + failureReason = "Lateral input and linearization are required."; + return false; + } + if (!TryValidateCandidateStations(input, linearization, out failureReason)) + return false; + + try + { + var layout = new LateralVariableLayout(input.ReferenceStations.Count); + var hessian = new SparseTripletBuilder(layout.VariableCount, layout.VariableCount, true); + var linearCost = new double[layout.VariableCount]; + _objectiveBuilder.AddTerms(input, layout, linearization, hessian, linearCost); + + int terminalRows = input.TerminalType == EmTerminalType.RollingSafetyStop ? 0 : 2; + var constraints = new SparseTripletBuilder(7 * layout.StationCount - 2 + terminalRows, layout.VariableCount); + var lower = new List(); + var upper = new List(); + int row = 0; + + if (!TryAddLateralBounds(input, layout, linearization, constraints, lower, upper, ref row, out failureReason)) + return false; + AddDerivativeBounds(input, layout, constraints, lower, upper, ref row); + AddStartConstraints(input, layout, constraints, lower, upper, ref row); + AddExactDynamics(input.ReferenceStations, layout, constraints, lower, upper, ref row); + if (input.TerminalType != EmTerminalType.RollingSafetyStop) + AddTerminalConstraints(layout, constraints, lower, upper, ref row); + + if (row != 7 * layout.StationCount - 2 + terminalRows) + throw new InvalidOperationException("Lateral constraint row accounting is inconsistent."); + problem = new QuadraticProgram(hessian.Build(), linearCost, constraints.Build(), lower, upper); + return true; + } + catch (ArgumentException exception) + { + failureReason = exception.Message; + return false; + } + } + + private static bool TryValidateCandidateStations(LateralPlanningInput input, LateralCandidate candidate, + out string failureReason) + { + failureReason = string.Empty; + if (candidate.ReferenceStations.Count != input.ReferenceStations.Count) + { + failureReason = "Linearization station count does not match the lateral input."; + return false; + } + for (int index = 0; index < input.ReferenceStations.Count; index++) + { + if (Math.Abs(candidate.ReferenceStations[index] - input.ReferenceStations[index]) > Epsilon) + { + failureReason = "Linearization stations do not match the lateral input."; + return false; + } + } + return true; + } + + private static bool TryAddLateralBounds(LateralPlanningInput input, LateralVariableLayout layout, + LateralCandidate linearization, SparseTripletBuilder constraints, IList lower, IList upper, + ref int row, out string failureReason) + { + failureReason = string.Empty; + double maximumOffset = RequireNonnegative(input.Configuration.Corridor.MaximumLateralOffsetMeters, + "maximum lateral offset"); + double trustRegion = RequirePositive(input.Configuration.Lateral.MaximumLateralStepPerIterationMeters, + "lateral trust region"); + double minimumDenominator = RequirePositive(input.Configuration.Frenet.MinimumFrenetDenominator, + "minimum Frenet denominator"); + + for (int station = 0; station < layout.StationCount; station++) + { + LateralInterval corridor = input.Corridor.Stations[station]; + double minimum = Math.Max(corridor.MinimumL, Math.Max(-maximumOffset, linearization.L[station] - trustRegion)); + double maximum = Math.Min(corridor.MaximumL, Math.Min(maximumOffset, linearization.L[station] + trustRegion)); + double referenceCurvature = ReferencePathInterpolator.Interpolate(input.ReferenceSegment, + input.ReferenceStations[station]).GeometricCurvature; + if (referenceCurvature > 0d) + maximum = Math.Min(maximum, (1d - minimumDenominator) / referenceCurvature); + else if (referenceCurvature < 0d) + minimum = Math.Max(minimum, (1d - minimumDenominator) / referenceCurvature); + + if (!IsFinite(minimum) || !IsFinite(maximum) || minimum > maximum + Epsilon) + { + failureReason = "The lateral corridor, offset, trust-region, and Frenet denominator bounds do not intersect at station " + station + "."; + return false; + } + AddSingleVariableRow(constraints, lower, upper, ref row, layout.L(station), minimum, maximum); + } + return true; + } + + private static void AddDerivativeBounds(LateralPlanningInput input, LateralVariableLayout layout, + SparseTripletBuilder constraints, IList lower, IList upper, ref int row) + { + double slope = RequirePositive(input.Configuration.Lateral.MaximumLateralSlope, "maximum lateral slope"); + double second = RequirePositive(input.Configuration.Lateral.MaximumLateralSecondDerivativePerMeter, + "maximum lateral second derivative"); + double third = RequirePositive(input.Configuration.Lateral.MaximumLateralThirdDerivativePerSquareMeter, + "maximum lateral third derivative"); + for (int station = 0; station < layout.StationCount; station++) + { + AddSingleVariableRow(constraints, lower, upper, ref row, layout.DL(station), -slope, slope); + AddSingleVariableRow(constraints, lower, upper, ref row, layout.DDL(station), -second, second); + } + for (int interval = 0; interval < layout.StationCount - 1; interval++) + AddSingleVariableRow(constraints, lower, upper, ref row, layout.DDDL(interval), -third, third); + } + + private static void AddStartConstraints(LateralPlanningInput input, LateralVariableLayout layout, + SparseTripletBuilder constraints, IList lower, IList upper, ref int row) + { + double denominator = 1d - input.StartProjection.ReferencePoint.GeometricCurvature * + input.StartProjection.LateralOffset; + double startSlope = denominator * Math.Tan(input.StartProjection.HeadingError); + if (!IsFinite(startSlope)) + throw new ArgumentException("The start lateral slope is non-finite.", nameof(input)); + AddSingleVariableRow(constraints, lower, upper, ref row, layout.L(0), input.StartProjection.LateralOffset, + input.StartProjection.LateralOffset); + AddSingleVariableRow(constraints, lower, upper, ref row, layout.DL(0), startSlope, startSlope); + } + + private static void AddExactDynamics(IReadOnlyList stations, LateralVariableLayout layout, + SparseTripletBuilder constraints, IList lower, IList upper, ref int row) + { + for (int interval = 0; interval < layout.StationCount - 1; interval++) + { + double ds = stations[interval + 1] - stations[interval]; + AddRow(constraints, lower, upper, ref row, 0d, 0d, + new[] { layout.DDL(interval), layout.DDL(interval + 1), layout.DDDL(interval) }, + new[] { -1d, 1d, -ds }); + AddRow(constraints, lower, upper, ref row, 0d, 0d, + new[] { layout.DL(interval), layout.DL(interval + 1), layout.DDL(interval), layout.DDDL(interval) }, + new[] { -1d, 1d, -ds, -0.5d * ds * ds }); + AddRow(constraints, lower, upper, ref row, 0d, 0d, + new[] { layout.L(interval), layout.L(interval + 1), layout.DL(interval), layout.DDL(interval), layout.DDDL(interval) }, + new[] { -1d, 1d, -ds, -0.5d * ds * ds, -ds * ds * ds / 6d }); + } + } + + private static void AddTerminalConstraints(LateralVariableLayout layout, SparseTripletBuilder constraints, + IList lower, IList upper, ref int row) + { + AddSingleVariableRow(constraints, lower, upper, ref row, layout.L(layout.StationCount - 1), 0d, 0d); + AddSingleVariableRow(constraints, lower, upper, ref row, layout.DL(layout.StationCount - 1), 0d, 0d); + } + + private static void AddSingleVariableRow(SparseTripletBuilder constraints, IList lower, IList upper, + ref int row, int variable, double minimum, double maximum) + { + AddRow(constraints, lower, upper, ref row, minimum, maximum, new[] { variable }, new[] { 1d }); + } + + private static void AddRow(SparseTripletBuilder constraints, IList lower, IList upper, ref int row, + double minimum, double maximum, IReadOnlyList variables, IReadOnlyList coefficients) + { + if (!IsFinite(minimum) || !IsFinite(maximum) || minimum > maximum || variables.Count != coefficients.Count) + throw new ArgumentException("Lateral constraint bounds are invalid."); + for (int index = 0; index < variables.Count; index++) + constraints.Add(row, variables[index], coefficients[index]); + lower.Add(minimum); + upper.Add(maximum); + row++; + } + + private static double RequirePositive(double value, string name) + { + if (!IsFinite(value) || value <= 0d) + throw new ArgumentOutOfRangeException(name); + return value; + } + + private static double RequireNonnegative(double value, string name) + { + if (!IsFinite(value) || value < 0d) + throw new ArgumentOutOfRangeException(name); + return value; + } + + private static bool IsFinite(double value) + { + return !double.IsNaN(value) && !double.IsInfinity(value); + } +} diff --git a/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Lateral/LateralObjectiveBuilder.cs b/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Lateral/LateralObjectiveBuilder.cs new file mode 100644 index 0000000..5f0c576 --- /dev/null +++ b/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Lateral/LateralObjectiveBuilder.cs @@ -0,0 +1,248 @@ +using System; +using System.Collections.Generic; +using MultiWheelC.TrajectoryPlanning.CoarsePath; + +namespace MultiWheelC.TrajectoryPlanning.EMPlanner; + +/// Builds normalized squared-residual costs in OSQP's 0.5*x'P*x + q'x convention. +public sealed class LateralObjectiveBuilder +{ + public void AddTerms(LateralPlanningInput input, LateralVariableLayout layout, LateralCandidate linearization, + SparseTripletBuilder hessian, IList linearCost) + { + if (input == null) + throw new ArgumentNullException(nameof(input)); + if (layout == null) + throw new ArgumentNullException(nameof(layout)); + if (linearization == null) + throw new ArgumentNullException(nameof(linearization)); + if (hessian == null) + throw new ArgumentNullException(nameof(hessian)); + if (linearCost == null || linearCost.Count != layout.VariableCount) + throw new ArgumentException("Linear cost must match the lateral layout.", nameof(linearCost)); + + LateralConfiguration lateral = input.Configuration.Lateral ?? throw new ArgumentException("Missing lateral configuration."); + LateralWeights weights = lateral.Weights ?? throw new ArgumentException("Missing lateral weights."); + double lateralScale = RequirePositive(input.Configuration.Corridor.MaximumLateralOffsetMeters, "lateral scale"); + double slopeScale = RequirePositive(lateral.MaximumLateralSlope, "slope scale"); + double secondDerivativeScale = RequirePositive(lateral.MaximumLateralSecondDerivativePerMeter, "second-derivative scale"); + double thirdDerivativeScale = RequirePositive(lateral.MaximumLateralThirdDerivativePerSquareMeter, "third-derivative scale"); + double curvatureScale = RequirePositive(GetMaximumVehicleCurvature(input.Vehicle), "curvature scale"); + double curvatureVariationScale = GetCurvatureVariationScale(input); + + for (int station = 0; station < layout.StationCount; station++) + { + AddSquaredResidual(hessian, linearCost, new[] { layout.L(station) }, new[] { 1d }, 0d, + weights.ReferenceOffset, lateralScale); + AddSquaredResidual(hessian, linearCost, new[] { layout.DL(station) }, new[] { 1d }, 0d, + weights.HeadingDeviation, slopeScale); + AddSquaredResidual(hessian, linearCost, new[] { layout.DDL(station) }, new[] { 1d }, 0d, + weights.SecondDerivative, secondDerivativeScale); + } + for (int interval = 0; interval < layout.StationCount - 1; interval++) + { + AddSquaredResidual(hessian, linearCost, new[] { layout.DDDL(interval) }, new[] { 1d }, 0d, + weights.ThirdDerivative, thirdDerivativeScale); + } + + AddPreviousTrajectoryTerms(input, layout, hessian, linearCost, weights.PreviousTrajectory, lateralScale); + CurvatureAffine[] curvature = CreateCurvatureAffines(input, layout, linearization); + for (int station = 0; station < curvature.Length; station++) + { + AddSquaredResidual(hessian, linearCost, curvature[station].Indices, curvature[station].Gradient, + curvature[station].Constant, weights.Curvature, curvatureScale); + } + AddCurvatureVariationTerms(input.ReferenceStations, curvature, hessian, linearCost, weights.CurvatureVariation, + curvatureVariationScale); + if (input.TerminalType == EmTerminalType.RollingSafetyStop) + { + AddSquaredResidual(hessian, linearCost, new[] { layout.L(layout.StationCount - 1) }, new[] { 1d }, 0d, + weights.RollingTerminal, lateralScale); + } + } + + private static void AddPreviousTrajectoryTerms(LateralPlanningInput input, LateralVariableLayout layout, + SparseTripletBuilder hessian, IList linearCost, double weight, double lateralScale) + { + if (input.PreviousTrajectorySeed.Count == 0) + return; + + for (int station = 0; station < layout.StationCount; station++) + { + double previousL = InterpolatePreviousL(input.PreviousTrajectorySeed, input.ReferenceStations[station]); + AddSquaredResidual(hessian, linearCost, new[] { layout.L(station) }, new[] { 1d }, -previousL, + weight, lateralScale); + } + } + + private static CurvatureAffine[] CreateCurvatureAffines(LateralPlanningInput input, LateralVariableLayout layout, + LateralCandidate linearization) + { + var affines = new CurvatureAffine[layout.StationCount]; + double directionSign = input.ReferenceSegment.Direction == TravelDirection.Forward ? 1d : -1d; + for (int station = 0; station < layout.StationCount; station++) + { + FrenetReferencePoint reference = ReferencePathInterpolator.Interpolate(input.ReferenceSegment, + input.ReferenceStations[station]); + double l = linearization.L[station]; + double dl = linearization.DL[station]; + double ddl = linearization.DDL[station]; + double referenceCurvature = reference.GeometricCurvature; + double referenceCurvatureDerivative = directionSign * reference.VehicleCurvatureDerivative; + double a = 1d - referenceCurvature * l; + double denominatorSquared = a * a + dl * dl; + if (!IsFinite(denominatorSquared) || denominatorSquared <= 0d) + throw new ArgumentException("Curvature linearization denominator is invalid.", nameof(linearization)); + + double denominatorPow3Over2 = denominatorSquared * Math.Sqrt(denominatorSquared); + double denominatorPow5Over2 = denominatorPow3Over2 * denominatorSquared; + double numerator = a * a * referenceCurvature + a * ddl + + referenceCurvatureDerivative * l * dl + 2d * referenceCurvature * dl * dl; + double geometricCurvature = numerator / denominatorPow3Over2; + double dNumeratorDLateral = -2d * a * referenceCurvature * referenceCurvature - + referenceCurvature * ddl + referenceCurvatureDerivative * dl; + double dNumeratorDSlope = referenceCurvatureDerivative * l + 4d * referenceCurvature * dl; + double dDenominatorSquaredDLateral = -2d * a * referenceCurvature; + double dDenominatorSquaredDSlope = 2d * dl; + double dGeometricDLateral = dNumeratorDLateral / denominatorPow3Over2 - + 1.5d * numerator * dDenominatorSquaredDLateral / denominatorPow5Over2; + double dGeometricDSlope = dNumeratorDSlope / denominatorPow3Over2 - + 1.5d * numerator * dDenominatorSquaredDSlope / denominatorPow5Over2; + double dGeometricDSecondDerivative = a / denominatorPow3Over2; + double vehicleCurvature = directionSign * geometricCurvature; + double[] gradient = + { + directionSign * dGeometricDLateral, + directionSign * dGeometricDSlope, + directionSign * dGeometricDSecondDerivative, + }; + double constant = vehicleCurvature - gradient[0] * l - gradient[1] * dl - gradient[2] * ddl; + if (!IsFinite(vehicleCurvature) || !IsFinite(constant) || !IsFinite(gradient[0]) || + !IsFinite(gradient[1]) || !IsFinite(gradient[2])) + { + throw new ArgumentException("Curvature linearization is non-finite.", nameof(linearization)); + } + affines[station] = new CurvatureAffine(new[] { layout.L(station), layout.DL(station), layout.DDL(station) }, + gradient, constant); + } + return affines; + } + + private static void AddCurvatureVariationTerms(IReadOnlyList stations, CurvatureAffine[] curvature, + SparseTripletBuilder hessian, IList linearCost, double weight, double scale) + { + for (int station = 0; station < curvature.Length; station++) + { + int lower = station == 0 ? 0 : station - 1; + int upper = station == curvature.Length - 1 ? curvature.Length - 1 : station + 1; + double ds = stations[upper] - stations[lower]; + if (!IsFinite(ds) || ds <= 0d) + throw new ArgumentException("Curvature variation requires strictly increasing stations.", nameof(stations)); + CurvatureAffine left = curvature[lower]; + CurvatureAffine right = curvature[upper]; + var indices = new int[left.Indices.Length + right.Indices.Length]; + var gradient = new double[indices.Length]; + for (int index = 0; index < left.Indices.Length; index++) + { + indices[index] = left.Indices[index]; + gradient[index] = -left.Gradient[index] / ds; + indices[left.Indices.Length + index] = right.Indices[index]; + gradient[left.Indices.Length + index] = right.Gradient[index] / ds; + } + AddSquaredResidual(hessian, linearCost, indices, gradient, (right.Constant - left.Constant) / ds, weight, scale); + } + } + + private static void AddSquaredResidual(SparseTripletBuilder hessian, IList linearCost, + IReadOnlyList indices, IReadOnlyList gradient, double constant, double weight, double scale) + { + if (indices.Count != gradient.Count || indices.Count == 0 || !IsFinite(constant)) + throw new ArgumentException("Affine residual is invalid."); + if (!IsFinite(weight) || weight < 0d) + throw new ArgumentOutOfRangeException(nameof(weight)); + double coefficient = 2d * weight / (scale * scale); + for (int left = 0; left < indices.Count; left++) + { + if (!IsFinite(gradient[left]) || indices[left] < 0 || indices[left] >= linearCost.Count) + throw new ArgumentOutOfRangeException(nameof(gradient)); + linearCost[indices[left]] += coefficient * constant * gradient[left]; + for (int right = left; right < indices.Count; right++) + { + if (!IsFinite(gradient[right]) || indices[right] < 0 || indices[right] >= linearCost.Count) + throw new ArgumentOutOfRangeException(nameof(gradient)); + int row = Math.Min(indices[left], indices[right]); + int column = Math.Max(indices[left], indices[right]); + hessian.Add(row, column, coefficient * gradient[left] * gradient[right]); + } + } + } + + private static double InterpolatePreviousL(IReadOnlyList seed, double referenceS) + { + if (referenceS <= seed[0].ReferenceS) + return seed[0].LateralOffset; + for (int index = 1; index < seed.Count; index++) + { + if (referenceS <= seed[index].ReferenceS) + { + FrenetProjection lower = seed[index - 1]; + FrenetProjection upper = seed[index]; + double span = upper.ReferenceS - lower.ReferenceS; + if (span <= 0d) + return upper.LateralOffset; + return lower.LateralOffset + (upper.LateralOffset - lower.LateralOffset) * + (referenceS - lower.ReferenceS) / span; + } + } + return seed[seed.Count - 1].LateralOffset; + } + + private static double GetCurvatureVariationScale(LateralPlanningInput input) + { + double maximum = 0d; + for (int station = 0; station < input.ReferenceStations.Count; station++) + { + FrenetReferencePoint reference = ReferencePathInterpolator.Interpolate(input.ReferenceSegment, + input.ReferenceStations[station]); + maximum = Math.Max(maximum, Math.Abs(reference.VehicleCurvatureDerivative)); + } + return Math.Max(1d, maximum); + } + + private static double GetMaximumVehicleCurvature(VehicleParameters vehicle) + { + if (vehicle == null) + throw new ArgumentNullException(nameof(vehicle)); + if (vehicle.MaximumCurvaturePerMeter.HasValue) + return vehicle.MaximumCurvaturePerMeter.Value; + if (vehicle.MinimumTurningRadiusMeters.HasValue && vehicle.MinimumTurningRadiusMeters.Value > 0d) + return 1d / vehicle.MinimumTurningRadiusMeters.Value; + throw new ArgumentException("Vehicle maximum curvature is required.", nameof(vehicle)); + } + + private static double RequirePositive(double value, string name) + { + if (!IsFinite(value) || value <= 0d) + throw new ArgumentOutOfRangeException(name); + return value; + } + + private static bool IsFinite(double value) + { + return !double.IsNaN(value) && !double.IsInfinity(value); + } + + private sealed class CurvatureAffine + { + public CurvatureAffine(int[] indices, double[] gradient, double constant) + { + Indices = indices; + Gradient = gradient; + Constant = constant; + } + + public int[] Indices { get; } + public double[] Gradient { get; } + public double Constant { get; } + } +} diff --git a/ClumsyPilot/tests/EMPlannerVerificationHost/FakeQpSolver.cs b/ClumsyPilot/tests/EMPlannerVerificationHost/FakeQpSolver.cs new file mode 100644 index 0000000..9d64a74 --- /dev/null +++ b/ClumsyPilot/tests/EMPlannerVerificationHost/FakeQpSolver.cs @@ -0,0 +1,39 @@ +using System; +using System.Collections.Generic; +using System.Collections.ObjectModel; +using System.Threading; +using MultiWheelC.TrajectoryPlanning.EMPlanner; + +namespace EMPlannerVerificationHost; + +internal sealed class FakeQpSolver : IQpSolver +{ + private readonly QpSolveResult _result; + + public FakeQpSolver(QpSolveResult result) + { + _result = result ?? throw new ArgumentNullException(nameof(result)); + LastWarmStart = Array.Empty(); + } + + public QuadraticProgram? LastProblem { get; private set; } + + public QpSolverSettings? LastSettings { get; private set; } + + public IReadOnlyList LastWarmStart { get; private set; } + + public QpSolveResult Solve(QuadraticProgram problem, QpSolverSettings settings, IReadOnlyList warmStart, + CancellationToken cancellationToken) + { + LastProblem = problem ?? throw new ArgumentNullException(nameof(problem)); + LastSettings = settings ?? throw new ArgumentNullException(nameof(settings)); + var copy = new List(warmStart == null ? 0 : warmStart.Count); + if (warmStart != null) + { + for (int index = 0; index < warmStart.Count; index++) + copy.Add(warmStart[index]); + } + LastWarmStart = new ReadOnlyCollection(copy); + return _result; + } +} diff --git a/ClumsyPilot/tests/EMPlannerVerificationHost/LateralModelChecks.cs b/ClumsyPilot/tests/EMPlannerVerificationHost/LateralModelChecks.cs index 40d8130..ea8b4fa 100644 --- a/ClumsyPilot/tests/EMPlannerVerificationHost/LateralModelChecks.cs +++ b/ClumsyPilot/tests/EMPlannerVerificationHost/LateralModelChecks.cs @@ -1,5 +1,6 @@ using System; using System.Collections.Generic; +using System.Threading; using EMPlannerVerificationHost; using MultiWheelC.TrajectoryPlanning.CoarsePath; using MultiWheelC.TrajectoryPlanning.CoarsePath.Vehicle; @@ -15,6 +16,10 @@ internal static class LateralModelChecks VerifiesExactDiscreteDynamicsForUnequalStations(); VerifiesPlanningInputBoundariesAndDefensiveCopies(); VerifiesLateralResultPublicationContract(); + VerifiesNormalizedObjectiveAndHardConstraints(); + VerifiesAllNamedCostScales(); + VerifiesEmptyHardBoundIntersectionFailsBeforeSolve(); + VerifiesFakeSolverCapturesTheNeutralQpBoundary(); } private static void VerifiesDeterministicVariableLayout() @@ -127,33 +132,291 @@ internal static class LateralModelChecks Verification.Equal(validated, result.Path, "fallback path is preserved"); } - private static DirectionSegmentView CreateStraightSegment() + 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 + { + { layout.DDL(interval), -1d }, + { layout.DDL(interval + 1), 1d }, + { layout.DDDL(interval), -ds }, + }), "ddl dynamics equality " + interval); + Verification.Equal(1, CountExactEqualityRows(problem, new Dictionary + { + { 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 + { + { 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 { { layout.L(2), 1d } }), + "goal terminal l equality"); + Verification.Equal(1, CountExactEqualityRows(problem, new Dictionary { { 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 { { layout.L(2), 1d } }), + "rolling terminal has no l equality"); + Verification.Equal(0, CountExactEqualityRows(rollingProblem, new Dictionary { { 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(), 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(), 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()); + 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 DirectionSegmentView CreateStraightSegment(double referenceCurvatureDerivative = 0d, + double referenceCurvature = 0d) { var points = new List { - Point(0d, 0d), - Point(1d, 1d), - Point(2d, 2d), + 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) + private static SmoothedPathPoint Point(double x, double s, double curvatureDerivative = 0d, double curvature = 0d) { - return new SmoothedPathPoint(x, 0d, 0d, 0d, s, TravelDirection.Forward, 0d, 0d, 0d, 1d, + return new SmoothedPathPoint(x, 0d, 0d, 0d, s, TravelDirection.Forward, curvature, curvature, + curvatureDerivative, 1d, false, SmoothedPathPointSource.Anchor); } - private static VehicleParameters CreateVehicle() + 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 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(); + 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 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 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 { { 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 { { variable, 1d } }) && + Math.Abs(problem.UpperBounds[row] - upper) <= 1e-12d) + { + return true; + } + } + return false; + } + + private static bool RowMatches(SparseCscMatrix matrix, int targetRow, IReadOnlyDictionary expected) + { + var actual = new Dictionary(); + 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 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 = 1d, + MaximumCurvaturePerMeter = maximumCurvature, }; }