[Bug] Possible sign error in beta_alt Taylor expansion for Quaternion overload of log6

[BeichenZhuo]

🐞 Bug

[Bug] Possible sign error in beta_alt Taylor expansion for Quaternion overload of log6

Description

I believe I have identified a potential sign error in the Taylor series expansion used for the beta coefficient in the SE(3) logarithmic map, specifically in the small-angle branch of the log6_impl::run function.

Mathematical Background

The beta coefficient in the SE(3) inverse left Jacobian is defined as:

$$ \beta(\theta) = \frac{1}{\theta^2} - \frac{1}{2\theta}\cot\left(\frac{\theta}{2}\right) $$

Its Taylor expansion around (\theta = 0) is:

$$ \beta(\theta) = \frac{1}{12} + \frac{\theta^2}{720} + \frac{\theta^4}{30240} + \mathcal{O}(\theta^6) $$

Suspected Issue Location

In the run function (file: include/pinocchio/src/spatial/log.hxx ) , the small-angle approximation is computed as:

    const Scalar th_2_squared = t2 / Scalar(4); // (theta / 2) squared
    const Scalar beta_alt = (Scalar(1) / Scalar(3) - th_2_squared / Scalar(45)) / Scalar(4);

Since t2 = θ², we have th_2_squared = θ²/4. Substituting:

$$ \beta_{\text{alt}} = \frac{1}{4}\left(\frac{1}{3} - \frac{\theta^2/4}{45}\right) = \frac{1}{12} - \frac{\theta^2}{720} $$

This yields:

$$ \beta_{\text{alt}} = \frac{1}{12} - \frac{\theta^2}{720} $$

However, the correct mathematical expansion is:

$$ \beta(\theta) = \frac{1}{12} + \frac{\theta^2}{720} + \cdots $$

The sign of the (\theta^2) term appears to be inverted (negative instead of positive).

Suggested Fix

If confirmed, the fix would be to change the sign in beta_alt:

// Current (suspect):
const Scalar beta_alt = (Scalar(1) / Scalar(3) - th_2_squared / Scalar(45)) / Scalar(4);

// Proposed:
const Scalar beta_alt = (Scalar(1) / Scalar(3) + th_2_squared / Scalar(45)) / Scalar(4);

This would yield:

$$ \beta_{\text{alt}} = \frac{1}{12} + \frac{\theta^2}{720} $$

which matches the mathematical expansion.

---

Thank you for considering this report.

[jorisv]

Hello @BeichenZhuo,

Do you think this can be related to #2768 ?

[jorisv]

You have delete the checklist in the bug report template. You must respect as much as possible the template as stated here.

You can try to reproduce the issue in a python script to show how much numeric error it generate.

[nim65s]

Nice, but out of curiosity, why did you investigate that ? Do you have any issue in a downstream application ?

[BeichenZhuo]

Hello @BeichenZhuo,

Do you think this can be related to #2768 ?

Hello @jcarpent,

I performed a step-by-step debugging of #2768 and found that the issue uses the rotation matrix version of log6, not the quaternion version. Therefore, the Taylor expansion sign issue I mentioned in the quaternion version is not related.

Moreover, the example from #2768 does not trigger the assertion on my machine. Here is the output from my test:

rotation matrix 1 -1.52788e-08 2.91212e-09
1.52788e-08 1 -1.92022e-08
-2.91212e-09 1.92022e-08 1
translation-2.85717e-07
2.41952e-07
-1.60283e-07
No NaN detected.

System Info

• OS: Ubuntu 24.04.4 LTS (running on WSL2)
• Kernel: 6.18.33.2-microsoft-standard-WSL2 (x86_64)
• Compiler: gcc (Ubuntu 13.3.0-6ubuntu2~24.04.1) 13.3.0
• Pinocchio: Cloned from current repo, devel branch, commit eac765ef186da6714a6d5f8ed651f6b96718e9eb
• Build: Configured with -DCMAKE_BUILD_TYPE=Debug

[BeichenZhuo]

You have delete the checklist in the bug report template. You must respect as much as possible the template as stated here.

You can try to reproduce the issue in a python script to show how much numeric error it generate.

Thanks for the suggestion. I have prepared a minimal C++ program that reproduces Pinocchio's current log6 (quaternion version) with the exact same logic, changing only the sign in beta_alt from - (current) to + (mathematically correct). The code and the results are attached below.

#include <iostream>
#include <iomanip>
#include <vector>
#include "pinocchio/spatial.hpp"

using Scalar = long double;
using Vector3 = Eigen::Matrix<Scalar, 3, 1>;
using Quaternion = Eigen::Quaternion<Scalar>;
using SE3 = pinocchio::SE3Tpl<Scalar>;
using Motion = pinocchio::MotionTpl<Scalar>;

