Transforming Euler angle to Quaternion

Question

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,

---

Originally posted by Oded on ROS Answers with karma: 105 on 2017-01-12

Post score: 0

---

Original comments

Comment by NEngelhard on 2017-01-12:
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?

Comment by Oded on 2017-01-12:
The idea behind quaternion is that you avoid these jumps by adding an extra dimension

Comment by jarvisschultz on 2017-01-12:
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).

Comment by jarvisschultz on 2017-01-12:
My guess the "jump" you are seeing is the flipping to the "negative" quaternion.

Answer 1

A quaternion multiplied by any real number represents the same rotation. If they are assumed/enforced to be of unit length (pretty much always) that leaves you with two possible quaternion representations for any rotation, q and -q. If you want the result to always be the same, you can check which hemisphere the quaternion lies in and change the sign accordingly (i.e., if w < 0 then q = -q).

I think the only time the sign might be of interest is when interpolating between two quaternions (e.g., with SLERP). Depending on the respective signs of the two quaternions of interest you will get one of two interpolated paths (geodesics on the unit sphere in R4 representing all quaternions). One will be the "long" and the other the "short" path.

---

Originally posted by BrettHemes with karma: 525 on 2017-01-12

This answer was ACCEPTED on the original site

Post score: 2