[wpimath] Add Translation2d/Translation3d slew rate limiter (#7806)

Co-authored-by: Tyler Veness <calcmogul@gmail.com>
Co-authored-by: Joseph Eng <91924258+KangarooKoala@users.noreply.github.com>

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 final class MathUtil {
     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;
+  }
 }

wpimath/src/main/native/include/frc/MathUtil.h

@@ -10,9 +10,15 @@
 #include <gcem.hpp>
 #include <wpi/SymbolExports.h>
 
+#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

wpimath/src/test/java/edu/wpi/first/math/MathUtilTest.java

@@ -8,6 +8,8 @@ import static org.junit.jupiter.api.Assertions.assertEquals;
 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 @@ class MathUtilTest extends UtilityClassTest<MathUtil> {
     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);
+  }
 }

wpimath/src/test/native/cpp/MathUtilTest.cpp

@@ -3,11 +3,17 @@
 // the WPILib BSD license file in the root directory of this project.
 
 #include <limits>
+#include <numbers>
 
 #include <gtest/gtest.h>
 
 #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);
+}