Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-19 00:41:43 +00:00
Code Issues Packages Projects Releases Wiki Activity

[wpimath] Fix Pose3d transformBy rotation type (#4545)

Browse Source 
Co-authored-by: Ryan Blue <ryanzblue@gmail.com>
This commit is contained in:
Griffin Della Grotte
2022-11-07 09:57:33 -08:00
committed by GitHub
parent a4054d702f
commit b1b4c1e9e7
6 changed files with 82 additions and 4 deletions
Download Patch File Download Diff File Expand all files Collapse all files

23
wpimath/src/test/native/cpp/geometry/Pose3dTest.cpp
View File

@@ -9,6 +9,29 @@
using namespace frc;
TEST(Pose3dTest, TestTransformByRotations) {
const double kEpsilon = 1E-9;
const Pose3d initialPose{0_m, 0_m, 0_m, Rotation3d{0_deg, 0_deg, 0_deg}};
const Transform3d transform1{Translation3d{0_m, 0_m, 0_m},
Rotation3d{90_deg, 45_deg, 0_deg}};
const Transform3d transform2{Translation3d{0_m, 0_m, 0_m},
Rotation3d{-90_deg, 0_deg, 0_deg}};
const Transform3d transform3{Translation3d{0_m, 0_m, 0_m},
Rotation3d{0_deg, -45_deg, 0_deg}};
Pose3d finalPose = initialPose.TransformBy(transform1)
.TransformBy(transform2)
.TransformBy(transform3);
EXPECT_NEAR(finalPose.Rotation().X().value(),
initialPose.Rotation().X().value(), kEpsilon);
EXPECT_NEAR(finalPose.Rotation().Y().value(),
initialPose.Rotation().Y().value(), kEpsilon);
EXPECT_NEAR(finalPose.Rotation().Z().value(),
initialPose.Rotation().Z().value(), kEpsilon);
}
TEST(Pose3dTest, TransformBy) {
Eigen::Vector3d zAxis{0.0, 0.0, 1.0};