Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-23 01:21:42 +00:00
Code Issues Packages Projects Releases Wiki Activity

[wpimath] Add 3D odometry and pose estimation (#7119)

Browse Source
This commit is contained in:
Joseph Eng
2024-11-16 07:56:14 -08:00
committed by GitHub
parent aa7dd258c4
commit 2acf111f56
49 changed files with 6716 additions and 116 deletions
Download Patch File Download Diff File Expand all files Collapse all files

462
wpimath/src/test/native/cpp/estimator/DifferentialDrivePoseEstimator3dTest.cpp Normal file
View File

@@ -0,0 +1,462 @@
// 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 <limits>
#include <random>
#include <tuple>
#include <utility>
#include <vector>
#include <gtest/gtest.h>
#include <wpi/print.h>
#include "frc/StateSpaceUtil.h"
#include "frc/estimator/DifferentialDrivePoseEstimator3d.h"
#include "frc/geometry/Pose2d.h"
#include "frc/geometry/Rotation2d.h"
#include "frc/kinematics/DifferentialDriveKinematics.h"
#include "frc/trajectory/TrajectoryGenerator.h"
#include "units/angle.h"
#include "units/length.h"
#include "units/time.h"
void testFollowTrajectory(
const frc::DifferentialDriveKinematics& kinematics,
frc::DifferentialDrivePoseEstimator3d& estimator,
const frc::Trajectory& trajectory,
std::function<frc::ChassisSpeeds(frc::Trajectory::State&)>
chassisSpeedsGenerator,
std::function<frc::Pose2d(frc::Trajectory::State&)>
visionMeasurementGenerator,
const frc::Pose2d& startingPose, const frc::Pose2d& endingPose,
const units::second_t dt, const units::second_t kVisionUpdateRate,
const units::second_t kVisionUpdateDelay, const bool checkError,
const bool debug) {
units::meter_t leftDistance = 0_m;
units::meter_t rightDistance = 0_m;
estimator.ResetPosition(frc::Rotation3d{}, leftDistance, rightDistance,
frc::Pose3d{startingPose});
std::default_random_engine generator;
std::normal_distribution<double> distribution(0.0, 1.0);
units::second_t t = 0_s;
std::vector<std::pair<units::second_t, frc::Pose2d>> visionPoses;
std::vector<std::tuple<units::second_t, units::second_t, frc::Pose2d>>
visionLog;
double maxError = -std::numeric_limits<double>::max();
double errorSum = 0;
if (debug) {
wpi::print(
"time, est_x, est_y, est_theta, true_x, true_y, true_theta, left, "
"right\n");
}
while (t < trajectory.TotalTime()) {
frc::Trajectory::State groundTruthState = trajectory.Sample(t);
// We are due for a new vision measurement if it's been `visionUpdateRate`
// seconds since the last vision measurement
if (visionPoses.empty() ||
visionPoses.back().first + kVisionUpdateRate < t) {
auto visionPose =
visionMeasurementGenerator(groundTruthState) +
frc::Transform2d{frc::Translation2d{distribution(generator) * 0.1_m,
distribution(generator) * 0.1_m},
frc::Rotation2d{distribution(generator) * 0.05_rad}};
visionPoses.push_back({t, visionPose});
}
// We should apply the oldest vision measurement if it has been
// `visionUpdateDelay` seconds since it was measured
if (!visionPoses.empty() &&
visionPoses.front().first + kVisionUpdateDelay < t) {
auto visionEntry = visionPoses.front();
estimator.AddVisionMeasurement(frc::Pose3d{visionEntry.second},
visionEntry.first);
visionPoses.erase(visionPoses.begin());
visionLog.push_back({t, visionEntry.first, visionEntry.second});
}
auto chassisSpeeds = chassisSpeedsGenerator(groundTruthState);
auto wheelSpeeds = kinematics.ToWheelSpeeds(chassisSpeeds);
leftDistance += wheelSpeeds.left * dt;
rightDistance += wheelSpeeds.right * dt;
auto xhat = estimator.UpdateWithTime(
t,
frc::Rotation3d{groundTruthState.pose.Rotation() +
frc::Rotation2d{distribution(generator) * 0.05_rad} -
trajectory.InitialPose().Rotation()},
leftDistance, rightDistance);
if (debug) {
wpi::print(
"{}, {}, {}, {}, {}, {}, {}, {}, {}\n", t.value(), xhat.X().value(),
xhat.Y().value(), xhat.Rotation().ToRotation2d().Radians().value(),
groundTruthState.pose.X().value(), groundTruthState.pose.Y().value(),
groundTruthState.pose.Rotation().Radians().value(),
leftDistance.value(), rightDistance.value());
}
double error = groundTruthState.pose.Translation()
.Distance(xhat.Translation().ToTranslation2d())
.value();
if (error > maxError) {
maxError = error;
}
errorSum += error;
t += dt;
}
if (debug) {
wpi::print("apply_time, measured_time, vision_x, vision_y, vision_theta\n");
units::second_t apply_time;
units::second_t measure_time;
frc::Pose2d vision_pose;
for (auto record : visionLog) {
std::tie(apply_time, measure_time, vision_pose) = record;
wpi::print("{}, {}, {}, {}, {}\n", apply_time.value(),
measure_time.value(), vision_pose.X().value(),
vision_pose.Y().value(),
vision_pose.Rotation().Radians().value());
}
}
EXPECT_NEAR(endingPose.X().value(),
estimator.GetEstimatedPosition().X().value(), 0.08);
EXPECT_NEAR(endingPose.Y().value(),
estimator.GetEstimatedPosition().Y().value(), 0.08);
EXPECT_NEAR(endingPose.Rotation().Radians().value(),
estimator.GetEstimatedPosition()
.Rotation()
.ToRotation2d()
.Radians()
.value(),
0.15);
if (checkError) {
// NOLINTNEXTLINE(bugprone-integer-division)
EXPECT_LT(errorSum / (trajectory.TotalTime() / dt), 0.05);
EXPECT_LT(maxError, 0.2);
}
}
TEST(DifferentialDrivePoseEstimator3dTest, Accuracy) {
frc::DifferentialDriveKinematics kinematics{1.0_m};
frc::DifferentialDrivePoseEstimator3d estimator{kinematics,
frc::Rotation3d{},
0_m,
0_m,
frc::Pose3d{},
{0.02, 0.02, 0.02, 0.01},
{0.1, 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(2_mps, 2_mps_sq));
testFollowTrajectory(
kinematics, estimator, trajectory,
[&](frc::Trajectory::State& state) {
return frc::ChassisSpeeds{state.velocity, 0_mps,
state.velocity * state.curvature};
},
[&](frc::Trajectory::State& state) { return state.pose; },
trajectory.InitialPose(), {0_m, 0_m, frc::Rotation2d{45_deg}}, 20_ms,
100_ms, 250_ms, true, false);
}
TEST(DifferentialDrivePoseEstimator3dTest, BadInitialPose) {
frc::DifferentialDriveKinematics kinematics{1.0_m};
frc::DifferentialDrivePoseEstimator3d estimator{kinematics,
frc::Rotation3d{},
0_m,
0_m,
frc::Pose3d{},
{0.02, 0.02, 0.02, 0.01},
{0.1, 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(2_mps, 2_mps_sq));
for (units::degree_t offset_direction_degs = 0_deg;
offset_direction_degs < 360_deg; offset_direction_degs += 45_deg) {
for (units::degree_t offset_heading_degs = 0_deg;
offset_heading_degs < 360_deg; offset_heading_degs += 45_deg) {
auto pose_offset = frc::Rotation2d{offset_direction_degs};
auto heading_offset = frc::Rotation2d{offset_heading_degs};
auto initial_pose =
trajectory.InitialPose() +
frc::Transform2d{frc::Translation2d{pose_offset.Cos() * 1_m,
pose_offset.Sin() * 1_m},
heading_offset};
testFollowTrajectory(
kinematics, estimator, trajectory,
[&](frc::Trajectory::State& state) {
return frc::ChassisSpeeds{state.velocity, 0_mps,
state.velocity * state.curvature};
},
[&](frc::Trajectory::State& state) { return state.pose; },
initial_pose, {0_m, 0_m, frc::Rotation2d{45_deg}}, 20_ms, 100_ms,
250_ms, false, false);
}
}
}
TEST(DifferentialDrivePoseEstimator3dTest, SimultaneousVisionMeasurements) {
// This tests for multiple vision measurements applied at the same time.
// The expected behavior is that all measurements affect the estimated pose.
// The alternative result is that only one vision measurement affects the
// outcome. If that were the case, after 1000 measurements, the estimated
// pose would converge to that measurement.
frc::DifferentialDriveKinematics kinematics{1.0_m};
frc::DifferentialDrivePoseEstimator3d estimator{
kinematics,
frc::Rotation3d{},
0_m,
0_m,
frc::Pose3d{1_m, 2_m, 0_m, frc::Rotation3d{0_deg, 0_deg, 270_deg}},
{0.02, 0.02, 0.02, 0.01},
{0.1, 0.1, 0.1, 0.1}};
estimator.UpdateWithTime(0_s, frc::Rotation3d{}, 0_m, 0_m);
for (int i = 0; i < 1000; i++) {
estimator.AddVisionMeasurement(
frc::Pose3d{0_m, 0_m, 0_m, frc::Rotation3d{0_deg, 0_deg, 0_deg}}, 0_s);
estimator.AddVisionMeasurement(
frc::Pose3d{3_m, 1_m, 0_m, frc::Rotation3d{0_deg, 0_deg, 90_deg}}, 0_s);
estimator.AddVisionMeasurement(
frc::Pose3d{2_m, 4_m, 0_m, frc::Rotation3d{0_deg, 0_deg, 180_deg}},
0_s);
}
{
auto dx = units::math::abs(estimator.GetEstimatedPosition().X() - 0_m);
auto dy = units::math::abs(estimator.GetEstimatedPosition().Y() - 0_m);
auto dtheta = units::math::abs(
estimator.GetEstimatedPosition().Rotation().ToRotation2d().Radians() -
0_deg);
EXPECT_TRUE(dx > 0.08_m || dy > 0.08_m || dtheta > 0.08_rad);
}
{
auto dx = units::math::abs(estimator.GetEstimatedPosition().X() - 3_m);
auto dy = units::math::abs(estimator.GetEstimatedPosition().Y() - 1_m);
auto dtheta = units::math::abs(
estimator.GetEstimatedPosition().Rotation().ToRotation2d().Radians() -
90_deg);
EXPECT_TRUE(dx > 0.08_m || dy > 0.08_m || dtheta > 0.08_rad);
}
{
auto dx = units::math::abs(estimator.GetEstimatedPosition().X() - 2_m);
auto dy = units::math::abs(estimator.GetEstimatedPosition().Y() - 4_m);
auto dtheta = units::math::abs(
estimator.GetEstimatedPosition().Rotation().ToRotation2d().Radians() -
180_deg);
EXPECT_TRUE(dx > 0.08_m || dy > 0.08_m || dtheta > 0.08_rad);
}
}
TEST(DifferentialDrivePoseEstimator3dTest, TestDiscardStaleVisionMeasurements) {
frc::DifferentialDriveKinematics kinematics{1_m};
frc::DifferentialDrivePoseEstimator3d estimator{
kinematics, frc::Rotation3d{}, 0_m, 0_m, frc::Pose3d{},
{0.1, 0.1, 0.1, 0.1}, {0.45, 0.45, 0.45, 0.45}};
// Add enough measurements to fill up the buffer
for (auto time = 0_s; time < 4_s; time += 20_ms) {
estimator.UpdateWithTime(time, frc::Rotation3d{}, 0_m, 0_m);
}
auto odometryPose = estimator.GetEstimatedPosition();
// Apply a vision measurement from 3 seconds ago
estimator.AddVisionMeasurement(
frc::Pose3d{10_m, 10_m, 0_m, frc::Rotation3d{0_rad, 0_rad, 0.1_rad}}, 1_s,
{0.1, 0.1, 0.1, 0.1});
EXPECT_NEAR(odometryPose.X().value(),
estimator.GetEstimatedPosition().X().value(), 1e-6);
EXPECT_NEAR(odometryPose.Y().value(),
estimator.GetEstimatedPosition().Y().value(), 1e-6);
EXPECT_NEAR(odometryPose.Z().value(),
estimator.GetEstimatedPosition().Z().value(), 1e-6);
EXPECT_NEAR(odometryPose.Rotation().X().value(),
estimator.GetEstimatedPosition().Rotation().X().value(), 1e-6);
EXPECT_NEAR(odometryPose.Rotation().Y().value(),
estimator.GetEstimatedPosition().Rotation().Y().value(), 1e-6);
EXPECT_NEAR(odometryPose.Rotation().Z().value(),
estimator.GetEstimatedPosition().Rotation().Z().value(), 1e-6);
}
TEST(DifferentialDrivePoseEstimator3dTest, TestSampleAt) {
frc::DifferentialDriveKinematics kinematics{1_m};
frc::DifferentialDrivePoseEstimator3d estimator{
kinematics, frc::Rotation3d{}, 0_m, 0_m, frc::Pose3d{},
{1.0, 1.0, 1.0, 1.0}, {1.0, 1.0, 1.0, 1.0}};
// Returns empty when null
EXPECT_EQ(std::nullopt, estimator.SampleAt(1_s));
// Add odometry measurements, but don't fill up the buffer
// Add a tiny tolerance for the upper bound because of floating point rounding
// error
for (double time = 1; time <= 2 + 1e-9; time += 0.02) {
estimator.UpdateWithTime(units::second_t{time}, frc::Rotation3d{},
units::meter_t{time}, units::meter_t{time});
}
// Sample at an added time
EXPECT_EQ(std::optional(frc::Pose3d{1.02_m, 0_m, 0_m, frc::Rotation3d{}}),
estimator.SampleAt(1.02_s));
// Sample between updates (test interpolation)
EXPECT_EQ(std::optional(frc::Pose3d{1.01_m, 0_m, 0_m, frc::Rotation3d{}}),
estimator.SampleAt(1.01_s));
// Sampling before the oldest value returns the oldest value
EXPECT_EQ(std::optional(frc::Pose3d{1_m, 0_m, 0_m, frc::Rotation3d{}}),
estimator.SampleAt(0.5_s));
// Sampling after the newest value returns the newest value
EXPECT_EQ(std::optional(frc::Pose3d{2_m, 0_m, 0_m, frc::Rotation3d{}}),
estimator.SampleAt(2.5_s));
// Add a vision measurement after the odometry measurements (while keeping all
// of the old odometry measurements)
estimator.AddVisionMeasurement(
frc::Pose3d{2_m, 0_m, 0_m, frc::Rotation3d{0_rad, 0_rad, 1_rad}}, 2.2_s);
// Make sure nothing changed (except the newest value)
EXPECT_EQ(std::optional(frc::Pose3d{1.02_m, 0_m, 0_m, frc::Rotation3d{}}),
estimator.SampleAt(1.02_s));
EXPECT_EQ(std::optional(frc::Pose3d{1.01_m, 0_m, 0_m, frc::Rotation3d{}}),
estimator.SampleAt(1.01_s));
EXPECT_EQ(std::optional(frc::Pose3d{1_m, 0_m, 0_m, frc::Rotation3d{}}),
estimator.SampleAt(0.5_s));
// Add a vision measurement before the odometry measurements that's still in
// the buffer
estimator.AddVisionMeasurement(
frc::Pose3d{1_m, 0.2_m, 0_m, frc::Rotation3d{}}, 0.9_s);
// Everything should be the same except Y is 0.1 (halfway between 0 and 0.2)
EXPECT_EQ(std::optional(frc::Pose3d{1.02_m, 0.1_m, 0_m, frc::Rotation3d{}}),
estimator.SampleAt(1.02_s));
EXPECT_EQ(std::optional(frc::Pose3d{1.01_m, 0.1_m, 0_m, frc::Rotation3d{}}),
estimator.SampleAt(1.01_s));
EXPECT_EQ(std::optional(frc::Pose3d{1_m, 0.1_m, 0_m, frc::Rotation3d{}}),
estimator.SampleAt(0.5_s));
EXPECT_EQ(std::optional(frc::Pose3d{2_m, 0.1_m, 0_m, frc::Rotation3d{}}),
estimator.SampleAt(2.5_s));
}
TEST(DifferentialDrivePoseEstimator3dTest, TestReset) {
frc::DifferentialDriveKinematics kinematics{1_m};
frc::DifferentialDrivePoseEstimator3d estimator{
kinematics,
frc::Rotation3d{},
0_m,
0_m,
frc::Pose3d{-1_m, -1_m, -1_m, frc::Rotation3d{0_rad, 0_rad, 1_rad}},
{1.0, 1.0, 1.0, 1.0},
{1.0, 1.0, 1.0, 1.0}};
// Test initial pose
EXPECT_DOUBLE_EQ(-1, estimator.GetEstimatedPosition().X().value());
EXPECT_DOUBLE_EQ(-1, estimator.GetEstimatedPosition().Y().value());
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(1, estimator.GetEstimatedPosition().Rotation().Z().value());
// Test reset position
estimator.ResetPosition(frc::Rotation3d{}, 1_m, 1_m,
frc::Pose3d{1_m, 0_m, 0_m, frc::Rotation3d{}});
EXPECT_DOUBLE_EQ(1, 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 orientation and wheel positions
estimator.Update(frc::Rotation3d{}, 2_m, 2_m);
EXPECT_DOUBLE_EQ(2, 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(frc::Rotation3d{0_deg, 0_deg, 90_deg});
EXPECT_DOUBLE_EQ(2, 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(std::numbers::pi / 2,
estimator.GetEstimatedPosition().Rotation().Z().value());
// Test orientation
estimator.Update(frc::Rotation3d{}, 3_m, 3_m);
EXPECT_DOUBLE_EQ(2, 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 / 2,
estimator.GetEstimatedPosition().Rotation().Z().value());
// Test reset translation
estimator.ResetTranslation(frc::Translation3d{-1_m, -1_m, -1_m});
EXPECT_DOUBLE_EQ(-1, estimator.GetEstimatedPosition().X().value());
EXPECT_DOUBLE_EQ(-1, estimator.GetEstimatedPosition().Y().value());
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,
estimator.GetEstimatedPosition().Rotation().Z().value());
// Test reset pose
estimator.ResetPose(frc::Pose3d{});
EXPECT_DOUBLE_EQ(0, 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());
}

55
wpimath/src/test/native/cpp/estimator/DifferentialDrivePoseEstimatorTest.cpp
View File

@@ -368,53 +368,60 @@ TEST(DifferentialDrivePoseEstimatorTest, TestReset) {
{1.0, 1.0, 1.0}};
// Test initial pose
EXPECT_EQ(-1, estimator.GetEstimatedPosition().X().value());
EXPECT_EQ(-1, estimator.GetEstimatedPosition().Y().value());
EXPECT_EQ(1, estimator.GetEstimatedPosition().Rotation().Radians().value());
EXPECT_DOUBLE_EQ(-1, estimator.GetEstimatedPosition().X().value());
EXPECT_DOUBLE_EQ(-1, estimator.GetEstimatedPosition().Y().value());
EXPECT_DOUBLE_EQ(
1, estimator.GetEstimatedPosition().Rotation().Radians().value());
// Test reset position
estimator.ResetPosition(frc::Rotation2d{}, 1_m, 1_m,
frc::Pose2d{1_m, 0_m, frc::Rotation2d{}});
EXPECT_EQ(1, estimator.GetEstimatedPosition().X().value());
EXPECT_EQ(0, estimator.GetEstimatedPosition().Y().value());
EXPECT_EQ(0, estimator.GetEstimatedPosition().Rotation().Radians().value());
EXPECT_DOUBLE_EQ(1, estimator.GetEstimatedPosition().X().value());
EXPECT_DOUBLE_EQ(0, estimator.GetEstimatedPosition().Y().value());
EXPECT_DOUBLE_EQ(
0, estimator.GetEstimatedPosition().Rotation().Radians().value());
// Test orientation and wheel positions
estimator.Update(frc::Rotation2d{}, 2_m, 2_m);
EXPECT_EQ(2, estimator.GetEstimatedPosition().X().value());
EXPECT_EQ(0, estimator.GetEstimatedPosition().Y().value());
EXPECT_EQ(0, estimator.GetEstimatedPosition().Rotation().Radians().value());
EXPECT_DOUBLE_EQ(2, 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(frc::Rotation2d{90_deg});
EXPECT_EQ(2, estimator.GetEstimatedPosition().X().value());
EXPECT_EQ(0, estimator.GetEstimatedPosition().Y().value());
EXPECT_EQ(std::numbers::pi / 2,
estimator.GetEstimatedPosition().Rotation().Radians().value());
EXPECT_DOUBLE_EQ(2, estimator.GetEstimatedPosition().X().value());
EXPECT_DOUBLE_EQ(0, estimator.GetEstimatedPosition().Y().value());
EXPECT_DOUBLE_EQ(
std::numbers::pi / 2,
estimator.GetEstimatedPosition().Rotation().Radians().value());
// Test orientation
estimator.Update(frc::Rotation2d{}, 3_m, 3_m);
EXPECT_EQ(2, estimator.GetEstimatedPosition().X().value());
EXPECT_EQ(1, estimator.GetEstimatedPosition().Y().value());
EXPECT_EQ(std::numbers::pi / 2,
estimator.GetEstimatedPosition().Rotation().Radians().value());
EXPECT_DOUBLE_EQ(2, estimator.GetEstimatedPosition().X().value());
EXPECT_DOUBLE_EQ(1, estimator.GetEstimatedPosition().Y().value());
EXPECT_DOUBLE_EQ(
std::numbers::pi / 2,
estimator.GetEstimatedPosition().Rotation().Radians().value());
// Test reset translation
estimator.ResetTranslation(frc::Translation2d{-1_m, -1_m});
EXPECT_EQ(-1, estimator.GetEstimatedPosition().X().value());
EXPECT_EQ(-1, estimator.GetEstimatedPosition().Y().value());
EXPECT_EQ(std::numbers::pi / 2,
estimator.GetEstimatedPosition().Rotation().Radians().value());
EXPECT_DOUBLE_EQ(-1, estimator.GetEstimatedPosition().X().value());
EXPECT_DOUBLE_EQ(-1, estimator.GetEstimatedPosition().Y().value());
EXPECT_DOUBLE_EQ(
std::numbers::pi / 2,
estimator.GetEstimatedPosition().Rotation().Radians().value());
// Test reset pose
estimator.ResetPose(frc::Pose2d{});
EXPECT_EQ(0, estimator.GetEstimatedPosition().X().value());
EXPECT_EQ(0, estimator.GetEstimatedPosition().Y().value());
EXPECT_EQ(0, estimator.GetEstimatedPosition().Rotation().Radians().value());
EXPECT_DOUBLE_EQ(0, estimator.GetEstimatedPosition().X().value());
EXPECT_DOUBLE_EQ(0, estimator.GetEstimatedPosition().Y().value());
EXPECT_DOUBLE_EQ(
0, estimator.GetEstimatedPosition().Rotation().Radians().value());
}

466
wpimath/src/test/native/cpp/estimator/MecanumDrivePoseEstimator3dTest.cpp Normal file
View File

@@ -0,0 +1,466 @@
// 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 <limits>
#include <random>
#include <tuple>
#include <vector>
#include <gtest/gtest.h>
#include <wpi/print.h>
#include "frc/estimator/MecanumDrivePoseEstimator3d.h"
#include "frc/geometry/Pose2d.h"
#include "frc/kinematics/MecanumDriveKinematics.h"
#include "frc/trajectory/TrajectoryGenerator.h"
void testFollowTrajectory(
const frc::MecanumDriveKinematics& kinematics,
frc::MecanumDrivePoseEstimator3d& estimator,
const frc::Trajectory& trajectory,
std::function<frc::ChassisSpeeds(frc::Trajectory::State&)>
chassisSpeedsGenerator,
std::function<frc::Pose2d(frc::Trajectory::State&)>
visionMeasurementGenerator,
const frc::Pose2d& startingPose, const frc::Pose2d& endingPose,
const units::second_t dt, const units::second_t kVisionUpdateRate,
const units::second_t kVisionUpdateDelay, const bool checkError,
const bool debug) {
frc::MecanumDriveWheelPositions wheelPositions{};
estimator.ResetPosition(frc::Rotation3d{}, wheelPositions,
frc::Pose3d{startingPose});
std::default_random_engine generator;
std::normal_distribution<double> distribution(0.0, 1.0);
units::second_t t = 0_s;
std::vector<std::pair<units::second_t, frc::Pose2d>> visionPoses;
std::vector<std::tuple<units::second_t, units::second_t, frc::Pose2d>>
visionLog;
double maxError = -std::numeric_limits<double>::max();
double errorSum = 0;
if (debug) {
wpi::print("time, est_x, est_y, est_theta, true_x, true_y, true_theta\n");
}
while (t < trajectory.TotalTime()) {
frc::Trajectory::State groundTruthState = trajectory.Sample(t);
// We are due for a new vision measurement if it's been `visionUpdateRate`
// seconds since the last vision measurement
if (visionPoses.empty() ||
visionPoses.back().first + kVisionUpdateRate < t) {
auto visionPose =
visionMeasurementGenerator(groundTruthState) +
frc::Transform2d{frc::Translation2d{distribution(generator) * 0.1_m,
distribution(generator) * 0.1_m},
frc::Rotation2d{distribution(generator) * 0.05_rad}};
visionPoses.push_back({t, visionPose});
}
// We should apply the oldest vision measurement if it has been
// `visionUpdateDelay` seconds since it was measured
if (!visionPoses.empty() &&
visionPoses.front().first + kVisionUpdateDelay < t) {
auto visionEntry = visionPoses.front();
estimator.AddVisionMeasurement(frc::Pose3d{visionEntry.second},
visionEntry.first);
visionPoses.erase(visionPoses.begin());
visionLog.push_back({t, visionEntry.first, visionEntry.second});
}
auto chassisSpeeds = chassisSpeedsGenerator(groundTruthState);
auto wheelSpeeds = kinematics.ToWheelSpeeds(chassisSpeeds);
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::Rotation3d{groundTruthState.pose.Rotation() +
frc::Rotation2d{distribution(generator) * 0.05_rad} -
trajectory.InitialPose().Rotation()},
wheelPositions);
if (debug) {
wpi::print(
"{}, {}, {}, {}, {}, {}, {}\n", t.value(), xhat.X().value(),
xhat.Y().value(), xhat.Rotation().ToRotation2d().Radians().value(),
groundTruthState.pose.X().value(), groundTruthState.pose.Y().value(),
groundTruthState.pose.Rotation().Radians().value());
}
double error = groundTruthState.pose.Translation()
.Distance(xhat.Translation().ToTranslation2d())
.value();
if (error > maxError) {
maxError = error;
}
errorSum += error;
t += dt;
}
if (debug) {
wpi::print("apply_time, measured_time, vision_x, vision_y, vision_theta\n");
units::second_t apply_time;
units::second_t measure_time;
frc::Pose2d vision_pose;
for (auto record : visionLog) {
std::tie(apply_time, measure_time, vision_pose) = record;
wpi::print("{}, {}, {}, {}, {}\n", apply_time.value(),
measure_time.value(), vision_pose.X().value(),
vision_pose.Y().value(),
vision_pose.Rotation().Radians().value());
}
}
EXPECT_NEAR(endingPose.X().value(),
estimator.GetEstimatedPosition().X().value(), 0.08);
EXPECT_NEAR(endingPose.Y().value(),
estimator.GetEstimatedPosition().Y().value(), 0.08);
EXPECT_NEAR(endingPose.Rotation().Radians().value(),
estimator.GetEstimatedPosition()
.Rotation()
.ToRotation2d()
.Radians()
.value(),
0.15);
if (checkError) {
// NOLINTNEXTLINE(bugprone-integer-division)
EXPECT_LT(errorSum / (trajectory.TotalTime() / dt), 0.051);
EXPECT_LT(maxError, 0.2);
}
}
TEST(MecanumDrivePoseEstimator3dTest, 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::MecanumDrivePoseEstimator3d estimator{
kinematics, frc::Rotation3d{}, wheelPositions,
frc::Pose3d{}, {0.1, 0.1, 0.1, 0.1}, {0.45, 0.45, 0.45, 0.45}};
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(2.0_mps, 2.0_mps_sq));
testFollowTrajectory(
kinematics, estimator, trajectory,
[&](frc::Trajectory::State& state) {
return frc::ChassisSpeeds{state.velocity, 0_mps,
state.velocity * state.curvature};
},
[&](frc::Trajectory::State& state) { return state.pose; },
trajectory.InitialPose(), {0_m, 0_m, frc::Rotation2d{45_deg}}, 20_ms,
100_ms, 250_ms, true, false);
}
TEST(MecanumDrivePoseEstimator3dTest, BadInitialPose) {
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::MecanumDrivePoseEstimator3d estimator{
kinematics, frc::Rotation3d{}, wheelPositions,
frc::Pose3d{}, {0.1, 0.1, 0.1, 0.1}, {0.45, 0.45, 0.45, 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(2.0_mps, 2.0_mps_sq));
for (units::degree_t offset_direction_degs = 0_deg;
offset_direction_degs < 360_deg; offset_direction_degs += 45_deg) {
for (units::degree_t offset_heading_degs = 0_deg;
offset_heading_degs < 360_deg; offset_heading_degs += 45_deg) {
auto pose_offset = frc::Rotation2d{offset_direction_degs};
auto heading_offset = frc::Rotation2d{offset_heading_degs};
auto initial_pose =
trajectory.InitialPose() +
frc::Transform2d{frc::Translation2d{pose_offset.Cos() * 1_m,
pose_offset.Sin() * 1_m},
heading_offset};
testFollowTrajectory(
kinematics, estimator, trajectory,
[&](frc::Trajectory::State& state) {
return frc::ChassisSpeeds{state.velocity, 0_mps,
state.velocity * state.curvature};
},
[&](frc::Trajectory::State& state) { return state.pose; },
initial_pose, {0_m, 0_m, frc::Rotation2d{45_deg}}, 20_ms, 100_ms,
250_ms, false, false);
}
}
}
TEST(MecanumDrivePoseEstimator3dTest, SimultaneousVisionMeasurements) {
// This tests for multiple vision measurements applied at the same time.
// The expected behavior is that all measurements affect the estimated pose.
// The alternative result is that only one vision measurement affects the
// outcome. If that were the case, after 1000 measurements, the estimated
// pose would converge to that measurement.
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::MecanumDrivePoseEstimator3d estimator{
kinematics,
frc::Rotation3d{},
wheelPositions,
frc::Pose3d{1_m, 2_m, 0_m, frc::Rotation3d{0_deg, 0_deg, 270_deg}},
{0.1, 0.1, 0.1, 0.1},
{0.45, 0.45, 0.45, 0.1}};
estimator.UpdateWithTime(0_s, frc::Rotation3d{}, wheelPositions);
for (int i = 0; i < 1000; i++) {
estimator.AddVisionMeasurement(
frc::Pose3d{0_m, 0_m, 0_m, frc::Rotation3d{0_deg, 0_deg, 0_deg}}, 0_s);
estimator.AddVisionMeasurement(
frc::Pose3d{3_m, 1_m, 0_m, frc::Rotation3d{0_deg, 0_deg, 90_deg}}, 0_s);
estimator.AddVisionMeasurement(
frc::Pose3d{2_m, 4_m, 0_m, frc::Rotation3d{0_deg, 0_deg, 180_deg}},
0_s);
}
{
auto dx = units::math::abs(estimator.GetEstimatedPosition().X() - 0_m);
auto dy = units::math::abs(estimator.GetEstimatedPosition().Y() - 0_m);
auto dtheta = units::math::abs(
estimator.GetEstimatedPosition().Rotation().ToRotation2d().Radians() -
0_deg);
EXPECT_TRUE(dx > 0.08_m || dy > 0.08_m || dtheta > 0.08_rad);
}
{
auto dx = units::math::abs(estimator.GetEstimatedPosition().X() - 3_m);
auto dy = units::math::abs(estimator.GetEstimatedPosition().Y() - 1_m);
auto dtheta = units::math::abs(
estimator.GetEstimatedPosition().Rotation().ToRotation2d().Radians() -
90_deg);
EXPECT_TRUE(dx > 0.08_m || dy > 0.08_m || dtheta > 0.08_rad);
}
{
auto dx = units::math::abs(estimator.GetEstimatedPosition().X() - 2_m);
auto dy = units::math::abs(estimator.GetEstimatedPosition().Y() - 4_m);
auto dtheta = units::math::abs(
estimator.GetEstimatedPosition().Rotation().ToRotation2d().Radians() -
180_deg);
EXPECT_TRUE(dx > 0.08_m || dy > 0.08_m || dtheta > 0.08_rad);
}
}
TEST(MecanumDrivePoseEstimator3dTest, TestDiscardStaleVisionMeasurements) {
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::MecanumDrivePoseEstimator3d estimator{
kinematics, frc::Rotation3d{}, frc::MecanumDriveWheelPositions{},
frc::Pose3d{}, {0.1, 0.1, 0.1, 0.1}, {0.45, 0.45, 0.45, 0.45}};
// Add enough measurements to fill up the buffer
for (auto time = 0_s; time < 4_s; time += 20_ms) {
estimator.UpdateWithTime(time, frc::Rotation3d{},
frc::MecanumDriveWheelPositions{});
}
auto odometryPose = estimator.GetEstimatedPosition();
// Apply a vision measurement from 3 seconds ago
estimator.AddVisionMeasurement(
frc::Pose3d{10_m, 10_m, 0_m, frc::Rotation3d{0_rad, 0_rad, 0.1_rad}}, 1_s,
{0.1, 0.1, 0.1, 0.1});
EXPECT_NEAR(odometryPose.X().value(),
estimator.GetEstimatedPosition().X().value(), 1e-6);
EXPECT_NEAR(odometryPose.Y().value(),
estimator.GetEstimatedPosition().Y().value(), 1e-6);
EXPECT_NEAR(odometryPose.Z().value(),
estimator.GetEstimatedPosition().Z().value(), 1e-6);
EXPECT_NEAR(odometryPose.Rotation().X().value(),
estimator.GetEstimatedPosition().Rotation().X().value(), 1e-6);
EXPECT_NEAR(odometryPose.Rotation().Y().value(),
estimator.GetEstimatedPosition().Rotation().Y().value(), 1e-6);
EXPECT_NEAR(odometryPose.Rotation().Z().value(),
estimator.GetEstimatedPosition().Rotation().Z().value(), 1e-6);
}
TEST(MecanumDrivePoseEstimator3dTest, TestSampleAt) {
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::MecanumDrivePoseEstimator3d estimator{
kinematics, frc::Rotation3d{}, frc::MecanumDriveWheelPositions{},
frc::Pose3d{}, {1.0, 1.0, 1.0, 1.0}, {1.0, 1.0, 1.0, 1.0}};
// Returns empty when null
EXPECT_EQ(std::nullopt, estimator.SampleAt(1_s));
// Add odometry measurements, but don't fill up the buffer
// Add a tiny tolerance for the upper bound because of floating point rounding
// error
for (double time = 1; time <= 2 + 1e-9; time += 0.02) {
frc::MecanumDriveWheelPositions wheelPositions{
units::meter_t{time}, units::meter_t{time}, units::meter_t{time},
units::meter_t{time}};
estimator.UpdateWithTime(units::second_t{time}, frc::Rotation3d{},
wheelPositions);
}
// Sample at an added time
EXPECT_EQ(std::optional(frc::Pose3d{1.02_m, 0_m, 0_m, frc::Rotation3d{}}),
estimator.SampleAt(1.02_s));
// Sample between updates (test interpolation)
EXPECT_EQ(std::optional(frc::Pose3d{1.01_m, 0_m, 0_m, frc::Rotation3d{}}),
estimator.SampleAt(1.01_s));
// Sampling before the oldest value returns the oldest value
EXPECT_EQ(std::optional(frc::Pose3d{1_m, 0_m, 0_m, frc::Rotation3d{}}),
estimator.SampleAt(0.5_s));
// Sampling after the newest value returns the newest value
EXPECT_EQ(std::optional(frc::Pose3d{2_m, 0_m, 0_m, frc::Rotation3d{}}),
estimator.SampleAt(2.5_s));
// Add a vision measurement after the odometry measurements (while keeping all
// of the old odometry measurements)
estimator.AddVisionMeasurement(
frc::Pose3d{2_m, 0_m, 0_m, frc::Rotation3d{0_rad, 0_rad, 1_rad}}, 2.2_s);
// Make sure nothing changed (except the newest value)
EXPECT_EQ(std::optional(frc::Pose3d{1.02_m, 0_m, 0_m, frc::Rotation3d{}}),
estimator.SampleAt(1.02_s));
EXPECT_EQ(std::optional(frc::Pose3d{1.01_m, 0_m, 0_m, frc::Rotation3d{}}),
estimator.SampleAt(1.01_s));
EXPECT_EQ(std::optional(frc::Pose3d{1_m, 0_m, 0_m, frc::Rotation3d{}}),
estimator.SampleAt(0.5_s));
// Add a vision measurement before the odometry measurements that's still in
// the buffer
estimator.AddVisionMeasurement(
frc::Pose3d{1_m, 0.2_m, 0_m, frc::Rotation3d{}}, 0.9_s);
// Everything should be the same except Y is 0.1 (halfway between 0 and 0.2)
EXPECT_EQ(std::optional(frc::Pose3d{1.02_m, 0.1_m, 0_m, frc::Rotation3d{}}),
estimator.SampleAt(1.02_s));
EXPECT_EQ(std::optional(frc::Pose3d{1.01_m, 0.1_m, 0_m, frc::Rotation3d{}}),
estimator.SampleAt(1.01_s));
EXPECT_EQ(std::optional(frc::Pose3d{1_m, 0.1_m, 0_m, frc::Rotation3d{}}),
estimator.SampleAt(0.5_s));
EXPECT_EQ(std::optional(frc::Pose3d{2_m, 0.1_m, 0_m, frc::Rotation3d{}}),
estimator.SampleAt(2.5_s));
}
TEST(MecanumDrivePoseEstimator3dTest, TestReset) {
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::MecanumDrivePoseEstimator3d estimator{
kinematics,
frc::Rotation3d{},
frc::MecanumDriveWheelPositions{},
frc::Pose3d{-1_m, -1_m, -1_m, frc::Rotation3d{0_rad, 0_rad, 1_rad}},
{1.0, 1.0, 1.0, 1.0},
{1.0, 1.0, 1.0, 1.0}};
// Test initial pose
EXPECT_DOUBLE_EQ(-1, estimator.GetEstimatedPosition().X().value());
EXPECT_DOUBLE_EQ(-1, estimator.GetEstimatedPosition().Y().value());
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(1, estimator.GetEstimatedPosition().Rotation().Z().value());
// Test reset position
estimator.ResetPosition(frc::Rotation3d{}, {1_m, 1_m, 1_m, 1_m},
frc::Pose3d{1_m, 0_m, 0_m, frc::Rotation3d{}});
EXPECT_DOUBLE_EQ(1, 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 orientation and wheel positions
estimator.Update(frc::Rotation3d{}, {2_m, 2_m, 2_m, 2_m});
EXPECT_DOUBLE_EQ(2, 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(frc::Rotation3d{0_deg, 0_deg, 90_deg});
EXPECT_DOUBLE_EQ(2, 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(std::numbers::pi / 2,
estimator.GetEstimatedPosition().Rotation().Z().value());
// Test orientation
estimator.Update(frc::Rotation3d{}, {3_m, 3_m, 3_m, 3_m});
EXPECT_DOUBLE_EQ(2, 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 / 2,
estimator.GetEstimatedPosition().Rotation().Z().value());
// Test reset translation
estimator.ResetTranslation(frc::Translation3d{-1_m, -1_m, -1_m});
EXPECT_DOUBLE_EQ(-1, estimator.GetEstimatedPosition().X().value());
EXPECT_DOUBLE_EQ(-1, estimator.GetEstimatedPosition().Y().value());
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,
estimator.GetEstimatedPosition().Rotation().Z().value());
// Test reset pose
estimator.ResetPose(frc::Pose3d{});
EXPECT_DOUBLE_EQ(0, 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());
}

55
wpimath/src/test/native/cpp/estimator/MecanumDrivePoseEstimatorTest.cpp
View File

@@ -376,53 +376,60 @@ TEST(MecanumDrivePoseEstimatorTest, TestReset) {
{1.0, 1.0, 1.0}};
// Test initial pose
EXPECT_EQ(-1, estimator.GetEstimatedPosition().X().value());
EXPECT_EQ(-1, estimator.GetEstimatedPosition().Y().value());
EXPECT_EQ(1, estimator.GetEstimatedPosition().Rotation().Radians().value());
EXPECT_DOUBLE_EQ(-1, estimator.GetEstimatedPosition().X().value());
EXPECT_DOUBLE_EQ(-1, estimator.GetEstimatedPosition().Y().value());
EXPECT_DOUBLE_EQ(
1, estimator.GetEstimatedPosition().Rotation().Radians().value());
// Test reset position
estimator.ResetPosition(frc::Rotation2d{}, {1_m, 1_m, 1_m, 1_m},
frc::Pose2d{1_m, 0_m, frc::Rotation2d{}});
EXPECT_EQ(1, estimator.GetEstimatedPosition().X().value());
EXPECT_EQ(0, estimator.GetEstimatedPosition().Y().value());
EXPECT_EQ(0, estimator.GetEstimatedPosition().Rotation().Radians().value());
EXPECT_DOUBLE_EQ(1, estimator.GetEstimatedPosition().X().value());
EXPECT_DOUBLE_EQ(0, estimator.GetEstimatedPosition().Y().value());
EXPECT_DOUBLE_EQ(
0, estimator.GetEstimatedPosition().Rotation().Radians().value());
// Test orientation and wheel positions
estimator.Update(frc::Rotation2d{}, {2_m, 2_m, 2_m, 2_m});
EXPECT_EQ(2, estimator.GetEstimatedPosition().X().value());
EXPECT_EQ(0, estimator.GetEstimatedPosition().Y().value());
EXPECT_EQ(0, estimator.GetEstimatedPosition().Rotation().Radians().value());
EXPECT_DOUBLE_EQ(2, 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(frc::Rotation2d{90_deg});
EXPECT_EQ(2, estimator.GetEstimatedPosition().X().value());
EXPECT_EQ(0, estimator.GetEstimatedPosition().Y().value());
EXPECT_EQ(std::numbers::pi / 2,
estimator.GetEstimatedPosition().Rotation().Radians().value());
EXPECT_DOUBLE_EQ(2, estimator.GetEstimatedPosition().X().value());
EXPECT_DOUBLE_EQ(0, estimator.GetEstimatedPosition().Y().value());
EXPECT_DOUBLE_EQ(
std::numbers::pi / 2,
estimator.GetEstimatedPosition().Rotation().Radians().value());
// Test orientation
estimator.Update(frc::Rotation2d{}, {3_m, 3_m, 3_m, 3_m});
EXPECT_EQ(2, estimator.GetEstimatedPosition().X().value());
EXPECT_EQ(1, estimator.GetEstimatedPosition().Y().value());
EXPECT_EQ(std::numbers::pi / 2,
estimator.GetEstimatedPosition().Rotation().Radians().value());
EXPECT_DOUBLE_EQ(2, estimator.GetEstimatedPosition().X().value());
EXPECT_DOUBLE_EQ(1, estimator.GetEstimatedPosition().Y().value());
EXPECT_DOUBLE_EQ(
std::numbers::pi / 2,
estimator.GetEstimatedPosition().Rotation().Radians().value());
// Test reset translation
estimator.ResetTranslation(frc::Translation2d{-1_m, -1_m});
EXPECT_EQ(-1, estimator.GetEstimatedPosition().X().value());
EXPECT_EQ(-1, estimator.GetEstimatedPosition().Y().value());
EXPECT_EQ(std::numbers::pi / 2,
estimator.GetEstimatedPosition().Rotation().Radians().value());
EXPECT_DOUBLE_EQ(-1, estimator.GetEstimatedPosition().X().value());
EXPECT_DOUBLE_EQ(-1, estimator.GetEstimatedPosition().Y().value());
EXPECT_DOUBLE_EQ(
std::numbers::pi / 2,
estimator.GetEstimatedPosition().Rotation().Radians().value());
// Test reset pose
estimator.ResetPose(frc::Pose2d{});
EXPECT_EQ(0, estimator.GetEstimatedPosition().X().value());
EXPECT_EQ(0, estimator.GetEstimatedPosition().Y().value());
EXPECT_EQ(0, estimator.GetEstimatedPosition().Rotation().Radians().value());
EXPECT_DOUBLE_EQ(0, estimator.GetEstimatedPosition().X().value());
EXPECT_DOUBLE_EQ(0, estimator.GetEstimatedPosition().Y().value());
EXPECT_DOUBLE_EQ(
0, estimator.GetEstimatedPosition().Rotation().Radians().value());
}

502
wpimath/src/test/native/cpp/estimator/SwerveDrivePoseEstimator3dTest.cpp Normal file
View File

@@ -0,0 +1,502 @@
// 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 <limits>
#include <random>
#include <tuple>
#include <vector>
#include <fmt/format.h>
#include <gtest/gtest.h>
#include <wpi/print.h>
#include <wpi/timestamp.h>
#include "frc/estimator/SwerveDrivePoseEstimator3d.h"
#include "frc/geometry/Pose2d.h"
#include "frc/kinematics/SwerveDriveKinematics.h"
#include "frc/trajectory/TrajectoryGenerator.h"
void testFollowTrajectory(
const frc::SwerveDriveKinematics<4>& kinematics,
frc::SwerveDrivePoseEstimator3d<4>& estimator,
const frc::Trajectory& trajectory,
std::function<frc::ChassisSpeeds(frc::Trajectory::State&)>
chassisSpeedsGenerator,
std::function<frc::Pose2d(frc::Trajectory::State&)>
visionMeasurementGenerator,
const frc::Pose2d& startingPose, const frc::Pose2d& endingPose,
const units::second_t dt, const units::second_t kVisionUpdateRate,
const units::second_t kVisionUpdateDelay, const bool checkError,
const bool debug) {
wpi::array<frc::SwerveModulePosition, 4> positions{wpi::empty_array};
estimator.ResetPosition(frc::Rotation3d{}, positions,
frc::Pose3d{startingPose});
std::default_random_engine generator;
std::normal_distribution<double> distribution(0.0, 1.0);
units::second_t t = 0_s;
std::vector<std::pair<units::second_t, frc::Pose2d>> visionPoses;
std::vector<std::tuple<units::second_t, units::second_t, frc::Pose2d>>
visionLog;
double maxError = -std::numeric_limits<double>::max();
double errorSum = 0;
if (debug) {
wpi::print("time, est_x, est_y, est_theta, true_x, true_y, true_theta\n");
}
while (t < trajectory.TotalTime()) {
frc::Trajectory::State groundTruthState = trajectory.Sample(t);
// We are due for a new vision measurement if it's been `visionUpdateRate`
// seconds since the last vision measurement
if (visionPoses.empty() ||
visionPoses.back().first + kVisionUpdateRate < t) {
auto visionPose =
visionMeasurementGenerator(groundTruthState) +
frc::Transform2d{frc::Translation2d{distribution(generator) * 0.1_m,
distribution(generator) * 0.1_m},
frc::Rotation2d{distribution(generator) * 0.05_rad}};
visionPoses.push_back({t, visionPose});
}
// We should apply the oldest vision measurement if it has been
// `visionUpdateDelay` seconds since it was measured
if (!visionPoses.empty() &&
visionPoses.front().first + kVisionUpdateDelay < t) {
auto visionEntry = visionPoses.front();
estimator.AddVisionMeasurement(frc::Pose3d{visionEntry.second},
visionEntry.first);
visionPoses.erase(visionPoses.begin());
visionLog.push_back({t, visionEntry.first, visionEntry.second});
}
auto chassisSpeeds = chassisSpeedsGenerator(groundTruthState);
auto moduleStates = kinematics.ToSwerveModuleStates(chassisSpeeds);
for (size_t i = 0; i < 4; i++) {
positions[i].distance += moduleStates[i].speed * dt;
positions[i].angle = moduleStates[i].angle;
}
auto xhat = estimator.UpdateWithTime(
t,
frc::Rotation3d{groundTruthState.pose.Rotation() +
frc::Rotation2d{distribution(generator) * 0.05_rad} -
trajectory.InitialPose().Rotation()},
positions);
if (debug) {
wpi::print(
"{}, {}, {}, {}, {}, {}, {}\n", t.value(), xhat.X().value(),
xhat.Y().value(), xhat.Rotation().ToRotation2d().Radians().value(),
groundTruthState.pose.X().value(), groundTruthState.pose.Y().value(),
groundTruthState.pose.Rotation().Radians().value());
}
double error = groundTruthState.pose.Translation()
.Distance(xhat.Translation().ToTranslation2d())
.value();
if (error > maxError) {
maxError = error;
}
errorSum += error;
t += dt;
}
if (debug) {
wpi::print("apply_time, measured_time, vision_x, vision_y, vision_theta\n");
units::second_t apply_time;
units::second_t measure_time;
frc::Pose2d vision_pose;
for (auto record : visionLog) {
std::tie(apply_time, measure_time, vision_pose) = record;
wpi::print("{}, {}, {}, {}, {}\n", apply_time.value(),
measure_time.value(), vision_pose.X().value(),
vision_pose.Y().value(),
vision_pose.Rotation().Radians().value());
}
}
EXPECT_NEAR(endingPose.X().value(),
estimator.GetEstimatedPosition().X().value(), 0.08);
EXPECT_NEAR(endingPose.Y().value(),
estimator.GetEstimatedPosition().Y().value(), 0.08);
EXPECT_NEAR(endingPose.Rotation().Radians().value(),
estimator.GetEstimatedPosition()
.Rotation()
.ToRotation2d()
.Radians()
.value(),
0.15);
if (checkError) {
// NOLINTNEXTLINE(bugprone-integer-division)
EXPECT_LT(errorSum / (trajectory.TotalTime() / dt), 0.058);
EXPECT_LT(maxError, 0.2);
}
}
TEST(SwerveDrivePoseEstimator3dTest, 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::SwerveModulePosition fl;
frc::SwerveModulePosition fr;
frc::SwerveModulePosition bl;
frc::SwerveModulePosition br;
frc::SwerveDrivePoseEstimator3d<4> estimator{
kinematics, frc::Rotation3d{}, {fl, fr, bl, br},
frc::Pose3d{}, {0.1, 0.1, 0.1, 0.1}, {0.45, 0.45, 0.45, 0.45}};
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(2_mps, 2.0_mps_sq));
testFollowTrajectory(
kinematics, estimator, trajectory,
[&](frc::Trajectory::State& state) {
return frc::ChassisSpeeds{state.velocity, 0_mps,
state.velocity * state.curvature};
},
[&](frc::Trajectory::State& state) { return state.pose; },
{0_m, 0_m, frc::Rotation2d{45_deg}}, {0_m, 0_m, frc::Rotation2d{45_deg}},
20_ms, 100_ms, 250_ms, true, false);
}
TEST(SwerveDrivePoseEstimator3dTest, BadInitialPose) {
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::SwerveDrivePoseEstimator3d<4> estimator{
kinematics, frc::Rotation3d{}, {fl, fr, bl, br},
frc::Pose3d{}, {0.1, 0.1, 0.1, 0.1}, {0.9, 0.9, 0.9, 0.9}};
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(2_mps, 2.0_mps_sq));
for (units::degree_t offset_direction_degs = 0_deg;
offset_direction_degs < 360_deg; offset_direction_degs += 45_deg) {
for (units::degree_t offset_heading_degs = 0_deg;
offset_heading_degs < 360_deg; offset_heading_degs += 45_deg) {
auto pose_offset = frc::Rotation2d{offset_direction_degs};
auto heading_offset = frc::Rotation2d{offset_heading_degs};
auto initial_pose =
trajectory.InitialPose() +
frc::Transform2d{frc::Translation2d{pose_offset.Cos() * 1_m,
pose_offset.Sin() * 1_m},
heading_offset};
testFollowTrajectory(
kinematics, estimator, trajectory,
[&](frc::Trajectory::State& state) {
return frc::ChassisSpeeds{state.velocity, 0_mps,
state.velocity * state.curvature};
},
[&](frc::Trajectory::State& state) { return state.pose; },
initial_pose, {0_m, 0_m, frc::Rotation2d{45_deg}}, 20_ms, 100_ms,
250_ms, false, false);
}
}
}
TEST(SwerveDrivePoseEstimator3dTest, SimultaneousVisionMeasurements) {
// This tests for multiple vision measurements applied at the same time.
// The expected behavior is that all measurements affect the estimated pose.
// The alternative result is that only one vision measurement affects the
// outcome. If that were the case, after 1000 measurements, the estimated
// pose would converge to that measurement.
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::SwerveDrivePoseEstimator3d<4> estimator{
kinematics,
frc::Rotation3d{},
{fl, fr, bl, br},
frc::Pose3d{1_m, 2_m, 0_m, frc::Rotation3d{0_deg, 0_deg, 270_deg}},
{0.1, 0.1, 0.1, 0.1},
{0.45, 0.45, 0.45, 0.45}};
estimator.UpdateWithTime(0_s, frc::Rotation3d{}, {fl, fr, bl, br});
for (int i = 0; i < 1000; i++) {
estimator.AddVisionMeasurement(
frc::Pose3d{0_m, 0_m, 0_m, frc::Rotation3d{0_deg, 0_deg, 0_deg}}, 0_s);
estimator.AddVisionMeasurement(
frc::Pose3d{3_m, 1_m, 0_m, frc::Rotation3d{0_deg, 0_deg, 90_deg}}, 0_s);
estimator.AddVisionMeasurement(
frc::Pose3d{2_m, 4_m, 0_m, frc::Rotation3d{0_deg, 0_deg, 180_deg}},
0_s);
}
{
auto dx = units::math::abs(estimator.GetEstimatedPosition().X() - 0_m);
auto dy = units::math::abs(estimator.GetEstimatedPosition().Y() - 0_m);
auto dtheta = units::math::abs(
estimator.GetEstimatedPosition().Rotation().ToRotation2d().Radians() -
0_deg);
EXPECT_TRUE(dx > 0.08_m || dy > 0.08_m || dtheta > 0.08_rad);
}
{
auto dx = units::math::abs(estimator.GetEstimatedPosition().X() - 3_m);
auto dy = units::math::abs(estimator.GetEstimatedPosition().Y() - 1_m);
auto dtheta = units::math::abs(
estimator.GetEstimatedPosition().Rotation().ToRotation2d().Radians() -
90_deg);
EXPECT_TRUE(dx > 0.08_m || dy > 0.08_m || dtheta > 0.08_rad);
}
{
auto dx = units::math::abs(estimator.GetEstimatedPosition().X() - 2_m);
auto dy = units::math::abs(estimator.GetEstimatedPosition().Y() - 4_m);
auto dtheta = units::math::abs(
estimator.GetEstimatedPosition().Rotation().ToRotation2d().Radians() -
180_deg);
EXPECT_TRUE(dx > 0.08_m || dy > 0.08_m || dtheta > 0.08_rad);
}
}
TEST(SwerveDrivePoseEstimator3dTest, TestDiscardStaleVisionMeasurements) {
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::SwerveDrivePoseEstimator3d<4> estimator{
kinematics, frc::Rotation3d{}, {fl, fr, bl, br},
frc::Pose3d{}, {0.1, 0.1, 0.1, 0.1}, {0.45, 0.45, 0.45, 0.45}};
// Add enough measurements to fill up the buffer
for (auto time = 0_s; time < 4_s; time += 20_ms) {
estimator.UpdateWithTime(time, frc::Rotation3d{}, {fl, fr, bl, br});
}
auto odometryPose = estimator.GetEstimatedPosition();
// Apply a vision measurement from 3 seconds ago
estimator.AddVisionMeasurement(
frc::Pose3d{10_m, 10_m, 0_m, frc::Rotation3d{0_rad, 0_rad, 0.1_rad}}, 1_s,
{0.1, 0.1, 0.1, 0.1});
EXPECT_NEAR(odometryPose.X().value(),
estimator.GetEstimatedPosition().X().value(), 1e-6);
EXPECT_NEAR(odometryPose.Y().value(),
estimator.GetEstimatedPosition().Y().value(), 1e-6);
EXPECT_NEAR(odometryPose.Z().value(),
estimator.GetEstimatedPosition().Z().value(), 1e-6);
EXPECT_NEAR(odometryPose.Rotation().X().value(),
estimator.GetEstimatedPosition().Rotation().X().value(), 1e-6);
EXPECT_NEAR(odometryPose.Rotation().Y().value(),
estimator.GetEstimatedPosition().Rotation().Y().value(), 1e-6);
EXPECT_NEAR(odometryPose.Rotation().Z().value(),
estimator.GetEstimatedPosition().Rotation().Z().value(), 1e-6);
}
TEST(SwerveDrivePoseEstimator3dTest, TestSampleAt) {
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::SwerveDrivePoseEstimator3d estimator{
kinematics,
frc::Rotation3d{},
{frc::SwerveModulePosition{}, frc::SwerveModulePosition{},
frc::SwerveModulePosition{}, frc::SwerveModulePosition{}},
frc::Pose3d{},
{1.0, 1.0, 1.0, 1.0},
{1.0, 1.0, 1.0, 1.0}};
// Returns empty when null
EXPECT_EQ(std::nullopt, estimator.SampleAt(1_s));
// Add odometry measurements, but don't fill up the buffer
// Add a tiny tolerance for the upper bound because of floating point rounding
// error
for (double time = 1; time <= 2 + 1e-9; time += 0.02) {
wpi::array<frc::SwerveModulePosition, 4> wheelPositions{
{frc::SwerveModulePosition{units::meter_t{time}, frc::Rotation2d{}},
frc::SwerveModulePosition{units::meter_t{time}, frc::Rotation2d{}},
frc::SwerveModulePosition{units::meter_t{time}, frc::Rotation2d{}},
frc::SwerveModulePosition{units::meter_t{time}, frc::Rotation2d{}}}};
estimator.UpdateWithTime(units::second_t{time}, frc::Rotation3d{},
wheelPositions);
}
// Sample at an added time
EXPECT_EQ(std::optional(frc::Pose3d{1.02_m, 0_m, 0_m, frc::Rotation3d{}}),
estimator.SampleAt(1.02_s));
// Sample between updates (test interpolation)
EXPECT_EQ(std::optional(frc::Pose3d{1.01_m, 0_m, 0_m, frc::Rotation3d{}}),
estimator.SampleAt(1.01_s));
// Sampling before the oldest value returns the oldest value
EXPECT_EQ(std::optional(frc::Pose3d{1_m, 0_m, 0_m, frc::Rotation3d{}}),
estimator.SampleAt(0.5_s));
// Sampling after the newest value returns the newest value
EXPECT_EQ(std::optional(frc::Pose3d{2_m, 0_m, 0_m, frc::Rotation3d{}}),
estimator.SampleAt(2.5_s));
// Add a vision measurement after the odometry measurements (while keeping all
// of the old odometry measurements)
estimator.AddVisionMeasurement(
frc::Pose3d{2_m, 0_m, 0_m, frc::Rotation3d{0_rad, 0_rad, 1_rad}}, 2.2_s);
// Make sure nothing changed (except the newest value)
EXPECT_EQ(std::optional(frc::Pose3d{1.02_m, 0_m, 0_m, frc::Rotation3d{}}),
estimator.SampleAt(1.02_s));
EXPECT_EQ(std::optional(frc::Pose3d{1.01_m, 0_m, 0_m, frc::Rotation3d{}}),
estimator.SampleAt(1.01_s));
EXPECT_EQ(std::optional(frc::Pose3d{1_m, 0_m, 0_m, frc::Rotation3d{}}),
estimator.SampleAt(0.5_s));
// Add a vision measurement before the odometry measurements that's still in
// the buffer
estimator.AddVisionMeasurement(
frc::Pose3d{1_m, 0.2_m, 0_m, frc::Rotation3d{}}, 0.9_s);
// Everything should be the same except Y is 0.1 (halfway between 0 and 0.2)
EXPECT_EQ(std::optional(frc::Pose3d{1.02_m, 0.1_m, 0_m, frc::Rotation3d{}}),
estimator.SampleAt(1.02_s));
EXPECT_EQ(std::optional(frc::Pose3d{1.01_m, 0.1_m, 0_m, frc::Rotation3d{}}),
estimator.SampleAt(1.01_s));
EXPECT_EQ(std::optional(frc::Pose3d{1_m, 0.1_m, 0_m, frc::Rotation3d{}}),
estimator.SampleAt(0.5_s));
EXPECT_EQ(std::optional(frc::Pose3d{2_m, 0.1_m, 0_m, frc::Rotation3d{}}),
estimator.SampleAt(2.5_s));
}
TEST(SwerveDrivePoseEstimator3dTest, TestReset) {
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::SwerveDrivePoseEstimator3d estimator{
kinematics,
frc::Rotation3d{},
{frc::SwerveModulePosition{}, frc::SwerveModulePosition{},
frc::SwerveModulePosition{}, frc::SwerveModulePosition{}},
frc::Pose3d{-1_m, -1_m, -1_m, frc::Rotation3d{0_rad, 0_rad, 1_rad}},
{1.0, 1.0, 1.0, 1.0},
{1.0, 1.0, 1.0, 1.0}};
// Test initial pose
EXPECT_DOUBLE_EQ(-1, estimator.GetEstimatedPosition().X().value());
EXPECT_DOUBLE_EQ(-1, estimator.GetEstimatedPosition().Y().value());
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(1, estimator.GetEstimatedPosition().Rotation().Z().value());
// Test reset position
{
frc::SwerveModulePosition modulePosition{1_m, frc::Rotation2d{}};
estimator.ResetPosition(
frc::Rotation3d{},
{modulePosition, modulePosition, modulePosition, modulePosition},
frc::Pose3d{1_m, 0_m, 0_m, frc::Rotation3d{}});
}
EXPECT_DOUBLE_EQ(1, 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 orientation and wheel positions
{
frc::SwerveModulePosition modulePosition{2_m, frc::Rotation2d{}};
estimator.Update(frc::Rotation3d{}, {modulePosition, modulePosition,
modulePosition, modulePosition});
}
EXPECT_DOUBLE_EQ(2, 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(frc::Rotation3d{0_deg, 0_deg, 90_deg});
EXPECT_DOUBLE_EQ(2, 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(std::numbers::pi / 2,
estimator.GetEstimatedPosition().Rotation().Z().value());
// Test orientation
{
frc::SwerveModulePosition modulePosition{3_m, frc::Rotation2d{}};
estimator.Update(frc::Rotation3d{}, {modulePosition, modulePosition,
modulePosition, modulePosition});
}
EXPECT_DOUBLE_EQ(2, 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 / 2,
estimator.GetEstimatedPosition().Rotation().Z().value());
// Test reset translation
estimator.ResetTranslation(frc::Translation3d{-1_m, -1_m, -1_m});
EXPECT_DOUBLE_EQ(-1, estimator.GetEstimatedPosition().X().value());
EXPECT_DOUBLE_EQ(-1, estimator.GetEstimatedPosition().Y().value());
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,
estimator.GetEstimatedPosition().Rotation().Z().value());
// Test reset pose
estimator.ResetPose(frc::Pose3d{});
EXPECT_DOUBLE_EQ(0, 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());
}

55
wpimath/src/test/native/cpp/estimator/SwerveDrivePoseEstimatorTest.cpp
View File

@@ -399,9 +399,10 @@ TEST(SwerveDrivePoseEstimatorTest, TestReset) {
{1.0, 1.0, 1.0}};
// Test initial pose
EXPECT_EQ(-1, estimator.GetEstimatedPosition().X().value());
EXPECT_EQ(-1, estimator.GetEstimatedPosition().Y().value());
EXPECT_EQ(1, estimator.GetEstimatedPosition().Rotation().Radians().value());
EXPECT_DOUBLE_EQ(-1, estimator.GetEstimatedPosition().X().value());
EXPECT_DOUBLE_EQ(-1, estimator.GetEstimatedPosition().Y().value());
EXPECT_DOUBLE_EQ(
1, estimator.GetEstimatedPosition().Rotation().Radians().value());
// Test reset position
{
@@ -412,9 +413,10 @@ TEST(SwerveDrivePoseEstimatorTest, TestReset) {
frc::Pose2d{1_m, 0_m, frc::Rotation2d{}});
}
EXPECT_EQ(1, estimator.GetEstimatedPosition().X().value());
EXPECT_EQ(0, estimator.GetEstimatedPosition().Y().value());
EXPECT_EQ(0, estimator.GetEstimatedPosition().Rotation().Radians().value());
EXPECT_DOUBLE_EQ(1, estimator.GetEstimatedPosition().X().value());
EXPECT_DOUBLE_EQ(0, estimator.GetEstimatedPosition().Y().value());
EXPECT_DOUBLE_EQ(
0, estimator.GetEstimatedPosition().Rotation().Radians().value());
// Test orientation and wheel positions
{
@@ -423,17 +425,19 @@ TEST(SwerveDrivePoseEstimatorTest, TestReset) {
modulePosition, modulePosition});
}
EXPECT_EQ(2, estimator.GetEstimatedPosition().X().value());
EXPECT_EQ(0, estimator.GetEstimatedPosition().Y().value());
EXPECT_EQ(0, estimator.GetEstimatedPosition().Rotation().Radians().value());
EXPECT_DOUBLE_EQ(2, 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(frc::Rotation2d{90_deg});
EXPECT_EQ(2, estimator.GetEstimatedPosition().X().value());
EXPECT_EQ(0, estimator.GetEstimatedPosition().Y().value());
EXPECT_EQ(std::numbers::pi / 2,
estimator.GetEstimatedPosition().Rotation().Radians().value());
EXPECT_DOUBLE_EQ(2, estimator.GetEstimatedPosition().X().value());
EXPECT_DOUBLE_EQ(0, estimator.GetEstimatedPosition().Y().value());
EXPECT_DOUBLE_EQ(
std::numbers::pi / 2,
estimator.GetEstimatedPosition().Rotation().Radians().value());
// Test orientation
{
@@ -442,23 +446,26 @@ TEST(SwerveDrivePoseEstimatorTest, TestReset) {
modulePosition, modulePosition});
}
EXPECT_EQ(2, estimator.GetEstimatedPosition().X().value());
EXPECT_EQ(1, estimator.GetEstimatedPosition().Y().value());
EXPECT_EQ(std::numbers::pi / 2,
estimator.GetEstimatedPosition().Rotation().Radians().value());
EXPECT_DOUBLE_EQ(2, estimator.GetEstimatedPosition().X().value());
EXPECT_DOUBLE_EQ(1, estimator.GetEstimatedPosition().Y().value());
EXPECT_DOUBLE_EQ(
std::numbers::pi / 2,
estimator.GetEstimatedPosition().Rotation().Radians().value());
// Test reset translation
estimator.ResetTranslation(frc::Translation2d{-1_m, -1_m});
EXPECT_EQ(-1, estimator.GetEstimatedPosition().X().value());
EXPECT_EQ(-1, estimator.GetEstimatedPosition().Y().value());
EXPECT_EQ(std::numbers::pi / 2,
estimator.GetEstimatedPosition().Rotation().Radians().value());
EXPECT_DOUBLE_EQ(-1, estimator.GetEstimatedPosition().X().value());
EXPECT_DOUBLE_EQ(-1, estimator.GetEstimatedPosition().Y().value());
EXPECT_DOUBLE_EQ(
std::numbers::pi / 2,
estimator.GetEstimatedPosition().Rotation().Radians().value());
// Test reset pose
estimator.ResetPose(frc::Pose2d{});
EXPECT_EQ(0, estimator.GetEstimatedPosition().X().value());
EXPECT_EQ(0, estimator.GetEstimatedPosition().Y().value());
EXPECT_EQ(0, estimator.GetEstimatedPosition().Rotation().Radians().value());
EXPECT_DOUBLE_EQ(0, estimator.GetEstimatedPosition().X().value());
EXPECT_DOUBLE_EQ(0, estimator.GetEstimatedPosition().Y().value());
EXPECT_DOUBLE_EQ(
0, estimator.GetEstimatedPosition().Rotation().Radians().value());
}

40
wpimath/src/test/native/cpp/kinematics/DifferentialDriveOdometry3dTest.cpp Normal file
View File

@@ -0,0 +1,40 @@
// 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 <numbers>
#include <gtest/gtest.h>
#include "frc/kinematics/DifferentialDriveKinematics.h"
#include "frc/kinematics/DifferentialDriveOdometry3d.h"
static constexpr double kEpsilon = 1E-9;
using namespace frc;
TEST(DifferentialDriveOdometry3dTest, Initialize) {
DifferentialDriveOdometry3d odometry{
frc::Rotation3d{0_deg, 0_deg, 90_deg}, 0_m, 0_m,
frc::Pose3d{1_m, 2_m, 0_m, frc::Rotation3d{0_deg, 0_deg, 45_deg}}};
const frc::Pose3d& pose = odometry.GetPose();
EXPECT_NEAR(pose.X().value(), 1, kEpsilon);
EXPECT_NEAR(pose.Y().value(), 2, kEpsilon);
EXPECT_NEAR(pose.Z().value(), 0, kEpsilon);
EXPECT_NEAR(pose.Rotation().ToRotation2d().Degrees().value(), 45, kEpsilon);
}
TEST(DifferentialDriveOdometry3dTest, EncoderDistances) {
DifferentialDriveOdometry3d odometry{frc::Rotation3d{0_deg, 0_deg, 45_deg},
0_m, 0_m};
const auto& pose = odometry.Update(frc::Rotation3d{0_deg, 0_deg, 135_deg},
0_m, units::meter_t{5 * std::numbers::pi});
EXPECT_NEAR(pose.X().value(), 5.0, kEpsilon);
EXPECT_NEAR(pose.Y().value(), 5.0, kEpsilon);
EXPECT_NEAR(pose.Z().value(), 0.0, kEpsilon);
EXPECT_NEAR(pose.Rotation().ToRotation2d().Degrees().value(), 90.0, kEpsilon);
}

223
wpimath/src/test/native/cpp/kinematics/MecanumDriveOdometry3dTest.cpp Normal file
View File

@@ -0,0 +1,223 @@
// 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 <limits>
#include <random>
#include <gtest/gtest.h>
#include "frc/kinematics/MecanumDriveOdometry3d.h"
#include "frc/trajectory/TrajectoryGenerator.h"
using namespace frc;
class MecanumDriveOdometry3dTest : public ::testing::Test {
protected:
Translation2d m_fl{12_m, 12_m};
Translation2d m_fr{12_m, -12_m};
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};
MecanumDriveOdometry3d odometry{kinematics, frc::Rotation3d{}, zero};
};
TEST_F(MecanumDriveOdometry3dTest, Initialize) {
MecanumDriveOdometry3d odometry{
kinematics, frc::Rotation3d{}, zero,
frc::Pose3d{1_m, 2_m, 0_m, frc::Rotation3d{0_deg, 0_deg, 45_deg}}};
const frc::Pose3d& pose = odometry.GetPose();
EXPECT_NEAR(pose.X().value(), 1, 1e-9);
EXPECT_NEAR(pose.Y().value(), 2, 1e-9);
EXPECT_NEAR(pose.Z().value(), 0, 1e-9);
EXPECT_NEAR(pose.Rotation().ToRotation2d().Degrees().value(), 45, 1e-9);
}
TEST_F(MecanumDriveOdometry3dTest, MultipleConsecutiveUpdates) {
MecanumDriveWheelPositions wheelDeltas{3.536_m, 3.536_m, 3.536_m, 3.536_m};
odometry.ResetPosition(frc::Rotation3d{}, wheelDeltas, Pose3d{});
odometry.Update(frc::Rotation3d{}, wheelDeltas);
auto secondPose = odometry.Update(frc::Rotation3d{}, wheelDeltas);
EXPECT_NEAR(secondPose.X().value(), 0.0, 0.01);
EXPECT_NEAR(secondPose.Y().value(), 0.0, 0.01);
EXPECT_NEAR(secondPose.Z().value(), 0.0, 0.01);
EXPECT_NEAR(secondPose.Rotation().ToRotation2d().Radians().value(), 0.0,
0.01);
}
TEST_F(MecanumDriveOdometry3dTest, TwoIterations) {
odometry.ResetPosition(frc::Rotation3d{}, zero, Pose3d{});
MecanumDriveWheelPositions wheelDeltas{0.3536_m, 0.3536_m, 0.3536_m,
0.3536_m};
odometry.Update(frc::Rotation3d{}, MecanumDriveWheelPositions{});
auto pose = odometry.Update(frc::Rotation3d{}, wheelDeltas);
EXPECT_NEAR(pose.X().value(), 0.3536, 0.01);
EXPECT_NEAR(pose.Y().value(), 0.0, 0.01);
EXPECT_NEAR(pose.Z().value(), 0.0, 0.01);
EXPECT_NEAR(pose.Rotation().ToRotation2d().Radians().value(), 0.0, 0.01);
}
TEST_F(MecanumDriveOdometry3dTest, 90DegreeTurn) {
odometry.ResetPosition(frc::Rotation3d{}, zero, Pose3d{});
MecanumDriveWheelPositions wheelDeltas{-13.328_m, 39.986_m, -13.329_m,
39.986_m};
odometry.Update(frc::Rotation3d{}, MecanumDriveWheelPositions{});
auto pose =
odometry.Update(frc::Rotation3d{0_deg, 0_deg, 90_deg}, wheelDeltas);
EXPECT_NEAR(pose.X().value(), 8.4855, 0.01);
EXPECT_NEAR(pose.Y().value(), 8.4855, 0.01);
EXPECT_NEAR(pose.Z().value(), 0, 0.01);
EXPECT_NEAR(pose.Rotation().ToRotation2d().Degrees().value(), 90.0, 0.01);
}
TEST_F(MecanumDriveOdometry3dTest, GyroAngleReset) {
odometry.ResetPosition(frc::Rotation3d{0_deg, 0_deg, 90_deg}, zero, Pose3d{});
MecanumDriveWheelPositions wheelDeltas{0.3536_m, 0.3536_m, 0.3536_m,
0.3536_m};
auto pose =
odometry.Update(frc::Rotation3d{0_deg, 0_deg, 90_deg}, wheelDeltas);
EXPECT_NEAR(pose.X().value(), 0.3536, 0.01);
EXPECT_NEAR(pose.Y().value(), 0.0, 0.01);
EXPECT_NEAR(pose.Z().value(), 0.0, 0.01);
EXPECT_NEAR(pose.Rotation().ToRotation2d().Radians().value(), 0.0, 0.01);
}
TEST_F(MecanumDriveOdometry3dTest, 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::MecanumDriveOdometry3d odometry{kinematics, frc::Rotation3d{},
wheelPositions};
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 = 20_ms;
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(
frc::Rotation3d{groundTruthState.pose.Rotation() +
frc::Rotation2d{distribution(generator) * 0.05_rad}},
wheelPositions);
double error = groundTruthState.pose.Translation()
.Distance(xhat.Translation().ToTranslation2d())
.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(MecanumDriveOdometry3dTest, 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::MecanumDriveOdometry3d odometry{kinematics, frc::Rotation3d{},
wheelPositions};
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 = 20_ms;
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::Rotation3d{0_rad, 0_rad, distribution(generator) * 0.05_rad},
wheelPositions);
double error = groundTruthState.pose.Translation()
.Distance(xhat.Translation().ToTranslation2d())
.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);
}

4
wpimath/src/test/native/cpp/kinematics/MecanumDriveOdometryTest.cpp
View File

@@ -85,7 +85,7 @@ TEST_F(MecanumDriveOdometryTest, AccuracyFacingTrajectory) {
frc::MecanumDriveWheelPositions wheelPositions;
frc::MecanumDriveOdometry odometry{kinematics, frc::Rotation2d{},
wheelPositions, frc::Pose2d{}};
wheelPositions};
frc::Trajectory trajectory = frc::TrajectoryGenerator::GenerateTrajectory(
std::vector{frc::Pose2d{0_m, 0_m, 45_deg}, frc::Pose2d{3_m, 0_m, -90_deg},
@@ -148,7 +148,7 @@ TEST_F(MecanumDriveOdometryTest, AccuracyFacingXAxis) {
frc::MecanumDriveWheelPositions wheelPositions;
frc::MecanumDriveOdometry odometry{kinematics, frc::Rotation2d{},
wheelPositions, frc::Pose2d{}};
wheelPositions};
frc::Trajectory trajectory = frc::TrajectoryGenerator::GenerateTrajectory(
std::vector{frc::Pose2d{0_m, 0_m, 45_deg}, frc::Pose2d{3_m, 0_m, -90_deg},

224
wpimath/src/test/native/cpp/kinematics/SwerveDriveOdometry3dTest.cpp Normal file
View File

@@ -0,0 +1,224 @@
// 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 <limits>
#include <random>
#include <gtest/gtest.h>
#include "frc/kinematics/SwerveDriveKinematics.h"
#include "frc/kinematics/SwerveDriveOdometry3d.h"
#include "frc/trajectory/Trajectory.h"
#include "frc/trajectory/TrajectoryConfig.h"
#include "frc/trajectory/TrajectoryGenerator.h"
using namespace frc;
static constexpr double kEpsilon = 0.01;
class SwerveDriveOdometry3dTest : public ::testing::Test {
protected:
Translation2d m_fl{12_m, 12_m};
Translation2d m_fr{12_m, -12_m};
Translation2d m_bl{-12_m, 12_m};
Translation2d m_br{-12_m, -12_m};
SwerveDriveKinematics<4> m_kinematics{m_fl, m_fr, m_bl, m_br};
SwerveModulePosition zero;
SwerveDriveOdometry3d<4> m_odometry{
m_kinematics, frc::Rotation3d{}, {zero, zero, zero, zero}};
};
TEST_F(SwerveDriveOdometry3dTest, Initialize) {
SwerveDriveOdometry3d odometry{
m_kinematics,
frc::Rotation3d{},
{zero, zero, zero, zero},
frc::Pose3d{1_m, 2_m, 0_m, frc::Rotation3d{0_deg, 0_deg, 45_deg}}};
const frc::Pose3d& pose = odometry.GetPose();
EXPECT_NEAR(pose.X().value(), 1, kEpsilon);
EXPECT_NEAR(pose.Y().value(), 2, kEpsilon);
EXPECT_NEAR(pose.Z().value(), 0, kEpsilon);
EXPECT_NEAR(pose.Rotation().ToRotation2d().Degrees().value(), 45, kEpsilon);
}
TEST_F(SwerveDriveOdometry3dTest, TwoIterations) {
SwerveModulePosition position{0.5_m, 0_deg};
m_odometry.ResetPosition(frc::Rotation3d{}, {zero, zero, zero, zero},
Pose3d{});
m_odometry.Update(frc::Rotation3d{}, {zero, zero, zero, zero});
auto pose = m_odometry.Update(frc::Rotation3d{},
{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.Z().value(), kEpsilon);
EXPECT_NEAR(0.0, pose.Rotation().ToRotation2d().Degrees().value(), kEpsilon);
}
TEST_F(SwerveDriveOdometry3dTest, 90DegreeTurn) {
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};
m_odometry.ResetPosition(frc::Rotation3d{}, {zero, zero, zero, zero},
Pose3d{});
auto pose = m_odometry.Update(frc::Rotation3d{0_deg, 0_deg, 90_deg},
{fl, fr, bl, br});
EXPECT_NEAR(12.0, pose.X().value(), kEpsilon);
EXPECT_NEAR(12.0, pose.Y().value(), kEpsilon);
EXPECT_NEAR(0.0, pose.Z().value(), kEpsilon);
EXPECT_NEAR(90.0, pose.Rotation().ToRotation2d().Degrees().value(), kEpsilon);
}
TEST_F(SwerveDriveOdometry3dTest, GyroAngleReset) {
m_odometry.ResetPosition(frc::Rotation3d{0_deg, 0_deg, 90_deg},
{zero, zero, zero, zero}, Pose3d{});
SwerveModulePosition position{0.5_m, 0_deg};
auto pose = m_odometry.Update(frc::Rotation3d{0_deg, 0_deg, 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.Z().value(), kEpsilon);
EXPECT_NEAR(0.0, pose.Rotation().ToRotation2d().Degrees().value(), kEpsilon);
}
TEST_F(SwerveDriveOdometry3dTest, AccuracyFacingTrajectory) {
SwerveDriveKinematics<4> kinematics{
Translation2d{1_m, 1_m}, Translation2d{1_m, -1_m},
Translation2d{-1_m, -1_m}, Translation2d{-1_m, 1_m}};
SwerveDriveOdometry3d<4> odometry{
kinematics, frc::Rotation3d{}, {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 = 20_ms;
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(
frc::Rotation3d{groundTruthState.pose.Rotation() +
frc::Rotation2d{distribution(generator) * 0.05_rad}},
{fl, fr, bl, br});
double error = groundTruthState.pose.Translation()
.Distance(xhat.Translation().ToTranslation2d())
.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(SwerveDriveOdometry3dTest, AccuracyFacingXAxis) {
SwerveDriveKinematics<4> kinematics{
Translation2d{1_m, 1_m}, Translation2d{1_m, -1_m},
Translation2d{-1_m, -1_m}, Translation2d{-1_m, 1_m}};
SwerveDriveOdometry3d<4> odometry{
kinematics, frc::Rotation3d{}, {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 = 20_ms;
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::Rotation3d{0_rad, 0_rad, distribution(generator) * 0.05_rad},
{fl, fr, bl, br});
double error = groundTruthState.pose.Translation()
.Distance(xhat.Translation().ToTranslation2d())
.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);
}

6
wpimath/src/test/native/cpp/kinematics/SwerveDriveOdometryTest.cpp
View File

@@ -77,7 +77,8 @@ TEST_F(SwerveDriveOdometryTest, AccuracyFacingTrajectory) {
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}};
SwerveDriveOdometry<4> odometry{
kinematics, frc::Rotation2d{}, {zero, zero, zero, zero}};
SwerveModulePosition fl;
SwerveModulePosition fr;
@@ -141,7 +142,8 @@ TEST_F(SwerveDriveOdometryTest, AccuracyFacingXAxis) {
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}};
SwerveDriveOdometry<4> odometry{
kinematics, frc::Rotation2d{}, {zero, zero, zero, zero}};
SwerveModulePosition fl;
SwerveModulePosition fr;