Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-21 01:01:43 +00:00
Code Issues Packages Projects Releases Wiki Activity

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

Browse Source 
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.
This commit is contained in:
Declan Freeman-Gleason
2020-01-01 18:23:08 -08:00
committed by Peter Johnson
parent 222669dc2c
commit 012d93b2bd
11 changed files with 281 additions and 88 deletions
Download Patch File Download Diff File Expand all files Collapse all files

11
wpilibc/src/test/native/cpp/spline/CubicHermiteSplineTest.cpp
View File

@@ -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<Translation2d>{},
Pose2d{1_m, 0_m, Rotation2d(180_deg)}),
SplineParameterizer::MalformedSplineException);
EXPECT_THROW(
Run(Pose2d{10_m, 10_m, Rotation2d(90_deg)}, std::vector<Translation2d>{},
Pose2d{10_m, 11_m, Rotation2d(-90_deg)}),
SplineParameterizer::MalformedSplineException);
}

9
wpilibc/src/test/native/cpp/spline/QuinticHermiteSplineTest.cpp
View File

@@ -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);
}

10
wpilibc/src/test/native/cpp/trajectory/TrajectoryGeneratorTest.cpp
View File

@@ -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>{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);
}