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-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

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

@@ -6,6 +6,7 @@ package edu.wpi.first.math.geometry;
import static org.junit.jupiter.api.Assertions.assertAll;
import static org.junit.jupiter.api.Assertions.assertEquals;
import static org.junit.jupiter.api.Assertions.assertFalse;
import static org.junit.jupiter.api.Assertions.assertNotEquals;
import edu.wpi.first.math.VecBuilder;
@@ -230,4 +231,26 @@ class Pose3dTest {
eps));
}
}
@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 twist = initialPose.log(finalPose);
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()));
}
}