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; return false;
} }
if (input.InitialProgressSpeedMetersPerSecond > speedLimit.MaximumSpeedAt(0d) + 1e-12d || if (input.InitialProgressSpeedMetersPerSecond > directionMaximum + 1e-12d ||
input.InitialAccelerationMetersPerSecondSquared < -maximumDeceleration - 1e-12d || input.InitialAccelerationMetersPerSecondSquared < -maximumDeceleration - 1e-12d ||
input.InitialAccelerationMetersPerSecondSquared > maximumAcceleration + 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) if (iterate.S[index] < 0d || iterate.S[index] > input.PathUpperBoundS)
throw new ArgumentException("The ST iterate progress lies outside actual PathS bounds."); 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.S(index), 0d, input.PathUpperBoundS, ref row);
AddSingleVariableRow(constraints, lower, upper, layout.U(index), 0d, double maximumSpeed = index == 0
Math.Min(input.DirectionMaximumSpeedMetersPerSecond, speedLimit.MaximumSpeedAt(iterate.S[index])), ref row); ? 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, AddSingleVariableRow(constraints, lower, upper, layout.A(index), -maximumDeceleration, maximumAcceleration,
ref row); 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 + "."; failureReason = "ST candidate violates physical bounds at knot " + index + ".";
return false; return false;
} }
double speedLimitAtProgress = speedLimit.MaximumSpeedAt( double speedLimitAtProgress = index == 0
Math.Max(0d, Math.Min(input.PathUpperBoundS, progress))); ? input.DirectionMaximumSpeedMetersPerSecond
: speedLimit.MaximumSpeedAt(Math.Max(0d, Math.Min(input.PathUpperBoundS, progress)));
if (speed > speedLimitAtProgress + tolerance) if (speed > speedLimitAtProgress + tolerance)
{ {
failureReason = "ST candidate violates the actual-PathS speed envelope at knot " + index + failureReason = "ST candidate violates the actual-PathS speed envelope at knot " + index +