[wpimath] Exit early when parameterizing malformed spline (wpilibsuit…

…e#6827)

Currently, a max iteration heuristic is used to determine when a spline
is malformed. Instead, we can report a failure immediately if dx and dy
are too small, because the heading won't be accurate either.

Fixes wpilibsuite#6826.

wpimath/src/main/java/edu/wpi/first/math/spline/Spline.java

@@ -7,6 +7,7 @@
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Rotation2d;
 import java.util.Arrays;
+import java.util.Optional;
 import org.ejml.simple.SimpleMatrix;
 
 /** Represents a two-dimensional parametric spline that interpolates between two points. */
@@ -49,7 +50,7 @@ public abstract class Spline {
    * @param t The point t
    * @return The pose and curvature at that point.
    */
-  public PoseWithCurvature getPoint(double t) {
+  public Optional<PoseWithCurvature> getPoint(double t) {
     SimpleMatrix polynomialBases = new SimpleMatrix(m_degree + 1, 1);
     final var coefficients = getCoefficients();
 
@@ -86,10 +87,14 @@ public PoseWithCurvature getPoint(double t) {
       ddy = combined.get(5, 0) / t / t;
     }
 
+    if (Math.hypot(dx, dy) < 1e-6) {
+      return Optional.empty();
+    }
+
     // Find the curvature.
     final double curvature = (dx * ddy - ddx * dy) / ((dx * dx + dy * dy) * Math.hypot(dx, dy));
 
-    return new PoseWithCurvature(new Pose2d(x, y, new Rotation2d(dx, dy)), curvature);
+    return Optional.of(new PoseWithCurvature(new Pose2d(x, y, new Rotation2d(dx, dy)), curvature));
   }
 
   /**

wpimath/src/main/java/edu/wpi/first/math/spline/SplineParameterizer.java

@@ -38,6 +38,11 @@ public final class SplineParameterizer {
   private static final double kMaxDy = 0.00127;
   private static final double kMaxDtheta = 0.0872;
 
+  private static final String kMalformedSplineExceptionMsg =
+      "Could not parameterize a malformed spline. This means that you probably had two or more "
+          + "adjacent waypoints that were very close together with headings in opposing "
+          + "directions.";
+
   /**
    * A malformed spline does not actually explode the LIFO stack size. Instead, the stack size stays
    * at a relatively small number (e.g. 30) and never decreases. Because of this, we must count
@@ -99,39 +104,41 @@ public static List<PoseWithCurvature> parameterize(Spline spline, double t0, dou
     var splinePoints = new ArrayList<PoseWithCurvature>();
 
     // The parameterization does not add the initial point. Let's add that.
-    splinePoints.add(spline.getPoint(t0));
+    splinePoints.add(spline.getPoint(t0).get());
 
     // We use an "explicit stack" to simulate recursion, instead of a recursive function call
     // This give us greater control, instead of a stack overflow
     var stack = new ArrayDeque<StackContents>();
     stack.push(new StackContents(t0, t1));
 
-    StackContents current;
-    PoseWithCurvature start;
-    PoseWithCurvature end;
     int iterations = 0;
 
     while (!stack.isEmpty()) {
-      current = stack.removeFirst();
-      start = spline.getPoint(current.t0);
-      end = spline.getPoint(current.t1);
+      var current = stack.removeFirst();
+
+      var start = spline.getPoint(current.t0);
+      if (!start.isPresent()) {
+        throw new MalformedSplineException(kMalformedSplineExceptionMsg);
+      }
+
+      var end = spline.getPoint(current.t1);
+      if (!end.isPresent()) {
+        throw new MalformedSplineException(kMalformedSplineExceptionMsg);
+      }
 
-      final var twist = start.poseMeters.log(end.poseMeters);
+      final var twist = start.get().poseMeters.log(end.get().poseMeters);
       if (Math.abs(twist.dy) > kMaxDy
           || Math.abs(twist.dx) > kMaxDx
           || Math.abs(twist.dtheta) > kMaxDtheta) {
         stack.addFirst(new StackContents((current.t0 + current.t1) / 2, current.t1));
         stack.addFirst(new StackContents(current.t0, (current.t0 + current.t1) / 2));
       } else {
-        splinePoints.add(spline.getPoint(current.t1));
+        splinePoints.add(end.get());
       }
 
       iterations++;
       if (iterations >= kMaxIterations) {
-        throw new MalformedSplineException(
-            "Could not parameterize a malformed spline. This means that you probably had two or "
-                + " more adjacent waypoints that were very close together with headings in "
-                + "opposing directions.");
+        throw new MalformedSplineException(kMalformedSplineExceptionMsg);
       }
     }
 

wpimath/src/main/java/edu/wpi/first/math/trajectory/TrajectoryGenerator.java

@@ -246,7 +246,7 @@ public static List<PoseWithCurvature> splinePointsFromSplines(Spline[] splines)
     var splinePoints = new ArrayList<PoseWithCurvature>();
 
     // Add the first point to the vector.
-    splinePoints.add(splines[0].getPoint(0.0));
+    splinePoints.add(splines[0].getPoint(0.0).get());
 
     // Iterate through the vector and parameterize each spline, adding the
     // parameterized points to the final vector.

wpimath/src/main/native/include/frc/spline/Spline.h

@@ -4,8 +4,8 @@
 
 #pragma once
 
+#include <optional>
 #include <utility>
-#include <vector>
 
 #include <wpi/array.h>
 
@@ -57,7 +57,7 @@ class Spline {
    * @param t The point t
    * @return The pose and curvature at that point.
    */
-  PoseWithCurvature GetPoint(double t) const {
+  std::optional<PoseWithCurvature> GetPoint(double t) const {
     Vectord<Degree + 1> polynomialBases;
 
     // Populate the polynomial bases
@@ -88,11 +88,15 @@ class Spline {
       ddy = combined(5) / t / t;
     }
 
+    if (std::hypot(dx, dy) < 1e-6) {
+      return std::nullopt;
+    }
+
     // Find the curvature.
     const auto curvature =
         (dx * ddy - ddx * dy) / ((dx * dx + dy * dy) * std::hypot(dx, dy));
 
-    return {
+    return PoseWithCurvature{
         {FromVector(combined.template block<2, 1>(0, 0)), Rotation2d{dx, dy}},
         units::curvature_t{curvature}};
   }

wpimath/src/main/native/include/frc/spline/SplineParameterizer.h

@@ -72,44 +72,53 @@ class WPILIB_DLLEXPORT SplineParameterizer {
   static std::vector<PoseWithCurvature> Parameterize(const Spline<Dim>& spline,
                                                      double t0 = 0.0,
                                                      double t1 = 1.0) {
+    constexpr const char* kMalformedSplineExceptionMsg =
+        "Could not parameterize a malformed spline. This means that you "
+        "probably had two or more adjacent waypoints that were very close "
+        "together with headings in opposing directions.";
     std::vector<PoseWithCurvature> splinePoints;
 
     // The parameterization does not add the initial point. Let's add that.
-    splinePoints.push_back(spline.GetPoint(t0));
+    if (auto point = spline.GetPoint(t0)) {
+      splinePoints.push_back(point.value());
+    } else {
+      throw MalformedSplineException(kMalformedSplineExceptionMsg);
+    }
 
     // We use an "explicit stack" to simulate recursion, instead of a recursive
     // function call This give us greater control, instead of a stack overflow
     std::stack<StackContents> stack;
     stack.emplace(StackContents{t0, t1});
 
-    StackContents current;
-    PoseWithCurvature start;
-    PoseWithCurvature end;
     int iterations = 0;
 
     while (!stack.empty()) {
-      current = stack.top();
+      auto current = stack.top();
       stack.pop();
-      start = spline.GetPoint(current.t0);
-      end = spline.GetPoint(current.t1);
 
-      const auto twist = start.first.Log(end.first);
+      auto start = spline.GetPoint(current.t0);
+      if (!start) {
+        throw MalformedSplineException(kMalformedSplineExceptionMsg);
+      }
+
+      auto end = spline.GetPoint(current.t1);
+      if (!end) {
+        throw MalformedSplineException(kMalformedSplineExceptionMsg);
+      }
+
+      const auto twist = start.value().first.Log(end.value().first);
 
       if (units::math::abs(twist.dy) > kMaxDy ||
           units::math::abs(twist.dx) > kMaxDx ||
           units::math::abs(twist.dtheta) > kMaxDtheta) {
         stack.emplace(StackContents{(current.t0 + current.t1) / 2, current.t1});
         stack.emplace(StackContents{current.t0, (current.t0 + current.t1) / 2});
       } else {
-        splinePoints.push_back(spline.GetPoint(current.t1));
+        splinePoints.push_back(end.value());
       }
 
       if (iterations++ >= kMaxIterations) {
-        throw MalformedSplineException(
-            "Could not parameterize a malformed spline. "
-            "This means that you probably had two or more adjacent "
-            "waypoints that were very close together with headings "
-            "in opposing directions.");
+        throw MalformedSplineException(kMalformedSplineExceptionMsg);
       }
     }
 

wpimath/src/main/native/include/frc/trajectory/TrajectoryGenerator.h

@@ -99,7 +99,7 @@ class WPILIB_DLLEXPORT TrajectoryGenerator {
     std::vector<PoseWithCurvature> splinePoints;
 
     // Add the first point to the vector.
-    splinePoints.push_back(splines.front().GetPoint(0.0));
+    splinePoints.push_back(splines.front().GetPoint(0.0).value());
 
     // Iterate through the vector and parameterize each spline, adding the
     // parameterized points to the final vector.

wpimath/src/test/java/edu/wpi/first/math/spline/CubicHermiteSplineTest.java

@@ -36,7 +36,7 @@ private void run(Pose2d a, List<Translation2d> waypoints, Pose2d b) {
 
     var poses = new ArrayList<PoseWithCurvature>();
 
-    poses.add(splines[0].getPoint(0.0));
+    poses.add(splines[0].getPoint(0.0).get());
 
     for (var spline : splines) {
       poses.addAll(SplineParameterizer.parameterize(spline));

wpimath/src/test/native/cpp/spline/CubicHermiteSplineTest.cpp

@@ -30,7 +30,7 @@ class CubicHermiteSplineTest : public ::testing::Test {
         SplineHelper::CubicSplinesFromControlVectors(startCV, waypoints, endCV);
     std::vector<Spline<3>::PoseWithCurvature> poses;
 
-    poses.push_back(splines[0].GetPoint(0.0));
+    poses.push_back(splines[0].GetPoint(0.0).value());
 
     for (auto&& spline : splines) {
       auto x = SplineParameterizer::Parameterize(spline);