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

fixup! feat: apply ST stop constraints only at real boundaries

Browse Source
This commit is contained in:
梁薄云
2026-08-05 18:18:16 +08:00
parent dbc7b7ce96
commit 26bd822ec9
2 changed files with 8 additions and 5 deletions
Download Patch File Download Diff File Expand all files Collapse all files
ClumsyPilot/ParkrobTrajplanner/EMPlanner/Longitudinal/LongitudinalConstraintBuilder.cs
+5 -3
View File
@@ -40,7 +40,7 @@ public sealed class LongitudinalConstraintBuilder
{
return false;
}
if (input.InitialProgressSpeedMetersPerSecond > speedLimit.MaximumSpeedAt(0d) + 1e-12d ||
if (input.InitialProgressSpeedMetersPerSecond > directionMaximum + 1e-12d ||
input.InitialAccelerationMetersPerSecondSquared < -maximumDeceleration - 1e-12d ||
input.InitialAccelerationMetersPerSecondSquared > maximumAcceleration + 1e-12d)
{
@@ -90,8 +90,10 @@ public sealed class LongitudinalConstraintBuilder
if (iterate.S[index] < 0d || iterate.S[index] > input.PathUpperBoundS)
throw new ArgumentException("The ST iterate progress lies outside actual PathS bounds.");
AddSingleVariableRow(constraints, lower, upper, layout.S(index), 0d, input.PathUpperBoundS, ref row);
AddSingleVariableRow(constraints, lower, upper, layout.U(index), 0d,
Math.Min(input.DirectionMaximumSpeedMetersPerSecond, speedLimit.MaximumSpeedAt(iterate.S[index])), ref row);
double maximumSpeed = index == 0
? input.DirectionMaximumSpeedMetersPerSecond
: Math.Min(input.DirectionMaximumSpeedMetersPerSecond, speedLimit.MaximumSpeedAt(iterate.S[index]));
AddSingleVariableRow(constraints, lower, upper, layout.U(index), 0d, maximumSpeed, ref row);
AddSingleVariableRow(constraints, lower, upper, layout.A(index), -maximumDeceleration, maximumAcceleration,
ref row);
}
ClumsyPilot/ParkrobTrajplanner/EMPlanner/Longitudinal/LongitudinalSolutionValidator.cs
+3 -2
View File
@@ -62,8 +62,9 @@ public sealed class LongitudinalSolutionValidator
failureReason = "ST candidate violates physical bounds at knot " + index + ".";
return false;
}
double speedLimitAtProgress = speedLimit.MaximumSpeedAt(
Math.Max(0d, Math.Min(input.PathUpperBoundS, progress)));
double speedLimitAtProgress = index == 0
? input.DirectionMaximumSpeedMetersPerSecond
: speedLimit.MaximumSpeedAt(Math.Max(0d, Math.Min(input.PathUpperBoundS, progress)));
if (speed > speedLimitAtProgress + tolerance)
{
failureReason = "ST candidate violates the actual-PathS speed envelope at knot " + index +