41 lines
1.0 KiB
Python
41 lines
1.0 KiB
Python
"""Roundtrip RPY (deg) ↔ quaternion xyzw."""
|
|
from __future__ import annotations
|
|
|
|
import numpy as np
|
|
import pytest
|
|
|
|
from presa_mano.geometry import (
|
|
quaternion_xyzw_to_rpy_deg,
|
|
rpy_deg_to_quaternion_xyzw,
|
|
)
|
|
|
|
|
|
def test_zero_rpy_is_identity_quat():
|
|
q = rpy_deg_to_quaternion_xyzw(0.0, 0.0, 0.0)
|
|
assert np.allclose(q, [0.0, 0.0, 0.0, 1.0], atol=1e-9)
|
|
|
|
|
|
def test_quat_identity_is_zero_rpy():
|
|
rpy = quaternion_xyzw_to_rpy_deg([0.0, 0.0, 0.0, 1.0])
|
|
assert np.allclose(rpy, [0.0, 0.0, 0.0], atol=1e-6)
|
|
|
|
|
|
@pytest.mark.parametrize("roll,pitch,yaw", [
|
|
(10.0, 20.0, 30.0),
|
|
(-45.0, 0.0, 90.0),
|
|
(0.0, 60.0, -30.0),
|
|
(15.0, -25.0, 80.0),
|
|
])
|
|
def test_roundtrip_rpy_quat_rpy(roll, pitch, yaw):
|
|
q = rpy_deg_to_quaternion_xyzw(roll, pitch, yaw)
|
|
r2, p2, y2 = quaternion_xyzw_to_rpy_deg(q)
|
|
assert abs(r2 - roll) < 1e-4
|
|
assert abs(p2 - pitch) < 1e-4
|
|
assert abs(y2 - yaw) < 1e-4
|
|
|
|
|
|
def test_quat_unit_norm():
|
|
q = rpy_deg_to_quaternion_xyzw(33.0, -17.0, 91.5)
|
|
n = float(np.linalg.norm(q))
|
|
assert abs(n - 1.0) < 1e-9
|