From 762347f00527a07539a013becf5ee2228335e732 Mon Sep 17 00:00:00 2001
From: Prateek Machiraju <prateek.machiraju@gmail.com>
Date: Mon, 8 Jun 2020 13:38:47 -0400
Subject: [PATCH] [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>
---
 .../trajectory/TrajectoryParameterizer.cpp    | 17 +++++++++++---
 .../trajectory/TrajectoryParameterizer.java   | 22 +++++++++++++++----
 2 files changed, 32 insertions(+), 7 deletions(-)

diff --git a/wpilibc/src/main/native/cpp/trajectory/TrajectoryParameterizer.cpp b/wpilibc/src/main/native/cpp/trajectory/TrajectoryParameterizer.cpp
index 131ae8a421..976b4e26e6 100644
--- a/wpilibc/src/main/native/cpp/trajectory/TrajectoryParameterizer.cpp
+++ b/wpilibc/src/main/native/cpp/trajectory/TrajectoryParameterizer.cpp
@@ -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.                                                               */
@@ -31,6 +31,8 @@
 
 #include "frc/trajectory/TrajectoryParameterizer.h"
 
+#include <string>
+
 using namespace frc;
 
 Trajectory TrajectoryParameterizer::TimeParameterizeTrajectory(
@@ -186,8 +188,9 @@ Trajectory TrajectoryParameterizer::TimeParameterizeTrajectory(
         // delta_x = v * t
         dt = ds / v;
       } else {
-        throw std::runtime_error(
-            "Something went wrong during trajectory generation.");
+        throw std::runtime_error("Something went wrong at iteration " +
+                                 std::to_string(i) +
+                                 " of time parameterization.");
       }
     }
 
@@ -213,6 +216,14 @@ void TrajectoryParameterizer::EnforceAccelerationLimits(
     auto minMaxAccel = constraint->MinMaxAcceleration(
         state->pose.first, state->pose.second, state->maxVelocity * factor);
 
+    if (minMaxAccel.minAcceleration > minMaxAccel.maxAcceleration) {
+      throw std::runtime_error(
+          "The constraint's min acceleration was greater than its max "
+          "acceleration. To debug this, remove all constraints from the config "
+          "and add each one individually. If the offending constraint was "
+          "packaged with WPILib, please file a bug report.");
+    }
+
     state->minAcceleration = units::math::max(
         state->minAcceleration,
         reverse ? -minMaxAccel.maxAcceleration : minMaxAccel.minAcceleration);
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/trajectory/TrajectoryParameterizer.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/trajectory/TrajectoryParameterizer.java
index 9459dd0cd0..e2bda033fa 100644
--- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/trajectory/TrajectoryParameterizer.java
+++ b/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);
+    }
+  }
 }