code: def _dcm_to_euler(self, dcm):
          
          assert(dcm.shape == (3, 3))
          theta = np.arcsin(min(1, max(-1, -dcm[2][0])))
  
          if abs(theta - np.pi/2) < 1.0e-3:
              phi = 0.0
              psi = (np.arctan2(dcm[1][2] - dcm[0][1],
                                dcm[0][2] + dcm[1][1]) + phi)
          elif abs(theta + np.pi/2) < 1.0e-3:
              phi = 0.0
              psi = np.arctan2(dcm[1][2] - dcm[0][1],
                               dcm[0][2] + dcm[1][1] - phi)
          else:
              phi = np.arctan2(dcm[2][1], dcm[2][2])
              psi = np.arctan2(dcm[1][0], dcm[0][0])
  
          return np.array([phi, theta, psi])
docs: Create DCM from euler angles
          :param dcm: 3x3 dcm array
          :returns: array [roll, pitch, yaw] in rad