[-0.5, -0.5, 0.5, -0.5], [0, 0, 1 / sqrt(2), 1 / sqrt(2)], [0, 0, -1 / sqrt(2), 1 / sqrt(2)], [0, -1 / sqrt(2), 0, 1 / sqrt(2)], ): q1 = Quaternion(q) r, p, y = q1.toRPY() q2 = Quaternion() q2.fromRPY(r, p, y) print("q1", str(q1)) print("q2", str(q2)) print("rpy", r, p, y) qdiff = q1 * q2.inv() if qdiff.array[3] < 0: qdiff.array *= -1 if not (np.abs(qdiff.array - np.array([0, 0, 0, 1])) < 1e-6).all(): print("ERROR") ok = False print("") assert ok for i in range(50): r, p, y = np.random.uniform(low=0, high=pi / 2, size=3) # Roll qr = Quaternion(sin(r / 2), 0, 0, cos(r / 2)) # Pitch qp = Quaternion(0, sin(p / 2), 0, cos(p / 2))