Bug Report: `KDL::diff` Returns Non-Zero Orientation Error for Identical Frames

[my-rice]

Description

The function KDL::Twist KDL::diff(const KDL::Frame &F_a_b1, const KDL::Frame &F_a_b2, double dt = 1.0) returns a non-zero orientation error when called with two identical frames. This is unexpected, as the orientation difference between identical frames should be zero.

Steps to Reproduce

To reproduce the issue, run the following code snippet:

KDL::Frame f1{ KDL::Frame(
    KDL::Rotation::Quaternion(-0.728750, 0.129740, -0.190550, 0.644808),
    KDL::Vector(0.042641, 0.618020, 0.595680)
) };

KDL::Twist error = KDL::diff(f1, f1);
std::cout << "Error: " << error(0) << " " << error(1) << " " << error(2)
          << " " << error(3) << " " << error(4) << " " << error(5) << std::endl;

Expected Behavior

Since f1 is passed as both inputs, the expected output is a zero twist (i.e., both translational and rotational parts should be zero): Error: 0 0 0 0 0 0.

Actual Behavior

The actual output is: Error: 0 0 0 1.39814 2.79715 -0.301034. The translational part is correctly zero, but the rotational part is non-zero, which is incorrect for identical frames.

Analysis

After inspecting the implementation of KDL::diff, it seems that the problem lies in the way the rotation difference is computed internally. Specifically, the call to:

angle = Rotation::GetRotAngle(axis, epsilon);

uses a very small default value for epsilon, which can lead to numerical instability or excessive sensitivity.

Even when comparing two identical frames, small numerical discrepancies can arise during intermediate steps, such as converting quaternions to rotation matrices or computing relative rotations. These floating-point approximations may cause GetRotAngle to return a non-zero rotation axis and angle, resulting in an incorrect non-zero orientation error in the final twist output.

This may be due to:

• Precision loss when converting from quaternions to rotation matrices, or an intermediate step.

• The tolerance (epsilon) in GetRotAngle being too strict for practical equality.

[MatthijsBurgh]

I think currently the precision used is 1e-6. Do you agree? This an unusual value IMO. The interfaces could be extended so all relevant functions accept an precision argument. Though this might be some work

[my-rice]

I can confirm that epsilon is currently set to 1e-6, and I also find this value to be too strict for my use case.

The interfaces could be extended so all relevant functions accept an precision argument.

While this could be a reasonable approach, I’m not sure it’s the best solution. It's already possible to adjust the precision globally in code. For example:

KDL::epsilon = 1e-4;

However, many existing functions already follow the approach you suggest, allowing precision to be set per function call. Aligning all interfaces to this pattern would be the most consistent solution IMO.