Motion log6_buggy(const Quaternion& quat, const Vector3& vec, Scalar& beta) {
    const Scalar eps = Eigen::NumTraits<Scalar>::epsilon();

    const Scalar pos_neg = (quat.w() >= Scalar(0)) ? Scalar(+1) : Scalar(-1);

    Scalar theta;
    Vector3 w = pinocchio::quaternion::log3(quat, theta);
    const Scalar t2 = w.squaredNorm();

    Scalar st_2, ct_2;
    ct_2 = pos_neg * quat.w();
    st_2 = std::sqrt(quat.vec().squaredNorm() + eps * eps);
    const Scalar cot_th_2 = ct_2 / st_2;

    const Scalar th_2_squared = t2 / Scalar(4);

    // Current implementation (sign is negative)
    const Scalar beta_alt = (Scalar(1) / Scalar(3) - th_2_squared / Scalar(45)) / Scalar(4);

    beta = (theta <= pinocchio::TaylorSeriesExpansion<Scalar>::template precision<3>())
                        ? beta_alt
                        : (Scalar(1) / t2 - cot_th_2 * Scalar(0.5) / theta);

    Motion mout;
    mout.linear().noalias() = vec - Scalar(0.5) * w.cross(vec) + beta * w.cross(w.cross(vec));
    mout.angular() = w;

    return mout;
}

Motion log6_fixed(const Quaternion& quat, const Vector3& vec, Scalar& beta) {
    const Scalar eps = Eigen::NumTraits<Scalar>::epsilon();

    const Scalar pos_neg = (quat.w() >= Scalar(0)) ? Scalar(+1) : Scalar(-1);

    Scalar theta;
    Vector3 w = pinocchio::quaternion::log3(quat, theta);
    const Scalar t2 = w.squaredNorm();

    Scalar st_2, ct_2;
    ct_2 = pos_neg * quat.w();
    st_2 = std::sqrt(quat.vec().squaredNorm() + eps * eps);
    const Scalar cot_th_2 = ct_2 / st_2;

    const Scalar th_2_squared = t2 / Scalar(4);

    // Fixed implementation (sign is positive, matches Taylor expansion)
    const Scalar beta_alt = (Scalar(1) / Scalar(3) + th_2_squared / Scalar(45)) / Scalar(4);

    beta = (theta <= pinocchio::TaylorSeriesExpansion<Scalar>::template precision<3>())
                        ? beta_alt
                        : (Scalar(1) / t2 - cot_th_2 * Scalar(0.5) / theta);

    Motion mout;
    mout.linear().noalias() = vec - Scalar(0.5) * w.cross(vec) + beta * w.cross(w.cross(vec));
    mout.angular() = w;

    return mout;
}

void test_comparison(Scalar theta, const Vector3& axis, const Vector3& vec) {
    Quaternion quat(Eigen::AngleAxis<Scalar>(theta, axis.normalized()));
    Scalar beta_buggy, beta_fixed;

    Motion m_buggy = log6_buggy(quat, vec, beta_buggy);
    Motion m_fixed = log6_fixed(quat, vec, beta_fixed);

    auto diff = m_buggy.linear() - m_fixed.linear();
    Scalar lin_diff = diff.norm();
    Scalar beta_diff = beta_buggy - beta_fixed;

    std::cout << std::scientific << std::setprecision(6);
    std::cout << "theta = " << std::setw(10) << theta << " | "
              << "lin_diff = " << std::setw(12) << lin_diff << " | "
              << "beta_diff = " << std::setw(12) << beta_diff << "\n";
}

int main() {
    std::cout << "Comparing log6 with beta_alt sign: '-' (buggy) vs '+' (fixed)\n";
    std::cout << "================================================================\n";
    std::cout << std::scientific << std::setprecision(6);

    Vector3 axis = Vector3(1, 0, 0).normalized();
    Vector3 vec = Vector3(10e20, 20e20, 30e20);  // Deliberately large

    std::vector<Scalar> thetas = {
        1e-8, 5e-8, 1e-7, 5e-7, 1e-6, 5e-6,
        1e-5, 5e-5
    };

    std::cout << "    theta     |   lin_diff    |   beta_diff   \n";
    std::cout << "--------------+---------------+---------------+ \n";

    for (Scalar theta : thetas) {
        test_comparison(theta, axis, vec);
    }

    std::cout << "\nFine scan near Taylor threshold (1e-5):\n";
    std::cout << "    theta     |   lin_diff    |   beta_diff   \n";
    std::cout << "--------------+---------------+---------------+ \n";
    for (Scalar theta = 1e-6; theta <= 1e-5; theta += 5e-7) {
        test_comparison(theta, axis, vec);
    }

    return 0;
}

Test conditions:

• Axis: (1,0,0)
• Translation vector: vec = (1e20, 2e20, 3e20) (deliberately large to amplify the difference)
• Angles θ scanned from 1e-8 to 5e-5

Output:

Comparing log6 with beta_alt sign: '-' (buggy) vs '+' (fixed)
================================================================
    theta     |   lin_diff    |   beta_diff   
