672 lines
21 KiB
Python
672 lines
21 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 _primary_button_teleop(*, move_error=None):
|
|
events = []
|
|
errors = []
|
|
snapshot = JointStateSnapshot([0.2] * 7, time.monotonic())
|
|
|
|
class Adapter:
|
|
def move_to_initial_pose(self):
|
|
events.append("move")
|
|
if move_error is not None:
|
|
raise move_error
|
|
|
|
def read_joint_state(self):
|
|
events.append("read")
|
|
return snapshot
|
|
|
|
teleop = object.__new__(SingleArmVelocityTeleop)
|
|
teleop._arm_name = "right_rm75"
|
|
teleop._adapter = Adapter()
|
|
teleop._last_primary_pressed = None
|
|
teleop._grip_rearm_required = False
|
|
teleop._safe_stop = lambda reset_active: events.append(
|
|
("stop", reset_active)
|
|
)
|
|
teleop._reset_joint_state = lambda value: events.append(("sync", value))
|
|
teleop._handle_trigger_gripper = lambda msg: None
|
|
teleop.get_clock = lambda: SimpleNamespace(now=lambda: FakeTime())
|
|
teleop.get_logger = lambda: SimpleNamespace(
|
|
info=lambda message: None,
|
|
error=lambda message: errors.append(message),
|
|
)
|
|
return teleop, events, errors, snapshot
|
|
|
|
|
|
def test_primary_button_rising_edge_moves_once_and_resyncs() -> None:
|
|
teleop, events, _, snapshot = _primary_button_teleop()
|
|
released = SimpleNamespace(primary=False)
|
|
pressed = SimpleNamespace(primary=True)
|
|
|
|
teleop._on_controller(released)
|
|
teleop._on_controller(pressed)
|
|
teleop._on_controller(pressed)
|
|
teleop._on_controller(released)
|
|
teleop._on_controller(pressed)
|
|
|
|
expected_once = [
|
|
("stop", True),
|
|
"move",
|
|
"read",
|
|
("sync", snapshot),
|
|
]
|
|
assert events == expected_once * 2
|
|
assert teleop._grip_rearm_required
|
|
|
|
|
|
def test_primary_button_move_failure_logs_and_stays_stopped() -> None:
|
|
failure = RuntimeError("rm_movej failed")
|
|
teleop, events, errors, _ = _primary_button_teleop(
|
|
move_error=failure
|
|
)
|
|
|
|
teleop._on_controller(SimpleNamespace(primary=False))
|
|
teleop._on_controller(SimpleNamespace(primary=True))
|
|
|
|
assert events == [("stop", True), "move"]
|
|
assert teleop._grip_rearm_required
|
|
assert errors == [
|
|
"right_rm75 回初始位姿失败：rm_movej failed"
|
|
]
|
|
|
|
|
|
def test_startup_joint_query_initializes_qp_and_command_history() -> None:
|
|
positions = [0.1] * 7
|
|
pose = np.eye(4)
|
|
teleop = object.__new__(SingleArmVelocityTeleop)
|
|
teleop._arm_name = "right_rm75"
|
|
teleop._adapter = SimpleNamespace(
|
|
read_joint_state=lambda: JointStateSnapshot(
|
|
positions,
|
|
time.monotonic(),
|
|
)
|
|
)
|
|
teleop._ik_solver = SimpleNamespace(
|
|
update_joint_state=lambda joints: pose
|
|
)
|
|
teleop.get_logger = lambda: FakeLogger()
|
|
|
|
teleop._initialize_joint_state()
|
|
|
|
assert teleop._latest_joint_positions == positions
|
|
assert teleop._last_valid_joint_target == positions
|
|
assert teleop._last_joint_command_target == positions
|
|
assert teleop._last_joint_command_velocity == [0.0] * 7
|
|
assert teleop._last_current_pose is pose
|
|
|
|
|
|
def test_startup_joint_query_failure_closes_adapter() -> None:
|
|
class FailingAdapter:
|
|
def __init__(self):
|
|
self.close_calls = 0
|
|
|
|
def read_joint_state(self):
|
|
raise RuntimeError("rm_get_joint_degree failed with code 7")
|
|
|
|
def close(self):
|
|
self.close_calls += 1
|
|
|
|
errors = []
|
|
teleop = object.__new__(SingleArmVelocityTeleop)
|
|
teleop._arm_name = "left_rm75"
|
|
teleop._adapter = FailingAdapter()
|
|
teleop.get_logger = lambda: SimpleNamespace(
|
|
error=lambda message: errors.append(message)
|
|
)
|
|
|
|
with pytest.raises(RuntimeError, match="code 7"):
|
|
teleop._initialize_joint_state()
|
|
|
|
assert teleop._adapter.close_calls == 1
|
|
assert "left_rm75" in errors[0]
|
|
assert "启动关节同步失败" in errors[0]
|
|
|
|
|
|
def _timeout_teleop(adapter) -> SingleArmVelocityTeleop:
|
|
teleop = object.__new__(SingleArmVelocityTeleop)
|
|
teleop._adapter = adapter
|
|
teleop._arm_name = "right_rm75"
|
|
teleop._follow = False
|
|
teleop._active = True
|
|
teleop._joint_feedback_ready = True
|
|
teleop._grip_rearm_required = False
|
|
teleop._feedback_resync_attempted = False
|
|
teleop._control_fault_latched = False
|
|
teleop._last_joint_command_target = [0.1] * 7
|
|
teleop._last_joint_command_velocity = [0.0] * 7
|
|
teleop._latest_joint_positions = [0.1] * 7
|
|
teleop._last_valid_joint_target = [0.1] * 7
|
|
teleop._last_current_pose = np.eye(4)
|
|
teleop._controller_start = None
|
|
teleop._controller_orientation_start = None
|
|
teleop._robot_start_transform = None
|
|
teleop._filtered_target = None
|
|
teleop._filtered_orientation_target = None
|
|
teleop._last_sent_target = None
|
|
teleop._last_sent_orientation = None
|
|
teleop._last_command_time = None
|
|
teleop._ik_solver = SimpleNamespace(
|
|
update_joint_state=lambda joints: np.eye(4)
|
|
)
|
|
teleop._stop_sent = False
|
|
teleop._feedback_resync_timeout_sec = 0.5
|
|
teleop._publish_stop_debug = lambda: None
|
|
teleop.get_logger = lambda: FakeLogger()
|
|
return teleop
|
|
|
|
|
|
def test_missing_or_disabled_joint_snapshot_is_not_motion_ready() -> None:
|
|
assert not SingleArmVelocityTeleop._joint_snapshot_is_motion_ready(None)
|
|
assert not SingleArmVelocityTeleop._joint_snapshot_is_motion_ready(
|
|
JointStateSnapshot(
|
|
[0.0] * 7,
|
|
time.monotonic(),
|
|
motion_ready=False,
|
|
)
|
|
)
|
|
|
|
|
|
def test_short_udp_timeout_repeats_last_limited_target_without_query() -> None:
|
|
sends = []
|
|
adapter = SimpleNamespace(
|
|
send_joint_target=lambda joints, follow: sends.append(
|
|
(list(joints), follow)
|
|
),
|
|
read_joint_state=lambda: pytest.fail("query must not run"),
|
|
stop=lambda: pytest.fail("stop must not run"),
|
|
)
|
|
teleop = _timeout_teleop(adapter)
|
|
warnings = []
|
|
teleop.get_logger = lambda: SimpleNamespace(
|
|
warn=lambda message: warnings.append(message)
|
|
)
|
|
|
|
teleop._handle_stale_joint_feedback(0.2)
|
|
|
|
assert sends == [([0.1] * 7, False)]
|
|
assert teleop._last_joint_command_target == [0.1] * 7
|
|
assert teleop._grip_rearm_required
|
|
assert warnings == [
|
|
"right_rm75 UDP关节反馈超时（age=200.0 ms），保持最后安全目标。"
|
|
]
|
|
|
|
|
|
def test_short_udp_timeout_without_active_target_stays_stopped() -> None:
|
|
stop_calls = []
|
|
adapter = SimpleNamespace(
|
|
send_joint_target=lambda joints, follow: pytest.fail(
|
|
"inactive control must not start CANFD output"
|
|
),
|
|
read_joint_state=lambda: pytest.fail("query must not run"),
|
|
stop=lambda: stop_calls.append(True),
|
|
)
|
|
teleop = _timeout_teleop(adapter)
|
|
teleop._active = False
|
|
|
|
teleop._handle_stale_joint_feedback(0.2)
|
|
|
|
assert len(stop_calls) == 1
|
|
|
|
|
|
def test_persistent_udp_timeout_queries_once_and_holds_actual_position() -> None:
|
|
sends = []
|
|
query_calls = []
|
|
adapter = SimpleNamespace(
|
|
send_joint_target=lambda joints, follow: sends.append(list(joints)),
|
|
read_joint_state=lambda: (
|
|
query_calls.append(True)
|
|
or JointStateSnapshot([0.2] * 7, time.monotonic())
|
|
),
|
|
stop=lambda: None,
|
|
)
|
|
teleop = _timeout_teleop(adapter)
|
|
|
|
teleop._handle_stale_joint_feedback(0.5)
|
|
teleop._handle_stale_joint_feedback(0.6)
|
|
|
|
assert len(query_calls) == 1
|
|
assert sends == [[0.2] * 7, [0.2] * 7]
|
|
assert teleop._last_valid_joint_target == [0.2] * 7
|
|
assert teleop._last_joint_command_velocity == [0.0] * 7
|
|
|
|
|
|
def test_persistent_udp_timeout_query_failure_latches_control() -> None:
|
|
stop_calls = []
|
|
adapter = SimpleNamespace(
|
|
send_joint_target=lambda joints, follow: pytest.fail(
|
|
"CANFD must stop after query failure"
|
|
),
|
|
read_joint_state=lambda: (_ for _ in ()).throw(
|
|
RuntimeError("rm_get_joint_degree failed with code 7")
|
|
),
|
|
stop=lambda: stop_calls.append(True),
|
|
)
|
|
teleop = _timeout_teleop(adapter)
|
|
|
|
teleop._handle_stale_joint_feedback(0.5)
|
|
teleop._handle_stale_joint_feedback(0.6)
|
|
|
|
assert teleop._control_fault_latched
|
|
assert len(stop_calls) == 1
|
|
|
|
|
|
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_joint_command_step_rejects_non_finite_limits() -> None:
|
|
for max_speed, max_acceleration, dt in (
|
|
(math.inf, 1.0, 0.1),
|
|
(1.0, math.inf, 0.1),
|
|
(1.0, 1.0, math.inf),
|
|
):
|
|
with pytest.raises(ValueError, match="finite and positive"):
|
|
SingleArmVelocityTeleop._limit_joint_command_step(
|
|
target=[0.5] * 7,
|
|
previous_target=[0.0] * 7,
|
|
previous_velocity=[0.0] * 7,
|
|
max_speed=max_speed,
|
|
max_acceleration=max_acceleration,
|
|
dt=dt,
|
|
)
|
|
|
|
|
|
def test_joint_command_step_arrival_respects_max_speed() -> None:
|
|
target, velocity = SingleArmVelocityTeleop._limit_joint_command_step(
|
|
target=[0.5] * 7,
|
|
previous_target=[0.0] * 7,
|
|
previous_velocity=[0.0] * 7,
|
|
max_speed=1.0,
|
|
max_acceleration=100.0,
|
|
dt=0.1,
|
|
)
|
|
|
|
assert velocity == pytest.approx([1.0] * 7)
|
|
assert target == pytest.approx([0.1] * 7)
|
|
|
|
|
|
def test_joint_command_step_reverses_with_acceleration_limit() -> None:
|
|
command = [0.0] * 7
|
|
velocity = [0.0] * 7
|
|
for _ in range(5):
|
|
command, velocity = SingleArmVelocityTeleop._limit_joint_command_step(
|
|
target=[1.0] * 7,
|
|
previous_target=command,
|
|
previous_velocity=velocity,
|
|
max_speed=1.0,
|
|
max_acceleration=1.0,
|
|
dt=0.1,
|
|
)
|
|
|
|
previous_command = list(command)
|
|
previous_velocity = list(velocity)
|
|
command, velocity = SingleArmVelocityTeleop._limit_joint_command_step(
|
|
target=[-1.0] * 7,
|
|
previous_target=command,
|
|
previous_velocity=velocity,
|
|
max_speed=1.0,
|
|
max_acceleration=1.0,
|
|
dt=0.1,
|
|
)
|
|
|
|
assert previous_velocity == pytest.approx([0.5] * 7)
|
|
assert velocity == pytest.approx([0.4] * 7)
|
|
assert [
|
|
current - previous
|
|
for current, previous in zip(command, previous_command)
|
|
] == pytest.approx([value * 0.1 for value in velocity])
|
|
assert all(
|
|
current > previous
|
|
for current, previous in zip(command, previous_command)
|
|
)
|
|
|
|
|
|
def test_joint_command_step_brakes_before_fixed_target_without_overshoot() -> None:
|
|
dt = 1.0 / 90.0
|
|
max_speed = math.radians(180.0)
|
|
max_acceleration = math.radians(300.0)
|
|
target = np.radians(
|
|
[10.0, -10.0, 3.0, -3.0, 1.0, -1.0, 0.1]
|
|
).tolist()
|
|
command = [0.0] * 7
|
|
velocity = [0.0] * 7
|
|
|
|
for _ in range(180):
|
|
previous_command = list(command)
|
|
previous_velocity = list(velocity)
|
|
command, velocity = (
|
|
SingleArmVelocityTeleop._limit_joint_command_step(
|
|
target=target,
|
|
previous_target=command,
|
|
previous_velocity=velocity,
|
|
max_speed=max_speed,
|
|
max_acceleration=max_acceleration,
|
|
dt=dt,
|
|
)
|
|
)
|
|
|
|
for index in range(7):
|
|
assert min(0.0, target[index]) - 1e-12 <= command[index]
|
|
assert command[index] <= max(0.0, target[index]) + 1e-12
|
|
assert abs(velocity[index]) <= max_speed + 1e-12
|
|
assert (
|
|
abs(velocity[index] - previous_velocity[index])
|
|
<= max_acceleration * dt + 1e-12
|
|
)
|
|
assert command[index] - previous_command[index] == pytest.approx(
|
|
velocity[index] * dt,
|
|
abs=1e-12,
|
|
)
|
|
|
|
assert command == pytest.approx(target, abs=1e-12)
|
|
assert velocity == pytest.approx([0.0] * 7, abs=1e-12)
|
|
|
|
|
|
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._control_fault_latched = False
|
|
teleop._feedback_resync_attempted = False
|
|
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_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_canfd_error_stops_queries_and_requires_grip_rearm() -> None:
|
|
class RecoveringAdapter:
|
|
def __init__(self):
|
|
self.stop_calls = 0
|
|
self.read_calls = 0
|
|
|
|
def send_joint_target(self, joints, follow):
|
|
del joints, follow
|
|
raise RuntimeError("rm_movej_canfd failed with code 9")
|
|
|
|
def stop(self):
|
|
self.stop_calls += 1
|
|
|
|
def read_joint_state(self):
|
|
self.read_calls += 1
|
|
return JointStateSnapshot([0.2] * 7, time.monotonic())
|
|
|
|
teleop = _timeout_teleop(RecoveringAdapter())
|
|
teleop._joint_command_max_speed = math.radians(180.0)
|
|
teleop._joint_command_max_acceleration = math.radians(300.0)
|
|
teleop._dt = 1.0 / 90.0
|
|
|
|
sent = teleop._send_joint_target([0.3] * 7)
|
|
|
|
assert not sent
|
|
assert teleop._adapter.stop_calls == 1
|
|
assert teleop._adapter.read_calls == 1
|
|
assert not teleop._control_fault_latched
|
|
assert teleop._grip_rearm_required
|
|
assert teleop._last_joint_command_target == [0.2] * 7
|
|
|
|
|
|
def test_canfd_error_latches_when_joint_query_also_fails() -> None:
|
|
class FailingAdapter:
|
|
def __init__(self):
|
|
self.stop_calls = 0
|
|
|
|
def send_joint_target(self, joints, follow):
|
|
del joints, follow
|
|
raise RuntimeError("rm_movej_canfd failed with code 9")
|
|
|
|
def stop(self):
|
|
self.stop_calls += 1
|
|
|
|
def read_joint_state(self):
|
|
raise RuntimeError("rm_get_joint_degree failed with code 7")
|
|
|
|
teleop = _timeout_teleop(FailingAdapter())
|
|
teleop._joint_command_max_speed = math.radians(180.0)
|
|
teleop._joint_command_max_acceleration = math.radians(300.0)
|
|
teleop._dt = 1.0 / 90.0
|
|
|
|
assert not teleop._send_joint_target([0.3] * 7)
|
|
assert teleop._control_fault_latched
|
|
assert teleop._adapter.stop_calls == 1
|