diff --git a/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Longitudinal/LongitudinalConstraintBuilder.cs b/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Longitudinal/LongitudinalConstraintBuilder.cs index 040fc17..49c5a5c 100644 --- a/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Longitudinal/LongitudinalConstraintBuilder.cs +++ b/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Longitudinal/LongitudinalConstraintBuilder.cs @@ -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); } diff --git a/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Longitudinal/LongitudinalSolutionValidator.cs b/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Longitudinal/LongitudinalSolutionValidator.cs index c365e57..3ff71c1 100644 --- a/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Longitudinal/LongitudinalSolutionValidator.cs +++ b/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Longitudinal/LongitudinalSolutionValidator.cs @@ -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 +