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

feat: detail trajectory jerk validation failures

Browse Source
This commit is contained in:
梁薄云
2026-08-05 15:25:18 +08:00
parent 0eeda2d824
commit 32083db4d0
2 changed files with 32 additions and 2 deletions
Download Patch File Download Diff File Expand all files Collapse all files
ClumsyPilot/ParkrobTrajplanner/EMPlanner/Validation/EmTrajectoryValidator.cs
+19 -2
View File
@@ -1,4 +1,5 @@
using System;
using System.Globalization;
using MultiWheelC.TrajectoryPlanning.CoarsePath;
using MultiWheelC.TrajectoryPlanning.CoarsePath.Vehicle;
using MultiWheelC.TrajectoryPlanning.Mapping;
@@ -182,11 +183,22 @@ public sealed class EmTrajectoryValidator
return Reject(EmTrajectoryValidationFailure.AccelerationLimitExceeded, index,
"Trajectory finite-difference acceleration exceeds its limit.");
}
if (hasPreviousAcceleration && Math.Abs((acceleration - previousAcceleration) / dt) > limits.MaximumJerk +
double finiteDifferenceJerk = (acceleration - previousAcceleration) / dt;
if (hasPreviousAcceleration && Math.Abs(finiteDifferenceJerk) > limits.MaximumJerk +
limits.KinematicTolerance)
{
double storedPreviousJerk = index >= 2 ? trajectory.Points[index - 2].LongitudinalJerk : 0d;
double storedCurrentJerk = previous.LongitudinalJerk;
return Reject(EmTrajectoryValidationFailure.JerkLimitExceeded, index,
"Trajectory finite-difference jerk exceeds its limit.");
"Trajectory finite-difference jerk exceeds its limit" +
" (time=" + Format(point.TimeFromStart) + "s, dt=" + Format(dt) + "s" +
", previousAcceleration=" + Format(previousAcceleration) + "m/s2" +
", acceleration=" + Format(acceleration) + "m/s2" +
", jerk=" + Format(finiteDifferenceJerk) + "m/s3" +
", limit=" + Format(limits.MaximumJerk) + "m/s3" +
", excess=" + Format(Math.Abs(finiteDifferenceJerk) - limits.MaximumJerk) + "m/s3" +
", storedPreviousJerk=" + Format(storedPreviousJerk) + "m/s3" +
", storedCurrentJerk=" + Format(storedCurrentJerk) + "m/s3).");
}
if (Math.Abs(point.VehicleCurvature - previous.VehicleCurvature) / dt > limits.MaximumCurvatureRate +
limits.KinematicTolerance)
@@ -302,6 +314,11 @@ public sealed class EmTrajectoryValidator
return Math.Abs(expected - actual) <= tolerance + tolerance * scale;
}
private static string Format(double value)
{
return value.ToString("G17", CultureInfo.InvariantCulture);
}
private static bool IsFinite(double value)
{
return !double.IsNaN(value) && !double.IsInfinity(value);