Transforming Euler angle to Quaternion
Hello, I have a new inertial sensor that provides the orientation of the robot in RPY. I want to load the information in an sensors_msgs/Imu ROS message using:
tf::Matrix3x3 obs_mat;
obs_mat.setEulerYPR(Yaw,Pitch,Roll);
tf::Quaternion q_tf;
obs_mat.getRotation(q_tf);
INS_msg.orientation.x = q_tf.getX();
INS_msg.orientation.y = q_tf.getY();
INS_msg.orientation.z = q_tf.getZ();
INS_msg.orientation.w = q_tf.getW();
The problem is when the yaw changes from +pi to -pi, the quaternion angle also has a "jump" in the angle from w=-0.99 to w=0.99 How can I transform the angle correctly?
Thanks,
What do you think would be the correct behaviour of the quaternion? Why do you think that a jump in the input should not produce a jump in the output?
The idea behind quaternion is that you avoid these jumps by adding an extra dimension
Note that there are always two possible unit quaternions that represent that same orientation. If q is thought of as an axis and a rotation about that axis then -q is an axis in the opposite direction with a rotation in the opposite direction (same overall rotation).
My guess the "jump" you are seeing is the flipping to the "negative" quaternion.