Esempio n. 1
0
def test_quaternion2euler(qtbot):
    """Test quaternion to euler angle conversion."""
    # Test roundtrip degrees
    angles = (12, 53, 92)
    q = Quaternion.create_from_euler_angles(*angles, degrees=True)
    ea = quaternion2euler(q, degrees=True)
    np.testing.assert_allclose(ea, angles)

    # Test roundtrip radians
    angles = (0.1, -0.2, 1.2)
    q = Quaternion.create_from_euler_angles(*angles)
    ea = quaternion2euler(q)
    np.testing.assert_allclose(ea, angles)
Esempio n. 2
0
def test_quaternion2euler(angles, degrees):
    """Test quaternion to euler angle conversion."""

    # Test for degrees
    q = Quaternion.create_from_euler_angles(*angles, degrees)
    ea = quaternion2euler(q, degrees=degrees)
    q_p = Quaternion.create_from_euler_angles(*ea, degrees=degrees)

    # We now compare the corresponding quaternions ; they should be equals or opposites (as they're already unit ones)
    q_values = np.array([q.w, q.x, q.y, q.z])
    q_p_values = np.array([q_p.w, q_p.x, q_p.y, q_p.z])

    nn_zero_ind = np.argmax((q_values != 0) & (q_p_values != 0))

    q_values *= np.sign(q_values[nn_zero_ind])
    q_p_values *= np.sign(q_p_values[nn_zero_ind])

    np.testing.assert_allclose(q_values, q_p_values)