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-9 and 1E-8 (#5143)

Browse Source 
Co-authored-by: Tyler Veness <calcmogul@gmail.com>
This commit is contained in:
nt
2023-02-26 19:41:36 -05:00
committed by GitHub
parent 4af84a1c12
commit 099d048d9e
4 changed files with 45 additions and 4 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -147,3 +147,21 @@ TEST(Pose3dTest, ComplexTwists) {
end.Rotation().GetQuaternion().Z(), eps);
}
}
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}}};
auto twist = initial.Log(final);
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()));
}