diff --git a/ThirdPartyNotices.txt b/ThirdPartyNotices.txt index a4c04df2f2..fae1250016 100644 --- a/ThirdPartyNotices.txt +++ b/ThirdPartyNotices.txt @@ -39,6 +39,11 @@ popper.js wpiutil/src/main/native/resources/popper-* units wpiutil/src/main/native/include/units/units.h Eigen wpiutil/src/main/native/eigeninclude/ StackWalker wpiutil/src/main/native/windows/StackWalker.* +Team 254 Library wpilibj/src/main/java/edu/wpi/first/wpilibj/spline/SplineParameterizer.java + wpilibj/src/main/java/edu/wpi/first/wpilibj/trajectory/TrajectoryParameterizer.java + wpilibc/src/main/native/include/spline/SplineParameterizer.h + wpilibc/src/main/native/include/trajectory/TrajectoryParameterizer.h + wpilibc/src/main/native/cpp/trajectory/TrajectoryParameterizer.cpp ============================================================================== @@ -775,3 +780,29 @@ SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + + +================ +Team 254 Library +================ +MIT License + +Copyright (c) 2018 Team 254 + +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: + +The above copyright notice and this permission notice shall be included in all +copies or substantial portions of the Software. + +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +SOFTWARE. diff --git a/wpilibc/src/main/native/cpp/geometry/Pose2d.cpp b/wpilibc/src/main/native/cpp/geometry/Pose2d.cpp index 6b4c2f0547..42d20f709e 100644 --- a/wpilibc/src/main/native/cpp/geometry/Pose2d.cpp +++ b/wpilibc/src/main/native/cpp/geometry/Pose2d.cpp @@ -27,6 +27,11 @@ Pose2d& Pose2d::operator+=(const Transform2d& other) { return *this; } +Transform2d Pose2d::operator-(const Pose2d& other) const { + const auto pose = this->RelativeTo(other); + return Transform2d(pose.Translation(), pose.Rotation()); +} + bool Pose2d::operator==(const Pose2d& other) const { return m_translation == other.m_translation && m_rotation == other.m_rotation; } @@ -67,3 +72,27 @@ Pose2d Pose2d::Exp(const Twist2d& twist) const { return *this + transform; } + +Twist2d Pose2d::Log(const Pose2d& end) const { + const auto transform = end.RelativeTo(*this); + const auto dtheta = transform.Rotation().Radians().to(); + const auto halfDtheta = dtheta / 2.0; + + const auto cosMinusOne = transform.Rotation().Cos() - 1; + + double halfThetaByTanOfHalfDtheta; + + if (std::abs(cosMinusOne) < 1E-9) { + halfThetaByTanOfHalfDtheta = 1.0 - 1.0 / 12.0 * dtheta * dtheta; + } else { + halfThetaByTanOfHalfDtheta = + -(halfDtheta * transform.Rotation().Sin()) / cosMinusOne; + } + + const Translation2d translationPart = + transform.Translation().RotateBy( + {halfThetaByTanOfHalfDtheta, -halfDtheta}) * + std::hypot(halfThetaByTanOfHalfDtheta, halfDtheta); + + return {translationPart.X(), translationPart.Y(), units::radian_t(dtheta)}; +} diff --git a/wpilibc/src/main/native/cpp/geometry/Rotation2d.cpp b/wpilibc/src/main/native/cpp/geometry/Rotation2d.cpp index 7aeb8a91c5..2723442480 100644 --- a/wpilibc/src/main/native/cpp/geometry/Rotation2d.cpp +++ b/wpilibc/src/main/native/cpp/geometry/Rotation2d.cpp @@ -52,6 +52,10 @@ Rotation2d& Rotation2d::operator-=(const Rotation2d& other) { Rotation2d Rotation2d::operator-() const { return Rotation2d(-m_value); } +Rotation2d Rotation2d::operator*(double scalar) const { + return Rotation2d(m_value * scalar); +} + bool Rotation2d::operator==(const Rotation2d& other) const { return units::math::abs(m_value - other.m_value) < 1E-9_rad; } diff --git a/wpilibc/src/main/native/cpp/spline/CubicHermiteSpline.cpp b/wpilibc/src/main/native/cpp/spline/CubicHermiteSpline.cpp new file mode 100644 index 0000000000..3578b1daec --- /dev/null +++ b/wpilibc/src/main/native/cpp/spline/CubicHermiteSpline.cpp @@ -0,0 +1,36 @@ +/*----------------------------------------------------------------------------*/ +/* 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. */ +/*----------------------------------------------------------------------------*/ + +#include "frc/spline/CubicHermiteSpline.h" + +using namespace frc; + +CubicHermiteSpline::CubicHermiteSpline( + std::array xInitialControlVector, + std::array xFinalControlVector, + std::array yInitialControlVector, + std::array yFinalControlVector) { + const auto hermite = MakeHermiteBasis(); + const auto x = + ControlVectorFromArrays(xInitialControlVector, xFinalControlVector); + const auto y = + ControlVectorFromArrays(yInitialControlVector, yFinalControlVector); + + // Populate first two rows with coefficients. + m_coefficients.template block<1, 4>(0, 0) = hermite * x; + m_coefficients.template block<1, 4>(1, 0) = hermite * y; + + // Populate Row 2 and Row 3 with the derivatives of the equations above. + // Then populate row 4 and 5 with the second derivatives. + for (int i = 0; i < 4; i++) { + m_coefficients.template block<2, 1>(2, i) = + m_coefficients.template block<2, 1>(0, i) * (3 - i); + + m_coefficients.template block<2, 1>(4, i) = + m_coefficients.template block<2, 1>(2, i) * (3 - i); + } +} diff --git a/wpilibc/src/main/native/cpp/spline/QuinticHermiteSpline.cpp b/wpilibc/src/main/native/cpp/spline/QuinticHermiteSpline.cpp new file mode 100644 index 0000000000..f714c6f617 --- /dev/null +++ b/wpilibc/src/main/native/cpp/spline/QuinticHermiteSpline.cpp @@ -0,0 +1,36 @@ +/*----------------------------------------------------------------------------*/ +/* 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. */ +/*----------------------------------------------------------------------------*/ + +#include "frc/spline/QuinticHermiteSpline.h" + +using namespace frc; + +QuinticHermiteSpline::QuinticHermiteSpline( + std::array xInitialControlVector, + std::array xFinalControlVector, + std::array yInitialControlVector, + std::array yFinalControlVector) { + const auto hermite = MakeHermiteBasis(); + const auto x = + ControlVectorFromArrays(xInitialControlVector, xFinalControlVector); + const auto y = + ControlVectorFromArrays(yInitialControlVector, yFinalControlVector); + + // Populate first two rows with coefficients. + m_coefficients.template block<1, 6>(0, 0) = (hermite * x).transpose(); + m_coefficients.template block<1, 6>(1, 0) = (hermite * y).transpose(); + + // Populate Row 2 and Row 3 with the derivatives of the equations above. + // Then populate row 4 and 5 with the second derivatives. + for (int i = 0; i < 6; i++) { + m_coefficients.template block<2, 1>(2, i) = + m_coefficients.template block<2, 1>(0, i) * (5 - i); + + m_coefficients.template block<2, 1>(4, i) = + m_coefficients.template block<2, 1>(2, i) * (5 - i); + } +} diff --git a/wpilibc/src/main/native/cpp/spline/SplineHelper.cpp b/wpilibc/src/main/native/cpp/spline/SplineHelper.cpp new file mode 100644 index 0000000000..b3eda49fcb --- /dev/null +++ b/wpilibc/src/main/native/cpp/spline/SplineHelper.cpp @@ -0,0 +1,190 @@ +/*----------------------------------------------------------------------------*/ +/* 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. */ +/*----------------------------------------------------------------------------*/ + +#include "frc/spline/SplineHelper.h" + +#include + +using namespace frc; + +std::vector SplineHelper::CubicSplinesFromWaypoints( + const Pose2d& start, std::vector waypoints, + const Pose2d& 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}; + + if (waypoints.size() > 1) { + waypoints.emplace(waypoints.begin(), start.Translation()); + waypoints.emplace_back(end.Translation()); + + std::vector a; + std::vector b(waypoints.size() - 2, 4.0); + std::vector c; + std::vector dx, dy; + std::vector fx(waypoints.size() - 2, 0.0), + fy(waypoints.size() - 2, 0.0); + + a.emplace_back(0); + for (unsigned int i = 0; i < waypoints.size() - 3; i++) { + a.emplace_back(1); + c.emplace_back(1); + } + c.emplace_back(0); + + dx.emplace_back( + 3 * (waypoints[2].X().to() - waypoints[0].X().to()) - + startXControlVector[1]); + dy.emplace_back( + 3 * (waypoints[2].Y().to() - waypoints[0].Y().to()) - + startYControlVector[1]); + if (waypoints.size() > 4) { + for (unsigned int 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() - + waypoints[i - 1].Y().to())); + } + } + dx.emplace_back(3 * (waypoints[waypoints.size() - 1].X().to() - + waypoints[waypoints.size() - 3].X().to()) - + endXControlVector[1]); + dy.emplace_back(3 * (waypoints[waypoints.size() - 1].Y().to() - + waypoints[waypoints.size() - 3].Y().to()) - + endYControlVector[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]); + + for (unsigned int i = 0; i < fx.size() - 1; i++) { + // Create the spline. + const CubicHermiteSpline spline{ + {waypoints[i].X().to(), fx[i]}, + {waypoints[i + 1].X().to(), fx[i + 1]}, + {waypoints[i].Y().to(), fy[i]}, + {waypoints[i + 1].Y().to(), fy[i + 1]}}; + + 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; + + 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); + + } else { + // Create the spline. + const CubicHermiteSpline spline{startXControlVector, endXControlVector, + startYControlVector, endYControlVector}; + splines.push_back(spline); + } + + return splines; +} + +std::vector SplineHelper::QuinticSplinesFromWaypoints( + const std::vector& waypoints) { + std::vector splines; + for (unsigned int i = 0; i < waypoints.size() - 1; i++) { + auto& p0 = waypoints[i]; + auto& p1 = waypoints[i + 1]; + + // This just makes the splines look better. + 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); + } + return splines; +} + +void SplineHelper::ThomasAlgorithm(const std::vector& a, + const std::vector& b, + const std::vector& c, + const std::vector& d, + std::vector* solutionVector) { + auto& f = *solutionVector; + size_t N = d.size(); + + // Create the temporary vectors + // Note that this is inefficient as it is possible to call + // this function many times. A better implementation would + // pass these temporary matrices by non-const reference to + // save excess allocation and deallocation + std::vector c_star(N, 0.0); + std::vector d_star(N, 0.0); + + // This updates the coefficients in the first row + // Note that we should be checking for division by zero here + c_star[0] = c[0] / b[0]; + 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++) { + 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; + } + + f[N - 1] = d_star[N - 1]; + // This is the reverse sweep, used to update the solution vector f + for (int i = N - 2; i >= 0; i--) { + f[i] = d_star[i] - c_star[i] * f[i + 1]; + } +} diff --git a/wpilibc/src/main/native/cpp/spline/SplineParameterizer.cpp b/wpilibc/src/main/native/cpp/spline/SplineParameterizer.cpp new file mode 100644 index 0000000000..f3c42c6d22 --- /dev/null +++ b/wpilibc/src/main/native/cpp/spline/SplineParameterizer.cpp @@ -0,0 +1,12 @@ +/*----------------------------------------------------------------------------*/ +/* 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. */ +/*----------------------------------------------------------------------------*/ + +#include "frc/spline/SplineParameterizer.h" + +constexpr units::meter_t frc::SplineParameterizer::kMaxDx; +constexpr units::meter_t frc::SplineParameterizer::kMaxDy; +constexpr units::radian_t frc::SplineParameterizer::kMaxDtheta; diff --git a/wpilibc/src/main/native/cpp/trajectory/Trajectory.cpp b/wpilibc/src/main/native/cpp/trajectory/Trajectory.cpp new file mode 100644 index 0000000000..9b4b4f5611 --- /dev/null +++ b/wpilibc/src/main/native/cpp/trajectory/Trajectory.cpp @@ -0,0 +1,96 @@ +/*----------------------------------------------------------------------------*/ +/* 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. */ +/*----------------------------------------------------------------------------*/ + +#include "frc/trajectory/Trajectory.h" + +using namespace frc; + +Trajectory::State Trajectory::State::Interpolate(State endValue, + double i) const { + // Find the new [t] value. + const auto newT = Lerp(t, endValue.t, i); + + // Find the delta time between the current state and the interpolated state. + const auto deltaT = newT - t; + + // If delta time is negative, flip the order of interpolation. + if (deltaT < 0_s) return endValue.Interpolate(*this, 1.0 - i); + + // Check whether the robot is reversing at this stage. + const auto reversing = + velocity < 0_mps || + (units::math::abs(velocity) < 1E-9_mps && acceleration < 0_mps_sq); + + // Calculate the new velocity. + // v = v_0 + at + const units::meters_per_second_t newV = velocity + (acceleration * deltaT); + + // Calculate the change in position. + // delta_s = v_0 t + 0.5 at^2 + const units::meter_t newS = + (velocity * deltaT + 0.5 * acceleration * deltaT * deltaT) * + (reversing ? -1.0 : 1.0); + + // Return the new state. To find the new position for the new state, we need + // to interpolate between the two endpoint poses. The fraction for + // interpolation is the change in position (delta s) divided by the total + // distance between the two endpoints. + const double interpolationFrac = + newS / endValue.pose.Translation().Distance(pose.Translation()); + + return {newT, newV, acceleration, + Lerp(pose, endValue.pose, interpolationFrac), + Lerp(curvature, endValue.curvature, interpolationFrac)}; +} + +Trajectory::Trajectory(const std::vector& states) : m_states(states) { + m_totalTime = states.back().t; +} + +Trajectory::State Trajectory::Sample(units::second_t t) const { + if (t <= m_states.front().t) return m_states.front(); + if (t >= m_totalTime) return m_states.back(); + + // To get the element that we want, we will use a binary search algorithm + // instead of iterating over a for-loop. A binary search is O(std::log(n)) + // whereas searching using a loop is O(n). + + // This starts at 1 because we use the previous state later on for + // interpolation. + int low = 1; + int high = m_states.size() - 1; + + while (low != high) { + int mid = (low + high) / 2; + if (m_states[mid].t < t) { + // This index and everything under it are less than the requested + // timestamp. Therefore, we can discard them. + low = mid + 1; + } else { + // t is at least as large as the element at this index. This means that + // anything after it cannot be what we are looking for. + high = mid; + } + } + + // High and Low should be the same. + + // The sample's timestamp is now greater than or equal to the requested + // timestamp. If it is greater, we need to interpolate between the + // previous state and the current state to get the exact state that we + // want. + const auto sample = m_states[low]; + const auto prevSample = m_states[low - 1]; + + // If the difference in states is negligible, then we are spot on! + if (units::math::abs(sample.t - prevSample.t) < 1E-9_s) { + return sample; + } + // Interpolate between the two states for the state that we want. + return prevSample.Interpolate(sample, + (t - prevSample.t) / (sample.t - prevSample.t)); +} diff --git a/wpilibc/src/main/native/cpp/trajectory/TrajectoryGenerator.cpp b/wpilibc/src/main/native/cpp/trajectory/TrajectoryGenerator.cpp new file mode 100644 index 0000000000..35585f6042 --- /dev/null +++ b/wpilibc/src/main/native/cpp/trajectory/TrajectoryGenerator.cpp @@ -0,0 +1,77 @@ +/*----------------------------------------------------------------------------*/ +/* 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. */ +/*----------------------------------------------------------------------------*/ + +#include "frc/trajectory/TrajectoryGenerator.h" + +#include + +#include "frc/spline/SplineHelper.h" +#include "frc/trajectory/TrajectoryParameterizer.h" + +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) { + 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); + } + + auto points = SplinePointsFromSplines( + SplineHelper::QuinticSplinesFromWaypoints(newWaypoints)); + + // After trajectory generation, flip theta back so it's relative to the + // field. Also fix curvature. + if (reversed) { + for (auto& point : points) { + point = {point.first + flip, -point.second}; + } + } + + return TrajectoryParameterizer::TimeParameterizeTrajectory( + points, std::move(constraints), startVelocity, endVelocity, maxVelocity, + maxAcceleration, reversed); +} + +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 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; + + auto points = SplinePointsFromSplines( + SplineHelper::CubicSplinesFromWaypoints(newStart, waypoints, newEnd)); + + // After trajectory generation, flip theta back so it's relative to the + // field. Also fix curvature. + if (reversed) { + for (auto& point : points) { + point = {point.first + flip, -point.second}; + } + } + + return TrajectoryParameterizer::TimeParameterizeTrajectory( + points, std::move(constraints), startVelocity, endVelocity, maxVelocity, + maxAcceleration, reversed); +} diff --git a/wpilibc/src/main/native/cpp/trajectory/TrajectoryParameterizer.cpp b/wpilibc/src/main/native/cpp/trajectory/TrajectoryParameterizer.cpp new file mode 100644 index 0000000000..f16b1daae9 --- /dev/null +++ b/wpilibc/src/main/native/cpp/trajectory/TrajectoryParameterizer.cpp @@ -0,0 +1,224 @@ +/*----------------------------------------------------------------------------*/ +/* 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. */ +/*----------------------------------------------------------------------------*/ + +/* + * MIT License + * + * Copyright (c) 2018 Team 254 + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +#include "frc/trajectory/TrajectoryParameterizer.h" + +using namespace frc; + +Trajectory TrajectoryParameterizer::TimeParameterizeTrajectory( + const std::vector& points, + 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) { + std::vector constrainedStates(points.size()); + + ConstrainedState predecessor{points.front(), 0_m, startVelocity, + -maxAcceleration, maxAcceleration}; + + constrainedStates[0] = predecessor; + + // Forward pass + for (unsigned int i = 0; i < points.size(); i++) { + auto& constrainedState = constrainedStates[i]; + constrainedState.pose = points[i]; + + // Begin constraining based on predecessor + units::meter_t ds = constrainedState.pose.first.Translation().Distance( + predecessor.pose.first.Translation()); + constrainedState.distance = ds + predecessor.distance; + + // We may need to iterate to find the maximum end velocity and common + // acceleration, since acceleration limits may be a function of velocity. + while (true) { + // Enforce global max velocity and max reachable velocity by global + // acceleration limit. vf = std::sqrt(vi^2 + 2*a*d). + + constrainedState.maxVelocity = units::math::min( + maxVelocity, + units::math::sqrt(predecessor.maxVelocity * predecessor.maxVelocity + + predecessor.maxAcceleration * ds * 2.0)); + + constrainedState.minAcceleration = -maxAcceleration; + constrainedState.maxAcceleration = maxAcceleration; + + // At this point, the constrained state is fully constructed apart from + // all the custom-defined user constraints. + for (const auto& constraint : constraints) { + constrainedState.maxVelocity = units::math::min( + constrainedState.maxVelocity, + constraint->MaxVelocity(constrainedState.pose.first, + constrainedState.pose.second, + constrainedState.maxVelocity)); + } + + // Now enforce all acceleration limits. + EnforceAccelerationLimits(reversed, constraints, &constrainedState); + + if (ds.to() < kEpsilon) break; + + // If the actual acceleration for this state is higher than the max + // acceleration that we applied, then we need to reduce the max + // acceleration of the predecessor and try again. + units::meters_per_second_squared_t actualAcceleration = + (constrainedState.maxVelocity * constrainedState.maxVelocity - + predecessor.maxVelocity * predecessor.maxVelocity) / + (ds * 2.0); + + // If we violate the max acceleration constraint, let's modify the + // predecessor. + if (constrainedState.maxAcceleration < actualAcceleration - 1E-6_mps_sq) { + predecessor.maxAcceleration = constrainedState.maxAcceleration; + } else { + // Constrain the predecessor's max acceleration to the current + // acceleration. + if (actualAcceleration > predecessor.minAcceleration + 1E-6_mps_sq) { + predecessor.maxAcceleration = actualAcceleration; + } + // If the actual acceleration is less than the predecessor's min + // acceleration, it will be repaired in the backward pass. + break; + } + } + predecessor = constrainedState; + } + + ConstrainedState successor{points.back(), constrainedStates.back().distance, + endVelocity, -maxAcceleration, maxAcceleration}; + + // Backward pass + for (int i = points.size() - 1; i >= 0; i--) { + auto& constrainedState = constrainedStates[i]; + units::meter_t ds = + constrainedState.distance - successor.distance; // negative + + while (true) { + // Enforce max velocity limit (reverse) + // vf = std::sqrt(vi^2 + 2*a*d), where vi = successor. + units::meters_per_second_t newMaxVelocity = + units::math::sqrt(successor.maxVelocity * successor.maxVelocity + + successor.minAcceleration * ds * 2.0); + + // No more limits to impose! This state can be finalized. + if (newMaxVelocity >= constrainedState.maxVelocity) break; + + constrainedState.maxVelocity = newMaxVelocity; + + // Check all acceleration constraints with the new max velocity. + EnforceAccelerationLimits(reversed, constraints, &constrainedState); + + if (ds.to() > -kEpsilon) break; + + // If the actual acceleration for this state is lower than the min + // acceleration, then we need to lower the min acceleration of the + // successor and try again. + units::meters_per_second_squared_t actualAcceleration = + (constrainedState.maxVelocity * constrainedState.maxVelocity - + successor.maxVelocity * successor.maxVelocity) / + (ds * 2.0); + if (constrainedState.minAcceleration > actualAcceleration + 1E-6_mps_sq) { + successor.minAcceleration = constrainedState.minAcceleration; + } else { + successor.minAcceleration = actualAcceleration; + break; + } + } + successor = constrainedState; + } + + // Now we can integrate the constrained states forward in time to obtain our + // trajectory states. + + std::vector states(points.size()); + units::second_t t = 0_s; + units::meter_t s = 0_m; + units::meters_per_second_t v = 0_mps; + + for (unsigned int i = 0; i < constrainedStates.size(); i++) { + auto state = constrainedStates[i]; + + // Calculate the change in position between the current state and the + // previous state. + units::meter_t ds = state.distance - s; + + // Calculate the acceleration between the current state and the previous + // state. + units::meters_per_second_squared_t accel = + (state.maxVelocity * state.maxVelocity - v * v) / (ds * 2); + + // Calculate dt. + units::second_t dt = 0_s; + if (i > 0) { + states.at(i - 1).acceleration = reversed ? -accel : accel; + if (units::math::abs(accel) > 1E-6_mps_sq) { + // v_f = v_0 + a * t + dt = (state.maxVelocity - v) / accel; + } else if (units::math::abs(v) > 1E-6_mps) { + // delta_x = v * t + dt = ds / v; + } else { + throw std::runtime_error( + "Something went wrong during trajectory generation."); + } + } + + v = state.maxVelocity; + s = state.distance; + + t += dt; + + states[i] = {t, reversed ? -v : v, reversed ? -accel : accel, + state.pose.first, state.pose.second}; + } + + return Trajectory(states); +} + +void TrajectoryParameterizer::EnforceAccelerationLimits( + bool reverse, + const std::vector>& constraints, + ConstrainedState* state) { + for (auto&& constraint : constraints) { + double factor = reverse ? -1.0 : 1.0; + + auto minMaxAccel = constraint->MinMaxAcceleration( + state->pose.first, state->pose.second, state->maxVelocity * factor); + + state->minAcceleration = units::math::max( + state->minAcceleration, + reverse ? -minMaxAccel.maxAcceleration : minMaxAccel.minAcceleration); + + state->maxAcceleration = units::math::min( + state->maxAcceleration, + reverse ? -minMaxAccel.minAcceleration : minMaxAccel.maxAcceleration); + } +} diff --git a/wpilibc/src/main/native/cpp/trajectory/constraint/CentripetalAccelerationConstraint.cpp b/wpilibc/src/main/native/cpp/trajectory/constraint/CentripetalAccelerationConstraint.cpp new file mode 100644 index 0000000000..bf45c349a2 --- /dev/null +++ b/wpilibc/src/main/native/cpp/trajectory/constraint/CentripetalAccelerationConstraint.cpp @@ -0,0 +1,40 @@ +/*----------------------------------------------------------------------------*/ +/* 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. */ +/*----------------------------------------------------------------------------*/ + +#include "frc/trajectory/constraint/CentripetalAccelerationConstraint.h" + +using namespace frc; + +CentripetalAccelerationConstraint::CentripetalAccelerationConstraint( + units::meters_per_second_squared_t maxCentripetalAcceleration) + : m_maxCentripetalAcceleration(maxCentripetalAcceleration) {} + +units::meters_per_second_t CentripetalAccelerationConstraint::MaxVelocity( + const Pose2d& pose, curvature_t curvature, + units::meters_per_second_t velocity) { + // ac = v^2 / r + // k (curvature) = 1 / r + + // therefore, ac = v^2 * k + // ac / k = v^2 + // v = std::sqrt(ac / k) + + // We have to multiply by 1_rad here to get the units to cancel out nicely. + // The units library defines a unit for radians although it is technically + // unitless. + return units::math::sqrt(m_maxCentripetalAcceleration / + units::math::abs(curvature) * 1_rad); +} + +TrajectoryConstraint::MinMax +CentripetalAccelerationConstraint::MinMaxAcceleration( + const Pose2d& pose, curvature_t curvature, + units::meters_per_second_t speed) { + // The acceleration of the robot has no impact on the centripetal acceleration + // of the robot. + return {}; +} diff --git a/wpilibc/src/main/native/cpp/trajectory/constraint/DifferentialDriveKinematicsConstraint.cpp b/wpilibc/src/main/native/cpp/trajectory/constraint/DifferentialDriveKinematicsConstraint.cpp new file mode 100644 index 0000000000..8b88bf4c43 --- /dev/null +++ b/wpilibc/src/main/native/cpp/trajectory/constraint/DifferentialDriveKinematicsConstraint.cpp @@ -0,0 +1,31 @@ +/*----------------------------------------------------------------------------*/ +/* 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. */ +/*----------------------------------------------------------------------------*/ + +#include "frc/trajectory/constraint/DifferentialDriveKinematicsConstraint.h" + +using namespace frc; + +DifferentialDriveKinematicsConstraint::DifferentialDriveKinematicsConstraint( + DifferentialDriveKinematics kinematics, units::meters_per_second_t maxSpeed) + : m_kinematics(kinematics), m_maxSpeed(maxSpeed) {} + +units::meters_per_second_t DifferentialDriveKinematicsConstraint::MaxVelocity( + const Pose2d& pose, curvature_t curvature, + units::meters_per_second_t velocity) { + auto wheelSpeeds = + m_kinematics.ToWheelSpeeds({velocity, 0_mps, velocity * curvature}); + wheelSpeeds.Normalize(m_maxSpeed); + + return m_kinematics.ToChassisSpeeds(wheelSpeeds).vx; +} + +TrajectoryConstraint::MinMax +DifferentialDriveKinematicsConstraint::MinMaxAcceleration( + const Pose2d& pose, curvature_t curvature, + units::meters_per_second_t speed) { + return {}; +} diff --git a/wpilibc/src/main/native/include/frc/geometry/Pose2d.h b/wpilibc/src/main/native/include/frc/geometry/Pose2d.h index 75f06bfd3a..2161e8280b 100644 --- a/wpilibc/src/main/native/include/frc/geometry/Pose2d.h +++ b/wpilibc/src/main/native/include/frc/geometry/Pose2d.h @@ -68,6 +68,14 @@ class Pose2d { */ Pose2d& operator+=(const Transform2d& other); + /** + * Returns the Transform2d that maps the one pose to another. + * + * @param other The initial pose of the transformation. + * @return The transform that maps the other pose to the current pose. + */ + Transform2d operator-(const Pose2d& other) const; + /** * Checks equality between this Pose2d and another object. * @@ -145,6 +153,16 @@ class Pose2d { */ Pose2d Exp(const Twist2d& twist) const; + /** + * Returns a Twist2d that maps this pose to the end pose. If c is the output + * of a.Log(b), then a.Exp(c) would yield b. + * + * @param end The end pose for the transformation. + * + * @return The twist that maps this to end. + */ + Twist2d Log(const Pose2d& end) const; + private: Translation2d m_translation; Rotation2d m_rotation; diff --git a/wpilibc/src/main/native/include/frc/geometry/Rotation2d.h b/wpilibc/src/main/native/include/frc/geometry/Rotation2d.h index 2c5773e028..9453e0cb78 100644 --- a/wpilibc/src/main/native/include/frc/geometry/Rotation2d.h +++ b/wpilibc/src/main/native/include/frc/geometry/Rotation2d.h @@ -97,6 +97,14 @@ class Rotation2d { */ Rotation2d operator-() const; + /** + * Multiplies the current rotation by a scalar. + * @param scalar The scalar. + * + * @return The new scaled Rotation2d. + */ + Rotation2d operator*(double scalar) const; + /** * Checks equality between this Rotation2d and another object. * diff --git a/wpilibc/src/main/native/include/frc/geometry/Transform2d.h b/wpilibc/src/main/native/include/frc/geometry/Transform2d.h index 24dea15673..c75fbebcaa 100644 --- a/wpilibc/src/main/native/include/frc/geometry/Transform2d.h +++ b/wpilibc/src/main/native/include/frc/geometry/Transform2d.h @@ -53,6 +53,16 @@ class Transform2d { */ const Rotation2d& Rotation() const { return m_rotation; } + /** + * Scales the transform by the scalar. + * + * @param scalar The scalar. + * @return The scaled Transform2d. + */ + Transform2d operator*(double scalar) const { + return Transform2d(m_translation * scalar, m_rotation * scalar); + } + /** * Checks equality between this Transform2d and another object. * diff --git a/wpilibc/src/main/native/include/frc/spline/CubicHermiteSpline.h b/wpilibc/src/main/native/include/frc/spline/CubicHermiteSpline.h new file mode 100644 index 0000000000..a8979c1651 --- /dev/null +++ b/wpilibc/src/main/native/include/frc/spline/CubicHermiteSpline.h @@ -0,0 +1,84 @@ +/*----------------------------------------------------------------------------*/ +/* 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 "frc/spline/Spline.h" + +namespace frc { +/** + * Represents a hermite spline of degree 3. + */ +class CubicHermiteSpline : public Spline<3> { + public: + /** + * Constructs a cubic hermite spline with the specified control vectors. Each + * control vector contains info about the location of the point and its first + * derivative. + * + * @param xInitialControlVector The control vector for the initial point in + * the x dimension. + * @param xFinalControlVector The control vector for the final point in + * the x dimension. + * @param yInitialControlVector The control vector for the initial point in + * the y dimension. + * @param yFinalControlVector The control vector for the final point in + * the y dimension. + */ + CubicHermiteSpline(std::array xInitialControlVector, + std::array xFinalControlVector, + std::array yInitialControlVector, + std::array yFinalControlVector); + + protected: + /** + * Returns the coefficients matrix. + * @return The coefficients matrix. + */ + Eigen::Matrix Coefficients() const override { + return m_coefficients; + } + + private: + Eigen::Matrix m_coefficients; + + /** + * Returns the hermite basis matrix for cubic hermite spline interpolation. + * @return The hermite basis matrix for cubic hermite spline interpolation. + */ + static Eigen::Matrix MakeHermiteBasis() { + // clang-format off + static auto basis = (Eigen::Matrix() << + +2.0, +1.0, -2.0, +1.0, + -3.0, -2.0, +3.0, -1.0, + +0.0, +1.0, +0.0, +0.0, + +1.0, +0.0, +0.0, +0.0).finished(); + // clang-format on + return basis; + } + + /** + * Returns the control vector for each dimension as a matrix from the + * user-provided arrays in the constructor. + * + * @param initialVector The control vector for the initial point. + * @param finalVector The control vector for the final point. + * + * @return The control vector matrix for a dimension. + */ + static Eigen::Vector4d ControlVectorFromArrays( + std::array initialVector, std::array finalVector) { + return (Eigen::Vector4d() << initialVector[0], initialVector[1], + finalVector[0], finalVector[1]) + .finished(); + } +}; +} // namespace frc diff --git a/wpilibc/src/main/native/include/frc/spline/QuinticHermiteSpline.h b/wpilibc/src/main/native/include/frc/spline/QuinticHermiteSpline.h new file mode 100644 index 0000000000..730eab42f3 --- /dev/null +++ b/wpilibc/src/main/native/include/frc/spline/QuinticHermiteSpline.h @@ -0,0 +1,86 @@ +/*----------------------------------------------------------------------------*/ +/* 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 "frc/spline/Spline.h" + +namespace frc { +/** + * Represents a hermite spline of degree 5. + */ +class QuinticHermiteSpline : public Spline<5> { + public: + /** + * Constructs a quintic hermite spline with the specified control vectors. + * Each control vector contains into about the location of the point, its + * first derivative, and its second derivative. + * + * @param xInitialControlVector The control vector for the initial point in + * the x dimension. + * @param xFinalControlVector The control vector for the final point in + * the x dimension. + * @param yInitialControlVector The control vector for the initial point in + * the y dimension. + * @param yFinalControlVector The control vector for the final point in + * the y dimension. + */ + QuinticHermiteSpline(std::array xInitialControlVector, + std::array xFinalControlVector, + std::array yInitialControlVector, + std::array yFinalControlVector); + + protected: + /** + * Returns the coefficients matrix. + * @return The coefficients matrix. + */ + Eigen::Matrix Coefficients() const override { + return m_coefficients; + } + + private: + Eigen::Matrix m_coefficients; + + /** + * Returns the hermite basis matrix for quintic hermite spline interpolation. + * @return The hermite basis matrix for quintic hermite spline interpolation. + */ + static Eigen::Matrix MakeHermiteBasis() { + // clang-format off + static const auto basis = (Eigen::Matrix() << + -06.0, -03.0, -00.5, +06.0, -03.0, +00.5, + +15.0, +08.0, +01.5, -15.0, +07.0, +01.0, + -10.0, -06.0, -01.5, +10.0, -04.0, +00.5, + +00.0, +00.0, +00.5, +00.0, +00.0, +00.0, + +00.0, +01.0, +00.0, +00.0, +00.0, +00.0, + +01.0, +00.0, +00.0, +00.0, +00.0, +00.0).finished(); + // clang-format on + return basis; + } + + /** + * Returns the control vector for each dimension as a matrix from the + * user-provided arrays in the constructor. + * + * @param initialVector The control vector for the initial point. + * @param finalVector The control vector for the final point. + * + * @return The control vector matrix for a dimension. + */ + static Eigen::Matrix ControlVectorFromArrays( + std::array initialVector, std::array finalVector) { + return (Eigen::Matrix() << initialVector[0], initialVector[1], + initialVector[2], finalVector[0], finalVector[1], finalVector[2]) + .finished(); + } +}; +} // namespace frc diff --git a/wpilibc/src/main/native/include/frc/spline/Spline.h b/wpilibc/src/main/native/include/frc/spline/Spline.h new file mode 100644 index 0000000000..236661b586 --- /dev/null +++ b/wpilibc/src/main/native/include/frc/spline/Spline.h @@ -0,0 +1,122 @@ +/*----------------------------------------------------------------------------*/ +/* 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 "frc/geometry/Pose2d.h" + +namespace frc { + +/** + * Define a unit for curvature. + */ +using curvature_t = units::unit_t< + units::compound_unit>>; + +/** + * Represents a two-dimensional parametric spline that interpolates between two + * points. + * + * @tparam Degree The degree of the spline. + */ +template +class Spline { + public: + using PoseWithCurvature = std::pair; + + Spline() = default; + + Spline(const Spline&) = default; + Spline& operator=(const Spline&) = default; + + Spline(Spline&&) = default; + Spline& operator=(Spline&&) = default; + + virtual ~Spline() = default; + + /** + * Gets the pose and curvature at some point t on the spline. + * + * @param t The point t + * @return The pose and curvature at that point. + */ + PoseWithCurvature GetPoint(double t) const { + Eigen::Matrix polynomialBases; + + // Populate the polynomial bases + for (int i = 0; i <= Degree; i++) { + polynomialBases(i) = std::pow(t, Degree - i); + } + + // This simply multiplies by the coefficients. We need to divide out t some + // n number of times where n is the derivative we want to take. + Eigen::Matrix combined = Coefficients() * polynomialBases; + + double dx, dy, ddx, ddy; + + // If t = 0, all other terms in the equation cancel out to zero. We can use + // the last x^0 term in the equation. + if (t == 0.0) { + dx = Coefficients()(2, Degree - 1); + dy = Coefficients()(3, Degree - 1); + ddx = Coefficients()(4, Degree - 2); + ddy = Coefficients()(5, Degree - 2); + } else { + // Divide out t for first derivative. + dx = combined(2) / t; + dy = combined(3) / t; + + // Divide out t for second derivative. + ddx = combined(4) / t / t; + ddy = combined(5) / t / t; + } + + // Find the curvature. + const auto curvature = + (dx * ddy - ddx * dy) / ((dx * dx + dy * dy) * std::hypot(dx, dy)); + + return { + {FromVector(combined.template block<2, 1>(0, 0)), Rotation2d(dx, dy)}, + curvature_t(curvature)}; + } + + protected: + /** + * Returns the coefficients of the spline. + * + * @return The coefficients of the spline. + */ + virtual Eigen::Matrix Coefficients() const = 0; + + /** + * Converts a Translation2d into a vector that is compatible with Eigen. + * + * @param translation The Translation2d to convert. + * @return The vector. + */ + static Eigen::Vector2d ToVector(const Translation2d& translation) { + return (Eigen::Vector2d() << translation.X().to(), + translation.Y().to()) + .finished(); + } + + /** + * Converts an Eigen vector into a Translation2d. + * + * @param vector The vector to convert. + * @return The Translation2d. + */ + static Translation2d FromVector(const Eigen::Vector2d& vector) { + return Translation2d(units::meter_t(vector(0)), units::meter_t(vector(1))); + } +}; +} // namespace frc diff --git a/wpilibc/src/main/native/include/frc/spline/SplineHelper.h b/wpilibc/src/main/native/include/frc/spline/SplineHelper.h new file mode 100644 index 0000000000..363debb185 --- /dev/null +++ b/wpilibc/src/main/native/include/frc/spline/SplineHelper.h @@ -0,0 +1,71 @@ +/*----------------------------------------------------------------------------*/ +/* 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 "frc/spline/CubicHermiteSpline.h" +#include "frc/spline/QuinticHermiteSpline.h" + +namespace frc { +/** + * Helper class that is used to generate cubic and quintic splines from user + * provided waypoints. + */ +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. + * + * 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. + * + * @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); + + /** + * Returns a set of quintic splines corresponding to the provided waypoints. + * The user is free to set the direction of all waypoints. Continuous + * curvature is guaranteed throughout the path. + * + * @param waypoints The waypoints. + * @return A vector of quintic hermite splines that interpolate through the + * provided waypoints. + */ + static std::vector QuinticSplinesFromWaypoints( + const std::vector& waypoints); + + private: + /** + * Thomas algorithm for solving tridiagonal systems Af = d. + * + * @param a the values of A above the diagonal + * @param b the values of A on the diagonal + * @param c the values of A below the diagonal + * @param d the vector on the rhs + * @param solutionVector the unknown (solution) vector, modified in-place + */ + static void ThomasAlgorithm(const std::vector& a, + const std::vector& b, + const std::vector& c, + const std::vector& d, + std::vector* solutionVector); +}; +} // namespace frc diff --git a/wpilibc/src/main/native/include/frc/spline/SplineParameterizer.h b/wpilibc/src/main/native/include/frc/spline/SplineParameterizer.h new file mode 100644 index 0000000000..34cf026279 --- /dev/null +++ b/wpilibc/src/main/native/include/frc/spline/SplineParameterizer.h @@ -0,0 +1,114 @@ +/*----------------------------------------------------------------------------*/ +/* 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. */ +/*----------------------------------------------------------------------------*/ + +/* + * MIT License + * + * Copyright (c) 2018 Team 254 + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +#pragma once + +#include + +#include +#include + +#include + +namespace frc { + +/** + * Class used to parameterize a spline by its arc length. + */ +class SplineParameterizer { + public: + using PoseWithCurvature = std::pair; + + /** + * Parameterizes the spline. This method breaks up the spline into various + * arcs until their dx, dy, and dtheta are within specific tolerances. + * + * @param spline The spline to parameterize. + * @param t0 Starting internal spline parameter. It is recommended to leave + * this as default. + * @param t1 Ending internal spline parameter. It is recommended to leave this + * as default. + * + * @return A vector of poses and curvatures that represents various points on + * the spline. + */ + template + static std::vector Parameterize(const Spline& spline, + double t0 = 0.0, + double t1 = 1.0) { + std::vector arr; + + // The parameterization does not add the first initial point. Let's add + // that. + arr.push_back(spline.GetPoint(t0)); + + GetSegmentArc(spline, &arr, t0, t1); + return arr; + } + + private: + // Constraints for spline parameterization. + static constexpr units::meter_t kMaxDx = 5_in; + static constexpr units::meter_t kMaxDy = 0.05_in; + static constexpr units::radian_t kMaxDtheta = 0.0872_rad; + + /** + * Breaks up the spline into arcs until the dx, dy, and theta of each arc is + * within tolerance. + * + * @param spline The spline to parameterize. + * @param vector Pointer to vector of poses. + * @param t0 Starting point for arc. + * @param t1 Ending point for arc. + */ + template + static void GetSegmentArc(const Spline& spline, + std::vector* vector, double t0, + double t1) { + const auto start = spline.GetPoint(t0); + const auto end = spline.GetPoint(t1); + + const auto twist = start.first.Log(end.first); + + if (units::math::abs(twist.dy) > kMaxDy || + units::math::abs(twist.dx) > kMaxDx || + units::math::abs(twist.dtheta) > kMaxDtheta) { + GetSegmentArc(spline, vector, t0, (t0 + t1) / 2); + GetSegmentArc(spline, vector, (t0 + t1) / 2, t1); + } else { + vector->push_back(spline.GetPoint(t1)); + } + } + + friend class CubicHermiteSplineTest; + friend class QuinticHermiteSplineTest; +}; +} // namespace frc diff --git a/wpilibc/src/main/native/include/frc/trajectory/Trajectory.h b/wpilibc/src/main/native/include/frc/trajectory/Trajectory.h new file mode 100644 index 0000000000..0a384fbb00 --- /dev/null +++ b/wpilibc/src/main/native/include/frc/trajectory/Trajectory.h @@ -0,0 +1,106 @@ +/*----------------------------------------------------------------------------*/ +/* 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 "frc/geometry/Pose2d.h" + +namespace frc { + +/** + * Define a unit for curvature. + */ +using curvature_t = units::unit_t< + units::compound_unit>>; + +/** + * Represents a time-parameterized trajectory. The trajectory contains of + * various States that represent the pose, curvature, time elapsed, velocity, + * and acceleration at that point. + */ +class Trajectory { + public: + /** + * Represents one point on the trajectory. + */ + struct State { + // The time elapsed since the beginning of the trajectory. + units::second_t t = 0_s; + + // The speed at that point of the trajectory. + units::meters_per_second_t velocity = 0_mps; + + // The acceleration at that point of the trajectory. + units::meters_per_second_squared_t acceleration = 0_mps_sq; + + // The pose at that point of the trajectory. + Pose2d pose; + + // The curvature at that point of the trajectory. + curvature_t curvature{0.0}; + + /** + * Interpolates between two States. + * + * @param endValue The end value for the interpolation. + * @param i The interpolant (fraction). + * + * @return The interpolated state. + */ + State Interpolate(State endValue, double i) const; + }; + + Trajectory() = default; + + /** + * Constructs a trajectory from a vector of states. + */ + explicit Trajectory(const std::vector& states); + + /** + * Returns the overall duration of the trajectory. + * @return The duration of the trajectory. + */ + units::second_t TotalTime() const { return m_totalTime; } + + /** + * Return the states of the trajectory. + * @return The states of the trajectory. + */ + const std::vector& States() const { return m_states; } + + /** + * Sample the trajectory at a point in time. + * + * @param t The point in time since the beginning of the trajectory to sample. + * @return The state at that point in time. + */ + State Sample(units::second_t t) const; + + private: + std::vector m_states; + units::second_t m_totalTime; + + /** + * Linearly interpolates between two values. + * + * @param startValue The start value. + * @param endValue The end value. + * @param t The fraction for interpolation. + * + * @return The interpolated value. + */ + template + static T Lerp(const T& startValue, const T& endValue, const double t) { + return startValue + (endValue - startValue) * t; + } +}; +} // namespace frc diff --git a/wpilibc/src/main/native/include/frc/trajectory/TrajectoryGenerator.h b/wpilibc/src/main/native/include/frc/trajectory/TrajectoryGenerator.h new file mode 100644 index 0000000000..b39f6d392f --- /dev/null +++ b/wpilibc/src/main/native/include/frc/trajectory/TrajectoryGenerator.h @@ -0,0 +1,107 @@ +/*----------------------------------------------------------------------------*/ +/* 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 "frc/spline/SplineParameterizer.h" +#include "frc/trajectory/Trajectory.h" +#include "frc/trajectory/constraint/TrajectoryConstraint.h" + +namespace frc { +/** + * Helper class used to generate trajectories with various constraints. + */ +class TrajectoryGenerator { + public: + using PoseWithCurvature = std::pair; + + /** + * Generates a trajectory with the given waypoints and constraints. + * + * @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. + */ + 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); + + /** + * Generates a trajectory with the given waypoints and constraints. + * + * @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. + */ + 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); + + private: + /** + * Generate spline points from a vector of splines by parameterizing the + * splines. + * + * @param splines The splines to parameterize. + * + * @return The spline points. + */ + template + static std::vector SplinePointsFromSplines( + const std::vector& splines) { + // Create the vector of spline points. + std::vector splinePoints; + + // Add the first point to the vector. + splinePoints.push_back(splines.front().GetPoint(0.0)); + + // Iterate through the vector and parameterize each spline, adding the + // parameterized points to the final vector. + for (auto&& spline : splines) { + auto points = SplineParameterizer::Parameterize(spline); + // Append the array of poses to the vector. We are removing the first + // point because it's a duplicate of the last point from the previous + // spline. + splinePoints.insert(std::end(splinePoints), std::begin(points) + 1, + std::end(points)); + } + return splinePoints; + } +}; +} // namespace frc diff --git a/wpilibc/src/main/native/include/frc/trajectory/TrajectoryParameterizer.h b/wpilibc/src/main/native/include/frc/trajectory/TrajectoryParameterizer.h new file mode 100644 index 0000000000..6fb43f3314 --- /dev/null +++ b/wpilibc/src/main/native/include/frc/trajectory/TrajectoryParameterizer.h @@ -0,0 +1,107 @@ +/*----------------------------------------------------------------------------*/ +/* 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. */ +/*----------------------------------------------------------------------------*/ + +/* + * MIT License + * + * Copyright (c) 2018 Team 254 + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +#pragma once + +#include +#include +#include + +#include "frc/trajectory/Trajectory.h" +#include "frc/trajectory/constraint/TrajectoryConstraint.h" + +namespace frc { +/** + * Class used to parameterize a trajectory by time. + */ +class TrajectoryParameterizer { + public: + using PoseWithCurvature = std::pair; + + /** + * Parameterize the trajectory by time. This is where the velocity profile is + * generated. + * + * The derivation of the algorithm used can be found here: + * + * + * @param points Reference to the spline points. + * @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. + */ + static Trajectory TimeParameterizeTrajectory( + const std::vector& points, + 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); + + private: + constexpr static double kEpsilon = 1E-6; + + /** + * Represents a constrained state that is used when time parameterizing a + * trajectory. Each state has the pose, curvature, distance from the start of + * the trajectory, max velocity, min acceleration and max acceleration. + */ + struct ConstrainedState { + PoseWithCurvature pose = {Pose2d(), curvature_t(0.0)}; + units::meter_t distance = 0_m; + units::meters_per_second_t maxVelocity = 0_mps; + units::meters_per_second_squared_t minAcceleration = 0_mps_sq; + units::meters_per_second_squared_t maxAcceleration = 0_mps_sq; + }; + + /** + * Enforces acceleration limits as defined by the constraints. This function + * is used when time parameterizing a trajectory. + * + * @param reverse Whether the robot is traveling backwards. + * @param constraints A vector of the user-defined velocity and acceleration + * constraints. + * @param state Pointer to the constrained state that we are operating on. + * This is mutated in place. + */ + static void EnforceAccelerationLimits( + bool reverse, + const std::vector>& constraints, + ConstrainedState* state); +}; +} // namespace frc diff --git a/wpilibc/src/main/native/include/frc/trajectory/constraint/CentripetalAccelerationConstraint.h b/wpilibc/src/main/native/include/frc/trajectory/constraint/CentripetalAccelerationConstraint.h new file mode 100644 index 0000000000..de25738a87 --- /dev/null +++ b/wpilibc/src/main/native/include/frc/trajectory/constraint/CentripetalAccelerationConstraint.h @@ -0,0 +1,40 @@ +/*----------------------------------------------------------------------------*/ +/* 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 "frc/trajectory/constraint/TrajectoryConstraint.h" + +namespace frc { + +/** + * A constraint on the maximum absolute centripetal acceleration allowed when + * traversing a trajectory. The centripetal acceleration of a robot is defined + * as the velocity squared divided by the radius of curvature. + * + * Effectively, limiting the maximum centripetal acceleration will cause the + * robot to slow down around tight turns, making it easier to track trajectories + * with sharp turns. + */ +class CentripetalAccelerationConstraint : public TrajectoryConstraint { + public: + explicit CentripetalAccelerationConstraint( + units::meters_per_second_squared_t maxCentripetalAcceleration); + + units::meters_per_second_t MaxVelocity( + const Pose2d& pose, curvature_t curvature, + units::meters_per_second_t velocity) override; + + MinMax MinMaxAcceleration(const Pose2d& pose, curvature_t curvature, + units::meters_per_second_t speed) override; + + private: + units::meters_per_second_squared_t m_maxCentripetalAcceleration; +}; +} // namespace frc diff --git a/wpilibc/src/main/native/include/frc/trajectory/constraint/DifferentialDriveKinematicsConstraint.h b/wpilibc/src/main/native/include/frc/trajectory/constraint/DifferentialDriveKinematicsConstraint.h new file mode 100644 index 0000000000..6259c96d43 --- /dev/null +++ b/wpilibc/src/main/native/include/frc/trajectory/constraint/DifferentialDriveKinematicsConstraint.h @@ -0,0 +1,38 @@ +/*----------------------------------------------------------------------------*/ +/* 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 "frc/kinematics/DifferentialDriveKinematics.h" +#include "frc/trajectory/constraint/TrajectoryConstraint.h" + +/** + * A class that enforces constraints on the differential drive kinematics. + * This can be used to ensure that the trajectory is constructed so that the + * commanded velocities for both sides of the drivetrain stay below a certain + * limit. + */ +namespace frc { +class DifferentialDriveKinematicsConstraint : public TrajectoryConstraint { + public: + DifferentialDriveKinematicsConstraint(DifferentialDriveKinematics kinematics, + units::meters_per_second_t maxSpeed); + + units::meters_per_second_t MaxVelocity( + const Pose2d& pose, curvature_t curvature, + units::meters_per_second_t velocity) override; + + MinMax MinMaxAcceleration(const Pose2d& pose, curvature_t curvature, + units::meters_per_second_t speed) override; + + private: + DifferentialDriveKinematics m_kinematics; + units::meters_per_second_t m_maxSpeed; +}; +} // namespace frc diff --git a/wpilibc/src/main/native/include/frc/trajectory/constraint/TrajectoryConstraint.h b/wpilibc/src/main/native/include/frc/trajectory/constraint/TrajectoryConstraint.h new file mode 100644 index 0000000000..dcde8c4590 --- /dev/null +++ b/wpilibc/src/main/native/include/frc/trajectory/constraint/TrajectoryConstraint.h @@ -0,0 +1,78 @@ +/*----------------------------------------------------------------------------*/ +/* 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 "frc/geometry/Pose2d.h" +#include "frc/spline/Spline.h" + +namespace frc { +/** + * An interface for defining user-defined velocity and acceleration constraints + * while generating trajectories. + */ +class TrajectoryConstraint { + public: + TrajectoryConstraint() = default; + + TrajectoryConstraint(const TrajectoryConstraint&) = default; + TrajectoryConstraint& operator=(const TrajectoryConstraint&) = default; + + TrajectoryConstraint(TrajectoryConstraint&&) = default; + TrajectoryConstraint& operator=(TrajectoryConstraint&&) = default; + + virtual ~TrajectoryConstraint() = default; + + /** + * Represents a minimum and maximum acceleration. + */ + struct MinMax { + /** + * The minimum acceleration. + */ + units::meters_per_second_squared_t minAcceleration{ + -std::numeric_limits::max()}; + + /** + * The maximum acceleration. + */ + units::meters_per_second_squared_t maxAcceleration{ + std::numeric_limits::max()}; + }; + + /** + * Returns the max velocity given the current pose and curvature. + * + * @param pose The pose at the current point in the trajectory. + * @param curvature The curvature at the current point in the trajectory. + * @param velocity The velocity at the current point in the trajectory before + * constraints are applied. + * + * @return The absolute maximum velocity. + */ + virtual units::meters_per_second_t MaxVelocity( + const Pose2d& pose, curvature_t curvature, + units::meters_per_second_t velocity) = 0; + + /** + * Returns the minimum and maximum allowable acceleration for the trajectory + * given pose, curvature, and speed. + * + * @param pose The pose at the current point in the trajectory. + * @param curvature The curvature at the current point in the trajectory. + * @param speed The speed at the current point in the trajectory. + * + * @return The min and max acceleration bounds. + */ + virtual MinMax MinMaxAcceleration(const Pose2d& pose, curvature_t curvature, + units::meters_per_second_t speed) = 0; +}; +} // namespace frc diff --git a/wpilibc/src/test/native/cpp/geometry/Pose2dTest.cpp b/wpilibc/src/test/native/cpp/geometry/Pose2dTest.cpp index 74d43fc237..8b5f6745f3 100644 --- a/wpilibc/src/test/native/cpp/geometry/Pose2dTest.cpp +++ b/wpilibc/src/test/native/cpp/geometry/Pose2dTest.cpp @@ -52,3 +52,15 @@ TEST(Pose2dTest, Inequality) { const Pose2d b{0_m, 5_ft, Rotation2d(43_deg)}; EXPECT_TRUE(a != b); } + +TEST(Pose2dTest, Minus) { + const Pose2d initial{0_m, 0_m, Rotation2d(45.0_deg)}; + const Pose2d final{5_m, 5_m, Rotation2d(45.0_deg)}; + + const auto transform = final - initial; + + EXPECT_NEAR(transform.Translation().X().to(), 5.0 * std::sqrt(2.0), + kEpsilon); + EXPECT_NEAR(transform.Translation().Y().to(), 0.0, kEpsilon); + EXPECT_NEAR(transform.Rotation().Degrees().to(), 0.0, kEpsilon); +} diff --git a/wpilibc/src/test/native/cpp/geometry/Twist2dTest.cpp b/wpilibc/src/test/native/cpp/geometry/Twist2dTest.cpp index 9b9e2a7284..faf36d9f21 100644 --- a/wpilibc/src/test/native/cpp/geometry/Twist2dTest.cpp +++ b/wpilibc/src/test/native/cpp/geometry/Twist2dTest.cpp @@ -56,3 +56,14 @@ TEST(Twist2dTest, Inequality) { const Twist2d two{5.0_m, 1.2_m, 3.0_rad}; EXPECT_TRUE(one != two); } + +TEST(Twist2dTest, Pose2dLog) { + const Pose2d end{5_m, 5_m, Rotation2d(90_deg)}; + const Pose2d start{}; + + const auto twist = start.Log(end); + + EXPECT_NEAR(twist.dx.to(), 5 / 2.0 * wpi::math::pi, kEpsilon); + EXPECT_NEAR(twist.dy.to(), 0.0, kEpsilon); + EXPECT_NEAR(twist.dtheta.to(), wpi::math::pi / 2.0, kEpsilon); +} diff --git a/wpilibc/src/test/native/cpp/spline/CubicHermiteSplineTest.cpp b/wpilibc/src/test/native/cpp/spline/CubicHermiteSplineTest.cpp new file mode 100644 index 0000000000..3e1da691dd --- /dev/null +++ b/wpilibc/src/test/native/cpp/spline/CubicHermiteSplineTest.cpp @@ -0,0 +1,95 @@ +/*----------------------------------------------------------------------------*/ +/* 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. */ +/*----------------------------------------------------------------------------*/ + +#include +#include +#include + +#include + +#include "frc/geometry/Pose2d.h" +#include "frc/geometry/Rotation2d.h" +#include "frc/spline/QuinticHermiteSpline.h" +#include "frc/spline/SplineHelper.h" +#include "frc/spline/SplineParameterizer.h" +#include "gtest/gtest.h" + +using namespace frc; + +namespace frc { +class CubicHermiteSplineTest : public ::testing::Test { + protected: + static void Run(const Pose2d& a, const std::vector& waypoints, + const Pose2d& b) { + // Start the timer. + const auto start = std::chrono::high_resolution_clock::now(); + + // Generate and parameterize the spline. + const auto splines = + SplineHelper::CubicSplinesFromWaypoints(a, waypoints, b); + std::vector::PoseWithCurvature> poses; + + poses.push_back(splines[0].GetPoint(0.0)); + + for (auto&& spline : splines) { + auto x = SplineParameterizer::Parameterize(spline); + poses.insert(std::end(poses), std::begin(x) + 1, std::end(x)); + } + + // End timer. + const auto finish = std::chrono::high_resolution_clock::now(); + + // Calculate the duration (used when benchmarking) + const auto duration = + std::chrono::duration_cast(finish - start); + + for (unsigned int i = 0; i < poses.size() - 1; i++) { + auto& p0 = poses[i]; + auto& p1 = poses[i + 1]; + + // Make sure the twist is under the tolerance defined by the Spline class. + auto twist = p0.first.Log(p1.first); + EXPECT_LT(std::abs(twist.dx.to()), + SplineParameterizer::kMaxDx.to()); + EXPECT_LT(std::abs(twist.dy.to()), + SplineParameterizer::kMaxDy.to()); + EXPECT_LT(std::abs(twist.dtheta.to()), + SplineParameterizer::kMaxDtheta.to()); + } + + // Check first point. + EXPECT_NEAR(poses.front().first.Translation().X().to(), + a.Translation().X().to(), 1E-9); + EXPECT_NEAR(poses.front().first.Translation().Y().to(), + a.Translation().Y().to(), 1E-9); + EXPECT_NEAR(poses.front().first.Rotation().Radians().to(), + a.Rotation().Radians().to(), 1E-9); + + // Check last point. + EXPECT_NEAR(poses.back().first.Translation().X().to(), + b.Translation().X().to(), 1E-9); + EXPECT_NEAR(poses.back().first.Translation().Y().to(), + b.Translation().Y().to(), 1E-9); + EXPECT_NEAR(poses.back().first.Rotation().Radians().to(), + b.Rotation().Radians().to(), 1E-9); + + static_cast(duration); + } +}; +} // namespace frc + +TEST_F(CubicHermiteSplineTest, StraightLine) { + Run(Pose2d(), std::vector(), Pose2d(3_m, 0_m, Rotation2d())); +} + +TEST_F(CubicHermiteSplineTest, SCurve) { + Pose2d start{0_m, 0_m, Rotation2d(90_deg)}; + std::vector waypoints{Translation2d(1_m, 1_m), + Translation2d(2_m, -1_m)}; + Pose2d end{3_m, 0_m, Rotation2d{90_deg}}; + Run(start, waypoints, end); +} diff --git a/wpilibc/src/test/native/cpp/spline/QuinticHermiteSplineTest.cpp b/wpilibc/src/test/native/cpp/spline/QuinticHermiteSplineTest.cpp new file mode 100644 index 0000000000..f4e03e4133 --- /dev/null +++ b/wpilibc/src/test/native/cpp/spline/QuinticHermiteSplineTest.cpp @@ -0,0 +1,86 @@ +/*----------------------------------------------------------------------------*/ +/* 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. */ +/*----------------------------------------------------------------------------*/ + +#include +#include + +#include + +#include "frc/geometry/Pose2d.h" +#include "frc/geometry/Rotation2d.h" +#include "frc/spline/QuinticHermiteSpline.h" +#include "frc/spline/SplineHelper.h" +#include "frc/spline/SplineParameterizer.h" +#include "gtest/gtest.h" + +using namespace frc; + +namespace frc { +class QuinticHermiteSplineTest : public ::testing::Test { + protected: + static void Run(const Pose2d& a, const Pose2d& b) { + // Start the timer. + const auto start = std::chrono::high_resolution_clock::now(); + + // Generate and parameterize the spline. + const auto spline = SplineHelper::QuinticSplinesFromWaypoints({a, b})[0]; + const auto poses = SplineParameterizer::Parameterize(spline); + + // End timer. + const auto finish = std::chrono::high_resolution_clock::now(); + + // Calculate the duration (used when benchmarking) + const auto duration = + std::chrono::duration_cast(finish - start); + + for (unsigned int i = 0; i < poses.size() - 1; i++) { + auto& p0 = poses[i]; + auto& p1 = poses[i + 1]; + + // Make sure the twist is under the tolerance defined by the Spline class. + auto twist = p0.first.Log(p1.first); + EXPECT_LT(std::abs(twist.dx.to()), + SplineParameterizer::kMaxDx.to()); + EXPECT_LT(std::abs(twist.dy.to()), + SplineParameterizer::kMaxDy.to()); + EXPECT_LT(std::abs(twist.dtheta.to()), + SplineParameterizer::kMaxDtheta.to()); + } + + // Check first point. + EXPECT_NEAR(poses.front().first.Translation().X().to(), + a.Translation().X().to(), 1E-9); + EXPECT_NEAR(poses.front().first.Translation().Y().to(), + a.Translation().Y().to(), 1E-9); + EXPECT_NEAR(poses.front().first.Rotation().Radians().to(), + a.Rotation().Radians().to(), 1E-9); + + // Check last point. + EXPECT_NEAR(poses.back().first.Translation().X().to(), + b.Translation().X().to(), 1E-9); + EXPECT_NEAR(poses.back().first.Translation().Y().to(), + b.Translation().Y().to(), 1E-9); + EXPECT_NEAR(poses.back().first.Rotation().Radians().to(), + b.Rotation().Radians().to(), 1E-9); + + static_cast(duration); + } +}; +} // namespace frc + +TEST_F(QuinticHermiteSplineTest, StraightLine) { + Run(Pose2d(), Pose2d(3_m, 0_m, Rotation2d())); +} + +TEST_F(QuinticHermiteSplineTest, SimpleSCurve) { + Run(Pose2d(), Pose2d(1_m, 1_m, Rotation2d())); +} + +TEST_F(QuinticHermiteSplineTest, SquigglyCurve) { + Run(Pose2d(0_m, 0_m, Rotation2d(90_deg)), + Pose2d(-1_m, 0_m, Rotation2d(90_deg))); +} diff --git a/wpilibc/src/test/native/cpp/trajectory/CentripetalAccelerationConstraintTest.cpp b/wpilibc/src/test/native/cpp/trajectory/CentripetalAccelerationConstraintTest.cpp new file mode 100644 index 0000000000..cb1d13b82c --- /dev/null +++ b/wpilibc/src/test/native/cpp/trajectory/CentripetalAccelerationConstraintTest.cpp @@ -0,0 +1,43 @@ +/*----------------------------------------------------------------------------*/ +/* 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. */ +/*----------------------------------------------------------------------------*/ + +#include +#include + +#include + +#include "frc/trajectory/constraint/CentripetalAccelerationConstraint.h" +#include "frc/trajectory/constraint/TrajectoryConstraint.h" +#include "gtest/gtest.h" +#include "trajectory/TestTrajectory.h" + +using namespace frc; + +TEST(CentripetalAccelerationConstraintTest, Constraint) { + const auto maxCentripetalAcceleration = 7_fps_sq; + + std::vector> constraints; + constraints.emplace_back(std::make_unique( + maxCentripetalAcceleration)); + + auto trajectory = TestTrajectory::GetTrajectory(std::move(constraints)); + + units::second_t time = 0_s; + units::second_t dt = 20_ms; + units::second_t duration = trajectory.TotalTime(); + + while (time < duration) { + const Trajectory::State point = trajectory.Sample(time); + time += dt; + + auto centripetalAcceleration = + units::math::pow<2>(point.velocity) * point.curvature / 1_rad; + + EXPECT_TRUE(centripetalAcceleration < + maxCentripetalAcceleration + 0.05_mps_sq); + } +} diff --git a/wpilibc/src/test/native/cpp/trajectory/DifferentialDriveKinematicsTest.cpp b/wpilibc/src/test/native/cpp/trajectory/DifferentialDriveKinematicsTest.cpp new file mode 100644 index 0000000000..3fe3477e43 --- /dev/null +++ b/wpilibc/src/test/native/cpp/trajectory/DifferentialDriveKinematicsTest.cpp @@ -0,0 +1,47 @@ +/*----------------------------------------------------------------------------*/ +/* 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. */ +/*----------------------------------------------------------------------------*/ + +#include +#include + +#include + +#include "frc/kinematics/DifferentialDriveKinematics.h" +#include "frc/trajectory/constraint/DifferentialDriveKinematicsConstraint.h" +#include "gtest/gtest.h" +#include "trajectory/TestTrajectory.h" + +using namespace frc; + +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 trajectory = TestTrajectory::GetTrajectory(std::move(constraints)); + + units::second_t time = 0_s; + units::second_t dt = 20_ms; + units::second_t duration = trajectory.TotalTime(); + + while (time < duration) { + const Trajectory::State point = trajectory.Sample(time); + time += dt; + + const ChassisSpeeds chassisSpeeds{point.velocity, 0_mps, + point.velocity * point.curvature}; + + auto [left, right] = kinematics.ToWheelSpeeds(chassisSpeeds); + + EXPECT_TRUE(left < maxVelocity + 0.05_mps); + EXPECT_TRUE(right < maxVelocity + 0.05_mps); + } +} diff --git a/wpilibc/src/test/native/cpp/trajectory/TrajectoryGeneratorTest.cpp b/wpilibc/src/test/native/cpp/trajectory/TrajectoryGeneratorTest.cpp new file mode 100644 index 0000000000..b687e67306 --- /dev/null +++ b/wpilibc/src/test/native/cpp/trajectory/TrajectoryGeneratorTest.cpp @@ -0,0 +1,35 @@ +/*----------------------------------------------------------------------------*/ +/* 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. */ +/*----------------------------------------------------------------------------*/ + +#include + +#include "frc/trajectory/Trajectory.h" +#include "frc/trajectory/TrajectoryGenerator.h" +#include "frc/trajectory/constraint/CentripetalAccelerationConstraint.h" +#include "frc/trajectory/constraint/TrajectoryConstraint.h" +#include "gtest/gtest.h" +#include "trajectory/TestTrajectory.h" + +using namespace frc; + +TEST(TrajectoryGenerationTest, ObeysConstraints) { + auto trajectory = TestTrajectory::GetTrajectory( + std::vector>()); + + units::second_t time = 0_s; + units::second_t dt = 20_ms; + units::second_t duration = trajectory.TotalTime(); + + while (time < duration) { + const Trajectory::State point = trajectory.Sample(time); + time += dt; + + EXPECT_TRUE(units::math::abs(point.velocity) <= 12_fps + 0.01_fps); + EXPECT_TRUE(units::math::abs(point.acceleration) <= + 12_fps_sq + 0.01_fps_sq); + } +} diff --git a/wpilibc/src/test/native/include/trajectory/TestTrajectory.h b/wpilibc/src/test/native/include/trajectory/TestTrajectory.h new file mode 100644 index 0000000000..21ce51e429 --- /dev/null +++ b/wpilibc/src/test/native/include/trajectory/TestTrajectory.h @@ -0,0 +1,45 @@ +/*----------------------------------------------------------------------------*/ +/* 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 "frc/trajectory/Trajectory.h" +#include "frc/trajectory/TrajectoryGenerator.h" +#include "frc/trajectory/constraint/TrajectoryConstraint.h" + +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; + + // 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)}; + + auto vector = std::vector{ + (sideStart + Transform2d{Translation2d(-13_ft, 0_ft), Rotation2d()}) + .Translation(), + (sideStart + + 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); + } +}; + +} // namespace frc diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/geometry/Pose2d.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/geometry/Pose2d.java index 982275e9f1..8f819ad475 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/geometry/Pose2d.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/geometry/Pose2d.java @@ -66,6 +66,17 @@ public class Pose2d { return transformBy(other); } + /** + * Returns the Transform2d that maps the one pose to another. + * + * @param other The initial pose of the transformation. + * @return The transform that maps the other pose to the current pose. + */ + public Transform2d minus(Pose2d other) { + final var pose = this.relativeTo(other); + return new Transform2d(pose.getTranslation(), pose.getRotation()); + } + /** * Returns the translation component of the transformation. * @@ -93,7 +104,7 @@ public class Pose2d { */ public Pose2d transformBy(Transform2d other) { return new Pose2d(m_translation.plus(other.getTranslation().rotateBy(m_rotation)), - m_rotation.plus(other.getRotation())); + m_rotation.plus(other.getRotation())); } /** @@ -152,11 +163,39 @@ public class Pose2d { c = (1 - cosTheta) / dtheta; } var transform = new Transform2d(new Translation2d(dx * s - dy * c, dx * c + dy * s), - new Rotation2d(cosTheta, sinTheta)); + new Rotation2d(cosTheta, sinTheta)); return this.plus(transform); } + /** + * Returns a Twist2d that maps this pose to the end pose. If c is the output + * of a.Log(b), then a.Exp(c) would yield b. + * + * @param end The end pose for the transformation. + * @return The twist that maps this to end. + */ + public Twist2d log(Pose2d end) { + final var transform = end.relativeTo(this); + final var dtheta = transform.getRotation().getRadians(); + final var halfDtheta = dtheta / 2.0; + + final var cosMinusOne = transform.getRotation().getCos() - 1; + + double halfThetaByTanOfHalfDtheta; + if (Math.abs(cosMinusOne) < 1E-9) { + halfThetaByTanOfHalfDtheta = 1.0 - 1.0 / 12.0 * dtheta * dtheta; + } else { + halfThetaByTanOfHalfDtheta = -(halfDtheta * transform.getRotation().getSin()) / cosMinusOne; + } + + Translation2d translationPart = transform.getTranslation().rotateBy( + new Rotation2d(halfThetaByTanOfHalfDtheta, -halfDtheta) + ).times(Math.hypot(halfThetaByTanOfHalfDtheta, halfDtheta)); + + return new Twist2d(translationPart.getX(), translationPart.getY(), dtheta); + } + /** * Checks equality between this Pose2d and another object. * diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/geometry/Rotation2d.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/geometry/Rotation2d.java index d6f2548423..7b916c1148 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/geometry/Rotation2d.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/geometry/Rotation2d.java @@ -107,6 +107,16 @@ public class Rotation2d { return new Rotation2d(-m_value); } + /** + * Multiplies the current rotation by a scalar. + * + * @param scalar The scalar. + * @return The new scaled Rotation2d. + */ + public Rotation2d times(double scalar) { + return new Rotation2d(m_value * scalar); + } + /** * Adds the new rotation to the current rotation using a rotation matrix. * diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/geometry/Transform2d.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/geometry/Transform2d.java index 49fa726310..dceb502078 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/geometry/Transform2d.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/geometry/Transform2d.java @@ -51,6 +51,16 @@ public class Transform2d { m_rotation = new Rotation2d(); } + /** + * Scales the transform by the scalar. + * + * @param scalar The scalar. + * @return The scaled Transform2d. + */ + public Transform2d times(double scalar) { + return new Transform2d(m_translation.times(scalar), m_rotation.times(scalar)); + } + /** * Returns the translation component of the transformation. * diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/spline/CubicHermiteSpline.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/spline/CubicHermiteSpline.java new file mode 100644 index 0000000000..5674231d96 --- /dev/null +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/spline/CubicHermiteSpline.java @@ -0,0 +1,104 @@ +/*----------------------------------------------------------------------------*/ +/* 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.spline; + +import org.ejml.simple.SimpleMatrix; + +public class CubicHermiteSpline extends Spline { + private static SimpleMatrix hermiteBasis; + private final SimpleMatrix m_coefficients; + + /** + * Constructs a cubic hermite spline with the specified control vectors. Each + * control vector contains info about the location of the point and its first + * derivative. + * + * @param xInitialControlVector The control vector for the initial point in + * the x dimension. + * @param xFinalControlVector The control vector for the final point in + * the x dimension. + * @param yInitialControlVector The control vector for the initial point in + * the y dimension. + * @param yFinalControlVector The control vector for the final point in + * the y dimension. + */ + @SuppressWarnings("ParameterName") + public CubicHermiteSpline(double[] xInitialControlVector, double[] xFinalControlVector, + double[] yInitialControlVector, double[] yFinalControlVector) { + super(3); + + // Populate the coefficients for the actual spline equations. + // Row 0 is x coefficients + // Row 1 is y coefficients + final var hermite = makeHermiteBasis(); + final var x = getControlVectorFromArrays(xInitialControlVector, xFinalControlVector); + final var y = getControlVectorFromArrays(yInitialControlVector, yFinalControlVector); + + final var xCoeffs = (hermite.mult(x)).transpose(); + final var yCoeffs = (hermite.mult(y)).transpose(); + + m_coefficients = new SimpleMatrix(6, 4); + + for (int i = 0; i < 4; i++) { + m_coefficients.set(0, i, xCoeffs.get(0, i)); + m_coefficients.set(1, i, yCoeffs.get(0, i)); + + // Populate Row 2 and Row 3 with the derivatives of the equations above. + // Then populate row 4 and 5 with the second derivatives. + m_coefficients.set(2, i, m_coefficients.get(0, i) * (3 - i)); + m_coefficients.set(3, i, m_coefficients.get(1, i) * (3 - i)); + m_coefficients.set(4, i, m_coefficients.get(2, i) * (3 - i)); + m_coefficients.set(5, i, m_coefficients.get(3, i) * (3 - i)); + } + + } + + /** + * Returns the coefficients matrix. + * + * @return The coefficients matrix. + */ + @Override + protected SimpleMatrix getCoefficients() { + return m_coefficients; + } + + /** + * Returns the hermite basis matrix for cubic hermite spline interpolation. + * + * @return The hermite basis matrix for cubic hermite spline interpolation. + */ + private SimpleMatrix makeHermiteBasis() { + if (hermiteBasis == null) { + hermiteBasis = new SimpleMatrix(4, 4, true, new double[]{ + +2.0, +1.0, -2.0, +1.0, + -3.0, -2.0, +3.0, -1.0, + +0.0, +1.0, +0.0, +0.0, + +1.0, +0.0, +0.0, +0.0 + }); + } + return hermiteBasis; + } + + /** + * Returns the control vector for each dimension as a matrix from the + * user-provided arrays in the constructor. + * + * @param initialVector The control vector for the initial point. + * @param finalVector The control vector for the final point. + * @return The control vector matrix for a dimension. + */ + private SimpleMatrix getControlVectorFromArrays(double[] initialVector, double[] finalVector) { + if (initialVector.length != 2 || finalVector.length != 2) { + throw new IllegalArgumentException("Size of vectors must be 2"); + } + return new SimpleMatrix(4, 1, true, new double[]{ + initialVector[0], initialVector[1], + finalVector[0], finalVector[1]}); + } +} diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/spline/PoseWithCurvature.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/spline/PoseWithCurvature.java new file mode 100644 index 0000000000..62c1e5a6c9 --- /dev/null +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/spline/PoseWithCurvature.java @@ -0,0 +1,40 @@ +/*----------------------------------------------------------------------------*/ +/* 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.spline; + +import edu.wpi.first.wpilibj.geometry.Pose2d; + +/** + * Represents a pair of a pose and a curvature. + */ +@SuppressWarnings("MemberName") +public class PoseWithCurvature { + // Represents the pose. + public Pose2d poseMeters; + + // Represents the curvature. + public double curvatureRadPerMeter; + + /** + * Constructs a PoseWithCurvature. + * + * @param poseMeters The pose. + * @param curvatureRadPerMeter The curvature. + */ + public PoseWithCurvature(Pose2d poseMeters, double curvatureRadPerMeter) { + this.poseMeters = poseMeters; + this.curvatureRadPerMeter = curvatureRadPerMeter; + } + + /** + * Constructs a PoseWithCurvature with default values. + */ + public PoseWithCurvature() { + poseMeters = new Pose2d(); + } +} diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/spline/QuinticHermiteSpline.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/spline/QuinticHermiteSpline.java new file mode 100644 index 0000000000..562134167d --- /dev/null +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/spline/QuinticHermiteSpline.java @@ -0,0 +1,105 @@ +/*----------------------------------------------------------------------------*/ +/* 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.spline; + +import org.ejml.simple.SimpleMatrix; + +public class QuinticHermiteSpline extends Spline { + private static SimpleMatrix hermiteBasis; + private final SimpleMatrix m_coefficients; + + /** + * Constructs a quintic hermite spline with the specified control vectors. + * Each control vector contains into about the location of the point, its + * first derivative, and its second derivative. + * + * @param xInitialControlVector The control vector for the initial point in + * the x dimension. + * @param xFinalControlVector The control vector for the final point in + * the x dimension. + * @param yInitialControlVector The control vector for the initial point in + * the y dimension. + * @param yFinalControlVector The control vector for the final point in + * the y dimension. + */ + @SuppressWarnings("ParameterName") + public QuinticHermiteSpline(double[] xInitialControlVector, double[] xFinalControlVector, + double[] yInitialControlVector, double[] yFinalControlVector) { + super(5); + + // Populate the coefficients for the actual spline equations. + // Row 0 is x coefficients + // Row 1 is y coefficients + final var hermite = makeHermiteBasis(); + final var x = getControlVectorFromArrays(xInitialControlVector, xFinalControlVector); + final var y = getControlVectorFromArrays(yInitialControlVector, yFinalControlVector); + + final var xCoeffs = (hermite.mult(x)).transpose(); + final var yCoeffs = (hermite.mult(y)).transpose(); + + m_coefficients = new SimpleMatrix(6, 6); + + for (int i = 0; i < 6; i++) { + m_coefficients.set(0, i, xCoeffs.get(0, i)); + m_coefficients.set(1, i, yCoeffs.get(0, i)); + + // Populate Row 2 and Row 3 with the derivatives of the equations above. + // Then populate row 4 and 5 with the second derivatives. + m_coefficients.set(2, i, m_coefficients.get(0, i) * (5 - i)); + m_coefficients.set(3, i, m_coefficients.get(1, i) * (5 - i)); + m_coefficients.set(4, i, m_coefficients.get(2, i) * (5 - i)); + m_coefficients.set(5, i, m_coefficients.get(3, i) * (5 - i)); + } + } + + /** + * Returns the coefficients matrix. + * + * @return The coefficients matrix. + */ + @Override + protected SimpleMatrix getCoefficients() { + return m_coefficients; + } + + /** + * Returns the hermite basis matrix for quintic hermite spline interpolation. + * + * @return The hermite basis matrix for quintic hermite spline interpolation. + */ + private SimpleMatrix makeHermiteBasis() { + if (hermiteBasis == null) { + hermiteBasis = new SimpleMatrix(6, 6, true, new double[]{ + -06.0, -03.0, -00.5, +06.0, -03.0, +00.5, + +15.0, +08.0, +01.5, -15.0, +07.0, +01.0, + -10.0, -06.0, -01.5, +10.0, -04.0, +00.5, + +00.0, +00.0, +00.5, +00.0, +00.0, +00.0, + +00.0, +01.0, +00.0, +00.0, +00.0, +00.0, + +01.0, +00.0, +00.0, +00.0, +00.0, +00.0 + }); + } + return hermiteBasis; + } + + /** + * Returns the control vector for each dimension as a matrix from the + * user-provided arrays in the constructor. + * + * @param initialVector The control vector for the initial point. + * @param finalVector The control vector for the final point. + * @return The control vector matrix for a dimension. + */ + private SimpleMatrix getControlVectorFromArrays(double[] initialVector, double[] finalVector) { + if (initialVector.length != 3 || finalVector.length != 3) { + throw new IllegalArgumentException("Size of vectors must be 3"); + } + return new SimpleMatrix(6, 1, true, new double[]{ + initialVector[0], initialVector[1], initialVector[2], + finalVector[0], finalVector[1], finalVector[2]}); + } +} 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 new file mode 100644 index 0000000000..448b6858a5 --- /dev/null +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/spline/Spline.java @@ -0,0 +1,91 @@ +/*----------------------------------------------------------------------------*/ +/* 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.spline; + +import org.ejml.simple.SimpleMatrix; + +import edu.wpi.first.wpilibj.geometry.Pose2d; +import edu.wpi.first.wpilibj.geometry.Rotation2d; + +/** + * Represents a two-dimensional parametric spline that interpolates between two + * points. + */ +public abstract class Spline { + private final int m_degree; + + /** + * Constructs a spline with the given degree. + * + * @param degree The degree of the spline. + */ + Spline(int degree) { + m_degree = degree; + } + + /** + * Returns the coefficients of the spline. + * + * @return The coefficients of the spline. + */ + protected abstract SimpleMatrix getCoefficients(); + + /** + * Gets the pose and curvature at some point t on the spline. + * + * @param t The point t + * @return The pose and curvature at that point. + */ + @SuppressWarnings("ParameterName") + public PoseWithCurvature getPoint(double t) { + SimpleMatrix polynomialBases = new SimpleMatrix(m_degree + 1, 1); + final var coefficients = getCoefficients(); + + // Populate the polynomial bases. + for (int i = 0; i <= m_degree; i++) { + polynomialBases.set(i, 0, Math.pow(t, m_degree - i)); + } + + // This simply multiplies by the coefficients. We need to divide out t some + // n number of times where n is the derivative we want to take. + SimpleMatrix combined = coefficients.mult(polynomialBases); + + // Get x and y + final double x = combined.get(0, 0); + final double y = combined.get(1, 0); + + double dx; + double dy; + double ddx; + double ddy; + + if (t == 0) { + dx = coefficients.get(2, m_degree - 1); + dy = coefficients.get(3, m_degree - 1); + ddx = coefficients.get(4, m_degree - 2); + ddy = coefficients.get(5, m_degree - 2); + } else { + // Divide out t once for first derivative. + dx = combined.get(2, 0) / t; + dy = combined.get(3, 0) / t; + + // Divide out t twice for second derivative. + ddx = combined.get(4, 0) / t / t; + ddy = combined.get(5, 0) / t / t; + } + + // Find the curvature. + final double curvature = + (dx * ddy - ddx * dy) / ((dx * dx + dy * dy) * Math.hypot(dx, dy)); + + return new PoseWithCurvature( + new Pose2d(x, y, new Rotation2d(dx, dy)), + curvature + ); + } +} 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 new file mode 100644 index 0000000000..a18c4900e6 --- /dev/null +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/spline/SplineHelper.java @@ -0,0 +1,221 @@ +/*----------------------------------------------------------------------------*/ +/* 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.spline; + +import java.util.Arrays; + +import edu.wpi.first.wpilibj.geometry.Pose2d; +import edu.wpi.first.wpilibj.geometry.Translation2d; + +public final class SplineHelper { + /** + * Private constructor because this is a utility class. + */ + private SplineHelper() { + } + + /** + * 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. + * + * @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. + * @return A vector of cubic hermite splines that interpolate through the + * provided waypoints. + */ + @SuppressWarnings({"LocalVariableName", "PMD.ExcessiveMethodLength", + "PMD.AvoidInstantiatingObjectsInLoops"}) + public static CubicHermiteSpline[] getCubicSplinesFromWaypoints( + Pose2d start, Translation2d[] waypoints, Pose2d 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()}; + + 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(); + System.arraycopy(waypoints, 0, newWaypts, 1, waypoints.length); + newWaypts[newWaypts.length - 1] = end.getTranslation(); + + final double[] a = new double[1 + newWaypts.length - 3]; + + final double[] b = new double[newWaypts.length - 2]; + Arrays.fill(b, 4.0); + + final double[] c = new double[1 + newWaypts.length - 3]; + + final double[] dx = new double[2 + newWaypts.length - 4]; + final double[] dy = new double[2 + newWaypts.length - 4]; + + final double[] fx = new double[newWaypts.length - 2]; + final double[] fy = new double[newWaypts.length - 2]; + + a[0] = 0.0; + for (int i = 0; i < newWaypts.length - 3; i++) { + a[i + 1] = 1; + c[i] = 1; + } + 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]; + + if (newWaypts.length > 4) { + for (int i = 1; i <= newWaypts.length; i++) { + dx[i] = newWaypts[i + 1].getX() - newWaypts[i - 1].getX(); + dy[i] = newWaypts[i + 1].getY() - newWaypts[i - 1].getY(); + } + } + + dx[dx.length - 1] = 3 * (newWaypts[newWaypts.length - 1].getX() + - newWaypts[newWaypts.length - 3].getX()) - xFinalControlVector[1]; + dy[dy.length - 1] = 3 * (newWaypts[newWaypts.length - 1].getY() + - newWaypts[newWaypts.length - 3].getY()) - yFinalControlVector[1]; + + thomasAlgorithm(a, b, c, dx, fx); + thomasAlgorithm(a, b, c, dy, fy); + + double[] newFx = new double[fx.length + 2]; + double[] newFy = new double[fy.length + 2]; + + newFx[0] = xInitialControlVector[1]; + newFy[0] = yInitialControlVector[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]; + + for (int i = 0; i < newFx.length - 1; i++) { + splines[i] = new CubicHermiteSpline( + new double[]{newWaypts[i].getX(), newFx[i]}, + new double[]{newWaypts[i + 1].getX(), newFx[i + 1]}, + new double[]{newWaypts[i].getY(), newFy[i]}, + new double[]{newWaypts[i + 1].getY(), newFy[i + 1]} + ); + } + } else if (waypoints.length == 1) { + final var xDeriv = (3 * (end.getTranslation().getX() + - start.getTranslation().getX()) + - xFinalControlVector[1] - xInitialControlVector[1]) + / 4.0; + final var yDeriv = (3 * (end.getTranslation().getY() + - start.getTranslation().getY()) + - yFinalControlVector[1] - yInitialControlVector[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); + } else { + splines[0] = new CubicHermiteSpline(xInitialControlVector, xFinalControlVector, + yInitialControlVector, yFinalControlVector); + } + 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 + * curvature is guaranteed throughout the path. + * + * @param waypoints The waypoints. + * @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); + } + return splines; + } + + /** + * Thomas algorithm for solving tridiagonal systems Af = d. + * + * @param a the values of A above the diagonal + * @param b the values of A on the diagonal + * @param c the values of A below the diagonal + * @param d the vector on the rhs + * @param solutionVector the unknown (solution) vector, modified in-place + */ + @SuppressWarnings({"ParameterName", "LocalVariableName"}) + private static void thomasAlgorithm(double[] a, double[] b, + double[] c, double[] d, double[] solutionVector) { + int N = d.length; + + double[] cStar = new double[N]; + double[] dStar = new double[N]; + + // This updates the coefficients in the first row + // Note that we should be checking for division by zero here + cStar[0] = c[0] / b[0]; + dStar[0] = d[0] / b[0]; + + // Create the c_star and d_star coefficients in the forward sweep + for (int i = 1; i < N; i++) { + double m = 1.0 / (b[i] - a[i] * cStar[i - 1]); + cStar[i] = c[i] * m; + dStar[i] = (d[i] - a[i] * dStar[i - 1]) * m; + } + solutionVector[N - 1] = dStar[N - 1]; + // This is the reverse sweep, used to update the solution vector f + for (int i = N - 2; i >= 0; i--) { + solutionVector[i] = dStar[i] - cStar[i] * solutionVector[i + 1]; + } + } +} diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/spline/SplineParameterizer.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/spline/SplineParameterizer.java new file mode 100644 index 0000000000..b78227d64c --- /dev/null +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/spline/SplineParameterizer.java @@ -0,0 +1,106 @@ +/*----------------------------------------------------------------------------*/ +/* 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. */ +/*----------------------------------------------------------------------------*/ + +/* + * MIT License + * + * Copyright (c) 2018 Team 254 + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +package edu.wpi.first.wpilibj.spline; + +import java.util.ArrayList; +import java.util.List; + +/** + * Class used to parameterize a spline by its arc length. + */ +public final class SplineParameterizer { + private static final double kMaxDx = 0.127; + private static final double kMaxDy = 0.00127; + private static final double kMaxDtheta = 0.0872; + + /** + * Private constructor because this is a utility class. + */ + private SplineParameterizer() { + } + + /** + * Parameterizes the spline. This method breaks up the spline into various + * arcs until their dx, dy, and dtheta are within specific tolerances. + * + * @param spline The spline to parameterize. + * @return A vector of poses and curvatures that represents various points on the spline. + */ + public static List parameterize(Spline spline) { + return parameterize(spline, 0.0, 1.0); + } + + /** + * Parameterizes the spline. This method breaks up the spline into various + * arcs until their dx, dy, and dtheta are within specific tolerances. + * + * @param spline The spline to parameterize. + * @param t0 Starting internal spline parameter. It is recommended to use 0.0. + * @param t1 Ending internal spline parameter. It is recommended to use 1.0. + * @return A vector of poses and curvatures that represents various points on the spline. + */ + public static List parameterize(Spline spline, double t0, double t1) { + var arr = new ArrayList(); + + // The parameterization does not add the first initial point. Let's add + // that. + arr.add(spline.getPoint(t0)); + + getSegmentArc(spline, arr, t0, t1); + return arr; + } + + /** + * Breaks up the spline into arcs until the dx, dy, and theta of each arc is + * within tolerance. + * + * @param spline The spline to parameterize. + * @param vector Pointer to vector of poses. + * @param t0 Starting point for arc. + * @param t1 Ending point for arc. + */ + private static void getSegmentArc(Spline spline, List vector, + double t0, double t1) { + final var start = spline.getPoint(t0); + final var end = spline.getPoint(t1); + + final var twist = start.poseMeters.log(end.poseMeters); + + if (Math.abs(twist.dy) > kMaxDy || Math.abs(twist.dx) > kMaxDx + || Math.abs(twist.dtheta) > kMaxDtheta) { + getSegmentArc(spline, vector, t0, (t0 + t1) / 2); + getSegmentArc(spline, vector, (t0 + t1) / 2, t1); + } else { + vector.add(spline.getPoint(t1)); + } + } +} diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/trajectory/Trajectory.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/trajectory/Trajectory.java new file mode 100644 index 0000000000..aa92831481 --- /dev/null +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/trajectory/Trajectory.java @@ -0,0 +1,223 @@ +/*----------------------------------------------------------------------------*/ +/* 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.List; + +import edu.wpi.first.wpilibj.geometry.Pose2d; + +/** + * Represents a time-parameterized trajectory. The trajectory contains of + * various States that represent the pose, curvature, time elapsed, velocity, + * and acceleration at that point. + */ +public class Trajectory { + private final double m_totalTimeSeconds; + private final List m_states; + + /** + * Constructs a trajectory from a vector of states. + * + * @param states A vector of states. + */ + public Trajectory(final List states) { + m_states = states; + m_totalTimeSeconds = m_states.get(m_states.size() - 1).timeSeconds; + } + + /** + * Linearly interpolates between two values. + * + * @param startValue The start value. + * @param endValue The end value. + * @param t The fraction for interpolation. + * @return The interpolated value. + */ + @SuppressWarnings("ParameterName") + private static double lerp(double startValue, double endValue, double t) { + return startValue + (endValue - startValue) * t; + } + + /** + * Linearly interpolates between two poses. + * + * @param startValue The start pose. + * @param endValue The end pose. + * @param t The fraction for interpolation. + * @return The interpolated pose. + */ + @SuppressWarnings("ParameterName") + private static Pose2d lerp(Pose2d startValue, Pose2d endValue, double t) { + return startValue.plus((endValue.minus(startValue)).times(t)); + } + + /** + * Returns the overall duration of the trajectory. + * + * @return The duration of the trajectory. + */ + public double getTotalTimeSeconds() { + return m_totalTimeSeconds; + } + + /** + * Return the states of the trajectory. + * + * @return The states of the trajectory. + */ + public List getStates() { + return m_states; + } + + /** + * Sample the trajectory at a point in time. + * + * @param timeSeconds The point in time since the beginning of the trajectory to sample. + * @return The state at that point in time. + */ + public State sample(double timeSeconds) { + if (timeSeconds <= m_states.get(0).timeSeconds) { + return m_states.get(0); + } + if (timeSeconds >= m_totalTimeSeconds) { + return m_states.get(m_states.size() - 1); + } + + // To get the element that we want, we will use a binary search algorithm + // instead of iterating over a for-loop. A binary search is O(std::log(n)) + // whereas searching using a loop is O(n). + + // This starts at 1 because we use the previous state later on for + // interpolation. + int low = 1; + int high = m_states.size() - 1; + + while (low != high) { + int mid = (low + high) / 2; + if (m_states.get(mid).timeSeconds < timeSeconds) { + // This index and everything under it are less than the requested + // timestamp. Therefore, we can discard them. + low = mid + 1; + } else { + // t is at least as large as the element at this index. This means that + // anything after it cannot be what we are looking for. + high = mid; + } + } + + // High and Low should be the same. + + // The sample's timestamp is now greater than or equal to the requested + // timestamp. If it is greater, we need to interpolate between the + // previous state and the current state to get the exact state that we + // want. + final State sample = m_states.get(low); + final State prevSample = m_states.get(low - 1); + + // If the difference in states is negligible, then we are spot on! + if (Math.abs(sample.timeSeconds - prevSample.timeSeconds) < 1E-9) { + return sample; + } + // Interpolate between the two states for the state that we want. + return prevSample.interpolate(sample, + (timeSeconds - prevSample.timeSeconds) / (sample.timeSeconds - prevSample.timeSeconds)); + } + + /** + * Represents a time-parameterized trajectory. The trajectory contains of + * various States that represent the pose, curvature, time elapsed, velocity, + * and acceleration at that point. + */ + @SuppressWarnings("MemberName") + public static class State { + // The time elapsed since the beginning of the trajectory. + public double timeSeconds; + + // The speed at that point of the trajectory. + public double velocityMetersPerSecond; + + // The acceleration at that point of the trajectory. + public double accelerationMetersPerSecondSq; + + // The pose at that point of the trajectory. + public Pose2d poseMeters; + + // The curvature at that point of the trajectory. + public double curvatureRadPerMeter; + + public State() { + poseMeters = new Pose2d(); + } + + /** + * Constructs a State with the specified parameters. + * + * @param timeSeconds The time elapsed since the beginning of the trajectory. + * @param velocityMetersPerSecond The speed at that point of the trajectory. + * @param accelerationMetersPerSecondSq The acceleration at that point of the trajectory. + * @param poseMeters The pose at that point of the trajectory. + * @param curvatureRadPerMeter The curvature at that point of the trajectory. + */ + public State(double timeSeconds, double velocityMetersPerSecond, + double accelerationMetersPerSecondSq, Pose2d poseMeters, + double curvatureRadPerMeter) { + this.timeSeconds = timeSeconds; + this.velocityMetersPerSecond = velocityMetersPerSecond; + this.accelerationMetersPerSecondSq = accelerationMetersPerSecondSq; + this.poseMeters = poseMeters; + this.curvatureRadPerMeter = curvatureRadPerMeter; + } + + /** + * Interpolates between two States. + * + * @param endValue The end value for the interpolation. + * @param i The interpolant (fraction). + * @return The interpolated state. + */ + @SuppressWarnings("ParameterName") + State interpolate(State endValue, double i) { + // Find the new t value. + final double newT = lerp(timeSeconds, endValue.timeSeconds, i); + + // Find the delta time between the current state and the interpolated state. + final double deltaT = newT - timeSeconds; + + // If delta time is negative, flip the order of interpolation. + if (deltaT < 0) { + return endValue.interpolate(this, 1 - i); + } + + // Check whether the robot is reversing at this stage. + final boolean reversing = velocityMetersPerSecond < 0 + || Math.abs(velocityMetersPerSecond) < 1E-9 && accelerationMetersPerSecondSq < 0; + + // Calculate the new velocity + // v_f = v_0 + at + final double newV = velocityMetersPerSecond + (accelerationMetersPerSecondSq * deltaT); + + // Calculate the change in position. + // delta_s = v_0 t + 0.5 at^2 + final double newS = (velocityMetersPerSecond * deltaT + + 0.5 * accelerationMetersPerSecondSq * Math.pow(deltaT, 2)) * (reversing ? -1.0 : 1.0); + + // Return the new state. To find the new position for the new state, we need + // to interpolate between the two endpoint poses. The fraction for + // interpolation is the change in position (delta s) divided by the total + // distance between the two endpoints. + final double interpolationFrac = newS + / endValue.poseMeters.getTranslation().getDistance(poseMeters.getTranslation()); + + return new State( + newT, newV, accelerationMetersPerSecondSq, + lerp(poseMeters, endValue.poseMeters, interpolationFrac), + lerp(curvatureRadPerMeter, endValue.curvatureRadPerMeter, interpolationFrac) + ); + } + } +} 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 new file mode 100644 index 0000000000..9e73c14f4c --- /dev/null +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/trajectory/TrajectoryGenerator.java @@ -0,0 +1,154 @@ +/*----------------------------------------------------------------------------*/ +/* 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.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.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.TrajectoryConstraint; + +public final class TrajectoryGenerator { + /** + * Private constructor because this is a utility class. + */ + private TrajectoryGenerator() { + } + + /** + * Generates a trajectory with the given waypoints and constraints. + * + * @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. + */ + + public static Trajectory generateTrajectory( + List waypoints, + List constraints, + double startVelocityMetersPerSecond, + double endVelocityMetersPerSecond, + double maxVelocityMetersPerSecond, + double maxAccelerationMetersPerSecondSq, + boolean reversed + ) { + 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); + } + + var points = splinePointsFromSplines(SplineHelper.getQuinticSplinesFromWaypoints( + newWaypoints.toArray(new Pose2d[0]) + )); + + // After trajectory generation, flip theta back so it's relative to the + // field. Also fix curvature. + if (reversed) { + for (var point : points) { + point.poseMeters = point.poseMeters.plus(flip); + point.curvatureRadPerMeter *= -1; + } + } + + return TrajectoryParameterizer.timeParameterizeTrajectory(points, constraints, + startVelocityMetersPerSecond, endVelocityMetersPerSecond, maxVelocityMetersPerSecond, + maxAccelerationMetersPerSecondSq, reversed); + } + + /** + * Generates a trajectory with the given waypoints and constraints. + * + * @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. + */ + public static Trajectory generateTrajectory( + Pose2d start, + List waypoints, + Pose2d end, + List constraints, + double startVelocityMetersPerSecond, + double endVelocityMetersPerSecond, + double maxVelocityMetersPerSecond, + double maxAccelerationMetersPerSecondSq, + boolean reversed + ) { + final var flip = new Transform2d(new Translation2d(), Rotation2d.fromDegrees(-180.0)); + + final var newStart = reversed ? start.plus(flip) : start; + final var newEnd = reversed ? end.plus(flip) : end; + + var points = splinePointsFromSplines(SplineHelper.getCubicSplinesFromWaypoints( + newStart, waypoints.toArray(new Translation2d[0]), newEnd + )); + + // After trajectory generation, flip theta back so it's relative to the + // field. Also fix curvature. + if (reversed) { + for (var point : points) { + point.poseMeters = point.poseMeters.plus(flip); + point.curvatureRadPerMeter *= -1; + } + } + + return TrajectoryParameterizer.timeParameterizeTrajectory(points, constraints, + startVelocityMetersPerSecond, endVelocityMetersPerSecond, maxVelocityMetersPerSecond, + maxAccelerationMetersPerSecondSq, reversed); + } + + private static List splinePointsFromSplines( + Spline[] splines) { + // Create the vector of spline points. + var splinePoints = new ArrayList(); + + // Add the first point to the vector. + splinePoints.add(splines[0].getPoint(0.0)); + + // Iterate through the vector and parameterize each spline, adding the + // parameterized points to the final vector. + for (final var spline : splines) { + var points = SplineParameterizer.parameterize(spline); + + // Append the array of poses to the vector. We are removing the first + // point because it's a duplicate of the last point from the previous + // spline. + splinePoints.addAll(points.subList(1, points.size())); + } + return splinePoints; + } +} diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/trajectory/TrajectoryParameterizer.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/trajectory/TrajectoryParameterizer.java new file mode 100644 index 0000000000..d7b0ceead7 --- /dev/null +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/trajectory/TrajectoryParameterizer.java @@ -0,0 +1,302 @@ +/*----------------------------------------------------------------------------*/ +/* 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. */ +/*----------------------------------------------------------------------------*/ + +/* + * MIT License + * + * Copyright (c) 2018 Team 254 + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +package edu.wpi.first.wpilibj.trajectory; + +import java.util.ArrayList; +import java.util.List; + +import edu.wpi.first.wpilibj.spline.PoseWithCurvature; +import edu.wpi.first.wpilibj.trajectory.constraint.TrajectoryConstraint; + +/** + * Class used to parameterize a trajectory by time. + */ +public final class TrajectoryParameterizer { + /** + * Private constructor because this is a utility class. + */ + private TrajectoryParameterizer() { + } + + /** + * Parameterize the trajectory by time. This is where the velocity profile is + * generated. + * + *

The derivation of the algorithm used can be found + * + * here. + * + * @param points Reference to the spline points. + * @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. + */ + @SuppressWarnings({"PMD.ExcessiveMethodLength", "PMD.CyclomaticComplexity", + "PMD.NPathComplexity", "PMD.AvoidInstantiatingObjectsInLoops", + "PMD.AvoidThrowingRawExceptionTypes"}) + public static Trajectory timeParameterizeTrajectory( + List points, + List constraints, + double startVelocityMetersPerSecond, + double endVelocityMetersPerSecond, + double maxVelocityMetersPerSecond, + double maxAccelerationMetersPerSecondSq, + boolean reversed + ) { + var constrainedStates = new ArrayList(points.size()); + var predecessor = new ConstrainedState(points.get(0), 0, startVelocityMetersPerSecond, + -maxAccelerationMetersPerSecondSq, maxAccelerationMetersPerSecondSq); + + // Forward pass + for (int i = 0; i < points.size(); i++) { + constrainedStates.add(new ConstrainedState()); + var constrainedState = constrainedStates.get(i); + constrainedState.pose = points.get(i); + + // Begin constraining based on predecessor. + double ds = constrainedState.pose.poseMeters.getTranslation().getDistance( + predecessor.pose.poseMeters.getTranslation()); + constrainedState.distanceMeters = predecessor.distanceMeters + ds; + + // We may need to iterate to find the maximum end velocity and common + // acceleration, since acceleration limits may be a function of velocity. + while (true) { + // Enforce global max velocity and max reachable velocity by global + // acceleration limit. vf = std::sqrt(vi^2 + 2*a*d). + constrainedState.maxVelocityMetersPerSecond = Math.min( + maxVelocityMetersPerSecond, + Math.sqrt(predecessor.maxVelocityMetersPerSecond + * predecessor.maxVelocityMetersPerSecond + + predecessor.maxAccelerationMetersPerSecondSq * ds * 2.0) + ); + + constrainedState.minAccelerationMetersPerSecondSq = -maxAccelerationMetersPerSecondSq; + constrainedState.maxAccelerationMetersPerSecondSq = maxAccelerationMetersPerSecondSq; + + // At this point, the constrained state is fully constructed apart from + // all the custom-defined user constraints. + for (final var constraint : constraints) { + constrainedState.maxVelocityMetersPerSecond = Math.min( + constrainedState.maxVelocityMetersPerSecond, + constraint.getMaxVelocityMetersPerSecond( + constrainedState.pose.poseMeters, constrainedState.pose.curvatureRadPerMeter, + constrainedState.maxVelocityMetersPerSecond) + ); + } + + // Now enforce all acceleration limits. + enforceAccelerationLimits(reversed, constraints, constrainedState); + + if (ds < 1E-6) { + break; + } + + // If the actual acceleration for this state is higher than the max + // acceleration that we applied, then we need to reduce the max + // acceleration of the predecessor and try again. + double actualAcceleration = (constrainedState.maxVelocityMetersPerSecond + * constrainedState.maxVelocityMetersPerSecond + - predecessor.maxVelocityMetersPerSecond * predecessor.maxVelocityMetersPerSecond) + / (ds * 2.0); + + // If we violate the max acceleration constraint, let's modify the + // predecessor. + if (constrainedState.maxAccelerationMetersPerSecondSq < actualAcceleration - 1E-6) { + predecessor.maxAccelerationMetersPerSecondSq + = constrainedState.maxAccelerationMetersPerSecondSq; + } else { + // Constrain the predecessor's max acceleration to the current + // acceleration. + if (actualAcceleration > predecessor.minAccelerationMetersPerSecondSq) { + predecessor.maxAccelerationMetersPerSecondSq = actualAcceleration; + } + // If the actual acceleration is less than the predecessor's min + // acceleration, it will be repaired in the backward pass. + break; + } + } + predecessor = constrainedState; + } + + var successor = new ConstrainedState(points.get(points.size() - 1), + constrainedStates.get(constrainedStates.size() - 1).distanceMeters, + endVelocityMetersPerSecond, + -maxAccelerationMetersPerSecondSq, maxAccelerationMetersPerSecondSq); + + // Backward pass + for (int i = points.size() - 1; i >= 0; i--) { + var constrainedState = constrainedStates.get(i); + double ds = constrainedState.distanceMeters - successor.distanceMeters; // negative + + while (true) { + // Enforce max velocity limit (reverse) + // vf = std::sqrt(vi^2 + 2*a*d), where vi = successor. + double newMaxVelocity = Math.sqrt( + successor.maxVelocityMetersPerSecond * successor.maxVelocityMetersPerSecond + + successor.minAccelerationMetersPerSecondSq * ds * 2.0 + ); + + // No more limits to impose! This state can be finalized. + if (newMaxVelocity >= constrainedState.maxVelocityMetersPerSecond) { + break; + } + + constrainedState.maxVelocityMetersPerSecond = newMaxVelocity; + + // Check all acceleration constraints with the new max velocity. + enforceAccelerationLimits(reversed, constraints, constrainedState); + + if (ds > -1E-6) { + break; + } + + // If the actual acceleration for this state is lower than the min + // acceleration, then we need to lower the min acceleration of the + // successor and try again. + double actualAcceleration = (constrainedState.maxVelocityMetersPerSecond + * constrainedState.maxVelocityMetersPerSecond + - successor.maxVelocityMetersPerSecond * successor.maxVelocityMetersPerSecond) + / (ds * 2.0); + + if (constrainedState.minAccelerationMetersPerSecondSq > actualAcceleration + 1E-6) { + successor.minAccelerationMetersPerSecondSq + = constrainedState.minAccelerationMetersPerSecondSq; + } else { + successor.minAccelerationMetersPerSecondSq = actualAcceleration; + break; + } + } + successor = constrainedState; + } + + // Now we can integrate the constrained states forward in time to obtain our + // trajectory states. + var states = new ArrayList(points.size()); + double timeSeconds = 0.0; + double distanceMeters = 0.0; + double velocityMetersPerSecond = 0.0; + + for (int i = 0; i < constrainedStates.size(); i++) { + final var state = constrainedStates.get(i); + + // Calculate the change in position between the current state and the previous + // state. + double ds = state.distanceMeters - distanceMeters; + + // Calculate the acceleration between the current state and the previous + // state. + double accel = (state.maxVelocityMetersPerSecond * state.maxVelocityMetersPerSecond + - velocityMetersPerSecond * velocityMetersPerSecond) / (ds * 2); + + // Calculate dt + double dt = 0.0; + if (i > 0) { + states.get(i - 1).accelerationMetersPerSecondSq = reversed ? -accel : accel; + if (Math.abs(accel) > 1E-6) { + // v_f = v_0 + a * t + dt = (state.maxVelocityMetersPerSecond - velocityMetersPerSecond) / accel; + } else if (Math.abs(velocityMetersPerSecond) > 1E-6) { + // delta_x = v * t + dt = ds / velocityMetersPerSecond; + } else { + throw new RuntimeException("Something went wrong"); + } + } + + velocityMetersPerSecond = state.maxVelocityMetersPerSecond; + distanceMeters = state.distanceMeters; + + timeSeconds += dt; + + states.add(new Trajectory.State( + timeSeconds, + reversed ? -velocityMetersPerSecond : velocityMetersPerSecond, + reversed ? -accel : accel, + state.pose.poseMeters, state.pose.curvatureRadPerMeter + )); + } + + return new Trajectory(states); + } + + private static void enforceAccelerationLimits(boolean reverse, + List constraints, + ConstrainedState state) { + + for (final var constraint : constraints) { + double factor = reverse ? -1.0 : 1.0; + final var minMaxAccel = constraint.getMinMaxAccelerationMetersPerSecondSq( + state.pose.poseMeters, state.pose.curvatureRadPerMeter, + state.maxVelocityMetersPerSecond * factor); + + state.minAccelerationMetersPerSecondSq = Math.max(state.minAccelerationMetersPerSecondSq, + reverse ? -minMaxAccel.maxAccelerationMetersPerSecondSq + : minMaxAccel.minAccelerationMetersPerSecondSq); + + state.maxAccelerationMetersPerSecondSq = Math.min(state.maxAccelerationMetersPerSecondSq, + reverse ? -minMaxAccel.minAccelerationMetersPerSecondSq + : minMaxAccel.maxAccelerationMetersPerSecondSq); + } + + } + + @SuppressWarnings("MemberName") + private static class ConstrainedState { + PoseWithCurvature pose; + double distanceMeters; + double maxVelocityMetersPerSecond; + double minAccelerationMetersPerSecondSq; + double maxAccelerationMetersPerSecondSq; + + ConstrainedState(PoseWithCurvature pose, double distanceMeters, + double maxVelocityMetersPerSecond, + double minAccelerationMetersPerSecondSq, + double maxAccelerationMetersPerSecondSq) { + this.pose = pose; + this.distanceMeters = distanceMeters; + this.maxVelocityMetersPerSecond = maxVelocityMetersPerSecond; + this.minAccelerationMetersPerSecondSq = minAccelerationMetersPerSecondSq; + this.maxAccelerationMetersPerSecondSq = maxAccelerationMetersPerSecondSq; + } + + ConstrainedState() { + pose = new PoseWithCurvature(); + } + } +} diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/trajectory/constraint/CentripetalAccelerationConstraint.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/trajectory/constraint/CentripetalAccelerationConstraint.java new file mode 100644 index 0000000000..f74e7b4246 --- /dev/null +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/trajectory/constraint/CentripetalAccelerationConstraint.java @@ -0,0 +1,73 @@ +/*----------------------------------------------------------------------------*/ +/* 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.constraint; + +import edu.wpi.first.wpilibj.geometry.Pose2d; + +/** + * A constraint on the maximum absolute centripetal acceleration allowed when + * traversing a trajectory. The centripetal acceleration of a robot is defined + * as the velocity squared divided by the radius of curvature. + * + *

Effectively, limiting the maximum centripetal acceleration will cause the + * robot to slow down around tight turns, making it easier to track trajectories + * with sharp turns. + */ +public class CentripetalAccelerationConstraint implements TrajectoryConstraint { + private final double m_maxCentripetalAccelerationMetersPerSecondSq; + + /** + * Constructs a centripetal acceleration constraint. + * + * @param maxCentripetalAccelerationMetersPerSecondSq The max centripetal acceleration. + */ + public CentripetalAccelerationConstraint(double maxCentripetalAccelerationMetersPerSecondSq) { + m_maxCentripetalAccelerationMetersPerSecondSq = maxCentripetalAccelerationMetersPerSecondSq; + } + + /** + * Returns the max velocity given the current pose and curvature. + * + * @param poseMeters The pose at the current point in the trajectory. + * @param curvatureRadPerMeter The curvature at the current point in the trajectory. + * @param velocityMetersPerSecond The velocity at the current point in the trajectory before + * constraints are applied. + * @return The absolute maximum velocity. + */ + @Override + public double getMaxVelocityMetersPerSecond(Pose2d poseMeters, double curvatureRadPerMeter, + double velocityMetersPerSecond) { + // ac = v^2 / r + // k (curvature) = 1 / r + + // therefore, ac = v^2 * k + // ac / k = v^2 + // v = std::sqrt(ac / k) + + return Math.sqrt(m_maxCentripetalAccelerationMetersPerSecondSq + / Math.abs(curvatureRadPerMeter)); + } + + /** + * Returns the minimum and maximum allowable acceleration for the trajectory + * given pose, curvature, and speed. + * + * @param poseMeters The pose at the current point in the trajectory. + * @param curvatureRadPerMeter The curvature at the current point in the trajectory. + * @param velocityMetersPerSecond The speed at the current point in the trajectory. + * @return The min and max acceleration bounds. + */ + @Override + public MinMax getMinMaxAccelerationMetersPerSecondSq(Pose2d poseMeters, + double curvatureRadPerMeter, + double velocityMetersPerSecond) { + // The acceleration of the robot has no impact on the centripetal acceleration + // of the robot. + return new MinMax(); + } +} diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/trajectory/constraint/DifferentialDriveKinematicsConstraint.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/trajectory/constraint/DifferentialDriveKinematicsConstraint.java new file mode 100644 index 0000000000..274bdfbe5d --- /dev/null +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/trajectory/constraint/DifferentialDriveKinematicsConstraint.java @@ -0,0 +1,75 @@ +/*----------------------------------------------------------------------------*/ +/* 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.constraint; + +import edu.wpi.first.wpilibj.geometry.Pose2d; +import edu.wpi.first.wpilibj.kinematics.ChassisSpeeds; +import edu.wpi.first.wpilibj.kinematics.DifferentialDriveKinematics; + +/** + * A class that enforces constraints on the differential drive kinematics. + * This can be used to ensure that the trajectory is constructed so that the + * commanded velocities for both sides of the drivetrain stay below a certain + * limit. + */ +public class DifferentialDriveKinematicsConstraint implements TrajectoryConstraint { + private final double m_maxSpeedMetersPerSecond; + private final DifferentialDriveKinematics m_kinematics; + + /** + * Constructs a differential drive dynamics constraint. + * + * @param maxSpeedMetersPerSecond The max speed that a side of the robot can travel at. + */ + public DifferentialDriveKinematicsConstraint(final DifferentialDriveKinematics kinematics, + double maxSpeedMetersPerSecond) { + m_maxSpeedMetersPerSecond = maxSpeedMetersPerSecond; + m_kinematics = kinematics; + } + + + /** + * Returns the max velocity given the current pose and curvature. + * + * @param poseMeters The pose at the current point in the trajectory. + * @param curvatureRadPerMeter The curvature at the current point in the trajectory. + * @param velocityMetersPerSecond The velocity at the current point in the trajectory before + * constraints are applied. + * @return The absolute maximum velocity. + */ + @Override + public double getMaxVelocityMetersPerSecond(Pose2d poseMeters, double curvatureRadPerMeter, + double velocityMetersPerSecond) { + // Create an object to represent the current chassis speeds. + var chassisSpeeds = new ChassisSpeeds(velocityMetersPerSecond, + 0, velocityMetersPerSecond * curvatureRadPerMeter); + + // Get the wheel speeds and normalize them to within the max velocity. + var wheelSpeeds = m_kinematics.toWheelSpeeds(chassisSpeeds); + wheelSpeeds.normalize(m_maxSpeedMetersPerSecond); + + // Return the new linear chassis speed. + return m_kinematics.toChassisSpeeds(wheelSpeeds).vxMetersPerSecond; + } + + /** + * Returns the minimum and maximum allowable acceleration for the trajectory + * given pose, curvature, and speed. + * + * @param poseMeters The pose at the current point in the trajectory. + * @param curvatureRadPerMeter The curvature at the current point in the trajectory. + * @param velocityMetersPerSecond The speed at the current point in the trajectory. + * @return The min and max acceleration bounds. + */ + @Override + public MinMax getMinMaxAccelerationMetersPerSecondSq(Pose2d poseMeters, + double curvatureRadPerMeter, + double velocityMetersPerSecond) { + return new MinMax(); + } +} diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/trajectory/constraint/TrajectoryConstraint.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/trajectory/constraint/TrajectoryConstraint.java new file mode 100644 index 0000000000..b338c3fdf0 --- /dev/null +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/trajectory/constraint/TrajectoryConstraint.java @@ -0,0 +1,68 @@ +/*----------------------------------------------------------------------------*/ +/* 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.constraint; + +import edu.wpi.first.wpilibj.geometry.Pose2d; + +/** + * An interface for defining user-defined velocity and acceleration constraints + * while generating trajectories. + */ +public interface TrajectoryConstraint { + + /** + * Returns the max velocity given the current pose and curvature. + * + * @param poseMeters The pose at the current point in the trajectory. + * @param curvatureRadPerMeter The curvature at the current point in the trajectory. + * @param velocityMetersPerSecond The velocity at the current point in the trajectory before + * constraints are applied. + * @return The absolute maximum velocity. + */ + double getMaxVelocityMetersPerSecond(Pose2d poseMeters, double curvatureRadPerMeter, + double velocityMetersPerSecond); + + /** + * Returns the minimum and maximum allowable acceleration for the trajectory + * given pose, curvature, and speed. + * + * @param poseMeters The pose at the current point in the trajectory. + * @param curvatureRadPerMeter The curvature at the current point in the trajectory. + * @param velocityMetersPerSecond The speed at the current point in the trajectory. + * @return The min and max acceleration bounds. + */ + MinMax getMinMaxAccelerationMetersPerSecondSq(Pose2d poseMeters, double curvatureRadPerMeter, + double velocityMetersPerSecond); + + /** + * Represents a minimum and maximum acceleration. + */ + @SuppressWarnings("MemberName") + class MinMax { + public double minAccelerationMetersPerSecondSq = -Double.MAX_VALUE; + public double maxAccelerationMetersPerSecondSq = +Double.MAX_VALUE; + + /** + * Constructs a MinMax. + * + * @param minAccelerationMetersPerSecondSq The minimum acceleration. + * @param maxAccelerationMetersPerSecondSq The maximum acceleration. + */ + public MinMax(double minAccelerationMetersPerSecondSq, + double maxAccelerationMetersPerSecondSq) { + this.minAccelerationMetersPerSecondSq = minAccelerationMetersPerSecondSq; + this.maxAccelerationMetersPerSecondSq = maxAccelerationMetersPerSecondSq; + } + + /** + * Constructs a MinMax with default values. + */ + public MinMax() { + } + } +} diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/geometry/Pose2dTest.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj/geometry/Pose2dTest.java index 03fdec7f01..066716e355 100644 --- a/wpilibj/src/test/java/edu/wpi/first/wpilibj/geometry/Pose2dTest.java +++ b/wpilibj/src/test/java/edu/wpi/first/wpilibj/geometry/Pose2dTest.java @@ -59,4 +59,17 @@ class Pose2dTest { var two = new Pose2d(0.0, 1.524, Rotation2d.fromDegrees(43.0)); assertNotEquals(one, two); } + + void testMinus() { + var initial = new Pose2d(0.0, 0.0, Rotation2d.fromDegrees(45.0)); + var last = new Pose2d(5.0, 5.0, Rotation2d.fromDegrees(45.0)); + + final var transform = last.minus(initial); + + assertAll( + () -> assertEquals(transform.getTranslation().getX(), 5.0 * Math.sqrt(2.0), kEpsilon), + () -> assertEquals(transform.getTranslation().getY(), 0.0, kEpsilon), + () -> assertEquals(transform.getRotation().getDegrees(), 0.0, kEpsilon) + ); + } } diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/geometry/Twist2dTest.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj/geometry/Twist2dTest.java index d356f339a4..903c43690d 100644 --- a/wpilibj/src/test/java/edu/wpi/first/wpilibj/geometry/Twist2dTest.java +++ b/wpilibj/src/test/java/edu/wpi/first/wpilibj/geometry/Twist2dTest.java @@ -65,4 +65,17 @@ class Twist2dTest { var two = new Twist2d(5, 1.2, 3); assertNotEquals(one, two); } + + void testPose2dLog() { + final var start = new Pose2d(); + final var end = new Pose2d(5.0, 5.0, Rotation2d.fromDegrees(90.0)); + + final var twist = start.log(end); + + assertAll( + () -> assertEquals(twist.dx, 5.0 / 2.0 * Math.PI, kEpsilon), + () -> assertEquals(twist.dy, 0.0, kEpsilon), + () -> assertEquals(twist.dtheta, Math.PI / 2.0, kEpsilon) + ); + } } 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 new file mode 100644 index 0000000000..8d7f601224 --- /dev/null +++ b/wpilibj/src/test/java/edu/wpi/first/wpilibj/spline/CubicHermiteSplineTest.java @@ -0,0 +1,102 @@ +/*----------------------------------------------------------------------------*/ +/* 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.spline; + +import java.util.ArrayList; +import java.util.List; + +import org.junit.jupiter.api.Test; + +import edu.wpi.first.wpilibj.geometry.Pose2d; +import edu.wpi.first.wpilibj.geometry.Rotation2d; +import edu.wpi.first.wpilibj.geometry.Translation2d; + +import static org.junit.jupiter.api.Assertions.assertAll; +import static org.junit.jupiter.api.Assertions.assertEquals; +import static org.junit.jupiter.api.Assertions.assertTrue; + + +class CubicHermiteSplineTest { + private static final double kMaxDx = 0.127; + private static final double kMaxDy = 0.00127; + private static final double kMaxDtheta = 0.0872; + + @SuppressWarnings({"ParameterName", "PMD.UnusedLocalVariable"}) + private void run(Pose2d a, List waypoints, Pose2d b) { + // Start the timer. + var start = System.nanoTime(); + + // Generate and parameterize the spline. + var splines + = SplineHelper.getCubicSplinesFromWaypoints(a, waypoints.toArray(new Translation2d[0]), b); + var poses = new ArrayList(); + + poses.add(splines[0].getPoint(0.0)); + + for (var spline : splines) { + poses.addAll(SplineParameterizer.parameterize(spline)); + } + + // End the timer. + var end = System.nanoTime(); + + // Calculate the duration (used when benchmarking) + var durationMicroseconds = (end - start) / 1000.0; + + for (int i = 0; i < poses.size() - 1; i++) { + var p0 = poses.get(i); + var p1 = poses.get(i + 1); + + // Make sure the twist is under the tolerance defined by the Spline class. + var twist = p0.poseMeters.log(p1.poseMeters); + assertAll( + () -> assertTrue(Math.abs(twist.dx) < kMaxDx), + () -> assertTrue(Math.abs(twist.dy) < kMaxDy), + () -> assertTrue(Math.abs(twist.dtheta) < kMaxDtheta) + ); + } + + // Check first point + assertAll( + () -> assertEquals(a.getTranslation().getX(), + poses.get(0).poseMeters.getTranslation().getX(), 1E-9), + () -> assertEquals(a.getTranslation().getY(), + poses.get(0).poseMeters.getTranslation().getY(), 1E-9), + () -> assertEquals(a.getRotation().getRadians(), + poses.get(0).poseMeters.getRotation().getRadians(), 1E-9) + ); + + // Check last point + assertAll( + () -> assertEquals(b.getTranslation().getX(), + poses.get(poses.size() - 1).poseMeters.getTranslation().getX(), 1E-9), + () -> assertEquals(b.getTranslation().getY(), + poses.get(poses.size() - 1).poseMeters.getTranslation().getY(), 1E-9), + () -> assertEquals(b.getRotation().getRadians(), + poses.get(poses.size() - 1).poseMeters.getRotation().getRadians(), 1E-9) + ); + } + + @SuppressWarnings("PMD.JUnitTestsShouldIncludeAssert") + @Test + void testStraightLine() { + run(new Pose2d(), new ArrayList<>(), new Pose2d(3, 0, new Rotation2d())); + } + + @SuppressWarnings("PMD.JUnitTestsShouldIncludeAssert") + @Test + void testSCurve() { + var start = new Pose2d(0, 0, Rotation2d.fromDegrees(90.0)); + ArrayList waypoints = new ArrayList<>(); + waypoints.add(new Translation2d(1, 1)); + waypoints.add(new Translation2d(2, -1)); + var end = new Pose2d(3, 0, Rotation2d.fromDegrees(90.0)); + + run(start, waypoints, end); + } +} 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 new file mode 100644 index 0000000000..8d942132da --- /dev/null +++ b/wpilibj/src/test/java/edu/wpi/first/wpilibj/spline/QuinticHermiteSplineTest.java @@ -0,0 +1,91 @@ +/*----------------------------------------------------------------------------*/ +/* 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.spline; + +import org.junit.jupiter.api.Test; + +import edu.wpi.first.wpilibj.geometry.Pose2d; +import edu.wpi.first.wpilibj.geometry.Rotation2d; + +import static org.junit.jupiter.api.Assertions.assertAll; +import static org.junit.jupiter.api.Assertions.assertEquals; +import static org.junit.jupiter.api.Assertions.assertTrue; + +class QuinticHermiteSplineTest { + private static final double kMaxDx = 0.127; + private static final double kMaxDy = 0.00127; + private static final double kMaxDtheta = 0.0872; + + @SuppressWarnings({"ParameterName", "PMD.UnusedLocalVariable"}) + private void run(Pose2d a, Pose2d b) { + // Start the timer. + var start = System.nanoTime(); + + // Generate and parameterize the spline. + var spline = SplineHelper.getQuinticSplinesFromWaypoints(new Pose2d[]{a, b})[0]; + var poses = SplineParameterizer.parameterize(spline); + + // End the timer. + var end = System.nanoTime(); + + // Calculate the duration (used when benchmarking) + var durationMicroseconds = (end - start) / 1000.0; + + for (int i = 0; i < poses.size() - 1; i++) { + var p0 = poses.get(i); + var p1 = poses.get(i + 1); + + // Make sure the twist is under the tolerance defined by the Spline class. + var twist = p0.poseMeters.log(p1.poseMeters); + assertAll( + () -> assertTrue(Math.abs(twist.dx) < kMaxDx), + () -> assertTrue(Math.abs(twist.dy) < kMaxDy), + () -> assertTrue(Math.abs(twist.dtheta) < kMaxDtheta) + ); + } + + // Check first point + assertAll( + () -> assertEquals(a.getTranslation().getX(), + poses.get(0).poseMeters.getTranslation().getX(), 1E-9), + () -> assertEquals(a.getTranslation().getY(), + poses.get(0).poseMeters.getTranslation().getY(), 1E-9), + () -> assertEquals(a.getRotation().getRadians(), + poses.get(0).poseMeters.getRotation().getRadians(), 1E-9) + ); + + // Check last point + assertAll( + () -> assertEquals(b.getTranslation().getX(), + poses.get(poses.size() - 1).poseMeters.getTranslation().getX(), 1E-9), + () -> assertEquals(b.getTranslation().getY(), + poses.get(poses.size() - 1).poseMeters.getTranslation().getY(), 1E-9), + () -> assertEquals(b.getRotation().getRadians(), + poses.get(poses.size() - 1).poseMeters.getRotation().getRadians(), 1E-9) + ); + } + + @SuppressWarnings("PMD.JUnitTestsShouldIncludeAssert") + @Test + void testStraightLine() { + run(new Pose2d(), new Pose2d(3, 0, new Rotation2d())); + } + + @SuppressWarnings("PMD.JUnitTestsShouldIncludeAssert") + @Test + void testSimpleSCurve() { + run(new Pose2d(), new Pose2d(1, 1, new Rotation2d())); + } + + @SuppressWarnings("PMD.JUnitTestsShouldIncludeAssert") + @Test + void testSquiggly() { + run(new Pose2d(0, 0, Rotation2d.fromDegrees(90)), + new Pose2d(-1, 0, Rotation2d.fromDegrees(90))); + } +} diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/trajectory/CentripetalAccelerationConstraintTest.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj/trajectory/CentripetalAccelerationConstraintTest.java new file mode 100644 index 0000000000..977c5abdd0 --- /dev/null +++ b/wpilibj/src/test/java/edu/wpi/first/wpilibj/trajectory/CentripetalAccelerationConstraintTest.java @@ -0,0 +1,43 @@ +/*----------------------------------------------------------------------------*/ +/* 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.Collections; + +import org.junit.jupiter.api.Test; + +import edu.wpi.first.wpilibj.trajectory.constraint.CentripetalAccelerationConstraint; +import edu.wpi.first.wpilibj.util.Units; + +import static org.junit.jupiter.api.Assertions.assertTrue; + +class CentripetalAccelerationConstraintTest { + @SuppressWarnings("LocalVariableName") + @Test + void testCentripetalAccelerationConstraint() { + double maxCentripetalAcceleration = Units.feetToMeters(7.0); // 7 feet per second squared + var constraint = new CentripetalAccelerationConstraint(maxCentripetalAcceleration); + + Trajectory trajectory = TrajectoryGeneratorTest.getTrajectory( + Collections.singletonList(constraint)); + + var duration = trajectory.getTotalTimeSeconds(); + var t = 0.0; + var dt = 0.02; + + while (t < duration) { + var point = trajectory.sample(t); + var centripetalAcceleration + = Math.pow(point.velocityMetersPerSecond, 2) * point.curvatureRadPerMeter; + + t += dt; + assertTrue(centripetalAcceleration <= maxCentripetalAcceleration + 0.05); + } + } + +} diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/trajectory/DifferentialDriveKinematicsConstraintTest.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj/trajectory/DifferentialDriveKinematicsConstraintTest.java new file mode 100644 index 0000000000..d025860a2d --- /dev/null +++ b/wpilibj/src/test/java/edu/wpi/first/wpilibj/trajectory/DifferentialDriveKinematicsConstraintTest.java @@ -0,0 +1,53 @@ +/*----------------------------------------------------------------------------*/ +/* 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.Collections; + +import org.junit.jupiter.api.Test; + +import edu.wpi.first.wpilibj.kinematics.ChassisSpeeds; +import edu.wpi.first.wpilibj.kinematics.DifferentialDriveKinematics; +import edu.wpi.first.wpilibj.trajectory.constraint.DifferentialDriveKinematicsConstraint; +import edu.wpi.first.wpilibj.util.Units; + +import static org.junit.jupiter.api.Assertions.assertAll; +import static org.junit.jupiter.api.Assertions.assertTrue; + +class DifferentialDriveKinematicsConstraintTest { + @SuppressWarnings({"LocalVariableName", "PMD.AvoidInstantiatingObjectsInLoops"}) + @Test + void testDifferentialDriveKinematicsConstraint() { + double maxVelocity = Units.feetToMeters(12.0); // 12 feet per second + var kinematics = new DifferentialDriveKinematics(Units.inchesToMeters(27)); + var constraint = new DifferentialDriveKinematicsConstraint(kinematics, maxVelocity); + + Trajectory trajectory = TrajectoryGeneratorTest.getTrajectory( + Collections.singletonList(constraint)); + + var duration = trajectory.getTotalTimeSeconds(); + var t = 0.0; + var dt = 0.02; + + while (t < duration) { + var point = trajectory.sample(t); + var chassisSpeeds = new ChassisSpeeds( + point.velocityMetersPerSecond, 0, + point.velocityMetersPerSecond * point.curvatureRadPerMeter + ); + + var wheelSpeeds = kinematics.toWheelSpeeds(chassisSpeeds); + + t += dt; + assertAll( + () -> assertTrue(wheelSpeeds.leftMetersPerSecond <= maxVelocity + 0.05), + () -> assertTrue(wheelSpeeds.rightMetersPerSecond <= maxVelocity + 0.05) + ); + } + } +} 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 new file mode 100644 index 0000000000..2d4242520a --- /dev/null +++ b/wpilibj/src/test/java/edu/wpi/first/wpilibj/trajectory/TrajectoryGeneratorTest.java @@ -0,0 +1,78 @@ +/*----------------------------------------------------------------------------*/ +/* 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 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.Transform2d; +import edu.wpi.first.wpilibj.geometry.Translation2d; +import edu.wpi.first.wpilibj.trajectory.constraint.TrajectoryConstraint; + +import static edu.wpi.first.wpilibj.util.Units.feetToMeters; +import static org.junit.jupiter.api.Assertions.assertAll; +import static org.junit.jupiter.api.Assertions.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); + + // 2018 cross scale auto waypoints. + var sideStart = new Pose2d(feetToMeters(1.54), feetToMeters(23.23), + Rotation2d.fromDegrees(-180)); + var crossScale = new Pose2d(feetToMeters(23.7), feetToMeters(6.8), + Rotation2d.fromDegrees(-160)); + + var waypoints = new ArrayList(); + waypoints.add(sideStart); + waypoints.add(sideStart.plus( + new Transform2d(new Translation2d(feetToMeters(-13), feetToMeters(0)), + new Rotation2d()))); + waypoints.add(sideStart.plus( + new Transform2d(new Translation2d(feetToMeters(-19.5), feetToMeters(5)), + Rotation2d.fromDegrees(-90)))); + waypoints.add(crossScale); + + return TrajectoryGenerator.generateTrajectory( + waypoints, + constraints, + startVelocity, + endVelocity, + maxVelocity, + maxAccel, + true + ); + } + + @Test + @SuppressWarnings("LocalVariableName") + void testGenerationAndConstraints() { + Trajectory trajectory = getTrajectory(new ArrayList<>()); + + double duration = trajectory.getTotalTimeSeconds(); + double t = 0.0; + double dt = 0.02; + + while (t < duration) { + var point = trajectory.sample(t); + t += dt; + assertAll( + () -> assertTrue(Math.abs(point.velocityMetersPerSecond) < feetToMeters(12.0) + 0.05), + () -> assertTrue(Math.abs(point.accelerationMetersPerSecondSq) < feetToMeters(12.0) + + 0.05) + ); + } + } +}