Rotate quaternion by body yaw
How to change a quaternion's body-fixed yaw?
Using Matrix3x3
's getRPY
and then setRPY
doesn't work, because this is about the axes, fixed to the initial orientation, so it's fixed yaw, not body yaw.
Using getEulerYPR
and then setEulerYPR
doesn't work as well, because yaw rotation it the first, and it gets affected the following pitch and roll rotation.
I think getEulerRPY
/setEulerRPY
might work, but there are no such functions in the tf
and tf2
libraries.