Enhance orientation control for RM75 arms in XR teleoperation

xr_rm_teleop/test/test_orientation_control.py

@@ -0,0 +1,124 @@
+import math
+from types import SimpleNamespace
+
+import pytest
+
+from xr_rm_teleop.realman_adapter import ArmPose, MockRealManAdapter
+from xr_rm_teleop.single_arm_velocity_teleop import (
+    SingleArmVelocityTeleop,
+    _euler_to_quaternion,
+    _normalize_quaternion,
+    _quaternion_to_euler,
+)
+
+
+def _make_teleop_for_orientation() -> SingleArmVelocityTeleop:
+    teleop = object.__new__(SingleArmVelocityTeleop)
+    teleop._enable_orientation_control = True
+    teleop._enable_orientation_axes = [True, True, True]
+    teleop._controller_orientation_start = (0.0, 0.0, 0.0, 1.0)
+    teleop._robot_start_pose = ArmPose(0.3, 0.0, 0.2, 0.1, -0.2, 0.3)
+    teleop._xr_to_robot_matrix = [
+        0.0, 1.0, 0.0,
+        0.0, 0.0, 1.0,
+        1.0, 0.0, 0.0,
+    ]
+    return teleop
+
+
+def assert_angles_close(actual: list[float] | tuple[float, ...], expected: list[float]) -> None:
+    assert len(actual) == len(expected)
+    for actual_value, expected_value in zip(actual, expected):
+        assert math.atan2(math.sin(actual_value - expected_value), math.cos(actual_value - expected_value)) == pytest.approx(0.0)
+
+
+def test_identity_controller_orientation_keeps_tcp_orientation() -> None:
+    teleop = _make_teleop_for_orientation()
+
+    target = teleop._raw_orientation_from_controller((0.0, 0.0, 0.0, 1.0))
+
+    assert_angles_close(target, teleop._robot_start_pose.rpy())
+
+
+def test_xr_relative_rotation_maps_through_xr_to_robot_matrix() -> None:
+    teleop = _make_teleop_for_orientation()
+    teleop._robot_start_pose = ArmPose(0.3, 0.0, 0.2, 0.0, 0.0, 0.0)
+    xr_roll = _euler_to_quaternion(0.2, 0.0, 0.0)
+
+    target = teleop._raw_orientation_from_controller(xr_roll)
+
+    assert_angles_close(target, [0.0, 0.0, 0.2])
+
+
+def test_orientation_deadband_filter_and_speed_limit() -> None:
+    teleop = object.__new__(SingleArmVelocityTeleop)
+    teleop._orientation_deadband_rad = 0.01
+    teleop._orientation_filter_alpha = 0.5
+    teleop._max_orientation_speed = 0.5
+    teleop._dt = 0.1
+    teleop._last_sent_orientation = [0.0, 0.0, 0.0]
+    teleop._filtered_orientation_target = [0.0, 0.0, 0.0]
+
+    assert teleop._apply_orientation_deadband([0.001, 0.0, 0.0]) == [0.0, 0.0, 0.0]
+
+    filtered = teleop._filter_orientation_target([0.2, 0.0, 0.0])
+    assert_angles_close(filtered, [0.1, 0.0, 0.0])
+
+    limited, was_limited = teleop._limit_orientation_step([0.2, 0.0, 0.0])
+    assert was_limited
+    assert_angles_close(limited, [0.05, 0.0, 0.0])
+
+
+def test_invalid_controller_quaternion_stops_current_tick() -> None:
+    class FakeTime:
+        def __sub__(self, other):
+            del other
+            return SimpleNamespace(nanoseconds=0)
+
+    class FakeClock:
+        def now(self):
+            return FakeTime()
+
+    class FakeLogger:
+        def warn(self, *args, **kwargs):
+            del args, kwargs
+
+    teleop = object.__new__(SingleArmVelocityTeleop)
+    teleop._last_msg = SimpleNamespace(
+        grip=True,
+        pose=SimpleNamespace(
+            position=SimpleNamespace(x=0.0, y=0.0, z=0.0),
+            orientation=SimpleNamespace(x=0.0, y=0.0, z=0.0, w=0.0),
+        ),
+    )
+    teleop._last_msg_time = FakeTime()
+    teleop._arm_name = "test_rm75"
+    teleop._command_timeout_sec = 0.12
+    teleop._enable_orientation_control = True
+    stopped = []
+    teleop.get_clock = lambda: FakeClock()
+    teleop.get_logger = lambda: FakeLogger()
+    teleop._safe_stop = lambda reset_active: stopped.append(reset_active)
+
+    teleop._control_tick()
+
+    assert stopped == [True]
+
+
+def test_quaternion_roundtrip_for_small_rpy() -> None:
+    quat = _normalize_quaternion(_euler_to_quaternion(0.2, -0.1, 0.3))
+    assert_angles_close(_quaternion_to_euler(quat), [0.2, -0.1, 0.3])
+
+
+def test_zero_quaternion_is_invalid() -> None:
+    with pytest.raises(ValueError):
+        _normalize_quaternion([0.0, 0.0, 0.0, 0.0])
+
+
+def test_mock_adapter_uses_shortest_angular_velocity() -> None:
+    adapter = MockRealManAdapter([0.0, 0.0, 0.0, 3.13, 0.0, -3.13], 0.1)
+
+    adapter.send_cartesian_target(ArmPose(0.0, 0.0, 0.0, -3.13, 0.0, 3.13), False)
+
+    assert abs(adapter.last_velocity[3]) < 1.0
+    assert abs(adapter.last_velocity[5]) < 1.0