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 log returning Twist3d NaN for theta between 1E-8 and 1E-7 (#5168)

Browse Source
This commit is contained in:
nt
2023-03-14 00:27:32 -04:00
committed by GitHub
parent 6d0c5b19db
commit 08f298e4cd
4 changed files with 64 additions and 33 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -149,19 +149,34 @@ TEST(Pose3dTest, ComplexTwists) {
}
TEST(Pose3dTest, TwistNaN) {
const Pose3d initial{6.32_m, 4.12_m, 0.00_m,
Rotation3d{Quaternion{-0.9999999999999999, 0.0, 0.0,
1.9208309264993548E-8}}};
const Pose3d final{6.33_m, 4.15_m, 0.00_m,
Rotation3d{Quaternion{-0.9999999999999999, 0.0, 0.0,
2.416890209039172E-8}}};
wpi::array<Pose3d, 2> initial_poses{
Pose3d{6.32_m, 4.12_m, 0.00_m,
Rotation3d{Quaternion{-0.9999999999999999, 0.0, 0.0,
1.9208309264993548E-8}}},
Pose3d{3.75_m, 2.95_m, 0.00_m,
Rotation3d{Quaternion{0.9999999999999793, 0.0, 0.0,
2.0352360299846772E-7}}},
};
auto twist = initial.Log(final);
wpi::array<Pose3d, 2> final_poses{
Pose3d{6.33_m, 4.15_m, 0.00_m,
Rotation3d{Quaternion{-0.9999999999999999, 0.0, 0.0,
2.416890209039172E-8}}},
Pose3d{3.66_m, 2.93_m, 0.00_m,
Rotation3d{Quaternion{0.9999999999999782, 0.0, 0.0,
2.0859477994905617E-7}}},
};
EXPECT_FALSE(std::isnan(twist.dx.value()));
EXPECT_FALSE(std::isnan(twist.dy.value()));
EXPECT_FALSE(std::isnan(twist.dz.value()));
EXPECT_FALSE(std::isnan(twist.rx.value()));
EXPECT_FALSE(std::isnan(twist.ry.value()));
EXPECT_FALSE(std::isnan(twist.rz.value()));
for (size_t i = 0; i < initial_poses.size(); i++) {
auto start = initial_poses[i];
auto end = final_poses[i];
auto twist = start.Log(end);
EXPECT_FALSE(std::isnan(twist.dx.value()));
EXPECT_FALSE(std::isnan(twist.dy.value()));
EXPECT_FALSE(std::isnan(twist.dz.value()));
EXPECT_FALSE(std::isnan(twist.rx.value()));
EXPECT_FALSE(std::isnan(twist.ry.value()));
EXPECT_FALSE(std::isnan(twist.rz.value()));
}
}