Incorrect Roll,Pitch, Yaw values using getRPY()

edit

This is likely due to a difference in how "Roll, Pitch, Yaw" are defined. It seems simple, but there is really no standard convention for what those terms mean. Usually, each term means a rotation about one of the coordinate axes (XYZ), but the order these are applies in varies by convention. And order is significant, in this case! To be further confusing, sometimes the angles are explained as rotations about fixed frames, and other times as rotations about the transformed frames. In fact, there are 24 different conventions for how to represent a rotation in terms of Euler Angles.

This is why it's sometimes "safer" to represent orientation (or rotation) using a full 3x3 rotation matrix or a 1x4 Quaternion. These are unambiguous, though slightly less "intuitive".

The ROS getRPY() function you're using above uses the following convention:

1. rotate about fixed X-axis by Roll
2. rotate about fixed Y-axis by Pitch
3. rotate about fixed Z-axis by Yaw

On the other hand, this discussion on the subject seems to suggest that the Optitrak Tracking Tools software uses different axis directions. In particular, Y & Z are swapped.

In fact, I see a similar swap in your other post:

btVector3 pos(t.pos[0], -t.pos[2], t.pos[1]);

I suggest that you perform a similar swap in your quaternion:

btQuaternion q_orig(t.quat[0], -t.quat[2], t.quat[1], t.quat[3]);

Maybe that at least points you in the right direction!