feat: detail trajectory jerk validation failures

ClumsyPilot/ParkrobTrajplanner/EMPlanner/Validation/EmTrajectoryValidator.cs

@@ -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);

ClumsyPilot/tests/EMPlannerVerificationHost/TrajectoryChecks.cs

@@ -85,6 +85,19 @@ internal static class TrajectoryChecks
         EmTrajectory jerkMutated = Replace(valid, 1, Clone(valid.Points[1], signedSpeed: 0.035d));
         jerkMutated = Replace(jerkMutated, 2, Clone(jerkMutated.Points[2], signedSpeed: 0.03d));
         AssertRejected(context, jerkMutated, EmTrajectoryValidationFailure.JerkLimitExceeded, 2, "jerk limit");
+        EmTrajectoryValidationResult jerkResult = new EmTrajectoryValidator().Validate(
+            jerkMutated, context.EmptyMap, context.Vehicle, context.Configuration, 2, 0.0055d, EmBoundaryType.Goal);
+        Verification.Equal(EmTrajectoryValidationFailure.JerkLimitExceeded, jerkResult.Failure,
+            "jerk diagnostic failure code");
+        Verification.Equal(2, jerkResult.PointIndex, "jerk diagnostic point index");
+        foreach (string field in new[]
+        {
+            "time=", "dt=", "previousAcceleration=", "acceleration=", "jerk=", "limit=", "excess=",
+            "storedPreviousJerk=", "storedCurrentJerk=",
+        })
+        {
+            Verification.True(jerkResult.Message.Contains(field), "jerk diagnostic includes " + field);
+        }
 
         AssertRejected(context, Replace(valid, 1, Clone(valid.Points[1], vehicleCurvature: 2d)),
             EmTrajectoryValidationFailure.CurvatureLimitExceeded, 1, "curvature limit");