Quaternion to Yaw pitch roll

Question

I am trying to convert a quaternion to yaw pitch roll euler angles. I am experiencing problems with the gimbal lock. The first strange thing that occurs is that errors already start to appear when the pitch angle is in the neighbourhood of +-pi/2. I thought problems should only occur at exactly pi/2.

Secondly my code shows an incorrect yaw angle of 180 degrees at a pitch of -90 degrees.

I tried the code at this post and from this site but none of them worked. I also tried the pyquaternion library, but this one does not even attempt to compensate for gimbal lock. In the end I made the python equivalent of this section of a handbook.

This worked the best but still gives the above issues. It seems like a problem that must have been solved a 1000 times but I can't pinpoint the issue.

For the quaternion: [ 0.86169383 0.02081877 -0.5058515 0.03412598] the code returns the correct yaw pitch roll angles: [0.15911653941132517, -60.832556785346696, -9.335093630495875]

For the quaternion: [ 0.81154224 0.01913839 -0.58165337 0.05207959] the code returns the incorrect yaw pitch roll angles: [-173.53260107524108, -71.09657335881491, 0.0]

Here is my code:

    def yaw_pitch_roll(self):
    """Get the equivalent yaw-pitch-roll angles aka. intrinsic Tait-Bryan angles following the z-y'-x'' convention

    Returns:
        yaw:    rotation angle around the z-axis in radians, in the range `[-pi, pi]`
        pitch:  rotation angle around the y'-axis in radians, in the range `[-pi/2, pi/2]`
        roll:   rotation angle around the x''-axis in radians, in the range `[-pi, pi]` 

    The resulting rotation_matrix would be R = R_x(roll) R_y(pitch) R_z(yaw)

    Note: 
        This feature only makes sense when referring to a unit quaternion. Calling this method will implicitly normalise the Quaternion object to a unit quaternion if it is not already one.
    """
    self._normalise()
    qw = self.q[0]
    qx = self.q[1]
    qy = self.q[2]
    qz = self.q[3]


    print(2*(qx*qz-qw*qy), self.q)
    if 2*(qx*qz-qw*qy)>=0.94: #Preventing gimbal lock for north pole
        yaw = np.arctan2(qx*qy-qw*qz,qx*qz+qw*qy)
        roll = 0
    elif 2*(qx*qz-qw*qy)<=-0.94: #Preventing gimbal lock for south pole
        yaw = -np.arctan2(qx*qy-qw*qz,qx*qz+qw*qy)
        roll = 0
    else:
        yaw = np.arctan2(qy*qz + qw*qx,
            1/2 - (qx**2 + qy**2))
        roll = np.arctan2(qx*qy - qw*qz,
            1/2 - (qy**2 + qz**2))
    pitch = np.arcsin(-2*(qx * qz - qw * qy))

    return yaw, pitch, roll

I actually have some sensors that send their orientation in the form of quaternions. These quaternions seem to be correct as the orientation of an avatar on the screen is correct. So I know the real orientation of my sensor (I have it in my hand) and i know the quaternion is correct --> it must be the conversion algorithm that is wrong. — ThaNoob
Comparing Euler Angels per component is not correct. Comparing quaternions per component can be done if sign of "w" identical. Comparing per component of rotation matrix is correct — minorlogic
@minorlogic could you elaborate a little? What do you mean with comparing Euler Angels per component is not correct? — ThaNoob
There is many Euler Angels combination with different component values, representing SAME rotation. — minorlogic

thewhiteambit thewhiteambit · Accepted Answer · 2020-11-17T12:03:08

Having given a Quaternion q, you can calculate roll, pitch and yaw like this:

yaw = atan2(2.0*(qy*qz + qw*qx), qw*qw - qx*qx - qy*qy + qz*qz);
pitch = asin(-2.0*(qx*qz - qw*qy));
roll = atan2(2.0*(qx*qy + qw*qz), qw*qw + qx*qx - qy*qy - qz*qz);

This should fit for intrinsic tait-bryan rotation of xyz-order. For other rotation orders, extrinsic and proper-euler rotations other conversions have to be used.

This works well for me in Autodesk Maya, where other solutions with pole exceptions had strange gimbal effects.

Quaternion to Yaw pitch roll

1 Answers