[wpilibj] Throw separate exception for constraint misbehavior (#2510)

The most common mistake users (including contributors to WPILib) seem to make while creating new constraints is ignoring some sort of edge case that causes the calculated minimum acceleration to be greater than the calculated maximum acceleration.

This specialized exception, with its detailed error message, should make it easier and quicker for said users to debug and fix bugs within their constraints.

Co-authored-by: Tyler Veness <calcmogul@gmail.com>

wpilibj/src/main/java/edu/wpi/first/wpilibj/trajectory/TrajectoryParameterizer.java

@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -69,8 +69,7 @@ public final class TrajectoryParameterizer {
    * @return The trajectory.
    */
   @SuppressWarnings({"PMD.ExcessiveMethodLength", "PMD.CyclomaticComplexity",
-      "PMD.NPathComplexity", "PMD.AvoidInstantiatingObjectsInLoops",
-      "PMD.AvoidThrowingRawExceptionTypes"})
+      "PMD.NPathComplexity", "PMD.AvoidInstantiatingObjectsInLoops"})
   public static Trajectory timeParameterizeTrajectory(
       List<PoseWithCurvature> points,
       List<TrajectoryConstraint> constraints,
@@ -236,7 +235,8 @@ public final class TrajectoryParameterizer {
           // delta_x = v * t
           dt = ds / velocityMetersPerSecond;
         } else {
-          throw new RuntimeException("Something went wrong");
+          throw new TrajectoryGenerationException("Something went wrong at iteration " + i
+              + " of time parameterization.");
         }
       }
 
@@ -266,6 +266,14 @@ public final class TrajectoryParameterizer {
           state.pose.poseMeters, state.pose.curvatureRadPerMeter,
           state.maxVelocityMetersPerSecond * factor);
 
+      if (minMaxAccel.minAccelerationMetersPerSecondSq
+          > minMaxAccel.maxAccelerationMetersPerSecondSq) {
+        throw new TrajectoryGenerationException("The constraint's min acceleration "
+            + "was greater than its max acceleration.\n Offending Constraint: "
+            + constraint.getClass().getName()
+            + "\n If the offending constraint was packaged with WPILib, please file a bug report.");
+      }
+
       state.minAccelerationMetersPerSecondSq = Math.max(state.minAccelerationMetersPerSecondSq,
           reverse ? -minMaxAccel.maxAccelerationMetersPerSecondSq
               : minMaxAccel.minAccelerationMetersPerSecondSq);
@@ -300,4 +308,10 @@ public final class TrajectoryParameterizer {
       pose = new PoseWithCurvature();
     }
   }
+
+  public static class TrajectoryGenerationException extends RuntimeException {
+    public TrajectoryGenerationException(String message) {
+      super(message);
+    }
+  }
 }