Bug Report: `pinocchio::log6` fails with NaN

[oomcth]

🐞 Bug

When calling pinocchio::log6 on a near-identity rotation matrix (with very small off-diagonal elements), the function fails with an assertion error due to a NaN value in the computation of theta_nominal. The issue arises in log.hxx during the calculation of acos_expansion and math::acos(cos_value), where cos_value is slightly above 1.0 due to floating-point precision errors causing both acos_expansion and math::acos(cos_value) to be NaN.

Using std::clamp(cos_value, Scalar(-1 + 1e-10), Scalar(1 - 1e-10)) fixes the bug but it might not be a good way to fix it.

Reproduction steps

I ran the following code that loads diff.bin that contains the Rotation matrix and vector that cause an error.

diff.bin.zip

#include <Eigen/Dense>
#include <fstream>
#include <iostream>

void assertIsRotationMatrix(const Eigen::Matrix3d &R, double tol = 1e-9) {
  Eigen::Matrix3d shouldBeIdentity = R * R.transpose();
  std::setprecision(17);
  if (!shouldBeIdentity.isApprox(Eigen::Matrix3d::Identity(), tol)) {
    std::cerr << "Rotation matrix check failed: not orthogonal\n";
    assert(false);
  }
  double det = R.determinant();
  if (std::abs(det - 1.0) > tol) {
    std::cerr << "Rotation matrix check failed: det(R) = " << det << "\n";
    assert(false);
  }
}

void test_pinocchio_motion_log() {
  std::ifstream in("diff.bin", std::ios::binary);
  Eigen::Matrix3d R;
  Eigen::Vector3d p;
  in.read(reinterpret_cast<char *>(R.data()), sizeof(double) * 9);
  in.read(reinterpret_cast<char *>(p.data()), sizeof(double) * 3);
  in.close();
  std::cout << "rotation matrix" << R << std::endl;
  std::cout << "translation" << p << std::endl;
  assertIsRotationMatrix(R);
  pinocchio::SE3 diff(R, p);
  Eigen::VectorXd err;

  try {
    pinocchio::Motion log_motion = pinocchio::log6(diff);
    err = log_motion.toVector();

    bool has_nan = false;
    for (int i = 0; i < err.size(); ++i) {
      if (std::isnan(err[i])) {
        std::cout << "NaN detected at err[" << i << "]!" << std::endl;
        has_nan = true;
      }
    }

    if (!has_nan) {
      std::cout << "No NaN detected." << std::endl;
    }

  } catch (const std::exception &e) {
    std::cout << "Exception during log6: " << e.what() << std::endl;
  }
}
int main() {
  test_pinocchio_motion_log();
  return 0;
}

Relevant log output / Error message

❯ ./build/main
rotation matrix           1 -1.55186e-08  1.27117e-09
 1.55186e-08            1 -1.59884e-08
-1.27117e-09  1.59884e-08            1
translation-2.80705e-07
  2.0515e-07
-1.77342e-07
Assertion failed: (check_expression_if_real<Scalar>(theta_nominal == theta_nominal) && "theta contains some NaN"), function run, file log.hxx, line 121.
[1]    68704 abort      ./build/main
❯ lldb build/main
(lldb) target create "build/main"
Current executable set to '/Users/mscheffl/Desktop/pinocchio-minimal-main/build/main' (arm64).
(lldb) run
Process 69115 launched: '/Users/mscheffl/Desktop/pinocchio-minimal-main/build/main' (arm64)
rotation matrix           1 -1.55186e-08  1.27117e-09
 1.55186e-08            1 -1.59884e-08
-1.27117e-09  1.59884e-08            1
translation-2.80705e-07
  2.0515e-07
-1.77342e-07
Assertion failed: (check_expression_if_real<Scalar>(theta_nominal == theta_nominal) && "theta contains some NaN"), function run, file log.hxx, line 121.
Process 69115 stopped
* thread #1, queue = 'com.apple.main-thread', stop reason = hit program assert
    frame #4: 0x00000001000076d8 main`void pinocchio::log3_impl<double>::run<Eigen::Matrix<double, 3, 3, 0, 3, 3>, Eigen::Matrix<double, 3, 1, 0, 3, 1>>(R=<unavailable>, theta=0x000000016fdfd558, angle_axis=0x000000016fdfd560) at log.hxx:119:7 [opt]
   116 	          ),
   117 	        static_cast<Scalar>(acos_expansion) // else
   118 	      );
-> 119 	      assert(
   120 	        check_expression_if_real<Scalar>(theta_nominal == theta_nominal)
   121 	        && "theta contains some NaN"); // theta != NaN
   122 	
