Use an explicit stack instead of recursion when parameterizing splines (#2197)

This PR changes the spline parameterizer to use an explicit stack instead of recursion. This is motivated by the fact that splines with adjacent waypoints with approximately opposite headings will never parameterize. In this case the parameterizer subdivides these malformed splines fine for a while, and then gets stuck parameterizing infinitely on some interval. In the recursive approach, this would lead to a stack overflow. We could implement a recursion depth counter (this is what my team did on our similar trajectory code last season), but it's hard to choose a good number for max depth because the initial amount of stack used varies based on how the user calls Parameterize.

A good solution for this is converting the recursion to an "explicit stack," which basically simulates recursion, but allows us to have a much larger maximum stack size. Because we avoid the stack overflow, we can instead throws a more informative MalformedSplineException. If the user is using the TrajectoryGenerator instead of the SplineParameterizer directly then the TrajectoryGenerator will go ahead and catch the exception, return a harmless empty trajectory, and report and error to the driver station.

wpilibj/src/test/java/edu/wpi/first/wpilibj/spline/CubicHermiteSplineTest.java

@@ -8,6 +8,7 @@
 package edu.wpi.first.wpilibj.spline;
 
 import java.util.ArrayList;
+import java.util.Arrays;
 import java.util.List;
 
 import org.junit.jupiter.api.Test;
@@ -15,9 +16,11 @@ import org.junit.jupiter.api.Test;
 import edu.wpi.first.wpilibj.geometry.Pose2d;
 import edu.wpi.first.wpilibj.geometry.Rotation2d;
 import edu.wpi.first.wpilibj.geometry.Translation2d;
+import edu.wpi.first.wpilibj.spline.SplineParameterizer.MalformedSplineException;
 
 import static org.junit.jupiter.api.Assertions.assertAll;
 import static org.junit.jupiter.api.Assertions.assertEquals;
+import static org.junit.jupiter.api.Assertions.assertThrows;
 import static org.junit.jupiter.api.Assertions.assertTrue;
 
 
@@ -145,4 +148,15 @@ class CubicHermiteSplineTest {
 
     run(start, waypoints, end);
   }
+
+  @Test
+  void testMalformed() {
+    assertThrows(MalformedSplineException.class, () -> run(
+        new Pose2d(0, 0, Rotation2d.fromDegrees(0)),
+        new ArrayList<>(), new Pose2d(1, 0, Rotation2d.fromDegrees(180))));
+    assertThrows(MalformedSplineException.class, () -> run(
+        new Pose2d(10, 10, Rotation2d.fromDegrees(90)),
+        Arrays.asList(new Translation2d(10, 10.5)),
+        new Pose2d(10, 11, Rotation2d.fromDegrees(-90))));
+  }
 }

wpilibj/src/test/java/edu/wpi/first/wpilibj/spline/QuinticHermiteSplineTest.java

@@ -13,9 +13,11 @@ import org.junit.jupiter.api.Test;
 
 import edu.wpi.first.wpilibj.geometry.Pose2d;
 import edu.wpi.first.wpilibj.geometry.Rotation2d;
+import edu.wpi.first.wpilibj.spline.SplineParameterizer.MalformedSplineException;
 
 import static org.junit.jupiter.api.Assertions.assertAll;
 import static org.junit.jupiter.api.Assertions.assertEquals;
