125 lines
4.1 KiB
Python
125 lines
4.1 KiB
Python
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
|