[wpimath] Position Delta Odometry for Swerve (#4493)

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

@@ -8,20 +8,27 @@
 #include "frc/estimator/SwerveDrivePoseEstimator.h"
 #include "frc/geometry/Pose2d.h"
 #include "frc/kinematics/SwerveDriveKinematics.h"
-#include "frc/kinematics/SwerveDriveOdometry.h"
 #include "frc/trajectory/TrajectoryGenerator.h"
 #include "gtest/gtest.h"
 
-TEST(SwerveDrivePoseEstimatorTest, Accuracy) {
+TEST(SwerveDrivePoseEstimatorTest, AccuracyFacingTrajectory) {
   frc::SwerveDriveKinematics<4> 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::SwerveDrivePoseEstimator<4> estimator{
-      frc::Rotation2d{}, frc::Pose2d{}, kinematics,
-      {0.1, 0.1, 0.1},   {0.05},        {0.1, 0.1, 0.1}};
+  frc::SwerveModulePosition fl;
+  frc::SwerveModulePosition fr;
+  frc::SwerveModulePosition bl;
+  frc::SwerveModulePosition br;
 
-  frc::SwerveDriveOdometry<4> odometry{kinematics, frc::Rotation2d{}};
+  frc::SwerveDrivePoseEstimator<4> estimator{
+      frc::Rotation2d{},
+      frc::Pose2d{},
+      {fl, fr, bl, br},
+      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},
@@ -54,10 +61,9 @@ TEST(SwerveDrivePoseEstimatorTest, Accuracy) {
       }
       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}};
+          frc::Transform2d{frc::Translation2d{distribution(generator) * 0.1_m,
+                                              distribution(generator) * 0.1_m},
+                           frc::Rotation2d{distribution(generator) * 0.1_rad}};
       visionPoses.push_back(lastVisionPose);
       lastVisionUpdateTime = t;
     }
@@ -66,11 +72,116 @@ TEST(SwerveDrivePoseEstimatorTest, Accuracy) {
         {groundTruthState.velocity, 0_mps,
          groundTruthState.velocity * groundTruthState.curvature});
 
+    fl.distance += moduleStates[0].speed * dt;
+    fr.distance += moduleStates[1].speed * dt;
+    bl.distance += moduleStates[2].speed * dt;
+    br.distance += moduleStates[3].speed * dt;
+
+    fl.angle = moduleStates[0].angle;
+    fr.angle = moduleStates[1].angle;
+    bl.angle = moduleStates[2].angle;
+    br.angle = moduleStates[3].angle;
+
     auto xhat = estimator.UpdateWithTime(
         t,
         groundTruthState.pose.Rotation() +
             frc::Rotation2d{distribution(generator) * 0.05_rad},
-        moduleStates[0], moduleStates[1], moduleStates[2], moduleStates[3]);
+        moduleStates, {fl, fr, bl, br});
+    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(SwerveDrivePoseEstimatorTest, AccuracyFacingXAxis) {
+  frc::SwerveDriveKinematics<4> 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::SwerveModulePosition fl;
+  frc::SwerveModulePosition fr;
+  frc::SwerveModulePosition bl;
+  frc::SwerveModulePosition br;
+
+  frc::SwerveDrivePoseEstimator<4> estimator{
+      frc::Rotation2d{},
+      frc::Pose2d{},
+      {fl, fr, bl, br},
+      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_rad}};
+      visionPoses.push_back(lastVisionPose);
+      lastVisionUpdateTime = t;
+    }
+
+    auto moduleStates = kinematics.ToSwerveModuleStates(
+        {groundTruthState.velocity * groundTruthState.pose.Rotation().Cos(),
+         groundTruthState.velocity * groundTruthState.pose.Rotation().Sin(),
+         0_rad_per_s});
+
+    fl.distance += groundTruthState.velocity * dt +
+                   0.5 * groundTruthState.acceleration * dt * dt;
+    fr.distance += groundTruthState.velocity * dt +
+                   0.5 * groundTruthState.acceleration * dt * dt;
+    bl.distance += groundTruthState.velocity * dt +
+                   0.5 * groundTruthState.acceleration * dt * dt;
+    br.distance += groundTruthState.velocity * dt +
+                   0.5 * groundTruthState.acceleration * dt * dt;
+
+    fl.angle = groundTruthState.pose.Rotation();
+    fr.angle = groundTruthState.pose.Rotation();
+    bl.angle = groundTruthState.pose.Rotation();
+    br.angle = groundTruthState.pose.Rotation();
+
+    auto xhat = estimator.UpdateWithTime(
+        t, frc::Rotation2d{distribution(generator) * 0.05_rad}, moduleStates,
+        {fl, fr, bl, br});
     double error = groundTruthState.pose.Translation()
                        .Distance(xhat.Translation())
                        .value();

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

