89 lines
2.6 KiB
Python
89 lines
2.6 KiB
Python
import math
|
|
|
|
import pytest
|
|
|
|
from xr_rm_teleop.realman_adapter import RealManAdapter
|
|
from xr_rm_teleop.realman_adapter import MockRealManAdapter
|
|
from xr_rm_teleop.fun_peripheral import PeripheralConfig
|
|
|
|
|
|
def test_initial_pose_uses_joint_move_only() -> None:
|
|
class FakeArm:
|
|
def __init__(self) -> None:
|
|
self.calls = []
|
|
|
|
def rm_movej(self, *args):
|
|
self.calls.append(args)
|
|
return 0
|
|
|
|
joints = [-167.21, 28.48, 28.21, 61.35, -14.40, 84.49, -124.51]
|
|
adapter = RealManAdapter("127.0.0.1", 8080, 0, 1, initial_joint_pose=joints)
|
|
adapter._arm = FakeArm()
|
|
|
|
adapter._move_to_initial_pose()
|
|
|
|
assert adapter._arm.calls == [(joints, 20, 0, 0, 1)]
|
|
|
|
|
|
def test_peripheral_config_exposes_selected_tool() -> None:
|
|
config = PeripheralConfig(
|
|
scissorgripper=1,
|
|
tools_in_ee={
|
|
"first": [[0.0] * 7, [0.0] * 7],
|
|
"second": [[0.0, 0.0, 0.16, 0.0, 0.0, 0.0, 1.0], [0.0] * 7],
|
|
},
|
|
)
|
|
|
|
assert config.tool_name == "second"
|
|
assert config.tool_pose == [0.0, 0.0, 0.16, 0.0, 0.0, 0.0, 1.0]
|
|
|
|
|
|
def test_joint_feedback_is_cached_in_radians() -> None:
|
|
class FakeArm:
|
|
def rm_get_joint_degree(self):
|
|
return 0, [0.0, 10.0, -20.0, 30.0, -40.0, 50.0, -60.0]
|
|
|
|
adapter = RealManAdapter("127.0.0.1", 8080, 0, 0.01)
|
|
adapter._arm = FakeArm()
|
|
|
|
adapter._read_joint_state_once()
|
|
snapshot = adapter.get_latest_joint_state()
|
|
|
|
assert snapshot is not None
|
|
assert snapshot.positions == pytest.approx(
|
|
[math.radians(value) for value in [0, 10, -20, 30, -40, 50, -60]]
|
|
)
|
|
|
|
|
|
def test_joint_target_uses_movej_canfd_in_degrees() -> None:
|
|
class FakeArm:
|
|
def __init__(self) -> None:
|
|
self.calls = []
|
|
|
|
def rm_movej_canfd(self, *args):
|
|
self.calls.append(args)
|
|
return 0
|
|
|
|
adapter = RealManAdapter("127.0.0.1", 8080, 0, 0.01)
|
|
adapter._arm = FakeArm()
|
|
target = [math.radians(value) for value in [1, 2, 3, 4, 5, 6, 7]]
|
|
|
|
adapter.send_joint_target(target, follow=False)
|
|
|
|
assert len(adapter._arm.calls) == 1
|
|
degrees, follow, expand, trajectory_mode, radio = adapter._arm.calls[0]
|
|
assert degrees == pytest.approx([1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0])
|
|
assert (follow, expand, trajectory_mode, radio) == (False, 0, 2, 0)
|
|
|
|
|
|
def test_mock_joint_feedback_is_available_without_vendor_sdk() -> None:
|
|
adapter = MockRealManAdapter([1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0])
|
|
|
|
adapter.connect()
|
|
snapshot = adapter.get_latest_joint_state()
|
|
|
|
assert snapshot is not None
|
|
assert snapshot.positions == pytest.approx(
|
|
[math.radians(value) for value in [1, 2, 3, 4, 5, 6, 7]]
|
|
)
|