From 099d048d9e940c11a4f86035752624e28eb3a910 Mon Sep 17 00:00:00 2001 From: nt <89554543+7910f6ba7ee4@users.noreply.github.com> Date: Sun, 26 Feb 2023 19:41:36 -0500 Subject: [PATCH] [wpimath] Fix Pose3d log returning Twist3d NaN for theta between 1E-9 and 1E-8 (#5143) Co-authored-by: Tyler Veness --- .../edu/wpi/first/math/geometry/Pose3d.java | 4 ++-- .../src/main/native/cpp/geometry/Pose3d.cpp | 4 ++-- .../wpi/first/math/geometry/Pose3dTest.java | 23 +++++++++++++++++++ .../test/native/cpp/geometry/Pose3dTest.cpp | 18 +++++++++++++++ 4 files changed, 45 insertions(+), 4 deletions(-) diff --git a/wpimath/src/main/java/edu/wpi/first/math/geometry/Pose3d.java b/wpimath/src/main/java/edu/wpi/first/math/geometry/Pose3d.java index 9926b33465..67ed33fc7f 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/geometry/Pose3d.java +++ b/wpimath/src/main/java/edu/wpi/first/math/geometry/Pose3d.java @@ -212,7 +212,7 @@ public class Pose3d implements Interpolatable { double A; double B; double C; - if (Math.abs(theta) < 1E-9) { + if (Math.abs(theta) < 1E-8) { // Taylor Expansions around θ = 0 // A = 1/1! - θ²/3! + θ⁴/5! // B = 1/2! - θ²/4! + θ⁴/6! @@ -268,7 +268,7 @@ public class Pose3d implements Interpolatable { final var thetaSq = theta * theta; double C; - if (Math.abs(theta) < 1E-9) { + if (Math.abs(theta) < 1E-8) { // Taylor Expansions around θ = 0 // A = 1/1! - θ²/3! + θ⁴/5! // B = 1/2! - θ²/4! + θ⁴/6! diff --git a/wpimath/src/main/native/cpp/geometry/Pose3d.cpp b/wpimath/src/main/native/cpp/geometry/Pose3d.cpp index 7eef6d5c9a..c17c08f035 100644 --- a/wpimath/src/main/native/cpp/geometry/Pose3d.cpp +++ b/wpimath/src/main/native/cpp/geometry/Pose3d.cpp @@ -82,7 +82,7 @@ Pose3d Pose3d::Exp(const Twist3d& twist) const { double A; double B; double C; - if (std::abs(theta) < 1E-9) { + if (std::abs(theta) < 1E-8) { // Taylor Expansions around θ = 0 // A = 1/1! - θ²/3! + θ⁴/5! // B = 1/2! - θ²/4! + θ⁴/6! @@ -134,7 +134,7 @@ Twist3d Pose3d::Log(const Pose3d& end) const { double thetaSq = theta * theta; double C; - if (std::abs(theta) < 1E-9) { + if (std::abs(theta) < 1E-8) { // Taylor Expansions around θ = 0 // A = 1/1! - θ²/3! + θ⁴/5! // B = 1/2! - θ²/4! + θ⁴/6! diff --git a/wpimath/src/test/java/edu/wpi/first/math/geometry/Pose3dTest.java b/wpimath/src/test/java/edu/wpi/first/math/geometry/Pose3dTest.java index f317066357..272b3987bb 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/geometry/Pose3dTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/geometry/Pose3dTest.java @@ -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())); + } } diff --git a/wpimath/src/test/native/cpp/geometry/Pose3dTest.cpp b/wpimath/src/test/native/cpp/geometry/Pose3dTest.cpp index 2ed74158c3..d05518d5bb 100644 --- a/wpimath/src/test/native/cpp/geometry/Pose3dTest.cpp +++ b/wpimath/src/test/native/cpp/geometry/Pose3dTest.cpp @@ -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())); +}