352 lines
11 KiB
Python
352 lines
11 KiB
Python
import math
|
|
import time
|
|
from types import SimpleNamespace
|
|
|
|
import numpy as np
|
|
import pytest
|
|
|
|
from xr_rm_teleop.realman_adapter import JointStateSnapshot
|
|
from xr_rm_teleop.single_arm_velocity_teleop import (
|
|
SingleArmVelocityTeleop,
|
|
_make_transform,
|
|
_so3_exp,
|
|
)
|
|
|
|
|
|
class FakeLogger:
|
|
def info(self, *args, **kwargs):
|
|
del args, kwargs
|
|
|
|
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_disabled_joint_feedback_does_not_enable_qp() -> None:
|
|
teleop = object.__new__(SingleArmVelocityTeleop)
|
|
teleop._command_timeout_sec = 0.12
|
|
teleop._adapter = SimpleNamespace(
|
|
get_latest_joint_state=lambda: JointStateSnapshot(
|
|
[0.0] * 7,
|
|
time.monotonic(),
|
|
motion_ready=False,
|
|
)
|
|
)
|
|
|
|
assert teleop._fresh_joint_state() is None
|
|
|
|
|
|
def test_joint_command_step_limits_acceleration_from_rest() -> None:
|
|
dt = 1.0 / 125.0
|
|
target, velocity = SingleArmVelocityTeleop._limit_joint_command_step(
|
|
target=[0.2] * 7,
|
|
previous_target=[0.0] * 7,
|
|
previous_velocity=[0.0] * 7,
|
|
max_speed=math.radians(180.0),
|
|
max_acceleration=math.radians(300.0),
|
|
dt=dt,
|
|
)
|
|
|
|
assert velocity == pytest.approx([math.radians(2.4)] * 7)
|
|
assert target == pytest.approx([math.radians(0.0192)] * 7)
|
|
|
|
|
|
def test_feedback_fault_blocks_grip_until_release() -> None:
|
|
class FakeClock:
|
|
def now(self):
|
|
return FakeTime()
|
|
|
|
teleop = object.__new__(SingleArmVelocityTeleop)
|
|
teleop._adapter = SimpleNamespace(
|
|
get_latest_joint_state=lambda: JointStateSnapshot(
|
|
[0.1] * 7,
|
|
time.monotonic(),
|
|
)
|
|
)
|
|
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._last_valid_joint_target = None
|
|
teleop._last_current_pose = None
|
|
teleop._ik_solver = SimpleNamespace(
|
|
update_joint_state=lambda joints: np.eye(4)
|
|
)
|
|
teleop._grip_rearm_required = True
|
|
teleop.get_clock = lambda: FakeClock()
|
|
teleop.get_logger = lambda: FakeLogger()
|
|
stopped = []
|
|
entered = []
|
|
teleop._safe_stop = lambda reset_active: stopped.append(reset_active)
|
|
teleop._enter_active_control = lambda *args: entered.append(args)
|
|
|
|
teleop._control_tick()
|
|
assert entered == []
|
|
|
|
teleop._last_msg.grip = False
|
|
teleop._control_tick()
|
|
assert teleop._grip_rearm_required is False
|
|
|
|
teleop._last_msg.grip = True
|
|
teleop._control_tick()
|
|
assert len(entered) == 1
|
|
|
|
|
|
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
|
|
transform = np.eye(4)
|
|
transform[:3, 3] = [0.3, 0.0, 0.2]
|
|
return transform
|
|
|
|
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 == pytest.approx(
|
|
_make_transform([0.3, 0.0, 0.2], np.eye(3))
|
|
)
|
|
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(np.eye(4))
|
|
|
|
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(np.eye(4))
|
|
|
|
assert target == pytest.approx([0.2] * 7)
|
|
assert teleop._last_valid_joint_target == pytest.approx([0.2] * 7)
|
|
|
|
|
|
def test_enter_active_control_initializes_se3_orientation_state() -> None:
|
|
teleop = object.__new__(SingleArmVelocityTeleop)
|
|
transform = _make_transform(
|
|
[0.3, -0.1, 0.2],
|
|
_so3_exp(np.asarray([0.1, -0.2, 0.3])),
|
|
)
|
|
published = []
|
|
teleop._arm_name = "right_rm75"
|
|
teleop.get_logger = lambda: FakeLogger()
|
|
teleop._publish_debug = lambda *args: published.append(args)
|
|
|
|
teleop._enter_active_control(
|
|
[0.0, 0.0, 0.0],
|
|
(0.0, 0.0, 0.0, 1.0),
|
|
transform,
|
|
FakeTime(),
|
|
)
|
|
|
|
assert teleop._robot_start_transform == pytest.approx(transform)
|
|
assert teleop._filtered_target == pytest.approx(transform[:3, 3])
|
|
assert teleop._filtered_orientation_target == pytest.approx(transform[:3, :3])
|
|
assert teleop._last_sent_orientation == pytest.approx(transform[:3, :3])
|
|
assert len(published) == 1
|
|
|
|
|
|
def test_command_angular_velocity_uses_so3_rotation_vector() -> None:
|
|
teleop = object.__new__(SingleArmVelocityTeleop)
|
|
teleop._dt = 0.1
|
|
teleop._last_sent_target = [0.0, 0.0, 0.0]
|
|
teleop._last_sent_orientation = np.eye(3)
|
|
teleop._last_command_time = None
|
|
|
|
velocity = teleop._estimate_command_velocity(
|
|
[0.0, 0.0, 0.0],
|
|
_so3_exp(np.asarray([0.0, 0.0, 0.1])),
|
|
FakeTime(),
|
|
)
|
|
|
|
assert velocity == pytest.approx([0.0, 0.0, 0.0, 0.0, 0.0, 1.0])
|
|
|
|
|
|
def test_timing_stats_logs_summary_and_clears_window() -> None:
|
|
messages = []
|
|
teleop = object.__new__(SingleArmVelocityTeleop)
|
|
teleop._arm_name = "right_rm75"
|
|
teleop._dt = 0.008
|
|
teleop._timing_stats_window = 3
|
|
teleop._timing_samples = {
|
|
name: []
|
|
for name in (
|
|
"period",
|
|
"total",
|
|
"qp",
|
|
"send",
|
|
"feedback_age",
|
|
"feedback_read",
|
|
"feedback_interval",
|
|
)
|
|
}
|
|
teleop._last_timing_feedback_received_at = None
|
|
teleop.get_logger = lambda: SimpleNamespace(
|
|
info=lambda message: messages.append(message)
|
|
)
|
|
first_feedback = JointStateSnapshot([0.0] * 7, 10.0, 2.0, None)
|
|
second_feedback = JointStateSnapshot([0.0] * 7, 10.011, 3.0, 11.0)
|
|
|
|
teleop._record_timing_sample(7.0, 6.0, 1.0, 0.5, 3.0, first_feedback)
|
|
teleop._record_timing_sample(8.0, 8.0, 1.5, 0.6, 3.5, first_feedback)
|
|
assert messages == []
|
|
|
|
teleop._record_timing_sample(9.0, 10.0, 2.0, 0.7, 4.0, second_feedback)
|
|
|
|
assert len(messages) == 1
|
|
assert "right_rm75 timing n=3 deadline=8.000 ms" in messages[0]
|
|
assert (
|
|
"period[n=3 mean=8.000 p95=8.900 p99=8.980 "
|
|
"max=9.000 ms overruns=1]"
|
|
) in messages[0]
|
|
assert (
|
|
"total[n=3 mean=8.000 p95=9.800 p99=9.960 "
|
|
"max=10.000 ms overruns=1]"
|
|
) in messages[0]
|
|
assert "qp[n=3" in messages[0]
|
|
assert "send[n=3" in messages[0]
|
|
assert "feedback_age[n=3" in messages[0]
|
|
assert "feedback_read[n=2" in messages[0]
|
|
assert "feedback_interval[n=1" in messages[0]
|
|
assert all(not samples for samples in teleop._timing_samples.values())
|
|
|
|
|
|
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._last_joint_command_target = [0.0] * 7
|
|
teleop._last_joint_command_velocity = [0.0] * 7
|
|
teleop._joint_command_max_speed = math.radians(180.0)
|
|
teleop._joint_command_max_acceleration = math.radians(300.0)
|
|
teleop._dt = 1.0 / 125.0
|
|
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]
|