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