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

48
wpimath/src/test/java/edu/wpi/first/math/geometry/Pose3dTest.java
View File

@@ -234,23 +234,39 @@ class Pose3dTest {
@Test
void testTwistNaN() {
var initialPose =
new Pose3d(
new Translation3d(6.32, 4.12, 0.00),
new Rotation3d(new Quaternion(-0.9999999999999999, 0.0, 0.0, 1.9208309264993548E-8)));
var finalPose =
new Pose3d(
new Translation3d(6.33, 4.15, 0.00),
new Rotation3d(new Quaternion(-0.9999999999999999, 0.0, 0.0, 2.416890209039172E-8)));
var initial_poses =
Arrays.asList(
new Pose3d(
new Translation3d(6.32, 4.12, 0.00),
new Rotation3d(
new Quaternion(-0.9999999999999999, 0.0, 0.0, 1.9208309264993548E-8))),
new Pose3d(
new Translation3d(3.75, 2.95, 0.00),
new Rotation3d(
new Quaternion(0.9999999999999793, 0.0, 0.0, 2.0352360299846772E-7))));
var final_poses =
Arrays.asList(
new Pose3d(
new Translation3d(6.33, 4.15, 0.00),
new Rotation3d(
new Quaternion(-0.9999999999999999, 0.0, 0.0, 2.416890209039172E-8))),
new Pose3d(
new Translation3d(3.66, 2.93, 0.00),
new Rotation3d(
new Quaternion(0.9999999999999782, 0.0, 0.0, 2.0859477994905617E-7))));
var twist = initialPose.log(finalPose);
for (int i = 0; i < initial_poses.size(); i++) {
var start = initial_poses.get(i);
var end = final_poses.get(i);
assertAll(
() -> assertFalse(((Double) twist.dx).isNaN()),
() -> assertFalse(((Double) twist.dy).isNaN()),
() -> assertFalse(((Double) twist.dz).isNaN()),
() -> assertFalse(((Double) twist.rx).isNaN()),
() -> assertFalse(((Double) twist.ry).isNaN()),
() -> assertFalse(((Double) twist.rz).isNaN()));
var twist = start.log(end);
assertAll(
() -> assertFalse(((Double) twist.dx).isNaN()),
() -> assertFalse(((Double) twist.dy).isNaN()),
() -> assertFalse(((Double) twist.dz).isNaN()),
() -> assertFalse(((Double) twist.rx).isNaN()),
() -> assertFalse(((Double) twist.ry).isNaN()),
() -> assertFalse(((Double) twist.rz).isNaN()));
}
}
}