diff --git a/wpimath/src/main/java/edu/wpi/first/math/MathUtil.java b/wpimath/src/main/java/edu/wpi/first/math/MathUtil.java index 5bf89d7e810..1836005cee3 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/MathUtil.java +++ b/wpimath/src/main/java/edu/wpi/first/math/MathUtil.java @@ -4,6 +4,9 @@ package edu.wpi.first.math; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.geometry.Translation3d; + /** Math utility functions. */ public final class MathUtil { private MathUtil() { @@ -209,4 +212,62 @@ public static boolean isNear( double error = MathUtil.inputModulus(expected - actual, -errorBound, errorBound); return Math.abs(error) < tolerance; } + + /** + * Limits translation velocity. + * + * @param current Translation at current timestep. + * @param next Translation at next timestep. + * @param dt Timestep duration. + * @param maxVelocity Maximum translation velocity. + * @return Returns the next Translation2d limited to maxVelocity + */ + public static Translation2d slewRateLimit( + Translation2d current, Translation2d next, double dt, double maxVelocity) { + if (maxVelocity < 0) { + Exception e = new IllegalArgumentException(); + MathSharedStore.reportError( + "maxVelocity must be a non-negative number, got " + maxVelocity, e.getStackTrace()); + return next; + } + Translation2d diff = next.minus(current); + double dist = diff.getNorm(); + if (dist < 1e-9) { + return next; + } + if (dist > maxVelocity * dt) { + // Move maximum allowed amount in direction of the difference + return current.plus(diff.times(maxVelocity * dt / dist)); + } + return next; + } + + /** + * Limits translation velocity. + * + * @param current Translation at current timestep. + * @param next Translation at next timestep. + * @param dt Timestep duration. + * @param maxVelocity Maximum translation velocity. + * @return Returns the next Translation3d limited to maxVelocity + */ + public static Translation3d slewRateLimit( + Translation3d current, Translation3d next, double dt, double maxVelocity) { + if (maxVelocity < 0) { + Exception e = new IllegalArgumentException(); + MathSharedStore.reportError( + "maxVelocity must be a non-negative number, got " + maxVelocity, e.getStackTrace()); + return next; + } + Translation3d diff = next.minus(current); + double dist = diff.getNorm(); + if (dist < 1e-9) { + return next; + } + if (dist > maxVelocity * dt) { + // Move maximum allowed amount in direction of the difference + return current.plus(diff.times(maxVelocity * dt / dist)); + } + return next; + } } diff --git a/wpimath/src/main/native/include/frc/MathUtil.h b/wpimath/src/main/native/include/frc/MathUtil.h index a0447ee2a17..e6ec32908d7 100644 --- a/wpimath/src/main/native/include/frc/MathUtil.h +++ b/wpimath/src/main/native/include/frc/MathUtil.h @@ -10,9 +10,15 @@ #include #include +#include "frc/geometry/Translation2d.h" +#include "frc/geometry/Translation3d.h" #include "units/angle.h" #include "units/base.h" +#include "units/length.h" #include "units/math.h" +#include "units/time.h" +#include "units/velocity.h" +#include "wpimath/MathShared.h" namespace frc { @@ -207,4 +213,65 @@ constexpr std::signed_integral auto FloorMod(std::signed_integral auto x, std::signed_integral auto y) { return x - FloorDiv(x, y) * y; } + +/** + * Limits translation velocity. + * + * @param current Translation at current timestep. + * @param next Translation at next timestep. + * @param dt Timestep duration. + * @param maxVelocity Maximum translation velocity. + * @return Returns the next Translation2d limited to maxVelocity + */ +constexpr Translation2d SlewRateLimit(const Translation2d& current, + const Translation2d& next, + units::second_t dt, + units::meters_per_second_t maxVelocity) { + if (maxVelocity < 0_mps) { + wpi::math::MathSharedStore::ReportError( + "maxVelocity must be a non-negative number, got {}!", maxVelocity); + return next; + } + Translation2d diff = next - current; + units::meter_t dist = diff.Norm(); + if (dist < 1e-9_m) { + return next; + } + if (dist > maxVelocity * dt) { + // Move maximum allowed amount in direction of the difference + return current + diff * (maxVelocity * dt / dist); + } + return next; +} + +/** + * Limits translation velocity. + * + * @param current Translation at current timestep. + * @param next Translation at next timestep. + * @param dt Timestep duration. + * @param maxVelocity Maximum translation velocity. + * @return Returns the next Translation3d limited to maxVelocity + */ +constexpr Translation3d SlewRateLimit(const Translation3d& current, + const Translation3d& next, + units::second_t dt, + units::meters_per_second_t maxVelocity) { + if (maxVelocity < 0_mps) { + wpi::math::MathSharedStore::ReportError( + "maxVelocity must be a non-negative number, got {}!", maxVelocity); + return next; + } + Translation3d diff = next - current; + units::meter_t dist = diff.Norm(); + if (dist < 1e-9_m) { + return next; + } + if (dist > maxVelocity * dt) { + // Move maximum allowed amount in direction of the difference + return current + diff * (maxVelocity * dt / dist); + } + return next; +} + } // namespace frc diff --git a/wpimath/src/test/java/edu/wpi/first/math/MathUtilTest.java b/wpimath/src/test/java/edu/wpi/first/math/MathUtilTest.java index 44f7c50a0e1..93ef1ef7342 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/MathUtilTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/MathUtilTest.java @@ -8,6 +8,8 @@ import static org.junit.jupiter.api.Assertions.assertFalse; import static org.junit.jupiter.api.Assertions.assertTrue; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.geometry.Translation3d; import edu.wpi.first.wpilibj.UtilityClassTest; import org.junit.jupiter.api.Test; @@ -167,4 +169,55 @@ void testIsNear() { assertFalse(MathUtil.isNear(400, -315, 5, 0, 360)); assertFalse(MathUtil.isNear(400, 395, 5, 0, 360)); } + + @Test + void testSlewRateTranslation2dUnchanged() { + Translation2d translation1 = new Translation2d(0, 0); + Translation2d translation2 = new Translation2d(2, 2); + + Translation2d result1 = MathUtil.slewRateLimit(translation1, translation2, 1, 50); + + Translation2d expected1 = new Translation2d(2, 2); + + assertEquals(expected1, result1); + } + + @Test + void testSlewRateTranslation2dChanged() { + Translation2d translation3 = new Translation2d(1, 1); + Translation2d translation4 = new Translation2d(3, 3); + + Translation2d result2 = MathUtil.slewRateLimit(translation3, translation4, 0.25, 2); + + Translation2d expected2 = + new Translation2d(1.0 + 1.0 / Math.sqrt(8.0), 1.0 + 1.0 / Math.sqrt(8.0)); + + assertEquals(expected2, result2); + } + + @Test + void testSlewRateTranslation3dUnchanged() { + Translation3d translation1 = new Translation3d(0, 0, 0); + Translation3d translation2 = new Translation3d(2, 2, 2); + + Translation3d result1 = MathUtil.slewRateLimit(translation1, translation2, 1, 50); + + Translation3d expected1 = new Translation3d(2, 2, 2); + + assertEquals(expected1, result1); + } + + @Test + void testSlewRateTranslation3dChanged() { + Translation3d translation3 = new Translation3d(1, 1, 1); + Translation3d translation4 = new Translation3d(3, 3, 3); + + Translation3d result2 = MathUtil.slewRateLimit(translation3, translation4, 0.25, 2); + + Translation3d expected2 = + new Translation3d( + 1.0 + 1.0 / Math.sqrt(12.0), 1.0 + 1.0 / Math.sqrt(12.0), 1.0 + 1.0 / Math.sqrt(12.0)); + + assertEquals(expected2, result2); + } } diff --git a/wpimath/src/test/native/cpp/MathUtilTest.cpp b/wpimath/src/test/native/cpp/MathUtilTest.cpp index cee983185b7..fc9405ce38e 100644 --- a/wpimath/src/test/native/cpp/MathUtilTest.cpp +++ b/wpimath/src/test/native/cpp/MathUtilTest.cpp @@ -3,11 +3,17 @@ // the WPILib BSD license file in the root directory of this project. #include +#include #include #include "frc/MathUtil.h" +#include "frc/geometry/Translation2d.h" +#include "frc/geometry/Translation3d.h" #include "units/angle.h" +#include "units/length.h" +#include "units/time.h" +#include "units/velocity.h" #define EXPECT_UNITS_EQ(a, b) EXPECT_DOUBLE_EQ((a).value(), (b).value()) @@ -164,3 +170,56 @@ TEST(MathUtilTest, IsNear) { EXPECT_FALSE(frc::IsNear(400, 395, 5, 0, 360)); EXPECT_FALSE(frc::IsNear(0_deg, -4_deg, 2.5_deg, 0_deg, 360_deg)); } + +TEST(MathUtilTest, Translation2dSlewRateLimitUnchanged) { + const frc::Translation2d translation1{0_m, 0_m}; + const frc::Translation2d translation2{2_m, 2_m}; + + const frc::Translation2d result1 = + frc::SlewRateLimit(translation1, translation2, 1_s, 50_mps); + + const frc::Translation2d expected1{2_m, 2_m}; + + EXPECT_EQ(result1, expected1); +} + +TEST(MathUtilTest, Translation2dSlewRateLimitChanged) { + const frc::Translation2d translation3{1_m, 1_m}; + const frc::Translation2d translation4{3_m, 3_m}; + + const frc::Translation2d result2 = + frc::SlewRateLimit(translation3, translation4, 0.25_s, 2_mps); + + const frc::Translation2d expected2{ + units::meter_t{1.0 + 0.5 * (std::numbers::sqrt2 / 2)}, + units::meter_t{1.0 + 0.5 * (std::numbers::sqrt2 / 2)}}; + + EXPECT_EQ(result2, expected2); +} + +TEST(MathUtilTest, Translation3dSlewRateLimitUnchanged) { + const frc::Translation3d translation1{0_m, 0_m, 0_m}; + const frc::Translation3d translation2{2_m, 2_m, 2_m}; + + const frc::Translation3d result1 = + frc::SlewRateLimit(translation1, translation2, 1_s, 50.0_mps); + + const frc::Translation3d expected1{2_m, 2_m, 2_m}; + + EXPECT_EQ(result1, expected1); +} + +TEST(MathUtilTest, Translation3dSlewRateLimitChanged) { + const frc::Translation3d translation3{1_m, 1_m, 1_m}; + const frc::Translation3d translation4{3_m, 3_m, 3_m}; + + const frc::Translation3d result2 = + frc::SlewRateLimit(translation3, translation4, 0.25_s, 2.0_mps); + + const frc::Translation3d expected2{ + units::meter_t{1.0 + 0.5 * std::numbers::inv_sqrt3}, + units::meter_t{1.0 + 0.5 * std::numbers::inv_sqrt3}, + units::meter_t{1.0 + 0.5 * std::numbers::inv_sqrt3}}; + + EXPECT_EQ(result2, expected2); +}