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

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);
+}