Open GiulioRomualdi opened 4 years ago
CC @robotology/iit-dynamic-interaction-control
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).
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);
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 aniDynTree
user. Indeed theasRPY()
iniDynTree
generates a 3d-vector organized as[roll pitch yaw]
even if the original rotation is generated byRotZ(yaw) * RotY(pitch) * RotX(roll)
. On the other hand. in Eigen the conversion from rotation matrix to Euler angles is done byeulerAngles()
. In this caseeulerAngles(2, 1, 0)
return a 3d-vector organized as[yaw pitch roll]
.The following example should clarify better the problem
and the output is
Here the order of the angles given by
iDynTree
isroll
pitch
andyaw
, whileEigen
gives usyaw
pitch
roll
.I think we should keep in mind this when we use
iDynTree::Rotation
withEigen::Quaterniond
cc @prashanthr05