Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-07-04 03:11:43 +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

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());