--------------+---------------+---------------+ 
theta = 1.000000e-08 | lin_diff = 0.000000e+00 | beta_diff = -2.710505e-19
theta = 5.000000e-08 | lin_diff = 0.000000e+00 | beta_diff = -6.938894e-18
theta = 1.000000e-07 | lin_diff = 0.000000e+00 | beta_diff = -2.778268e-17
theta = 5.000000e-07 | lin_diff = 0.000000e+00 | beta_diff = -6.944450e-16
theta = 1.000000e-06 | lin_diff = 2.560000e+02 | beta_diff = -2.777780e-15
theta = 5.000000e-06 | lin_diff = 2.560000e+02 | beta_diff = -6.944445e-14
theta = 1.000000e-05 | lin_diff = 0.000000e+00 | beta_diff = -2.777778e-13
theta = 5.000000e-05 | lin_diff = 0.000000e+00 | beta_diff = 0.000000e+00

Fine scan near Taylor threshold (1e-5):
    theta     |   lin_diff    |   beta_diff   
--------------+---------------+---------------+ 
theta = 1.000000e-06 | lin_diff = 2.560000e+02 | beta_diff = -2.777780e-15
theta = 1.500000e-06 | lin_diff = 1.280000e+02 | beta_diff = -6.250005e-15
theta = 2.000000e-06 | lin_diff = 0.000000e+00 | beta_diff = -1.111111e-14
theta = 2.500000e-06 | lin_diff = 0.000000e+00 | beta_diff = -1.736111e-14
theta = 3.000000e-06 | lin_diff = 2.560000e+02 | beta_diff = -2.499999e-14
theta = 3.500000e-06 | lin_diff = 0.000000e+00 | beta_diff = -3.402778e-14
theta = 4.000000e-06 | lin_diff = 0.000000e+00 | beta_diff = -4.444444e-14
theta = 4.500000e-06 | lin_diff = 1.280000e+02 | beta_diff = -5.624999e-14
theta = 5.000000e-06 | lin_diff = 2.560000e+02 | beta_diff = -6.944445e-14
theta = 5.500000e-06 | lin_diff = 0.000000e+00 | beta_diff = -8.402778e-14
theta = 6.000000e-06 | lin_diff = 0.000000e+00 | beta_diff = -1.000000e-13
theta = 6.500000e-06 | lin_diff = 0.000000e+00 | beta_diff = -1.173611e-13
theta = 7.000000e-06 | lin_diff = 2.560000e+02 | beta_diff = -1.361111e-13
theta = 7.500000e-06 | lin_diff = 1.280000e+02 | beta_diff = -1.562500e-13
theta = 8.000000e-06 | lin_diff = 0.000000e+00 | beta_diff = -1.777778e-13
theta = 8.500000e-06 | lin_diff = 0.000000e+00 | beta_diff = -2.006944e-13
theta = 9.000000e-06 | lin_diff = 2.560000e+02 | beta_diff = -2.250000e-13
theta = 9.500000e-06 | lin_diff = 0.000000e+00 | beta_diff = -2.506944e-13
theta = 1.000000e-05 | lin_diff = 0.000000e+00 | beta_diff = -2.777778e-13

1. beta_diff grows as θ²/360 (theoretical expectation) when the Taylor branch is active.

2. lin_diff is zero for most of vec, but becomes nonzero (up to 2.56e2) when vec is extremely large.

3. However, the relative error lin_diff / ||vec|| is only about 2.56e2 / (3.74e20) ≈ 6.8e-19, which is below double precision. For physically meaningful vec (e.g., ||vec|| ≈ 1 or 10), the difference remains at machine precision.

Interpretation:

• The sign error is mathematically real and can be amplified by artificially huge vec.
• But for any realistic robotics application (where positions are in meters), the error is completely negligible.

📋 Checklist (per the bug report template)

• I have read the CONTRIBUTING guidelines.
• I have checked that this issue is not a duplicate.
• I have provided a minimal, self‑contained code example that reproduces the behaviour.
• I have included the version of Pinocchio, compiler, and operating system.
• I have described the expected behaviour and the actual observed behaviour.
• I have explained why this is a potential issue (mathematical correctness) and what impact it may have.

Given that:

• The sign error only affects the Taylor approximation for β, used only when θ < ~1e-5.
• Even with unrealistically large vec (1e20), the absolute error in linear velocity is only ~2.56e2, which is below the noise level of any physical system.
• For normal-scale inputs, the difference is completely invisible.

I leave it up to the maintainers to decide whether to apply the trivial fix (change - to + in beta_alt). I am happy to provide a pull request if desired, but I want to emphasize that this is not a critical bug — purely a mathematical curiosity.

[BeichenZhuo]

Nice, but out of curiosity, why did you investigate that ? Do you have any issue in a downstream application ?

@jcarpent Thanks for the question!

I came across this while I was studying the SE(3) logarithm implementation in Pinocchio — partly for my own learning, and also because I was thinking about using it in a control application where numerical precision matters.

To be honest, I haven't encountered any real failure or abnormal behaviour in a downstream application — this is purely a theoretical inconsistency. I just thought it was worth reporting for mathematical correctness. If you consider it harmless, I'm perfectly fine with closing this as “won't fix”. I mainly wanted to bring it to your attention.