Merge branch 'main' into 2027

wpimath/src/test/native/cpp/estimator/SwerveDrivePoseEstimator3dTest.cpp

@@ -498,10 +498,22 @@ TEST(SwerveDrivePoseEstimator3dTest, TestReset) {
   EXPECT_DOUBLE_EQ(0, estimator.GetEstimatedPosition().Rotation().Y().value());
   EXPECT_DOUBLE_EQ(0, estimator.GetEstimatedPosition().Rotation().Z().value());
 
+  // Add a vision measurement with a different translation
+  estimator.AddVisionMeasurement(
+      wpi::math::Pose3d(3_m, 0_m, 0_m, wpi::math::Rotation3d{}),
+      wpi::math::MathSharedStore::GetTimestamp());
+
+  EXPECT_DOUBLE_EQ(2.5, estimator.GetEstimatedPosition().X().value());
+  EXPECT_DOUBLE_EQ(0, estimator.GetEstimatedPosition().Y().value());
+  EXPECT_DOUBLE_EQ(0, estimator.GetEstimatedPosition().Z().value());
+  EXPECT_DOUBLE_EQ(0, estimator.GetEstimatedPosition().Rotation().X().value());
+  EXPECT_DOUBLE_EQ(0, estimator.GetEstimatedPosition().Rotation().Y().value());
+  EXPECT_DOUBLE_EQ(0, estimator.GetEstimatedPosition().Rotation().Z().value());
+
   // Test reset rotation
   estimator.ResetRotation(wpi::math::Rotation3d{0_deg, 0_deg, 90_deg});
 
-  EXPECT_DOUBLE_EQ(2, estimator.GetEstimatedPosition().X().value());
+  EXPECT_DOUBLE_EQ(2.5, estimator.GetEstimatedPosition().X().value());
   EXPECT_DOUBLE_EQ(0, estimator.GetEstimatedPosition().Y().value());
   EXPECT_DOUBLE_EQ(0, estimator.GetEstimatedPosition().Z().value());
   EXPECT_DOUBLE_EQ(0, estimator.GetEstimatedPosition().Rotation().X().value());
@@ -517,7 +529,7 @@ TEST(SwerveDrivePoseEstimator3dTest, TestReset) {
                                                modulePosition, modulePosition});
   }
 
-  EXPECT_DOUBLE_EQ(2, estimator.GetEstimatedPosition().X().value());
+  EXPECT_DOUBLE_EQ(2.5, estimator.GetEstimatedPosition().X().value());
   EXPECT_DOUBLE_EQ(1, estimator.GetEstimatedPosition().Y().value());
   EXPECT_DOUBLE_EQ(0, estimator.GetEstimatedPosition().Z().value());
   EXPECT_DOUBLE_EQ(0, estimator.GetEstimatedPosition().Rotation().X().value());
