From 5e97d78d82225cb74e6d26024cbc53b299eb06b8 Mon Sep 17 00:00:00 2001 From: Tyler Veness Date: Fri, 12 Jul 2024 13:48:10 -0700 Subject: [PATCH] [wpimath] Exit early when parameterizing malformed spline 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 #6826. --- .../edu/wpi/first/math/spline/Spline.java | 9 ++++- .../math/spline/SplineParameterizer.java | 31 +++++++++------- .../math/trajectory/TrajectoryGenerator.java | 2 +- .../main/native/include/frc/spline/Spline.h | 10 +++-- .../include/frc/spline/SplineParameterizer.h | 37 ++++++++++++------- .../frc/trajectory/TrajectoryGenerator.h | 2 +- .../math/spline/CubicHermiteSplineTest.java | 2 +- .../cpp/spline/CubicHermiteSplineTest.cpp | 2 +- 8 files changed, 59 insertions(+), 36 deletions(-) diff --git a/wpimath/src/main/java/edu/wpi/first/math/spline/Spline.java b/wpimath/src/main/java/edu/wpi/first/math/spline/Spline.java index dc6c5e69969..80db954380a 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/spline/Spline.java +++ b/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 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)); } /** diff --git a/wpimath/src/main/java/edu/wpi/first/math/spline/SplineParameterizer.java b/wpimath/src/main/java/edu/wpi/first/math/spline/SplineParameterizer.java index 36bddb1feb1..1d391f284b9 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/spline/SplineParameterizer.java +++ b/wpimath/src/main/java/edu/wpi/first/math/spline/SplineParameterizer.java @@ -96,42 +96,47 @@ public static List parameterize(Spline spline) { * with approximately opposing headings) */ public static List parameterize(Spline spline, double t0, double t1) { + 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."; + var splinePoints = new ArrayList(); // 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(); 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); } } diff --git a/wpimath/src/main/java/edu/wpi/first/math/trajectory/TrajectoryGenerator.java b/wpimath/src/main/java/edu/wpi/first/math/trajectory/TrajectoryGenerator.java index 3de3effd826..a17d5fb1bc1 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/trajectory/TrajectoryGenerator.java +++ b/wpimath/src/main/java/edu/wpi/first/math/trajectory/TrajectoryGenerator.java @@ -246,7 +246,7 @@ public static List splinePointsFromSplines(Spline[] splines) var splinePoints = new ArrayList(); // 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. diff --git a/wpimath/src/main/native/include/frc/spline/Spline.h b/wpimath/src/main/native/include/frc/spline/Spline.h index 257b167f38c..9b369e31b62 100644 --- a/wpimath/src/main/native/include/frc/spline/Spline.h +++ b/wpimath/src/main/native/include/frc/spline/Spline.h @@ -4,8 +4,8 @@ #pragma once +#include #include -#include #include @@ -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 GetPoint(double t) const { Vectord 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}}; } diff --git a/wpimath/src/main/native/include/frc/spline/SplineParameterizer.h b/wpimath/src/main/native/include/frc/spline/SplineParameterizer.h index e1d2d5230de..1b49656d6a8 100644 --- a/wpimath/src/main/native/include/frc/spline/SplineParameterizer.h +++ b/wpimath/src/main/native/include/frc/spline/SplineParameterizer.h @@ -72,28 +72,41 @@ class WPILIB_DLLEXPORT SplineParameterizer { static std::vector Parameterize(const Spline& 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 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 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 || @@ -101,15 +114,11 @@ class WPILIB_DLLEXPORT SplineParameterizer { 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); } } diff --git a/wpimath/src/main/native/include/frc/trajectory/TrajectoryGenerator.h b/wpimath/src/main/native/include/frc/trajectory/TrajectoryGenerator.h index 3df6c89d3e2..f7076bce1d9 100644 --- a/wpimath/src/main/native/include/frc/trajectory/TrajectoryGenerator.h +++ b/wpimath/src/main/native/include/frc/trajectory/TrajectoryGenerator.h @@ -99,7 +99,7 @@ class WPILIB_DLLEXPORT TrajectoryGenerator { std::vector 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. diff --git a/wpimath/src/test/java/edu/wpi/first/math/spline/CubicHermiteSplineTest.java b/wpimath/src/test/java/edu/wpi/first/math/spline/CubicHermiteSplineTest.java index 4552c20a2ba..74b06947c7f 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/spline/CubicHermiteSplineTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/spline/CubicHermiteSplineTest.java @@ -36,7 +36,7 @@ private void run(Pose2d a, List waypoints, Pose2d b) { var poses = new ArrayList(); - poses.add(splines[0].getPoint(0.0)); + poses.add(splines[0].getPoint(0.0).get()); for (var spline : splines) { poses.addAll(SplineParameterizer.parameterize(spline)); diff --git a/wpimath/src/test/native/cpp/spline/CubicHermiteSplineTest.cpp b/wpimath/src/test/native/cpp/spline/CubicHermiteSplineTest.cpp index 3a4acc0e65e..d16444cd675 100644 --- a/wpimath/src/test/native/cpp/spline/CubicHermiteSplineTest.cpp +++ b/wpimath/src/test/native/cpp/spline/CubicHermiteSplineTest.cpp @@ -30,7 +30,7 @@ class CubicHermiteSplineTest : public ::testing::Test { SplineHelper::CubicSplinesFromControlVectors(startCV, waypoints, endCV); std::vector::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);