165 lines
4.9 KiB
Python
165 lines
4.9 KiB
Python
import time
|
|
from types import SimpleNamespace
|
|
|
|
import pytest
|
|
|
|
from xr_rm_teleop.realman_adapter import ArmPose, JointStateSnapshot
|
|
from xr_rm_teleop.single_arm_velocity_teleop import SingleArmVelocityTeleop
|
|
|
|
|
|
class FakeLogger:
|
|
def warn(self, *args, **kwargs):
|
|
del args, kwargs
|
|
|
|
def error(self, *args, **kwargs):
|
|
del args, kwargs
|
|
|
|
|
|
class FakeTime:
|
|
def __sub__(self, other):
|
|
del other
|
|
return SimpleNamespace(nanoseconds=0)
|
|
|
|
|
|
def test_missing_or_stale_feedback_does_not_enable_qp() -> None:
|
|
teleop = object.__new__(SingleArmVelocityTeleop)
|
|
teleop._command_timeout_sec = 0.12
|
|
teleop._adapter = SimpleNamespace(get_latest_joint_state=lambda: None)
|
|
|
|
assert teleop._fresh_joint_state() is None
|
|
|
|
teleop._adapter = SimpleNamespace(
|
|
get_latest_joint_state=lambda: JointStateSnapshot(
|
|
[0.0] * 7,
|
|
time.monotonic() - 1.0,
|
|
)
|
|
)
|
|
assert teleop._fresh_joint_state() is None
|
|
|
|
|
|
def test_stale_feedback_stops_before_active_control() -> None:
|
|
stopped = []
|
|
entered = []
|
|
teleop = object.__new__(SingleArmVelocityTeleop)
|
|
teleop._adapter = SimpleNamespace(
|
|
get_latest_joint_state=lambda: JointStateSnapshot(
|
|
[0.0] * 7,
|
|
time.monotonic() - 1.0,
|
|
)
|
|
)
|
|
teleop._command_timeout_sec = 0.12
|
|
teleop._joint_feedback_ready = True
|
|
teleop._arm_name = "right_rm75"
|
|
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=1.0),
|
|
),
|
|
)
|
|
teleop._last_msg_time = FakeTime()
|
|
teleop._active = False
|
|
teleop._enable_orientation_control = False
|
|
teleop.get_clock = lambda: SimpleNamespace(now=lambda: FakeTime())
|
|
teleop.get_logger = lambda: FakeLogger()
|
|
teleop._safe_stop = lambda reset_active: stopped.append(reset_active)
|
|
teleop._enter_active_control = lambda *args: entered.append(args)
|
|
|
|
teleop._control_tick()
|
|
|
|
assert stopped == [True]
|
|
assert entered == []
|
|
|
|
|
|
def test_first_feedback_initializes_last_valid_target_without_solving() -> None:
|
|
class FakeSolver:
|
|
def __init__(self) -> None:
|
|
self.solve_calls = 0
|
|
|
|
def update_joint_state(self, joints):
|
|
assert joints == [0.1] * 7
|
|
return ArmPose(0.3, 0.0, 0.2)
|
|
|
|
def solve(self, target):
|
|
del target
|
|
self.solve_calls += 1
|
|
return [0.2] * 7
|
|
|
|
teleop = object.__new__(SingleArmVelocityTeleop)
|
|
teleop._ik_solver = FakeSolver()
|
|
teleop._active = False
|
|
teleop._last_valid_joint_target = None
|
|
teleop._last_current_pose = None
|
|
|
|
pose = teleop._sync_joint_feedback(
|
|
JointStateSnapshot([0.1] * 7, time.monotonic())
|
|
)
|
|
|
|
assert pose == ArmPose(0.3, 0.0, 0.2)
|
|
assert teleop._last_valid_joint_target == [0.1] * 7
|
|
assert teleop._ik_solver.solve_calls == 0
|
|
|
|
|
|
def test_qp_failure_returns_last_known_good_target() -> None:
|
|
class FailingSolver:
|
|
def solve(self, target):
|
|
del target
|
|
raise RuntimeError("NaN in QP solution")
|
|
|
|
teleop = object.__new__(SingleArmVelocityTeleop)
|
|
teleop._ik_solver = FailingSolver()
|
|
teleop._last_valid_joint_target = [0.1] * 7
|
|
teleop._arm_name = "right_rm75"
|
|
teleop.get_logger = lambda: FakeLogger()
|
|
|
|
target = teleop._solve_joint_target(ArmPose(0.3, 0.0, 0.2))
|
|
|
|
assert target == pytest.approx([0.1] * 7)
|
|
assert teleop._last_valid_joint_target == pytest.approx([0.1] * 7)
|
|
|
|
|
|
def test_qp_success_updates_last_known_good_target() -> None:
|
|
class SuccessfulSolver:
|
|
def solve(self, target):
|
|
del target
|
|
return [0.2] * 7
|
|
|
|
teleop = object.__new__(SingleArmVelocityTeleop)
|
|
teleop._ik_solver = SuccessfulSolver()
|
|
teleop._last_valid_joint_target = [0.1] * 7
|
|
teleop._arm_name = "left_rm75"
|
|
teleop.get_logger = lambda: FakeLogger()
|
|
|
|
target = teleop._solve_joint_target(ArmPose(0.3, 0.0, 0.2))
|
|
|
|
assert target == pytest.approx([0.2] * 7)
|
|
assert teleop._last_valid_joint_target == pytest.approx([0.2] * 7)
|
|
|
|
|
|
def test_joint_send_failure_requests_slow_stop_and_resets_control() -> None:
|
|
class FailingAdapter:
|
|
def __init__(self) -> None:
|
|
self.stop_calls = 0
|
|
|
|
def send_joint_target(self, joints, follow):
|
|
del joints, follow
|
|
raise RuntimeError("send failed")
|
|
|
|
def stop(self):
|
|
self.stop_calls += 1
|
|
|
|
reset_calls = []
|
|
teleop = object.__new__(SingleArmVelocityTeleop)
|
|
teleop._adapter = FailingAdapter()
|
|
teleop._follow = False
|
|
teleop._arm_name = "left_rm75"
|
|
teleop._stop_sent = False
|
|
teleop.get_logger = lambda: FakeLogger()
|
|
teleop._safe_stop = lambda reset_active: reset_calls.append(reset_active)
|
|
|
|
sent = teleop._send_joint_target([0.1] * 7)
|
|
|
|
assert not sent
|
|
assert teleop._adapter.stop_calls == 1
|
|
assert reset_calls == [True]
|