From bf266565476e8996adae04109efa61d9bf59523e Mon Sep 17 00:00:00 2001 From: Prateek Machiraju Date: Sun, 4 Oct 2020 15:51:48 -0400 Subject: [PATCH] [wpimath] Fix quintic spline generation from control vectors (#2762) This does not introduce any breaking changes for teams that used the TrajectoryGenerator API for quintic splines with poses. The GetQuinticControlVectorsFromWaypoints() method was removed because it is not possible for us (or would require breaking changes to the shape of the splines) to generate only one quintic control vector per Pose2d. Users who actually have control vectors directly (i.e. not from Pose2d objects, but a dashboard such as PathWeaver) should have the number of control vectors correspond to the number of "waypoints" and can call GetQuinticSplinesFromControlVectors() directly. --- .../first/wpilibj/spline/SplineHelper.java | 32 ++++++++--------- .../trajectory/TrajectoryGenerator.java | 35 +++++++++++++++++-- .../main/native/cpp/spline/SplineHelper.cpp | 18 +++++----- .../cpp/trajectory/TrajectoryGenerator.cpp | 28 +++++++++++++-- .../native/include/frc/spline/SplineHelper.h | 8 ++--- .../spline/QuinticHermiteSplineTest.java | 4 +-- .../cpp/spline/QuinticHermiteSplineTest.cpp | 3 +- 7 files changed, 89 insertions(+), 39 deletions(-) diff --git a/wpimath/src/main/java/edu/wpi/first/wpilibj/spline/SplineHelper.java b/wpimath/src/main/java/edu/wpi/first/wpilibj/spline/SplineHelper.java index 73c6aeef61..a2bf9dd108 100644 --- a/wpimath/src/main/java/edu/wpi/first/wpilibj/spline/SplineHelper.java +++ b/wpimath/src/main/java/edu/wpi/first/wpilibj/spline/SplineHelper.java @@ -7,7 +7,6 @@ package edu.wpi.first.wpilibj.spline; -import java.util.ArrayList; import java.util.Arrays; import java.util.List; @@ -53,26 +52,28 @@ public final class SplineHelper { } /** - * Returns quintic control vectors from a set of waypoints. + * Returns quintic splines from a set of waypoints. * * @param waypoints The waypoints - * @return List of control vectors + * @return List of splines. */ - public static List getQuinticControlVectorsFromWaypoints( - List waypoints - ) { - List vectors = new ArrayList<>(); - for (int i = 0; i < waypoints.size() - 1; i++) { + @SuppressWarnings("PMD.AvoidInstantiatingObjectsInLoops") + public static QuinticHermiteSpline[] getQuinticSplinesFromWaypoints(List waypoints) { + QuinticHermiteSpline[] splines = new QuinticHermiteSpline[waypoints.size() - 1]; + for (int i = 0; i < waypoints.size() - 1; ++i) { var p0 = waypoints.get(i); var p1 = waypoints.get(i + 1); // This just makes the splines look better. final var scalar = 1.2 * p0.getTranslation().getDistance(p1.getTranslation()); - vectors.add(getQuinticControlVector(scalar, p0)); - vectors.add(getQuinticControlVector(scalar, p1)); + var controlVecA = getQuinticControlVector(scalar, p0); + var controlVecB = getQuinticControlVector(scalar, p1); + + splines[i] + = new QuinticHermiteSpline(controlVecA.x, controlVecB.x, controlVecA.y, controlVecB.y); } - return vectors; + return splines; } /** @@ -216,15 +217,14 @@ public final class SplineHelper { @SuppressWarnings({"LocalVariableName", "PMD.AvoidInstantiatingObjectsInLoops"}) public static QuinticHermiteSpline[] getQuinticSplinesFromControlVectors( Spline.ControlVector[] controlVectors) { - // There are twice as many control vectors are there are splines. - QuinticHermiteSpline[] splines = new QuinticHermiteSpline[controlVectors.length / 2]; - for (int i = 0; i < controlVectors.length - 1; i += 2) { + QuinticHermiteSpline[] splines = new QuinticHermiteSpline[controlVectors.length - 1]; + for (int i = 0; i < controlVectors.length - 1; i++) { var xInitial = controlVectors[i].x; var xFinal = controlVectors[i + 1].x; var yInitial = controlVectors[i].y; var yFinal = controlVectors[i + 1].y; - splines[i / 2] = new QuinticHermiteSpline(xInitial, xFinal, - yInitial, yFinal); + splines[i] = new QuinticHermiteSpline(xInitial, xFinal, + yInitial, yFinal); } return splines; } diff --git a/wpimath/src/main/java/edu/wpi/first/wpilibj/trajectory/TrajectoryGenerator.java b/wpimath/src/main/java/edu/wpi/first/wpilibj/trajectory/TrajectoryGenerator.java index 32c2873957..5e55c503c4 100644 --- a/wpimath/src/main/java/edu/wpi/first/wpilibj/trajectory/TrajectoryGenerator.java +++ b/wpimath/src/main/java/edu/wpi/first/wpilibj/trajectory/TrajectoryGenerator.java @@ -196,9 +196,38 @@ public final class TrajectoryGenerator { */ @SuppressWarnings("LocalVariableName") public static Trajectory generateTrajectory(List waypoints, TrajectoryConfig config) { - var originalList = SplineHelper.getQuinticControlVectorsFromWaypoints(waypoints); - var newList = new ControlVectorList(originalList); - return generateTrajectory(newList, config); + final var flip = new Transform2d(new Translation2d(), Rotation2d.fromDegrees(180.0)); + + List newWaypoints = new ArrayList<>(); + if (config.isReversed()) { + for (Pose2d originalWaypoint : waypoints) { + newWaypoints.add(originalWaypoint.plus(flip)); + } + } else { + newWaypoints.addAll(waypoints); + } + + // Get the spline points + List points; + try { + points = splinePointsFromSplines(SplineHelper.getQuinticSplinesFromWaypoints(newWaypoints)); + } catch (MalformedSplineException ex) { + reportError(ex.getMessage(), ex.getStackTrace()); + return kDoNothingTrajectory; + } + + // Change the points back to their original orientation. + if (config.isReversed()) { + for (var point : points) { + point.poseMeters = point.poseMeters.plus(flip); + point.curvatureRadPerMeter *= -1; + } + } + + // Generate and return trajectory. + return TrajectoryParameterizer.timeParameterizeTrajectory(points, config.getConstraints(), + config.getStartVelocity(), config.getEndVelocity(), config.getMaxVelocity(), + config.getMaxAcceleration(), config.isReversed()); } /** diff --git a/wpimath/src/main/native/cpp/spline/SplineHelper.cpp b/wpimath/src/main/native/cpp/spline/SplineHelper.cpp index e50e686c17..58f7537882 100644 --- a/wpimath/src/main/native/cpp/spline/SplineHelper.cpp +++ b/wpimath/src/main/native/cpp/spline/SplineHelper.cpp @@ -127,9 +127,7 @@ std::vector SplineHelper::QuinticSplinesFromControlVectors( const std::vector::ControlVector>& controlVectors) { std::vector splines; - // There are twice as many control vectors are there are splines, - // so we increment the counter by 2. - for (size_t i = 0; i < controlVectors.size() - 1; i += 2) { + for (size_t i = 0; i < controlVectors.size() - 1; ++i) { auto& xInitial = controlVectors[i].x; auto& yInitial = controlVectors[i].y; auto& xFinal = controlVectors[i + 1].x; @@ -160,10 +158,10 @@ SplineHelper::CubicControlVectorsFromWaypoints( return {initialCV, finalCV}; } -std::vector::ControlVector> -SplineHelper::QuinticControlVectorsFromWaypoints( +std::vector SplineHelper::QuinticSplinesFromWaypoints( const std::vector& waypoints) { - std::vector::ControlVector> vectors; + std::vector splines; + splines.reserve(waypoints.size() - 1); for (size_t i = 0; i < waypoints.size() - 1; ++i) { auto& p0 = waypoints[i]; auto& p1 = waypoints[i + 1]; @@ -172,10 +170,12 @@ SplineHelper::QuinticControlVectorsFromWaypoints( const auto scalar = 1.2 * p0.Translation().Distance(p1.Translation()).to(); - vectors.push_back(QuinticControlVector(scalar, p0)); - vectors.push_back(QuinticControlVector(scalar, p1)); + auto controlVectorA = QuinticControlVector(scalar, p0); + auto controlVectorB = QuinticControlVector(scalar, p1); + splines.emplace_back(controlVectorA.x, controlVectorB.x, controlVectorA.y, + controlVectorB.y); } - return vectors; + return splines; } void SplineHelper::ThomasAlgorithm(const std::vector& a, diff --git a/wpimath/src/main/native/cpp/trajectory/TrajectoryGenerator.cpp b/wpimath/src/main/native/cpp/trajectory/TrajectoryGenerator.cpp index 03821e2337..6cb3f7aa9c 100644 --- a/wpimath/src/main/native/cpp/trajectory/TrajectoryGenerator.cpp +++ b/wpimath/src/main/native/cpp/trajectory/TrajectoryGenerator.cpp @@ -113,6 +113,30 @@ Trajectory TrajectoryGenerator::GenerateTrajectory( Trajectory TrajectoryGenerator::GenerateTrajectory( const std::vector& waypoints, const TrajectoryConfig& config) { - return GenerateTrajectory( - SplineHelper::QuinticControlVectorsFromWaypoints(waypoints), config); + auto newWaypoints = waypoints; + const Transform2d flip{Translation2d(), Rotation2d(180_deg)}; + if (config.IsReversed()) + for (auto& waypoint : newWaypoints) waypoint += flip; + + std::vector points; + try { + points = SplinePointsFromSplines( + SplineHelper::QuinticSplinesFromWaypoints(newWaypoints)); + } catch (SplineParameterizer::MalformedSplineException& e) { + ReportError(e.what()); + return kDoNothingTrajectory; + } + + // After trajectory generation, flip theta back so it's relative to the + // field. Also fix curvature. + if (config.IsReversed()) { + for (auto& point : points) { + point = {point.first + flip, -point.second}; + } + } + + return TrajectoryParameterizer::TimeParameterizeTrajectory( + points, config.Constraints(), config.StartVelocity(), + config.EndVelocity(), config.MaxVelocity(), config.MaxAcceleration(), + config.IsReversed()); } diff --git a/wpimath/src/main/native/include/frc/spline/SplineHelper.h b/wpimath/src/main/native/include/frc/spline/SplineHelper.h index 7a12542582..e04fa4532d 100644 --- a/wpimath/src/main/native/include/frc/spline/SplineHelper.h +++ b/wpimath/src/main/native/include/frc/spline/SplineHelper.h @@ -36,13 +36,13 @@ class SplineHelper { const Pose2d& end); /** - * Returns quintic control vectors from a set of waypoints. + * Returns quintic splines from a set of waypoints. * * @param waypoints The waypoints - * @return List of control vectors + * @return List of quintic splines. */ - static std::vector::ControlVector> - QuinticControlVectorsFromWaypoints(const std::vector& waypoints); + static std::vector QuinticSplinesFromWaypoints( + const std::vector& waypoints); /** * Returns a set of cubic splines corresponding to the provided control diff --git a/wpimath/src/test/java/edu/wpi/first/wpilibj/spline/QuinticHermiteSplineTest.java b/wpimath/src/test/java/edu/wpi/first/wpilibj/spline/QuinticHermiteSplineTest.java index b84f1f750d..3b35db0094 100644 --- a/wpimath/src/test/java/edu/wpi/first/wpilibj/spline/QuinticHermiteSplineTest.java +++ b/wpimath/src/test/java/edu/wpi/first/wpilibj/spline/QuinticHermiteSplineTest.java @@ -31,9 +31,7 @@ class QuinticHermiteSplineTest { //var start = System.nanoTime(); // Generate and parameterize the spline. - var controlVectors = SplineHelper.getQuinticControlVectorsFromWaypoints(List.of(a, b)); - var spline = SplineHelper.getQuinticSplinesFromControlVectors( - controlVectors.toArray(new Spline.ControlVector[0]))[0]; + var spline = SplineHelper.getQuinticSplinesFromWaypoints(List.of(a, b))[0]; var poses = SplineParameterizer.parameterize(spline); // End the timer. diff --git a/wpimath/src/test/native/cpp/spline/QuinticHermiteSplineTest.cpp b/wpimath/src/test/native/cpp/spline/QuinticHermiteSplineTest.cpp index 3dd70a579d..30b5b31182 100644 --- a/wpimath/src/test/native/cpp/spline/QuinticHermiteSplineTest.cpp +++ b/wpimath/src/test/native/cpp/spline/QuinticHermiteSplineTest.cpp @@ -27,8 +27,7 @@ class QuinticHermiteSplineTest : public ::testing::Test { const auto start = std::chrono::high_resolution_clock::now(); // Generate and parameterize the spline. - const auto spline = SplineHelper::QuinticSplinesFromControlVectors( - SplineHelper::QuinticControlVectorsFromWaypoints({a, b}))[0]; + const auto spline = SplineHelper::QuinticSplinesFromWaypoints({a, b})[0]; const auto poses = SplineParameterizer::Parameterize(spline); // End timer.