title: Quaternion to Yaw pitch roll
body: <p>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.</p>
  
  <p>Secondly my code shows an incorrect yaw angle of 180 degrees at a pitch of -90 degrees.</p>
  
  <p>I tried the code at <a href="https://stackoverflow.com/questions/50157131/quaternion-rotation-to-pitch-roll-yaw">this post</a> and from <a href="http://www.euclideanspace.com/maths/geometry/rotations/conversions/quaternionToEuler/" rel="nofollow noreferrer">this site</a> but none of them worked. I also tried the <a href="http://kieranwynn.github.io/pyquaternion/" rel="nofollow noreferrer">pyquaternion</a> 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. </p>
  
  <p><a href="https://i.stack.imgur.com/PstHl.gif" rel="nofollow noreferrer"><img src="https://i.stack.imgur.com/PstHl.gif" alt=""></a><br>
  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.</p>
  
  <p>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]</p>
  
  <p>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]</p>
  
  <p>Here is my code:</p>
  
  <pre><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)&gt;=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)&lt;=-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  
  </code></pre>