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

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-8) {
if (Math.abs(theta) < 1E-7) {
// 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-8) {
if (Math.abs(theta) < 1E-7) {
// 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-8) {
if (std::abs(theta) < 1E-7) {
// 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-8) {
if (std::abs(theta) < 1E-7) {
// Taylor Expansions around θ = 0
// A = 1/1! - θ²/3! + θ⁴/5!
// B = 1/2! - θ²/4! + θ⁴/6!

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

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

@@ -149,19 +149,34 @@ TEST(Pose3dTest, ComplexTwists) {
}
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}}};
wpi::array<Pose3d, 2> initial_poses{
Pose3d{6.32_m, 4.12_m, 0.00_m,
Rotation3d{Quaternion{-0.9999999999999999, 0.0, 0.0,
1.9208309264993548E-8}}},
Pose3d{3.75_m, 2.95_m, 0.00_m,
Rotation3d{Quaternion{0.9999999999999793, 0.0, 0.0,
2.0352360299846772E-7}}},
};
auto twist = initial.Log(final);
wpi::array<Pose3d, 2> final_poses{
Pose3d{6.33_m, 4.15_m, 0.00_m,
Rotation3d{Quaternion{-0.9999999999999999, 0.0, 0.0,
2.416890209039172E-8}}},
Pose3d{3.66_m, 2.93_m, 0.00_m,
Rotation3d{Quaternion{0.9999999999999782, 0.0, 0.0,
2.0859477994905617E-7}}},
};
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()));
for (size_t i = 0; i < initial_poses.size(); i++) {
auto start = initial_poses[i];
auto end = final_poses[i];
auto twist = start.Log(end);
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()));
}
}