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

SCRIPT: wpiformat

Browse Source
This commit is contained in:
PJ Reiniger
2025-11-07 20:01:58 -05:00
committed by Peter Johnson
parent ae6bdc9d25
commit 2109161534
749 changed files with 5504 additions and 3936 deletions
Download Patch File Download Diff File Expand all files Collapse all files

8
wpimath/src/test/native/cpp/kinematics/SwerveDriveOdometry3dTest.cpp
View File

@@ -35,7 +35,8 @@ TEST_F(SwerveDriveOdometry3dTest, Initialize) {
m_kinematics,
wpi::math::Rotation3d{},
{zero, zero, zero, zero},
wpi::math::Pose3d{1_m, 2_m, 0_m, wpi::math::Rotation3d{0_deg, 0_deg, 45_deg}}};
wpi::math::Pose3d{1_m, 2_m, 0_m,
wpi::math::Rotation3d{0_deg, 0_deg, 45_deg}}};
const wpi::math::Pose3d& pose = odometry.GetPose();
@@ -140,8 +141,9 @@ TEST_F(SwerveDriveOdometry3dTest, AccuracyFacingTrajectory) {
br.angle = moduleStates[3].angle;
auto xhat = odometry.Update(
wpi::math::Rotation3d{groundTruthState.pose.Rotation() +
wpi::math::Rotation2d{distribution(generator) * 0.05_rad}},
wpi::math::Rotation3d{
groundTruthState.pose.Rotation() +
wpi::math::Rotation2d{distribution(generator) * 0.05_rad}},
{fl, fr, bl, br});
double error = groundTruthState.pose.Translation()
.Distance(xhat.Translation().ToTranslation2d())