feat: assemble lateral LS quadratic programs
This commit is contained in:
梁薄云
@@ -0,0 +1,208 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
|
||||
|
||||
/// <summary>Assembles one lateral SQP QP with exact dynamics and finite hard bounds.</summary>
|
||||
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<double>();
|
||||
var upper = new List<double>();
|
||||
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<double> lower, IList<double> 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<double> lower, IList<double> 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<double> lower, IList<double> 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<double> stations, LateralVariableLayout layout,
|
||||
SparseTripletBuilder constraints, IList<double> lower, IList<double> 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<double> lower, IList<double> 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<double> lower, IList<double> 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<double> lower, IList<double> upper, ref int row,
|
||||
double minimum, double maximum, IReadOnlyList<int> variables, IReadOnlyList<double> 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);
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,248 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using MultiWheelC.TrajectoryPlanning.CoarsePath;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
|
||||
|
||||
/// <summary>Builds normalized squared-residual costs in OSQP's 0.5*x'P*x + q'x convention.</summary>
|
||||
public sealed class LateralObjectiveBuilder
|
||||
{
|
||||
public void AddTerms(LateralPlanningInput input, LateralVariableLayout layout, LateralCandidate linearization,
|
||||
SparseTripletBuilder hessian, IList<double> 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<double> 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<double> stations, CurvatureAffine[] curvature,
|
||||
SparseTripletBuilder hessian, IList<double> 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<double> linearCost,
|
||||
IReadOnlyList<int> indices, IReadOnlyList<double> 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<FrenetProjection> 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; }
|
||||
}
|
||||
}
|
||||
@@ -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<double>();
|
||||
}
|
||||
|
||||
public QuadraticProgram? LastProblem { get; private set; }
|
||||
|
||||
public QpSolverSettings? LastSettings { get; private set; }
|
||||
|
||||
public IReadOnlyList<double> LastWarmStart { get; private set; }
|
||||
|
||||
public QpSolveResult Solve(QuadraticProgram problem, QpSolverSettings settings, IReadOnlyList<double> warmStart,
|
||||
CancellationToken cancellationToken)
|
||||
{
|
||||
LastProblem = problem ?? throw new ArgumentNullException(nameof(problem));
|
||||
LastSettings = settings ?? throw new ArgumentNullException(nameof(settings));
|
||||
var copy = new List<double>(warmStart == null ? 0 : warmStart.Count);
|
||||
if (warmStart != null)
|
||||
{
|
||||
for (int index = 0; index < warmStart.Count; index++)
|
||||
copy.Add(warmStart[index]);
|
||||
}
|
||||
LastWarmStart = new ReadOnlyCollection<double>(copy);
|
||||
return _result;
|
||||
}
|
||||
}
|
||||
@@ -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<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 DirectionSegmentView CreateStraightSegment(double referenceCurvatureDerivative = 0d,
|
||||
double referenceCurvature = 0d)
|
||||
{
|
||||
var points = new List<SmoothedPathPoint>
|
||||
{
|
||||
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<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 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 = 1d,
|
||||
MaximumCurvaturePerMeter = maximumCurvature,
|
||||
};
|
||||
}
|
||||
|
||||
|
||||
Reference in New Issue
Block a user