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

4
wpimath/src/main/java/edu/wpi/first/math/geometry/Pose3d.java
View File

@@ -212,7 +212,7 @@ public class Pose3d implements Interpolatable<Pose3d> {
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<Pose3d> {
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!

4
wpimath/src/main/native/cpp/geometry/Pose3d.cpp
View File

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

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()));
}
}

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()));
}