Quaternion to Yaw pitch roll - python-3.x

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

Answer 1

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.