From 9440edf2b5a64cf6dced67666ed14d86553e3ea0 Mon Sep 17 00:00:00 2001 From: Prateek Machiraju Date: Sat, 26 Oct 2019 12:39:47 -0400 Subject: [PATCH] Refactor TrajectoryGenerator (#1972) --- .../main/native/cpp/spline/SplineHelper.cpp | 144 +++---- .../cpp/trajectory/TrajectoryGenerator.cpp | 102 ++--- .../trajectory/TrajectoryParameterizer.cpp | 2 +- .../main/native/include/frc/spline/Spline.h | 13 + .../native/include/frc/spline/SplineHelper.h | 74 +++- .../include/frc/trajectory/TrajectoryConfig.h | 140 +++++++ .../frc/trajectory/TrajectoryGenerator.h | 132 ++----- .../frc/trajectory/TrajectoryParameterizer.h | 2 +- .../cpp/controller/RamseteControllerTest.cpp | 2 +- .../cpp/spline/CubicHermiteSplineTest.cpp | 6 +- .../cpp/spline/QuinticHermiteSplineTest.cpp | 3 +- .../CentripetalAccelerationConstraintTest.cpp | 8 +- .../DifferentialDriveKinematicsTest.cpp | 9 +- .../trajectory/TrajectoryGeneratorTest.cpp | 4 +- .../include/trajectory/TestTrajectory.h | 15 +- .../edu/wpi/first/wpilibj/spline/Spline.java | 25 ++ .../first/wpilibj/spline/SplineHelper.java | 188 +++++---- .../wpilibj/trajectory/TrajectoryConfig.java | 165 ++++++++ .../trajectory/TrajectoryGenerator.java | 359 +++++------------- .../controller/RamseteControllerTest.java | 32 +- .../spline/CubicHermiteSplineTest.java | 7 +- .../spline/QuinticHermiteSplineTest.java | 6 +- .../trajectory/TrajectoryGeneratorTest.java | 16 +- 23 files changed, 825 insertions(+), 629 deletions(-) create mode 100644 wpilibc/src/main/native/include/frc/trajectory/TrajectoryConfig.h create mode 100644 wpilibj/src/main/java/edu/wpi/first/wpilibj/trajectory/TrajectoryConfig.java diff --git a/wpilibc/src/main/native/cpp/spline/SplineHelper.cpp b/wpilibc/src/main/native/cpp/spline/SplineHelper.cpp index b3eda49fcb..7c3fdc1964 100644 --- a/wpilibc/src/main/native/cpp/spline/SplineHelper.cpp +++ b/wpilibc/src/main/native/cpp/spline/SplineHelper.cpp @@ -11,39 +11,22 @@ using namespace frc; -std::vector SplineHelper::CubicSplinesFromWaypoints( - const Pose2d& start, std::vector waypoints, - const Pose2d& end) { +std::vector SplineHelper::CubicSplinesFromControlVectors( + const Spline<3>::ControlVector& start, std::vector waypoints, + const Spline<3>::ControlVector& end) { std::vector splines; - double scalar; - // This just makes the splines look better. - if (waypoints.empty()) { - scalar = 1.2 * start.Translation().Distance(end.Translation()).to(); - } else { - scalar = 1.2 * start.Translation().Distance(waypoints.front()).to(); - } - - std::array startXControlVector{ - start.Translation().X().to(), start.Rotation().Cos() * scalar}; - - std::array startYControlVector{ - start.Translation().Y().to(), start.Rotation().Sin() * scalar}; - - // This just makes the splines look better. - if (!waypoints.empty()) { - scalar = 1.2 * end.Translation().Distance(waypoints.back()).to(); - } - - std::array endXControlVector{end.Translation().X().to(), - end.Rotation().Cos() * scalar}; - - std::array endYControlVector{end.Translation().Y().to(), - end.Rotation().Sin() * scalar}; + std::array xInitial = start.x; + std::array yInitial = start.y; + std::array xFinal = end.x; + std::array yFinal = end.y; if (waypoints.size() > 1) { - waypoints.emplace(waypoints.begin(), start.Translation()); - waypoints.emplace_back(end.Translation()); + waypoints.emplace(waypoints.begin(), + Translation2d{units::meter_t(xInitial[0]), + units::meter_t(yInitial[0])}); + waypoints.emplace_back( + Translation2d{units::meter_t(xFinal[0]), units::meter_t(yFinal[0])}); std::vector a; std::vector b(waypoints.size() - 2, 4.0); @@ -53,7 +36,7 @@ std::vector SplineHelper::CubicSplinesFromWaypoints( fy(waypoints.size() - 2, 0.0); a.emplace_back(0); - for (unsigned int i = 0; i < waypoints.size() - 3; i++) { + for (size_t i = 0; i < waypoints.size() - 3; ++i) { a.emplace_back(1); c.emplace_back(1); } @@ -61,12 +44,12 @@ std::vector SplineHelper::CubicSplinesFromWaypoints( dx.emplace_back( 3 * (waypoints[2].X().to() - waypoints[0].X().to()) - - startXControlVector[1]); + xInitial[1]); dy.emplace_back( 3 * (waypoints[2].Y().to() - waypoints[0].Y().to()) - - startYControlVector[1]); + yInitial[1]); if (waypoints.size() > 4) { - for (unsigned int i = 1; i <= waypoints.size() - 4; i++) { + for (size_t i = 1; i <= waypoints.size() - 4; ++i) { dx.emplace_back(3 * (waypoints[i + 1].X().to() - waypoints[i - 1].X().to())); dy.emplace_back(3 * (waypoints[i + 1].Y().to() - @@ -75,20 +58,20 @@ std::vector SplineHelper::CubicSplinesFromWaypoints( } dx.emplace_back(3 * (waypoints[waypoints.size() - 1].X().to() - waypoints[waypoints.size() - 3].X().to()) - - endXControlVector[1]); + xFinal[1]); dy.emplace_back(3 * (waypoints[waypoints.size() - 1].Y().to() - waypoints[waypoints.size() - 3].Y().to()) - - endYControlVector[1]); + yFinal[1]); ThomasAlgorithm(a, b, c, dx, &fx); ThomasAlgorithm(a, b, c, dy, &fy); - fx.emplace(fx.begin(), startXControlVector[1]); - fx.emplace_back(endXControlVector[1]); - fy.emplace(fy.begin(), startYControlVector[1]); - fy.emplace_back(endYControlVector[1]); + fx.emplace(fx.begin(), xInitial[1]); + fx.emplace_back(xFinal[1]); + fy.emplace(fy.begin(), yInitial[1]); + fy.emplace_back(yFinal[1]); - for (unsigned int i = 0; i < fx.size() - 1; i++) { + for (size_t i = 0; i < fx.size() - 1; ++i) { // Create the spline. const CubicHermiteSpline spline{ {waypoints[i].X().to(), fx[i]}, @@ -99,39 +82,69 @@ std::vector SplineHelper::CubicSplinesFromWaypoints( splines.push_back(spline); } } else if (waypoints.size() == 1) { - const double xDeriv = (3 * (end.Translation().X().to() - - start.Translation().X().to()) - - endXControlVector[1] - startXControlVector[1]) / - 4.0; - const double yDeriv = (3 * (end.Translation().Y().to() - - start.Translation().Y().to()) - - endYControlVector[1] - startYControlVector[1]) / - 4.0; + const double xDeriv = + (3 * (xFinal[0] - xInitial[0]) - xFinal[1] - xInitial[1]) / 4.0; + const double yDeriv = + (3 * (yFinal[0] - yInitial[0]) - yFinal[1] - yInitial[1]) / 4.0; std::array midXControlVector{waypoints[0].X().to(), xDeriv}; std::array midYControlVector{waypoints[0].Y().to(), yDeriv}; - splines.emplace_back(startXControlVector, midXControlVector, - startYControlVector, midYControlVector); - splines.emplace_back(midXControlVector, endXControlVector, - midYControlVector, endYControlVector); + splines.emplace_back(xInitial, midXControlVector, yInitial, + midYControlVector); + splines.emplace_back(midXControlVector, xFinal, midYControlVector, yFinal); } else { // Create the spline. - const CubicHermiteSpline spline{startXControlVector, endXControlVector, - startYControlVector, endYControlVector}; + const CubicHermiteSpline spline{xInitial, xFinal, yInitial, yFinal}; splines.push_back(spline); } return splines; } -std::vector SplineHelper::QuinticSplinesFromWaypoints( - const std::vector& waypoints) { +std::vector +SplineHelper::QuinticSplinesFromControlVectors( + const std::vector::ControlVector>& controlVectors) { std::vector splines; - for (unsigned int i = 0; i < waypoints.size() - 1; i++) { + 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; + auto& yFinal = controlVectors[i + 1].y; + splines.emplace_back(xInitial, xFinal, yInitial, yFinal); + } + return splines; +} + +std::array::ControlVector, 2> +SplineHelper::CubicControlVectorsFromWaypoints( + const Pose2d& start, const std::vector& interiorWaypoints, + const Pose2d& end) { + double scalar; + if (interiorWaypoints.empty()) { + scalar = 1.2 * start.Translation().Distance(end.Translation()).to(); + } else { + scalar = + 1.2 * + start.Translation().Distance(interiorWaypoints.front()).to(); + } + const auto initialCV = CubicControlVector(scalar, start); + if (!interiorWaypoints.empty()) { + scalar = + 1.2 * end.Translation().Distance(interiorWaypoints.back()).to(); + } + const auto finalCV = CubicControlVector(scalar, end); + return {initialCV, finalCV}; +} + +std::vector::ControlVector> +SplineHelper::QuinticControlVectorsFromWaypoints( + const std::vector& waypoints) { + std::vector::ControlVector> vectors; + for (size_t i = 0; i < waypoints.size() - 1; ++i) { auto& p0 = waypoints[i]; auto& p1 = waypoints[i + 1]; @@ -139,19 +152,10 @@ std::vector SplineHelper::QuinticSplinesFromWaypoints( const auto scalar = 1.2 * p0.Translation().Distance(p1.Translation()).to(); - const std::array xInitialControlVector{ - p0.Translation().X().to(), p0.Rotation().Cos() * scalar, 0.0}; - const std::array xFinalControlVector{ - p1.Translation().X().to(), p1.Rotation().Cos() * scalar, 0.0}; - const std::array yInitialControlVector{ - p0.Translation().Y().to(), p0.Rotation().Sin() * scalar, 0.0}; - const std::array yFinalControlVector{ - p1.Translation().Y().to(), p1.Rotation().Sin() * scalar, 0.0}; - - splines.emplace_back(xInitialControlVector, xFinalControlVector, - yInitialControlVector, yFinalControlVector); + vectors.push_back(QuinticControlVector(scalar, p0)); + vectors.push_back(QuinticControlVector(scalar, p1)); } - return splines; + return vectors; } void SplineHelper::ThomasAlgorithm(const std::vector& a, @@ -176,7 +180,7 @@ void SplineHelper::ThomasAlgorithm(const std::vector& a, d_star[0] = d[0] / b[0]; // Create the c_star and d_star coefficients in the forward sweep - for (unsigned int i = 1; i < N; i++) { + for (size_t i = 1; i < N; ++i) { double m = 1.0 / (b[i] - a[i] * c_star[i - 1]); c_star[i] = c[i] * m; d_star[i] = (d[i] - a[i] * d_star[i - 1]) * m; diff --git a/wpilibc/src/main/native/cpp/trajectory/TrajectoryGenerator.cpp b/wpilibc/src/main/native/cpp/trajectory/TrajectoryGenerator.cpp index ed97a904f1..92c21f630a 100644 --- a/wpilibc/src/main/native/cpp/trajectory/TrajectoryGenerator.cpp +++ b/wpilibc/src/main/native/cpp/trajectory/TrajectoryGenerator.cpp @@ -15,98 +15,78 @@ using namespace frc; Trajectory TrajectoryGenerator::GenerateTrajectory( - const std::vector& waypoints, - std::vector>&& constraints, - units::meters_per_second_t startVelocity, - units::meters_per_second_t endVelocity, - units::meters_per_second_t maxVelocity, - units::meters_per_second_squared_t maxAcceleration, bool reversed) { + Spline<3>::ControlVector initial, + const std::vector& interiorWaypoints, + Spline<3>::ControlVector end, const TrajectoryConfig& config) { const Transform2d flip{Translation2d(), Rotation2d(180_deg)}; // Make theta normal for trajectory generation if path is reversed. - std::vector newWaypoints; - newWaypoints.reserve(waypoints.size()); - for (auto&& point : waypoints) { - newWaypoints.push_back(reversed ? point + flip : point); + // Flip the headings. + if (config.IsReversed()) { + initial.x[1] *= -1; + initial.y[1] *= -1; + end.x[1] *= -1; + end.y[1] *= -1; } - auto points = SplinePointsFromSplines( - SplineHelper::QuinticSplinesFromWaypoints(newWaypoints)); + auto points = + SplinePointsFromSplines(SplineHelper::CubicSplinesFromControlVectors( + initial, interiorWaypoints, end)); // After trajectory generation, flip theta back so it's relative to the // field. Also fix curvature. - if (reversed) { + if (config.IsReversed()) { for (auto& point : points) { point = {point.first + flip, -point.second}; } } return TrajectoryParameterizer::TimeParameterizeTrajectory( - points, std::move(constraints), startVelocity, endVelocity, maxVelocity, - maxAcceleration, reversed); + points, config.Constraints(), config.StartVelocity(), + config.EndVelocity(), config.MaxVelocity(), config.MaxAcceleration(), + config.IsReversed()); } Trajectory TrajectoryGenerator::GenerateTrajectory( - const Pose2d& start, const std::vector& waypoints, - const Pose2d& end, - std::vector>&& constraints, - units::meters_per_second_t startVelocity, - units::meters_per_second_t endVelocity, - units::meters_per_second_t maxVelocity, - units::meters_per_second_squared_t maxAcceleration, bool reversed) { + const Pose2d& start, const std::vector& interiorWaypoints, + const Pose2d& end, const TrajectoryConfig& config) { + auto [startCV, endCV] = SplineHelper::CubicControlVectorsFromWaypoints( + start, interiorWaypoints, end); + return GenerateTrajectory(startCV, interiorWaypoints, endCV, config); +} + +Trajectory TrajectoryGenerator::GenerateTrajectory( + std::vector::ControlVector> controlVectors, + const TrajectoryConfig& config) { const Transform2d flip{Translation2d(), Rotation2d(180_deg)}; // Make theta normal for trajectory generation if path is reversed. - - const Pose2d newStart = reversed ? start + flip : start; - const Pose2d newEnd = reversed ? end + flip : end; + if (config.IsReversed()) { + for (auto& vector : controlVectors) { + // Flip the headings. + vector.x[1] *= -1; + vector.y[1] *= -1; + } + } auto points = SplinePointsFromSplines( - SplineHelper::CubicSplinesFromWaypoints(newStart, waypoints, newEnd)); + SplineHelper::QuinticSplinesFromControlVectors(controlVectors)); // After trajectory generation, flip theta back so it's relative to the // field. Also fix curvature. - if (reversed) { + if (config.IsReversed()) { for (auto& point : points) { point = {point.first + flip, -point.second}; } } return TrajectoryParameterizer::TimeParameterizeTrajectory( - points, std::move(constraints), startVelocity, endVelocity, maxVelocity, - maxAcceleration, reversed); + points, config.Constraints(), config.StartVelocity(), + config.EndVelocity(), config.MaxVelocity(), config.MaxAcceleration(), + config.IsReversed()); } Trajectory TrajectoryGenerator::GenerateTrajectory( - const std::vector& waypoints, - const DifferentialDriveKinematics& differentialDriveKinematics, - units::meters_per_second_t startVelocity, - units::meters_per_second_t endVelocity, - units::meters_per_second_t maxVelocity, - units::meters_per_second_squared_t maxAcceleration, bool reversed) { - std::vector> constraints; - constraints.emplace_back( - std::make_unique( - differentialDriveKinematics, maxVelocity)); - - return GenerateTrajectory(waypoints, std::move(constraints), startVelocity, - endVelocity, maxVelocity, maxAcceleration, - reversed); -} - -Trajectory TrajectoryGenerator::GenerateTrajectory( - const Pose2d& start, const std::vector& waypoints, - const Pose2d& end, - const DifferentialDriveKinematics& differentialDriveKinematics, - units::meters_per_second_t startVelocity, - units::meters_per_second_t endVelocity, - units::meters_per_second_t maxVelocity, - units::meters_per_second_squared_t maxAcceleration, bool reversed) { - std::vector> constraints; - constraints.emplace_back( - std::make_unique( - differentialDriveKinematics, maxVelocity)); - - return GenerateTrajectory(start, waypoints, end, std::move(constraints), - startVelocity, endVelocity, maxVelocity, - maxAcceleration, reversed); + const std::vector& waypoints, const TrajectoryConfig& config) { + return GenerateTrajectory( + SplineHelper::QuinticControlVectorsFromWaypoints(waypoints), config); } diff --git a/wpilibc/src/main/native/cpp/trajectory/TrajectoryParameterizer.cpp b/wpilibc/src/main/native/cpp/trajectory/TrajectoryParameterizer.cpp index f16b1daae9..131ae8a421 100644 --- a/wpilibc/src/main/native/cpp/trajectory/TrajectoryParameterizer.cpp +++ b/wpilibc/src/main/native/cpp/trajectory/TrajectoryParameterizer.cpp @@ -35,7 +35,7 @@ using namespace frc; Trajectory TrajectoryParameterizer::TimeParameterizeTrajectory( const std::vector& points, - std::vector>&& constraints, + const std::vector>& constraints, units::meters_per_second_t startVelocity, units::meters_per_second_t endVelocity, units::meters_per_second_t maxVelocity, diff --git a/wpilibc/src/main/native/include/frc/spline/Spline.h b/wpilibc/src/main/native/include/frc/spline/Spline.h index 236661b586..e8f2372f02 100644 --- a/wpilibc/src/main/native/include/frc/spline/Spline.h +++ b/wpilibc/src/main/native/include/frc/spline/Spline.h @@ -7,6 +7,7 @@ #pragma once +#include #include #include @@ -43,6 +44,18 @@ class Spline { virtual ~Spline() = default; + /** + * Represents a control vector for a spline. + * + * Each element in each array represents the value of the derivative at the + * index. For example, the value of x[2] is the second derivative in the x + * dimension. + */ + struct ControlVector { + std::array x; + std::array y; + }; + /** * Gets the pose and curvature at some point t on the spline. * diff --git a/wpilibc/src/main/native/include/frc/spline/SplineHelper.h b/wpilibc/src/main/native/include/frc/spline/SplineHelper.h index 363debb185..4ed1cf52f1 100644 --- a/wpilibc/src/main/native/include/frc/spline/SplineHelper.h +++ b/wpilibc/src/main/native/include/frc/spline/SplineHelper.h @@ -7,6 +7,8 @@ #pragma once +#include +#include #include #include "frc/spline/CubicHermiteSpline.h" @@ -20,39 +22,79 @@ namespace frc { class SplineHelper { public: /** - * Returns a set of cubic splines corresponding to the provided waypoints. The - * user is free to set the direction of the start and end point. The - * directions for the middle waypoints are determined automatically to ensure - * continuous curvature throughout the path. + * Returns 2 cubic control vectors from a set of exterior waypoints and + * interior translations. + * + * @param start The starting pose. + * @param interiorWaypoints The interior waypoints. + * @param end The ending pose. + * @return 2 cubic control vectors. + */ + static std::array::ControlVector, 2> + CubicControlVectorsFromWaypoints( + const Pose2d& start, const std::vector& interiorWaypoints, + const Pose2d& end); + + /** + * Returns quintic control vectors from a set of waypoints. + * + * @param waypoints The waypoints + * @return List of control vectors + */ + static std::vector::ControlVector> + QuinticControlVectorsFromWaypoints(const std::vector& waypoints); + + /** + * Returns a set of cubic splines corresponding to the provided control + * vectors. The user is free to set the direction of the start and end + * point. The directions for the middle waypoints are determined + * automatically to ensure continuous curvature throughout the path. * * The derivation for the algorithm used can be found here: * * - * @param start The starting waypoint. - * @param waypoints The middle waypoints. This can be left blank if you only - * wish to create a path with two waypoints. - * @param end The ending waypoint. + * @param start The starting control vector. + * @param waypoints The middle waypoints. This can be left blank if you + * only wish to create a path with two waypoints. + * @param end The ending control vector. * * @return A vector of cubic hermite splines that interpolate through the * provided waypoints. */ - static std::vector CubicSplinesFromWaypoints( - const Pose2d& start, std::vector waypoints, - const Pose2d& end); + static std::vector CubicSplinesFromControlVectors( + const Spline<3>::ControlVector& start, + std::vector waypoints, + const Spline<3>::ControlVector& end); /** - * Returns a set of quintic splines corresponding to the provided waypoints. - * The user is free to set the direction of all waypoints. Continuous + * Returns a set of quintic splines corresponding to the provided control + * vectors. The user is free to set the direction of all waypoints. Continuous * curvature is guaranteed throughout the path. * - * @param waypoints The waypoints. + * @param controlVectors The control vectors. * @return A vector of quintic hermite splines that interpolate through the * provided waypoints. */ - static std::vector QuinticSplinesFromWaypoints( - const std::vector& waypoints); + static std::vector QuinticSplinesFromControlVectors( + const std::vector::ControlVector>& controlVectors); private: + static Spline<3>::ControlVector CubicControlVector(double scalar, + const Pose2d& point) { + return { + {point.Translation().X().to(), scalar * point.Rotation().Cos()}, + {point.Translation().Y().to(), + scalar * point.Rotation().Sin()}}; + } + + static Spline<5>::ControlVector QuinticControlVector(double scalar, + const Pose2d& point) { + return {{point.Translation().X().to(), + scalar * point.Rotation().Cos(), 0.0}, + {point.Translation().Y().to(), + scalar * point.Rotation().Sin(), 0.0}}; + } + /** * Thomas algorithm for solving tridiagonal systems Af = d. * diff --git a/wpilibc/src/main/native/include/frc/trajectory/TrajectoryConfig.h b/wpilibc/src/main/native/include/frc/trajectory/TrajectoryConfig.h new file mode 100644 index 0000000000..a739070aca --- /dev/null +++ b/wpilibc/src/main/native/include/frc/trajectory/TrajectoryConfig.h @@ -0,0 +1,140 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#pragma once + +#include +#include +#include + +#include + +#include "frc/kinematics/DifferentialDriveKinematics.h" +#include "frc/trajectory/constraint/DifferentialDriveKinematicsConstraint.h" +#include "frc/trajectory/constraint/TrajectoryConstraint.h" + +namespace frc { +/** + * Represents the configuration for generating a trajectory. This class stores + * the start velocity, end velocity, max velocity, max acceleration, custom + * constraints, and the reversed flag. + * + * The class must be constructed with a max velocity and max acceleration. + * The other parameters (start velocity, end velocity, constraints, reversed) + * have been defaulted to reasonable values (0, 0, {}, false). These values can + * be changed via the SetXXX methods. + */ +class TrajectoryConfig { + public: + /** + * Constructs a config object. + * @param maxVelocity The max velocity of the trajectory. + * @param maxAcceleration The max acceleration of the trajectory. + */ + TrajectoryConfig(units::meters_per_second_t maxVelocity, + units::meters_per_second_squared_t maxAcceleration) + : m_maxVelocity(maxVelocity), m_maxAcceleration(maxAcceleration) {} + + TrajectoryConfig(const TrajectoryConfig&) = delete; + TrajectoryConfig& operator=(const TrajectoryConfig&) = delete; + + TrajectoryConfig(TrajectoryConfig&&) = default; + TrajectoryConfig& operator=(TrajectoryConfig&&) = default; + + /** + * Sets the start velocity of the trajectory. + * @param startVelocity The start velocity of the trajectory. + */ + void SetStartVelocity(units::meters_per_second_t startVelocity) { + m_startVelocity = startVelocity; + } + + /** + * Sets the end velocity of the trajectory. + * @param endVelocity The end velocity of the trajectory. + */ + void SetEndVelocity(units::meters_per_second_t endVelocity) { + m_endVelocity = endVelocity; + } + + /** + * Sets the reversed flag of the trajectory. + * @param reversed Whether the trajectory should be reversed or not. + */ + void SetReversed(bool reversed) { m_reversed = reversed; } + + /** + * Adds a user-defined constraint to the trajectory. + * @param constraint The user-defined constraint. + */ + template >> + void AddConstraint(Constraint constraint) { + m_constraints.emplace_back(std::make_unique(constraint)); + } + + /** + * Adds a differential drive kinematics constraint to ensure that + * no wheel velocity of a differential drive goes above the max velocity. + * + * @param kinematics The differential drive kinematics. + */ + void SetKinematics(const DifferentialDriveKinematics& kinematics) { + AddConstraint( + DifferentialDriveKinematicsConstraint(kinematics, m_maxVelocity)); + } + + /** + * Returns the starting velocity of the trajectory. + * @return The starting velocity of the trajectory. + */ + units::meters_per_second_t StartVelocity() const { return m_startVelocity; } + + /** + * Returns the ending velocity of the trajectory. + * @return The ending velocity of the trajectory. + */ + units::meters_per_second_t EndVelocity() const { return m_endVelocity; } + + /** + * Returns the maximum velocity of the trajectory. + * @return The maximum velocity of the trajectory. + */ + units::meters_per_second_t MaxVelocity() const { return m_maxVelocity; } + + /** + * Returns the maximum acceleration of the trajectory. + * @return The maximum acceleration of the trajectory. + */ + units::meters_per_second_squared_t MaxAcceleration() const { + return m_maxAcceleration; + } + + /** + * Returns the user-defined constraints of the trajectory. + * @return The user-defined constraints of the trajectory. + */ + const std::vector>& Constraints() + const { + return m_constraints; + } + + /** + * Returns whether the trajectory is reversed or not. + * @return whether the trajectory is reversed or not. + */ + bool IsReversed() const { return m_reversed; } + + private: + units::meters_per_second_t m_startVelocity = 0_mps; + units::meters_per_second_t m_endVelocity = 0_mps; + units::meters_per_second_t m_maxVelocity; + units::meters_per_second_squared_t m_maxAcceleration; + std::vector> m_constraints; + bool m_reversed = false; +}; +} // namespace frc diff --git a/wpilibc/src/main/native/include/frc/trajectory/TrajectoryGenerator.h b/wpilibc/src/main/native/include/frc/trajectory/TrajectoryGenerator.h index 9ca48c6543..4d6eff600c 100644 --- a/wpilibc/src/main/native/include/frc/trajectory/TrajectoryGenerator.h +++ b/wpilibc/src/main/native/include/frc/trajectory/TrajectoryGenerator.h @@ -13,6 +13,7 @@ #include "frc/spline/SplineParameterizer.h" #include "frc/trajectory/Trajectory.h" +#include "frc/trajectory/TrajectoryConfig.h" #include "frc/trajectory/constraint/DifferentialDriveKinematicsConstraint.h" #include "frc/trajectory/constraint/TrajectoryConstraint.h" @@ -25,114 +26,63 @@ class TrajectoryGenerator { using PoseWithCurvature = std::pair; /** - * Generates a trajectory with the given waypoints and constraints. + * Generates a trajectory from the given control vectors and config. This + * method uses clamped cubic splines -- a method in which the exterior control + * vectors and interior waypoints are provided. The headings are automatically + * determined at the interior points to ensure continuous curvature. * - * @param waypoints A vector of points that the trajectory must go through. - * @param constraints A vector of various velocity and acceleration - * constraints. - * @param startVelocity The start velocity for the trajectory. - * @param endVelocity The end velocity for the trajectory. - * @param maxVelocity The max velocity for the trajectory. - * @param maxAcceleration The max acceleration for the trajectory. - * @param reversed Whether the robot should move backwards. Note that the - * robot will still move from a -> b -> ... -> z as defined in the waypoints. - * - * @return The trajectory. + * @param initial The initial control vector. + * @param interiorWaypoints The interior waypoints. + * @param end The ending control vector. + * @param config The configuration for the trajectory. + * @return The generated trajectory. */ static Trajectory GenerateTrajectory( - const std::vector& waypoints, - std::vector>&& constraints, - units::meters_per_second_t startVelocity, - units::meters_per_second_t endVelocity, - units::meters_per_second_t maxVelocity, - units::meters_per_second_squared_t maxAcceleration, - bool reversed = false); + Spline<3>::ControlVector initial, + const std::vector& interiorWaypoints, + Spline<3>::ControlVector end, const TrajectoryConfig& config); /** - * Generates a trajectory with the given waypoints and constraints. + * Generates a trajectory from the given waypoints and config. This method + * uses clamped cubic splines -- a method in which the initial pose, final + * pose, and interior waypoints are provided. The headings are automatically + * determined at the interior points to ensure continuous curvature. * - * @param start The starting pose for the trajectory. - * @param waypoints The interior waypoints for the trajectory. The headings - * will be determined automatically to ensure continuous curvature. - * @param end The ending pose for the trajectory. - * @param constraints A vector of various velocity and acceleration - * constraints. - * @param startVelocity The start velocity for the trajectory. - * @param endVelocity The end velocity for the trajectory. - * @param maxVelocity The max velocity for the trajectory. - * @param maxAcceleration The max acceleration for the trajectory. - * @param reversed Whether the robot should move backwards. Note that the - * robot will still move from a -> b -> ... -> z as defined in the waypoints. - * - * @return The trajectory. + * @param start The starting pose. + * @param interiorWaypoints The interior waypoints. + * @param end The ending pose. + * @param config The configuration for the trajectory. + * @return The generated trajectory. */ static Trajectory GenerateTrajectory( - const Pose2d& start, const std::vector& waypoints, - const Pose2d& end, - std::vector>&& constraints, - units::meters_per_second_t startVelocity, - units::meters_per_second_t endVelocity, - units::meters_per_second_t maxVelocity, - units::meters_per_second_squared_t maxAcceleration, - bool reversed = false); + const Pose2d& start, const std::vector& interiorWaypoints, + const Pose2d& end, const TrajectoryConfig& config); /** - * Generates a trajectory with the given waypoints and differential drive - * constraints. Use this method if you just want a constraint such that none - * of the wheels on your differential drive exceed the specified max velocity. - * If you desire to impose more constraints, please use the other overloads. + * Generates a trajectory from the given quintic control vectors and config. + * This method uses quintic hermite splines -- therefore, all points must be + * represented by control vectors. Continuous curvature is guaranteed in this + * method. * - * @param waypoints A vector of points that the trajectory must go through. - * @param differentialDriveKinematics The DifferentialDriveKinematics - * object that represents your drivetrain. - * @param startVelocity The start velocity for the trajectory. - * @param endVelocity The end velocity for the trajectory. - * @param maxVelocity The max velocity for the trajectory. - * @param maxAcceleration The max acceleration for the trajectory. - * @param reversed Whether the robot should move backwards. Note that the - * robot will still move from a -> b -> ... -> z as defined in the waypoints. - * - * @return The trajectory. + * @param controlVectors List of quintic control vectors. + * @param config The configuration for the trajectory. + * @return The generated trajectory. */ static Trajectory GenerateTrajectory( - const std::vector& waypoints, - const DifferentialDriveKinematics& differentialDriveKinematics, - units::meters_per_second_t startVelocity, - units::meters_per_second_t endVelocity, - units::meters_per_second_t maxVelocity, - units::meters_per_second_squared_t maxAcceleration, - bool reversed = false); + std::vector::ControlVector> controlVectors, + const TrajectoryConfig& config); /** - * Generates a trajectory with the given waypoints and differential drive - * constraints. Use this method if you just want a constraint such that none - * of the wheels on your differential drive exceed the specified max velocity. - * If you desire to impose more constraints, please use the other overloads. + * Generates a trajectory from the given waypoints and config. This method + * uses quintic hermite splines -- therefore, all points must be represented + * by Pose2d objects. Continuous curvature is guaranteed in this method. * - * @param start The starting pose for the trajectory. - * @param waypoints The interior waypoints for the trajectory. The headings - * will be determined automatically to ensure continuous curvature. - * @param end The ending pose for the trajectory. - * @param differentialDriveKinematics The DifferentialDriveKinematics - * object that represents your drivetrain. - * @param startVelocity The start velocity for the trajectory. - * @param endVelocity The end velocity for the trajectory. - * @param maxVelocity The max velocity for the trajectory. - * @param maxAcceleration The max acceleration for the trajectory. - * @param reversed Whether the robot should move backwards. Note that the - * robot will still move from a -> b -> ... -> z as defined in the waypoints. - * - * @return The trajectory. + * @param waypoints List of waypoints.. + * @param config The configuration for the trajectory. + * @return The generated trajectory. */ - static Trajectory GenerateTrajectory( - const Pose2d& start, const std::vector& waypoints, - const Pose2d& end, - const DifferentialDriveKinematics& differentialDriveKinematics, - units::meters_per_second_t startVelocity, - units::meters_per_second_t endVelocity, - units::meters_per_second_t maxVelocity, - units::meters_per_second_squared_t maxAcceleration, - bool reversed = false); + static Trajectory GenerateTrajectory(const std::vector& waypoints, + const TrajectoryConfig& config); /** * Generate spline points from a vector of splines by parameterizing the diff --git a/wpilibc/src/main/native/include/frc/trajectory/TrajectoryParameterizer.h b/wpilibc/src/main/native/include/frc/trajectory/TrajectoryParameterizer.h index 6fb43f3314..b8ea8daf8e 100644 --- a/wpilibc/src/main/native/include/frc/trajectory/TrajectoryParameterizer.h +++ b/wpilibc/src/main/native/include/frc/trajectory/TrajectoryParameterizer.h @@ -67,7 +67,7 @@ class TrajectoryParameterizer { */ static Trajectory TimeParameterizeTrajectory( const std::vector& points, - std::vector>&& constraints, + const std::vector>& constraints, units::meters_per_second_t startVelocity, units::meters_per_second_t endVelocity, units::meters_per_second_t maxVelocity, diff --git a/wpilibc/src/test/native/cpp/controller/RamseteControllerTest.cpp b/wpilibc/src/test/native/cpp/controller/RamseteControllerTest.cpp index d54c034a3b..a6000544dd 100644 --- a/wpilibc/src/test/native/cpp/controller/RamseteControllerTest.cpp +++ b/wpilibc/src/test/native/cpp/controller/RamseteControllerTest.cpp @@ -34,7 +34,7 @@ TEST(RamseteControllerTest, ReachesReference) { auto waypoints = std::vector{frc::Pose2d{2.75_m, 22.521_m, 0_rad}, frc::Pose2d{24.73_m, 19.68_m, 5.846_rad}}; auto trajectory = frc::TrajectoryGenerator::GenerateTrajectory( - waypoints, {}, 0_mps, 0_mps, 8.8_mps, 0.1_mps_sq, false); + waypoints, {8.8_mps, 0.1_mps_sq}); constexpr auto kDt = 0.02_s; auto totalTime = trajectory.TotalTime(); diff --git a/wpilibc/src/test/native/cpp/spline/CubicHermiteSplineTest.cpp b/wpilibc/src/test/native/cpp/spline/CubicHermiteSplineTest.cpp index 3e1da691dd..8254dd4f90 100644 --- a/wpilibc/src/test/native/cpp/spline/CubicHermiteSplineTest.cpp +++ b/wpilibc/src/test/native/cpp/spline/CubicHermiteSplineTest.cpp @@ -29,8 +29,12 @@ class CubicHermiteSplineTest : public ::testing::Test { const auto start = std::chrono::high_resolution_clock::now(); // Generate and parameterize the spline. + + const auto [startCV, endCV] = + SplineHelper::CubicControlVectorsFromWaypoints(a, waypoints, b); + const auto splines = - SplineHelper::CubicSplinesFromWaypoints(a, waypoints, b); + SplineHelper::CubicSplinesFromControlVectors(startCV, waypoints, endCV); std::vector::PoseWithCurvature> poses; poses.push_back(splines[0].GetPoint(0.0)); diff --git a/wpilibc/src/test/native/cpp/spline/QuinticHermiteSplineTest.cpp b/wpilibc/src/test/native/cpp/spline/QuinticHermiteSplineTest.cpp index f4e03e4133..cc10c6c122 100644 --- a/wpilibc/src/test/native/cpp/spline/QuinticHermiteSplineTest.cpp +++ b/wpilibc/src/test/native/cpp/spline/QuinticHermiteSplineTest.cpp @@ -27,7 +27,8 @@ class QuinticHermiteSplineTest : public ::testing::Test { const auto start = std::chrono::high_resolution_clock::now(); // Generate and parameterize the spline. - const auto spline = SplineHelper::QuinticSplinesFromWaypoints({a, b})[0]; + const auto spline = SplineHelper::QuinticSplinesFromControlVectors( + SplineHelper::QuinticControlVectorsFromWaypoints({a, b}))[0]; const auto poses = SplineParameterizer::Parameterize(spline); // End timer. diff --git a/wpilibc/src/test/native/cpp/trajectory/CentripetalAccelerationConstraintTest.cpp b/wpilibc/src/test/native/cpp/trajectory/CentripetalAccelerationConstraintTest.cpp index cb1d13b82c..37f471a1e8 100644 --- a/wpilibc/src/test/native/cpp/trajectory/CentripetalAccelerationConstraintTest.cpp +++ b/wpilibc/src/test/native/cpp/trajectory/CentripetalAccelerationConstraintTest.cpp @@ -20,11 +20,11 @@ using namespace frc; TEST(CentripetalAccelerationConstraintTest, Constraint) { const auto maxCentripetalAcceleration = 7_fps_sq; - std::vector> constraints; - constraints.emplace_back(std::make_unique( - maxCentripetalAcceleration)); + auto config = TrajectoryConfig(12_fps, 12_fps_sq); + config.AddConstraint( + CentripetalAccelerationConstraint(maxCentripetalAcceleration)); - auto trajectory = TestTrajectory::GetTrajectory(std::move(constraints)); + auto trajectory = TestTrajectory::GetTrajectory(config); units::second_t time = 0_s; units::second_t dt = 20_ms; diff --git a/wpilibc/src/test/native/cpp/trajectory/DifferentialDriveKinematicsTest.cpp b/wpilibc/src/test/native/cpp/trajectory/DifferentialDriveKinematicsTest.cpp index 3fe3477e43..df5ddbd665 100644 --- a/wpilibc/src/test/native/cpp/trajectory/DifferentialDriveKinematicsTest.cpp +++ b/wpilibc/src/test/native/cpp/trajectory/DifferentialDriveKinematicsTest.cpp @@ -21,12 +21,11 @@ TEST(DifferentialDriveKinematicsConstraintTest, Constraint) { const auto maxVelocity = 12_fps; const DifferentialDriveKinematics kinematics{27_in}; - std::vector> constraints; - constraints.emplace_back( - std::make_unique(kinematics, - maxVelocity)); + auto config = TrajectoryConfig(12_fps, 12_fps_sq); + config.AddConstraint( + DifferentialDriveKinematicsConstraint(kinematics, maxVelocity)); - auto trajectory = TestTrajectory::GetTrajectory(std::move(constraints)); + auto trajectory = TestTrajectory::GetTrajectory(config); units::second_t time = 0_s; units::second_t dt = 20_ms; diff --git a/wpilibc/src/test/native/cpp/trajectory/TrajectoryGeneratorTest.cpp b/wpilibc/src/test/native/cpp/trajectory/TrajectoryGeneratorTest.cpp index b687e67306..7e47e112a7 100644 --- a/wpilibc/src/test/native/cpp/trajectory/TrajectoryGeneratorTest.cpp +++ b/wpilibc/src/test/native/cpp/trajectory/TrajectoryGeneratorTest.cpp @@ -17,8 +17,8 @@ using namespace frc; TEST(TrajectoryGenerationTest, ObeysConstraints) { - auto trajectory = TestTrajectory::GetTrajectory( - std::vector>()); + TrajectoryConfig config{12_fps, 12_fps_sq}; + auto trajectory = TestTrajectory::GetTrajectory(config); units::second_t time = 0_s; units::second_t dt = 20_ms; diff --git a/wpilibc/src/test/native/include/trajectory/TestTrajectory.h b/wpilibc/src/test/native/include/trajectory/TestTrajectory.h index 21ce51e429..4a496d76bc 100644 --- a/wpilibc/src/test/native/include/trajectory/TestTrajectory.h +++ b/wpilibc/src/test/native/include/trajectory/TestTrajectory.h @@ -18,17 +18,13 @@ namespace frc { class TestTrajectory { public: - static Trajectory GetTrajectory( - std::vector>&& constraints) { - const units::meters_per_second_t startVelocity = 0_mps; - const units::meters_per_second_t endVelocity = 0_mps; - const units::meters_per_second_t maxVelocity = 12_fps; - const units::meters_per_second_squared_t maxAcceleration = 12_fps_sq; - + static Trajectory GetTrajectory(TrajectoryConfig& config) { // 2018 cross scale auto waypoints const Pose2d sideStart{1.54_ft, 23.23_ft, Rotation2d(180_deg)}; const Pose2d crossScale{23.7_ft, 6.8_ft, Rotation2d(-160_deg)}; + config.SetReversed(true); + auto vector = std::vector{ (sideStart + Transform2d{Translation2d(-13_ft, 0_ft), Rotation2d()}) .Translation(), @@ -36,9 +32,8 @@ class TestTrajectory { Transform2d{Translation2d(-19.5_ft, 5.0_ft), Rotation2d(-90_deg)}) .Translation()}; - return TrajectoryGenerator::GenerateTrajectory( - sideStart, vector, crossScale, std::move(constraints), startVelocity, - endVelocity, maxVelocity, maxAcceleration, true); + return TrajectoryGenerator::GenerateTrajectory(sideStart, vector, + crossScale, config); } }; diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/spline/Spline.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/spline/Spline.java index 448b6858a5..a0dfd19355 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/spline/Spline.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/spline/Spline.java @@ -7,6 +7,8 @@ package edu.wpi.first.wpilibj.spline; +import java.util.Arrays; + import org.ejml.simple.SimpleMatrix; import edu.wpi.first.wpilibj.geometry.Pose2d; @@ -88,4 +90,27 @@ public abstract class Spline { curvature ); } + + /** + * Represents a control vector for a spline. + * + *

Each element in each array represents the value of the derivative at the index. For + * example, the value of x[2] is the second derivative in the x dimension. + */ + @SuppressWarnings("MemberName") + public static class ControlVector { + public double[] x; + public double[] y; + + /** + * Instantiates a control vector. + * @param x The x dimension of the control vector. + * @param y The y dimension of the control vector. + */ + @SuppressWarnings("ParameterName") + public ControlVector(double[] x, double[] y) { + this.x = Arrays.copyOf(x, x.length); + this.y = Arrays.copyOf(y, y.length); + } + } } diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/spline/SplineHelper.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/spline/SplineHelper.java index a18c4900e6..1c71229390 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/spline/SplineHelper.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/spline/SplineHelper.java @@ -7,7 +7,9 @@ package edu.wpi.first.wpilibj.spline; +import java.util.ArrayList; import java.util.Arrays; +import java.util.List; import edu.wpi.first.wpilibj.geometry.Pose2d; import edu.wpi.first.wpilibj.geometry.Translation2d; @@ -20,55 +22,91 @@ public final class SplineHelper { } /** - * Returns a set of cubic splines corresponding to the provided waypoints. The + * Returns 2 cubic control vectors from a set of exterior waypoints and + * interior translations. + * + * @param start The starting pose. + * @param interiorWaypoints The interior waypoints. + * @param end The ending pose. + * @return 2 cubic control vectors. + */ + public static Spline.ControlVector[] getCubicControlVectorsFromWaypoints( + Pose2d start, Translation2d[] interiorWaypoints, Pose2d end + ) { + // Generate control vectors from poses. + Spline.ControlVector initialCV; + Spline.ControlVector endCV; + + // Chooses a magnitude automatically that makes the splines look better. + if (interiorWaypoints.length < 1) { + double scalar = start.getTranslation().getDistance(end.getTranslation()) * 1.2; + initialCV = getCubicControlVector(scalar, start); + endCV = getCubicControlVector(scalar, end); + } else { + double scalar = start.getTranslation().getDistance(interiorWaypoints[0]) * 1.2; + initialCV = getCubicControlVector(scalar, start); + scalar = end.getTranslation().getDistance(interiorWaypoints[interiorWaypoints.length - 1]) + * 1.2; + endCV = getCubicControlVector(scalar, end); + } + return new Spline.ControlVector[]{initialCV, endCV}; + } + + /** + * Returns quintic control vectors from a set of waypoints. + * + * @param waypoints The waypoints + * @return List of control vectors + */ + public static List getQuinticControlVectorsFromWaypoints( + List waypoints + ) { + List vectors = new ArrayList<>(); + 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)); + } + return vectors; + } + + /** + * Returns a set of cubic splines corresponding to the provided control vectors. The * user is free to set the direction of the start and end point. The * directions for the middle waypoints are determined automatically to ensure * continuous curvature throughout the path. * - * @param start The starting waypoint. + * @param start The starting control vector. * @param waypoints The middle waypoints. This can be left blank if you only * wish to create a path with two waypoints. - * @param end The ending waypoint. + * @param end The ending control vector. * @return A vector of cubic hermite splines that interpolate through the - * provided waypoints. + * provided waypoints and control vectors. */ @SuppressWarnings({"LocalVariableName", "PMD.ExcessiveMethodLength", "PMD.AvoidInstantiatingObjectsInLoops"}) - public static CubicHermiteSpline[] getCubicSplinesFromWaypoints( - Pose2d start, Translation2d[] waypoints, Pose2d end) { + public static CubicHermiteSpline[] getCubicSplinesFromControlVectors( + Spline.ControlVector start, Translation2d[] waypoints, Spline.ControlVector end) { CubicHermiteSpline[] splines = new CubicHermiteSpline[waypoints.length + 1]; - double scalar; - // This just makes the splines look better. - if (waypoints.length == 0) { - scalar = 1.2 * start.getTranslation().getDistance(end.getTranslation()); - } else { - scalar = 1.2 * start.getTranslation().getDistance(waypoints[0]); - } - - double[] xInitialControlVector - = {start.getTranslation().getX(), scalar * start.getRotation().getCos()}; - double[] yInitialControlVector - = {start.getTranslation().getY(), scalar * start.getRotation().getSin()}; - - // This just makes the splines look better. - if (waypoints.length != 0) { - scalar = 1.2 * end.getTranslation().getDistance(waypoints[waypoints.length - 1]); - } - - double[] xFinalControlVector - = {end.getTranslation().getX(), scalar * end.getRotation().getCos()}; - double[] yFinalControlVector - = {end.getTranslation().getY(), scalar * end.getRotation().getSin()}; + double[] xInitial = start.x; + double[] yInitial = start.y; + double[] xFinal = end.x; + double[] yFinal = end.y; if (waypoints.length > 1) { Translation2d[] newWaypts = new Translation2d[waypoints.length + 2]; // Create an array of all waypoints, including the start and end. - newWaypts[0] = start.getTranslation(); + newWaypts[0] = new Translation2d(xInitial[0], yInitial[0]); System.arraycopy(waypoints, 0, newWaypts, 1, waypoints.length); - newWaypts[newWaypts.length - 1] = end.getTranslation(); + newWaypts[newWaypts.length - 1] = new Translation2d(xFinal[0], yFinal[0]); final double[] a = new double[1 + newWaypts.length - 3]; @@ -90,8 +128,8 @@ public final class SplineHelper { } c[c.length - 1] = 0.0; - dx[0] = 3 * (newWaypts[2].getX() - newWaypts[0].getX()) - xInitialControlVector[1]; - dy[0] = 3 * (newWaypts[2].getY() - newWaypts[0].getY()) - yInitialControlVector[1]; + dx[0] = 3 * (newWaypts[2].getX() - newWaypts[0].getX()) - xInitial[1]; + dy[0] = 3 * (newWaypts[2].getY() - newWaypts[0].getY()) - yInitial[1]; if (newWaypts.length > 4) { for (int i = 1; i <= newWaypts.length; i++) { @@ -101,9 +139,9 @@ public final class SplineHelper { } dx[dx.length - 1] = 3 * (newWaypts[newWaypts.length - 1].getX() - - newWaypts[newWaypts.length - 3].getX()) - xFinalControlVector[1]; + - newWaypts[newWaypts.length - 3].getX()) - xFinal[1]; dy[dy.length - 1] = 3 * (newWaypts[newWaypts.length - 1].getY() - - newWaypts[newWaypts.length - 3].getY()) - yFinalControlVector[1]; + - newWaypts[newWaypts.length - 3].getY()) - yFinal[1]; thomasAlgorithm(a, b, c, dx, fx); thomasAlgorithm(a, b, c, dy, fy); @@ -111,12 +149,12 @@ public final class SplineHelper { double[] newFx = new double[fx.length + 2]; double[] newFy = new double[fy.length + 2]; - newFx[0] = xInitialControlVector[1]; - newFy[0] = yInitialControlVector[1]; + newFx[0] = xInitial[1]; + newFy[0] = yInitial[1]; System.arraycopy(fx, 0, newFx, 1, fx.length); System.arraycopy(fy, 0, newFy, 1, fy.length); - newFx[newFx.length - 1] = xFinalControlVector[1]; - newFy[newFy.length - 1] = yFinalControlVector[1]; + newFx[newFx.length - 1] = xFinal[1]; + newFy[newFy.length - 1] = yFinal[1]; for (int i = 0; i < newFx.length - 1; i++) { splines[i] = new CubicHermiteSpline( @@ -127,59 +165,49 @@ public final class SplineHelper { ); } } else if (waypoints.length == 1) { - final var xDeriv = (3 * (end.getTranslation().getX() - - start.getTranslation().getX()) - - xFinalControlVector[1] - xInitialControlVector[1]) + final var xDeriv = (3 * (xFinal[0] + - xInitial[0]) + - xFinal[1] - xInitial[1]) / 4.0; - final var yDeriv = (3 * (end.getTranslation().getY() - - start.getTranslation().getY()) - - yFinalControlVector[1] - yInitialControlVector[1]) + final var yDeriv = (3 * (yFinal[0] + - yInitial[0]) + - yFinal[1] - yInitial[1]) / 4.0; double[] midXControlVector = {waypoints[0].getX(), xDeriv}; double[] midYControlVector = {waypoints[0].getX(), yDeriv}; - splines[0] = new CubicHermiteSpline(xInitialControlVector, midXControlVector, - yInitialControlVector, midYControlVector); - splines[1] = new CubicHermiteSpline(midXControlVector, xFinalControlVector, - midYControlVector, yFinalControlVector); + splines[0] = new CubicHermiteSpline(xInitial, midXControlVector, + yInitial, midYControlVector); + splines[1] = new CubicHermiteSpline(midXControlVector, xFinal, + midYControlVector, yFinal); } else { - splines[0] = new CubicHermiteSpline(xInitialControlVector, xFinalControlVector, - yInitialControlVector, yFinalControlVector); + splines[0] = new CubicHermiteSpline(xInitial, xFinal, + yInitial, yFinal); } return splines; } /** - * Returns a set of quintic splines corresponding to the provided waypoints. - * The user is free to set the direction of all waypoints. Continuous + * Returns a set of quintic splines corresponding to the provided control vectors. + * The user is free to set the direction of all control vectors. Continuous * curvature is guaranteed throughout the path. * - * @param waypoints The waypoints. + * @param controlVectors The control vectors. * @return A vector of quintic hermite splines that interpolate through the * provided waypoints. */ @SuppressWarnings({"LocalVariableName", "PMD.AvoidInstantiatingObjectsInLoops"}) - public static QuinticHermiteSpline[] getQuinticSplinesFromWaypoints(Pose2d[] waypoints) { - QuinticHermiteSpline[] splines = new QuinticHermiteSpline[waypoints.length - 1]; - for (int i = 0; i < waypoints.length - 1; i++) { - var p0 = waypoints[i]; - var p1 = waypoints[i + 1]; - - // This just makes the splines look better. - final var scalar = 1.2 * p0.getTranslation().getDistance(p1.getTranslation()); - - double[] xInitialControlVector = - {p0.getTranslation().getX(), scalar * p0.getRotation().getCos(), 0.0}; - double[] xFinalControlVector = - {p1.getTranslation().getX(), scalar * p1.getRotation().getCos(), 0.0}; - double[] yInitialControlVector = - {p0.getTranslation().getY(), scalar * p0.getRotation().getSin(), 0.0}; - double[] yFinalControlVector = - {p1.getTranslation().getY(), scalar * p1.getRotation().getSin(), 0.0}; - - splines[i] = new QuinticHermiteSpline(xInitialControlVector, xFinalControlVector, - yInitialControlVector, yFinalControlVector); + public static QuinticHermiteSpline[] getQuinticSplinesFromControlVectors( + Spline.ControlVector[] controlVectors) { + 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] = new QuinticHermiteSpline(xInitial, xFinal, + yInitial, yFinal); } return splines; } @@ -218,4 +246,18 @@ public final class SplineHelper { solutionVector[i] = dStar[i] - cStar[i] * solutionVector[i + 1]; } } + + private static Spline.ControlVector getCubicControlVector(double scalar, Pose2d point) { + return new Spline.ControlVector( + new double[]{point.getTranslation().getX(), scalar * point.getRotation().getCos()}, + new double[]{point.getTranslation().getY(), scalar * point.getRotation().getSin()} + ); + } + + private static Spline.ControlVector getQuinticControlVector(double scalar, Pose2d point) { + return new Spline.ControlVector( + new double[]{point.getTranslation().getX(), scalar * point.getRotation().getCos(), 0.0}, + new double[]{point.getTranslation().getY(), scalar * point.getRotation().getSin(), 0.0} + ); + } } diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/trajectory/TrajectoryConfig.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/trajectory/TrajectoryConfig.java new file mode 100644 index 0000000000..7bff2cc3ba --- /dev/null +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/trajectory/TrajectoryConfig.java @@ -0,0 +1,165 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package edu.wpi.first.wpilibj.trajectory; + +import java.util.ArrayList; +import java.util.List; + +import edu.wpi.first.wpilibj.kinematics.DifferentialDriveKinematics; +import edu.wpi.first.wpilibj.trajectory.constraint.DifferentialDriveKinematicsConstraint; +import edu.wpi.first.wpilibj.trajectory.constraint.TrajectoryConstraint; + +/** + * Represents the configuration for generating a trajectory. This class stores the start velocity, + * end velocity, max velocity, max acceleration, custom constraints, and the reversed flag. + * + *

The class must be constructed with a max velocity and max acceleration. The other parameters + * (start velocity, end velocity, constraints, reversed) have been defaulted to reasonable + * values (0, 0, {}, false). These values can be changed via the setXXX methods. + */ +public class TrajectoryConfig { + private final double m_maxVelocity; + private final double m_maxAcceleration; + private final List m_constraints; + private double m_startVelocity; + private double m_endVelocity; + private boolean m_reversed; + + /** + * Constructs the trajectory configuration class. + * + * @param maxVelocityMetersPerSecond The max velocity for the trajectory. + * @param maxAccelerationMetersPerSecondSq The max acceleration for the trajectory. + */ + public TrajectoryConfig(double maxVelocityMetersPerSecond, + double maxAccelerationMetersPerSecondSq) { + m_maxVelocity = maxVelocityMetersPerSecond; + m_maxAcceleration = maxAccelerationMetersPerSecondSq; + m_constraints = new ArrayList<>(); + } + + /** + * Adds a user-defined constraint to the trajectory. + * + * @param constraint The user-defined constraint. + * @return Instance of the current config object. + */ + public TrajectoryConfig addConstraint(TrajectoryConstraint constraint) { + m_constraints.add(constraint); + return this; + } + + /** + * Adds all user-defined constraints from a list to the trajectory. + * @param constraints List of user-defined constraints. + * @return Instance of the current config object. + */ + public TrajectoryConfig addConstraints(List constraints) { + m_constraints.addAll(constraints); + return this; + } + + /** + * Adds a differential drive kinematics constraint to ensure that + * no wheel velocity of a differential drive goes above the max velocity. + * + * @param kinematics The differential drive kinematics. + * @return Instance of the current config object. + */ + public TrajectoryConfig setKinematics(DifferentialDriveKinematics kinematics) { + addConstraint(new DifferentialDriveKinematicsConstraint(kinematics, m_maxVelocity)); + return this; + } + + /** + * Returns the starting velocity of the trajectory. + * + * @return The starting velocity of the trajectory. + */ + public double getStartVelocity() { + return m_startVelocity; + } + + /** + * Sets the start velocity of the trajectory. + * + * @param startVelocityMetersPerSecond The start velocity of the trajectory. + * @return Instance of the current config object. + */ + public TrajectoryConfig setStartVelocity(double startVelocityMetersPerSecond) { + m_startVelocity = startVelocityMetersPerSecond; + return this; + } + + /** + * Returns the starting velocity of the trajectory. + * + * @return The starting velocity of the trajectory. + */ + public double getEndVelocity() { + return m_endVelocity; + } + + /** + * Sets the end velocity of the trajectory. + * + * @param endVelocityMetersPerSecond The end velocity of the trajectory. + * @return Instance of the current config object. + */ + public TrajectoryConfig setEndVelocity(double endVelocityMetersPerSecond) { + m_endVelocity = endVelocityMetersPerSecond; + return this; + } + + /** + * Returns the maximum velocity of the trajectory. + * + * @return The maximum velocity of the trajectory. + */ + public double getMaxVelocity() { + return m_maxVelocity; + } + + /** + * Returns the maximum acceleration of the trajectory. + * + * @return The maximum acceleration of the trajectory. + */ + public double getMaxAcceleration() { + return m_maxAcceleration; + } + + /** + * Returns the user-defined constraints of the trajectory. + * + * @return The user-defined constraints of the trajectory. + */ + public List getConstraints() { + return m_constraints; + } + + /** + * Returns whether the trajectory is reversed or not. + * + * @return whether the trajectory is reversed or not. + */ + public boolean isReversed() { + return m_reversed; + } + + /** + * Sets the reversed flag of the trajectory. + * + * @param reversed Whether the trajectory should be reversed or not. + * @return Instance of the current config object. + */ + public TrajectoryConfig setReversed(boolean reversed) { + m_reversed = reversed; + return this; + } +} 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 a93e4683a1..aa3840a1e0 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 @@ -14,13 +14,10 @@ import edu.wpi.first.wpilibj.geometry.Pose2d; import edu.wpi.first.wpilibj.geometry.Rotation2d; import edu.wpi.first.wpilibj.geometry.Transform2d; import edu.wpi.first.wpilibj.geometry.Translation2d; -import edu.wpi.first.wpilibj.kinematics.DifferentialDriveKinematics; 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.trajectory.constraint.DifferentialDriveKinematicsConstraint; -import edu.wpi.first.wpilibj.trajectory.constraint.TrajectoryConstraint; public final class TrajectoryGenerator { /** @@ -30,309 +27,142 @@ public final class TrajectoryGenerator { } /** - * Generates a trajectory with the given waypoints and constraints. + * Generates a trajectory from the given control vectors and config. This method uses clamped + * cubic splines -- a method in which the exterior control vectors and interior waypoints + * are provided. The headings are automatically determined at the interior points to + * ensure continuous curvature. * - * @param waypoints A vector of points that the trajectory must go through. - * @param constraints A vector of various velocity and acceleration - * constraints. - * @param startVelocityMetersPerSecond The start velocity for the trajectory. - * @param endVelocityMetersPerSecond The end velocity for the trajectory. - * @param maxVelocityMetersPerSecond The max velocity for the trajectory. - * @param maxAccelerationMetersPerSecondSq The max acceleration for the trajectory. - * @param reversed Whether the robot should move backwards. Note that the - * robot will still move from a -> b -> ... -> z - * as defined in the waypoints. - * @return The trajectory. + * @param initial The initial control vector. + * @param interiorWaypoints The interior waypoints. + * @param end The ending control vector. + * @param config The configuration for the trajectory. + * @return The generated trajectory. */ - public static Trajectory generateTrajectory( - List waypoints, - List constraints, - double startVelocityMetersPerSecond, - double endVelocityMetersPerSecond, - double maxVelocityMetersPerSecond, - double maxAccelerationMetersPerSecondSq, - boolean reversed + Spline.ControlVector initial, + List interiorWaypoints, + Spline.ControlVector end, + TrajectoryConfig config ) { - final var flip = new Transform2d(new Translation2d(), Rotation2d.fromDegrees(-180.0)); + final var flip = new Transform2d(new Translation2d(), Rotation2d.fromDegrees(180.0)); - // Make theta normal for trajectory generation if path is reversed. - final var newWaypoints = new ArrayList(waypoints.size()); - for (final var point : waypoints) { - newWaypoints.add(reversed ? point.plus(flip) : point); + // Clone the control vectors. + var newInitial = new Spline.ControlVector(initial.x, initial.y); + var newEnd = new Spline.ControlVector(end.x, end.y); + + // Change the orientation if reversed. + if (config.isReversed()) { + newInitial.x[1] *= -1; + newInitial.y[1] *= -1; + newEnd.x[1] *= -1; + newEnd.y[1] *= -1; } - var points = splinePointsFromSplines(SplineHelper.getQuinticSplinesFromWaypoints( - newWaypoints.toArray(new Pose2d[0]) + // Get the spline points + var points = splinePointsFromSplines(SplineHelper.getCubicSplinesFromControlVectors( + newInitial, interiorWaypoints.toArray(new Translation2d[0]), newEnd )); - // After trajectory generation, flip theta back so it's relative to the - // field. Also fix curvature. - if (reversed) { + // Change the points back to their original orientation. + if (config.isReversed()) { for (var point : points) { point.poseMeters = point.poseMeters.plus(flip); point.curvatureRadPerMeter *= -1; } } - return TrajectoryParameterizer.timeParameterizeTrajectory(points, constraints, - startVelocityMetersPerSecond, endVelocityMetersPerSecond, maxVelocityMetersPerSecond, - maxAccelerationMetersPerSecondSq, reversed); + // Generate and return trajectory. + return TrajectoryParameterizer.timeParameterizeTrajectory(points, config.getConstraints(), + config.getStartVelocity(), config.getEndVelocity(), config.getMaxVelocity(), + config.getMaxAcceleration(), config.isReversed()); } /** - * Generates a trajectory with the given waypoints and constraints. + * Generates a trajectory from the given waypoints and config. This method uses clamped + * cubic splines -- a method in which the initial pose, final pose, and interior waypoints + * are provided. The headings are automatically determined at the interior points to + * ensure continuous curvature. * - * @param waypoints A vector of points that the trajectory must go through. - * @param constraints A vector of various velocity and acceleration - * constraints. - * @param startVelocityMetersPerSecond The start velocity for the trajectory. - * @param endVelocityMetersPerSecond The end velocity for the trajectory. - * @param maxVelocityMetersPerSecond The max velocity for the trajectory. - * @param maxAccelerationMetersPerSecondSq The max acceleration for the trajectory. - * @return The trajectory. + * @param start The starting pose. + * @param interiorWaypoints The interior waypoints. + * @param end The ending pose. + * @param config The configuration for the trajectory. + * @return The generated trajectory. */ public static Trajectory generateTrajectory( - List waypoints, - List constraints, - double startVelocityMetersPerSecond, - double endVelocityMetersPerSecond, - double maxVelocityMetersPerSecond, - double maxAccelerationMetersPerSecondSq + Pose2d start, List interiorWaypoints, Pose2d end, + TrajectoryConfig config ) { - return generateTrajectory(waypoints, constraints, startVelocityMetersPerSecond, - endVelocityMetersPerSecond, maxVelocityMetersPerSecond, maxAccelerationMetersPerSecondSq, - false); + var controlVectors = SplineHelper.getCubicControlVectorsFromWaypoints( + start, interiorWaypoints.toArray(new Translation2d[0]), end + ); + + // Return the generated trajectory. + return generateTrajectory(controlVectors[0], interiorWaypoints, controlVectors[1], config); } /** - * Generates a trajectory with the given waypoints and constraints. + * Generates a trajectory from the given quintic control vectors and config. This method + * uses quintic hermite splines -- therefore, all points must be represented by control + * vectors. Continuous curvature is guaranteed in this method. * - * @param start The starting pose for the trajectory. - * @param waypoints The interior waypoints for the trajectory. The headings - * will be determined automatically to ensure continuous - * curvature. - * @param end The ending pose for the trajectory. - * @param constraints A vector of various velocity and acceleration - * constraints. - * @param startVelocityMetersPerSecond The start velocity for the trajectory. - * @param endVelocityMetersPerSecond The end velocity for the trajectory. - * @param maxVelocityMetersPerSecond The max velocity for the trajectory. - * @param maxAccelerationMetersPerSecondSq The max acceleration for the trajectory. - * @param reversed Whether the robot should move backwards. Note that the - * robot will still move from a -> b -> ... -> z - * as defined in the waypoints. - * @return The trajectory. + * @param controlVectors List of quintic control vectors. + * @param config The configuration for the trajectory. + * @return The generated trajectory. */ + @SuppressWarnings("PMD.AvoidInstantiatingObjectsInLoops") public static Trajectory generateTrajectory( - Pose2d start, - List waypoints, - Pose2d end, - List constraints, - double startVelocityMetersPerSecond, - double endVelocityMetersPerSecond, - double maxVelocityMetersPerSecond, - double maxAccelerationMetersPerSecondSq, - boolean reversed + ControlVectorList controlVectors, + TrajectoryConfig config ) { - final var flip = new Transform2d(new Translation2d(), Rotation2d.fromDegrees(-180.0)); + final var flip = new Transform2d(new Translation2d(), Rotation2d.fromDegrees(180.0)); + final var newControlVectors = new ArrayList(controlVectors.size()); - final var newStart = reversed ? start.plus(flip) : start; - final var newEnd = reversed ? end.plus(flip) : end; + // Create a new control vector list, flipping the orientation if reversed. + for (final var vector : controlVectors) { + var newVector = new Spline.ControlVector(vector.x, vector.y); + if (config.isReversed()) { + newVector.x[1] *= -1; + newVector.y[1] *= -1; + } + newControlVectors.add(newVector); + } - var points = splinePointsFromSplines(SplineHelper.getCubicSplinesFromWaypoints( - newStart, waypoints.toArray(new Translation2d[0]), newEnd + // Get the spline points + var points = splinePointsFromSplines(SplineHelper.getQuinticSplinesFromControlVectors( + newControlVectors.toArray(new Spline.ControlVector[]{}) )); - // After trajectory generation, flip theta back so it's relative to the - // field. Also fix curvature. - if (reversed) { + // Change the points back to their original orientation. + if (config.isReversed()) { for (var point : points) { point.poseMeters = point.poseMeters.plus(flip); point.curvatureRadPerMeter *= -1; } } - return TrajectoryParameterizer.timeParameterizeTrajectory(points, constraints, - startVelocityMetersPerSecond, endVelocityMetersPerSecond, maxVelocityMetersPerSecond, - maxAccelerationMetersPerSecondSq, reversed); + // Generate and return trajectory. + return TrajectoryParameterizer.timeParameterizeTrajectory(points, config.getConstraints(), + config.getStartVelocity(), config.getEndVelocity(), config.getMaxVelocity(), + config.getMaxAcceleration(), config.isReversed()); + } /** - * Generates a trajectory with the given waypoints and constraints. + * Generates a trajectory from the given waypoints and config. This method + * uses quintic hermite splines -- therefore, all points must be represented by Pose2d + * objects. Continuous curvature is guaranteed in this method. * - * @param start The starting pose for the trajectory. - * @param waypoints The interior waypoints for the trajectory. The headings - * will be determined automatically to ensure continuous - * curvature. - * @param end The ending pose for the trajectory. - * @param constraints A vector of various velocity and acceleration - * constraints. - * @param startVelocityMetersPerSecond The start velocity for the trajectory. - * @param endVelocityMetersPerSecond The end velocity for the trajectory. - * @param maxVelocityMetersPerSecond The max velocity for the trajectory. - * @param maxAccelerationMetersPerSecondSq The max acceleration for the trajectory. - * @return The trajectory. + * @param waypoints List of waypoints.. + * @param config The configuration for the trajectory. + * @return The generated trajectory. */ - public static Trajectory generateTrajectory( - Pose2d start, - List waypoints, - Pose2d end, - List constraints, - double startVelocityMetersPerSecond, - double endVelocityMetersPerSecond, - double maxVelocityMetersPerSecond, - double maxAccelerationMetersPerSecondSq - ) { - return generateTrajectory(start, waypoints, end, constraints, - startVelocityMetersPerSecond, endVelocityMetersPerSecond, maxVelocityMetersPerSecond, - maxAccelerationMetersPerSecondSq, false); - } - - /** - * Generates a trajectory with the given waypoints and differential drive constraints. Use - * this method if you just want a constraint such that none of the wheels on your differential - * drive exceed the specified max velocity. If you desire to impose more constraints, please - * use the other overloads. - * - * @param waypoints A vector of points that the trajectory must go through. - * @param differentialDriveKinematics The DifferentialDriveKinematics object that represents - * your drivetrain. - * @param startVelocityMetersPerSecond The start velocity for the trajectory. - * @param endVelocityMetersPerSecond The end velocity for the trajectory. - * @param maxVelocityMetersPerSecond The max velocity for the trajectory. - * @param maxAccelerationMetersPerSecondSq The max acceleration for the trajectory. - * @param reversed Whether the robot should move backwards. Note that the - * robot will still move from a -> b -> ... -> z - * as defined in the waypoints. - * @return The trajectory. - */ - public static Trajectory generateTrajectory( - List waypoints, - DifferentialDriveKinematics differentialDriveKinematics, - double startVelocityMetersPerSecond, - double endVelocityMetersPerSecond, - double maxVelocityMetersPerSecond, - double maxAccelerationMetersPerSecondSq, - boolean reversed - ) { - return generateTrajectory( - waypoints, - List.of(new DifferentialDriveKinematicsConstraint(differentialDriveKinematics, - maxVelocityMetersPerSecond)), - startVelocityMetersPerSecond, - endVelocityMetersPerSecond, - maxVelocityMetersPerSecond, - maxAccelerationMetersPerSecondSq, - reversed - ); - } - - /** - * Generates a trajectory with the given waypoints and differential drive constraints. Use - * this method if you just want a constraint such that none of the wheels on your differential - * drive exceed the specified max velocity. If you desire to impose more constraints, please - * use the other overloads. - * - * @param waypoints A vector of points that the trajectory must go through. - * @param differentialDriveKinematics The DifferentialDriveKinematics object that represents - * your drivetrain. - * @param startVelocityMetersPerSecond The start velocity for the trajectory. - * @param endVelocityMetersPerSecond The end velocity for the trajectory. - * @param maxVelocityMetersPerSecond The max velocity for the trajectory. - * @param maxAccelerationMetersPerSecondSq The max acceleration for the trajectory. - * @return The trajectory. - */ - public static Trajectory generateTrajectory( - List waypoints, - DifferentialDriveKinematics differentialDriveKinematics, - double startVelocityMetersPerSecond, - double endVelocityMetersPerSecond, - double maxVelocityMetersPerSecond, - double maxAccelerationMetersPerSecondSq - ) { - return generateTrajectory(waypoints, differentialDriveKinematics, startVelocityMetersPerSecond, - endVelocityMetersPerSecond, maxVelocityMetersPerSecond, - maxAccelerationMetersPerSecondSq, false); - } - - /** - * Generates a trajectory with the given waypoints and differential drive constraints. Use - * this method if you just want a constraint such that none of the wheels on your differential - * drive exceed the specified max velocity. If you desire to impose more constraints, please - * use the other overloads. - * - * @param start The starting pose for the trajectory. - * @param waypoints The interior waypoints for the trajectory. The headings - * will be determined automatically to ensure continuous - * curvature. - * @param end The ending pose for the trajectory. - * @param differentialDriveKinematics The DifferentialDriveKinematics object that represents - * your drivetrain. - * @param startVelocityMetersPerSecond The start velocity for the trajectory. - * @param endVelocityMetersPerSecond The end velocity for the trajectory. - * @param maxVelocityMetersPerSecond The max velocity for the trajectory. - * @param maxAccelerationMetersPerSecondSq The max acceleration for the trajectory. - * @param reversed Whether the robot should move backwards. Note that the - * robot will still move from a -> b -> ... -> z - * as defined in the waypoints. - * @return The trajectory. - */ - public static Trajectory generateTrajectory( - Pose2d start, - List waypoints, - Pose2d end, - DifferentialDriveKinematics differentialDriveKinematics, - double startVelocityMetersPerSecond, - double endVelocityMetersPerSecond, - double maxVelocityMetersPerSecond, - double maxAccelerationMetersPerSecondSq, - boolean reversed - ) { - return generateTrajectory( - start, waypoints, end, - List.of(new DifferentialDriveKinematicsConstraint(differentialDriveKinematics, - maxVelocityMetersPerSecond)), - startVelocityMetersPerSecond, - endVelocityMetersPerSecond, - maxVelocityMetersPerSecond, - maxAccelerationMetersPerSecondSq, - reversed - ); - } - - /** - * Generates a trajectory with the given waypoints and differential drive constraints. Use - * this method if you just want a constraint such that none of the wheels on your differential - * drive exceed the specified max velocity. If you desire to impose more constraints, please - * use the other overloads. - * - * @param start The starting pose for the trajectory. - * @param waypoints The interior waypoints for the trajectory. The headings - * will be determined automatically to ensure continuous - * curvature. - * @param end The ending pose for the trajectory. - * @param differentialDriveKinematics The DifferentialDriveKinematics object that represents - * your drivetrain. - * @param startVelocityMetersPerSecond The start velocity for the trajectory. - * @param endVelocityMetersPerSecond The end velocity for the trajectory. - * @param maxVelocityMetersPerSecond The max velocity for the trajectory. - * @param maxAccelerationMetersPerSecondSq The max acceleration for the trajectory. - * @return The trajectory. - */ - public static Trajectory generateTrajectory( - Pose2d start, - List waypoints, - Pose2d end, - DifferentialDriveKinematics differentialDriveKinematics, - double startVelocityMetersPerSecond, - double endVelocityMetersPerSecond, - double maxVelocityMetersPerSecond, - double maxAccelerationMetersPerSecondSq - ) { - return generateTrajectory(start, waypoints, end, differentialDriveKinematics, - startVelocityMetersPerSecond, endVelocityMetersPerSecond, - maxVelocityMetersPerSecond, maxAccelerationMetersPerSecondSq, false); + @SuppressWarnings("LocalVariableName") + public static Trajectory generateTrajectory(List waypoints, TrajectoryConfig config) { + var originalList = SplineHelper.getQuinticControlVectorsFromWaypoints(waypoints); + var newList = new ControlVectorList(); + newList.addAll(originalList); + return generateTrajectory(newList, config); } /** @@ -340,7 +170,6 @@ public final class TrajectoryGenerator { * splines. * * @param splines The splines to parameterize. - * * @return The spline points for use in time parameterization of a trajectory. */ public static List splinePointsFromSplines( @@ -363,4 +192,8 @@ public final class TrajectoryGenerator { } return splinePoints; } + + // Work around type erasure signatures + private static class ControlVectorList extends ArrayList { + } } diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/controller/RamseteControllerTest.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj/controller/RamseteControllerTest.java index b8447299df..2bef50d8d8 100644 --- a/wpilibj/src/test/java/edu/wpi/first/wpilibj/controller/RamseteControllerTest.java +++ b/wpilibj/src/test/java/edu/wpi/first/wpilibj/controller/RamseteControllerTest.java @@ -14,8 +14,8 @@ 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.Twist2d; +import edu.wpi.first.wpilibj.trajectory.TrajectoryConfig; import edu.wpi.first.wpilibj.trajectory.TrajectoryGenerator; -import edu.wpi.first.wpilibj.trajectory.constraint.TrajectoryConstraint; import static org.junit.jupiter.api.Assertions.assertAll; import static org.junit.jupiter.api.Assertions.assertEquals; @@ -24,6 +24,16 @@ class RamseteControllerTest { private static final double kTolerance = 1 / 12.0; private static final double kAngularTolerance = Math.toRadians(2); + private static double boundRadians(double value) { + while (value > Math.PI) { + value -= Math.PI * 2; + } + while (value <= -Math.PI) { + value += Math.PI * 2; + } + return value; + } + @Test @SuppressWarnings("PMD.AvoidInstantiatingObjectsInLoops") void testReachesReference() { @@ -33,8 +43,8 @@ class RamseteControllerTest { final var waypoints = new ArrayList(); waypoints.add(new Pose2d(2.75, 22.521, new Rotation2d(0))); waypoints.add(new Pose2d(24.73, 19.68, new Rotation2d(5.846))); - final var trajectory = TrajectoryGenerator.generateTrajectory(waypoints, - new ArrayList(), 0, 0, 8.8, 0.1, false); + var config = new TrajectoryConfig(8.8, 0.1); + final var trajectory = TrajectoryGenerator.generateTrajectory(waypoints, config); final double kDt = 0.02; final var totalTime = trajectory.getTotalTimeSeconds(); @@ -54,23 +64,13 @@ class RamseteControllerTest { final var finalRobotPose = robotPose; assertAll( () -> assertEquals(endPose.getTranslation().getX(), finalRobotPose.getTranslation().getX(), - kTolerance), + kTolerance), () -> assertEquals(endPose.getTranslation().getY(), finalRobotPose.getTranslation().getY(), - kTolerance), + kTolerance), () -> assertEquals(0.0, boundRadians(endPose.getRotation().getRadians() - - finalRobotPose.getRotation().getRadians()), + - finalRobotPose.getRotation().getRadians()), kAngularTolerance) ); } - - private static double boundRadians(double value) { - while (value > Math.PI) { - value -= Math.PI * 2; - } - while (value <= -Math.PI) { - value += Math.PI * 2; - } - return value; - } } 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 8d7f601224..d7d4558ae7 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 @@ -32,8 +32,13 @@ class CubicHermiteSplineTest { var start = System.nanoTime(); // Generate and parameterize the spline. + var controlVectors = + SplineHelper.getCubicControlVectorsFromWaypoints(a, + waypoints.toArray(new Translation2d[0]), b); var splines - = SplineHelper.getCubicSplinesFromWaypoints(a, waypoints.toArray(new Translation2d[0]), b); + = SplineHelper.getCubicSplinesFromControlVectors( + controlVectors[0], waypoints.toArray(new Translation2d[0]), controlVectors[1]); + var poses = new ArrayList(); poses.add(splines[0].getPoint(0.0)); 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 8d942132da..5eb84374bb 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 @@ -7,6 +7,8 @@ package edu.wpi.first.wpilibj.spline; +import java.util.List; + import org.junit.jupiter.api.Test; import edu.wpi.first.wpilibj.geometry.Pose2d; @@ -27,7 +29,9 @@ class QuinticHermiteSplineTest { var start = System.nanoTime(); // Generate and parameterize the spline. - var spline = SplineHelper.getQuinticSplinesFromWaypoints(new Pose2d[]{a, b})[0]; + var controlVectors = SplineHelper.getQuinticControlVectorsFromWaypoints(List.of(a, b)); + var spline = SplineHelper.getQuinticSplinesFromControlVectors( + controlVectors.toArray(new Spline.ControlVector[0]))[0]; var poses = SplineParameterizer.parameterize(spline); // End the timer. 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 2d4242520a..19b93740e9 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 @@ -24,8 +24,6 @@ import static org.junit.jupiter.api.Assertions.assertTrue; class TrajectoryGeneratorTest { static Trajectory getTrajectory(List constraints) { - final double startVelocity = 0; - final double endVelocity = 0; final double maxVelocity = feetToMeters(12.0); final double maxAccel = feetToMeters(12); @@ -45,15 +43,11 @@ class TrajectoryGeneratorTest { Rotation2d.fromDegrees(-90)))); waypoints.add(crossScale); - return TrajectoryGenerator.generateTrajectory( - waypoints, - constraints, - startVelocity, - endVelocity, - maxVelocity, - maxAccel, - true - ); + TrajectoryConfig config = new TrajectoryConfig(maxVelocity, maxAccel) + .setReversed(true) + .addConstraints(constraints); + + return TrajectoryGenerator.generateTrajectory(waypoints, config); } @Test