Target 0: (main) stopped.
warning: main was compiled with optimization - stepping may behave oddly; variables may not be available.
(lldb) bt
* thread #1, queue = 'com.apple.main-thread', stop reason = hit program assert
    frame #0: 0x000000018b216388 libsystem_kernel.dylib`__pthread_kill + 8
    frame #1: 0x000000018b24f88c libsystem_pthread.dylib`pthread_kill + 296
    frame #2: 0x000000018b158a3c libsystem_c.dylib`abort + 124
    frame #3: 0x000000018b157c70 libsystem_c.dylib`__assert_rtn + 284
  * frame #4: 0x00000001000076d8 main`void pinocchio::log3_impl<double>::run<Eigen::Matrix<double, 3, 3, 0, 3, 3>, Eigen::Matrix<double, 3, 1, 0, 3, 1>>(R=<unavailable>, theta=0x000000016fdfd558, angle_axis=0x000000016fdfd560) at log.hxx:119:7 [opt]
    frame #5: 0x0000000100006fbc main`void pinocchio::log6_impl<double>::run<double, 0, pinocchio::MotionTpl<double, 0>>(pinocchio::SE3Tpl<double, 0> const&, pinocchio::MotionDense<pinocchio::MotionTpl<double, 0>>&) [inlined] Eigen::Matrix<Eigen::Matrix<double, 3, 3, 0, 3, 3>::Scalar, 3, 1, Eigen::internal::plain_matrix_type<pinocchio::helper::argument_type<void (Eigen::Matrix<double, 3, 3, 0, 3, 3>)>::type>::type::Options, 3, 1> pinocchio::log3<Eigen::Matrix<double, 3, 3, 0, 3, 3>>(R=0x000000016fdfd620, theta=0x000000016fdfd558) at explog.hpp:88:5 [opt]
    frame #6: 0x0000000100006fb0 main`void pinocchio::log6_impl<double>::run<double, 0, pinocchio::MotionTpl<double, 0>>(M=0x000000016fdfd620, mout=0x000000016fdfd5f0) at log.hxx:208:23 [opt]
    frame #7: 0x000000010000531c main`test_pinocchio_motion_log() [inlined] pinocchio::MotionTpl<double, 0> pinocchio::log6<double, 0>(M=0x000000016fdfd620) at explog.hpp:439:5 [opt]
    frame #8: 0x0000000100005310 main`test_pinocchio_motion_log() at main.cpp:48:36 [opt]
    frame #9: 0x0000000100007768 main`main at main.cpp:68:3 [opt]
    frame #10: 0x000000018aeaeb98 dyld`start + 6076
(lldb)

System Info

MacBook Pro : 14 pouces, nov. 2024
Apple M4 Pro
MACOS Sequoia Version 15.6.1 (24G90)

I git clone the current pinocchio repo, devel branch (19/09/2025)
and build it and installed it in release.

I then build the above cpp code with
cmake .. -DCMAKE_BUILD_TYPE=Debug
and then ran main
I use clang :
clang version 20.1.5
Target: arm64-apple-darwin24.6.0
Thread model: posix

Checklist

• I have checked that all my packages are installed from the same package manager (conda-forge, Nix, PyPI, robotpkg, ROS, ...)
• I have checked that there is no similar issue/discussion in the repo
• I have provided a minimal and working example to reproduce the bug
• I have used Markdown code blocks for both code and stack traces/compiler errors.

[oomcth]

Additional Details

To provide further insight into the numerical behavior of the matrix, here are the detailed results of key computations:

Matrix Determinant

The determinant of matrix R is computed as:

std::cout << "det" << std::fixed << std::setprecision(17) << R.determinant()
            << std::endl;
det 1.00000000000000000

---

Orthogonality Check (R @ R.T)

The resulting matrix from R @ R.T shows minor floating-point precision errors:

R @ R.T =
[ 0.99999999999999989  -0.00000000000000005  -0.00000000000000005 ]
[ -0.00000000000000005   1.00000000000000022   0.00000000000000007 ]
[ -0.00000000000000005   0.00000000000000007   1.00000000000000022 ]

---

[jcarpent]

@ymontmarin will handle this issue

[ymontmarin]

The formal best solution would be to project R on SO(3) before doing the rest of log3.
Computationally it is a 3x3 SVD: R = U @ S @ Vt --> R_reg = U @ Vt

Differentiability through SVD is well-defined, but it is better to handle derivative manually. For pinocchio, it is ok as we have the Jlog3. In Jlog3 we would have to add a corrective term due to SVD chain rule.

What we could do is codegen the 3x3 SVD and use that.

Other than, that, clamping is one of the option and is OK to me when close to SO(3). We could also do Gram Schmidt orthonormal procedure or something else, even if it is a bit better it would not give the "closest R" in any sense.

@jcarpent what do you think ?