[wpiutil] Add angle normalization method

wpilibc/src/test/native/cpp/controller/HolonomicDriveControllerTest.cpp

@@ -0,0 +1,51 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2020 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 <units/math.h>
+#include <units/time.h>
+#include <wpi/math>
+
+#include "frc/controller/HolonomicDriveController.h"
+#include "frc/trajectory/TrajectoryGenerator.h"
+#include "gtest/gtest.h"
+
+#define EXPECT_NEAR_UNITS(val1, val2, eps) \
+  EXPECT_LE(units::math::abs(val1 - val2), eps)
+
+static constexpr units::meter_t kTolerance{1 / 12.0};
+static constexpr units::radian_t kAngularTolerance{2.0 * wpi::math::pi / 180.0};
+
+TEST(HolonomicDriveControllerTest, ReachesReference) {
+  frc::HolonomicDriveController controller{
+      frc2::PIDController{1.0, 0.0, 0.0}, frc2::PIDController{1.0, 0.0, 0.0},
+      frc::ProfiledPIDController<units::radian>{
+          1.0, 0.0, 0.0,
+          frc::TrapezoidProfile<units::radian>::Constraints{
+              6.28_rad_per_s, 3.14_rad_per_s / 1_s}}};
+
+  frc::Pose2d robotPose{2.7_m, 23_m, frc::Rotation2d{0_deg}};
+
+  auto waypoints = std::vector{frc::Pose2d{2.75_m, 22.521_m, 0_rad},
+                               frc::Pose2d{24.73_m, 19.68_m, 5.846_rad}};
+  auto trajectory = frc::TrajectoryGenerator::GenerateTrajectory(
+      waypoints, {8.0_mps, 4.0_mps_sq});
+
+  constexpr auto kDt = 0.02_s;
+  auto totalTime = trajectory.TotalTime();
+  for (size_t i = 0; i < (totalTime / kDt).to<double>(); ++i) {
+    auto state = trajectory.Sample(kDt * i);
+    auto [vx, vy, omega] = controller.Calculate(robotPose, state, 0_rad);
+
+    robotPose = robotPose.Exp(frc::Twist2d{vx * kDt, vy * kDt, omega * kDt});
+  }
+
+  auto& endPose = trajectory.States().back().pose;
+  EXPECT_NEAR_UNITS(endPose.X(), robotPose.X(), kTolerance);
+  EXPECT_NEAR_UNITS(endPose.Y(), robotPose.Y(), kTolerance);
+  EXPECT_NEAR_UNITS(units::math::NormalizeAngle(robotPose.Rotation().Radians()),
+                    0_rad, kAngularTolerance);
+}

wpilibc/src/test/native/cpp/controller/RamseteControllerTest.cpp

@@ -6,7 +6,6 @@
 /*----------------------------------------------------------------------------*/
 
 #include <units/math.h>
-#include <wpi/math>
 
 #include "frc/controller/RamseteController.h"
 #include "frc/trajectory/TrajectoryGenerator.h"
@@ -18,16 +17,6 @@
 static constexpr units::meter_t kTolerance{1 / 12.0};
 static constexpr units::radian_t kAngularTolerance{2.0 * wpi::math::pi / 180.0};
 
