Clarification request: What does FloatingJointModel::distanceRotation return?

[werner291]

Hello,

the doc comment for FloatingJointModel::distanceRotation reads: /// Get the distance between the rotation components of two states

As implemented here, the method is implemented as: https://github.com/ros-planning/moveit2/blob/main/moveit_core/robot_model/src/floating_joint_model.cpp#L122

For convenience, here's the code in question:

double dq =
      fabs(values1[3] * values2[3] + values1[4] * values2[4] + values1[5] * values2[5] + values1[6] * values2[6]);
  if (dq + std::numeric_limits<double>::epsilon() >= 1.0)
    return 0.0;
  else
    return acos(dq);

From what I understand about quaternions, this formula indeed computes the angle between two quaternions, but I'm about 90% sure that this is not equivalent to the angle needed to rotate one transformation to match another. We may be missing a factor 2 (if that's indeed what the function does, which is not documented)

For instance, see the all of the factor 2's popping up left and right in this discussion: https://nl.mathworks.com/matlabcentral/answers/415936-angle-between-2-quaternions

[werner291]

For instance, this test case fails:

TEST(FloatingJointTest, distance_rotation_test) {

    // Create a simple floating joint model with some dummy parameters (these are not used by the test)
    moveit::core::FloatingJointModel fjm("joint", 0, 0);

    // We set some bounds where the joint position's translation component is bounded between -1 and 1 in all
    // dimensions. This is necessary, otherwise we just get (0,0,0) translations.
    moveit::core::JointModel::Bounds bounds;
    bounds = fjm.getVariableBounds();
    bounds[0].min_position_ = -1.0;
    bounds[0].max_position_ = 1.0;
    bounds[1].min_position_ = -1.0;
    bounds[1].max_position_ = 1.0;
    bounds[2].min_position_ = -1.0;
    bounds[2].max_position_ = 1.0;

    double jv1[7];
    double jv2[7];
    
    random_numbers::RandomNumberGenerator rng;

    for (size_t i = 0; i < 1000; ++i)
    {
        // Randomize the joint settings.
        fjm.getVariableRandomPositions(rng, jv1, bounds);
        
        Eigen::Isometry3d tf1;
        fjm.computeTransform(jv1, tf1);

        double angle = rng.uniformReal(-M_PI, M_PI);
        Eigen::AngleAxisd random_rotation(angle, Eigen::Vector3d::Random().normalized());

        Eigen::Quaterniond q2(tf1.rotation() * random_rotation);
        jv2[3] = q2.x();
        jv2[4] = q2.y();
        jv2[5] = q2.z();
        jv2[6] = q2.w();
        
        // Get the distances between the two joint configurations
        double d1 = fjm.distanceRotation(jv1, jv2);

        EXPECT_NEAR(abs(angle), d1, 1e-6);

    }
    
}

If I add a *2.0 to d1, it passes.

[henningkayser]

I think you are right, distanceRotation() computes the half angle. Btw, Eigen implements the alternative approach from this comment in the discussion you linked.

Maybe related: I wonder what has been the motivation implement the angular_distance_weight_ https://github.com/ros-planning/moveit2/blob/363af3e43defe8a6e0dd5ef935eb57aee6c1f169/moveit_core/robot_model/src/floating_joint_model.cpp#L42 which doesn't have any effect when set to 1.0. To me it's also odd that the maximum extend is limited to 90 degrees here. Shouldn't the maximum extend be at 180 degrees? The angular_distance_weight_ is also used in the distance() function. This makes me think that the distance weight could have been added to be able to switch between half and full angle representation for some reason. Anyway, distance() also has a redundant implementation of distanceRotation(), so if you touch this code, I'd ask you to implement it as the sum of distanceTranslation() and distanceRotation().

[werner291]

angular_distance_weight_ is just that, I think... the weight given to angular distance; maybe a robot needs to expend more or less energy to rotate in place, versus translate through space. There's no constructor parameter, but there is a setter for it.

Also, I'm pretty sure you accidentally linked old code there... the redundant implementation isn't there on the main branch.

As for this the maximum extent here... https://github.com/ros-planning/moveit2/blob/main/moveit_core/robot_model/src/floating_joint_model.cpp#L106

I'm not sure at all what the 0.5 factor is for... But it is correct if you leave out the 2.0 factor in the distanceRotation code... Without that 2.0 factor, the rotation distance will be at most PI/2.

Maybe someone just figured that using angle between quaternions was a perfectly valid way to measure distance between rotations? Which, let's face it... it is? Doing it this way perfectly well captures the idea that rotating more is generally more expensive... (Though I sincerely hope that the peer-reviewers who will scrutinize my paper will think the same.)

I think my issue with doing it this way is more that quaternions are more an implementation detail, and that a user (like me) would normally assume it's just about rotation angles?

[werner291]

Btw, possibly related issue #1478

[tylerjw]

I closed the PR with the test case as it seemed to be abandoned, feel free to re-open it.

• https://github.com/ros-planning/moveit2/pull/1529

[sea-bass]

We just landed https://github.com/ros-planning/moveit2/pull/2538 to use Eigen's angularDistance() implementation, as we ran into some incorrect results recently for some unit tests. What do you think?

In addition to the factor of 2, I think there is a -1 involved as well, see e.g., here: https://math.stackexchange.com/a/90098

So our lesson was: Trust Eigen :)

[sea-bass]

Closing this as we have fixed the issue. Please feel free to reopen if anything else is needed.