Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-27 02:01: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

94
wpimath/src/test/native/cpp/kinematics/MecanumDriveOdometry3dTest.cpp
View File

@@ -10,7 +10,7 @@
#include "wpi/math/kinematics/MecanumDriveOdometry3d.hpp"
#include "wpi/math/trajectory/TrajectoryGenerator.hpp"
using namespace frc;
using namespace wpi::math;
class MecanumDriveOdometry3dTest : public ::testing::Test {
protected:
@@ -22,15 +22,15 @@ class MecanumDriveOdometry3dTest : public ::testing::Test {
MecanumDriveWheelPositions zero;
MecanumDriveKinematics kinematics{m_fl, m_fr, m_bl, m_br};
MecanumDriveOdometry3d odometry{kinematics, frc::Rotation3d{}, zero};
MecanumDriveOdometry3d odometry{kinematics, wpi::math::Rotation3d{}, zero};
};
TEST_F(MecanumDriveOdometry3dTest, Initialize) {
MecanumDriveOdometry3d odometry{
kinematics, frc::Rotation3d{}, zero,
frc::Pose3d{1_m, 2_m, 0_m, frc::Rotation3d{0_deg, 0_deg, 45_deg}}};
kinematics, wpi::math::Rotation3d{}, zero,
wpi::math::Pose3d{1_m, 2_m, 0_m, wpi::math::Rotation3d{0_deg, 0_deg, 45_deg}}};
const frc::Pose3d& pose = odometry.GetPose();
const wpi::math::Pose3d& pose = odometry.GetPose();
EXPECT_NEAR(pose.X().value(), 1, 1e-9);
EXPECT_NEAR(pose.Y().value(), 2, 1e-9);
@@ -41,10 +41,10 @@ TEST_F(MecanumDriveOdometry3dTest, Initialize) {
TEST_F(MecanumDriveOdometry3dTest, MultipleConsecutiveUpdates) {
MecanumDriveWheelPositions wheelDeltas{3.536_m, 3.536_m, 3.536_m, 3.536_m};
odometry.ResetPosition(frc::Rotation3d{}, wheelDeltas, Pose3d{});
odometry.ResetPosition(wpi::math::Rotation3d{}, wheelDeltas, Pose3d{});
odometry.Update(frc::Rotation3d{}, wheelDeltas);
auto secondPose = odometry.Update(frc::Rotation3d{}, wheelDeltas);
odometry.Update(wpi::math::Rotation3d{}, wheelDeltas);
auto secondPose = odometry.Update(wpi::math::Rotation3d{}, wheelDeltas);
EXPECT_NEAR(secondPose.X().value(), 0.0, 0.01);
EXPECT_NEAR(secondPose.Y().value(), 0.0, 0.01);
@@ -54,12 +54,12 @@ TEST_F(MecanumDriveOdometry3dTest, MultipleConsecutiveUpdates) {
}
TEST_F(MecanumDriveOdometry3dTest, TwoIterations) {
odometry.ResetPosition(frc::Rotation3d{}, zero, Pose3d{});
odometry.ResetPosition(wpi::math::Rotation3d{}, zero, Pose3d{});
MecanumDriveWheelPositions wheelDeltas{0.3536_m, 0.3536_m, 0.3536_m,
0.3536_m};
odometry.Update(frc::Rotation3d{}, MecanumDriveWheelPositions{});
auto pose = odometry.Update(frc::Rotation3d{}, wheelDeltas);
odometry.Update(wpi::math::Rotation3d{}, MecanumDriveWheelPositions{});
auto pose = odometry.Update(wpi::math::Rotation3d{}, wheelDeltas);
EXPECT_NEAR(pose.X().value(), 0.3536, 0.01);
EXPECT_NEAR(pose.Y().value(), 0.0, 0.01);
@@ -68,12 +68,12 @@ TEST_F(MecanumDriveOdometry3dTest, TwoIterations) {
}
TEST_F(MecanumDriveOdometry3dTest, 90DegreeTurn) {
odometry.ResetPosition(frc::Rotation3d{}, zero, Pose3d{});
odometry.ResetPosition(wpi::math::Rotation3d{}, zero, Pose3d{});
MecanumDriveWheelPositions wheelDeltas{-13.328_m, 39.986_m, -13.329_m,
39.986_m};
odometry.Update(frc::Rotation3d{}, MecanumDriveWheelPositions{});
odometry.Update(wpi::math::Rotation3d{}, MecanumDriveWheelPositions{});
auto pose =
odometry.Update(frc::Rotation3d{0_deg, 0_deg, 90_deg}, wheelDeltas);
odometry.Update(wpi::math::Rotation3d{0_deg, 0_deg, 90_deg}, wheelDeltas);
EXPECT_NEAR(pose.X().value(), 8.4855, 0.01);
EXPECT_NEAR(pose.Y().value(), 8.4855, 0.01);
@@ -82,13 +82,13 @@ TEST_F(MecanumDriveOdometry3dTest, 90DegreeTurn) {
}
TEST_F(MecanumDriveOdometry3dTest, GyroAngleReset) {
odometry.ResetPosition(frc::Rotation3d{0_deg, 0_deg, 90_deg}, zero, Pose3d{});
odometry.ResetPosition(wpi::math::Rotation3d{0_deg, 0_deg, 90_deg}, zero, Pose3d{});
MecanumDriveWheelPositions wheelDeltas{0.3536_m, 0.3536_m, 0.3536_m,
0.3536_m};
auto pose =
odometry.Update(frc::Rotation3d{0_deg, 0_deg, 90_deg}, wheelDeltas);
odometry.Update(wpi::math::Rotation3d{0_deg, 0_deg, 90_deg}, wheelDeltas);
EXPECT_NEAR(pose.X().value(), 0.3536, 0.01);
EXPECT_NEAR(pose.Y().value(), 0.0, 0.01);
@@ -97,33 +97,33 @@ TEST_F(MecanumDriveOdometry3dTest, GyroAngleReset) {
}
TEST_F(MecanumDriveOdometry3dTest, AccuracyFacingTrajectory) {
frc::MecanumDriveKinematics kinematics{
frc::Translation2d{1_m, 1_m}, frc::Translation2d{1_m, -1_m},
frc::Translation2d{-1_m, -1_m}, frc::Translation2d{-1_m, 1_m}};
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::MecanumDriveOdometry3d odometry{kinematics, frc::Rotation3d{},
wpi::math::MecanumDriveOdometry3d odometry{kinematics, wpi::math::Rotation3d{},
wheelPositions};
frc::Trajectory trajectory = frc::TrajectoryGenerator::GenerateTrajectory(
std::vector{frc::Pose2d{0_m, 0_m, 45_deg}, frc::Pose2d{3_m, 0_m, -90_deg},
frc::Pose2d{0_m, 0_m, 135_deg},
frc::Pose2d{-3_m, 0_m, -90_deg},
frc::Pose2d{0_m, 0_m, 45_deg}},
frc::TrajectoryConfig(5.0_mps, 2.0_mps_sq));
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(5.0_mps, 2.0_mps_sq));
std::default_random_engine generator;
std::normal_distribution<double> distribution(0.0, 1.0);
units::second_t dt = 20_ms;
units::second_t t = 0_s;
wpi::units::second_t dt = 20_ms;
wpi::units::second_t t = 0_s;
double maxError = -std::numeric_limits<double>::max();
double errorSum = 0;
while (t < trajectory.TotalTime()) {
frc::Trajectory::State groundTruthState = trajectory.Sample(t);
wpi::math::Trajectory::State groundTruthState = trajectory.Sample(t);
auto wheelSpeeds = kinematics.ToWheelSpeeds(
{groundTruthState.velocity, 0_mps,
@@ -140,8 +140,8 @@ TEST_F(MecanumDriveOdometry3dTest, AccuracyFacingTrajectory) {
wheelPositions.rearRight += wheelSpeeds.rearRight * dt;
auto xhat = odometry.Update(
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}},
wheelPositions);
double error = groundTruthState.pose.Translation()
.Distance(xhat.Translation().ToTranslation2d())
@@ -160,33 +160,33 @@ TEST_F(MecanumDriveOdometry3dTest, AccuracyFacingTrajectory) {
}
TEST_F(MecanumDriveOdometry3dTest, AccuracyFacingXAxis) {
frc::MecanumDriveKinematics kinematics{
frc::Translation2d{1_m, 1_m}, frc::Translation2d{1_m, -1_m},
frc::Translation2d{-1_m, -1_m}, frc::Translation2d{-1_m, 1_m}};
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::MecanumDriveOdometry3d odometry{kinematics, frc::Rotation3d{},
wpi::math::MecanumDriveOdometry3d odometry{kinematics, wpi::math::Rotation3d{},
wheelPositions};
frc::Trajectory trajectory = frc::TrajectoryGenerator::GenerateTrajectory(
std::vector{frc::Pose2d{0_m, 0_m, 45_deg}, frc::Pose2d{3_m, 0_m, -90_deg},
frc::Pose2d{0_m, 0_m, 135_deg},
frc::Pose2d{-3_m, 0_m, -90_deg},
frc::Pose2d{0_m, 0_m, 45_deg}},
frc::TrajectoryConfig(5.0_mps, 2.0_mps_sq));
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(5.0_mps, 2.0_mps_sq));
std::default_random_engine generator;
std::normal_distribution<double> distribution(0.0, 1.0);
units::second_t dt = 20_ms;
units::second_t t = 0_s;
wpi::units::second_t dt = 20_ms;
wpi::units::second_t t = 0_s;
double maxError = -std::numeric_limits<double>::max();
double errorSum = 0;
while (t < trajectory.TotalTime()) {
frc::Trajectory::State groundTruthState = trajectory.Sample(t);
wpi::math::Trajectory::State groundTruthState = trajectory.Sample(t);
auto wheelSpeeds = kinematics.ToWheelSpeeds(
{groundTruthState.velocity * groundTruthState.pose.Rotation().Cos(),
@@ -204,7 +204,7 @@ TEST_F(MecanumDriveOdometry3dTest, AccuracyFacingXAxis) {
wheelPositions.rearRight += wheelSpeeds.rearRight * dt;
auto xhat = odometry.Update(
frc::Rotation3d{0_rad, 0_rad, distribution(generator) * 0.05_rad},
wpi::math::Rotation3d{0_rad, 0_rad, distribution(generator) * 0.05_rad},
wheelPositions);
double error = groundTruthState.pose.Translation()
.Distance(xhat.Translation().ToTranslation2d())