First_Dev_Dept/ParkingRobot
10
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

chore: save current workspace progress

Browse Source
This commit is contained in:
梁薄云
2026-08-09 22:13:18 +08:00
parent 650c2ab0e3
commit 2f4fd15e52
449 changed files with 76593 additions and 971 deletions
Download Patch File Download Diff File Expand all files Collapse all files
ClumsyPilot/ParkrobTrajplanner/EMPlanner/Lateral/LateralConstraintBuilder.cs
+18 -2
View File
@@ -35,7 +35,7 @@ public sealed class LateralConstraintBuilder
_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 constraints = new SparseTripletBuilder(8 * layout.StationCount - 2 + terminalRows, layout.VariableCount);
var lower = new List<double>();
var upper = new List<double>();
int row = 0;
@@ -43,12 +43,13 @@ public sealed class LateralConstraintBuilder
if (!TryAddLateralBounds(input, layout, linearization, constraints, lower, upper, ref row, out failureReason))
return false;
AddDerivativeBounds(input, layout, constraints, lower, upper, ref row);
AddCurvatureBounds(input, layout, linearization, 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)
if (row != 8 * 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;
@@ -131,6 +132,21 @@ public sealed class LateralConstraintBuilder
AddSingleVariableRow(constraints, lower, upper, ref row, layout.DDDL(interval), -third, third);
}
private static void AddCurvatureBounds(LateralPlanningInput input, LateralVariableLayout layout,
LateralCandidate linearization, SparseTripletBuilder constraints, IList<double> lower, IList<double> upper,
ref int row)
{
double maximumCurvature = LateralCurvatureLinearization.GetMaximumVehicleCurvature(input.Vehicle);
IReadOnlyList<LateralCurvatureLinearization> affines = LateralCurvatureLinearization.Create(input, layout,
linearization);
for (int station = 0; station < affines.Count; station++)
{
LateralCurvatureLinearization affine = affines[station];
AddRow(constraints, lower, upper, ref row, -maximumCurvature - affine.Constant,
maximumCurvature - affine.Constant, affine.VariableIndices, affine.Gradient);
}
}
private static void AddStartConstraints(LateralPlanningInput input, LateralVariableLayout layout,
SparseTripletBuilder constraints, IList<double> lower, IList<double> upper, ref int row)
{