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