@@ -49,6 +49,16 @@ TEST_F(SwerveDriveKinematicsTest, StraightLineForwardKinematics) {
   EXPECT_NEAR(chassisSpeeds.omega.value(), 0.0, kEpsilon);
 }
 
+TEST_F(SwerveDriveKinematicsTest, StraightLineForwardKinematicsWithDeltas) {
+  SwerveModulePosition delta{5.0_m, 0_deg};
+
+  auto twist = m_kinematics.ToTwist2d(delta, delta, delta, delta);
+
+  EXPECT_NEAR(twist.dx.value(), 5.0, kEpsilon);
+  EXPECT_NEAR(twist.dy.value(), 0.0, kEpsilon);
+  EXPECT_NEAR(twist.dtheta.value(), 0.0, kEpsilon);
+}
+
 TEST_F(SwerveDriveKinematicsTest, StraightStrafeInverseKinematics) {
   ChassisSpeeds speeds{0_mps, 5_mps, 0_rad_per_s};
   auto [fl, fr, bl, br] = m_kinematics.ToSwerveModuleStates(speeds);
@@ -73,6 +83,16 @@ TEST_F(SwerveDriveKinematicsTest, StraightStrafeForwardKinematics) {
   EXPECT_NEAR(chassisSpeeds.omega.value(), 0.0, kEpsilon);
 }
 
+TEST_F(SwerveDriveKinematicsTest, StraightStrafeForwardKinematicsWithDeltas) {
+  SwerveModulePosition delta{5_m, 90_deg};
+
+  auto twist = m_kinematics.ToTwist2d(delta, delta, delta, delta);
+
+  EXPECT_NEAR(twist.dx.value(), 0.0, kEpsilon);
+  EXPECT_NEAR(twist.dy.value(), 5.0, kEpsilon);
+  EXPECT_NEAR(twist.dtheta.value(), 0.0, kEpsilon);
+}
+
 TEST_F(SwerveDriveKinematicsTest, TurnInPlaceInverseKinematics) {
   ChassisSpeeds speeds{0_mps, 0_mps,
                        units::radians_per_second_t{2 * std::numbers::pi}};
@@ -119,6 +139,19 @@ TEST_F(SwerveDriveKinematicsTest, TurnInPlaceForwardKinematics) {
   EXPECT_NEAR(chassisSpeeds.omega.value(), 2 * std::numbers::pi, kEpsilon);
 }
 
+TEST_F(SwerveDriveKinematicsTest, TurnInPlaceForwardKinematicsWithDeltas) {
+  SwerveModulePosition fl{106.629_m, 135_deg};
+  SwerveModulePosition fr{106.629_m, 45_deg};
+  SwerveModulePosition bl{106.629_m, -135_deg};
+  SwerveModulePosition br{106.629_m, -45_deg};
+
+  auto twist = m_kinematics.ToTwist2d(fl, fr, bl, br);
+
+  EXPECT_NEAR(twist.dx.value(), 0.0, kEpsilon);
+  EXPECT_NEAR(twist.dy.value(), 0.0, kEpsilon);
+  EXPECT_NEAR(twist.dtheta.value(), 2 * std::numbers::pi, kEpsilon);
+}
+
 TEST_F(SwerveDriveKinematicsTest, OffCenterCORRotationInverseKinematics) {
   ChassisSpeeds speeds{0_mps, 0_mps,
                        units::radians_per_second_t{2 * std::numbers::pi}};
@@ -148,6 +181,20 @@ TEST_F(SwerveDriveKinematicsTest, OffCenterCORRotationForwardKinematics) {
   EXPECT_NEAR(chassisSpeeds.omega.value(), 2 * std::numbers::pi, kEpsilon);
 }
 
+TEST_F(SwerveDriveKinematicsTest,
+       OffCenterCORRotationForwardKinematicsWithDeltas) {
+  SwerveModulePosition fl{0.0_m, 0_deg};
+  SwerveModulePosition fr{150.796_m, 0_deg};
+  SwerveModulePosition bl{150.796_m, -90_deg};
+  SwerveModulePosition br{213.258_m, -45_deg};
+
+  auto twist = m_kinematics.ToTwist2d(fl, fr, bl, br);
+
+  EXPECT_NEAR(twist.dx.value(), 75.398, kEpsilon);
+  EXPECT_NEAR(twist.dy.value(), -75.398, kEpsilon);
+  EXPECT_NEAR(twist.dtheta.value(), 2 * std::numbers::pi, kEpsilon);
+}
+
 TEST_F(SwerveDriveKinematicsTest,
        OffCenterCORRotationAndTranslationInverseKinematics) {
   ChassisSpeeds speeds{0_mps, 3.0_mps, 1.5_rad_per_s};
@@ -179,6 +226,20 @@ TEST_F(SwerveDriveKinematicsTest,
   EXPECT_NEAR(chassisSpeeds.omega.value(), 1.5, kEpsilon);
 }
 
+TEST_F(SwerveDriveKinematicsTest,
+       OffCenterCORRotationAndTranslationForwardKinematicsWithDeltas) {
+  SwerveModulePosition fl{23.43_m, -140.19_deg};
+  SwerveModulePosition fr{23.43_m, -39.81_deg};
+  SwerveModulePosition bl{54.08_m, -109.44_deg};
+  SwerveModulePosition br{54.08_m, -70.56_deg};
+
+  auto twist = m_kinematics.ToTwist2d(fl, fr, bl, br);
+
+  EXPECT_NEAR(twist.dx.value(), 0.0, kEpsilon);
+  EXPECT_NEAR(twist.dy.value(), -33.0, kEpsilon);
+  EXPECT_NEAR(twist.dtheta.value(), 1.5, kEpsilon);
+}
+
 TEST_F(SwerveDriveKinematicsTest, Desaturate) {
   SwerveModuleState state1{5.0_mps, 0_deg};
   SwerveModuleState state2{6.0_mps, 0_deg};

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

@@ -2,8 +2,14 @@
 // 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/SwerveDriveKinematics.h"
 #include "frc/kinematics/SwerveDriveOdometry.h"
+#include "frc/trajectory/Trajectory.h"
+#include "frc/trajectory/TrajectoryConfig.h"
+#include "frc/trajectory/TrajectoryGenerator.h"
 #include "gtest/gtest.h"
 
 using namespace frc;
@@ -18,18 +24,19 @@ class SwerveDriveOdometryTest : public ::testing::Test {
   Translation2d m_br{-12_m, -12_m};
 
   SwerveDriveKinematics<4> m_kinematics{m_fl, m_fr, m_bl, m_br};
-  SwerveDriveOdometry<4> m_odometry{m_kinematics, 0_rad};
+  SwerveModulePosition zero;
+  SwerveDriveOdometry<4> m_odometry{
+      m_kinematics, 0_rad, {zero, zero, zero, zero}};
 };
 
 TEST_F(SwerveDriveOdometryTest, TwoIterations) {
-  SwerveModuleState state{5_mps, 0_deg};
+  SwerveModulePosition position{0.5_m, 0_deg};
 
-  m_odometry.ResetPosition(Pose2d{}, 0_rad);
-  m_odometry.UpdateWithTime(0_s, 0_deg, SwerveModuleState{},
-                            SwerveModuleState{}, SwerveModuleState{},
-                            SwerveModuleState{});
-  auto pose =
-      m_odometry.UpdateWithTime(0.1_s, 0_deg, state, state, state, state);
+  m_odometry.ResetPosition(Pose2d{}, 0_rad, zero, zero, zero, zero);
+
+  m_odometry.Update(0_deg, zero, zero, zero, zero);
+
+  auto pose = m_odometry.Update(0_deg, position, position, position, position);
 
   EXPECT_NEAR(0.5, pose.X().value(), kEpsilon);
   EXPECT_NEAR(0.0, pose.Y().value(), kEpsilon);
@@ -37,16 +44,13 @@ TEST_F(SwerveDriveOdometryTest, TwoIterations) {
 }
 
 TEST_F(SwerveDriveOdometryTest, 90DegreeTurn) {
-  SwerveModuleState fl{18.85_mps, 90_deg};
-  SwerveModuleState fr{42.15_mps, 26.565_deg};
-  SwerveModuleState bl{18.85_mps, -90_deg};
-  SwerveModuleState br{42.15_mps, -26.565_deg};
+  SwerveModulePosition fl{18.85_m, 90_deg};
+  SwerveModulePosition fr{42.15_m, 26.565_deg};
+  SwerveModulePosition bl{18.85_m, -90_deg};
+  SwerveModulePosition br{42.15_m, -26.565_deg};
 
-  SwerveModuleState zero{0_mps, 0_deg};
-
-  m_odometry.ResetPosition(Pose2d{}, 0_rad);
-  m_odometry.UpdateWithTime(0_s, 0_deg, zero, zero, zero, zero);
-  auto pose = m_odometry.UpdateWithTime(1_s, 90_deg, fl, fr, bl, br);
+  m_odometry.ResetPosition(Pose2d{}, 0_rad, zero, zero, zero, zero);
+  auto pose = m_odometry.Update(90_deg, fl, fr, bl, br);
 
   EXPECT_NEAR(12.0, pose.X().value(), kEpsilon);
   EXPECT_NEAR(12.0, pose.Y().value(), kEpsilon);
@@ -54,17 +58,139 @@ TEST_F(SwerveDriveOdometryTest, 90DegreeTurn) {
 }
 
 TEST_F(SwerveDriveOdometryTest, GyroAngleReset) {
-  m_odometry.ResetPosition(Pose2d{}, 90_deg);
+  m_odometry.ResetPosition(Pose2d{}, 90_deg, zero, zero, zero, zero);
 
-  SwerveModuleState state{5_mps, 0_deg};
+  SwerveModulePosition position{0.5_m, 0_deg};
 
-  m_odometry.UpdateWithTime(0_s, 90_deg, SwerveModuleState{},
-                            SwerveModuleState{}, SwerveModuleState{},
-                            SwerveModuleState{});
-  auto pose =
-      m_odometry.UpdateWithTime(0.1_s, 90_deg, state, state, state, state);
+  auto pose = m_odometry.Update(90_deg, position, position, position, position);
 
   EXPECT_NEAR(0.5, pose.X().value(), kEpsilon);
   EXPECT_NEAR(0.0, pose.Y().value(), kEpsilon);
   EXPECT_NEAR(0.0, pose.Rotation().Degrees().value(), kEpsilon);
 }
+
+TEST_F(SwerveDriveOdometryTest, AccuracyFacingTrajectory) {
+  SwerveDriveKinematics<4> kinematics{
+      Translation2d{1_m, 1_m}, Translation2d{1_m, -1_m},
+      Translation2d{-1_m, -1_m}, Translation2d{-1_m, 1_m}};
+
+  SwerveDriveOdometry<4> odometry{kinematics, 0_rad, {zero, zero, zero, zero}};
+
+  SwerveModulePosition fl;
+  SwerveModulePosition fr;
+  SwerveModulePosition bl;
+  SwerveModulePosition br;
+
+  Trajectory trajectory = TrajectoryGenerator::GenerateTrajectory(
+      std::vector{Pose2d{0_m, 0_m, 45_deg}, Pose2d{3_m, 0_m, -90_deg},
+                  Pose2d{0_m, 0_m, 135_deg}, Pose2d{-3_m, 0_m, -90_deg},
+                  Pose2d{0_m, 0_m, 45_deg}},
+      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()) {
+    Trajectory::State groundTruthState = trajectory.Sample(t);
+
+    auto moduleStates = kinematics.ToSwerveModuleStates(
+        {groundTruthState.velocity, 0_mps,
+         groundTruthState.velocity * groundTruthState.curvature});
+
+    fl.distance += moduleStates[0].speed * dt;
+    fr.distance += moduleStates[1].speed * dt;
+    bl.distance += moduleStates[2].speed * dt;
+    br.distance += moduleStates[3].speed * dt;
+
+    fl.angle = moduleStates[0].angle;
+    fr.angle = moduleStates[1].angle;
+    bl.angle = moduleStates[2].angle;
+    br.angle = moduleStates[3].angle;
+
+    auto xhat =
+        odometry.Update(groundTruthState.pose.Rotation() +
+                            frc::Rotation2d{distribution(generator) * 0.05_rad},
+                        fl, fr, bl, br);
+    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_F(SwerveDriveOdometryTest, AccuracyFacingXAxis) {
+  SwerveDriveKinematics<4> kinematics{
+      Translation2d{1_m, 1_m}, Translation2d{1_m, -1_m},
+      Translation2d{-1_m, -1_m}, Translation2d{-1_m, 1_m}};
+
+  SwerveDriveOdometry<4> odometry{kinematics, 0_rad, {zero, zero, zero, zero}};
+
+  SwerveModulePosition fl;
+  SwerveModulePosition fr;
+  SwerveModulePosition bl;
+  SwerveModulePosition br;
+
+  Trajectory trajectory = TrajectoryGenerator::GenerateTrajectory(
+      std::vector{Pose2d{0_m, 0_m, 45_deg}, Pose2d{3_m, 0_m, -90_deg},
+                  Pose2d{0_m, 0_m, 135_deg}, Pose2d{-3_m, 0_m, -90_deg},
+                  Pose2d{0_m, 0_m, 45_deg}},
+      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()) {
+    Trajectory::State groundTruthState = trajectory.Sample(t);
+
+    fl.distance += groundTruthState.velocity * dt +
+                   0.5 * groundTruthState.acceleration * dt * dt;
+    fr.distance += groundTruthState.velocity * dt +
+                   0.5 * groundTruthState.acceleration * dt * dt;
+    bl.distance += groundTruthState.velocity * dt +
+                   0.5 * groundTruthState.acceleration * dt * dt;
+    br.distance += groundTruthState.velocity * dt +
+                   0.5 * groundTruthState.acceleration * dt * dt;
+
+    fl.angle = groundTruthState.pose.Rotation();
+    fr.angle = groundTruthState.pose.Rotation();
+    bl.angle = groundTruthState.pose.Rotation();
+    br.angle = groundTruthState.pose.Rotation();
+
+    auto xhat = odometry.Update(
+        frc::Rotation2d{distribution(generator) * 0.05_rad}, fl, fr, bl, br);
+    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);
+}