Initialize pose2d cache in 3d odometry constructors

KangarooKoala · Oct 23, 2024 · 4e59279 · 4e59279
1 parent 90dbdc1
commit 4e59279
Show file tree

Hide file tree

Showing 8 changed files with 76 additions and 0 deletions.
diff --git a/wpimath/src/main/java/edu/wpi/first/math/kinematics/Odometry3d.java b/wpimath/src/main/java/edu/wpi/first/math/kinematics/Odometry3d.java
@@ -67,6 +67,7 @@ public Odometry3d(
       Pose3d initialPoseMeters) {
     m_kinematics = kinematics;
     m_poseMeters = initialPoseMeters;
+    m_pose2dMeters = m_poseMeters.toPose2d();
     m_gyroOffset = m_poseMeters.getRotation().minus(gyroAngle);
     m_previousAngle = m_poseMeters.getRotation();
     m_previousWheelPositions = m_kinematics.copy(wheelPositions);

diff --git a/wpimath/src/main/native/include/frc/kinematics/Odometry3d.h b/wpimath/src/main/native/include/frc/kinematics/Odometry3d.h
@@ -62,6 +62,7 @@ class WPILIB_DLLEXPORT Odometry3d {
       : m_kinematics(kinematics),
         m_pose(initialPose),
         m_previousWheelPositions(wheelPositions) {
+    m_pose2d = m_pose.ToPose2d();
     m_previousAngle = m_pose.Rotation();
     m_gyroOffset = m_pose.Rotation() - gyroAngle;
   }

diff --git a/wpimath/src/test/java/edu/wpi/first/math/kinematics/DifferentialDriveOdometry3dTest.java b/wpimath/src/test/java/edu/wpi/first/math/kinematics/DifferentialDriveOdometry3dTest.java
@@ -16,6 +16,18 @@ class DifferentialDriveOdometry3dTest {
   private final DifferentialDriveOdometry3d m_odometry =
       new DifferentialDriveOdometry3d(Rotation2d.kZero, 0, 0);
 
+  @Test
+  void testInitialize() {
+    DifferentialDriveOdometry3d odometry =
+        new DifferentialDriveOdometry3d(
+            Rotation2d.kZero, 0, 0, new Pose2d(1, 2, Rotation2d.fromDegrees(45)));
+    var pose = odometry.getPoseMeters();
+    assertAll(
+        () -> assertEquals(pose.getX(), 1.0, kEpsilon),
+        () -> assertEquals(pose.getY(), 2.0, kEpsilon),
+        () -> assertEquals(pose.getRotation().getDegrees(), 45.0, kEpsilon));
+  }
+
   @Test
   void testOdometryWithEncoderDistances() {
     m_odometry.resetPosition(Rotation2d.fromDegrees(45), 0, 0, Pose2d.kZero);

diff --git a/wpimath/src/test/java/edu/wpi/first/math/kinematics/MecanumDriveOdometry3dTest.java b/wpimath/src/test/java/edu/wpi/first/math/kinematics/MecanumDriveOdometry3dTest.java
@@ -30,6 +30,18 @@ class MecanumDriveOdometry3dTest {
   private final MecanumDriveOdometry3d m_odometry =
       new MecanumDriveOdometry3d(m_kinematics, Rotation2d.kZero, zero);
 
+  @Test
+  void testInitialize() {
+    MecanumDriveOdometry3d odometry =
+        new MecanumDriveOdometry3d(
+            m_kinematics, Rotation2d.kZero, zero, new Pose2d(1, 2, Rotation2d.fromDegrees(45)));
+    var pose = odometry.getPoseMeters();
+    assertAll(
+        () -> assertEquals(pose.getX(), 1.0, 1e-9),
+        () -> assertEquals(pose.getY(), 2.0, 1e-9),
+        () -> assertEquals(pose.getRotation().getDegrees(), 45.0, 1e-9));
+  }
+
   @Test
   void testMultipleConsecutiveUpdates() {
     var wheelPositions = new MecanumDriveWheelPositions(3.536, 3.536, 3.536, 3.536);

diff --git a/wpimath/src/test/java/edu/wpi/first/math/kinematics/SwerveDriveOdometry3dTest.java b/wpimath/src/test/java/edu/wpi/first/math/kinematics/SwerveDriveOdometry3dTest.java
@@ -31,6 +31,21 @@ class SwerveDriveOdometry3dTest {
       new SwerveDriveOdometry3d(
           m_kinematics, Rotation2d.kZero, new SwerveModulePosition[] {zero, zero, zero, zero});
 
+  @Test
+  void testInitialize() {
+    SwerveDriveOdometry3d odometry =
+        new SwerveDriveOdometry3d(
+            m_kinematics,
+            Rotation2d.kZero,
+            new SwerveModulePosition[] {zero, zero, zero, zero},
+            new Pose2d(1, 2, Rotation2d.fromDegrees(45)));
+    var pose = odometry.getPoseMeters();
+    assertAll(
+        () -> assertEquals(pose.getX(), 1.0, 1e-9),
+        () -> assertEquals(pose.getY(), 2.0, 1e-9),
+        () -> assertEquals(pose.getRotation().getDegrees(), 45.0, 1e-9));
+  }
+
   @Test
   void testTwoIterations() {
     // 5 units/sec  in the x-axis (forward)

diff --git a/wpimath/src/test/native/cpp/kinematics/DifferentialDriveOdometry3dTest.cpp b/wpimath/src/test/native/cpp/kinematics/DifferentialDriveOdometry3dTest.cpp
@@ -13,6 +13,17 @@ static constexpr double kEpsilon = 1E-9;
 
 using namespace frc;
 
+TEST(DifferentialDriveOdometry3dTest, Initialize) {
+  DifferentialDriveOdometry3d odometry{90_deg, 0_m, 0_m,
+                                       frc::Pose2d{1_m, 2_m, 45_deg}};
+
+  const frc::Pose2d& pose = odometry.GetPose();
+
+  EXPECT_NEAR(pose.X().value(), 1, kEpsilon);
+  EXPECT_NEAR(pose.Y().value(), 2, kEpsilon);
+  EXPECT_NEAR(pose.Rotation().Degrees().value(), 45, kEpsilon);
+}
+
 TEST(DifferentialDriveOdometry3dTest, EncoderDistances) {
   DifferentialDriveOdometry3d odometry{45_deg, 0_m, 0_m};
 

diff --git a/wpimath/src/test/native/cpp/kinematics/MecanumDriveOdometry3dTest.cpp b/wpimath/src/test/native/cpp/kinematics/MecanumDriveOdometry3dTest.cpp
@@ -25,6 +25,17 @@ class MecanumDriveOdometry3dTest : public ::testing::Test {
   MecanumDriveOdometry3d odometry{kinematics, 0_rad, zero};
 };
 
+TEST_F(MecanumDriveOdometry3dTest, Foo) {
+  MecanumDriveOdometry3d odometry{kinematics, 0_rad, zero,
+                                  frc::Pose2d{1_m, 2_m, 45_deg}};
+
+  const frc::Pose2d& pose = odometry.GetPose();
+
+  EXPECT_NEAR(pose.X().value(), 1, 1e-9);
+  EXPECT_NEAR(pose.Y().value(), 2, 1e-9);
+  EXPECT_NEAR(pose.Rotation().Degrees().value(), 45, 1e-9);
+}
+
 TEST_F(MecanumDriveOdometry3dTest, MultipleConsecutiveUpdates) {
   MecanumDriveWheelPositions wheelDeltas{3.536_m, 3.536_m, 3.536_m, 3.536_m};
 

diff --git a/wpimath/src/test/native/cpp/kinematics/SwerveDriveOdometry3dTest.cpp b/wpimath/src/test/native/cpp/kinematics/SwerveDriveOdometry3dTest.cpp
@@ -30,6 +30,19 @@ class SwerveDriveOdometry3dTest : public ::testing::Test {
       m_kinematics, 0_rad, {zero, zero, zero, zero}};
 };
 
+TEST_F(SwerveDriveOdometry3dTest, Foo) {
+  SwerveDriveOdometry3d odometry{m_kinematics,
+                                 0_rad,
+                                 {zero, zero, zero, zero},
+                                 frc::Pose2d{1_m, 2_m, 45_deg}};
+
+  const frc::Pose2d& pose = odometry.GetPose();
+
+  EXPECT_NEAR(pose.X().value(), 1, kEpsilon);
+  EXPECT_NEAR(pose.Y().value(), 2, kEpsilon);
+  EXPECT_NEAR(pose.Rotation().Degrees().value(), 45, kEpsilon);
+}
+
 TEST_F(SwerveDriveOdometry3dTest, TwoIterations) {
   SwerveModulePosition position{0.5_m, 0_deg};