Document the difference between iDynTree::Rotation and Eigen::Quaterniond

[GiulioRomualdi]

The Eigen library already has the ability to manage rotation thanks to Eigen::Quaterniond . However the conversion from rotation matrix to Euler angles may lead to error for an iDynTree user. Indeed the asRPY() in iDynTree generates a 3d-vector organized as [roll pitch yaw] even if the original rotation is generated by RotZ(yaw) * RotY(pitch) * RotX(roll). On the other hand. in Eigen the conversion from rotation matrix to Euler angles is done by eulerAngles(). In this case eulerAngles(2, 1, 0) return a 3d-vector organized as [yaw pitch roll].

The following example should clarify better the problem

#include <iostream>

#include <Eigen/Geometry>

#include <iDynTree/Core/EigenHelpers.h>
#include <iDynTree/Core/Rotation.h>

int main ()
{
    Eigen::Matrix3d m;

    m = Eigen::AngleAxisd(0.25*M_PI, Eigen::Vector3d::UnitX())
        * Eigen::AngleAxisd(0.1*M_PI,  Eigen::Vector3d::UnitY())
        * Eigen::AngleAxisd(0.12*M_PI, Eigen::Vector3d::UnitZ());

    Eigen::Quaterniond eigenRot(m);

    iDynTree::Rotation iDynTreeRot;
    iDynTree::toEigen(iDynTreeRot) = m;

    std::cout << "rotation idyntree" << std::endl;
    std::cout <<iDynTreeRot.toString() << std::endl;

    std::cout << "rpy idyntree" << std::endl;
    std::cout << iDynTreeRot.asRPY().toString() << std::endl;

    std::cout << "rotation eigen" << std::endl;
    std::cout <<eigenRot.toRotationMatrix() << std::endl;

    std::cout << "rpy eigen" << std::endl;
    std::cout << eigenRot.toRotationMatrix().eulerAngles(2, 1, 0) << std::endl;
    
return 0;
}

and the output is

rotation idyntree
0.88427 -0.350107 0.309017
0.463467 0.577013 -0.672499
0.0571398 0.737889 0.672499

rpy idyntree
0.831729 -0.0571709 0.48276 

rotation eigen
  0.88427 -0.350107  0.309017
 0.463467  0.577013 -0.672499
0.0571398  0.737889  0.672499

rpy eigen
   0.48276
-0.0571709
  0.831729

Here the order of the angles given by iDynTree is roll pitch and yaw, while Eigen gives us yaw pitch roll.

I think we should keep in mind this when we use iDynTree::Rotation with Eigen::Quaterniond

cc @prashanthr05

[DanielePucci]

CC @robotology/iit-dynamic-interaction-control

[traversaro]

Thanks @GiulioRomualdi . Just FYI, the RPY convention used in iDynTree is exactly the one used in several robotics software and formats (URDF, KDL, OPC UA, etc etc).

[prashanthr05]

I also noticed inconsistent results when converting iDynTree Rotations directly to Eigen Quaternions using the asQuaternion() method.

Considering iDynRotation as a random iDynTree::Rotation object. I used to do

auto q = Eigen::Quaterniond(iDynTree::toEigen(iDynRotation.asQuaternion()));

This resulted in a different rotation matrix when calling q.toRotationMatrix(). This is possibly because since we are passing the quaternion as an array, we are populating the coeffs of the Eigen object. And the ordering for the coeffs is xyzw and not the expected ordering wxyz.

The consistent way to convert from iDynTree::Rotation to Eigen::Quaterniond seems to be through Eigen::AngleAxisd. So I do,

auto angleAxis = Eigen::AngleAxisd(iDynTree::toEigen(iDynRotation));
auto q = Eigen::Quaterniond(angleAxis);