-units::radian_t boundRadians(units::radian_t value) {
-  while (value > units::radian_t{wpi::math::pi}) {
-    value -= units::radian_t{wpi::math::pi * 2};
-  }
-  while (value <= units::radian_t{-wpi::math::pi}) {
-    value += units::radian_t{wpi::math::pi * 2};
-  }
-  return value;
-}
-
 TEST(RamseteControllerTest, ReachesReference) {
   frc::RamseteController controller{2.0, 0.7};
   frc::Pose2d robotPose{2.7_m, 23_m, frc::Rotation2d{0_deg}};
@@ -50,7 +39,7 @@ TEST(RamseteControllerTest, ReachesReference) {
   auto& endPose = trajectory.States().back().pose;
   EXPECT_NEAR_UNITS(endPose.X(), robotPose.X(), kTolerance);
   EXPECT_NEAR_UNITS(endPose.Y(), robotPose.Y(), kTolerance);
-  EXPECT_NEAR_UNITS(boundRadians(endPose.Rotation().Radians() -
-                                 robotPose.Rotation().Radians()),
+  EXPECT_NEAR_UNITS(units::math::NormalizeAngle(endPose.Rotation().Radians() -
+                                                robotPose.Rotation().Radians()),
                     0_rad, kAngularTolerance);
 }

wpilibj/src/test/java/edu/wpi/first/wpilibj/controller/RamseteControllerTest.java

@@ -16,6 +16,7 @@ import edu.wpi.first.wpilibj.geometry.Rotation2d;
 import edu.wpi.first.wpilibj.geometry.Twist2d;
 import edu.wpi.first.wpilibj.trajectory.TrajectoryConfig;
 import edu.wpi.first.wpilibj.trajectory.TrajectoryGenerator;
+import edu.wpi.first.wpiutil.math.MathUtil;
 
 import static org.junit.jupiter.api.Assertions.assertAll;
 import static org.junit.jupiter.api.Assertions.assertEquals;
@@ -24,16 +25,6 @@ class RamseteControllerTest {
   private static final double kTolerance = 1 / 12.0;
   private static final double kAngularTolerance = Math.toRadians(2);
 
-  private static double boundRadians(double value) {
-    while (value > Math.PI) {
-      value -= Math.PI * 2;
-    }
-    while (value <= -Math.PI) {
-      value += Math.PI * 2;
-    }
-    return value;
-  }
-
   @Test
   @SuppressWarnings("PMD.AvoidInstantiatingObjectsInLoops")
   void testReachesReference() {
@@ -68,7 +59,7 @@ class RamseteControllerTest {
         () -> assertEquals(endPose.getY(), finalRobotPose.getY(),
             kTolerance),
         () -> assertEquals(0.0,
-            boundRadians(endPose.getRotation().getRadians()
+            MathUtil.normalizeAngle(endPose.getRotation().getRadians()
                 - finalRobotPose.getRotation().getRadians()),
             kAngularTolerance)
     );

wpiutil/src/main/java/edu/wpi/first/wpiutil/math/MathUtil.java

@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Copyright (c) 2019-2020 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.                                                               */
@@ -33,4 +33,24 @@ public final class MathUtil {
   public static double clamp(double value, double low, double high) {
     return Math.max(low, Math.min(value, high));
   }
+
+  /**
+   * Constrains theta to within the range (-pi, pi].
+   *
+   * @param theta The angle to normalize.
+   * @return The normalized angle.
+   */
+  @SuppressWarnings("LocalVariableName")
+  public static double normalizeAngle(double theta) {
+    // Constraint theta to within (-3pi, pi)
+    int nPiPos = (int) ((theta + Math.PI) / 2.0 / Math.PI);
+    theta -= nPiPos * 2.0 * Math.PI;
+
+    // Cut off the bottom half of the above range to constrain within
+    // (-pi, pi]
+    int nPiNeg = (int) ((theta - Math.PI) / 2.0 / Math.PI);
+    theta -= nPiNeg * 2.0 * Math.PI;
+
+    return theta;
+  }
 }

wpiutil/src/main/native/include/units/math.h

@@ -31,6 +31,8 @@
 
 #include <cmath>
 
+#include <wpi/math>
+
 #include "units/angle.h"
 #include "units/base.h"
 #include "units/dimensionless.h"
@@ -754,5 +756,24 @@ auto fma(const UnitTypeLhs x, const UnitMultiply y, const UnitAdd z) noexcept
   return resultType(std::fma(x(), y(), resultType(z)()));
 }
 
+/**
+ * Constrains theta to within the range (-pi, pi].
+ *
+ * @param theta Angle to normalize.
+ */
+constexpr units::radian_t NormalizeAngle(units::radian_t theta) {
+  units::radian_t pi(wpi::math::pi);
+
+  // Constrain theta to within (-3pi, pi)
+  const int n_pi_pos = (theta + pi) / 2.0 / pi;
+  theta = theta - units::radian_t{n_pi_pos * 2.0 * wpi::math::pi};
+
+  // Cut off the bottom half of the above range to constrain within
+  // (-pi, pi]
+  const int n_pi_neg = (theta - pi) / 2.0 / pi;
+  theta = theta - units::radian_t{n_pi_neg * 2.0 * wpi::math::pi};
+
+  return theta;
+}
 }  // namespace math
 }  // namespace units

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

@@ -0,0 +1,22 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2020 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.wpiutil.math;
+
+import org.junit.jupiter.api.Test;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+class MathUtilTest {
+  @Test
+  void testAngleNormalize() {
+    assertEquals(MathUtil.normalizeAngle(5 * Math.PI), Math.PI);
+    assertEquals(MathUtil.normalizeAngle(-5 * Math.PI), Math.PI);
+    assertEquals(MathUtil.normalizeAngle(Math.PI / 2), Math.PI / 2);
+    assertEquals(MathUtil.normalizeAngle(-Math.PI / 2), -Math.PI / 2);
+  }
+}

wpiutil/src/test/native/cpp/UnitsTest.cpp

@@ -2979,11 +2979,15 @@ TEST_F(UnitMath, abs) {
   EXPECT_EQ(meter_t(10.0), abs(meter_t(10.0)));
 }
 
-TEST_F(UnitMath, fma) {
-  meter_t x(2.0);
-  meter_t y(3.0);
-  square_meter_t z(1.0);
-  EXPECT_EQ(square_meter_t(7.0), fma(x, y, z));
+TEST_F(UnitMath, normalize) {
+  EXPECT_EQ(NormalizeAngle(radian_t(5 * wpi::math::pi)),
+      radian_t(wpi::math::pi));
+  EXPECT_EQ(NormalizeAngle(radian_t(-5 * wpi::math::pi)),
+      radian_t(wpi::math::pi));
+  EXPECT_EQ(NormalizeAngle(radian_t(wpi::math::pi / 2)),
+      radian_t(wpi::math::pi / 2));
+  EXPECT_EQ(NormalizeAngle(radian_t(-wpi::math::pi / 2)),
+      radian_t(-wpi::math::pi / 2));
 }
 
 // Constexpr