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

SCRIPT namespace replacements

Browse Source
This commit is contained in:
PJ Reiniger
2025-11-07 20:00:05 -05:00
committed by Peter Johnson
parent ae6c043632
commit 9aca8e0fd6
2622 changed files with 22275 additions and 22275 deletions
Download Patch File Download Diff File Expand all files Collapse all files

14
wpimath/src/test/native/cpp/estimator/AngleStatisticsTest.cpp
View File

@@ -10,7 +10,7 @@
#include "wpi/math/linalg/EigenCore.hpp"
TEST(AngleStatisticsTest, Mean) {
frc::Matrixd<3, 3> sigmas{
wpi::math::Matrixd<3, 3> sigmas{
{1, 1.2, 0},
{359 * std::numbers::pi / 180, 3 * std::numbers::pi / 180, 0},
{1, 2, 0}};
@@ -19,7 +19,7 @@ TEST(AngleStatisticsTest, Mean) {
weights.fill(1.0 / sigmas.cols());
EXPECT_TRUE(Eigen::Vector3d(0.7333333, 0.01163323, 1)
.isApprox(frc::AngleMean<3, 3>(sigmas, weights, 1), 1e-3));
.isApprox(wpi::math::AngleMean<3, 3>(sigmas, weights, 1), 1e-3));
}
TEST(AngleStatisticsTest, Mean_DynamicSize) {
@@ -32,7 +32,7 @@ TEST(AngleStatisticsTest, Mean_DynamicSize) {
weights.fill(1.0 / sigmas.cols());
EXPECT_TRUE(Eigen::Vector3d(0.7333333, 0.01163323, 1)
.isApprox(frc::AngleMean<Eigen::Dynamic, Eigen::Dynamic>(
.isApprox(wpi::math::AngleMean<Eigen::Dynamic, Eigen::Dynamic>(
sigmas, weights, 1),
1e-3));
}
@@ -41,7 +41,7 @@ TEST(AngleStatisticsTest, Residual) {
Eigen::Vector3d a{1, 1 * std::numbers::pi / 180, 2};
Eigen::Vector3d b{1, 359 * std::numbers::pi / 180, 1};
EXPECT_TRUE(frc::AngleResidual<3>(a, b, 1).isApprox(
EXPECT_TRUE(wpi::math::AngleResidual<3>(a, b, 1).isApprox(
Eigen::Vector3d{0, 2 * std::numbers::pi / 180, 1}));
}
@@ -49,7 +49,7 @@ TEST(AngleStatisticsTest, Residual_DynamicSize) {
Eigen::VectorXd a{{1, 1 * std::numbers::pi / 180, 2}};
Eigen::VectorXd b{{1, 359 * std::numbers::pi / 180, 1}};
EXPECT_TRUE(frc::AngleResidual<Eigen::Dynamic>(a, b, 1).isApprox(
EXPECT_TRUE(wpi::math::AngleResidual<Eigen::Dynamic>(a, b, 1).isApprox(
Eigen::VectorXd{{0, 2 * std::numbers::pi / 180, 1}}));
}
@@ -57,13 +57,13 @@ TEST(AngleStatisticsTest, Add) {
Eigen::Vector3d a{1, 1 * std::numbers::pi / 180, 2};
Eigen::Vector3d b{1, 359 * std::numbers::pi / 180, 1};
EXPECT_TRUE(frc::AngleAdd<3>(a, b, 1).isApprox(Eigen::Vector3d{2, 0, 3}));
EXPECT_TRUE(wpi::math::AngleAdd<3>(a, b, 1).isApprox(Eigen::Vector3d{2, 0, 3}));
}
TEST(AngleStatisticsTest, Add_DynamicSize) {
Eigen::VectorXd a{{1, 1 * std::numbers::pi / 180, 2}};
Eigen::VectorXd b{{1, 359 * std::numbers::pi / 180, 1}};
EXPECT_TRUE(frc::AngleAdd<Eigen::Dynamic>(a, b, 1).isApprox(
EXPECT_TRUE(wpi::math::AngleAdd<Eigen::Dynamic>(a, b, 1).isApprox(
Eigen::VectorXd{{2, 0, 3}}));
}

224
wpimath/src/test/native/cpp/estimator/DifferentialDrivePoseEstimator3dTest.cpp
View File

@@ -22,43 +22,43 @@
#include "wpi/util/print.hpp"
void testFollowTrajectory(
const frc::DifferentialDriveKinematics& kinematics,
frc::DifferentialDrivePoseEstimator3d& estimator,
const frc::Trajectory& trajectory,
std::function<frc::ChassisSpeeds(frc::Trajectory::State&)>
const wpi::math::DifferentialDriveKinematics& kinematics,
wpi::math::DifferentialDrivePoseEstimator3d& estimator,
const wpi::math::Trajectory& trajectory,
std::function<wpi::math::ChassisSpeeds(wpi::math::Trajectory::State&)>
chassisSpeedsGenerator,
std::function<frc::Pose2d(frc::Trajectory::State&)>
std::function<wpi::math::Pose2d(wpi::math::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 wpi::math::Pose2d& startingPose, const wpi::math::Pose2d& endingPose,
const wpi::units::second_t dt, const wpi::units::second_t kVisionUpdateRate,
const wpi::units::second_t kVisionUpdateDelay, const bool checkError,
const bool debug) {
units::meter_t leftDistance = 0_m;
units::meter_t rightDistance = 0_m;
wpi::units::meter_t leftDistance = 0_m;
wpi::units::meter_t rightDistance = 0_m;
estimator.ResetPosition(frc::Rotation3d{}, leftDistance, rightDistance,
frc::Pose3d{startingPose});
estimator.ResetPosition(wpi::math::Rotation3d{}, leftDistance, rightDistance,
wpi::math::Pose3d{startingPose});
std::default_random_engine generator;
std::normal_distribution<double> distribution(0.0, 1.0);
units::second_t t = 0_s;
wpi::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>>
std::vector<std::pair<wpi::units::second_t, wpi::math::Pose2d>> visionPoses;
std::vector<std::tuple<wpi::units::second_t, wpi::units::second_t, wpi::math::Pose2d>>
visionLog;
double maxError = -std::numeric_limits<double>::max();
double errorSum = 0;
if (debug) {
wpi::print(
wpi::util::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);
wpi::math::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
@@ -66,9 +66,9 @@ void testFollowTrajectory(
visionPoses.back().first + kVisionUpdateRate < t) {
auto visionPose =
visionMeasurementGenerator(groundTruthState) +
frc::Transform2d{frc::Translation2d{distribution(generator) * 0.1_m,
wpi::math::Transform2d{wpi::math::Translation2d{distribution(generator) * 0.1_m,
distribution(generator) * 0.1_m},
frc::Rotation2d{distribution(generator) * 0.05_rad}};
wpi::math::Rotation2d{distribution(generator) * 0.05_rad}};
visionPoses.push_back({t, visionPose});
}
@@ -77,7 +77,7 @@ void testFollowTrajectory(
if (!visionPoses.empty() &&
visionPoses.front().first + kVisionUpdateDelay < t) {
auto visionEntry = visionPoses.front();
estimator.AddVisionMeasurement(frc::Pose3d{visionEntry.second},
estimator.AddVisionMeasurement(wpi::math::Pose3d{visionEntry.second},
visionEntry.first);
visionPoses.erase(visionPoses.begin());
visionLog.push_back({t, visionEntry.first, visionEntry.second});
@@ -92,13 +92,13 @@ void testFollowTrajectory(
auto xhat = estimator.UpdateWithTime(
t,
frc::Rotation3d{groundTruthState.pose.Rotation() +
frc::Rotation2d{distribution(generator) * 0.05_rad} -
wpi::math::Rotation3d{groundTruthState.pose.Rotation() +
wpi::math::Rotation2d{distribution(generator) * 0.05_rad} -
trajectory.InitialPose().Rotation()},
leftDistance, rightDistance);
if (debug) {
wpi::print(
wpi::util::print(
"{}, {}, {}, {}, {}, {}, {}, {}, {}\n", t.value(), xhat.X().value(),
xhat.Y().value(), xhat.Rotation().ToRotation2d().Radians().value(),
groundTruthState.pose.X().value(), groundTruthState.pose.Y().value(),
@@ -119,14 +119,14 @@ void testFollowTrajectory(
}
if (debug) {
wpi::print("apply_time, measured_time, vision_x, vision_y, vision_theta\n");
wpi::util::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;
wpi::units::second_t apply_time;
wpi::units::second_t measure_time;
wpi::math::Pose2d vision_pose;
for (auto record : visionLog) {
std::tie(apply_time, measure_time, vision_pose) = record;
wpi::print("{}, {}, {}, {}, {}\n", apply_time.value(),
wpi::util::print("{}, {}, {}, {}, {}\n", apply_time.value(),
measure_time.value(), vision_pose.X().value(),
vision_pose.Y().value(),
vision_pose.Rotation().Radians().value());
@@ -153,73 +153,73 @@ void testFollowTrajectory(
}
TEST(DifferentialDrivePoseEstimator3dTest, Accuracy) {
frc::DifferentialDriveKinematics kinematics{1.0_m};
wpi::math::DifferentialDriveKinematics kinematics{1.0_m};
frc::DifferentialDrivePoseEstimator3d estimator{kinematics,
frc::Rotation3d{},
wpi::math::DifferentialDrivePoseEstimator3d estimator{kinematics,
wpi::math::Rotation3d{},
0_m,
0_m,
frc::Pose3d{},
wpi::math::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));
wpi::math::Trajectory trajectory = wpi::math::TrajectoryGenerator::GenerateTrajectory(
std::vector{wpi::math::Pose2d{0_m, 0_m, 45_deg}, wpi::math::Pose2d{3_m, 0_m, -90_deg},
wpi::math::Pose2d{0_m, 0_m, 135_deg},
wpi::math::Pose2d{-3_m, 0_m, -90_deg},
wpi::math::Pose2d{0_m, 0_m, 45_deg}},
wpi::math::TrajectoryConfig(2_mps, 2_mps_sq));
testFollowTrajectory(
kinematics, estimator, trajectory,
[&](frc::Trajectory::State& state) {
return frc::ChassisSpeeds{state.velocity, 0_mps,
[&](wpi::math::Trajectory::State& state) {
return wpi::math::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,
[&](wpi::math::Trajectory::State& state) { return state.pose; },
trajectory.InitialPose(), {0_m, 0_m, wpi::math::Rotation2d{45_deg}}, 20_ms,
100_ms, 250_ms, true, false);
}
TEST(DifferentialDrivePoseEstimator3dTest, BadInitialPose) {
frc::DifferentialDriveKinematics kinematics{1.0_m};
wpi::math::DifferentialDriveKinematics kinematics{1.0_m};
frc::DifferentialDrivePoseEstimator3d estimator{kinematics,
frc::Rotation3d{},
wpi::math::DifferentialDrivePoseEstimator3d estimator{kinematics,
wpi::math::Rotation3d{},
0_m,
0_m,
frc::Pose3d{},
wpi::math::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));
wpi::math::Trajectory trajectory = wpi::math::TrajectoryGenerator::GenerateTrajectory(
std::vector{wpi::math::Pose2d{0_m, 0_m, 45_deg}, wpi::math::Pose2d{3_m, 0_m, -90_deg},
wpi::math::Pose2d{0_m, 0_m, 135_deg},
wpi::math::Pose2d{-3_m, 0_m, -90_deg},
wpi::math::Pose2d{0_m, 0_m, 45_deg}},
wpi::math::TrajectoryConfig(2_mps, 2_mps_sq));
for (units::degree_t offset_direction_degs = 0_deg;
for (wpi::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;
for (wpi::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 pose_offset = wpi::math::Rotation2d{offset_direction_degs};
auto heading_offset = wpi::math::Rotation2d{offset_heading_degs};
auto initial_pose =
trajectory.InitialPose() +
frc::Transform2d{frc::Translation2d{pose_offset.Cos() * 1_m,
wpi::math::Transform2d{wpi::math::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,
[&](wpi::math::Trajectory::State& state) {
return wpi::math::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,
[&](wpi::math::Trajectory::State& state) { return state.pose; },
initial_pose, {0_m, 0_m, wpi::math::Rotation2d{45_deg}}, 20_ms, 100_ms,
250_ms, false, false);
}
}
@@ -231,33 +231,33 @@ TEST(DifferentialDrivePoseEstimator3dTest, SimultaneousVisionMeasurements) {
// 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};
wpi::math::DifferentialDriveKinematics kinematics{1.0_m};
frc::DifferentialDrivePoseEstimator3d estimator{
wpi::math::DifferentialDrivePoseEstimator3d estimator{
kinematics,
frc::Rotation3d{},
wpi::math::Rotation3d{},
0_m,
0_m,
frc::Pose3d{1_m, 2_m, 0_m, frc::Rotation3d{0_deg, 0_deg, 270_deg}},
wpi::math::Pose3d{1_m, 2_m, 0_m, wpi::math::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);
estimator.UpdateWithTime(0_s, wpi::math::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);
wpi::math::Pose3d{0_m, 0_m, 0_m, wpi::math::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);
wpi::math::Pose3d{3_m, 1_m, 0_m, wpi::math::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}},
wpi::math::Pose3d{2_m, 4_m, 0_m, wpi::math::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(
auto dx = wpi::units::math::abs(estimator.GetEstimatedPosition().X() - 0_m);
auto dy = wpi::units::math::abs(estimator.GetEstimatedPosition().Y() - 0_m);
auto dtheta = wpi::units::math::abs(
estimator.GetEstimatedPosition().Rotation().ToRotation2d().Radians() -
0_deg);
@@ -265,9 +265,9 @@ TEST(DifferentialDrivePoseEstimator3dTest, SimultaneousVisionMeasurements) {
}
{
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(
auto dx = wpi::units::math::abs(estimator.GetEstimatedPosition().X() - 3_m);
auto dy = wpi::units::math::abs(estimator.GetEstimatedPosition().Y() - 1_m);
auto dtheta = wpi::units::math::abs(
estimator.GetEstimatedPosition().Rotation().ToRotation2d().Radians() -
90_deg);
@@ -275,9 +275,9 @@ TEST(DifferentialDrivePoseEstimator3dTest, SimultaneousVisionMeasurements) {
}
{
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(
auto dx = wpi::units::math::abs(estimator.GetEstimatedPosition().X() - 2_m);
auto dy = wpi::units::math::abs(estimator.GetEstimatedPosition().Y() - 4_m);
auto dtheta = wpi::units::math::abs(
estimator.GetEstimatedPosition().Rotation().ToRotation2d().Radians() -
180_deg);
@@ -286,22 +286,22 @@ TEST(DifferentialDrivePoseEstimator3dTest, SimultaneousVisionMeasurements) {
}
TEST(DifferentialDrivePoseEstimator3dTest, TestDiscardStaleVisionMeasurements) {
frc::DifferentialDriveKinematics kinematics{1_m};
wpi::math::DifferentialDriveKinematics kinematics{1_m};
frc::DifferentialDrivePoseEstimator3d estimator{
kinematics, frc::Rotation3d{}, 0_m, 0_m, frc::Pose3d{},
wpi::math::DifferentialDrivePoseEstimator3d estimator{
kinematics, wpi::math::Rotation3d{}, 0_m, 0_m, wpi::math::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);
estimator.UpdateWithTime(time, wpi::math::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,
wpi::math::Pose3d{10_m, 10_m, 0_m, wpi::math::Rotation3d{0_rad, 0_rad, 0.1_rad}}, 1_s,
{0.1, 0.1, 0.1, 0.1});
EXPECT_NEAR(odometryPose.X().value(),
@@ -319,9 +319,9 @@ TEST(DifferentialDrivePoseEstimator3dTest, TestDiscardStaleVisionMeasurements) {
}
TEST(DifferentialDrivePoseEstimator3dTest, TestSampleAt) {
frc::DifferentialDriveKinematics kinematics{1_m};
frc::DifferentialDrivePoseEstimator3d estimator{
kinematics, frc::Rotation3d{}, 0_m, 0_m, frc::Pose3d{},
wpi::math::DifferentialDriveKinematics kinematics{1_m};
wpi::math::DifferentialDrivePoseEstimator3d estimator{
kinematics, wpi::math::Rotation3d{}, 0_m, 0_m, wpi::math::Pose3d{},
{1.0, 1.0, 1.0, 1.0}, {1.0, 1.0, 1.0, 1.0}};
// Returns empty when null
@@ -331,60 +331,60 @@ TEST(DifferentialDrivePoseEstimator3dTest, TestSampleAt) {
// 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});
estimator.UpdateWithTime(wpi::units::second_t{time}, wpi::math::Rotation3d{},
wpi::units::meter_t{time}, wpi::units::meter_t{time});
}
// Sample at an added time
EXPECT_EQ(std::optional(frc::Pose3d{1.02_m, 0_m, 0_m, frc::Rotation3d{}}),
EXPECT_EQ(std::optional(wpi::math::Pose3d{1.02_m, 0_m, 0_m, wpi::math::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{}}),
EXPECT_EQ(std::optional(wpi::math::Pose3d{1.01_m, 0_m, 0_m, wpi::math::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{}}),
EXPECT_EQ(std::optional(wpi::math::Pose3d{1_m, 0_m, 0_m, wpi::math::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{}}),
EXPECT_EQ(std::optional(wpi::math::Pose3d{2_m, 0_m, 0_m, wpi::math::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);
wpi::math::Pose3d{2_m, 0_m, 0_m, wpi::math::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{}}),
EXPECT_EQ(std::optional(wpi::math::Pose3d{1.02_m, 0_m, 0_m, wpi::math::Rotation3d{}}),
estimator.SampleAt(1.02_s));
EXPECT_EQ(std::optional(frc::Pose3d{1.01_m, 0_m, 0_m, frc::Rotation3d{}}),
EXPECT_EQ(std::optional(wpi::math::Pose3d{1.01_m, 0_m, 0_m, wpi::math::Rotation3d{}}),
estimator.SampleAt(1.01_s));
EXPECT_EQ(std::optional(frc::Pose3d{1_m, 0_m, 0_m, frc::Rotation3d{}}),
EXPECT_EQ(std::optional(wpi::math::Pose3d{1_m, 0_m, 0_m, wpi::math::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);
wpi::math::Pose3d{1_m, 0.2_m, 0_m, wpi::math::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{}}),
EXPECT_EQ(std::optional(wpi::math::Pose3d{1.02_m, 0.1_m, 0_m, wpi::math::Rotation3d{}}),
estimator.SampleAt(1.02_s));
EXPECT_EQ(std::optional(frc::Pose3d{1.01_m, 0.1_m, 0_m, frc::Rotation3d{}}),
EXPECT_EQ(std::optional(wpi::math::Pose3d{1.01_m, 0.1_m, 0_m, wpi::math::Rotation3d{}}),
estimator.SampleAt(1.01_s));
EXPECT_EQ(std::optional(frc::Pose3d{1_m, 0.1_m, 0_m, frc::Rotation3d{}}),
EXPECT_EQ(std::optional(wpi::math::Pose3d{1_m, 0.1_m, 0_m, wpi::math::Rotation3d{}}),
estimator.SampleAt(0.5_s));
EXPECT_EQ(std::optional(frc::Pose3d{2_m, 0.1_m, 0_m, frc::Rotation3d{}}),
EXPECT_EQ(std::optional(wpi::math::Pose3d{2_m, 0.1_m, 0_m, wpi::math::Rotation3d{}}),
estimator.SampleAt(2.5_s));
}
TEST(DifferentialDrivePoseEstimator3dTest, TestReset) {
frc::DifferentialDriveKinematics kinematics{1_m};
frc::DifferentialDrivePoseEstimator3d estimator{
wpi::math::DifferentialDriveKinematics kinematics{1_m};
wpi::math::DifferentialDrivePoseEstimator3d estimator{
kinematics,
frc::Rotation3d{},
wpi::math::Rotation3d{},
0_m,
0_m,
frc::Pose3d{-1_m, -1_m, -1_m, frc::Rotation3d{0_rad, 0_rad, 1_rad}},
wpi::math::Pose3d{-1_m, -1_m, -1_m, wpi::math::Rotation3d{0_rad, 0_rad, 1_rad}},
{1.0, 1.0, 1.0, 1.0},
{1.0, 1.0, 1.0, 1.0}};
@@ -397,8 +397,8 @@ TEST(DifferentialDrivePoseEstimator3dTest, TestReset) {
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{}});
estimator.ResetPosition(wpi::math::Rotation3d{}, 1_m, 1_m,
wpi::math::Pose3d{1_m, 0_m, 0_m, wpi::math::Rotation3d{}});
EXPECT_DOUBLE_EQ(1, estimator.GetEstimatedPosition().X().value());
EXPECT_DOUBLE_EQ(0, estimator.GetEstimatedPosition().Y().value());
@@ -408,7 +408,7 @@ TEST(DifferentialDrivePoseEstimator3dTest, TestReset) {
EXPECT_DOUBLE_EQ(0, estimator.GetEstimatedPosition().Rotation().Z().value());
// Test orientation and wheel positions
estimator.Update(frc::Rotation3d{}, 2_m, 2_m);
estimator.Update(wpi::math::Rotation3d{}, 2_m, 2_m);
EXPECT_DOUBLE_EQ(2, estimator.GetEstimatedPosition().X().value());
EXPECT_DOUBLE_EQ(0, estimator.GetEstimatedPosition().Y().value());
@@ -418,7 +418,7 @@ TEST(DifferentialDrivePoseEstimator3dTest, TestReset) {
EXPECT_DOUBLE_EQ(0, estimator.GetEstimatedPosition().Rotation().Z().value());
// Test reset rotation
estimator.ResetRotation(frc::Rotation3d{0_deg, 0_deg, 90_deg});
estimator.ResetRotation(wpi::math::Rotation3d{0_deg, 0_deg, 90_deg});
EXPECT_DOUBLE_EQ(2, estimator.GetEstimatedPosition().X().value());
EXPECT_DOUBLE_EQ(0, estimator.GetEstimatedPosition().Y().value());
@@ -429,7 +429,7 @@ TEST(DifferentialDrivePoseEstimator3dTest, TestReset) {
estimator.GetEstimatedPosition().Rotation().Z().value());
// Test orientation
estimator.Update(frc::Rotation3d{}, 3_m, 3_m);
estimator.Update(wpi::math::Rotation3d{}, 3_m, 3_m);
EXPECT_DOUBLE_EQ(2, estimator.GetEstimatedPosition().X().value());
EXPECT_DOUBLE_EQ(1, estimator.GetEstimatedPosition().Y().value());
@@ -440,7 +440,7 @@ TEST(DifferentialDrivePoseEstimator3dTest, TestReset) {
estimator.GetEstimatedPosition().Rotation().Z().value());
// Test reset translation
estimator.ResetTranslation(frc::Translation3d{-1_m, -1_m, -1_m});
estimator.ResetTranslation(wpi::math::Translation3d{-1_m, -1_m, -1_m});
EXPECT_DOUBLE_EQ(-1, estimator.GetEstimatedPosition().X().value());
EXPECT_DOUBLE_EQ(-1, estimator.GetEstimatedPosition().Y().value());
@@ -451,7 +451,7 @@ TEST(DifferentialDrivePoseEstimator3dTest, TestReset) {
estimator.GetEstimatedPosition().Rotation().Z().value());
// Test reset pose
estimator.ResetPose(frc::Pose3d{});
estimator.ResetPose(wpi::math::Pose3d{});
EXPECT_DOUBLE_EQ(0, estimator.GetEstimatedPosition().X().value());
EXPECT_DOUBLE_EQ(0, estimator.GetEstimatedPosition().Y().value());

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

@@ -23,43 +23,43 @@
#include "wpi/util/print.hpp"
void testFollowTrajectory(
const frc::DifferentialDriveKinematics& kinematics,
frc::DifferentialDrivePoseEstimator& estimator,
const frc::Trajectory& trajectory,
std::function<frc::ChassisSpeeds(frc::Trajectory::State&)>
const wpi::math::DifferentialDriveKinematics& kinematics,
wpi::math::DifferentialDrivePoseEstimator& estimator,
const wpi::math::Trajectory& trajectory,
std::function<wpi::math::ChassisSpeeds(wpi::math::Trajectory::State&)>
chassisSpeedsGenerator,
std::function<frc::Pose2d(frc::Trajectory::State&)>
std::function<wpi::math::Pose2d(wpi::math::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 wpi::math::Pose2d& startingPose, const wpi::math::Pose2d& endingPose,
const wpi::units::second_t dt, const wpi::units::second_t kVisionUpdateRate,
const wpi::units::second_t kVisionUpdateDelay, const bool checkError,
const bool debug) {
units::meter_t leftDistance = 0_m;
units::meter_t rightDistance = 0_m;
wpi::units::meter_t leftDistance = 0_m;
wpi::units::meter_t rightDistance = 0_m;
estimator.ResetPosition(frc::Rotation2d{}, leftDistance, rightDistance,
estimator.ResetPosition(wpi::math::Rotation2d{}, leftDistance, rightDistance,
startingPose);
std::default_random_engine generator;
std::normal_distribution<double> distribution(0.0, 1.0);
units::second_t t = 0_s;
wpi::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>>
std::vector<std::pair<wpi::units::second_t, wpi::math::Pose2d>> visionPoses;
std::vector<std::tuple<wpi::units::second_t, wpi::units::second_t, wpi::math::Pose2d>>
visionLog;
double maxError = -std::numeric_limits<double>::max();
double errorSum = 0;
if (debug) {
wpi::print(
wpi::util::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);
wpi::math::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
@@ -67,9 +67,9 @@ void testFollowTrajectory(
visionPoses.back().first + kVisionUpdateRate < t) {
auto visionPose =
visionMeasurementGenerator(groundTruthState) +
frc::Transform2d{frc::Translation2d{distribution(generator) * 0.1_m,
wpi::math::Transform2d{wpi::math::Translation2d{distribution(generator) * 0.1_m,
distribution(generator) * 0.1_m},
frc::Rotation2d{distribution(generator) * 0.05_rad}};
wpi::math::Rotation2d{distribution(generator) * 0.05_rad}};
visionPoses.push_back({t, visionPose});
}
@@ -93,12 +93,12 @@ void testFollowTrajectory(
auto xhat = estimator.UpdateWithTime(
t,
groundTruthState.pose.Rotation() +
frc::Rotation2d{distribution(generator) * 0.05_rad} -
wpi::math::Rotation2d{distribution(generator) * 0.05_rad} -
trajectory.InitialPose().Rotation(),
leftDistance, rightDistance);
if (debug) {
wpi::print(
wpi::util::print(
"{}, {}, {}, {}, {}, {}, {}, {}, {}\n", t.value(), xhat.X().value(),
xhat.Y().value(), xhat.Rotation().Radians().value(),
groundTruthState.pose.X().value(), groundTruthState.pose.Y().value(),
@@ -119,14 +119,14 @@ void testFollowTrajectory(
}
if (debug) {
wpi::print("apply_time, measured_time, vision_x, vision_y, vision_theta\n");
wpi::util::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;
wpi::units::second_t apply_time;
wpi::units::second_t measure_time;
wpi::math::Pose2d vision_pose;
for (auto record : visionLog) {
std::tie(apply_time, measure_time, vision_pose) = record;
wpi::print("{}, {}, {}, {}, {}\n", apply_time.value(),
wpi::util::print("{}, {}, {}, {}, {}\n", apply_time.value(),
measure_time.value(), vision_pose.X().value(),
vision_pose.Y().value(),
vision_pose.Rotation().Radians().value());
@@ -149,65 +149,65 @@ void testFollowTrajectory(
}
TEST(DifferentialDrivePoseEstimatorTest, Accuracy) {
frc::DifferentialDriveKinematics kinematics{1.0_m};
wpi::math::DifferentialDriveKinematics kinematics{1.0_m};
frc::DifferentialDrivePoseEstimator estimator{
kinematics, frc::Rotation2d{}, 0_m, 0_m, frc::Pose2d{},
wpi::math::DifferentialDrivePoseEstimator estimator{
kinematics, wpi::math::Rotation2d{}, 0_m, 0_m, wpi::math::Pose2d{},
{0.02, 0.02, 0.01}, {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));
wpi::math::Trajectory trajectory = wpi::math::TrajectoryGenerator::GenerateTrajectory(
std::vector{wpi::math::Pose2d{0_m, 0_m, 45_deg}, wpi::math::Pose2d{3_m, 0_m, -90_deg},
wpi::math::Pose2d{0_m, 0_m, 135_deg},
wpi::math::Pose2d{-3_m, 0_m, -90_deg},
wpi::math::Pose2d{0_m, 0_m, 45_deg}},
wpi::math::TrajectoryConfig(2_mps, 2_mps_sq));
testFollowTrajectory(
kinematics, estimator, trajectory,
[&](frc::Trajectory::State& state) {
return frc::ChassisSpeeds{state.velocity, 0_mps,
[&](wpi::math::Trajectory::State& state) {
return wpi::math::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,
[&](wpi::math::Trajectory::State& state) { return state.pose; },
trajectory.InitialPose(), {0_m, 0_m, wpi::math::Rotation2d{45_deg}}, 20_ms,
100_ms, 250_ms, true, false);
}
TEST(DifferentialDrivePoseEstimatorTest, BadInitialPose) {
frc::DifferentialDriveKinematics kinematics{1.0_m};
wpi::math::DifferentialDriveKinematics kinematics{1.0_m};
frc::DifferentialDrivePoseEstimator estimator{
kinematics, frc::Rotation2d{}, 0_m, 0_m, frc::Pose2d{},
wpi::math::DifferentialDrivePoseEstimator estimator{
kinematics, wpi::math::Rotation2d{}, 0_m, 0_m, wpi::math::Pose2d{},
{0.02, 0.02, 0.01}, {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));
wpi::math::Trajectory trajectory = wpi::math::TrajectoryGenerator::GenerateTrajectory(
std::vector{wpi::math::Pose2d{0_m, 0_m, 45_deg}, wpi::math::Pose2d{3_m, 0_m, -90_deg},
wpi::math::Pose2d{0_m, 0_m, 135_deg},
wpi::math::Pose2d{-3_m, 0_m, -90_deg},
wpi::math::Pose2d{0_m, 0_m, 45_deg}},
wpi::math::TrajectoryConfig(2_mps, 2_mps_sq));
for (units::degree_t offset_direction_degs = 0_deg;
for (wpi::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;
for (wpi::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 pose_offset = wpi::math::Rotation2d{offset_direction_degs};
auto heading_offset = wpi::math::Rotation2d{offset_heading_degs};
auto initial_pose =
trajectory.InitialPose() +
frc::Transform2d{frc::Translation2d{pose_offset.Cos() * 1_m,
wpi::math::Transform2d{wpi::math::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,
[&](wpi::math::Trajectory::State& state) {
return wpi::math::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,
[&](wpi::math::Trajectory::State& state) { return state.pose; },
initial_pose, {0_m, 0_m, wpi::math::Rotation2d{45_deg}}, 20_ms, 100_ms,
250_ms, false, false);
}
}
@@ -219,50 +219,50 @@ TEST(DifferentialDrivePoseEstimatorTest, SimultaneousVisionMeasurements) {
// 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};
wpi::math::DifferentialDriveKinematics kinematics{1.0_m};
frc::DifferentialDrivePoseEstimator estimator{
wpi::math::DifferentialDrivePoseEstimator estimator{
kinematics,
frc::Rotation2d{},
wpi::math::Rotation2d{},
0_m,
0_m,
frc::Pose2d{1_m, 2_m, frc::Rotation2d{270_deg}},
wpi::math::Pose2d{1_m, 2_m, wpi::math::Rotation2d{270_deg}},
{0.02, 0.02, 0.01},
{0.1, 0.1, 0.1}};
estimator.UpdateWithTime(0_s, frc::Rotation2d{}, 0_m, 0_m);
estimator.UpdateWithTime(0_s, wpi::math::Rotation2d{}, 0_m, 0_m);
for (int i = 0; i < 1000; i++) {
estimator.AddVisionMeasurement(
frc::Pose2d{0_m, 0_m, frc::Rotation2d{0_deg}}, 0_s);
wpi::math::Pose2d{0_m, 0_m, wpi::math::Rotation2d{0_deg}}, 0_s);
estimator.AddVisionMeasurement(
frc::Pose2d{3_m, 1_m, frc::Rotation2d{90_deg}}, 0_s);
wpi::math::Pose2d{3_m, 1_m, wpi::math::Rotation2d{90_deg}}, 0_s);
estimator.AddVisionMeasurement(
frc::Pose2d{2_m, 4_m, frc::Rotation2d{180_deg}}, 0_s);
wpi::math::Pose2d{2_m, 4_m, wpi::math::Rotation2d{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(
auto dx = wpi::units::math::abs(estimator.GetEstimatedPosition().X() - 0_m);
auto dy = wpi::units::math::abs(estimator.GetEstimatedPosition().Y() - 0_m);
auto dtheta = wpi::units::math::abs(
estimator.GetEstimatedPosition().Rotation().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(
auto dx = wpi::units::math::abs(estimator.GetEstimatedPosition().X() - 3_m);
auto dy = wpi::units::math::abs(estimator.GetEstimatedPosition().Y() - 1_m);
auto dtheta = wpi::units::math::abs(
estimator.GetEstimatedPosition().Rotation().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(
auto dx = wpi::units::math::abs(estimator.GetEstimatedPosition().X() - 2_m);
auto dy = wpi::units::math::abs(estimator.GetEstimatedPosition().Y() - 4_m);
auto dtheta = wpi::units::math::abs(
estimator.GetEstimatedPosition().Rotation().Radians() - 180_deg);
EXPECT_TRUE(dx > 0.08_m || dy > 0.08_m || dtheta > 0.08_rad);
@@ -270,22 +270,22 @@ TEST(DifferentialDrivePoseEstimatorTest, SimultaneousVisionMeasurements) {
}
TEST(DifferentialDrivePoseEstimatorTest, TestDiscardStaleVisionMeasurements) {
frc::DifferentialDriveKinematics kinematics{1_m};
wpi::math::DifferentialDriveKinematics kinematics{1_m};
frc::DifferentialDrivePoseEstimator estimator{
kinematics, frc::Rotation2d{}, 0_m, 0_m, frc::Pose2d{},
wpi::math::DifferentialDrivePoseEstimator estimator{
kinematics, wpi::math::Rotation2d{}, 0_m, 0_m, wpi::math::Pose2d{},
{0.1, 0.1, 0.1}, {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::Rotation2d{}, 0_m, 0_m);
estimator.UpdateWithTime(time, wpi::math::Rotation2d{}, 0_m, 0_m);
}
auto odometryPose = estimator.GetEstimatedPosition();
// Apply a vision measurement from 3 seconds ago
estimator.AddVisionMeasurement(
frc::Pose2d{frc::Translation2d{10_m, 10_m}, frc::Rotation2d{0.1_rad}},
wpi::math::Pose2d{wpi::math::Translation2d{10_m, 10_m}, wpi::math::Rotation2d{0.1_rad}},
1_s, {0.1, 0.1, 0.1});
EXPECT_NEAR(odometryPose.X().value(),
@@ -298,9 +298,9 @@ TEST(DifferentialDrivePoseEstimatorTest, TestDiscardStaleVisionMeasurements) {
}
TEST(DifferentialDrivePoseEstimatorTest, TestSampleAt) {
frc::DifferentialDriveKinematics kinematics{1_m};
frc::DifferentialDrivePoseEstimator estimator{
kinematics, frc::Rotation2d{}, 0_m, 0_m, frc::Pose2d{},
wpi::math::DifferentialDriveKinematics kinematics{1_m};
wpi::math::DifferentialDrivePoseEstimator estimator{
kinematics, wpi::math::Rotation2d{}, 0_m, 0_m, wpi::math::Pose2d{},
{1.0, 1.0, 1.0}, {1.0, 1.0, 1.0}};
// Returns empty when null
@@ -310,60 +310,60 @@ TEST(DifferentialDrivePoseEstimatorTest, TestSampleAt) {
// 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::Rotation2d{},
units::meter_t{time}, units::meter_t{time});
estimator.UpdateWithTime(wpi::units::second_t{time}, wpi::math::Rotation2d{},
wpi::units::meter_t{time}, wpi::units::meter_t{time});
}
// Sample at an added time
EXPECT_EQ(std::optional(frc::Pose2d{1.02_m, 0_m, frc::Rotation2d{}}),
EXPECT_EQ(std::optional(wpi::math::Pose2d{1.02_m, 0_m, wpi::math::Rotation2d{}}),
estimator.SampleAt(1.02_s));
// Sample between updates (test interpolation)
EXPECT_EQ(std::optional(frc::Pose2d{1.01_m, 0_m, frc::Rotation2d{}}),
EXPECT_EQ(std::optional(wpi::math::Pose2d{1.01_m, 0_m, wpi::math::Rotation2d{}}),
estimator.SampleAt(1.01_s));
// Sampling before the oldest value returns the oldest value
EXPECT_EQ(std::optional(frc::Pose2d{1_m, 0_m, frc::Rotation2d{}}),
EXPECT_EQ(std::optional(wpi::math::Pose2d{1_m, 0_m, wpi::math::Rotation2d{}}),
estimator.SampleAt(0.5_s));
// Sampling after the newest value returns the newest value
EXPECT_EQ(std::optional(frc::Pose2d{2_m, 0_m, frc::Rotation2d{}}),
EXPECT_EQ(std::optional(wpi::math::Pose2d{2_m, 0_m, wpi::math::Rotation2d{}}),
estimator.SampleAt(2.5_s));
// Add a vision measurement after the odometry measurements (while keeping all
// of the old odometry measurements)
estimator.AddVisionMeasurement(frc::Pose2d{2_m, 0_m, frc::Rotation2d{1_rad}},
estimator.AddVisionMeasurement(wpi::math::Pose2d{2_m, 0_m, wpi::math::Rotation2d{1_rad}},
2.2_s);
// Make sure nothing changed (except the newest value)
EXPECT_EQ(std::optional(frc::Pose2d{1.02_m, 0_m, frc::Rotation2d{}}),
EXPECT_EQ(std::optional(wpi::math::Pose2d{1.02_m, 0_m, wpi::math::Rotation2d{}}),
estimator.SampleAt(1.02_s));
EXPECT_EQ(std::optional(frc::Pose2d{1.01_m, 0_m, frc::Rotation2d{}}),
EXPECT_EQ(std::optional(wpi::math::Pose2d{1.01_m, 0_m, wpi::math::Rotation2d{}}),
estimator.SampleAt(1.01_s));
EXPECT_EQ(std::optional(frc::Pose2d{1_m, 0_m, frc::Rotation2d{}}),
EXPECT_EQ(std::optional(wpi::math::Pose2d{1_m, 0_m, wpi::math::Rotation2d{}}),
estimator.SampleAt(0.5_s));
// Add a vision measurement before the odometry measurements that's still in
// the buffer
estimator.AddVisionMeasurement(frc::Pose2d{1_m, 0.2_m, frc::Rotation2d{}},
estimator.AddVisionMeasurement(wpi::math::Pose2d{1_m, 0.2_m, wpi::math::Rotation2d{}},
0.9_s);
// Everything should be the same except Y is 0.1 (halfway between 0 and 0.2)
EXPECT_EQ(std::optional(frc::Pose2d{1.02_m, 0.1_m, frc::Rotation2d{}}),
EXPECT_EQ(std::optional(wpi::math::Pose2d{1.02_m, 0.1_m, wpi::math::Rotation2d{}}),
estimator.SampleAt(1.02_s));
EXPECT_EQ(std::optional(frc::Pose2d{1.01_m, 0.1_m, frc::Rotation2d{}}),
EXPECT_EQ(std::optional(wpi::math::Pose2d{1.01_m, 0.1_m, wpi::math::Rotation2d{}}),
estimator.SampleAt(1.01_s));
EXPECT_EQ(std::optional(frc::Pose2d{1_m, 0.1_m, frc::Rotation2d{}}),
EXPECT_EQ(std::optional(wpi::math::Pose2d{1_m, 0.1_m, wpi::math::Rotation2d{}}),
estimator.SampleAt(0.5_s));
EXPECT_EQ(std::optional(frc::Pose2d{2_m, 0.1_m, frc::Rotation2d{}}),
EXPECT_EQ(std::optional(wpi::math::Pose2d{2_m, 0.1_m, wpi::math::Rotation2d{}}),
estimator.SampleAt(2.5_s));
}
TEST(DifferentialDrivePoseEstimatorTest, TestReset) {
frc::DifferentialDriveKinematics kinematics{1_m};
frc::DifferentialDrivePoseEstimator estimator{
wpi::math::DifferentialDriveKinematics kinematics{1_m};
wpi::math::DifferentialDrivePoseEstimator estimator{
kinematics,
frc::Rotation2d{},
wpi::math::Rotation2d{},
0_m,
0_m,
frc::Pose2d{-1_m, -1_m, frc::Rotation2d{1_rad}},
wpi::math::Pose2d{-1_m, -1_m, wpi::math::Rotation2d{1_rad}},
{1.0, 1.0, 1.0},
{1.0, 1.0, 1.0}};
@@ -374,8 +374,8 @@ TEST(DifferentialDrivePoseEstimatorTest, TestReset) {
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{}});
estimator.ResetPosition(wpi::math::Rotation2d{}, 1_m, 1_m,
wpi::math::Pose2d{1_m, 0_m, wpi::math::Rotation2d{}});
EXPECT_DOUBLE_EQ(1, estimator.GetEstimatedPosition().X().value());
EXPECT_DOUBLE_EQ(0, estimator.GetEstimatedPosition().Y().value());
@@ -383,7 +383,7 @@ TEST(DifferentialDrivePoseEstimatorTest, TestReset) {
0, estimator.GetEstimatedPosition().Rotation().Radians().value());
// Test orientation and wheel positions
estimator.Update(frc::Rotation2d{}, 2_m, 2_m);
estimator.Update(wpi::math::Rotation2d{}, 2_m, 2_m);
EXPECT_DOUBLE_EQ(2, estimator.GetEstimatedPosition().X().value());
EXPECT_DOUBLE_EQ(0, estimator.GetEstimatedPosition().Y().value());
@@ -391,7 +391,7 @@ TEST(DifferentialDrivePoseEstimatorTest, TestReset) {
0, estimator.GetEstimatedPosition().Rotation().Radians().value());
// Test reset rotation
estimator.ResetRotation(frc::Rotation2d{90_deg});
estimator.ResetRotation(wpi::math::Rotation2d{90_deg});
EXPECT_DOUBLE_EQ(2, estimator.GetEstimatedPosition().X().value());
EXPECT_DOUBLE_EQ(0, estimator.GetEstimatedPosition().Y().value());
@@ -400,7 +400,7 @@ TEST(DifferentialDrivePoseEstimatorTest, TestReset) {
estimator.GetEstimatedPosition().Rotation().Radians().value());
// Test orientation
estimator.Update(frc::Rotation2d{}, 3_m, 3_m);
estimator.Update(wpi::math::Rotation2d{}, 3_m, 3_m);
EXPECT_DOUBLE_EQ(2, estimator.GetEstimatedPosition().X().value());
EXPECT_DOUBLE_EQ(1, estimator.GetEstimatedPosition().Y().value());
@@ -409,7 +409,7 @@ TEST(DifferentialDrivePoseEstimatorTest, TestReset) {
estimator.GetEstimatedPosition().Rotation().Radians().value());
// Test reset translation
estimator.ResetTranslation(frc::Translation2d{-1_m, -1_m});
estimator.ResetTranslation(wpi::math::Translation2d{-1_m, -1_m});
EXPECT_DOUBLE_EQ(-1, estimator.GetEstimatedPosition().X().value());
EXPECT_DOUBLE_EQ(-1, estimator.GetEstimatedPosition().Y().value());
@@ -418,7 +418,7 @@ TEST(DifferentialDrivePoseEstimatorTest, TestReset) {
estimator.GetEstimatedPosition().Rotation().Radians().value());
// Test reset pose
estimator.ResetPose(frc::Pose2d{});
estimator.ResetPose(wpi::math::Pose2d{});
EXPECT_DOUBLE_EQ(0, estimator.GetEstimatedPosition().X().value());
EXPECT_DOUBLE_EQ(0, estimator.GetEstimatedPosition().Y().value());

76
wpimath/src/test/native/cpp/estimator/ExtendedKalmanFilterTest.cpp
View File

@@ -18,8 +18,8 @@
namespace {
frc::Vectord<5> Dynamics(const frc::Vectord<5>& x, const frc::Vectord<2>& u) {
auto motors = frc::DCMotor::CIM(2);
wpi::math::Vectord<5> Dynamics(const wpi::math::Vectord<5>& x, const wpi::math::Vectord<2>& u) {
auto motors = wpi::math::DCMotor::CIM(2);
// constexpr double Glow = 15.32; // Low gear ratio
constexpr double Ghigh = 7.08; // High gear ratio
@@ -29,18 +29,18 @@ frc::Vectord<5> Dynamics(const frc::Vectord<5>& x, const frc::Vectord<2>& u) {
constexpr auto J = 5.6_kg_sq_m; // Robot moment of inertia
auto C1 = -std::pow(Ghigh, 2) * motors.Kt /
(motors.Kv * motors.R * units::math::pow<2>(r));
(motors.Kv * motors.R * wpi::units::math::pow<2>(r));
auto C2 = Ghigh * motors.Kt / (motors.R * r);
auto k1 = (1 / m + units::math::pow<2>(rb) / J);
auto k2 = (1 / m - units::math::pow<2>(rb) / J);
auto k1 = (1 / m + wpi::units::math::pow<2>(rb) / J);
auto k2 = (1 / m - wpi::units::math::pow<2>(rb) / J);
units::meters_per_second_t vl{x(3)};
units::meters_per_second_t vr{x(4)};
units::volt_t Vl{u(0)};
units::volt_t Vr{u(1)};
wpi::units::meters_per_second_t vl{x(3)};
wpi::units::meters_per_second_t vr{x(4)};
wpi::units::volt_t Vl{u(0)};
wpi::units::volt_t Vr{u(1)};
auto v = 0.5 * (vl + vr);
return frc::Vectord<5>{
return wpi::math::Vectord<5>{
v.value() * std::cos(x(2)), v.value() * std::sin(x(2)),
((vr - vl) / (2.0 * rb)).value(),
k1.value() * ((C1 * vl).value() + (C2 * Vl).value()) +
@@ -49,59 +49,59 @@ frc::Vectord<5> Dynamics(const frc::Vectord<5>& x, const frc::Vectord<2>& u) {
k1.value() * ((C1 * vr).value() + (C2 * Vr).value())};
}
frc::Vectord<3> LocalMeasurementModel(
const frc::Vectord<5>& x, [[maybe_unused]] const frc::Vectord<2>& u) {
return frc::Vectord<3>{x(2), x(3), x(4)};
wpi::math::Vectord<3> LocalMeasurementModel(
const wpi::math::Vectord<5>& x, [[maybe_unused]] const wpi::math::Vectord<2>& u) {
return wpi::math::Vectord<3>{x(2), x(3), x(4)};
}
frc::Vectord<5> GlobalMeasurementModel(
const frc::Vectord<5>& x, [[maybe_unused]] const frc::Vectord<2>& u) {
return frc::Vectord<5>{x(0), x(1), x(2), x(3), x(4)};
wpi::math::Vectord<5> GlobalMeasurementModel(
const wpi::math::Vectord<5>& x, [[maybe_unused]] const wpi::math::Vectord<2>& u) {
return wpi::math::Vectord<5>{x(0), x(1), x(2), x(3), x(4)};
}
} // namespace
TEST(ExtendedKalmanFilterTest, Init) {
constexpr units::second_t dt = 5_ms;
constexpr wpi::units::second_t dt = 5_ms;
frc::ExtendedKalmanFilter<5, 2, 3> observer{Dynamics,
wpi::math::ExtendedKalmanFilter<5, 2, 3> observer{Dynamics,
LocalMeasurementModel,
{0.5, 0.5, 10.0, 1.0, 1.0},
{0.0001, 0.01, 0.01},
dt};
frc::Vectord<2> u{12.0, 12.0};
wpi::math::Vectord<2> u{12.0, 12.0};
observer.Predict(u, dt);
auto localY = LocalMeasurementModel(observer.Xhat(), u);
observer.Correct(u, localY);
auto globalY = GlobalMeasurementModel(observer.Xhat(), u);
auto R = frc::MakeCovMatrix(0.01, 0.01, 0.0001, 0.01, 0.01);
auto R = wpi::math::MakeCovMatrix(0.01, 0.01, 0.0001, 0.01, 0.01);
observer.Correct<5>(u, globalY, GlobalMeasurementModel, R);
}
TEST(ExtendedKalmanFilterTest, Convergence) {
constexpr units::second_t dt = 5_ms;
constexpr wpi::units::second_t dt = 5_ms;
constexpr auto rb = 0.8382_m / 2.0; // Robot radius
frc::ExtendedKalmanFilter<5, 2, 3> observer{Dynamics,
wpi::math::ExtendedKalmanFilter<5, 2, 3> observer{Dynamics,
LocalMeasurementModel,
{0.5, 0.5, 10.0, 1.0, 1.0},
{0.0001, 0.5, 0.5},
dt};
auto waypoints =
std::vector<frc::Pose2d>{frc::Pose2d{2.75_m, 22.521_m, 0_rad},
frc::Pose2d{24.73_m, 19.68_m, 5.846_rad}};
auto trajectory = frc::TrajectoryGenerator::GenerateTrajectory(
std::vector<wpi::math::Pose2d>{wpi::math::Pose2d{2.75_m, 22.521_m, 0_rad},
wpi::math::Pose2d{24.73_m, 19.68_m, 5.846_rad}};
auto trajectory = wpi::math::TrajectoryGenerator::GenerateTrajectory(
waypoints, {8.8_mps, 0.1_mps_sq});
frc::Vectord<5> r = frc::Vectord<5>::Zero();
frc::Vectord<2> u = frc::Vectord<2>::Zero();
wpi::math::Vectord<5> r = wpi::math::Vectord<5>::Zero();
wpi::math::Vectord<2> u = wpi::math::Vectord<2>::Zero();
auto B = frc::NumericalJacobianU<5, 5, 2>(Dynamics, frc::Vectord<5>::Zero(),
frc::Vectord<2>::Zero());
auto B = wpi::math::NumericalJacobianU<5, 5, 2>(Dynamics, wpi::math::Vectord<5>::Zero(),
wpi::math::Vectord<2>::Zero());
observer.SetXhat(frc::Vectord<5>{
observer.SetXhat(wpi::math::Vectord<5>{
trajectory.InitialPose().Translation().X().value(),
trajectory.InitialPose().Translation().Y().value(),
trajectory.InitialPose().Rotation().Radians().value(), 0.0, 0.0});
@@ -109,20 +109,20 @@ TEST(ExtendedKalmanFilterTest, Convergence) {
auto totalTime = trajectory.TotalTime();
for (size_t i = 0; i < (totalTime / dt).value(); ++i) {
auto ref = trajectory.Sample(dt * i);
units::meters_per_second_t vl =
wpi::units::meters_per_second_t vl =
ref.velocity * (1 - (ref.curvature * rb).value());
units::meters_per_second_t vr =
wpi::units::meters_per_second_t vr =
ref.velocity * (1 + (ref.curvature * rb).value());
frc::Vectord<5> nextR{
wpi::math::Vectord<5> nextR{
ref.pose.Translation().X().value(), ref.pose.Translation().Y().value(),
ref.pose.Rotation().Radians().value(), vl.value(), vr.value()};
auto localY = LocalMeasurementModel(nextR, frc::Vectord<2>::Zero());
observer.Correct(u, localY + frc::MakeWhiteNoiseVector(0.0001, 0.5, 0.5));
auto localY = LocalMeasurementModel(nextR, wpi::math::Vectord<2>::Zero());
observer.Correct(u, localY + wpi::math::MakeWhiteNoiseVector(0.0001, 0.5, 0.5));
frc::Vectord<5> rdot = (nextR - r) / dt.value();
u = B.householderQr().solve(rdot - Dynamics(r, frc::Vectord<2>::Zero()));
wpi::math::Vectord<5> rdot = (nextR - r) / dt.value();
u = B.householderQr().solve(rdot - Dynamics(r, wpi::math::Vectord<2>::Zero()));
observer.Predict(u, dt);
@@ -133,7 +133,7 @@ TEST(ExtendedKalmanFilterTest, Convergence) {
observer.Correct(u, localY);
auto globalY = GlobalMeasurementModel(observer.Xhat(), u);
auto R = frc::MakeCovMatrix(0.01, 0.01, 0.0001, 0.5, 0.5);
auto R = wpi::math::MakeCovMatrix(0.01, 0.01, 0.0001, 0.5, 0.5);
observer.Correct<5>(u, globalY, GlobalMeasurementModel, R);
auto finalPosition = trajectory.Sample(trajectory.TotalTime());

6
wpimath/src/test/native/cpp/estimator/KalmanFilterTest.cpp
View File

@@ -15,7 +15,7 @@
#include "wpi/units/time.hpp"
TEST(KalmanFilterTest, Flywheel) {
auto motor = frc::DCMotor::NEO();
auto flywheel = frc::LinearSystemId::FlywheelSystem(motor, 1_kg_sq_m, 1.0);
frc::KalmanFilter<1, 1, 1> kf{flywheel, {1}, {1}, 5_ms};
auto motor = wpi::math::DCMotor::NEO();
auto flywheel = wpi::math::LinearSystemId::FlywheelSystem(motor, 1_kg_sq_m, 1.0);
wpi::math::KalmanFilter<1, 1, 1> kf{flywheel, {1}, {1}, 5_ms};
}

264
wpimath/src/test/native/cpp/estimator/MecanumDrivePoseEstimator3dTest.cpp
View File

@@ -16,40 +16,40 @@
#include "wpi/util/print.hpp"
void testFollowTrajectory(
const frc::MecanumDriveKinematics& kinematics,
frc::MecanumDrivePoseEstimator3d& estimator,
const frc::Trajectory& trajectory,
std::function<frc::ChassisSpeeds(frc::Trajectory::State&)>
const wpi::math::MecanumDriveKinematics& kinematics,
wpi::math::MecanumDrivePoseEstimator3d& estimator,
const wpi::math::Trajectory& trajectory,
std::function<wpi::math::ChassisSpeeds(wpi::math::Trajectory::State&)>
chassisSpeedsGenerator,
std::function<frc::Pose2d(frc::Trajectory::State&)>
std::function<wpi::math::Pose2d(wpi::math::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 wpi::math::Pose2d& startingPose, const wpi::math::Pose2d& endingPose,
const wpi::units::second_t dt, const wpi::units::second_t kVisionUpdateRate,
const wpi::units::second_t kVisionUpdateDelay, const bool checkError,
const bool debug) {
frc::MecanumDriveWheelPositions wheelPositions{};
wpi::math::MecanumDriveWheelPositions wheelPositions{};
estimator.ResetPosition(frc::Rotation3d{}, wheelPositions,
frc::Pose3d{startingPose});
estimator.ResetPosition(wpi::math::Rotation3d{}, wheelPositions,
wpi::math::Pose3d{startingPose});
std::default_random_engine generator;
std::normal_distribution<double> distribution(0.0, 1.0);
units::second_t t = 0_s;
wpi::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>>
std::vector<std::pair<wpi::units::second_t, wpi::math::Pose2d>> visionPoses;
std::vector<std::tuple<wpi::units::second_t, wpi::units::second_t, wpi::math::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");
wpi::util::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);
wpi::math::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
@@ -57,9 +57,9 @@ void testFollowTrajectory(
visionPoses.back().first + kVisionUpdateRate < t) {
auto visionPose =
visionMeasurementGenerator(groundTruthState) +
frc::Transform2d{frc::Translation2d{distribution(generator) * 0.1_m,
wpi::math::Transform2d{wpi::math::Translation2d{distribution(generator) * 0.1_m,
distribution(generator) * 0.1_m},
frc::Rotation2d{distribution(generator) * 0.05_rad}};
wpi::math::Rotation2d{distribution(generator) * 0.05_rad}};
visionPoses.push_back({t, visionPose});
}
@@ -68,7 +68,7 @@ void testFollowTrajectory(
if (!visionPoses.empty() &&
visionPoses.front().first + kVisionUpdateDelay < t) {
auto visionEntry = visionPoses.front();
estimator.AddVisionMeasurement(frc::Pose3d{visionEntry.second},
estimator.AddVisionMeasurement(wpi::math::Pose3d{visionEntry.second},
visionEntry.first);
visionPoses.erase(visionPoses.begin());
visionLog.push_back({t, visionEntry.first, visionEntry.second});
@@ -85,13 +85,13 @@ void testFollowTrajectory(
auto xhat = estimator.UpdateWithTime(
t,
frc::Rotation3d{groundTruthState.pose.Rotation() +
frc::Rotation2d{distribution(generator) * 0.05_rad} -
wpi::math::Rotation3d{groundTruthState.pose.Rotation() +
wpi::math::Rotation2d{distribution(generator) * 0.05_rad} -
trajectory.InitialPose().Rotation()},
wheelPositions);
if (debug) {
wpi::print(
wpi::util::print(
"{}, {}, {}, {}, {}, {}, {}\n", t.value(), xhat.X().value(),
xhat.Y().value(), xhat.Rotation().ToRotation2d().Radians().value(),
groundTruthState.pose.X().value(), groundTruthState.pose.Y().value(),
@@ -111,14 +111,14 @@ void testFollowTrajectory(
}
if (debug) {
wpi::print("apply_time, measured_time, vision_x, vision_y, vision_theta\n");
wpi::util::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;
wpi::units::second_t apply_time;
wpi::units::second_t measure_time;
wpi::math::Pose2d vision_pose;
for (auto record : visionLog) {
std::tie(apply_time, measure_time, vision_pose) = record;
wpi::print("{}, {}, {}, {}, {}\n", apply_time.value(),
wpi::util::print("{}, {}, {}, {}, {}\n", apply_time.value(),
measure_time.value(), vision_pose.X().value(),
vision_pose.Y().value(),
vision_pose.Rotation().Radians().value());
@@ -145,73 +145,73 @@ void testFollowTrajectory(
}
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}};
wpi::math::MecanumDriveKinematics kinematics{
wpi::math::Translation2d{1_m, 1_m}, wpi::math::Translation2d{1_m, -1_m},
wpi::math::Translation2d{-1_m, -1_m}, wpi::math::Translation2d{-1_m, 1_m}};
frc::MecanumDriveWheelPositions wheelPositions;
wpi::math::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}};
wpi::math::MecanumDrivePoseEstimator3d estimator{
kinematics, wpi::math::Rotation3d{}, wheelPositions,
wpi::math::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));
wpi::math::Trajectory trajectory = wpi::math::TrajectoryGenerator::GenerateTrajectory(
std::vector{wpi::math::Pose2d{0_m, 0_m, 45_deg}, wpi::math::Pose2d{3_m, 0_m, -90_deg},
wpi::math::Pose2d{0_m, 0_m, 135_deg},
wpi::math::Pose2d{-3_m, 0_m, -90_deg},
wpi::math::Pose2d{0_m, 0_m, 45_deg}},
wpi::math::TrajectoryConfig(2.0_mps, 2.0_mps_sq));
testFollowTrajectory(
kinematics, estimator, trajectory,
[&](frc::Trajectory::State& state) {
return frc::ChassisSpeeds{state.velocity, 0_mps,
[&](wpi::math::Trajectory::State& state) {
return wpi::math::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,
[&](wpi::math::Trajectory::State& state) { return state.pose; },
trajectory.InitialPose(), {0_m, 0_m, wpi::math::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}};
wpi::math::MecanumDriveKinematics kinematics{
wpi::math::Translation2d{1_m, 1_m}, wpi::math::Translation2d{1_m, -1_m},
wpi::math::Translation2d{-1_m, -1_m}, wpi::math::Translation2d{-1_m, 1_m}};
frc::MecanumDriveWheelPositions wheelPositions;
wpi::math::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}};
wpi::math::MecanumDrivePoseEstimator3d estimator{
kinematics, wpi::math::Rotation3d{}, wheelPositions,
wpi::math::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));
wpi::math::Trajectory trajectory = wpi::math::TrajectoryGenerator::GenerateTrajectory(
std::vector{wpi::math::Pose2d{0_m, 0_m, 45_deg}, wpi::math::Pose2d{3_m, 0_m, -90_deg},
wpi::math::Pose2d{0_m, 0_m, 135_deg},
wpi::math::Pose2d{-3_m, 0_m, -90_deg},
wpi::math::Pose2d{0_m, 0_m, 45_deg}},
wpi::math::TrajectoryConfig(2.0_mps, 2.0_mps_sq));
for (units::degree_t offset_direction_degs = 0_deg;
for (wpi::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;
for (wpi::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 pose_offset = wpi::math::Rotation2d{offset_direction_degs};
auto heading_offset = wpi::math::Rotation2d{offset_heading_degs};
auto initial_pose =
trajectory.InitialPose() +
frc::Transform2d{frc::Translation2d{pose_offset.Cos() * 1_m,
wpi::math::Transform2d{wpi::math::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,
[&](wpi::math::Trajectory::State& state) {
return wpi::math::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,
[&](wpi::math::Trajectory::State& state) { return state.pose; },
initial_pose, {0_m, 0_m, wpi::math::Rotation2d{45_deg}}, 20_ms, 100_ms,
250_ms, false, false);
}
}
@@ -223,36 +223,36 @@ TEST(MecanumDrivePoseEstimator3dTest, SimultaneousVisionMeasurements) {
// 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}};
wpi::math::MecanumDriveKinematics kinematics{
wpi::math::Translation2d{1_m, 1_m}, wpi::math::Translation2d{1_m, -1_m},
wpi::math::Translation2d{-1_m, -1_m}, wpi::math::Translation2d{-1_m, 1_m}};
frc::MecanumDriveWheelPositions wheelPositions;
wpi::math::MecanumDriveWheelPositions wheelPositions;
frc::MecanumDrivePoseEstimator3d estimator{
wpi::math::MecanumDrivePoseEstimator3d estimator{
kinematics,
frc::Rotation3d{},
wpi::math::Rotation3d{},
wheelPositions,
frc::Pose3d{1_m, 2_m, 0_m, frc::Rotation3d{0_deg, 0_deg, 270_deg}},
wpi::math::Pose3d{1_m, 2_m, 0_m, wpi::math::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);
estimator.UpdateWithTime(0_s, wpi::math::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);
wpi::math::Pose3d{0_m, 0_m, 0_m, wpi::math::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);
wpi::math::Pose3d{3_m, 1_m, 0_m, wpi::math::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}},
wpi::math::Pose3d{2_m, 4_m, 0_m, wpi::math::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(
auto dx = wpi::units::math::abs(estimator.GetEstimatedPosition().X() - 0_m);
auto dy = wpi::units::math::abs(estimator.GetEstimatedPosition().Y() - 0_m);
auto dtheta = wpi::units::math::abs(
estimator.GetEstimatedPosition().Rotation().ToRotation2d().Radians() -
0_deg);
@@ -260,9 +260,9 @@ TEST(MecanumDrivePoseEstimator3dTest, SimultaneousVisionMeasurements) {
}
{
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(
auto dx = wpi::units::math::abs(estimator.GetEstimatedPosition().X() - 3_m);
auto dy = wpi::units::math::abs(estimator.GetEstimatedPosition().Y() - 1_m);
auto dtheta = wpi::units::math::abs(
estimator.GetEstimatedPosition().Rotation().ToRotation2d().Radians() -
90_deg);
@@ -270,9 +270,9 @@ TEST(MecanumDrivePoseEstimator3dTest, SimultaneousVisionMeasurements) {
}
{
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(
auto dx = wpi::units::math::abs(estimator.GetEstimatedPosition().X() - 2_m);
auto dy = wpi::units::math::abs(estimator.GetEstimatedPosition().Y() - 4_m);
auto dtheta = wpi::units::math::abs(
estimator.GetEstimatedPosition().Rotation().ToRotation2d().Radians() -
180_deg);
@@ -281,25 +281,25 @@ TEST(MecanumDrivePoseEstimator3dTest, SimultaneousVisionMeasurements) {
}
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}};
wpi::math::MecanumDriveKinematics kinematics{
wpi::math::Translation2d{1_m, 1_m}, wpi::math::Translation2d{1_m, -1_m},
wpi::math::Translation2d{-1_m, -1_m}, wpi::math::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}};
wpi::math::MecanumDrivePoseEstimator3d estimator{
kinematics, wpi::math::Rotation3d{}, wpi::math::MecanumDriveWheelPositions{},
wpi::math::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{});
estimator.UpdateWithTime(time, wpi::math::Rotation3d{},
wpi::math::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,
wpi::math::Pose3d{10_m, 10_m, 0_m, wpi::math::Rotation3d{0_rad, 0_rad, 0.1_rad}}, 1_s,
{0.1, 0.1, 0.1, 0.1});
EXPECT_NEAR(odometryPose.X().value(),
@@ -317,12 +317,12 @@ TEST(MecanumDrivePoseEstimator3dTest, TestDiscardStaleVisionMeasurements) {
}
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}};
wpi::math::MecanumDriveKinematics kinematics{
wpi::math::Translation2d{1_m, 1_m}, wpi::math::Translation2d{1_m, -1_m},
wpi::math::Translation2d{-1_m, -1_m}, wpi::math::Translation2d{-1_m, 1_m}};
wpi::math::MecanumDrivePoseEstimator3d estimator{
kinematics, wpi::math::Rotation3d{}, wpi::math::MecanumDriveWheelPositions{},
wpi::math::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));
@@ -331,64 +331,64 @@ TEST(MecanumDrivePoseEstimator3dTest, TestSampleAt) {
// 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{},
wpi::math::MecanumDriveWheelPositions wheelPositions{
wpi::units::meter_t{time}, wpi::units::meter_t{time}, wpi::units::meter_t{time},
wpi::units::meter_t{time}};
estimator.UpdateWithTime(wpi::units::second_t{time}, wpi::math::Rotation3d{},
wheelPositions);
}
// Sample at an added time
EXPECT_EQ(std::optional(frc::Pose3d{1.02_m, 0_m, 0_m, frc::Rotation3d{}}),
EXPECT_EQ(std::optional(wpi::math::Pose3d{1.02_m, 0_m, 0_m, wpi::math::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{}}),
EXPECT_EQ(std::optional(wpi::math::Pose3d{1.01_m, 0_m, 0_m, wpi::math::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{}}),
EXPECT_EQ(std::optional(wpi::math::Pose3d{1_m, 0_m, 0_m, wpi::math::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{}}),
EXPECT_EQ(std::optional(wpi::math::Pose3d{2_m, 0_m, 0_m, wpi::math::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);
wpi::math::Pose3d{2_m, 0_m, 0_m, wpi::math::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{}}),
EXPECT_EQ(std::optional(wpi::math::Pose3d{1.02_m, 0_m, 0_m, wpi::math::Rotation3d{}}),
estimator.SampleAt(1.02_s));
EXPECT_EQ(std::optional(frc::Pose3d{1.01_m, 0_m, 0_m, frc::Rotation3d{}}),
EXPECT_EQ(std::optional(wpi::math::Pose3d{1.01_m, 0_m, 0_m, wpi::math::Rotation3d{}}),
estimator.SampleAt(1.01_s));
EXPECT_EQ(std::optional(frc::Pose3d{1_m, 0_m, 0_m, frc::Rotation3d{}}),
EXPECT_EQ(std::optional(wpi::math::Pose3d{1_m, 0_m, 0_m, wpi::math::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);
wpi::math::Pose3d{1_m, 0.2_m, 0_m, wpi::math::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{}}),
EXPECT_EQ(std::optional(wpi::math::Pose3d{1.02_m, 0.1_m, 0_m, wpi::math::Rotation3d{}}),
estimator.SampleAt(1.02_s));
EXPECT_EQ(std::optional(frc::Pose3d{1.01_m, 0.1_m, 0_m, frc::Rotation3d{}}),
EXPECT_EQ(std::optional(wpi::math::Pose3d{1.01_m, 0.1_m, 0_m, wpi::math::Rotation3d{}}),
estimator.SampleAt(1.01_s));
EXPECT_EQ(std::optional(frc::Pose3d{1_m, 0.1_m, 0_m, frc::Rotation3d{}}),
EXPECT_EQ(std::optional(wpi::math::Pose3d{1_m, 0.1_m, 0_m, wpi::math::Rotation3d{}}),
estimator.SampleAt(0.5_s));
EXPECT_EQ(std::optional(frc::Pose3d{2_m, 0.1_m, 0_m, frc::Rotation3d{}}),
EXPECT_EQ(std::optional(wpi::math::Pose3d{2_m, 0.1_m, 0_m, wpi::math::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{
wpi::math::MecanumDriveKinematics kinematics{
wpi::math::Translation2d{1_m, 1_m}, wpi::math::Translation2d{1_m, -1_m},
wpi::math::Translation2d{-1_m, -1_m}, wpi::math::Translation2d{-1_m, 1_m}};
wpi::math::MecanumDrivePoseEstimator3d estimator{
kinematics,
frc::Rotation3d{},
frc::MecanumDriveWheelPositions{},
frc::Pose3d{-1_m, -1_m, -1_m, frc::Rotation3d{0_rad, 0_rad, 1_rad}},
wpi::math::Rotation3d{},
wpi::math::MecanumDriveWheelPositions{},
wpi::math::Pose3d{-1_m, -1_m, -1_m, wpi::math::Rotation3d{0_rad, 0_rad, 1_rad}},
{1.0, 1.0, 1.0, 1.0},
{1.0, 1.0, 1.0, 1.0}};
@@ -401,8 +401,8 @@ TEST(MecanumDrivePoseEstimator3dTest, TestReset) {
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{}});
estimator.ResetPosition(wpi::math::Rotation3d{}, {1_m, 1_m, 1_m, 1_m},
wpi::math::Pose3d{1_m, 0_m, 0_m, wpi::math::Rotation3d{}});
EXPECT_DOUBLE_EQ(1, estimator.GetEstimatedPosition().X().value());
EXPECT_DOUBLE_EQ(0, estimator.GetEstimatedPosition().Y().value());
@@ -412,7 +412,7 @@ TEST(MecanumDrivePoseEstimator3dTest, TestReset) {
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});
estimator.Update(wpi::math::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());
@@ -422,7 +422,7 @@ TEST(MecanumDrivePoseEstimator3dTest, TestReset) {
EXPECT_DOUBLE_EQ(0, estimator.GetEstimatedPosition().Rotation().Z().value());
// Test reset rotation
estimator.ResetRotation(frc::Rotation3d{0_deg, 0_deg, 90_deg});
estimator.ResetRotation(wpi::math::Rotation3d{0_deg, 0_deg, 90_deg});
EXPECT_DOUBLE_EQ(2, estimator.GetEstimatedPosition().X().value());
EXPECT_DOUBLE_EQ(0, estimator.GetEstimatedPosition().Y().value());
@@ -433,7 +433,7 @@ TEST(MecanumDrivePoseEstimator3dTest, TestReset) {
estimator.GetEstimatedPosition().Rotation().Z().value());
// Test orientation
estimator.Update(frc::Rotation3d{}, {3_m, 3_m, 3_m, 3_m});
estimator.Update(wpi::math::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());
@@ -444,7 +444,7 @@ TEST(MecanumDrivePoseEstimator3dTest, TestReset) {
estimator.GetEstimatedPosition().Rotation().Z().value());
// Test reset translation
estimator.ResetTranslation(frc::Translation3d{-1_m, -1_m, -1_m});
estimator.ResetTranslation(wpi::math::Translation3d{-1_m, -1_m, -1_m});
EXPECT_DOUBLE_EQ(-1, estimator.GetEstimatedPosition().X().value());
EXPECT_DOUBLE_EQ(-1, estimator.GetEstimatedPosition().Y().value());
@@ -455,7 +455,7 @@ TEST(MecanumDrivePoseEstimator3dTest, TestReset) {
estimator.GetEstimatedPosition().Rotation().Z().value());
// Test reset pose
estimator.ResetPose(frc::Pose3d{});
estimator.ResetPose(wpi::math::Pose3d{});
EXPECT_DOUBLE_EQ(0, estimator.GetEstimatedPosition().X().value());
EXPECT_DOUBLE_EQ(0, estimator.GetEstimatedPosition().Y().value());

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

@@ -17,39 +17,39 @@
#include "wpi/util/print.hpp"
void testFollowTrajectory(
const frc::MecanumDriveKinematics& kinematics,
frc::MecanumDrivePoseEstimator& estimator,
const frc::Trajectory& trajectory,
std::function<frc::ChassisSpeeds(frc::Trajectory::State&)>
const wpi::math::MecanumDriveKinematics& kinematics,
wpi::math::MecanumDrivePoseEstimator& estimator,
const wpi::math::Trajectory& trajectory,
std::function<wpi::math::ChassisSpeeds(wpi::math::Trajectory::State&)>
chassisSpeedsGenerator,
std::function<frc::Pose2d(frc::Trajectory::State&)>
std::function<wpi::math::Pose2d(wpi::math::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 wpi::math::Pose2d& startingPose, const wpi::math::Pose2d& endingPose,
const wpi::units::second_t dt, const wpi::units::second_t kVisionUpdateRate,
const wpi::units::second_t kVisionUpdateDelay, const bool checkError,
const bool debug) {
frc::MecanumDriveWheelPositions wheelPositions{};
wpi::math::MecanumDriveWheelPositions wheelPositions{};
estimator.ResetPosition(frc::Rotation2d{}, wheelPositions, startingPose);
estimator.ResetPosition(wpi::math::Rotation2d{}, wheelPositions, startingPose);
std::default_random_engine generator;
std::normal_distribution<double> distribution(0.0, 1.0);
units::second_t t = 0_s;
wpi::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>>
std::vector<std::pair<wpi::units::second_t, wpi::math::Pose2d>> visionPoses;
std::vector<std::tuple<wpi::units::second_t, wpi::units::second_t, wpi::math::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");
wpi::util::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);
wpi::math::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
@@ -57,9 +57,9 @@ void testFollowTrajectory(
visionPoses.back().first + kVisionUpdateRate < t) {
auto visionPose =
visionMeasurementGenerator(groundTruthState) +
frc::Transform2d{frc::Translation2d{distribution(generator) * 0.1_m,
wpi::math::Transform2d{wpi::math::Translation2d{distribution(generator) * 0.1_m,
distribution(generator) * 0.1_m},
frc::Rotation2d{distribution(generator) * 0.05_rad}};
wpi::math::Rotation2d{distribution(generator) * 0.05_rad}};
visionPoses.push_back({t, visionPose});
}
@@ -85,12 +85,12 @@ void testFollowTrajectory(
auto xhat = estimator.UpdateWithTime(
t,
groundTruthState.pose.Rotation() +
frc::Rotation2d{distribution(generator) * 0.05_rad} -
wpi::math::Rotation2d{distribution(generator) * 0.05_rad} -
trajectory.InitialPose().Rotation(),
wheelPositions);
if (debug) {
wpi::print("{}, {}, {}, {}, {}, {}, {}\n", t.value(), xhat.X().value(),
wpi::util::print("{}, {}, {}, {}, {}, {}, {}\n", t.value(), xhat.X().value(),
xhat.Y().value(), xhat.Rotation().Radians().value(),
groundTruthState.pose.X().value(),
groundTruthState.pose.Y().value(),
@@ -110,14 +110,14 @@ void testFollowTrajectory(
}
if (debug) {
wpi::print("apply_time, measured_time, vision_x, vision_y, vision_theta\n");
wpi::util::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;
wpi::units::second_t apply_time;
wpi::units::second_t measure_time;
wpi::math::Pose2d vision_pose;
for (auto record : visionLog) {
std::tie(apply_time, measure_time, vision_pose) = record;
wpi::print("{}, {}, {}, {}, {}\n", apply_time.value(),
wpi::util::print("{}, {}, {}, {}, {}\n", apply_time.value(),
measure_time.value(), vision_pose.X().value(),
vision_pose.Y().value(),
vision_pose.Rotation().Radians().value());
@@ -140,73 +140,73 @@ void testFollowTrajectory(
}
TEST(MecanumDrivePoseEstimatorTest, AccuracyFacingTrajectory) {
frc::MecanumDriveKinematics kinematics{
frc::Translation2d{1_m, 1_m}, frc::Translation2d{1_m, -1_m},
frc::Translation2d{-1_m, -1_m}, frc::Translation2d{-1_m, 1_m}};
wpi::math::MecanumDriveKinematics kinematics{
wpi::math::Translation2d{1_m, 1_m}, wpi::math::Translation2d{1_m, -1_m},
wpi::math::Translation2d{-1_m, -1_m}, wpi::math::Translation2d{-1_m, 1_m}};
frc::MecanumDriveWheelPositions wheelPositions;
wpi::math::MecanumDriveWheelPositions wheelPositions;
frc::MecanumDrivePoseEstimator estimator{kinematics, frc::Rotation2d{},
wheelPositions, frc::Pose2d{},
wpi::math::MecanumDrivePoseEstimator estimator{kinematics, wpi::math::Rotation2d{},
wheelPositions, wpi::math::Pose2d{},
{0.1, 0.1, 0.1}, {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));
wpi::math::Trajectory trajectory = wpi::math::TrajectoryGenerator::GenerateTrajectory(
std::vector{wpi::math::Pose2d{0_m, 0_m, 45_deg}, wpi::math::Pose2d{3_m, 0_m, -90_deg},
wpi::math::Pose2d{0_m, 0_m, 135_deg},
wpi::math::Pose2d{-3_m, 0_m, -90_deg},
wpi::math::Pose2d{0_m, 0_m, 45_deg}},
wpi::math::TrajectoryConfig(2.0_mps, 2.0_mps_sq));
testFollowTrajectory(
kinematics, estimator, trajectory,
[&](frc::Trajectory::State& state) {
return frc::ChassisSpeeds{state.velocity, 0_mps,
[&](wpi::math::Trajectory::State& state) {
return wpi::math::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,
[&](wpi::math::Trajectory::State& state) { return state.pose; },
trajectory.InitialPose(), {0_m, 0_m, wpi::math::Rotation2d{45_deg}}, 20_ms,
100_ms, 250_ms, true, false);
}
TEST(MecanumDrivePoseEstimatorTest, 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}};
wpi::math::MecanumDriveKinematics kinematics{
wpi::math::Translation2d{1_m, 1_m}, wpi::math::Translation2d{1_m, -1_m},
wpi::math::Translation2d{-1_m, -1_m}, wpi::math::Translation2d{-1_m, 1_m}};
frc::MecanumDriveWheelPositions wheelPositions;
wpi::math::MecanumDriveWheelPositions wheelPositions;
frc::MecanumDrivePoseEstimator estimator{kinematics, frc::Rotation2d{},
wheelPositions, frc::Pose2d{},
wpi::math::MecanumDrivePoseEstimator estimator{kinematics, wpi::math::Rotation2d{},
wheelPositions, wpi::math::Pose2d{},
{0.1, 0.1, 0.1}, {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));
wpi::math::Trajectory trajectory = wpi::math::TrajectoryGenerator::GenerateTrajectory(
std::vector{wpi::math::Pose2d{0_m, 0_m, 45_deg}, wpi::math::Pose2d{3_m, 0_m, -90_deg},
wpi::math::Pose2d{0_m, 0_m, 135_deg},
wpi::math::Pose2d{-3_m, 0_m, -90_deg},
wpi::math::Pose2d{0_m, 0_m, 45_deg}},
wpi::math::TrajectoryConfig(2.0_mps, 2.0_mps_sq));
for (units::degree_t offset_direction_degs = 0_deg;
for (wpi::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;
for (wpi::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 pose_offset = wpi::math::Rotation2d{offset_direction_degs};
auto heading_offset = wpi::math::Rotation2d{offset_heading_degs};
auto initial_pose =
trajectory.InitialPose() +
frc::Transform2d{frc::Translation2d{pose_offset.Cos() * 1_m,
wpi::math::Transform2d{wpi::math::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,
[&](wpi::math::Trajectory::State& state) {
return wpi::math::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,
[&](wpi::math::Trajectory::State& state) { return state.pose; },
initial_pose, {0_m, 0_m, wpi::math::Rotation2d{45_deg}}, 20_ms, 100_ms,
250_ms, false, false);
}
}
@@ -218,50 +218,50 @@ TEST(MecanumDrivePoseEstimatorTest, SimultaneousVisionMeasurements) {
// 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}};
wpi::math::MecanumDriveKinematics kinematics{
wpi::math::Translation2d{1_m, 1_m}, wpi::math::Translation2d{1_m, -1_m},
wpi::math::Translation2d{-1_m, -1_m}, wpi::math::Translation2d{-1_m, 1_m}};
frc::MecanumDriveWheelPositions wheelPositions;
wpi::math::MecanumDriveWheelPositions wheelPositions;
frc::MecanumDrivePoseEstimator estimator{
kinematics, frc::Rotation2d{},
wheelPositions, frc::Pose2d{1_m, 2_m, frc::Rotation2d{270_deg}},
wpi::math::MecanumDrivePoseEstimator estimator{
kinematics, wpi::math::Rotation2d{},
wheelPositions, wpi::math::Pose2d{1_m, 2_m, wpi::math::Rotation2d{270_deg}},
{0.1, 0.1, 0.1}, {0.45, 0.45, 0.1}};
estimator.UpdateWithTime(0_s, frc::Rotation2d{}, wheelPositions);
estimator.UpdateWithTime(0_s, wpi::math::Rotation2d{}, wheelPositions);
for (int i = 0; i < 1000; i++) {
estimator.AddVisionMeasurement(
frc::Pose2d{0_m, 0_m, frc::Rotation2d{0_deg}}, 0_s);
wpi::math::Pose2d{0_m, 0_m, wpi::math::Rotation2d{0_deg}}, 0_s);
estimator.AddVisionMeasurement(
frc::Pose2d{3_m, 1_m, frc::Rotation2d{90_deg}}, 0_s);
wpi::math::Pose2d{3_m, 1_m, wpi::math::Rotation2d{90_deg}}, 0_s);
estimator.AddVisionMeasurement(
frc::Pose2d{2_m, 4_m, frc::Rotation2d{180_deg}}, 0_s);
wpi::math::Pose2d{2_m, 4_m, wpi::math::Rotation2d{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(
auto dx = wpi::units::math::abs(estimator.GetEstimatedPosition().X() - 0_m);
auto dy = wpi::units::math::abs(estimator.GetEstimatedPosition().Y() - 0_m);
auto dtheta = wpi::units::math::abs(
estimator.GetEstimatedPosition().Rotation().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(
auto dx = wpi::units::math::abs(estimator.GetEstimatedPosition().X() - 3_m);
auto dy = wpi::units::math::abs(estimator.GetEstimatedPosition().Y() - 1_m);
auto dtheta = wpi::units::math::abs(
estimator.GetEstimatedPosition().Rotation().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(
auto dx = wpi::units::math::abs(estimator.GetEstimatedPosition().X() - 2_m);
auto dy = wpi::units::math::abs(estimator.GetEstimatedPosition().Y() - 4_m);
auto dtheta = wpi::units::math::abs(
estimator.GetEstimatedPosition().Rotation().Radians() - 180_deg);
EXPECT_TRUE(dx > 0.08_m || dy > 0.08_m || dtheta > 0.08_rad);
@@ -269,25 +269,25 @@ TEST(MecanumDrivePoseEstimatorTest, SimultaneousVisionMeasurements) {
}
TEST(MecanumDrivePoseEstimatorTest, 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}};
wpi::math::MecanumDriveKinematics kinematics{
wpi::math::Translation2d{1_m, 1_m}, wpi::math::Translation2d{1_m, -1_m},
wpi::math::Translation2d{-1_m, -1_m}, wpi::math::Translation2d{-1_m, 1_m}};
frc::MecanumDrivePoseEstimator estimator{
kinematics, frc::Rotation2d{}, frc::MecanumDriveWheelPositions{},
frc::Pose2d{}, {0.1, 0.1, 0.1}, {0.45, 0.45, 0.45}};
wpi::math::MecanumDrivePoseEstimator estimator{
kinematics, wpi::math::Rotation2d{}, wpi::math::MecanumDriveWheelPositions{},
wpi::math::Pose2d{}, {0.1, 0.1, 0.1}, {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::Rotation2d{},
frc::MecanumDriveWheelPositions{});
estimator.UpdateWithTime(time, wpi::math::Rotation2d{},
wpi::math::MecanumDriveWheelPositions{});
}
auto odometryPose = estimator.GetEstimatedPosition();
// Apply a vision measurement from 3 seconds ago
estimator.AddVisionMeasurement(
frc::Pose2d{frc::Translation2d{10_m, 10_m}, frc::Rotation2d{0.1_rad}},
wpi::math::Pose2d{wpi::math::Translation2d{10_m, 10_m}, wpi::math::Rotation2d{0.1_rad}},
1_s, {0.1, 0.1, 0.1});
EXPECT_NEAR(odometryPose.X().value(),
@@ -300,12 +300,12 @@ TEST(MecanumDrivePoseEstimatorTest, TestDiscardStaleVisionMeasurements) {
}
TEST(MecanumDrivePoseEstimatorTest, 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::MecanumDrivePoseEstimator estimator{
kinematics, frc::Rotation2d{}, frc::MecanumDriveWheelPositions{},
frc::Pose2d{}, {1.0, 1.0, 1.0}, {1.0, 1.0, 1.0}};
wpi::math::MecanumDriveKinematics kinematics{
wpi::math::Translation2d{1_m, 1_m}, wpi::math::Translation2d{1_m, -1_m},
wpi::math::Translation2d{-1_m, -1_m}, wpi::math::Translation2d{-1_m, 1_m}};
wpi::math::MecanumDrivePoseEstimator estimator{
kinematics, wpi::math::Rotation2d{}, wpi::math::MecanumDriveWheelPositions{},
wpi::math::Pose2d{}, {1.0, 1.0, 1.0}, {1.0, 1.0, 1.0}};
// Returns empty when null
EXPECT_EQ(std::nullopt, estimator.SampleAt(1_s));
@@ -314,64 +314,64 @@ TEST(MecanumDrivePoseEstimatorTest, TestSampleAt) {
// 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::Rotation2d{},
wpi::math::MecanumDriveWheelPositions wheelPositions{
wpi::units::meter_t{time}, wpi::units::meter_t{time}, wpi::units::meter_t{time},
wpi::units::meter_t{time}};
estimator.UpdateWithTime(wpi::units::second_t{time}, wpi::math::Rotation2d{},
wheelPositions);
}
// Sample at an added time
EXPECT_EQ(std::optional(frc::Pose2d{1.02_m, 0_m, frc::Rotation2d{}}),
EXPECT_EQ(std::optional(wpi::math::Pose2d{1.02_m, 0_m, wpi::math::Rotation2d{}}),
estimator.SampleAt(1.02_s));
// Sample between updates (test interpolation)
EXPECT_EQ(std::optional(frc::Pose2d{1.01_m, 0_m, frc::Rotation2d{}}),
EXPECT_EQ(std::optional(wpi::math::Pose2d{1.01_m, 0_m, wpi::math::Rotation2d{}}),
estimator.SampleAt(1.01_s));
// Sampling before the oldest value returns the oldest value
EXPECT_EQ(std::optional(frc::Pose2d{1_m, 0_m, frc::Rotation2d{}}),
EXPECT_EQ(std::optional(wpi::math::Pose2d{1_m, 0_m, wpi::math::Rotation2d{}}),
estimator.SampleAt(0.5_s));
// Sampling after the newest value returns the newest value
EXPECT_EQ(std::optional(frc::Pose2d{2_m, 0_m, frc::Rotation2d{}}),
EXPECT_EQ(std::optional(wpi::math::Pose2d{2_m, 0_m, wpi::math::Rotation2d{}}),
estimator.SampleAt(2.5_s));
// Add a vision measurement after the odometry measurements (while keeping all
// of the old odometry measurements)
estimator.AddVisionMeasurement(frc::Pose2d{2_m, 0_m, frc::Rotation2d{1_rad}},
estimator.AddVisionMeasurement(wpi::math::Pose2d{2_m, 0_m, wpi::math::Rotation2d{1_rad}},
2.2_s);
// Make sure nothing changed (except the newest value)
EXPECT_EQ(std::optional(frc::Pose2d{1.02_m, 0_m, frc::Rotation2d{}}),
EXPECT_EQ(std::optional(wpi::math::Pose2d{1.02_m, 0_m, wpi::math::Rotation2d{}}),
estimator.SampleAt(1.02_s));
EXPECT_EQ(std::optional(frc::Pose2d{1.01_m, 0_m, frc::Rotation2d{}}),
EXPECT_EQ(std::optional(wpi::math::Pose2d{1.01_m, 0_m, wpi::math::Rotation2d{}}),
estimator.SampleAt(1.01_s));
EXPECT_EQ(std::optional(frc::Pose2d{1_m, 0_m, frc::Rotation2d{}}),
EXPECT_EQ(std::optional(wpi::math::Pose2d{1_m, 0_m, wpi::math::Rotation2d{}}),
estimator.SampleAt(0.5_s));
// Add a vision measurement before the odometry measurements that's still in
// the buffer
estimator.AddVisionMeasurement(frc::Pose2d{1_m, 0.2_m, frc::Rotation2d{}},
estimator.AddVisionMeasurement(wpi::math::Pose2d{1_m, 0.2_m, wpi::math::Rotation2d{}},
0.9_s);
// Everything should be the same except Y is 0.1 (halfway between 0 and 0.2)
EXPECT_EQ(std::optional(frc::Pose2d{1.02_m, 0.1_m, frc::Rotation2d{}}),
EXPECT_EQ(std::optional(wpi::math::Pose2d{1.02_m, 0.1_m, wpi::math::Rotation2d{}}),
estimator.SampleAt(1.02_s));
EXPECT_EQ(std::optional(frc::Pose2d{1.01_m, 0.1_m, frc::Rotation2d{}}),
EXPECT_EQ(std::optional(wpi::math::Pose2d{1.01_m, 0.1_m, wpi::math::Rotation2d{}}),
estimator.SampleAt(1.01_s));
EXPECT_EQ(std::optional(frc::Pose2d{1_m, 0.1_m, frc::Rotation2d{}}),
EXPECT_EQ(std::optional(wpi::math::Pose2d{1_m, 0.1_m, wpi::math::Rotation2d{}}),
estimator.SampleAt(0.5_s));
EXPECT_EQ(std::optional(frc::Pose2d{2_m, 0.1_m, frc::Rotation2d{}}),
EXPECT_EQ(std::optional(wpi::math::Pose2d{2_m, 0.1_m, wpi::math::Rotation2d{}}),
estimator.SampleAt(2.5_s));
}
TEST(MecanumDrivePoseEstimatorTest, 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::MecanumDrivePoseEstimator estimator{
wpi::math::MecanumDriveKinematics kinematics{
wpi::math::Translation2d{1_m, 1_m}, wpi::math::Translation2d{1_m, -1_m},
wpi::math::Translation2d{-1_m, -1_m}, wpi::math::Translation2d{-1_m, 1_m}};
wpi::math::MecanumDrivePoseEstimator estimator{
kinematics,
frc::Rotation2d{},
frc::MecanumDriveWheelPositions{},
frc::Pose2d{-1_m, -1_m, frc::Rotation2d{1_rad}},
wpi::math::Rotation2d{},
wpi::math::MecanumDriveWheelPositions{},
wpi::math::Pose2d{-1_m, -1_m, wpi::math::Rotation2d{1_rad}},
{1.0, 1.0, 1.0},
{1.0, 1.0, 1.0}};
@@ -382,8 +382,8 @@ TEST(MecanumDrivePoseEstimatorTest, TestReset) {
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{}});
estimator.ResetPosition(wpi::math::Rotation2d{}, {1_m, 1_m, 1_m, 1_m},
wpi::math::Pose2d{1_m, 0_m, wpi::math::Rotation2d{}});
EXPECT_DOUBLE_EQ(1, estimator.GetEstimatedPosition().X().value());
EXPECT_DOUBLE_EQ(0, estimator.GetEstimatedPosition().Y().value());
@@ -391,7 +391,7 @@ TEST(MecanumDrivePoseEstimatorTest, TestReset) {
0, estimator.GetEstimatedPosition().Rotation().Radians().value());
// Test orientation and wheel positions
estimator.Update(frc::Rotation2d{}, {2_m, 2_m, 2_m, 2_m});
estimator.Update(wpi::math::Rotation2d{}, {2_m, 2_m, 2_m, 2_m});
EXPECT_DOUBLE_EQ(2, estimator.GetEstimatedPosition().X().value());
EXPECT_DOUBLE_EQ(0, estimator.GetEstimatedPosition().Y().value());
@@ -399,7 +399,7 @@ TEST(MecanumDrivePoseEstimatorTest, TestReset) {
0, estimator.GetEstimatedPosition().Rotation().Radians().value());
// Test reset rotation
estimator.ResetRotation(frc::Rotation2d{90_deg});
estimator.ResetRotation(wpi::math::Rotation2d{90_deg});
EXPECT_DOUBLE_EQ(2, estimator.GetEstimatedPosition().X().value());
EXPECT_DOUBLE_EQ(0, estimator.GetEstimatedPosition().Y().value());
@@ -408,7 +408,7 @@ TEST(MecanumDrivePoseEstimatorTest, TestReset) {
estimator.GetEstimatedPosition().Rotation().Radians().value());
// Test orientation
estimator.Update(frc::Rotation2d{}, {3_m, 3_m, 3_m, 3_m});
estimator.Update(wpi::math::Rotation2d{}, {3_m, 3_m, 3_m, 3_m});
EXPECT_DOUBLE_EQ(2, estimator.GetEstimatedPosition().X().value());
EXPECT_DOUBLE_EQ(1, estimator.GetEstimatedPosition().Y().value());
@@ -417,7 +417,7 @@ TEST(MecanumDrivePoseEstimatorTest, TestReset) {
estimator.GetEstimatedPosition().Rotation().Radians().value());
// Test reset translation
estimator.ResetTranslation(frc::Translation2d{-1_m, -1_m});
estimator.ResetTranslation(wpi::math::Translation2d{-1_m, -1_m});
EXPECT_DOUBLE_EQ(-1, estimator.GetEstimatedPosition().X().value());
EXPECT_DOUBLE_EQ(-1, estimator.GetEstimatedPosition().Y().value());
@@ -426,7 +426,7 @@ TEST(MecanumDrivePoseEstimatorTest, TestReset) {
estimator.GetEstimatedPosition().Rotation().Radians().value());
// Test reset pose
estimator.ResetPose(frc::Pose2d{});
estimator.ResetPose(wpi::math::Pose2d{});
EXPECT_DOUBLE_EQ(0, estimator.GetEstimatedPosition().X().value());
EXPECT_DOUBLE_EQ(0, estimator.GetEstimatedPosition().Y().value());

14
wpimath/src/test/native/cpp/estimator/MerweScaledSigmaPointsTest.cpp
View File

@@ -7,24 +7,24 @@
#include "wpi/math/estimator/MerweScaledSigmaPoints.hpp"
TEST(MerweScaledSigmaPointsTest, ZeroMean) {
frc::MerweScaledSigmaPoints<2> sigmaPoints;
wpi::math::MerweScaledSigmaPoints<2> sigmaPoints;
auto points = sigmaPoints.SquareRootSigmaPoints(
frc::Vectord<2>{0.0, 0.0}, frc::Matrixd<2, 2>{{1.0, 0.0}, {0.0, 1.0}});
wpi::math::Vectord<2>{0.0, 0.0}, wpi::math::Matrixd<2, 2>{{1.0, 0.0}, {0.0, 1.0}});
EXPECT_TRUE(
(points - frc::Matrixd<2, 5>{{0.0, 0.00173205, 0.0, -0.00173205, 0.0},
(points - wpi::math::Matrixd<2, 5>{{0.0, 0.00173205, 0.0, -0.00173205, 0.0},
{0.0, 0.0, 0.00173205, 0.0, -0.00173205}})
.norm() < 1e-3);
}
TEST(MerweScaledSigmaPointsTest, NonzeroMean) {
frc::MerweScaledSigmaPoints<2> sigmaPoints;
wpi::math::MerweScaledSigmaPoints<2> sigmaPoints;
auto points = sigmaPoints.SquareRootSigmaPoints(
frc::Vectord<2>{1.0, 2.0},
frc::Matrixd<2, 2>{{1.0, 0.0}, {0.0, std::sqrt(10.0)}});
wpi::math::Vectord<2>{1.0, 2.0},
wpi::math::Matrixd<2, 2>{{1.0, 0.0}, {0.0, std::sqrt(10.0)}});
EXPECT_TRUE(
(points - frc::Matrixd<2, 5>{{1.0, 1.00173205, 1.0, 0.998268, 1.0},
(points - wpi::math::Matrixd<2, 5>{{1.0, 1.00173205, 1.0, 0.998268, 1.0},
{2.0, 2.0, 2.00548, 2.0, 1.99452}})
.norm() < 1e-3);
}

186
wpimath/src/test/native/cpp/estimator/MerweUKFTest.cpp
View File

@@ -25,9 +25,9 @@
namespace {
// First test system, differential drive
frc::Vectord<5> DriveDynamics(const frc::Vectord<5>& x,
const frc::Vectord<2>& u) {
auto motors = frc::DCMotor::CIM(2);
wpi::math::Vectord<5> DriveDynamics(const wpi::math::Vectord<5>& x,
const wpi::math::Vectord<2>& u) {
auto motors = wpi::math::DCMotor::CIM(2);
// constexpr double Glow = 15.32; // Low gear ratio
constexpr double Ghigh = 7.08; // High gear ratio
@@ -37,18 +37,18 @@ frc::Vectord<5> DriveDynamics(const frc::Vectord<5>& x,
constexpr auto J = 5.6_kg_sq_m; // Robot moment of inertia
auto C1 = -std::pow(Ghigh, 2) * motors.Kt /
(motors.Kv * motors.R * units::math::pow<2>(r));
(motors.Kv * motors.R * wpi::units::math::pow<2>(r));
auto C2 = Ghigh * motors.Kt / (motors.R * r);
auto k1 = (1 / m + units::math::pow<2>(rb) / J);
auto k2 = (1 / m - units::math::pow<2>(rb) / J);
auto k1 = (1 / m + wpi::units::math::pow<2>(rb) / J);
auto k2 = (1 / m - wpi::units::math::pow<2>(rb) / J);
units::meters_per_second_t vl{x(3)};
units::meters_per_second_t vr{x(4)};
units::volt_t Vl{u(0)};
units::volt_t Vr{u(1)};
wpi::units::meters_per_second_t vl{x(3)};
wpi::units::meters_per_second_t vr{x(4)};
wpi::units::volt_t Vl{u(0)};
wpi::units::volt_t Vr{u(1)};
auto v = 0.5 * (vl + vr);
return frc::Vectord<5>{
return wpi::math::Vectord<5>{
v.value() * std::cos(x(2)), v.value() * std::sin(x(2)),
((vr - vl) / (2.0 * rb)).value(),
k1.value() * ((C1 * vl).value() + (C2 * Vl).value()) +
@@ -57,70 +57,70 @@ frc::Vectord<5> DriveDynamics(const frc::Vectord<5>& x,
k1.value() * ((C1 * vr).value() + (C2 * Vr).value())};
}
frc::Vectord<3> DriveLocalMeasurementModel(
const frc::Vectord<5>& x, [[maybe_unused]] const frc::Vectord<2>& u) {
return frc::Vectord<3>{x(2), x(3), x(4)};
wpi::math::Vectord<3> DriveLocalMeasurementModel(
const wpi::math::Vectord<5>& x, [[maybe_unused]] const wpi::math::Vectord<2>& u) {
return wpi::math::Vectord<3>{x(2), x(3), x(4)};
}
frc::Vectord<5> DriveGlobalMeasurementModel(
const frc::Vectord<5>& x, [[maybe_unused]] const frc::Vectord<2>& u) {
return frc::Vectord<5>{x(0), x(1), x(2), x(3), x(4)};
wpi::math::Vectord<5> DriveGlobalMeasurementModel(
const wpi::math::Vectord<5>& x, [[maybe_unused]] const wpi::math::Vectord<2>& u) {
return wpi::math::Vectord<5>{x(0), x(1), x(2), x(3), x(4)};
}
TEST(MerweUKFTest, DriveInit) {
constexpr units::second_t dt = 5_ms;
constexpr wpi::units::second_t dt = 5_ms;
frc::MerweUKF<5, 2, 3> observer{DriveDynamics,
wpi::math::MerweUKF<5, 2, 3> observer{DriveDynamics,
DriveLocalMeasurementModel,
{0.5, 0.5, 10.0, 1.0, 1.0},
{0.0001, 0.01, 0.01},
frc::AngleMean<5, 2 * 5 + 1>(2),
frc::AngleMean<3, 2 * 5 + 1>(0),
frc::AngleResidual<5>(2),
frc::AngleResidual<3>(0),
frc::AngleAdd<5>(2),
wpi::math::AngleMean<5, 2 * 5 + 1>(2),
wpi::math::AngleMean<3, 2 * 5 + 1>(0),
wpi::math::AngleResidual<5>(2),
wpi::math::AngleResidual<3>(0),
wpi::math::AngleAdd<5>(2),
dt};
frc::Vectord<2> u{12.0, 12.0};
wpi::math::Vectord<2> u{12.0, 12.0};
observer.Predict(u, dt);
auto localY = DriveLocalMeasurementModel(observer.Xhat(), u);
observer.Correct(u, localY);
auto globalY = DriveGlobalMeasurementModel(observer.Xhat(), u);
auto R = frc::MakeCovMatrix(0.01, 0.01, 0.0001, 0.01, 0.01);
auto R = wpi::math::MakeCovMatrix(0.01, 0.01, 0.0001, 0.01, 0.01);
observer.Correct<5>(u, globalY, DriveGlobalMeasurementModel, R,
frc::AngleMean<5, 2 * 5 + 1>(2), frc::AngleResidual<5>(2),
frc::AngleResidual<5>(2), frc::AngleAdd<5>(2));
wpi::math::AngleMean<5, 2 * 5 + 1>(2), wpi::math::AngleResidual<5>(2),
wpi::math::AngleResidual<5>(2), wpi::math::AngleAdd<5>(2));
}
TEST(MerweUKFTest, DriveConvergence) {
constexpr units::second_t dt = 5_ms;
constexpr wpi::units::second_t dt = 5_ms;
constexpr auto rb = 0.8382_m / 2.0; // Robot radius
frc::MerweUKF<5, 2, 3> observer{DriveDynamics,
wpi::math::MerweUKF<5, 2, 3> observer{DriveDynamics,
DriveLocalMeasurementModel,
{0.5, 0.5, 10.0, 1.0, 1.0},
{0.0001, 0.5, 0.5},
frc::AngleMean<5, 2 * 5 + 1>(2),
frc::AngleMean<3, 2 * 5 + 1>(0),
frc::AngleResidual<5>(2),
frc::AngleResidual<3>(0),
frc::AngleAdd<5>(2),
wpi::math::AngleMean<5, 2 * 5 + 1>(2),
wpi::math::AngleMean<3, 2 * 5 + 1>(0),
wpi::math::AngleResidual<5>(2),
wpi::math::AngleResidual<3>(0),
wpi::math::AngleAdd<5>(2),
dt};
auto waypoints =
std::vector<frc::Pose2d>{frc::Pose2d{2.75_m, 22.521_m, 0_rad},
frc::Pose2d{24.73_m, 19.68_m, 5.846_rad}};
auto trajectory = frc::TrajectoryGenerator::GenerateTrajectory(
std::vector<wpi::math::Pose2d>{wpi::math::Pose2d{2.75_m, 22.521_m, 0_rad},
wpi::math::Pose2d{24.73_m, 19.68_m, 5.846_rad}};
auto trajectory = wpi::math::TrajectoryGenerator::GenerateTrajectory(
waypoints, {8.8_mps, 0.1_mps_sq});
frc::Vectord<5> r = frc::Vectord<5>::Zero();
frc::Vectord<2> u = frc::Vectord<2>::Zero();
wpi::math::Vectord<5> r = wpi::math::Vectord<5>::Zero();
wpi::math::Vectord<2> u = wpi::math::Vectord<2>::Zero();
auto B = frc::NumericalJacobianU<5, 5, 2>(
DriveDynamics, frc::Vectord<5>::Zero(), frc::Vectord<2>::Zero());
auto B = wpi::math::NumericalJacobianU<5, 5, 2>(
DriveDynamics, wpi::math::Vectord<5>::Zero(), wpi::math::Vectord<2>::Zero());
observer.SetXhat(frc::Vectord<5>{
observer.SetXhat(wpi::math::Vectord<5>{
trajectory.InitialPose().Translation().X().value(),
trajectory.InitialPose().Translation().Y().value(),
trajectory.InitialPose().Rotation().Radians().value(), 0.0, 0.0});
@@ -130,36 +130,36 @@ TEST(MerweUKFTest, DriveConvergence) {
auto totalTime = trajectory.TotalTime();
for (size_t i = 0; i < (totalTime / dt).value(); ++i) {
auto ref = trajectory.Sample(dt * i);
units::meters_per_second_t vl =
wpi::units::meters_per_second_t vl =
ref.velocity * (1 - (ref.curvature * rb).value());
units::meters_per_second_t vr =
wpi::units::meters_per_second_t vr =
ref.velocity * (1 + (ref.curvature * rb).value());
frc::Vectord<5> nextR{
wpi::math::Vectord<5> nextR{
ref.pose.Translation().X().value(), ref.pose.Translation().Y().value(),
ref.pose.Rotation().Radians().value(), vl.value(), vr.value()};
auto localY = DriveLocalMeasurementModel(trueXhat, frc::Vectord<2>::Zero());
observer.Correct(u, localY + frc::MakeWhiteNoiseVector(0.0001, 0.5, 0.5));
auto localY = DriveLocalMeasurementModel(trueXhat, wpi::math::Vectord<2>::Zero());
observer.Correct(u, localY + wpi::math::MakeWhiteNoiseVector(0.0001, 0.5, 0.5));
frc::Vectord<5> rdot = (nextR - r) / dt.value();
wpi::math::Vectord<5> rdot = (nextR - r) / dt.value();
u = B.householderQr().solve(rdot -
DriveDynamics(r, frc::Vectord<2>::Zero()));
DriveDynamics(r, wpi::math::Vectord<2>::Zero()));
observer.Predict(u, dt);
r = nextR;
trueXhat = frc::RK4(DriveDynamics, trueXhat, u, dt);
trueXhat = wpi::math::RK4(DriveDynamics, trueXhat, u, dt);
}
auto localY = DriveLocalMeasurementModel(trueXhat, u);
observer.Correct(u, localY);
auto globalY = DriveGlobalMeasurementModel(trueXhat, u);
auto R = frc::MakeCovMatrix(0.01, 0.01, 0.0001, 0.5, 0.5);
auto R = wpi::math::MakeCovMatrix(0.01, 0.01, 0.0001, 0.5, 0.5);
observer.Correct<5>(u, globalY, DriveGlobalMeasurementModel, R,
frc::AngleMean<5, 2 * 5 + 1>(2), frc::AngleResidual<5>(2),
frc::AngleResidual<5>(2), frc::AngleAdd<5>(2)
wpi::math::AngleMean<5, 2 * 5 + 1>(2), wpi::math::AngleResidual<5>(2),
wpi::math::AngleResidual<5>(2), wpi::math::AngleAdd<5>(2)
);
@@ -175,26 +175,26 @@ TEST(MerweUKFTest, DriveConvergence) {
}
TEST(MerweUKFTest, LinearUKF) {
constexpr units::second_t dt = 20_ms;
auto plant = frc::LinearSystemId::IdentifyVelocitySystem<units::meters>(
constexpr wpi::units::second_t dt = 20_ms;
auto plant = wpi::math::LinearSystemId::IdentifyVelocitySystem<wpi::units::meters>(
0.02_V / 1_mps, 0.006_V / 1_mps_sq);
frc::MerweUKF<1, 1, 1> observer{
[&](const frc::Vectord<1>& x, const frc::Vectord<1>& u) {
wpi::math::MerweUKF<1, 1, 1> observer{
[&](const wpi::math::Vectord<1>& x, const wpi::math::Vectord<1>& u) {
return plant.A() * x + plant.B() * u;
},
[&](const frc::Vectord<1>& x, const frc::Vectord<1>& u) {
[&](const wpi::math::Vectord<1>& x, const wpi::math::Vectord<1>& u) {
return plant.CalculateY(x, u);
},
{0.05},
{1.0},
dt};
frc::Matrixd<1, 1> discA;
frc::Matrixd<1, 1> discB;
frc::DiscretizeAB<1, 1>(plant.A(), plant.B(), dt, &discA, &discB);
wpi::math::Matrixd<1, 1> discA;
wpi::math::Matrixd<1, 1> discB;
wpi::math::DiscretizeAB<1, 1>(plant.A(), plant.B(), dt, &discA, &discB);
frc::Vectord<1> ref{100.0};
frc::Vectord<1> u{0.0};
wpi::math::Vectord<1> ref{100.0};
wpi::math::Vectord<1> u{0.0};
for (int i = 0; i < 2.0 / dt.value(); ++i) {
observer.Predict(u, dt);
@@ -206,24 +206,24 @@ TEST(MerweUKFTest, LinearUKF) {
}
TEST(MerweUKFTest, RoundTripP) {
constexpr units::second_t dt = 5_ms;
constexpr wpi::units::second_t dt = 5_ms;
frc::MerweUKF<2, 2, 2> observer{
[](const frc::Vectord<2>& x, const frc::Vectord<2>& u) { return x; },
[](const frc::Vectord<2>& x, const frc::Vectord<2>& u) { return x; },
wpi::math::MerweUKF<2, 2, 2> observer{
[](const wpi::math::Vectord<2>& x, const wpi::math::Vectord<2>& u) { return x; },
[](const wpi::math::Vectord<2>& x, const wpi::math::Vectord<2>& u) { return x; },
{0.0, 0.0},
{0.0, 0.0},
dt};
frc::Matrixd<2, 2> P({{2, 1}, {1, 2}});
wpi::math::Matrixd<2, 2> P({{2, 1}, {1, 2}});
observer.SetP(P);
ASSERT_TRUE(observer.P().isApprox(P));
}
// Second system, single motor feedforward estimator
frc::Vectord<4> MotorDynamics(const frc::Vectord<4>& x,
const frc::Vectord<1>& u) {
wpi::math::Vectord<4> MotorDynamics(const wpi::math::Vectord<4>& x,
const wpi::math::Vectord<1>& u) {
double v = x(1);
double kV = x(2);
double kA = x(3);
@@ -231,11 +231,11 @@ frc::Vectord<4> MotorDynamics(const frc::Vectord<4>& x,
double V = u(0);
double a = -kV / kA * v + 1.0 / kA * V;
return frc::Vectord<4>{v, a, 0.0, 0.0};
return wpi::math::Vectord<4>{v, a, 0.0, 0.0};
}
frc::Vectord<3> MotorMeasurementModel(const frc::Vectord<4>& x,
const frc::Vectord<1>& u) {
wpi::math::Vectord<3> MotorMeasurementModel(const wpi::math::Vectord<4>& x,
const wpi::math::Vectord<1>& u) {
double p = x(0);
double v = x(1);
double kV = x(2);
@@ -244,11 +244,11 @@ frc::Vectord<3> MotorMeasurementModel(const frc::Vectord<4>& x,
double V = u(0);
double a = -kV / kA * v + 1.0 / kA * V;
return frc::Vectord<3>{p, v, a};
return wpi::math::Vectord<3>{p, v, a};
}
frc::Vectord<1> MotorControlInput(double t) {
return frc::Vectord<1>{
wpi::math::Vectord<1> MotorControlInput(double t) {
return wpi::math::Vectord<1>{
std::clamp(8 * std::sin(std::numbers::pi * std::sqrt(2.0) * t) +
6 * std::sin(std::numbers::pi * std::sqrt(3.0) * t) +
4 * std::sin(std::numbers::pi * std::sqrt(5.0) * t),
@@ -256,7 +256,7 @@ frc::Vectord<1> MotorControlInput(double t) {
}
TEST(MerweUKFTest, MotorConvergence) {
constexpr units::second_t dt = 10_ms;
constexpr wpi::units::second_t dt = 10_ms;
constexpr int steps = 500;
constexpr double true_kV = 3;
constexpr double true_kA = 0.2;
@@ -265,36 +265,36 @@ TEST(MerweUKFTest, MotorConvergence) {
constexpr double vel_stddev = 0.1;
constexpr double accel_stddev = 0.1;
std::vector<frc::Vectord<4>> states(steps + 1);
std::vector<frc::Vectord<1>> inputs(steps);
std::vector<frc::Vectord<3>> measurements(steps);
states[0] = frc::Vectord<4>{{0.0}, {0.0}, {true_kV}, {true_kA}};
std::vector<wpi::math::Vectord<4>> states(steps + 1);
std::vector<wpi::math::Vectord<1>> inputs(steps);
std::vector<wpi::math::Vectord<3>> measurements(steps);
states[0] = wpi::math::Vectord<4>{{0.0}, {0.0}, {true_kV}, {true_kA}};
constexpr frc::Matrixd<4, 4> A{{0.0, 1.0, 0.0, 0.0},
constexpr wpi::math::Matrixd<4, 4> A{{0.0, 1.0, 0.0, 0.0},
{0.0, -true_kV / true_kA, 0.0, 0.0},
{0.0, 0.0, 0.0, 0.0},
{0.0, 0.0, 0.0, 0.0}};
constexpr frc::Matrixd<4, 1> B{{0.0}, {1.0 / true_kA}, {0.0}, {0.0}};
constexpr wpi::math::Matrixd<4, 1> B{{0.0}, {1.0 / true_kA}, {0.0}, {0.0}};
frc::Matrixd<4, 4> discA;
frc::Matrixd<4, 1> discB;
frc::DiscretizeAB(A, B, dt, &discA, &discB);
wpi::math::Matrixd<4, 4> discA;
wpi::math::Matrixd<4, 1> discB;
wpi::math::DiscretizeAB(A, B, dt, &discA, &discB);
for (int i = 0; i < steps; ++i) {
inputs[i] = MotorControlInput(i * dt.value());
states[i + 1] = discA * states[i] + discB * inputs[i];
measurements[i] =
MotorMeasurementModel(states[i + 1], inputs[i]) +
frc::MakeWhiteNoiseVector(pos_stddev, vel_stddev, accel_stddev);
wpi::math::MakeWhiteNoiseVector(pos_stddev, vel_stddev, accel_stddev);
}
frc::Vectord<4> P0{0.001, 0.001, 10, 10};
wpi::math::Vectord<4> P0{0.001, 0.001, 10, 10};
frc::MerweUKF<4, 1, 3> observer{
MotorDynamics, MotorMeasurementModel, wpi::array{0.1, 1.0, 1e-10, 1e-10},
wpi::array{pos_stddev, vel_stddev, accel_stddev}, dt};
wpi::math::MerweUKF<4, 1, 3> observer{
MotorDynamics, MotorMeasurementModel, wpi::util::array{0.1, 1.0, 1e-10, 1e-10},
wpi::util::array{pos_stddev, vel_stddev, accel_stddev}, dt};
observer.SetXhat(frc::Vectord<4>{0.0, 0.0, 2.0, 2.0});
observer.SetXhat(wpi::math::Vectord<4>{0.0, 0.0, 2.0, 2.0});
observer.SetP(P0.asDiagonal());
for (int i = 0; i < steps; ++i) {

18
wpimath/src/test/native/cpp/estimator/S3SigmaPointsTest.cpp
View File

@@ -11,9 +11,9 @@ TEST(S3SigmaPointsTest, Simplex) {
constexpr double beta = 2;
constexpr size_t N = 2;
frc::S3SigmaPoints<N> sigmaPoints{alpha, beta};
wpi::math::S3SigmaPoints<N> sigmaPoints{alpha, beta};
auto points = sigmaPoints.SquareRootSigmaPoints(
frc::Vectord<N>::Zero(), frc::Matrixd<N, N>::Identity());
wpi::math::Vectord<N>::Zero(), wpi::math::Matrixd<N, N>::Identity());
auto v1 = points.template block<2, 1>(0, 1);
auto v2 = points.template block<2, 1>(0, 2);
@@ -27,24 +27,24 @@ TEST(S3SigmaPointsTest, Simplex) {
}
TEST(S3SigmaPointsTest, ZeroMean) {
frc::S3SigmaPoints<2> sigmaPoints;
wpi::math::S3SigmaPoints<2> sigmaPoints;
auto points = sigmaPoints.SquareRootSigmaPoints(
frc::Vectord<2>{0.0, 0.0}, frc::Matrixd<2, 2>{{1.0, 0.0}, {0.0, 1.0}});
wpi::math::Vectord<2>{0.0, 0.0}, wpi::math::Matrixd<2, 2>{{1.0, 0.0}, {0.0, 1.0}});
EXPECT_TRUE(
(points - frc::Matrixd<2, 4>{{0.0, -0.00122474, 0.00122474, 0.0},
(points - wpi::math::Matrixd<2, 4>{{0.0, -0.00122474, 0.00122474, 0.0},
{0.0, -0.00070711, -0.00070711, 0.00141421}})
.norm() < 1e-7);
}
TEST(S3SigmaPointsTest, NonzeroMean) {
frc::S3SigmaPoints<2> sigmaPoints;
wpi::math::S3SigmaPoints<2> sigmaPoints;
auto points = sigmaPoints.SquareRootSigmaPoints(
frc::Vectord<2>{1.0, 2.0},
frc::Matrixd<2, 2>{{1.0, 0.0}, {0.0, std::sqrt(10.0)}});
wpi::math::Vectord<2>{1.0, 2.0},
wpi::math::Matrixd<2, 2>{{1.0, 0.0}, {0.0, std::sqrt(10.0)}});
EXPECT_TRUE(
(points - frc::Matrixd<2, 4>{{1.0, 0.99877526, 1.00122474, 1.0},
(points - wpi::math::Matrixd<2, 4>{{1.0, 0.99877526, 1.00122474, 1.0},
{2.0, 1.99776393, 1.99776393, 2.00447214}})
.norm() < 1e-7);
}

180
wpimath/src/test/native/cpp/estimator/S3UKFTest.cpp
View File

@@ -25,9 +25,9 @@
namespace {
// First test system, differential drive
frc::Vectord<5> DriveDynamics(const frc::Vectord<5>& x,
const frc::Vectord<2>& u) {
auto motors = frc::DCMotor::CIM(2);
wpi::math::Vectord<5> DriveDynamics(const wpi::math::Vectord<5>& x,
const wpi::math::Vectord<2>& u) {
auto motors = wpi::math::DCMotor::CIM(2);
// constexpr double Glow = 15.32; // Low gear ratio
constexpr double Ghigh = 7.08; // High gear ratio
@@ -37,18 +37,18 @@ frc::Vectord<5> DriveDynamics(const frc::Vectord<5>& x,
constexpr auto J = 5.6_kg_sq_m; // Robot moment of inertia
auto C1 = -std::pow(Ghigh, 2) * motors.Kt /
(motors.Kv * motors.R * units::math::pow<2>(r));
(motors.Kv * motors.R * wpi::units::math::pow<2>(r));
auto C2 = Ghigh * motors.Kt / (motors.R * r);
auto k1 = (1 / m + units::math::pow<2>(rb) / J);
auto k2 = (1 / m - units::math::pow<2>(rb) / J);
auto k1 = (1 / m + wpi::units::math::pow<2>(rb) / J);
auto k2 = (1 / m - wpi::units::math::pow<2>(rb) / J);
units::meters_per_second_t vl{x(3)};
units::meters_per_second_t vr{x(4)};
units::volt_t Vl{u(0)};
units::volt_t Vr{u(1)};
wpi::units::meters_per_second_t vl{x(3)};
wpi::units::meters_per_second_t vr{x(4)};
wpi::units::volt_t Vl{u(0)};
wpi::units::volt_t Vr{u(1)};
auto v = 0.5 * (vl + vr);
return frc::Vectord<5>{
return wpi::math::Vectord<5>{
v.value() * std::cos(x(2)), v.value() * std::sin(x(2)),
((vr - vl) / (2.0 * rb)).value(),
k1.value() * ((C1 * vl).value() + (C2 * Vl).value()) +
@@ -57,70 +57,70 @@ frc::Vectord<5> DriveDynamics(const frc::Vectord<5>& x,
k1.value() * ((C1 * vr).value() + (C2 * Vr).value())};
}
frc::Vectord<3> DriveLocalMeasurementModel(
const frc::Vectord<5>& x, [[maybe_unused]] const frc::Vectord<2>& u) {
return frc::Vectord<3>{x(2), x(3), x(4)};
wpi::math::Vectord<3> DriveLocalMeasurementModel(
const wpi::math::Vectord<5>& x, [[maybe_unused]] const wpi::math::Vectord<2>& u) {
return wpi::math::Vectord<3>{x(2), x(3), x(4)};
}
frc::Vectord<5> DriveGlobalMeasurementModel(
const frc::Vectord<5>& x, [[maybe_unused]] const frc::Vectord<2>& u) {
return frc::Vectord<5>{x(0), x(1), x(2), x(3), x(4)};
wpi::math::Vectord<5> DriveGlobalMeasurementModel(
const wpi::math::Vectord<5>& x, [[maybe_unused]] const wpi::math::Vectord<2>& u) {
return wpi::math::Vectord<5>{x(0), x(1), x(2), x(3), x(4)};
}
TEST(S3UKFTest, DriveInit) {
constexpr auto dt = 5_ms;
frc::S3UKF<5, 2, 3> observer{DriveDynamics,
wpi::math::S3UKF<5, 2, 3> observer{DriveDynamics,
DriveLocalMeasurementModel,
{0.5, 0.5, 10.0, 1.0, 1.0},
{0.0001, 0.01, 0.01},
frc::AngleMean<5, 5 + 2>(2),
frc::AngleMean<3, 5 + 2>(0),
frc::AngleResidual<5>(2),
frc::AngleResidual<3>(0),
frc::AngleAdd<5>(2),
wpi::math::AngleMean<5, 5 + 2>(2),
wpi::math::AngleMean<3, 5 + 2>(0),
wpi::math::AngleResidual<5>(2),
wpi::math::AngleResidual<3>(0),
wpi::math::AngleAdd<5>(2),
dt};
frc::Vectord<2> u{12.0, 12.0};
wpi::math::Vectord<2> u{12.0, 12.0};
observer.Predict(u, dt);
auto localY = DriveLocalMeasurementModel(observer.Xhat(), u);
observer.Correct(u, localY);
auto globalY = DriveGlobalMeasurementModel(observer.Xhat(), u);
auto R = frc::MakeCovMatrix(0.01, 0.01, 0.0001, 0.01, 0.01);
auto R = wpi::math::MakeCovMatrix(0.01, 0.01, 0.0001, 0.01, 0.01);
observer.Correct<5>(u, globalY, DriveGlobalMeasurementModel, R,
frc::AngleMean<5, 5 + 2>(2), frc::AngleResidual<5>(2),
frc::AngleResidual<5>(2), frc::AngleAdd<5>(2));
wpi::math::AngleMean<5, 5 + 2>(2), wpi::math::AngleResidual<5>(2),
wpi::math::AngleResidual<5>(2), wpi::math::AngleAdd<5>(2));
}
TEST(S3UKFTest, DriveConvergence) {
constexpr auto dt = 5_ms;
constexpr auto rb = 0.8382_m / 2.0; // Robot radius
frc::S3UKF<5, 2, 3> observer{DriveDynamics,
wpi::math::S3UKF<5, 2, 3> observer{DriveDynamics,
DriveLocalMeasurementModel,
{0.5, 0.5, 10.0, 1.0, 1.0},
{0.0001, 0.5, 0.5},
frc::AngleMean<5, 5 + 2>(2),
frc::AngleMean<3, 5 + 2>(0),
frc::AngleResidual<5>(2),
frc::AngleResidual<3>(0),
frc::AngleAdd<5>(2),
wpi::math::AngleMean<5, 5 + 2>(2),
wpi::math::AngleMean<3, 5 + 2>(0),
wpi::math::AngleResidual<5>(2),
wpi::math::AngleResidual<3>(0),
wpi::math::AngleAdd<5>(2),
dt};
auto waypoints =
std::vector<frc::Pose2d>{frc::Pose2d{2.75_m, 22.521_m, 0_rad},
frc::Pose2d{24.73_m, 19.68_m, 5.846_rad}};
auto trajectory = frc::TrajectoryGenerator::GenerateTrajectory(
std::vector<wpi::math::Pose2d>{wpi::math::Pose2d{2.75_m, 22.521_m, 0_rad},
wpi::math::Pose2d{24.73_m, 19.68_m, 5.846_rad}};
auto trajectory = wpi::math::TrajectoryGenerator::GenerateTrajectory(
waypoints, {8.8_mps, 0.1_mps_sq});
frc::Vectord<5> r = frc::Vectord<5>::Zero();
frc::Vectord<2> u = frc::Vectord<2>::Zero();
wpi::math::Vectord<5> r = wpi::math::Vectord<5>::Zero();
wpi::math::Vectord<2> u = wpi::math::Vectord<2>::Zero();
auto B = frc::NumericalJacobianU<5, 5, 2>(
DriveDynamics, frc::Vectord<5>::Zero(), frc::Vectord<2>::Zero());
auto B = wpi::math::NumericalJacobianU<5, 5, 2>(
DriveDynamics, wpi::math::Vectord<5>::Zero(), wpi::math::Vectord<2>::Zero());
observer.SetXhat(frc::Vectord<5>{
observer.SetXhat(wpi::math::Vectord<5>{
trajectory.InitialPose().Translation().X().value(),
trajectory.InitialPose().Translation().Y().value(),
trajectory.InitialPose().Rotation().Radians().value(), 0.0, 0.0});
@@ -130,36 +130,36 @@ TEST(S3UKFTest, DriveConvergence) {
auto totalTime = trajectory.TotalTime();
for (size_t i = 0; i < (totalTime / dt).value(); ++i) {
auto ref = trajectory.Sample(dt * i);
units::meters_per_second_t vl =
wpi::units::meters_per_second_t vl =
ref.velocity * (1 - (ref.curvature * rb).value());
units::meters_per_second_t vr =
wpi::units::meters_per_second_t vr =
ref.velocity * (1 + (ref.curvature * rb).value());
frc::Vectord<5> nextR{
wpi::math::Vectord<5> nextR{
ref.pose.Translation().X().value(), ref.pose.Translation().Y().value(),
ref.pose.Rotation().Radians().value(), vl.value(), vr.value()};
auto localY = DriveLocalMeasurementModel(trueXhat, frc::Vectord<2>::Zero());
observer.Correct(u, localY + frc::MakeWhiteNoiseVector(0.0001, 0.5, 0.5));
auto localY = DriveLocalMeasurementModel(trueXhat, wpi::math::Vectord<2>::Zero());
observer.Correct(u, localY + wpi::math::MakeWhiteNoiseVector(0.0001, 0.5, 0.5));
frc::Vectord<5> rdot = (nextR - r) / dt.value();
wpi::math::Vectord<5> rdot = (nextR - r) / dt.value();
u = B.householderQr().solve(rdot -
DriveDynamics(r, frc::Vectord<2>::Zero()));
DriveDynamics(r, wpi::math::Vectord<2>::Zero()));
observer.Predict(u, dt);
r = nextR;
trueXhat = frc::RK4(DriveDynamics, trueXhat, u, dt);
trueXhat = wpi::math::RK4(DriveDynamics, trueXhat, u, dt);
}
auto localY = DriveLocalMeasurementModel(trueXhat, u);
observer.Correct(u, localY);
auto globalY = DriveGlobalMeasurementModel(trueXhat, u);
auto R = frc::MakeCovMatrix(0.01, 0.01, 0.0001, 0.5, 0.5);
auto R = wpi::math::MakeCovMatrix(0.01, 0.01, 0.0001, 0.5, 0.5);
observer.Correct<5>(u, globalY, DriveGlobalMeasurementModel, R,
frc::AngleMean<5, 5 + 2>(2), frc::AngleResidual<5>(2),
frc::AngleResidual<5>(2), frc::AngleAdd<5>(2)
wpi::math::AngleMean<5, 5 + 2>(2), wpi::math::AngleResidual<5>(2),
wpi::math::AngleResidual<5>(2), wpi::math::AngleAdd<5>(2)
);
@@ -175,26 +175,26 @@ TEST(S3UKFTest, DriveConvergence) {
}
TEST(S3UKFTest, LinearUKF) {
constexpr units::second_t dt = 20_ms;
auto plant = frc::LinearSystemId::IdentifyVelocitySystem<units::meters>(
constexpr wpi::units::second_t dt = 20_ms;
auto plant = wpi::math::LinearSystemId::IdentifyVelocitySystem<wpi::units::meters>(
0.02_V / 1_mps, 0.006_V / 1_mps_sq);
frc::S3UKF<1, 1, 1> observer{
[&](const frc::Vectord<1>& x, const frc::Vectord<1>& u) {
wpi::math::S3UKF<1, 1, 1> observer{
[&](const wpi::math::Vectord<1>& x, const wpi::math::Vectord<1>& u) {
return plant.A() * x + plant.B() * u;
},
[&](const frc::Vectord<1>& x, const frc::Vectord<1>& u) {
[&](const wpi::math::Vectord<1>& x, const wpi::math::Vectord<1>& u) {
return plant.CalculateY(x, u);
},
{0.05},
{1.0},
dt};
frc::Matrixd<1, 1> discA;
frc::Matrixd<1, 1> discB;
frc::DiscretizeAB<1, 1>(plant.A(), plant.B(), dt, &discA, &discB);
wpi::math::Matrixd<1, 1> discA;
wpi::math::Matrixd<1, 1> discB;
wpi::math::DiscretizeAB<1, 1>(plant.A(), plant.B(), dt, &discA, &discB);
frc::Vectord<1> ref{100.0};
frc::Vectord<1> u{0.0};
wpi::math::Vectord<1> ref{100.0};
wpi::math::Vectord<1> u{0.0};
for (int i = 0; i < 2.0 / dt.value(); ++i) {
observer.Predict(u, dt);
@@ -208,22 +208,22 @@ TEST(S3UKFTest, LinearUKF) {
TEST(S3UKFTest, RoundTripP) {
constexpr auto dt = 5_ms;
frc::S3UKF<2, 2, 2> observer{
[](const frc::Vectord<2>& x, const frc::Vectord<2>& u) { return x; },
[](const frc::Vectord<2>& x, const frc::Vectord<2>& u) { return x; },
wpi::math::S3UKF<2, 2, 2> observer{
[](const wpi::math::Vectord<2>& x, const wpi::math::Vectord<2>& u) { return x; },
[](const wpi::math::Vectord<2>& x, const wpi::math::Vectord<2>& u) { return x; },
{0.0, 0.0},
{0.0, 0.0},
dt};
frc::Matrixd<2, 2> P({{2, 1}, {1, 2}});
wpi::math::Matrixd<2, 2> P({{2, 1}, {1, 2}});
observer.SetP(P);
ASSERT_TRUE(observer.P().isApprox(P));
}
// Second system, single motor feedforward estimator
frc::Vectord<4> MotorDynamics(const frc::Vectord<4>& x,
const frc::Vectord<1>& u) {
wpi::math::Vectord<4> MotorDynamics(const wpi::math::Vectord<4>& x,
const wpi::math::Vectord<1>& u) {
double v = x(1);
double kV = x(2);
double kA = x(3);
@@ -231,11 +231,11 @@ frc::Vectord<4> MotorDynamics(const frc::Vectord<4>& x,
double V = u(0);
double a = -kV / kA * v + 1.0 / kA * V;
return frc::Vectord<4>{v, a, 0.0, 0.0};
return wpi::math::Vectord<4>{v, a, 0.0, 0.0};
}
frc::Vectord<3> MotorMeasurementModel(const frc::Vectord<4>& x,
const frc::Vectord<1>& u) {
wpi::math::Vectord<3> MotorMeasurementModel(const wpi::math::Vectord<4>& x,
const wpi::math::Vectord<1>& u) {
double p = x(0);
double v = x(1);
double kV = x(2);
@@ -244,11 +244,11 @@ frc::Vectord<3> MotorMeasurementModel(const frc::Vectord<4>& x,
double V = u(0);
double a = -kV / kA * v + 1.0 / kA * V;
return frc::Vectord<3>{p, v, a};
return wpi::math::Vectord<3>{p, v, a};
}
frc::Vectord<1> MotorControlInput(double t) {
return frc::Vectord<1>{
wpi::math::Vectord<1> MotorControlInput(double t) {
return wpi::math::Vectord<1>{
std::clamp(8 * std::sin(std::numbers::pi * std::sqrt(2.0) * t) +
6 * std::sin(std::numbers::pi * std::sqrt(3.0) * t) +
4 * std::sin(std::numbers::pi * std::sqrt(5.0) * t),
@@ -256,7 +256,7 @@ frc::Vectord<1> MotorControlInput(double t) {
}
TEST(S3UKFTest, MotorConvergence) {
constexpr units::second_t dt = 10_ms;
constexpr wpi::units::second_t dt = 10_ms;
constexpr int steps = 500;
constexpr double true_kV = 3;
constexpr double true_kA = 0.2;
@@ -265,36 +265,36 @@ TEST(S3UKFTest, MotorConvergence) {
constexpr double vel_stddev = 0.1;
constexpr double accel_stddev = 0.1;
std::vector<frc::Vectord<4>> states(steps + 1);
std::vector<frc::Vectord<1>> inputs(steps);
std::vector<frc::Vectord<3>> measurements(steps);
states[0] = frc::Vectord<4>{{0.0}, {0.0}, {true_kV}, {true_kA}};
std::vector<wpi::math::Vectord<4>> states(steps + 1);
std::vector<wpi::math::Vectord<1>> inputs(steps);
std::vector<wpi::math::Vectord<3>> measurements(steps);
states[0] = wpi::math::Vectord<4>{{0.0}, {0.0}, {true_kV}, {true_kA}};
constexpr frc::Matrixd<4, 4> A{{0.0, 1.0, 0.0, 0.0},
constexpr wpi::math::Matrixd<4, 4> A{{0.0, 1.0, 0.0, 0.0},
{0.0, -true_kV / true_kA, 0.0, 0.0},
{0.0, 0.0, 0.0, 0.0},
{0.0, 0.0, 0.0, 0.0}};
constexpr frc::Matrixd<4, 1> B{{0.0}, {1.0 / true_kA}, {0.0}, {0.0}};
constexpr wpi::math::Matrixd<4, 1> B{{0.0}, {1.0 / true_kA}, {0.0}, {0.0}};
frc::Matrixd<4, 4> discA;
frc::Matrixd<4, 1> discB;
frc::DiscretizeAB(A, B, dt, &discA, &discB);
wpi::math::Matrixd<4, 4> discA;
wpi::math::Matrixd<4, 1> discB;
wpi::math::DiscretizeAB(A, B, dt, &discA, &discB);
for (int i = 0; i < steps; ++i) {
inputs[i] = MotorControlInput(i * dt.value());
states[i + 1] = discA * states[i] + discB * inputs[i];
measurements[i] =
MotorMeasurementModel(states[i + 1], inputs[i]) +
frc::MakeWhiteNoiseVector(pos_stddev, vel_stddev, accel_stddev);
wpi::math::MakeWhiteNoiseVector(pos_stddev, vel_stddev, accel_stddev);
}
frc::Vectord<4> P0{0.001, 0.001, 10, 10};
wpi::math::Vectord<4> P0{0.001, 0.001, 10, 10};
frc::S3UKF<4, 1, 3> observer{
MotorDynamics, MotorMeasurementModel, wpi::array{0.1, 1.0, 1e-10, 1e-10},
wpi::array{pos_stddev, vel_stddev, accel_stddev}, dt};
wpi::math::S3UKF<4, 1, 3> observer{
MotorDynamics, MotorMeasurementModel, wpi::util::array{0.1, 1.0, 1e-10, 1e-10},
wpi::util::array{pos_stddev, vel_stddev, accel_stddev}, dt};
observer.SetXhat(frc::Vectord<4>{0.0, 0.0, 2.0, 2.0});
observer.SetXhat(wpi::math::Vectord<4>{0.0, 0.0, 2.0, 2.0});
observer.SetP(P0.asDiagonal());
for (int i = 0; i < steps; ++i) {

304
wpimath/src/test/native/cpp/estimator/SwerveDrivePoseEstimator3dTest.cpp
View File

@@ -18,40 +18,40 @@
#include "wpi/util/timestamp.h"
void testFollowTrajectory(
const frc::SwerveDriveKinematics<4>& kinematics,
frc::SwerveDrivePoseEstimator3d<4>& estimator,
const frc::Trajectory& trajectory,
std::function<frc::ChassisSpeeds(frc::Trajectory::State&)>
const wpi::math::SwerveDriveKinematics<4>& kinematics,
wpi::math::SwerveDrivePoseEstimator3d<4>& estimator,
const wpi::math::Trajectory& trajectory,
std::function<wpi::math::ChassisSpeeds(wpi::math::Trajectory::State&)>
chassisSpeedsGenerator,
std::function<frc::Pose2d(frc::Trajectory::State&)>
std::function<wpi::math::Pose2d(wpi::math::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 wpi::math::Pose2d& startingPose, const wpi::math::Pose2d& endingPose,
const wpi::units::second_t dt, const wpi::units::second_t kVisionUpdateRate,
const wpi::units::second_t kVisionUpdateDelay, const bool checkError,
const bool debug) {
wpi::array<frc::SwerveModulePosition, 4> positions{wpi::empty_array};
wpi::util::array<wpi::math::SwerveModulePosition, 4> positions{wpi::util::empty_array};
estimator.ResetPosition(frc::Rotation3d{}, positions,
frc::Pose3d{startingPose});
estimator.ResetPosition(wpi::math::Rotation3d{}, positions,
wpi::math::Pose3d{startingPose});
std::default_random_engine generator;
std::normal_distribution<double> distribution(0.0, 1.0);
units::second_t t = 0_s;
wpi::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>>
std::vector<std::pair<wpi::units::second_t, wpi::math::Pose2d>> visionPoses;
std::vector<std::tuple<wpi::units::second_t, wpi::units::second_t, wpi::math::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");
wpi::util::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);
wpi::math::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
@@ -59,9 +59,9 @@ void testFollowTrajectory(
visionPoses.back().first + kVisionUpdateRate < t) {
auto visionPose =
visionMeasurementGenerator(groundTruthState) +
frc::Transform2d{frc::Translation2d{distribution(generator) * 0.1_m,
wpi::math::Transform2d{wpi::math::Translation2d{distribution(generator) * 0.1_m,
distribution(generator) * 0.1_m},
frc::Rotation2d{distribution(generator) * 0.05_rad}};
wpi::math::Rotation2d{distribution(generator) * 0.05_rad}};
visionPoses.push_back({t, visionPose});
}
@@ -70,7 +70,7 @@ void testFollowTrajectory(
if (!visionPoses.empty() &&
visionPoses.front().first + kVisionUpdateDelay < t) {
auto visionEntry = visionPoses.front();
estimator.AddVisionMeasurement(frc::Pose3d{visionEntry.second},
estimator.AddVisionMeasurement(wpi::math::Pose3d{visionEntry.second},
visionEntry.first);
visionPoses.erase(visionPoses.begin());
visionLog.push_back({t, visionEntry.first, visionEntry.second});
@@ -87,13 +87,13 @@ void testFollowTrajectory(
auto xhat = estimator.UpdateWithTime(
t,
frc::Rotation3d{groundTruthState.pose.Rotation() +
frc::Rotation2d{distribution(generator) * 0.05_rad} -
wpi::math::Rotation3d{groundTruthState.pose.Rotation() +
wpi::math::Rotation2d{distribution(generator) * 0.05_rad} -
trajectory.InitialPose().Rotation()},
positions);
if (debug) {
wpi::print(
wpi::util::print(
"{}, {}, {}, {}, {}, {}, {}\n", t.value(), xhat.X().value(),
xhat.Y().value(), xhat.Rotation().ToRotation2d().Radians().value(),
groundTruthState.pose.X().value(), groundTruthState.pose.Y().value(),
@@ -113,14 +113,14 @@ void testFollowTrajectory(
}
if (debug) {
wpi::print("apply_time, measured_time, vision_x, vision_y, vision_theta\n");
wpi::util::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;
wpi::units::second_t apply_time;
wpi::units::second_t measure_time;
wpi::math::Pose2d vision_pose;
for (auto record : visionLog) {
std::tie(apply_time, measure_time, vision_pose) = record;
wpi::print("{}, {}, {}, {}, {}\n", apply_time.value(),
wpi::util::print("{}, {}, {}, {}, {}\n", apply_time.value(),
measure_time.value(), vision_pose.X().value(),
vision_pose.Y().value(),
vision_pose.Rotation().Radians().value());
@@ -147,79 +147,79 @@ void testFollowTrajectory(
}
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}};
wpi::math::SwerveDriveKinematics<4> kinematics{
wpi::math::Translation2d{1_m, 1_m}, wpi::math::Translation2d{1_m, -1_m},
wpi::math::Translation2d{-1_m, -1_m}, wpi::math::Translation2d{-1_m, 1_m}};
frc::SwerveModulePosition fl;
frc::SwerveModulePosition fr;
frc::SwerveModulePosition bl;
frc::SwerveModulePosition br;
wpi::math::SwerveModulePosition fl;
wpi::math::SwerveModulePosition fr;
wpi::math::SwerveModulePosition bl;
wpi::math::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}};
wpi::math::SwerveDrivePoseEstimator3d<4> estimator{
kinematics, wpi::math::Rotation3d{}, {fl, fr, bl, br},
wpi::math::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));
wpi::math::Trajectory trajectory = wpi::math::TrajectoryGenerator::GenerateTrajectory(
std::vector{wpi::math::Pose2d{0_m, 0_m, 45_deg}, wpi::math::Pose2d{3_m, 0_m, -90_deg},
wpi::math::Pose2d{0_m, 0_m, 135_deg},
wpi::math::Pose2d{-3_m, 0_m, -90_deg},
wpi::math::Pose2d{0_m, 0_m, 45_deg}},
wpi::math::TrajectoryConfig(2_mps, 2.0_mps_sq));
testFollowTrajectory(
kinematics, estimator, trajectory,
[&](frc::Trajectory::State& state) {
return frc::ChassisSpeeds{state.velocity, 0_mps,
[&](wpi::math::Trajectory::State& state) {
return wpi::math::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}},
[&](wpi::math::Trajectory::State& state) { return state.pose; },
{0_m, 0_m, wpi::math::Rotation2d{45_deg}}, {0_m, 0_m, wpi::math::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}};
wpi::math::SwerveDriveKinematics<4> kinematics{
wpi::math::Translation2d{1_m, 1_m}, wpi::math::Translation2d{1_m, -1_m},
wpi::math::Translation2d{-1_m, -1_m}, wpi::math::Translation2d{-1_m, 1_m}};
frc::SwerveModulePosition fl;
frc::SwerveModulePosition fr;
frc::SwerveModulePosition bl;
frc::SwerveModulePosition br;
wpi::math::SwerveModulePosition fl;
wpi::math::SwerveModulePosition fr;
wpi::math::SwerveModulePosition bl;
wpi::math::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}};
wpi::math::SwerveDrivePoseEstimator3d<4> estimator{
kinematics, wpi::math::Rotation3d{}, {fl, fr, bl, br},
wpi::math::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));
wpi::math::Trajectory trajectory = wpi::math::TrajectoryGenerator::GenerateTrajectory(
std::vector{wpi::math::Pose2d{0_m, 0_m, 45_deg}, wpi::math::Pose2d{3_m, 0_m, -90_deg},
wpi::math::Pose2d{0_m, 0_m, 135_deg},
wpi::math::Pose2d{-3_m, 0_m, -90_deg},
wpi::math::Pose2d{0_m, 0_m, 45_deg}},
wpi::math::TrajectoryConfig(2_mps, 2.0_mps_sq));
for (units::degree_t offset_direction_degs = 0_deg;
for (wpi::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;
for (wpi::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 pose_offset = wpi::math::Rotation2d{offset_direction_degs};
auto heading_offset = wpi::math::Rotation2d{offset_heading_degs};
auto initial_pose =
trajectory.InitialPose() +
frc::Transform2d{frc::Translation2d{pose_offset.Cos() * 1_m,
wpi::math::Transform2d{wpi::math::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,
[&](wpi::math::Trajectory::State& state) {
return wpi::math::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,
[&](wpi::math::Trajectory::State& state) { return state.pose; },
initial_pose, {0_m, 0_m, wpi::math::Rotation2d{45_deg}}, 20_ms, 100_ms,
250_ms, false, false);
}
}
@@ -231,39 +231,39 @@ TEST(SwerveDrivePoseEstimator3dTest, SimultaneousVisionMeasurements) {
// 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}};
wpi::math::SwerveDriveKinematics<4> kinematics{
wpi::math::Translation2d{1_m, 1_m}, wpi::math::Translation2d{1_m, -1_m},
wpi::math::Translation2d{-1_m, -1_m}, wpi::math::Translation2d{-1_m, 1_m}};
frc::SwerveModulePosition fl;
frc::SwerveModulePosition fr;
frc::SwerveModulePosition bl;
frc::SwerveModulePosition br;
wpi::math::SwerveModulePosition fl;
wpi::math::SwerveModulePosition fr;
wpi::math::SwerveModulePosition bl;
wpi::math::SwerveModulePosition br;
frc::SwerveDrivePoseEstimator3d<4> estimator{
wpi::math::SwerveDrivePoseEstimator3d<4> estimator{
kinematics,
frc::Rotation3d{},
wpi::math::Rotation3d{},
{fl, fr, bl, br},
frc::Pose3d{1_m, 2_m, 0_m, frc::Rotation3d{0_deg, 0_deg, 270_deg}},
wpi::math::Pose3d{1_m, 2_m, 0_m, wpi::math::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});
estimator.UpdateWithTime(0_s, wpi::math::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);
wpi::math::Pose3d{0_m, 0_m, 0_m, wpi::math::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);
wpi::math::Pose3d{3_m, 1_m, 0_m, wpi::math::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}},
wpi::math::Pose3d{2_m, 4_m, 0_m, wpi::math::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(
auto dx = wpi::units::math::abs(estimator.GetEstimatedPosition().X() - 0_m);
auto dy = wpi::units::math::abs(estimator.GetEstimatedPosition().Y() - 0_m);
auto dtheta = wpi::units::math::abs(
estimator.GetEstimatedPosition().Rotation().ToRotation2d().Radians() -
0_deg);
@@ -271,9 +271,9 @@ TEST(SwerveDrivePoseEstimator3dTest, SimultaneousVisionMeasurements) {
}
{
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(
auto dx = wpi::units::math::abs(estimator.GetEstimatedPosition().X() - 3_m);
auto dy = wpi::units::math::abs(estimator.GetEstimatedPosition().Y() - 1_m);
auto dtheta = wpi::units::math::abs(
estimator.GetEstimatedPosition().Rotation().ToRotation2d().Radians() -
90_deg);
@@ -281,9 +281,9 @@ TEST(SwerveDrivePoseEstimator3dTest, SimultaneousVisionMeasurements) {
}
{
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(
auto dx = wpi::units::math::abs(estimator.GetEstimatedPosition().X() - 2_m);
auto dy = wpi::units::math::abs(estimator.GetEstimatedPosition().Y() - 4_m);
auto dtheta = wpi::units::math::abs(
estimator.GetEstimatedPosition().Rotation().ToRotation2d().Radians() -
180_deg);
@@ -292,29 +292,29 @@ TEST(SwerveDrivePoseEstimator3dTest, SimultaneousVisionMeasurements) {
}
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}};
wpi::math::SwerveDriveKinematics<4> kinematics{
wpi::math::Translation2d{1_m, 1_m}, wpi::math::Translation2d{1_m, -1_m},
wpi::math::Translation2d{-1_m, -1_m}, wpi::math::Translation2d{-1_m, 1_m}};
frc::SwerveModulePosition fl;
frc::SwerveModulePosition fr;
frc::SwerveModulePosition bl;
frc::SwerveModulePosition br;
wpi::math::SwerveModulePosition fl;
wpi::math::SwerveModulePosition fr;
wpi::math::SwerveModulePosition bl;
wpi::math::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}};
wpi::math::SwerveDrivePoseEstimator3d<4> estimator{
kinematics, wpi::math::Rotation3d{}, {fl, fr, bl, br},
wpi::math::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});
estimator.UpdateWithTime(time, wpi::math::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,
wpi::math::Pose3d{10_m, 10_m, 0_m, wpi::math::Rotation3d{0_rad, 0_rad, 0.1_rad}}, 1_s,
{0.1, 0.1, 0.1, 0.1});
EXPECT_NEAR(odometryPose.X().value(),
@@ -332,15 +332,15 @@ TEST(SwerveDrivePoseEstimator3dTest, TestDiscardStaleVisionMeasurements) {
}
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{
wpi::math::SwerveDriveKinematics<4> kinematics{
wpi::math::Translation2d{1_m, 1_m}, wpi::math::Translation2d{1_m, -1_m},
wpi::math::Translation2d{-1_m, -1_m}, wpi::math::Translation2d{-1_m, 1_m}};
wpi::math::SwerveDrivePoseEstimator3d estimator{
kinematics,
frc::Rotation3d{},
{frc::SwerveModulePosition{}, frc::SwerveModulePosition{},
frc::SwerveModulePosition{}, frc::SwerveModulePosition{}},
frc::Pose3d{},
wpi::math::Rotation3d{},
{wpi::math::SwerveModulePosition{}, wpi::math::SwerveModulePosition{},
wpi::math::SwerveModulePosition{}, wpi::math::SwerveModulePosition{}},
wpi::math::Pose3d{},
{1.0, 1.0, 1.0, 1.0},
{1.0, 1.0, 1.0, 1.0}};
@@ -351,67 +351,67 @@ TEST(SwerveDrivePoseEstimator3dTest, TestSampleAt) {
// 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{},
wpi::util::array<wpi::math::SwerveModulePosition, 4> wheelPositions{
{wpi::math::SwerveModulePosition{wpi::units::meter_t{time}, wpi::math::Rotation2d{}},
wpi::math::SwerveModulePosition{wpi::units::meter_t{time}, wpi::math::Rotation2d{}},
wpi::math::SwerveModulePosition{wpi::units::meter_t{time}, wpi::math::Rotation2d{}},
wpi::math::SwerveModulePosition{wpi::units::meter_t{time}, wpi::math::Rotation2d{}}}};
estimator.UpdateWithTime(wpi::units::second_t{time}, wpi::math::Rotation3d{},
wheelPositions);
}
// Sample at an added time
EXPECT_EQ(std::optional(frc::Pose3d{1.02_m, 0_m, 0_m, frc::Rotation3d{}}),
EXPECT_EQ(std::optional(wpi::math::Pose3d{1.02_m, 0_m, 0_m, wpi::math::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{}}),
EXPECT_EQ(std::optional(wpi::math::Pose3d{1.01_m, 0_m, 0_m, wpi::math::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{}}),
EXPECT_EQ(std::optional(wpi::math::Pose3d{1_m, 0_m, 0_m, wpi::math::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{}}),
EXPECT_EQ(std::optional(wpi::math::Pose3d{2_m, 0_m, 0_m, wpi::math::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);
wpi::math::Pose3d{2_m, 0_m, 0_m, wpi::math::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{}}),
EXPECT_EQ(std::optional(wpi::math::Pose3d{1.02_m, 0_m, 0_m, wpi::math::Rotation3d{}}),
estimator.SampleAt(1.02_s));
EXPECT_EQ(std::optional(frc::Pose3d{1.01_m, 0_m, 0_m, frc::Rotation3d{}}),
EXPECT_EQ(std::optional(wpi::math::Pose3d{1.01_m, 0_m, 0_m, wpi::math::Rotation3d{}}),
estimator.SampleAt(1.01_s));
EXPECT_EQ(std::optional(frc::Pose3d{1_m, 0_m, 0_m, frc::Rotation3d{}}),
EXPECT_EQ(std::optional(wpi::math::Pose3d{1_m, 0_m, 0_m, wpi::math::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);
wpi::math::Pose3d{1_m, 0.2_m, 0_m, wpi::math::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{}}),
EXPECT_EQ(std::optional(wpi::math::Pose3d{1.02_m, 0.1_m, 0_m, wpi::math::Rotation3d{}}),
estimator.SampleAt(1.02_s));
EXPECT_EQ(std::optional(frc::Pose3d{1.01_m, 0.1_m, 0_m, frc::Rotation3d{}}),
EXPECT_EQ(std::optional(wpi::math::Pose3d{1.01_m, 0.1_m, 0_m, wpi::math::Rotation3d{}}),
estimator.SampleAt(1.01_s));
EXPECT_EQ(std::optional(frc::Pose3d{1_m, 0.1_m, 0_m, frc::Rotation3d{}}),
EXPECT_EQ(std::optional(wpi::math::Pose3d{1_m, 0.1_m, 0_m, wpi::math::Rotation3d{}}),
estimator.SampleAt(0.5_s));
EXPECT_EQ(std::optional(frc::Pose3d{2_m, 0.1_m, 0_m, frc::Rotation3d{}}),
EXPECT_EQ(std::optional(wpi::math::Pose3d{2_m, 0.1_m, 0_m, wpi::math::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{
wpi::math::SwerveDriveKinematics<4> kinematics{
wpi::math::Translation2d{1_m, 1_m}, wpi::math::Translation2d{1_m, -1_m},
wpi::math::Translation2d{-1_m, -1_m}, wpi::math::Translation2d{-1_m, 1_m}};
wpi::math::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}},
wpi::math::Rotation3d{},
{wpi::math::SwerveModulePosition{}, wpi::math::SwerveModulePosition{},
wpi::math::SwerveModulePosition{}, wpi::math::SwerveModulePosition{}},
wpi::math::Pose3d{-1_m, -1_m, -1_m, wpi::math::Rotation3d{0_rad, 0_rad, 1_rad}},
{1.0, 1.0, 1.0, 1.0},
{1.0, 1.0, 1.0, 1.0}};
@@ -425,11 +425,11 @@ TEST(SwerveDrivePoseEstimator3dTest, TestReset) {
// Test reset position
{
frc::SwerveModulePosition modulePosition{1_m, frc::Rotation2d{}};
wpi::math::SwerveModulePosition modulePosition{1_m, wpi::math::Rotation2d{}};
estimator.ResetPosition(
frc::Rotation3d{},
wpi::math::Rotation3d{},
{modulePosition, modulePosition, modulePosition, modulePosition},
frc::Pose3d{1_m, 0_m, 0_m, frc::Rotation3d{}});
wpi::math::Pose3d{1_m, 0_m, 0_m, wpi::math::Rotation3d{}});
}
EXPECT_DOUBLE_EQ(1, estimator.GetEstimatedPosition().X().value());
@@ -441,8 +441,8 @@ TEST(SwerveDrivePoseEstimator3dTest, TestReset) {
// Test orientation and wheel positions
{
frc::SwerveModulePosition modulePosition{2_m, frc::Rotation2d{}};
estimator.Update(frc::Rotation3d{}, {modulePosition, modulePosition,
wpi::math::SwerveModulePosition modulePosition{2_m, wpi::math::Rotation2d{}};
estimator.Update(wpi::math::Rotation3d{}, {modulePosition, modulePosition,
modulePosition, modulePosition});
}
@@ -454,7 +454,7 @@ TEST(SwerveDrivePoseEstimator3dTest, TestReset) {
EXPECT_DOUBLE_EQ(0, estimator.GetEstimatedPosition().Rotation().Z().value());
// Test reset rotation
estimator.ResetRotation(frc::Rotation3d{0_deg, 0_deg, 90_deg});
estimator.ResetRotation(wpi::math::Rotation3d{0_deg, 0_deg, 90_deg});
EXPECT_DOUBLE_EQ(2, estimator.GetEstimatedPosition().X().value());
EXPECT_DOUBLE_EQ(0, estimator.GetEstimatedPosition().Y().value());
@@ -466,8 +466,8 @@ TEST(SwerveDrivePoseEstimator3dTest, TestReset) {
// Test orientation
{
frc::SwerveModulePosition modulePosition{3_m, frc::Rotation2d{}};
estimator.Update(frc::Rotation3d{}, {modulePosition, modulePosition,
wpi::math::SwerveModulePosition modulePosition{3_m, wpi::math::Rotation2d{}};
estimator.Update(wpi::math::Rotation3d{}, {modulePosition, modulePosition,
modulePosition, modulePosition});
}
@@ -480,7 +480,7 @@ TEST(SwerveDrivePoseEstimator3dTest, TestReset) {
estimator.GetEstimatedPosition().Rotation().Z().value());
// Test reset translation
estimator.ResetTranslation(frc::Translation3d{-1_m, -1_m, -1_m});
estimator.ResetTranslation(wpi::math::Translation3d{-1_m, -1_m, -1_m});
EXPECT_DOUBLE_EQ(-1, estimator.GetEstimatedPosition().X().value());
EXPECT_DOUBLE_EQ(-1, estimator.GetEstimatedPosition().Y().value());
@@ -491,7 +491,7 @@ TEST(SwerveDrivePoseEstimator3dTest, TestReset) {
estimator.GetEstimatedPosition().Rotation().Z().value());
// Test reset pose
estimator.ResetPose(frc::Pose3d{});
estimator.ResetPose(wpi::math::Pose3d{});
EXPECT_DOUBLE_EQ(0, estimator.GetEstimatedPosition().X().value());
EXPECT_DOUBLE_EQ(0, estimator.GetEstimatedPosition().Y().value());

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

@@ -19,39 +19,39 @@
#include "wpi/util/timestamp.h"
void testFollowTrajectory(
const frc::SwerveDriveKinematics<4>& kinematics,
frc::SwerveDrivePoseEstimator<4>& estimator,
const frc::Trajectory& trajectory,
std::function<frc::ChassisSpeeds(frc::Trajectory::State&)>
const wpi::math::SwerveDriveKinematics<4>& kinematics,
wpi::math::SwerveDrivePoseEstimator<4>& estimator,
const wpi::math::Trajectory& trajectory,
std::function<wpi::math::ChassisSpeeds(wpi::math::Trajectory::State&)>
chassisSpeedsGenerator,
std::function<frc::Pose2d(frc::Trajectory::State&)>
std::function<wpi::math::Pose2d(wpi::math::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 wpi::math::Pose2d& startingPose, const wpi::math::Pose2d& endingPose,
const wpi::units::second_t dt, const wpi::units::second_t kVisionUpdateRate,
const wpi::units::second_t kVisionUpdateDelay, const bool checkError,
const bool debug) {
wpi::array<frc::SwerveModulePosition, 4> positions{wpi::empty_array};
wpi::util::array<wpi::math::SwerveModulePosition, 4> positions{wpi::util::empty_array};
estimator.ResetPosition(frc::Rotation2d{}, positions, startingPose);
estimator.ResetPosition(wpi::math::Rotation2d{}, positions, startingPose);
std::default_random_engine generator;
std::normal_distribution<double> distribution(0.0, 1.0);
units::second_t t = 0_s;
wpi::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>>
std::vector<std::pair<wpi::units::second_t, wpi::math::Pose2d>> visionPoses;
std::vector<std::tuple<wpi::units::second_t, wpi::units::second_t, wpi::math::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");
wpi::util::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);
wpi::math::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
@@ -59,9 +59,9 @@ void testFollowTrajectory(
visionPoses.back().first + kVisionUpdateRate < t) {
auto visionPose =
visionMeasurementGenerator(groundTruthState) +
frc::Transform2d{frc::Translation2d{distribution(generator) * 0.1_m,
wpi::math::Transform2d{wpi::math::Translation2d{distribution(generator) * 0.1_m,
distribution(generator) * 0.1_m},
frc::Rotation2d{distribution(generator) * 0.05_rad}};
wpi::math::Rotation2d{distribution(generator) * 0.05_rad}};
visionPoses.push_back({t, visionPose});
}
@@ -87,12 +87,12 @@ void testFollowTrajectory(
auto xhat = estimator.UpdateWithTime(
t,
groundTruthState.pose.Rotation() +
frc::Rotation2d{distribution(generator) * 0.05_rad} -
wpi::math::Rotation2d{distribution(generator) * 0.05_rad} -
trajectory.InitialPose().Rotation(),
positions);
if (debug) {
wpi::print("{}, {}, {}, {}, {}, {}, {}\n", t.value(), xhat.X().value(),
wpi::util::print("{}, {}, {}, {}, {}, {}, {}\n", t.value(), xhat.X().value(),
xhat.Y().value(), xhat.Rotation().Radians().value(),
groundTruthState.pose.X().value(),
groundTruthState.pose.Y().value(),
@@ -112,14 +112,14 @@ void testFollowTrajectory(
}
if (debug) {
wpi::print("apply_time, measured_time, vision_x, vision_y, vision_theta\n");
wpi::util::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;
wpi::units::second_t apply_time;
wpi::units::second_t measure_time;
wpi::math::Pose2d vision_pose;
for (auto record : visionLog) {
std::tie(apply_time, measure_time, vision_pose) = record;
wpi::print("{}, {}, {}, {}, {}\n", apply_time.value(),
wpi::util::print("{}, {}, {}, {}, {}\n", apply_time.value(),
measure_time.value(), vision_pose.X().value(),
vision_pose.Y().value(),
vision_pose.Rotation().Radians().value());
@@ -142,79 +142,79 @@ void testFollowTrajectory(
}
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}};
wpi::math::SwerveDriveKinematics<4> kinematics{
wpi::math::Translation2d{1_m, 1_m}, wpi::math::Translation2d{1_m, -1_m},
wpi::math::Translation2d{-1_m, -1_m}, wpi::math::Translation2d{-1_m, 1_m}};
frc::SwerveModulePosition fl;
frc::SwerveModulePosition fr;
frc::SwerveModulePosition bl;
frc::SwerveModulePosition br;
wpi::math::SwerveModulePosition fl;
wpi::math::SwerveModulePosition fr;
wpi::math::SwerveModulePosition bl;
wpi::math::SwerveModulePosition br;
frc::SwerveDrivePoseEstimator<4> estimator{
kinematics, frc::Rotation2d{}, {fl, fr, bl, br},
frc::Pose2d{}, {0.1, 0.1, 0.1}, {0.45, 0.45, 0.45}};
wpi::math::SwerveDrivePoseEstimator<4> estimator{
kinematics, wpi::math::Rotation2d{}, {fl, fr, bl, br},
wpi::math::Pose2d{}, {0.1, 0.1, 0.1}, {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));
wpi::math::Trajectory trajectory = wpi::math::TrajectoryGenerator::GenerateTrajectory(
std::vector{wpi::math::Pose2d{0_m, 0_m, 45_deg}, wpi::math::Pose2d{3_m, 0_m, -90_deg},
wpi::math::Pose2d{0_m, 0_m, 135_deg},
wpi::math::Pose2d{-3_m, 0_m, -90_deg},
wpi::math::Pose2d{0_m, 0_m, 45_deg}},
wpi::math::TrajectoryConfig(2_mps, 2.0_mps_sq));
testFollowTrajectory(
kinematics, estimator, trajectory,
[&](frc::Trajectory::State& state) {
return frc::ChassisSpeeds{state.velocity, 0_mps,
[&](wpi::math::Trajectory::State& state) {
return wpi::math::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}},
[&](wpi::math::Trajectory::State& state) { return state.pose; },
{0_m, 0_m, wpi::math::Rotation2d{45_deg}}, {0_m, 0_m, wpi::math::Rotation2d{45_deg}},
20_ms, 100_ms, 250_ms, true, false);
}
TEST(SwerveDrivePoseEstimatorTest, 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}};
wpi::math::SwerveDriveKinematics<4> kinematics{
wpi::math::Translation2d{1_m, 1_m}, wpi::math::Translation2d{1_m, -1_m},
wpi::math::Translation2d{-1_m, -1_m}, wpi::math::Translation2d{-1_m, 1_m}};
frc::SwerveModulePosition fl;
frc::SwerveModulePosition fr;
frc::SwerveModulePosition bl;
frc::SwerveModulePosition br;
wpi::math::SwerveModulePosition fl;
wpi::math::SwerveModulePosition fr;
wpi::math::SwerveModulePosition bl;
wpi::math::SwerveModulePosition br;
frc::SwerveDrivePoseEstimator<4> estimator{
kinematics, frc::Rotation2d{}, {fl, fr, bl, br},
frc::Pose2d{}, {0.1, 0.1, 0.1}, {0.9, 0.9, 0.9}};
wpi::math::SwerveDrivePoseEstimator<4> estimator{
kinematics, wpi::math::Rotation2d{}, {fl, fr, bl, br},
wpi::math::Pose2d{}, {0.1, 0.1, 0.1}, {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));
wpi::math::Trajectory trajectory = wpi::math::TrajectoryGenerator::GenerateTrajectory(
std::vector{wpi::math::Pose2d{0_m, 0_m, 45_deg}, wpi::math::Pose2d{3_m, 0_m, -90_deg},
wpi::math::Pose2d{0_m, 0_m, 135_deg},
wpi::math::Pose2d{-3_m, 0_m, -90_deg},
wpi::math::Pose2d{0_m, 0_m, 45_deg}},
wpi::math::TrajectoryConfig(2_mps, 2.0_mps_sq));
for (units::degree_t offset_direction_degs = 0_deg;
for (wpi::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;
for (wpi::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 pose_offset = wpi::math::Rotation2d{offset_direction_degs};
auto heading_offset = wpi::math::Rotation2d{offset_heading_degs};
auto initial_pose =
trajectory.InitialPose() +
frc::Transform2d{frc::Translation2d{pose_offset.Cos() * 1_m,
wpi::math::Transform2d{wpi::math::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,
[&](wpi::math::Trajectory::State& state) {
return wpi::math::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,
[&](wpi::math::Trajectory::State& state) { return state.pose; },
initial_pose, {0_m, 0_m, wpi::math::Rotation2d{45_deg}}, 20_ms, 100_ms,
250_ms, false, false);
}
}
@@ -226,53 +226,53 @@ TEST(SwerveDrivePoseEstimatorTest, SimultaneousVisionMeasurements) {
// 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}};
wpi::math::SwerveDriveKinematics<4> kinematics{
wpi::math::Translation2d{1_m, 1_m}, wpi::math::Translation2d{1_m, -1_m},
wpi::math::Translation2d{-1_m, -1_m}, wpi::math::Translation2d{-1_m, 1_m}};
frc::SwerveModulePosition fl;
frc::SwerveModulePosition fr;
frc::SwerveModulePosition bl;
frc::SwerveModulePosition br;
wpi::math::SwerveModulePosition fl;
wpi::math::SwerveModulePosition fr;
wpi::math::SwerveModulePosition bl;
wpi::math::SwerveModulePosition br;
frc::SwerveDrivePoseEstimator<4> estimator{
kinematics, frc::Rotation2d{},
{fl, fr, bl, br}, frc::Pose2d{1_m, 2_m, frc::Rotation2d{270_deg}},
wpi::math::SwerveDrivePoseEstimator<4> estimator{
kinematics, wpi::math::Rotation2d{},
{fl, fr, bl, br}, wpi::math::Pose2d{1_m, 2_m, wpi::math::Rotation2d{270_deg}},
{0.1, 0.1, 0.1}, {0.45, 0.45, 0.45}};
estimator.UpdateWithTime(0_s, frc::Rotation2d{}, {fl, fr, bl, br});
estimator.UpdateWithTime(0_s, wpi::math::Rotation2d{}, {fl, fr, bl, br});
for (int i = 0; i < 1000; i++) {
estimator.AddVisionMeasurement(
frc::Pose2d{0_m, 0_m, frc::Rotation2d{0_deg}}, 0_s);
wpi::math::Pose2d{0_m, 0_m, wpi::math::Rotation2d{0_deg}}, 0_s);
estimator.AddVisionMeasurement(
frc::Pose2d{3_m, 1_m, frc::Rotation2d{90_deg}}, 0_s);
wpi::math::Pose2d{3_m, 1_m, wpi::math::Rotation2d{90_deg}}, 0_s);
estimator.AddVisionMeasurement(
frc::Pose2d{2_m, 4_m, frc::Rotation2d{180_deg}}, 0_s);
wpi::math::Pose2d{2_m, 4_m, wpi::math::Rotation2d{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(
auto dx = wpi::units::math::abs(estimator.GetEstimatedPosition().X() - 0_m);
auto dy = wpi::units::math::abs(estimator.GetEstimatedPosition().Y() - 0_m);
auto dtheta = wpi::units::math::abs(
estimator.GetEstimatedPosition().Rotation().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(
auto dx = wpi::units::math::abs(estimator.GetEstimatedPosition().X() - 3_m);
auto dy = wpi::units::math::abs(estimator.GetEstimatedPosition().Y() - 1_m);
auto dtheta = wpi::units::math::abs(
estimator.GetEstimatedPosition().Rotation().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(
auto dx = wpi::units::math::abs(estimator.GetEstimatedPosition().X() - 2_m);
auto dy = wpi::units::math::abs(estimator.GetEstimatedPosition().Y() - 4_m);
auto dtheta = wpi::units::math::abs(
estimator.GetEstimatedPosition().Rotation().Radians() - 180_deg);
EXPECT_TRUE(dx > 0.08_m || dy > 0.08_m || dtheta > 0.08_rad);
@@ -280,29 +280,29 @@ TEST(SwerveDrivePoseEstimatorTest, SimultaneousVisionMeasurements) {
}
TEST(SwerveDrivePoseEstimatorTest, 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}};
wpi::math::SwerveDriveKinematics<4> kinematics{
wpi::math::Translation2d{1_m, 1_m}, wpi::math::Translation2d{1_m, -1_m},
wpi::math::Translation2d{-1_m, -1_m}, wpi::math::Translation2d{-1_m, 1_m}};
frc::SwerveModulePosition fl;
frc::SwerveModulePosition fr;
frc::SwerveModulePosition bl;
frc::SwerveModulePosition br;
wpi::math::SwerveModulePosition fl;
wpi::math::SwerveModulePosition fr;
wpi::math::SwerveModulePosition bl;
wpi::math::SwerveModulePosition br;
frc::SwerveDrivePoseEstimator<4> estimator{
kinematics, frc::Rotation2d{}, {fl, fr, bl, br},
frc::Pose2d{}, {0.1, 0.1, 0.1}, {0.45, 0.45, 0.45}};
wpi::math::SwerveDrivePoseEstimator<4> estimator{
kinematics, wpi::math::Rotation2d{}, {fl, fr, bl, br},
wpi::math::Pose2d{}, {0.1, 0.1, 0.1}, {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::Rotation2d{}, {fl, fr, bl, br});
estimator.UpdateWithTime(time, wpi::math::Rotation2d{}, {fl, fr, bl, br});
}
auto odometryPose = estimator.GetEstimatedPosition();
// Apply a vision measurement from 3 seconds ago
estimator.AddVisionMeasurement(
frc::Pose2d{frc::Translation2d{10_m, 10_m}, frc::Rotation2d{0.1_rad}},
wpi::math::Pose2d{wpi::math::Translation2d{10_m, 10_m}, wpi::math::Rotation2d{0.1_rad}},
1_s, {0.1, 0.1, 0.1});
EXPECT_NEAR(odometryPose.X().value(),
@@ -315,15 +315,15 @@ TEST(SwerveDrivePoseEstimatorTest, TestDiscardStaleVisionMeasurements) {
}
TEST(SwerveDrivePoseEstimatorTest, 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::SwerveDrivePoseEstimator estimator{
wpi::math::SwerveDriveKinematics<4> kinematics{
wpi::math::Translation2d{1_m, 1_m}, wpi::math::Translation2d{1_m, -1_m},
wpi::math::Translation2d{-1_m, -1_m}, wpi::math::Translation2d{-1_m, 1_m}};
wpi::math::SwerveDrivePoseEstimator estimator{
kinematics,
frc::Rotation2d{},
{frc::SwerveModulePosition{}, frc::SwerveModulePosition{},
frc::SwerveModulePosition{}, frc::SwerveModulePosition{}},
frc::Pose2d{},
wpi::math::Rotation2d{},
{wpi::math::SwerveModulePosition{}, wpi::math::SwerveModulePosition{},
wpi::math::SwerveModulePosition{}, wpi::math::SwerveModulePosition{}},
wpi::math::Pose2d{},
{1.0, 1.0, 1.0},
{1.0, 1.0, 1.0}};
@@ -334,67 +334,67 @@ TEST(SwerveDrivePoseEstimatorTest, TestSampleAt) {
// 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::Rotation2d{},
wpi::util::array<wpi::math::SwerveModulePosition, 4> wheelPositions{
{wpi::math::SwerveModulePosition{wpi::units::meter_t{time}, wpi::math::Rotation2d{}},
wpi::math::SwerveModulePosition{wpi::units::meter_t{time}, wpi::math::Rotation2d{}},
wpi::math::SwerveModulePosition{wpi::units::meter_t{time}, wpi::math::Rotation2d{}},
wpi::math::SwerveModulePosition{wpi::units::meter_t{time}, wpi::math::Rotation2d{}}}};
estimator.UpdateWithTime(wpi::units::second_t{time}, wpi::math::Rotation2d{},
wheelPositions);
}
// Sample at an added time
EXPECT_EQ(std::optional(frc::Pose2d{1.02_m, 0_m, frc::Rotation2d{}}),
EXPECT_EQ(std::optional(wpi::math::Pose2d{1.02_m, 0_m, wpi::math::Rotation2d{}}),
estimator.SampleAt(1.02_s));
// Sample between updates (test interpolation)
EXPECT_EQ(std::optional(frc::Pose2d{1.01_m, 0_m, frc::Rotation2d{}}),
EXPECT_EQ(std::optional(wpi::math::Pose2d{1.01_m, 0_m, wpi::math::Rotation2d{}}),
estimator.SampleAt(1.01_s));
// Sampling before the oldest value returns the oldest value
EXPECT_EQ(std::optional(frc::Pose2d{1_m, 0_m, frc::Rotation2d{}}),
EXPECT_EQ(std::optional(wpi::math::Pose2d{1_m, 0_m, wpi::math::Rotation2d{}}),
estimator.SampleAt(0.5_s));
// Sampling after the newest value returns the newest value
EXPECT_EQ(std::optional(frc::Pose2d{2_m, 0_m, frc::Rotation2d{}}),
EXPECT_EQ(std::optional(wpi::math::Pose2d{2_m, 0_m, wpi::math::Rotation2d{}}),
estimator.SampleAt(2.5_s));
// Add a vision measurement after the odometry measurements (while keeping all
// of the old odometry measurements)
estimator.AddVisionMeasurement(frc::Pose2d{2_m, 0_m, frc::Rotation2d{1_rad}},
estimator.AddVisionMeasurement(wpi::math::Pose2d{2_m, 0_m, wpi::math::Rotation2d{1_rad}},
2.2_s);
// Make sure nothing changed (except the newest value)
EXPECT_EQ(std::optional(frc::Pose2d{1.02_m, 0_m, frc::Rotation2d{}}),
EXPECT_EQ(std::optional(wpi::math::Pose2d{1.02_m, 0_m, wpi::math::Rotation2d{}}),
estimator.SampleAt(1.02_s));
EXPECT_EQ(std::optional(frc::Pose2d{1.01_m, 0_m, frc::Rotation2d{}}),
EXPECT_EQ(std::optional(wpi::math::Pose2d{1.01_m, 0_m, wpi::math::Rotation2d{}}),
estimator.SampleAt(1.01_s));
EXPECT_EQ(std::optional(frc::Pose2d{1_m, 0_m, frc::Rotation2d{}}),
EXPECT_EQ(std::optional(wpi::math::Pose2d{1_m, 0_m, wpi::math::Rotation2d{}}),
estimator.SampleAt(0.5_s));
// Add a vision measurement before the odometry measurements that's still in
// the buffer
estimator.AddVisionMeasurement(frc::Pose2d{1_m, 0.2_m, frc::Rotation2d{}},
estimator.AddVisionMeasurement(wpi::math::Pose2d{1_m, 0.2_m, wpi::math::Rotation2d{}},
0.9_s);
// Everything should be the same except Y is 0.1 (halfway between 0 and 0.2)
EXPECT_EQ(std::optional(frc::Pose2d{1.02_m, 0.1_m, frc::Rotation2d{}}),
EXPECT_EQ(std::optional(wpi::math::Pose2d{1.02_m, 0.1_m, wpi::math::Rotation2d{}}),
estimator.SampleAt(1.02_s));
EXPECT_EQ(std::optional(frc::Pose2d{1.01_m, 0.1_m, frc::Rotation2d{}}),
EXPECT_EQ(std::optional(wpi::math::Pose2d{1.01_m, 0.1_m, wpi::math::Rotation2d{}}),
estimator.SampleAt(1.01_s));
EXPECT_EQ(std::optional(frc::Pose2d{1_m, 0.1_m, frc::Rotation2d{}}),
EXPECT_EQ(std::optional(wpi::math::Pose2d{1_m, 0.1_m, wpi::math::Rotation2d{}}),
estimator.SampleAt(0.5_s));
EXPECT_EQ(std::optional(frc::Pose2d{2_m, 0.1_m, frc::Rotation2d{}}),
EXPECT_EQ(std::optional(wpi::math::Pose2d{2_m, 0.1_m, wpi::math::Rotation2d{}}),
estimator.SampleAt(2.5_s));
}
TEST(SwerveDrivePoseEstimatorTest, 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::SwerveDrivePoseEstimator estimator{
wpi::math::SwerveDriveKinematics<4> kinematics{
wpi::math::Translation2d{1_m, 1_m}, wpi::math::Translation2d{1_m, -1_m},
wpi::math::Translation2d{-1_m, -1_m}, wpi::math::Translation2d{-1_m, 1_m}};
wpi::math::SwerveDrivePoseEstimator estimator{
kinematics,
frc::Rotation2d{},
{frc::SwerveModulePosition{}, frc::SwerveModulePosition{},
frc::SwerveModulePosition{}, frc::SwerveModulePosition{}},
frc::Pose2d{-1_m, -1_m, frc::Rotation2d{1_rad}},
wpi::math::Rotation2d{},
{wpi::math::SwerveModulePosition{}, wpi::math::SwerveModulePosition{},
wpi::math::SwerveModulePosition{}, wpi::math::SwerveModulePosition{}},
wpi::math::Pose2d{-1_m, -1_m, wpi::math::Rotation2d{1_rad}},
{1.0, 1.0, 1.0},
{1.0, 1.0, 1.0}};
@@ -406,11 +406,11 @@ TEST(SwerveDrivePoseEstimatorTest, TestReset) {
// Test reset position
{
frc::SwerveModulePosition modulePosition{1_m, frc::Rotation2d{}};
wpi::math::SwerveModulePosition modulePosition{1_m, wpi::math::Rotation2d{}};
estimator.ResetPosition(
frc::Rotation2d{},
wpi::math::Rotation2d{},
{modulePosition, modulePosition, modulePosition, modulePosition},
frc::Pose2d{1_m, 0_m, frc::Rotation2d{}});
wpi::math::Pose2d{1_m, 0_m, wpi::math::Rotation2d{}});
}
EXPECT_DOUBLE_EQ(1, estimator.GetEstimatedPosition().X().value());
@@ -420,8 +420,8 @@ TEST(SwerveDrivePoseEstimatorTest, TestReset) {
// Test orientation and wheel positions
{
frc::SwerveModulePosition modulePosition{2_m, frc::Rotation2d{}};
estimator.Update(frc::Rotation2d{}, {modulePosition, modulePosition,
wpi::math::SwerveModulePosition modulePosition{2_m, wpi::math::Rotation2d{}};
estimator.Update(wpi::math::Rotation2d{}, {modulePosition, modulePosition,
modulePosition, modulePosition});
}
@@ -431,7 +431,7 @@ TEST(SwerveDrivePoseEstimatorTest, TestReset) {
0, estimator.GetEstimatedPosition().Rotation().Radians().value());
// Test reset rotation
estimator.ResetRotation(frc::Rotation2d{90_deg});
estimator.ResetRotation(wpi::math::Rotation2d{90_deg});
EXPECT_DOUBLE_EQ(2, estimator.GetEstimatedPosition().X().value());
EXPECT_DOUBLE_EQ(0, estimator.GetEstimatedPosition().Y().value());
@@ -441,8 +441,8 @@ TEST(SwerveDrivePoseEstimatorTest, TestReset) {
// Test orientation
{
frc::SwerveModulePosition modulePosition{3_m, frc::Rotation2d{}};
estimator.Update(frc::Rotation2d{}, {modulePosition, modulePosition,
wpi::math::SwerveModulePosition modulePosition{3_m, wpi::math::Rotation2d{}};
estimator.Update(wpi::math::Rotation2d{}, {modulePosition, modulePosition,
modulePosition, modulePosition});
}
@@ -453,7 +453,7 @@ TEST(SwerveDrivePoseEstimatorTest, TestReset) {
estimator.GetEstimatedPosition().Rotation().Radians().value());
// Test reset translation
estimator.ResetTranslation(frc::Translation2d{-1_m, -1_m});
estimator.ResetTranslation(wpi::math::Translation2d{-1_m, -1_m});
EXPECT_DOUBLE_EQ(-1, estimator.GetEstimatedPosition().X().value());
EXPECT_DOUBLE_EQ(-1, estimator.GetEstimatedPosition().Y().value());
@@ -462,7 +462,7 @@ TEST(SwerveDrivePoseEstimatorTest, TestReset) {
estimator.GetEstimatedPosition().Rotation().Radians().value());
// Test reset pose
estimator.ResetPose(frc::Pose2d{});
estimator.ResetPose(wpi::math::Pose2d{});
EXPECT_DOUBLE_EQ(0, estimator.GetEstimatedPosition().X().value());
EXPECT_DOUBLE_EQ(0, estimator.GetEstimatedPosition().Y().value());