[wpimath] Position Delta Odometry for Mecanum (#4514)

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

@@ -8,20 +8,23 @@
 #include "frc/estimator/MecanumDrivePoseEstimator.h"
 #include "frc/geometry/Pose2d.h"
 #include "frc/kinematics/MecanumDriveKinematics.h"
-#include "frc/kinematics/MecanumDriveOdometry.h"
 #include "frc/trajectory/TrajectoryGenerator.h"
 #include "gtest/gtest.h"
 
-TEST(MecanumDrivePoseEstimatorTest, Accuracy) {
+TEST(MecanumDrivePoseEstimatorTest, AccuracyFacingTrajectory) {
   frc::MecanumDriveKinematics kinematics{
       frc::Translation2d{1_m, 1_m}, frc::Translation2d{1_m, -1_m},
       frc::Translation2d{-1_m, -1_m}, frc::Translation2d{-1_m, 1_m}};
 
-  frc::MecanumDrivePoseEstimator estimator{
-      frc::Rotation2d{}, frc::Pose2d{}, kinematics,
-      {0.1, 0.1, 0.1},   {0.05},        {0.1, 0.1, 0.1}};
+  frc::MecanumDriveWheelPositions wheelPositions;
 
-  frc::MecanumDriveOdometry odometry{kinematics, frc::Rotation2d{}};
+  frc::MecanumDrivePoseEstimator estimator{frc::Rotation2d{},
+                                           frc::Pose2d{},
+                                           wheelPositions,
+                                           kinematics,
+                                           {0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1},
+                                           {0.05, 0.05, 0.05, 0.05, 0.05},
+                                           {0.1, 0.1, 0.1}};
 
   frc::Trajectory trajectory = frc::TrajectoryGenerator::GenerateTrajectory(
       std::vector{frc::Pose2d{0_m, 0_m, 45_deg}, frc::Pose2d{3_m, 0_m, -90_deg},
@@ -66,11 +69,99 @@ TEST(MecanumDrivePoseEstimatorTest, Accuracy) {
         {groundTruthState.velocity, 0_mps,
          groundTruthState.velocity * groundTruthState.curvature});
 
+    wheelPositions.frontLeft += wheelSpeeds.frontLeft * dt;
+    wheelPositions.frontRight += wheelSpeeds.frontRight * dt;
+    wheelPositions.rearLeft += wheelSpeeds.rearLeft * dt;
+    wheelPositions.rearRight += wheelSpeeds.rearRight * dt;
+
     auto xhat = estimator.UpdateWithTime(
         t,
         groundTruthState.pose.Rotation() +
             frc::Rotation2d{distribution(generator) * 0.05_rad},
-        wheelSpeeds);
+        wheelSpeeds, wheelPositions);
+    double error = groundTruthState.pose.Translation()
+                       .Distance(xhat.Translation())
+                       .value();
+
+    if (error > maxError) {
+      maxError = error;
+    }
+    errorSum += error;
+
+    t += dt;
+  }
+
+  EXPECT_LT(errorSum / (trajectory.TotalTime().value() / dt.value()), 0.05);
+  EXPECT_LT(maxError, 0.125);
+}
+
+TEST(MecanumDrivePoseEstimatorTest, AccuracyFacingXAxis) {
+  frc::MecanumDriveKinematics kinematics{
+      frc::Translation2d{1_m, 1_m}, frc::Translation2d{1_m, -1_m},
+      frc::Translation2d{-1_m, -1_m}, frc::Translation2d{-1_m, 1_m}};
+
+  frc::MecanumDriveWheelPositions wheelPositions;
+
+  frc::MecanumDrivePoseEstimator estimator{frc::Rotation2d{},
+                                           frc::Pose2d{},
+                                           wheelPositions,
+                                           kinematics,
+                                           {0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1},
+                                           {0.05, 0.05, 0.05, 0.05, 0.05},
+                                           {0.1, 0.1, 0.1}};
+
+  frc::Trajectory trajectory = frc::TrajectoryGenerator::GenerateTrajectory(
+      std::vector{frc::Pose2d{0_m, 0_m, 45_deg}, frc::Pose2d{3_m, 0_m, -90_deg},
+                  frc::Pose2d{0_m, 0_m, 135_deg},
+                  frc::Pose2d{-3_m, 0_m, -90_deg},
+                  frc::Pose2d{0_m, 0_m, 45_deg}},
+      frc::TrajectoryConfig(5.0_mps, 2.0_mps_sq));
+
+  std::default_random_engine generator;
+  std::normal_distribution<double> distribution(0.0, 1.0);
+
+  units::second_t dt = 0.02_s;
+  units::second_t t = 0_s;
+
+  units::second_t kVisionUpdateRate = 0.1_s;
+  frc::Pose2d lastVisionPose;
+  units::second_t lastVisionUpdateTime{-std::numeric_limits<double>::max()};
+
+  std::vector<frc::Pose2d> visionPoses;
+
+  double maxError = -std::numeric_limits<double>::max();
+  double errorSum = 0;
+
+  while (t < trajectory.TotalTime()) {
+    frc::Trajectory::State groundTruthState = trajectory.Sample(t);
+
+    if (lastVisionUpdateTime + kVisionUpdateRate < t) {
+      if (lastVisionPose != frc::Pose2d{}) {
+        estimator.AddVisionMeasurement(lastVisionPose, lastVisionUpdateTime);
+      }
+      lastVisionPose =
+          groundTruthState.pose +
+          frc::Transform2d{
+              frc::Translation2d{distribution(generator) * 0.1_m,
+                                 distribution(generator) * 0.1_m},
+              frc::Rotation2d{distribution(generator) * 0.1 * 1_rad}};
+      visionPoses.push_back(lastVisionPose);
+      lastVisionUpdateTime = t;
+    }
+
+    auto wheelSpeeds = kinematics.ToWheelSpeeds(
+        {groundTruthState.velocity * groundTruthState.pose.Rotation().Cos(),
+         groundTruthState.velocity * groundTruthState.pose.Rotation().Sin(),
+         0_rad_per_s});
+
+    wheelPositions.frontLeft += wheelSpeeds.frontLeft * dt;
+    wheelPositions.frontRight += wheelSpeeds.frontRight * dt;
+    wheelPositions.rearLeft += wheelSpeeds.rearLeft * dt;
+    wheelPositions.rearRight += wheelSpeeds.rearRight * dt;
+
+    auto xhat = estimator.UpdateWithTime(
+        t, frc::Rotation2d{distribution(generator) * 0.05_rad}, wheelSpeeds,
+        wheelPositions);
     double error = groundTruthState.pose.Translation()
                        .Distance(xhat.Translation())
                        .value();

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

@@ -40,6 +40,15 @@ TEST_F(MecanumDriveKinematicsTest, StraightLineForwardKinematics) {
   EXPECT_NEAR(0.0, chassisSpeeds.omega.value(), 0.1);
 }
 
+TEST_F(MecanumDriveKinematicsTest, StraightLineForwardKinematicsWithDeltas) {
+  MecanumDriveWheelPositions wheelDeltas{5_m, 5_m, 5_m, 5_m};
+  auto twist = kinematics.ToTwist2d(wheelDeltas);
+
+  EXPECT_NEAR(5.0, twist.dx.value(), 0.1);
+  EXPECT_NEAR(0.0, twist.dy.value(), 0.1);
+  EXPECT_NEAR(0.0, twist.dtheta.value(), 0.1);
+}
+
 TEST_F(MecanumDriveKinematicsTest, StrafeInverseKinematics) {
   ChassisSpeeds speeds{0_mps, 4_mps, 0_rad_per_s};
   auto moduleStates = kinematics.ToWheelSpeeds(speeds);
@@ -59,6 +68,15 @@ TEST_F(MecanumDriveKinematicsTest, StrafeForwardKinematics) {
   EXPECT_NEAR(0.0, chassisSpeeds.omega.value(), 0.1);
 }
 
+TEST_F(MecanumDriveKinematicsTest, StrafeForwardKinematicsWithDeltas) {
+  MecanumDriveWheelPositions wheelDeltas{-5_m, 5_m, 5_m, -5_m};
+  auto twist = kinematics.ToTwist2d(wheelDeltas);
+
+  EXPECT_NEAR(0.0, twist.dx.value(), 0.1);
+  EXPECT_NEAR(5.0, twist.dy.value(), 0.1);
+  EXPECT_NEAR(0.0, twist.dtheta.value(), 0.1);
+}
+
 TEST_F(MecanumDriveKinematicsTest, RotationInverseKinematics) {
   ChassisSpeeds speeds{0_mps, 0_mps,
                        units::radians_per_second_t{2 * std::numbers::pi}};
@@ -80,6 +98,16 @@ TEST_F(MecanumDriveKinematicsTest, RotationForwardKinematics) {
   EXPECT_NEAR(2 * std::numbers::pi, chassisSpeeds.omega.value(), 0.1);
 }
 
+TEST_F(MecanumDriveKinematicsTest, RotationForwardKinematicsWithDeltas) {
+  MecanumDriveWheelPositions wheelDeltas{-150.79644737_m, 150.79644737_m,
+                                         -150.79644737_m, 150.79644737_m};
+  auto twist = kinematics.ToTwist2d(wheelDeltas);
+
+  EXPECT_NEAR(0.0, twist.dx.value(), 0.1);
+  EXPECT_NEAR(0.0, twist.dy.value(), 0.1);
+  EXPECT_NEAR(2 * std::numbers::pi, twist.dtheta.value(), 0.1);
+}
+
 TEST_F(MecanumDriveKinematicsTest, MixedRotationTranslationInverseKinematics) {
   ChassisSpeeds speeds{2_mps, 3_mps, 1_rad_per_s};
   auto moduleStates = kinematics.ToWheelSpeeds(speeds);
@@ -101,6 +129,18 @@ TEST_F(MecanumDriveKinematicsTest, MixedRotationTranslationForwardKinematics) {
   EXPECT_NEAR(0.707, chassisSpeeds.omega.value(), 0.1);
 }
 
+TEST_F(MecanumDriveKinematicsTest,
+       MixedRotationTranslationForwardKinematicsWithDeltas) {
+  MecanumDriveWheelPositions wheelDeltas{-17.677670_m, 20.506097_m, -13.435_m,
+                                         16.26_m};
+
+  auto twist = kinematics.ToTwist2d(wheelDeltas);
+
+  EXPECT_NEAR(1.41335, twist.dx.value(), 0.1);
+  EXPECT_NEAR(2.1221, twist.dy.value(), 0.1);
+  EXPECT_NEAR(0.707, twist.dtheta.value(), 0.1);
+}
+
 TEST_F(MecanumDriveKinematicsTest, OffCenterRotationInverseKinematics) {
   ChassisSpeeds speeds{0_mps, 0_mps, 1_rad_per_s};
   auto moduleStates = kinematics.ToWheelSpeeds(speeds, m_fl);
@@ -121,6 +161,16 @@ TEST_F(MecanumDriveKinematicsTest, OffCenterRotationForwardKinematics) {
   EXPECT_NEAR(0.707, chassisSpeeds.omega.value(), 0.1);
 }
 
+TEST_F(MecanumDriveKinematicsTest,
+       OffCenterRotationForwardKinematicsWithDeltas) {
+  MecanumDriveWheelPositions wheelDeltas{0_m, 16.971_m, -16.971_m, 33.941_m};
+  auto twist = kinematics.ToTwist2d(wheelDeltas);
+
+  EXPECT_NEAR(8.48525, twist.dx.value(), 0.1);
+  EXPECT_NEAR(-8.48525, twist.dy.value(), 0.1);
+  EXPECT_NEAR(0.707, twist.dtheta.value(), 0.1);
+}
+
 TEST_F(MecanumDriveKinematicsTest,
        OffCenterTranslationRotationInverseKinematics) {
   ChassisSpeeds speeds{5_mps, 2_mps, 1_rad_per_s};
@@ -143,6 +193,16 @@ TEST_F(MecanumDriveKinematicsTest,
   EXPECT_NEAR(0.707, chassisSpeeds.omega.value(), 0.1);
 }
 
+TEST_F(MecanumDriveKinematicsTest,
+       OffCenterTranslationRotationForwardKinematicsWithDeltas) {
+  MecanumDriveWheelPositions wheelDeltas{2.12_m, 21.92_m, -12.02_m, 36.06_m};
+  auto twist = kinematics.ToTwist2d(wheelDeltas);
+
+  EXPECT_NEAR(12.02, twist.dx.value(), 0.1);
+  EXPECT_NEAR(-7.07, twist.dy.value(), 0.1);
+  EXPECT_NEAR(0.707, twist.dtheta.value(), 0.1);
+}
+
 TEST_F(MecanumDriveKinematicsTest, Desaturate) {
   MecanumDriveWheelSpeeds wheelSpeeds{5_mps, 6_mps, 4_mps, 7_mps};
   wheelSpeeds.Desaturate(5.5_mps);

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

@@ -2,7 +2,11 @@
 // 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 <limits>
+#include <random>
+
 #include "frc/kinematics/MecanumDriveOdometry.h"
+#include "frc/trajectory/TrajectoryGenerator.h"
 #include "gtest/gtest.h"
 
 using namespace frc;
@@ -14,17 +18,19 @@ class MecanumDriveOdometryTest : public ::testing::Test {
   Translation2d m_bl{-12_m, 12_m};
   Translation2d m_br{-12_m, -12_m};
 
+  MecanumDriveWheelPositions zero;
+
   MecanumDriveKinematics kinematics{m_fl, m_fr, m_bl, m_br};
-  MecanumDriveOdometry odometry{kinematics, 0_rad};
+  MecanumDriveOdometry odometry{kinematics, 0_rad, zero};
 };
 
 TEST_F(MecanumDriveOdometryTest, MultipleConsecutiveUpdates) {
-  odometry.ResetPosition(Pose2d{}, 0_rad);
-  MecanumDriveWheelSpeeds wheelSpeeds{3.536_mps, 3.536_mps, 3.536_mps,
-                                      3.536_mps};
+  MecanumDriveWheelPositions wheelDeltas{3.536_m, 3.536_m, 3.536_m, 3.536_m};
 
-  odometry.UpdateWithTime(0_s, 0_deg, wheelSpeeds);
-  auto secondPose = odometry.UpdateWithTime(0.0_s, 0_deg, wheelSpeeds);
+  odometry.ResetPosition(Pose2d{}, 0_rad, wheelDeltas);
+
+  odometry.Update(0_deg, wheelDeltas);
+  auto secondPose = odometry.Update(0_deg, wheelDeltas);
 
   EXPECT_NEAR(secondPose.X().value(), 0.0, 0.01);
   EXPECT_NEAR(secondPose.Y().value(), 0.0, 0.01);
@@ -32,11 +38,12 @@ TEST_F(MecanumDriveOdometryTest, MultipleConsecutiveUpdates) {
 }
 
 TEST_F(MecanumDriveOdometryTest, TwoIterations) {
-  odometry.ResetPosition(Pose2d{}, 0_rad);
-  MecanumDriveWheelSpeeds speeds{3.536_mps, 3.536_mps, 3.536_mps, 3.536_mps};
+  odometry.ResetPosition(Pose2d{}, 0_rad, zero);
+  MecanumDriveWheelPositions wheelDeltas{0.3536_m, 0.3536_m, 0.3536_m,
+                                         0.3536_m};
 
-  odometry.UpdateWithTime(0_s, 0_deg, MecanumDriveWheelSpeeds{});
-  auto pose = odometry.UpdateWithTime(0.10_s, 0_deg, speeds);
+  odometry.Update(0_deg, MecanumDriveWheelPositions{});
+  auto pose = odometry.Update(0_deg, wheelDeltas);
 
   EXPECT_NEAR(pose.X().value(), 0.3536, 0.01);
   EXPECT_NEAR(pose.Y().value(), 0.0, 0.01);
@@ -44,11 +51,11 @@ TEST_F(MecanumDriveOdometryTest, TwoIterations) {
 }
 
 TEST_F(MecanumDriveOdometryTest, 90DegreeTurn) {
-  odometry.ResetPosition(Pose2d{}, 0_rad);
-  MecanumDriveWheelSpeeds speeds{-13.328_mps, 39.986_mps, -13.329_mps,
-                                 39.986_mps};
-  odometry.UpdateWithTime(0_s, 0_deg, MecanumDriveWheelSpeeds{});
-  auto pose = odometry.UpdateWithTime(1_s, 90_deg, speeds);
+  odometry.ResetPosition(Pose2d{}, 0_rad, zero);
+  MecanumDriveWheelPositions wheelDeltas{-13.328_m, 39.986_m, -13.329_m,
+                                         39.986_m};
+  odometry.Update(0_deg, MecanumDriveWheelPositions{});
+  auto pose = odometry.Update(90_deg, wheelDeltas);
 
   EXPECT_NEAR(pose.X().value(), 8.4855, 0.01);
   EXPECT_NEAR(pose.Y().value(), 8.4855, 0.01);
@@ -56,14 +63,140 @@ TEST_F(MecanumDriveOdometryTest, 90DegreeTurn) {
 }
 
 TEST_F(MecanumDriveOdometryTest, GyroAngleReset) {
-  odometry.ResetPosition(Pose2d{}, 90_deg);
+  odometry.ResetPosition(Pose2d{}, 90_deg, zero);
 
-  MecanumDriveWheelSpeeds speeds{3.536_mps, 3.536_mps, 3.536_mps, 3.536_mps};
+  MecanumDriveWheelPositions wheelDeltas{0.3536_m, 0.3536_m, 0.3536_m,
+                                         0.3536_m};
 
-  odometry.UpdateWithTime(0_s, 90_deg, MecanumDriveWheelSpeeds{});
-  auto pose = odometry.UpdateWithTime(0.10_s, 90_deg, speeds);
+  odometry.Update(90_deg, MecanumDriveWheelPositions{});
+  auto pose = odometry.Update(90_deg, wheelDeltas);
 
   EXPECT_NEAR(pose.X().value(), 0.3536, 0.01);
   EXPECT_NEAR(pose.Y().value(), 0.0, 0.01);
   EXPECT_NEAR(pose.Rotation().Radians().value(), 0.0, 0.01);
 }
+
+TEST_F(MecanumDriveOdometryTest, AccuracyFacingTrajectory) {
+  frc::MecanumDriveKinematics kinematics{
+      frc::Translation2d{1_m, 1_m}, frc::Translation2d{1_m, -1_m},
+      frc::Translation2d{-1_m, -1_m}, frc::Translation2d{-1_m, 1_m}};
+
+  frc::MecanumDriveWheelPositions wheelPositions;
+
+  frc::MecanumDriveOdometry odometry{kinematics, frc::Rotation2d{},
+                                     wheelPositions, frc::Pose2d{}};
+
+  frc::Trajectory trajectory = frc::TrajectoryGenerator::GenerateTrajectory(
+      std::vector{frc::Pose2d{0_m, 0_m, 45_deg}, frc::Pose2d{3_m, 0_m, -90_deg},
+                  frc::Pose2d{0_m, 0_m, 135_deg},
+                  frc::Pose2d{-3_m, 0_m, -90_deg},
+                  frc::Pose2d{0_m, 0_m, 45_deg}},
+      frc::TrajectoryConfig(5.0_mps, 2.0_mps_sq));
+
+  std::default_random_engine generator;
+  std::normal_distribution<double> distribution(0.0, 1.0);
+
+  units::second_t dt = 0.02_s;
+  units::second_t t = 0_s;
+
+  double maxError = -std::numeric_limits<double>::max();
+  double errorSum = 0;
+
+  while (t < trajectory.TotalTime()) {
+    frc::Trajectory::State groundTruthState = trajectory.Sample(t);
+
+    auto wheelSpeeds = kinematics.ToWheelSpeeds(
+        {groundTruthState.velocity, 0_mps,
+         groundTruthState.velocity * groundTruthState.curvature});
+
+    wheelSpeeds.frontLeft += distribution(generator) * 0.1_mps;
+    wheelSpeeds.frontRight += distribution(generator) * 0.1_mps;
+    wheelSpeeds.rearLeft += distribution(generator) * 0.1_mps;
+    wheelSpeeds.rearRight += distribution(generator) * 0.1_mps;
+
+    wheelPositions.frontLeft += wheelSpeeds.frontLeft * dt;
+    wheelPositions.frontRight += wheelSpeeds.frontRight * dt;
+    wheelPositions.rearLeft += wheelSpeeds.rearLeft * dt;
+    wheelPositions.rearRight += wheelSpeeds.rearRight * dt;
+
+    auto xhat =
+        odometry.Update(groundTruthState.pose.Rotation() +
+                            frc::Rotation2d{distribution(generator) * 0.05_rad},
+                        wheelPositions);
+    double error = groundTruthState.pose.Translation()
+                       .Distance(xhat.Translation())
+                       .value();
+
+    if (error > maxError) {
+      maxError = error;
+    }
+    errorSum += error;
+
+    t += dt;
+  }
+
+  EXPECT_LT(errorSum / (trajectory.TotalTime().value() / dt.value()), 0.06);
+  EXPECT_LT(maxError, 0.125);
+}
+
+TEST_F(MecanumDriveOdometryTest, AccuracyFacingXAxis) {
+  frc::MecanumDriveKinematics kinematics{
+      frc::Translation2d{1_m, 1_m}, frc::Translation2d{1_m, -1_m},
+      frc::Translation2d{-1_m, -1_m}, frc::Translation2d{-1_m, 1_m}};
+
+  frc::MecanumDriveWheelPositions wheelPositions;
+
+  frc::MecanumDriveOdometry odometry{kinematics, frc::Rotation2d{},
+                                     wheelPositions, frc::Pose2d{}};
+
+  frc::Trajectory trajectory = frc::TrajectoryGenerator::GenerateTrajectory(
+      std::vector{frc::Pose2d{0_m, 0_m, 45_deg}, frc::Pose2d{3_m, 0_m, -90_deg},
+                  frc::Pose2d{0_m, 0_m, 135_deg},
+                  frc::Pose2d{-3_m, 0_m, -90_deg},
+                  frc::Pose2d{0_m, 0_m, 45_deg}},
+      frc::TrajectoryConfig(5.0_mps, 2.0_mps_sq));
+
+  std::default_random_engine generator;
+  std::normal_distribution<double> distribution(0.0, 1.0);
+
+  units::second_t dt = 0.02_s;
+  units::second_t t = 0_s;
+
+  double maxError = -std::numeric_limits<double>::max();
+  double errorSum = 0;
+
+  while (t < trajectory.TotalTime()) {
+    frc::Trajectory::State groundTruthState = trajectory.Sample(t);
+
+    auto wheelSpeeds = kinematics.ToWheelSpeeds(
+        {groundTruthState.velocity * groundTruthState.pose.Rotation().Cos(),
+         groundTruthState.velocity * groundTruthState.pose.Rotation().Sin(),
+         0_rad_per_s});
+
+    wheelSpeeds.frontLeft += distribution(generator) * 0.1_mps;
+    wheelSpeeds.frontRight += distribution(generator) * 0.1_mps;
+    wheelSpeeds.rearLeft += distribution(generator) * 0.1_mps;
+    wheelSpeeds.rearRight += distribution(generator) * 0.1_mps;
+
+    wheelPositions.frontLeft += wheelSpeeds.frontLeft * dt;
+    wheelPositions.frontRight += wheelSpeeds.frontRight * dt;
+    wheelPositions.rearLeft += wheelSpeeds.rearLeft * dt;
+    wheelPositions.rearRight += wheelSpeeds.rearRight * dt;
+
+    auto xhat = odometry.Update(
+        frc::Rotation2d{distribution(generator) * 0.05_rad}, wheelPositions);
+    double error = groundTruthState.pose.Translation()
+                       .Distance(xhat.Translation())
+                       .value();
+
+    if (error > maxError) {
+      maxError = error;
+    }
+    errorSum += error;
+
+    t += dt;
+  }
+
+  EXPECT_LT(errorSum / (trajectory.TotalTime().value() / dt.value()), 0.06);
+  EXPECT_LT(maxError, 0.125);
+}