feat: Implement UDP feedback for RM75 robot arms

xr_rm_teleop/test/test_joint_control.py

@@ -1,3 +1,4 @@
+import math
 import time
 from types import SimpleNamespace
 
@@ -45,6 +46,85 @@ def test_missing_or_stale_feedback_does_not_enable_qp() -> None:
     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 = []
@@ -256,6 +336,11 @@ def test_joint_send_failure_requests_slow_stop_and_resets_control() -> None:
     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)