@@ -525,6 +537,20 @@ TEST(SwerveDrivePoseEstimator3dTest, TestReset) {
   EXPECT_DOUBLE_EQ(std::numbers::pi / 2,
                    estimator.GetEstimatedPosition().Rotation().Z().value());
 
+  // Add a vision measurement with a different rotation
+  estimator.AddVisionMeasurement(
+      wpi::math::Pose3d(2.5_m, 1_m, 0_m,
+                        wpi::math::Rotation3d{wpi::math::Rotation2d{180_deg}}),
+      wpi::math::MathSharedStore::GetTimestamp());
+
+  EXPECT_DOUBLE_EQ(2.5, estimator.GetEstimatedPosition().X().value());
+  EXPECT_DOUBLE_EQ(1, estimator.GetEstimatedPosition().Y().value());
+  EXPECT_DOUBLE_EQ(0, estimator.GetEstimatedPosition().Z().value());
+  EXPECT_DOUBLE_EQ(0, estimator.GetEstimatedPosition().Rotation().X().value());
+  EXPECT_DOUBLE_EQ(0, estimator.GetEstimatedPosition().Rotation().Y().value());
+  EXPECT_DOUBLE_EQ(std::numbers::pi * 3.0 / 4,
+                   estimator.GetEstimatedPosition().Rotation().Z().value());
+
   // Test reset translation
   estimator.ResetTranslation(wpi::math::Translation3d{-1_m, -1_m, -1_m});
 
@@ -533,7 +559,7 @@ TEST(SwerveDrivePoseEstimator3dTest, TestReset) {
   EXPECT_DOUBLE_EQ(-1, estimator.GetEstimatedPosition().Z().value());
   EXPECT_DOUBLE_EQ(0, estimator.GetEstimatedPosition().Rotation().X().value());
   EXPECT_DOUBLE_EQ(0, estimator.GetEstimatedPosition().Rotation().Y().value());
-  EXPECT_DOUBLE_EQ(std::numbers::pi / 2,
+  EXPECT_DOUBLE_EQ(std::numbers::pi * 3.0 / 4,
                    estimator.GetEstimatedPosition().Rotation().Z().value());
 
   // Test reset pose

wpimath/src/test/native/cpp/estimator/SwerveDrivePoseEstimatorTest.cpp

@@ -464,10 +464,20 @@ TEST(SwerveDrivePoseEstimatorTest, TestReset) {
   EXPECT_DOUBLE_EQ(
       0, estimator.GetEstimatedPosition().Rotation().Radians().value());
 
+  // Add a vision measurement with a different translation
+  estimator.AddVisionMeasurement(
+      wpi::math::Pose2d{3_m, 0_m, wpi::math::Rotation2d{}},
+      wpi::math::MathSharedStore::GetTimestamp());
+
+  EXPECT_DOUBLE_EQ(2.5, estimator.GetEstimatedPosition().X().value());
+  EXPECT_DOUBLE_EQ(0, estimator.GetEstimatedPosition().Y().value());
+  EXPECT_DOUBLE_EQ(
+      0, estimator.GetEstimatedPosition().Rotation().Radians().value());
+
   // Test reset rotation
   estimator.ResetRotation(wpi::math::Rotation2d{90_deg});
 
-  EXPECT_DOUBLE_EQ(2, estimator.GetEstimatedPosition().X().value());
+  EXPECT_DOUBLE_EQ(2.5, estimator.GetEstimatedPosition().X().value());
   EXPECT_DOUBLE_EQ(0, estimator.GetEstimatedPosition().Y().value());
   EXPECT_DOUBLE_EQ(
       std::numbers::pi / 2,
@@ -481,19 +491,30 @@ TEST(SwerveDrivePoseEstimatorTest, TestReset) {
                                                modulePosition, modulePosition});
   }
 
-  EXPECT_DOUBLE_EQ(2, estimator.GetEstimatedPosition().X().value());
+  EXPECT_DOUBLE_EQ(2.5, estimator.GetEstimatedPosition().X().value());
   EXPECT_DOUBLE_EQ(1, estimator.GetEstimatedPosition().Y().value());
   EXPECT_DOUBLE_EQ(
       std::numbers::pi / 2,
       estimator.GetEstimatedPosition().Rotation().Radians().value());
 
+  // Add a vision measurement with a different rotation
+  estimator.AddVisionMeasurement(
+      wpi::math::Pose2d{2.5_m, 1_m, wpi::math::Rotation2d{180_deg}},
+      wpi::math::MathSharedStore::GetTimestamp());
+
+  EXPECT_DOUBLE_EQ(2.5, estimator.GetEstimatedPosition().X().value());
+  EXPECT_DOUBLE_EQ(1, estimator.GetEstimatedPosition().Y().value());
+  EXPECT_DOUBLE_EQ(
+      std::numbers::pi * 3.0 / 4,
+      estimator.GetEstimatedPosition().Rotation().Radians().value());
+
   // Test reset translation
   estimator.ResetTranslation(wpi::math::Translation2d{-1_m, -1_m});
 
   EXPECT_DOUBLE_EQ(-1, estimator.GetEstimatedPosition().X().value());
   EXPECT_DOUBLE_EQ(-1, estimator.GetEstimatedPosition().Y().value());
   EXPECT_DOUBLE_EQ(
-      std::numbers::pi / 2,
+      std::numbers::pi * 3.0 / 4,
       estimator.GetEstimatedPosition().Rotation().Radians().value());
 
   // Test reset pose

wpimath/src/test/native/cpp/filter/EdgeCounterFilterTest.cpp

@@ -0,0 +1,77 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "wpi/math/filter/EdgeCounterFilter.hpp"
+
+#include <gtest/gtest.h>
+
+#include "wpi/units/time.hpp"
+#include "wpi/util/timestamp.h"
+
+static wpi::units::second_t now = 0_s;
+
+class EdgeCounterFilterTest : public ::testing::Test {
+ protected:
+  void SetUp() override {
+    WPI_SetNowImpl(
+        [] { return wpi::units::microsecond_t{now}.to<uint64_t>(); });
+    now = 0_ms;
+  }
+
+  void TearDown() override {
+    now = 0_ms;
+    WPI_SetNowImpl(nullptr);
+  }
+};
+
+TEST_F(EdgeCounterFilterTest, EdgeCounterFilterActivated) {
+  wpi::math::EdgeCounterFilter filter{2, 0.2_s};
+
+  EXPECT_FALSE(filter.Calculate(true));  // First edge
+
+  now = 50_ms;
+  EXPECT_FALSE(filter.Calculate(false));  // First edge ended
+
+  now = 100_ms;
+  EXPECT_TRUE(filter.Calculate(true));  // Second edge
+
+  now = 150_ms;
+  EXPECT_TRUE(filter.Calculate(true));  // Still true
+
+  now = 250_ms;
+  EXPECT_TRUE(filter.Calculate(true));  // Still true
+
+  now = 300_ms;
+  EXPECT_FALSE(filter.Calculate(false));  // Input false, should reset
+}
+
+TEST_F(EdgeCounterFilterTest, EdgeCounterFilterExpired) {
+  wpi::math::EdgeCounterFilter filter{2, 0.2_s};
+
+  EXPECT_FALSE(filter.Calculate(true));  // First edge
+
+  now = 50_ms;
+  filter.Calculate(false);  // First edge ended
+
+  now = 250_ms;
+  EXPECT_FALSE(filter.Calculate(true));  // Second edge after window expired
+
+  now = 300_ms;
+  EXPECT_FALSE(filter.Calculate(true));  // Still false
+}
+
+TEST_F(EdgeCounterFilterTest, EdgeCounterFilterParams) {
+  wpi::math::EdgeCounterFilter filter{2, 0.2_s};
+
+  EXPECT_EQ(filter.GetRequiredEdges(), 2);
+  EXPECT_EQ(filter.GetWindowTime(), 0.2_s);
+
+  filter.SetRequiredEdges(3);
+
+  EXPECT_EQ(filter.GetRequiredEdges(), 3);
+
+  filter.SetWindowTime(0.5_s);
+
+  EXPECT_EQ(filter.GetWindowTime(), 0.5_s);
+}

wpimath/src/test/native/cpp/geometry/Rotation2dTest.cpp

@@ -41,6 +41,15 @@ TEST(Rotation2dTest, RotateByNonZero) {
   EXPECT_DOUBLE_EQ(120.0, rot.Degrees().value());
 }
 
+TEST(Rotation2dTest, RelativeTo) {
+  auto start = Rotation2d{30_deg};
+  auto end = Rotation2d{90_deg};
+
+  auto result = end.RelativeTo(start);
+
+  EXPECT_DOUBLE_EQ(60.0, result.Degrees().value());
+}
+
 TEST(Rotation2dTest, Minus) {
   const auto rot1 = Rotation2d{70_deg};
   const auto rot2 = Rotation2d{30_deg};

wpimath/src/test/native/cpp/geometry/Rotation3dTest.cpp

@@ -261,6 +261,21 @@ TEST(Rotation3dTest, RotateByNonZeroZ) {
   EXPECT_EQ(expected, rot);
 }
 
+TEST(Rotation3dTest, RelativeTo) {
+  const Eigen::Vector3d yAxis{0.0, 1.0, 0.0};
+  const Eigen::Vector3d zAxis{0.0, 0.0, 1.0};
+
+  auto start = Rotation3d{yAxis, -90_deg};
+  auto end = Rotation3d{zAxis, 90_deg};
+
+  const Eigen::Vector3d intrinsicAxis{1.0, 1.0, 1.0};
+  auto expected = Rotation3d{intrinsicAxis, 120_deg};
+
+  auto result = end.RelativeTo(start);
+
+  EXPECT_EQ(expected, result);
+}
+
 TEST(Rotation3dTest, Minus) {
   const Eigen::Vector3d zAxis{0.0, 0.0, 1.0};
 
@@ -379,4 +394,32 @@ TEST(Rotation3dTest, Interpolate) {
   EXPECT_DOUBLE_EQ(0.0, wpi::units::degree_t{interpolated.X()}.value());
   EXPECT_DOUBLE_EQ(0.0, wpi::units::degree_t{interpolated.Y()}.value());
   EXPECT_DOUBLE_EQ(-175.0, wpi::units::degree_t{interpolated.Z()}.value());
+
+  // t value of 0 should always produce the start
+  rot1 = Rotation3d{yAxis, -90_deg};
+  rot2 = Rotation3d{zAxis, 90_deg};
+  interpolated = wpi::util::Lerp(rot1, rot2, 0.0);
+  EXPECT_DOUBLE_EQ(rot1.X().value(), interpolated.X().value());
+  EXPECT_DOUBLE_EQ(rot1.Y().value(), interpolated.Y().value());
+  EXPECT_DOUBLE_EQ(rot1.Z().value(), interpolated.Z().value());
+
+  // The full rotation from rot1 to rot2 is 120 degrees around extrinsic
+  // <-1.0, 1.0, 1.0>
+  const Eigen::Vector3d extrinsicAxis{-1.0, 1.0, 1.0};
+  rot1 = Rotation3d{yAxis, -90_deg};
+  rot2 = Rotation3d{zAxis, 90_deg};
+  EXPECT_EQ(rot2, rot1.RotateBy(Rotation3d{extrinsicAxis, 120_deg}));
+  interpolated = wpi::util::Lerp(rot1, rot2, 0.5);
+  auto expected = rot1.RotateBy(Rotation3d{extrinsicAxis, 60_deg});
+  EXPECT_DOUBLE_EQ(expected.X().value(), interpolated.X().value());
+  EXPECT_DOUBLE_EQ(expected.Y().value(), interpolated.Y().value());
+  EXPECT_DOUBLE_EQ(expected.Z().value(), interpolated.Z().value());
+
+  // t value of 1 should always produce the end
+  rot1 = Rotation3d{yAxis, -90_deg};
+  rot2 = Rotation3d{zAxis, 90_deg};
+  interpolated = wpi::util::Lerp(rot1, rot2, 1.0);
+  EXPECT_NEAR(rot2.X().value(), interpolated.X().value(), 1e-9);
+  EXPECT_NEAR(rot2.Y().value(), interpolated.Y().value(), 1e-9);
+  EXPECT_NEAR(rot2.Z().value(), interpolated.Z().value(), 1e-9);
 }

wpimath/src/test/native/cpp/kinematics/DifferentialDriveOdometry3dTest.cpp

@@ -8,10 +8,10 @@
 
 #include <gtest/gtest.h>
 
-static constexpr double kEpsilon = 1E-9;
-
 using namespace wpi::math;
 
+static constexpr double kEpsilon = 1E-9;
+
 TEST(DifferentialDriveOdometry3dTest, Initialize) {
   DifferentialDriveOdometry3d odometry{
       wpi::math::Rotation3d{0_deg, 0_deg, 90_deg}, 0_m, 0_m,
@@ -39,3 +39,20 @@ TEST(DifferentialDriveOdometry3dTest, EncoderDistances) {
   EXPECT_NEAR(pose.Z().value(), 0.0, kEpsilon);
   EXPECT_NEAR(pose.Rotation().ToRotation2d().Degrees().value(), 90.0, kEpsilon);
 }
+
+TEST(DifferentialDriveOdometry3dTest, GyroOffset) {
+  DifferentialDriveOdometry3d odometry{
+      wpi::math::Rotation3d{0_deg, 5_deg, 0_deg}, 0_m, 0_m,
+      wpi::math::Pose3d{wpi::math::Translation3d{},
+                        wpi::math::Rotation3d{0_deg, 0_deg, 90_deg}}};
+  const auto& pose =
+      odometry.Update(wpi::math::Rotation3d{0_deg, 10_deg, 0_deg}, 0_m, 0_m);
+
+  EXPECT_NEAR(pose.X().value(), 0.0, kEpsilon);
+  EXPECT_NEAR(pose.Y().value(), 0.0, kEpsilon);
+  EXPECT_NEAR(pose.Z().value(), 0.0, kEpsilon);
+  EXPECT_NEAR(wpi::units::degree_t{pose.Rotation().X()}.value(), 0.0, kEpsilon);
+  EXPECT_NEAR(wpi::units::degree_t{pose.Rotation().Y()}.value(), 5.0, kEpsilon);
+  EXPECT_NEAR(wpi::units::degree_t{pose.Rotation().Z()}.value(), 90.0,
+              kEpsilon);
+}

wpimath/src/test/native/cpp/kinematics/MecanumDriveOdometry3dTest.cpp

@@ -231,3 +231,20 @@ TEST_F(MecanumDriveOdometry3dTest, AccuracyFacingXAxis) {
   EXPECT_LT(errorSum / (trajectory.TotalTime().value() / dt.value()), 0.06);
   EXPECT_LT(maxError, 0.125);
 }
+
+TEST_F(MecanumDriveOdometry3dTest, GyroOffset) {
+  wpi::math::MecanumDriveWheelPositions wheelPositions;
+  odometry.ResetPosition(
+      wpi::math::Rotation3d{0_deg, 5_deg, 0_deg}, wheelPositions,
+      wpi::math::Pose3d{wpi::math::Translation3d{},
+                        wpi::math::Rotation3d{0_deg, 0_deg, 90_deg}});
+  auto pose = odometry.Update(wpi::math::Rotation3d{0_deg, 10_deg, 0_deg},
+                              wheelPositions);
+
+  EXPECT_NEAR(pose.X().value(), 0.0, 1e-9);
+  EXPECT_NEAR(pose.Y().value(), 0.0, 1e-9);
+  EXPECT_NEAR(pose.Z().value(), 0.0, 1e-9);
+  EXPECT_NEAR(wpi::units::degree_t{pose.Rotation().X()}.value(), 0.0, 1e-9);
+  EXPECT_NEAR(wpi::units::degree_t{pose.Rotation().Y()}.value(), 5.0, 1e-9);
+  EXPECT_NEAR(wpi::units::degree_t{pose.Rotation().Z()}.value(), 90.0, 1e-9);
+}

wpimath/src/test/native/cpp/kinematics/SwerveDriveOdometry3dTest.cpp

@@ -225,3 +225,19 @@ TEST_F(SwerveDriveOdometry3dTest, AccuracyFacingXAxis) {
   EXPECT_LT(errorSum / (trajectory.TotalTime().value() / dt.value()), 0.06);
   EXPECT_LT(maxError, 0.125);
 }
+
+TEST_F(SwerveDriveOdometry3dTest, GyroOffset) {
+  m_odometry.ResetPosition(
+      wpi::math::Rotation3d{0_deg, 5_deg, 0_deg}, {zero, zero, zero, zero},
+      wpi::math::Pose3d{wpi::math::Translation3d{},
+                        wpi::math::Rotation3d{0_deg, 0_deg, 90_deg}});
+  auto pose = m_odometry.Update(wpi::math::Rotation3d{0_deg, 10_deg, 0_deg},
+                                {zero, zero, zero, zero});
+
+  EXPECT_NEAR(pose.X().value(), 0.0, 1e-9);
+  EXPECT_NEAR(pose.Y().value(), 0.0, 1e-9);
+  EXPECT_NEAR(pose.Z().value(), 0.0, 1e-9);
+  EXPECT_NEAR(wpi::units::degree_t{pose.Rotation().X()}.value(), 0.0, 1e-9);
+  EXPECT_NEAR(wpi::units::degree_t{pose.Rotation().Y()}.value(), 5.0, 1e-9);
+  EXPECT_NEAR(wpi::units::degree_t{pose.Rotation().Z()}.value(), 90.0, 1e-9);
+}