From 012d93b2bdc620a1151ae916e93bf626895046a2 Mon Sep 17 00:00:00 2001 From: Declan Freeman-Gleason Date: Wed, 1 Jan 2020 18:23:08 -0800 Subject: [PATCH] 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. --- .../cpp/trajectory/TrajectoryGenerator.cpp | 27 ++++- .../include/frc/spline/SplineParameterizer.h | 90 +++++++++------ .../frc/trajectory/TrajectoryGenerator.h | 3 + .../cpp/spline/CubicHermiteSplineTest.cpp | 11 ++ .../cpp/spline/QuinticHermiteSplineTest.cpp | 9 ++ .../trajectory/TrajectoryGeneratorTest.cpp | 10 ++ .../wpilibj/spline/SplineParameterizer.java | 104 +++++++++++++----- .../trajectory/TrajectoryGenerator.java | 31 +++++- .../spline/CubicHermiteSplineTest.java | 14 +++ .../spline/QuinticHermiteSplineTest.java | 47 +++++--- .../trajectory/TrajectoryGeneratorTest.java | 23 ++++ 11 files changed, 281 insertions(+), 88 deletions(-) diff --git a/wpilibc/src/main/native/cpp/trajectory/TrajectoryGenerator.cpp b/wpilibc/src/main/native/cpp/trajectory/TrajectoryGenerator.cpp index 92c21f630a..a9686337f7 100644 --- a/wpilibc/src/main/native/cpp/trajectory/TrajectoryGenerator.cpp +++ b/wpilibc/src/main/native/cpp/trajectory/TrajectoryGenerator.cpp @@ -9,11 +9,16 @@ #include +#include "frc/DriverStation.h" #include "frc/spline/SplineHelper.h" +#include "frc/spline/SplineParameterizer.h" #include "frc/trajectory/TrajectoryParameterizer.h" using namespace frc; +const Trajectory TrajectoryGenerator::kDoNothingTrajectory( + std::vector{Trajectory::State()}); + Trajectory TrajectoryGenerator::GenerateTrajectory( Spline<3>::ControlVector initial, const std::vector& interiorWaypoints, @@ -29,9 +34,15 @@ Trajectory TrajectoryGenerator::GenerateTrajectory( end.y[1] *= -1; } - auto points = - SplinePointsFromSplines(SplineHelper::CubicSplinesFromControlVectors( - initial, interiorWaypoints, end)); + std::vector points; + try { + points = + SplinePointsFromSplines(SplineHelper::CubicSplinesFromControlVectors( + initial, interiorWaypoints, end)); + } catch (SplineParameterizer::MalformedSplineException& e) { + DriverStation::ReportError(e.what()); + return kDoNothingTrajectory; + } // After trajectory generation, flip theta back so it's relative to the // field. Also fix curvature. @@ -68,8 +79,14 @@ Trajectory TrajectoryGenerator::GenerateTrajectory( } } - auto points = SplinePointsFromSplines( - SplineHelper::QuinticSplinesFromControlVectors(controlVectors)); + std::vector points; + try { + points = SplinePointsFromSplines( + SplineHelper::QuinticSplinesFromControlVectors(controlVectors)); + } catch (SplineParameterizer::MalformedSplineException& e) { + DriverStation::ReportError(e.what()); + return kDoNothingTrajectory; + } // After trajectory generation, flip theta back so it's relative to the // field. Also fix curvature. diff --git a/wpilibc/src/main/native/include/frc/spline/SplineParameterizer.h b/wpilibc/src/main/native/include/frc/spline/SplineParameterizer.h index 34cf026279..ecdede0e12 100644 --- a/wpilibc/src/main/native/include/frc/spline/SplineParameterizer.h +++ b/wpilibc/src/main/native/include/frc/spline/SplineParameterizer.h @@ -33,10 +33,13 @@ #include +#include +#include #include #include #include +#include namespace frc { @@ -47,6 +50,11 @@ class SplineParameterizer { public: using PoseWithCurvature = std::pair; + struct MalformedSplineException : public std::runtime_error { + explicit MalformedSplineException(const char* what_arg) + : runtime_error(what_arg) {} + }; + /** * Parameterizes the spline. This method breaks up the spline into various * arcs until their dx, dy, and dtheta are within specific tolerances. @@ -64,14 +72,48 @@ class SplineParameterizer { static std::vector Parameterize(const Spline& spline, double t0 = 0.0, double t1 = 1.0) { - std::vector arr; + std::vector splinePoints; - // The parameterization does not add the first initial point. Let's add - // that. - arr.push_back(spline.GetPoint(t0)); + // The parameterization does not add the initial point. Let's add that. + splinePoints.push_back(spline.GetPoint(t0)); - GetSegmentArc(spline, &arr, t0, t1); - return arr; + // 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 stack; + stack.emplace(StackContents{t0, t1}); + + StackContents current; + PoseWithCurvature start; + PoseWithCurvature end; + int iterations = 0; + + while (!stack.empty()) { + current = stack.top(); + stack.pop(); + start = spline.GetPoint(current.t0); + end = spline.GetPoint(current.t1); + + const auto twist = start.first.Log(end.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)); + } + + 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."); + } + } + + return splinePoints; } private: @@ -80,33 +122,19 @@ class SplineParameterizer { static constexpr units::meter_t kMaxDy = 0.05_in; static constexpr units::radian_t kMaxDtheta = 0.0872_rad; + struct StackContents { + double t0; + double t1; + }; + /** - * Breaks up the spline into arcs until the dx, dy, and theta of each arc is - * within tolerance. - * - * @param spline The spline to parameterize. - * @param vector Pointer to vector of poses. - * @param t0 Starting point for arc. - * @param t1 Ending point for arc. + * 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 iterations. Even long, complex + * paths don't usually go over 300 iterations, so hitting this maximum should + * definitely indicate something has gone wrong. */ - template - static void GetSegmentArc(const Spline& spline, - std::vector* vector, double t0, - double t1) { - const auto start = spline.GetPoint(t0); - const auto end = spline.GetPoint(t1); - - const auto twist = start.first.Log(end.first); - - if (units::math::abs(twist.dy) > kMaxDy || - units::math::abs(twist.dx) > kMaxDx || - units::math::abs(twist.dtheta) > kMaxDtheta) { - GetSegmentArc(spline, vector, t0, (t0 + t1) / 2); - GetSegmentArc(spline, vector, (t0 + t1) / 2, t1); - } else { - vector->push_back(spline.GetPoint(t1)); - } - } + static constexpr int kMaxIterations = 5000; friend class CubicHermiteSplineTest; friend class QuinticHermiteSplineTest; diff --git a/wpilibc/src/main/native/include/frc/trajectory/TrajectoryGenerator.h b/wpilibc/src/main/native/include/frc/trajectory/TrajectoryGenerator.h index 4d6eff600c..55ef7829b0 100644 --- a/wpilibc/src/main/native/include/frc/trajectory/TrajectoryGenerator.h +++ b/wpilibc/src/main/native/include/frc/trajectory/TrajectoryGenerator.h @@ -113,5 +113,8 @@ class TrajectoryGenerator { } return splinePoints; } + + private: + static const Trajectory kDoNothingTrajectory; }; } // namespace frc diff --git a/wpilibc/src/test/native/cpp/spline/CubicHermiteSplineTest.cpp b/wpilibc/src/test/native/cpp/spline/CubicHermiteSplineTest.cpp index 319be79d0b..63ecf2bc9c 100644 --- a/wpilibc/src/test/native/cpp/spline/CubicHermiteSplineTest.cpp +++ b/wpilibc/src/test/native/cpp/spline/CubicHermiteSplineTest.cpp @@ -120,3 +120,14 @@ TEST_F(CubicHermiteSplineTest, OneInterior) { Pose2d end{4_m, 0_m, 0_rad}; Run(start, waypoints, end); } + +TEST_F(CubicHermiteSplineTest, ThrowsOnMalformed) { + EXPECT_THROW( + Run(Pose2d{0_m, 0_m, Rotation2d(0_deg)}, std::vector{}, + Pose2d{1_m, 0_m, Rotation2d(180_deg)}), + SplineParameterizer::MalformedSplineException); + EXPECT_THROW( + Run(Pose2d{10_m, 10_m, Rotation2d(90_deg)}, std::vector{}, + Pose2d{10_m, 11_m, Rotation2d(-90_deg)}), + SplineParameterizer::MalformedSplineException); +} diff --git a/wpilibc/src/test/native/cpp/spline/QuinticHermiteSplineTest.cpp b/wpilibc/src/test/native/cpp/spline/QuinticHermiteSplineTest.cpp index cc10c6c122..706ad20c53 100644 --- a/wpilibc/src/test/native/cpp/spline/QuinticHermiteSplineTest.cpp +++ b/wpilibc/src/test/native/cpp/spline/QuinticHermiteSplineTest.cpp @@ -85,3 +85,12 @@ TEST_F(QuinticHermiteSplineTest, SquigglyCurve) { Run(Pose2d(0_m, 0_m, Rotation2d(90_deg)), Pose2d(-1_m, 0_m, Rotation2d(90_deg))); } + +TEST_F(QuinticHermiteSplineTest, ThrowsOnMalformed) { + EXPECT_THROW(Run(Pose2d(0_m, 0_m, Rotation2d(0_deg)), + Pose2d(1_m, 0_m, Rotation2d(180_deg))), + SplineParameterizer::MalformedSplineException); + EXPECT_THROW(Run(Pose2d(10_m, 10_m, Rotation2d(90_deg)), + Pose2d(10_m, 11_m, Rotation2d(-90_deg))), + SplineParameterizer::MalformedSplineException); +} diff --git a/wpilibc/src/test/native/cpp/trajectory/TrajectoryGeneratorTest.cpp b/wpilibc/src/test/native/cpp/trajectory/TrajectoryGeneratorTest.cpp index 7e47e112a7..c524281679 100644 --- a/wpilibc/src/test/native/cpp/trajectory/TrajectoryGeneratorTest.cpp +++ b/wpilibc/src/test/native/cpp/trajectory/TrajectoryGeneratorTest.cpp @@ -33,3 +33,13 @@ TEST(TrajectoryGenerationTest, ObeysConstraints) { 12_fps_sq + 0.01_fps_sq); } } + +TEST(TrajectoryGenertionTest, ReturnsEmptyOnMalformed) { + const auto t = TrajectoryGenerator::GenerateTrajectory( + std::vector{Pose2d(0_m, 0_m, Rotation2d(0_deg)), + Pose2d(1_m, 0_m, Rotation2d(180_deg))}, + TrajectoryConfig(12_fps, 12_fps_sq)); + + ASSERT_EQ(t.States().size(), 1u); + ASSERT_EQ(t.TotalTime(), 0_s); +} diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/spline/SplineParameterizer.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/spline/SplineParameterizer.java index b78227d64c..1f141d05df 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/spline/SplineParameterizer.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/spline/SplineParameterizer.java @@ -31,6 +31,7 @@ package edu.wpi.first.wpilibj.spline; +import java.util.ArrayDeque; import java.util.ArrayList; import java.util.List; @@ -42,6 +43,36 @@ public final class SplineParameterizer { private static final double kMaxDy = 0.00127; private static final double kMaxDtheta = 0.0872; + /** + * 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 iterations. Even long, complex paths don't usually go over 300 iterations, so hitting + * this maximum should definitely indicate something has gone wrong. + */ + private static final int kMaxIterations = 5000; + + @SuppressWarnings("MemberName") + private static class StackContents { + final double t1; + final double t0; + + StackContents(double t0, double t1) { + this.t0 = t0; + this.t1 = t1; + } + } + + public static class MalformedSplineException extends RuntimeException { + /** + * Create a new exception with the given message. + * + * @param message the message to pass with the exception + */ + private MalformedSplineException(String message) { + super(message); + } + } + /** * Private constructor because this is a utility class. */ @@ -53,7 +84,9 @@ public final class SplineParameterizer { * arcs until their dx, dy, and dtheta are within specific tolerances. * * @param spline The spline to parameterize. - * @return A vector of poses and curvatures that represents various points on the spline. + * @return A list of poses and curvatures that represents various points on the spline. + * @throws MalformedSplineException When the spline is malformed (e.g. has close adjacent points + * with approximately opposing headings) */ public static List parameterize(Spline spline) { return parameterize(spline, 0.0, 1.0); @@ -66,41 +99,54 @@ public final class SplineParameterizer { * @param spline The spline to parameterize. * @param t0 Starting internal spline parameter. It is recommended to use 0.0. * @param t1 Ending internal spline parameter. It is recommended to use 1.0. - * @return A vector of poses and curvatures that represents various points on the spline. + * @return A list of poses and curvatures that represents various points on the spline. + * @throws MalformedSplineException When the spline is malformed (e.g. has close adjacent points + * with approximately opposing headings) */ + @SuppressWarnings("PMD.AvoidInstantiatingObjectsInLoops") public static List parameterize(Spline spline, double t0, double t1) { - var arr = new ArrayList(); + var splinePoints = new ArrayList(); - // The parameterization does not add the first initial point. Let's add - // that. - arr.add(spline.getPoint(t0)); + // The parameterization does not add the initial point. Let's add that. + splinePoints.add(spline.getPoint(t0)); - getSegmentArc(spline, arr, t0, t1); - return arr; - } + // 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(); + stack.push(new StackContents(t0, t1)); - /** - * Breaks up the spline into arcs until the dx, dy, and theta of each arc is - * within tolerance. - * - * @param spline The spline to parameterize. - * @param vector Pointer to vector of poses. - * @param t0 Starting point for arc. - * @param t1 Ending point for arc. - */ - private static void getSegmentArc(Spline spline, List vector, - double t0, double t1) { - final var start = spline.getPoint(t0); - final var end = spline.getPoint(t1); + StackContents current; + PoseWithCurvature start; + PoseWithCurvature end; + int iterations = 0; - final var twist = start.poseMeters.log(end.poseMeters); + while (!stack.isEmpty()) { + current = stack.removeFirst(); + start = spline.getPoint(current.t0); + end = spline.getPoint(current.t1); - if (Math.abs(twist.dy) > kMaxDy || Math.abs(twist.dx) > kMaxDx - || Math.abs(twist.dtheta) > kMaxDtheta) { - getSegmentArc(spline, vector, t0, (t0 + t1) / 2); - getSegmentArc(spline, vector, (t0 + t1) / 2, t1); - } else { - vector.add(spline.getPoint(t1)); + final var twist = start.poseMeters.log(end.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)); + } + + 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." + ); + } } + + return splinePoints; } } diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/trajectory/TrajectoryGenerator.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/trajectory/TrajectoryGenerator.java index a4d6898597..6d3a89f771 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/trajectory/TrajectoryGenerator.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/trajectory/TrajectoryGenerator.java @@ -8,9 +8,11 @@ package edu.wpi.first.wpilibj.trajectory; import java.util.ArrayList; +import java.util.Arrays; import java.util.Collection; import java.util.List; +import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.geometry.Pose2d; import edu.wpi.first.wpilibj.geometry.Rotation2d; import edu.wpi.first.wpilibj.geometry.Transform2d; @@ -19,8 +21,12 @@ import edu.wpi.first.wpilibj.spline.PoseWithCurvature; import edu.wpi.first.wpilibj.spline.Spline; import edu.wpi.first.wpilibj.spline.SplineHelper; import edu.wpi.first.wpilibj.spline.SplineParameterizer; +import edu.wpi.first.wpilibj.spline.SplineParameterizer.MalformedSplineException; public final class TrajectoryGenerator { + private static final Trajectory kDoNothingTrajectory = + new Trajectory(Arrays.asList(new Trajectory.State())); + /** * Private constructor because this is a utility class. */ @@ -60,9 +66,14 @@ public final class TrajectoryGenerator { } // Get the spline points - var points = splinePointsFromSplines(SplineHelper.getCubicSplinesFromControlVectors( - newInitial, interiorWaypoints.toArray(new Translation2d[0]), newEnd - )); + List points; + try { + points = splinePointsFromSplines(SplineHelper.getCubicSplinesFromControlVectors(newInitial, + interiorWaypoints.toArray(new Translation2d[0]), newEnd)); + } catch (MalformedSplineException ex) { + DriverStation.reportError(ex.getMessage(), ex.getStackTrace()); + return kDoNothingTrajectory; + } // Change the points back to their original orientation. if (config.isReversed()) { @@ -130,9 +141,15 @@ public final class TrajectoryGenerator { } // Get the spline points - var points = splinePointsFromSplines(SplineHelper.getQuinticSplinesFromControlVectors( - newControlVectors.toArray(new Spline.ControlVector[]{}) - )); + List points; + try { + points = splinePointsFromSplines(SplineHelper.getQuinticSplinesFromControlVectors( + newControlVectors.toArray(new Spline.ControlVector[]{}) + )); + } catch (MalformedSplineException ex) { + DriverStation.reportError(ex.getMessage(), ex.getStackTrace()); + return kDoNothingTrajectory; + } // Change the points back to their original orientation. if (config.isReversed()) { @@ -171,6 +188,8 @@ public final class TrajectoryGenerator { * * @param splines The splines to parameterize. * @return The spline points for use in time parameterization of a trajectory. + * @throws MalformedSplineException When the spline is malformed (e.g. has close adjacent points + * with approximately opposing headings) */ public static List splinePointsFromSplines( Spline[] splines) { diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/spline/CubicHermiteSplineTest.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj/spline/CubicHermiteSplineTest.java index 5a8349d6d3..e656f81493 100644 --- a/wpilibj/src/test/java/edu/wpi/first/wpilibj/spline/CubicHermiteSplineTest.java +++ b/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)))); + } } diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/spline/QuinticHermiteSplineTest.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj/spline/QuinticHermiteSplineTest.java index 5eb84374bb..0bd4f6934c 100644 --- a/wpilibj/src/test/java/edu/wpi/first/wpilibj/spline/QuinticHermiteSplineTest.java +++ b/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)))); + } } diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/trajectory/TrajectoryGeneratorTest.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj/trajectory/TrajectoryGeneratorTest.java index 5b45e3e590..2cd309d9d5 100644 --- a/wpilibj/src/test/java/edu/wpi/first/wpilibj/trajectory/TrajectoryGeneratorTest.java +++ b/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); + } }