+import static org.junit.jupiter.api.Assertions.assertThrows;
 import static org.junit.jupiter.api.Assertions.assertTrue;
 
 class QuinticHermiteSplineTest {
@@ -23,7 +25,7 @@ class QuinticHermiteSplineTest {
   private static final double kMaxDy = 0.00127;
   private static final double kMaxDtheta = 0.0872;
 
-  @SuppressWarnings({"ParameterName", "PMD.UnusedLocalVariable"})
+  @SuppressWarnings({ "ParameterName", "PMD.UnusedLocalVariable" })
   private void run(Pose2d a, Pose2d b) {
     // Start the timer.
     var start = System.nanoTime();
@@ -49,29 +51,27 @@ class QuinticHermiteSplineTest {
       assertAll(
           () -> assertTrue(Math.abs(twist.dx) < kMaxDx),
           () -> assertTrue(Math.abs(twist.dy) < kMaxDy),
-          () -> assertTrue(Math.abs(twist.dtheta) < kMaxDtheta)
-      );
+          () -> assertTrue(Math.abs(twist.dtheta) < kMaxDtheta));
     }
 
     // Check first point
     assertAll(
-        () -> assertEquals(a.getTranslation().getX(),
-            poses.get(0).poseMeters.getTranslation().getX(), 1E-9),
-        () -> assertEquals(a.getTranslation().getY(),
-            poses.get(0).poseMeters.getTranslation().getY(), 1E-9),
-        () -> assertEquals(a.getRotation().getRadians(),
-            poses.get(0).poseMeters.getRotation().getRadians(), 1E-9)
-    );
+        () -> assertEquals(
+            a.getTranslation().getX(), poses.get(0).poseMeters.getTranslation().getX(), 1E-9),
+        () -> assertEquals(
+            a.getTranslation().getY(), poses.get(0).poseMeters.getTranslation().getY(), 1E-9),
+        () -> assertEquals(
+            a.getRotation().getRadians(), poses.get(0).poseMeters.getRotation().getRadians(),
+            1E-9));
 
     // Check last point
     assertAll(
-        () -> assertEquals(b.getTranslation().getX(),
-            poses.get(poses.size() - 1).poseMeters.getTranslation().getX(), 1E-9),
-        () -> assertEquals(b.getTranslation().getY(),
-            poses.get(poses.size() - 1).poseMeters.getTranslation().getY(), 1E-9),
+        () -> assertEquals(b.getTranslation().getX(), poses.get(poses.size() - 1)
+            .poseMeters.getTranslation().getX(), 1E-9),
+        () -> assertEquals(b.getTranslation().getY(), poses.get(poses.size() - 1)
+            .poseMeters.getTranslation().getY(), 1E-9),
         () -> assertEquals(b.getRotation().getRadians(),
-            poses.get(poses.size() - 1).poseMeters.getRotation().getRadians(), 1E-9)
-    );
+            poses.get(poses.size() - 1).poseMeters.getRotation().getRadians(), 1E-9));
   }
 
   @SuppressWarnings("PMD.JUnitTestsShouldIncludeAssert")
@@ -89,7 +89,20 @@ class QuinticHermiteSplineTest {
   @SuppressWarnings("PMD.JUnitTestsShouldIncludeAssert")
   @Test
   void testSquiggly() {
-    run(new Pose2d(0, 0, Rotation2d.fromDegrees(90)),
+    run(
+        new Pose2d(0, 0, Rotation2d.fromDegrees(90)),
         new Pose2d(-1, 0, Rotation2d.fromDegrees(90)));
   }
+
+  @Test
+  void testMalformed() {
+    assertThrows(MalformedSplineException.class,
+        () -> run(
+          new Pose2d(0, 0, Rotation2d.fromDegrees(0)),
+          new Pose2d(1, 0, Rotation2d.fromDegrees(180))));
+    assertThrows(MalformedSplineException.class,
+        () -> run(
+          new Pose2d(10, 10, Rotation2d.fromDegrees(90)),
+          new Pose2d(10, 11, Rotation2d.fromDegrees(-90))));
+  }
 }

wpilibj/src/test/java/edu/wpi/first/wpilibj/trajectory/TrajectoryGeneratorTest.java

@@ -8,10 +8,12 @@
 package edu.wpi.first.wpilibj.trajectory;
 
 import java.util.ArrayList;
+import java.util.Arrays;
 import java.util.List;
 
 import org.junit.jupiter.api.Test;
 
+import edu.wpi.first.hal.sim.DriverStationSim;
 import edu.wpi.first.wpilibj.geometry.Pose2d;
 import edu.wpi.first.wpilibj.geometry.Rotation2d;
 import edu.wpi.first.wpilibj.geometry.Transform2d;
@@ -20,6 +22,7 @@ import edu.wpi.first.wpilibj.trajectory.constraint.TrajectoryConstraint;
 
 import static edu.wpi.first.wpilibj.util.Units.feetToMeters;
 import static org.junit.jupiter.api.Assertions.assertAll;
+import static org.junit.jupiter.api.Assertions.assertEquals;
 import static org.junit.jupiter.api.Assertions.assertTrue;
 
 class TrajectoryGeneratorTest {
@@ -69,4 +72,24 @@ class TrajectoryGeneratorTest {
       );
     }
   }
+
+  @Test
+  void testMalformedTrajectory() {
+    var dsSim = new DriverStationSim();
+    dsSim.setSendError(false);
+
+    var traj =
+        TrajectoryGenerator.generateTrajectory(
+          Arrays.asList(
+            new Pose2d(0, 0, Rotation2d.fromDegrees(0)),
+            new Pose2d(1, 0, Rotation2d.fromDegrees(180))
+          ),
+          new TrajectoryConfig(feetToMeters(12), feetToMeters(12))
+        );
+
+    assertEquals(traj.getStates().size(), 1);
+    assertEquals(traj.getTotalTimeSeconds(), 0);
+
+    dsSim.setSendError(true);
+  }
 }