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