Quaternion multiplication for orientation update yielding incorrect values
My robot needs to update its orientation from its previous orientation (represented by quaternion q_orig) to its updated orientation (represented by quaternion q_new) by multiplying with a rotation matrix (represented by quaternion q_rot). This q_rot captures the radians rotated since the last received information. I have angular velocity value (represented by Euler angles rot_rate) and time since the last update (represented by dt) which is very small. I use the following steps:
float rot_x = rot_rate.x * dt;
float rot_y = rot_rate.y * dt;
float rot_z = rot_rate.z * dt;
q_rot.setRPY(rot_x, rot_y, rot_z);
q_new = q_rot * q_orig;
q_new.normalize();
However, q_new values seem to be incorrect. I verified it by comparing the visuals of robot rotation with the values of q_new on rviz. Please suggest what could be wrong or comment if I should add more information.
Without really studying your problem, it feels to me like the
q_newmultiplication is in the wrong order. Have you tried swappingq_rotandq_orig?@jarvisschultz, I did. But it still yields incorrect results.
As per the question text, rot_rate are Euler angles, which means rot_x, rot_y and rot_z are Euler angles. However, quaternion is set using the API setRPY which requires fixed axis angles. You should use another API of quaternion that takes Euler angle values.