std::max mixes translational (m) and rotational (rad) errors in kdl_kinematics_plugin.cpp

[Tarkiya]

Description

During our academic research on automated dimensional analysis and physical unit consistency in C/C++ robotics projects, we identified a semantic dimensional mismatch in the KDL kinematics plugin (kdl_kinematics_plugin.cpp).
The code calculates a unified delta_twist_norm by taking the std::max() of a translational error and a rotational error. Mathematically and physically, comparing meters to radians via a max() function is a dimensional violation.

Location
moveit_kinematics/kdl_kinematics_plugin/src/kdl_kinematics_plugin.cpp (around line 444)
const double position_error = delta_twist.vel.Norm();
const double orientation_error = ik_solver.isPositionOnly() ? 0 : delta_twist.rot.Norm();
const double delta_twist_norm = std::max(position_error, orientation_error);

Expected behavior

if (position_error < epsilon && orientation_error < epsilon) {
// IK solved
}

Actual behavior

const double delta_twist_norm = std::max(position_error, orientation_error);
if (delta_twist_norm < epsilon) {
// IK solved
}

[Rudyy75]

Hi @Tarkiya and maintainers,

I would like to take this up. My plan is to decouple the convergence check to evaluate the positional and rotational norms independently.

I will also audit downstream logging to ensure removing delta_twist_norm doesn't break any debug outputs, and verify against the moveit_kinematics test suite.

Please let me know if I can be assigned.

[Tarkiya]

Hi @Rudyy75,

Thank you for looking into this and offering to take it up!

Your proposed approach to decouple the convergence check and evaluate the translational and rotational norms independently is exactly the right mathematical solution to fix this dimensional mismatch. It will definitely make the kinematics plugin much more robust.

I fully support you taking this on. Looking forward to your PR!