I'm trying to control the trajectory of the orientation of an end-effector of a robot, to do so I need to work with axis angle representation.
First I obtain the Rotation matrix (Rot) from the actual pose (Ri) to the final pose (Rf). And then I build the Eigen::AngleAxisd object from the rotation matrix. But when I check the values of the Eigen::AngleAxisd they are not correct.
I have tried to convert back to a matrix representation and print the results from the original Matrix rotation and the rotation extracted from the AxisAngle object. But I get different values.
The part of the code:
Eigen::Matrix3d Rot = Ri.transpose() * Rf;
Eigen::AngleAxisd ax_rot;
ax_rot.fromRotationMatrix(Rot);
std::cout << "The relative rot before:\n" << Rot << std::endl;
std::cout << "The relative rot after:\n" << ax_rot.matrix() << std::endl;
The output:
The relative rot before:
0.707107 -0.707107 -1.91864e-17
0.707107 -0.707107 1.91861e-17
1.05879e-22 0 1
The relative rot after:
0.333333 -0.942809 -2.18354e-17
0.942809 0.333333 3.74618e-18
3.74654e-18 -2.18353e-17 1
The The relative rot before: is correct but the The relative rot after: should be the same. Does anybody know how to convert properly from Matrix3d to AxisAngled representation?