feat: Implement UDP feedback for RM75 robot arms

2026-07-29 15:26:59 +08:00
parent 687a0b401a
commit 08996434e5
16 changed files with 1600 additions and 329 deletions
@@ -0,0 +1,508 @@
+# RM75 CANFD UDP Feedback Implementation Plan
+
+> **For agentic workers:** REQUIRED SUB-SKILL: Use superpowers:subagent-driven-development (recommended) or superpowers:executing-plans to implement this plan task-by-task. Steps use checkbox (`- [ ]`) syntax for tracking.
+
+**Goal:** Replace synchronous TCP joint polling with the vendor UDP realtime callback while making YAML the source of robot behavior and hardware defaults.
+
+**Architecture:** Keep one `RoboticArm(RM_TRIPLE_MODE_E)` handle per arm. TCP sends CANFD and safety/tool commands; a 5 ms controller UDP push invokes a minimal callback that updates the existing locked joint snapshot. Launch keeps only topology, mock safety mode, PICO input, and generated paths/topics.
+
+**Tech Stack:** Python 3.10, ROS2 Humble, RealMan Python API2, YAML, pytest, colcon
+
+---
+
+### Task 1: Add failing UDP feedback adapter tests
+
+**Files:**
+- Modify: `xr_rm_teleop/test/test_initial_joint_pose.py`
+
+- [ ] **Step 1: Replace polling-specific tests with UDP callback tests**
+
+Add `sys`, `types`, and `SimpleNamespace` imports. Replace
+`test_joint_feedback_is_cached_in_radians` and
+`test_feedback_loop_uses_absolute_schedule_without_catch_up` with helpers and
+tests equivalent to:
+
+```python
+def _udp_state(robot_ip="127.0.0.1", joints=None, error_code=0):
+    return SimpleNamespace(
+        errCode=error_code,
+        arm_ip=robot_ip.encode(),
+        joint_status=SimpleNamespace(
+            joint_position=joints or [0.0, 10.0, -20.0, 30.0, -40.0, 50.0, -60.0]
+        ),
+    )
+
+
+def test_udp_feedback_is_cached_in_radians(monkeypatch) -> None:
+    monotonic = iter([10.0, 10.005])
+    monkeypatch.setattr(realman_adapter.time, "monotonic", lambda: next(monotonic))
+    adapter = RealManAdapter(
+        "127.0.0.1",
+        8080,
+        0,
+        "127.0.0.1",
+        8090,
+    )
+
+    adapter._accept_realtime_feedback = True
+    adapter._on_realtime_arm_state(_udp_state())
+    first = adapter.get_latest_joint_state()
+    adapter._on_realtime_arm_state(_udp_state())
+    second = adapter.get_latest_joint_state()
+
+    assert first is not None
+    assert first.positions == pytest.approx(
+        [math.radians(value) for value in [0, 10, -20, 30, -40, 50, -60]]
+    )
+    assert first.read_duration_ms is None
+    assert first.update_interval_ms is None
+    assert second is not None
+    assert second.update_interval_ms == pytest.approx(5.0)
+
+
+@pytest.mark.parametrize(
+    "state",
+    [
+        _udp_state(error_code=-3),
+        _udp_state(robot_ip="192.168.192.18"),
+        _udp_state(joints=[0.0] * 6),
+        _udp_state(joints=[0.0, 0.0, 0.0, math.nan, 0.0, 0.0, 0.0]),
+    ],
+)
+def test_invalid_udp_feedback_does_not_replace_snapshot(state) -> None:
+    adapter = RealManAdapter(
+        "127.0.0.1",
+        8080,
+        0,
+        "127.0.0.1",
+        8090,
+    )
+    adapter._accept_realtime_feedback = True
+    adapter._on_realtime_arm_state(_udp_state(joints=[1.0] * 7))
+    before = adapter.get_latest_joint_state()
+
+    adapter._on_realtime_arm_state(state)
+
+    assert adapter.get_latest_joint_state() == before
+```
+
+Add a fake vendor module that records callback registration and push config.
+Its `rm_set_realtime_push()` invokes the registered callback with `_udp_state()`.
+Assert:
+
+```python
+adapter.connect()
+arm = fake_module.RoboticArm.instance
+assert arm.config.args == (5, True, 8090, 0, "192.168.192.148")
+assert arm.callback is adapter._realtime_callback
+assert adapter.get_latest_joint_state() is not None
+assert not hasattr(adapter, "_feedback_thread")
+```
+
+Add failure cases where `rm_set_realtime_push()` returns `1`, and where
+`adapter._feedback_ready.wait` returns `False`. Both must raise `RuntimeError`;
+the fake arm must record one `rm_delete_robot_arm()` call.
+
+- [ ] **Step 2: Run the focused tests and verify RED**
+
+Run:
+
+```bash
+cd /home/robot/WS_xr
+source /opt/ros/humble/setup.bash
+python3 -m pytest -q src/xr_rm_teleop/test/test_initial_joint_pose.py
+```
+
+Expected: FAIL because `RealManAdapter` does not accept realtime push
+parameters and has no `_on_realtime_arm_state`.
+
+---
+
+### Task 2: Implement single-handle UDP feedback
+
+**Files:**
+- Modify: `xr_rm_teleop/xr_rm_teleop/realman_adapter.py`
+- Modify: `xr_rm_teleop/xr_rm_teleop/single_arm_velocity_teleop.py`
+- Modify: `xr_rm_teleop/test/test_initial_joint_pose.py`
+
+- [ ] **Step 1: Replace polling constructor state with realtime push state**
+
+Change `RealManAdapter.__init__` positional parameters from `feedback_period`
+to:
+
+```python
+realtime_push_host_ip: str,
+realtime_push_port: int,
+realtime_push_cycle_ms: int = 5,
+```
+
+Validate with stdlib `ipaddress.IPv4Address`:
+
+```python
+try:
+    self._realtime_push_host_ip = str(
+        ipaddress.IPv4Address(realtime_push_host_ip)
+    )
+except ipaddress.AddressValueError as exc:
+    raise ValueError("realtime_push_host_ip must be a valid IPv4 address") from exc
+if not 1 <= realtime_push_port <= 65535:
+    raise ValueError("realtime_push_port must be between 1 and 65535")
+if realtime_push_cycle_ms <= 0 or realtime_push_cycle_ms % 5 != 0:
+    raise ValueError("realtime_push_cycle_ms must be a positive multiple of 5")
+```
+
+Store the port and cycle, then replace feedback thread members with:
+
+```python
+self._feedback_ready = threading.Event()
+self._realtime_callback: Any | None = None
+self._accept_realtime_feedback = False
+self._feedback_fault_logged = False
+```
+
+- [ ] **Step 2: Configure callback and UDP push during connect**
+
+Import these SDK symbols inside `connect()` so mock mode stays SDK-free:
+
+```python
+from Robotic_Arm.rm_robot_interface import (
+    RoboticArm,
+    rm_realtime_arm_state_callback_ptr,
+    rm_realtime_push_config_t,
+    rm_thread_mode_e,
+)
+```
+
+After existing safety and optional initial-pose configuration:
+
+```python
+self._feedback_ready.clear()
+self._accept_realtime_feedback = True
+self._realtime_callback = rm_realtime_arm_state_callback_ptr(
+    self._on_realtime_arm_state
+)
+self._arm.rm_realtime_arm_state_call_back(self._realtime_callback)
+config = rm_realtime_push_config_t(
+    self._realtime_push_cycle_ms,
+    True,
+    self._realtime_push_port,
+    0,
+    self._realtime_push_host_ip,
+)
+self._check_return(
+    self._arm.rm_set_realtime_push(config),
+    "rm_set_realtime_push",
+)
+if not self._feedback_ready.wait(timeout=2.0):
+    raise RuntimeError(
+        "RealMan UDP realtime feedback did not receive a valid frame within 2 seconds"
+    )
+```
+
+Wrap post-handle initialization so any exception disables callback acceptance,
+deletes the handle, sets `_arm = None`, and re-raises.
+
+- [ ] **Step 3: Implement the bounded callback**
+
+Replace `_feedback_loop()` and `_read_joint_state_once()` with:
+
+```python
+def _on_realtime_arm_state(self, data: Any) -> None:
+    if not self._accept_realtime_feedback:
+        return
+    try:
+        if data is None or int(data.errCode) != 0:
+            raise ValueError("invalid realtime feedback error code")
+        arm_ip = data.arm_ip
+        if isinstance(arm_ip, bytes):
+            arm_ip = arm_ip.decode("utf-8").split("\x00", 1)[0]
+        if str(arm_ip) != self._robot_ip:
+            raise ValueError(f"unexpected realtime feedback source: {arm_ip}")
+        degrees = list(data.joint_status.joint_position)
+        if (
+            len(degrees) != 7
+            or not all(isinstance(value, Number) for value in degrees)
+        ):
+            raise ValueError("RM75 UDP feedback must contain 7 numeric joints")
+        positions = [math.radians(float(value)) for value in degrees]
+        if not all(math.isfinite(value) for value in positions):
+            raise ValueError("RM75 UDP feedback contains NaN/Inf")
+        received_at = time.monotonic()
+        with self._joint_state_lock:
+            update_interval_ms = (
+                None
+                if self._latest_joint_state is None
+                else (received_at - self._latest_joint_state.received_at) * 1000.0
+            )
+            self._latest_joint_state = JointStateSnapshot(
+                positions,
+                received_at,
+                None,
+                update_interval_ms,
+            )
+        self._feedback_fault_logged = False
+        self._feedback_ready.set()
+    except Exception as exc:
+        if not self._feedback_fault_logged:
+            self._log_warn(f"RealMan UDP realtime feedback invalid: {exc}")
+            self._feedback_fault_logged = True
+```
+
+In `close()`, set `_accept_realtime_feedback = False` before slow-stop and
+handle deletion. Remove feedback thread stop/join logic. Keep the callback
+reference alive until after `rm_delete_robot_arm()`.
+
+- [ ] **Step 4: Declare and pass ROS parameters**
+
+In `SingleArmVelocityTeleop`, declare:
+
+```python
+self.declare_parameter("realtime_push_host_ip", "")
+self.declare_parameter("realtime_push_port", 0)
+self.declare_parameter("realtime_push_cycle_ms", 5)
+```
+
+Replace `feedback_period=self._dt` in `_make_adapter()` with:
+
+```python
+realtime_push_host_ip=str(
+    self.get_parameter("realtime_push_host_ip").value
+),
+realtime_push_port=int(
+    self.get_parameter("realtime_push_port").value
+),
+realtime_push_cycle_ms=int(
+    self.get_parameter("realtime_push_cycle_ms").value
+),
+```
+
+Update all direct `RealManAdapter(...)` calls in tests to pass a host and port.
+
+- [ ] **Step 5: Run focused tests and verify GREEN**
+
+Run:
+
+```bash
+cd /home/robot/WS_xr
+source /opt/ros/humble/setup.bash
+python3 -m pytest -q src/xr_rm_teleop/test/test_initial_joint_pose.py
+```
+
+Expected: all focused tests pass, with no real SDK connection.
+
+---
+
+### Task 3: Move robot defaults into YAML and simplify launch
+
+**Files:**
+- Modify: `xr_rm_bringup/config/right_arm_rm75.yaml`
+- Modify: `xr_rm_bringup/config/left_arm_rm75.yaml`
+- Modify: `xr_rm_bringup/config/dual_arm_rm75.yaml`
+- Modify: `xr_rm_bringup/launch/arm_debug.launch.py`
+
+- [ ] **Step 1: Run a failing ownership assertion**
+
+Run a one-off Python assertion that requires the three YAMLs to contain UDP
+and tool parameters, and requires launch not to declare robot behavior
+arguments:
+
+```python
+from pathlib import Path
+import yaml
+
+config_dir = Path("xr_rm_bringup/config")
+for name in ("left_arm_rm75.yaml", "right_arm_rm75.yaml"):
+    params = yaml.safe_load((config_dir / name).read_text())
+    params = params["single_arm_velocity_teleop"]["ros__parameters"]
+    assert "use_mock" not in params
+    assert params["realtime_push_host_ip"] == "192.168.192.148"
+    assert params["realtime_push_cycle_ms"] == 5
+    assert params["enable_tool_control"] is True
+
+source = Path("xr_rm_bringup/launch/arm_debug.launch.py").read_text()
+for name in (
+    "left_robot_ip",
+    "right_robot_ip",
+    "robot_port",
+    "avoid_singularity",
+    "control_rate_hz",
+    "follow",
+    "configure_safety_limits",
+    "move_to_initial_pose_on_connect",
+):
+    assert f'DeclareLaunchArgument("{name}"' not in source
+```
+
+Expected: FAIL because the YAML parameters are missing and launch still
+declares overrides.
+
+- [ ] **Step 2: Update all YAML nodes**
+
+Remove `use_mock`. Add:
+
+```yaml
+realtime_push_host_ip: 192.168.192.148
+realtime_push_cycle_ms: 5
+enable_tool_control: true
+enable_trigger_gripper_control: true
+trigger_close_threshold: 0.95
+configure_peripheral_on_connect: true
+```
+
+Use `realtime_push_port: 8089` for left-arm nodes and `8090` for right-arm
+nodes. Keep:
+
+```yaml
+# all single-arm and dual-arm nodes
+follow: false
+canfd_trajectory_mode: 2
+```
+
+The right-arm high-follow default was reverted after the first hardware test
+exposed an unplanned stationary null-space trajectory. Do not change speeds,
+workspace limits, timeouts, safety limits, or initial pose defaults.
+
+- [ ] **Step 3: Reduce launch overrides**
+
+Make `_single_arm_node(arm, use_mock)` and `_dual_arm_nodes(use_mock)` load
+their YAML first, then pass only:
+
+```python
+{
+    "use_mock": use_mock,
+    "robot_urdf_path": _rm75_urdf(),
+    "peripheral_config_file": _config_file("peripherals_rm75.yaml"),
+    "peripheral_arm": arm,
+    "tool_command_topic": f"/xr_rm/{_arm_name(arm)}/tool_enable",
+}
+```
+
+Keep equivalent per-side generated values in dual mode. Remove
+`_initial_pose_override`, robot IP/port, avoid-singularity, control-rate,
+follow, safety, tool-control and initial-pose parsing from `_launch_setup`.
+
+Keep only these launch arguments:
+
+```python
+DeclareLaunchArgument("arm", default_value="right")
+DeclareLaunchArgument("use_mock", default_value="true")
+DeclareLaunchArgument("udp_host", default_value="0.0.0.0")
+DeclareLaunchArgument("udp_port", default_value="15000")
+DeclareLaunchArgument("udp_timer_hz", default_value="200.0")
+```
+
+- [ ] **Step 4: Re-run ownership assertion and inspect launch arguments**
+
+Run the assertion from Step 1, then:
+
+```bash
+cd /home/robot/WS_xr
+source /opt/ros/humble/setup.bash
+source install/setup.bash
+ros2 launch xr_rm_bringup arm_debug.launch.py --show-args
+```
+
+Expected: the assertion passes; launch lists only `arm`, `use_mock`,
+`udp_host`, `udp_port`, and `udp_timer_hz`.
+
+---
+
+### Task 4: Synchronize launcher UI and README
+
+**Files:**
+- Modify: `xr_rm_bringup/tools/launcher_ui.py`
+- Modify: `README.md`
+
+- [ ] **Step 1: Remove deleted launch arguments from UI commands**
+
+Keep ping targets unchanged. Change real launch commands to:
+
+```python
+"ros2 launch xr_rm_bringup arm_debug.launch.py arm:=left use_mock:=false"
+"ros2 launch xr_rm_bringup arm_debug.launch.py arm:=right use_mock:=false"
+"ros2 launch xr_rm_bringup arm_debug.launch.py arm:=both use_mock:=false"
+```
+
+- [ ] **Step 2: Update README ownership and commands**
+
+Remove examples and launch-argument descriptions for robot IP/port,
+avoid-singularity, control-rate, follow, safety/tool flags, and initial-pose
+overrides. State that these values live in the selected YAML. Add the UDP
+feedback parameters, host `192.168.192.148`, ports `8089/8090`, 5 ms cycle,
+and the command used after Wi-Fi changes:
+
+```bash
+ip -4 route get 192.168.192.19
+```
+
+Keep `arm`, `use_mock`, and PICO UDP arguments documented as launch
+arguments. Keep the warning that checked-in default `use_mock=true` prevents
+an accidental real connection.
+
+- [ ] **Step 3: Check syntax and stale references**
+
+Run:
+
+```bash
+python3 -m py_compile \
+  xr_rm_bringup/launch/arm_debug.launch.py \
+  xr_rm_bringup/tools/launcher_ui.py
+rg -n "left_robot_ip:=|right_robot_ip:=|move_to_initial_pose_on_connect:=" \
+  README.md xr_rm_bringup/tools/launcher_ui.py
+```
+
+Expected: compilation passes; `rg` returns no stale command-line overrides.
+
+---
+
+### Task 5: Full verification
+
+**Files:**
+- Verify all files changed by Tasks 1–4
+
+- [ ] **Step 1: Run teleop tests**
+
+```bash
+cd /home/robot/WS_xr
+source /opt/ros/humble/setup.bash
+python3 -m pytest -q src/xr_rm_teleop/test
+python3 -m pytest -q src/xr_rm_teleop/test/test_orientation_control.py
+```
+
+Expected: all tests pass.
+
+- [ ] **Step 2: Build all workspace packages**
+
+```bash
+cd /home/robot/WS_xr
+source /opt/ros/humble/setup.bash
+colcon build --symlink-install --executor sequential
+```
+
+Expected: `xr_rm_interfaces`, `xr_rm_input`, `xr_rm_teleop`, and
+`xr_rm_bringup` all finish successfully.
+
+- [ ] **Step 3: Verify mock launch without vendor hardware**
+
+```bash
+cd /home/robot/WS_xr
+source /opt/ros/humble/setup.bash
+source install/setup.bash
+timeout 8s ros2 launch xr_rm_bringup arm_debug.launch.py \
+  arm:=right use_mock:=true
+```
+
+Expected: the mock teleop and UDP input nodes start; timeout ends the launch.
+No RealMan SDK connection is attempted.
+
+- [ ] **Step 4: Inspect final diff**
+
+```bash
+cd /home/robot/WS_xr/src
+git diff --check
+git status --short
+git diff --stat
+```
+
+Expected: no whitespace errors and no unrelated files. Do not commit, push,
+or connect to the real robot unless the user explicitly requests it.
@@ -0,0 +1,42 @@
+# RM75 Right-Arm High-Follow YAML Defaults Implementation Plan
+
+> **For agentic workers:** REQUIRED SUB-SKILL: Use superpowers:subagent-driven-development (recommended) or superpowers:executing-plans to implement this plan task-by-task. Steps use checkbox (`- [ ]`) syntax for tracking.
+
+**Goal:** Make right-arm single-arm debugging default to RealMan high-follow complete passthrough for phase-two testing.
+
+**Architecture:** Change only the existing right-arm YAML parameters. Keep launch, left-arm, dual-arm, speed limits, safety limits, timeouts, and stop behavior unchanged.
+
+**Tech Stack:** ROS2 Humble, YAML, pytest, colcon
+
+---
+
+### Task 1: Change right-arm CANFD defaults
+
+**Files:**
+- Modify: `xr_rm_bringup/config/right_arm_rm75.yaml`
+
+- [ ] **Step 1: Run a failing configuration assertion**
+
+Run a Python YAML assertion requiring `follow is True` and
+`canfd_trajectory_mode == 0`.
+
+Expected: FAIL because the current values are `false` and `2`.
+
+- [ ] **Step 2: Apply the minimal configuration change**
+
+```yaml
+follow: true
+canfd_trajectory_mode: 0
+```
+
+- [ ] **Step 3: Verify configuration and regressions**
+
+Run the same YAML assertion and expect PASS. Then run:
+
+```bash
+source /opt/ros/humble/setup.bash
+python3 -m pytest -q src/xr_rm_teleop/test
+colcon build --symlink-install --executor sequential
+```
+
+Expected: all tests and all four workspace packages pass.
@@ -0,0 +1,198 @@
+# RM75 CANFD UDP 主动反馈设计
+
+> 真机验证修正：右臂高跟随首轮测试触发掉使能。离线复现发现静止目标存在
+> `23.582°` 空空间漂移，第一周期约 `3315°/s²`。当前实现已移除
+> manipulability 自运动、增加软件关节加速度限幅和故障 Grip 锁存，并将三份
+> YAML 恢复为 `follow: false` 安全基线；高跟随须在基线验证后单独测试。
+
+## 目标
+
+按睿尔曼 MovejCANFD 示例，将真机关节反馈从同一 TCP 控制连接上的
+`rm_get_joint_degree()` 周期轮询，替换为控制器 UDP 主动状态推送。
+
+控制命令继续通过现有单个 `RoboticArm(RM_TRIPLE_MODE_E)` 句柄发送，不新增
+RealMan 连接，不修改 Placo QP、工作空间/圆柱限位、速度限制、指令超时或安全
+停止条件。
+
+## 根因与证据
+
+低跟随模式下，125 Hz CANFD 发送与绝对周期 TCP 反馈轮询能够同时工作：
+
+- `feedback_interval mean=10.041–10.389 ms`；
+- `feedback_age mean=5.730–6.138 ms`；
+- 控制周期最大值不超过 `8.979 ms`。
+
+启用高跟随后，即使 `canfd_trajectory_mode=2`，控制发送仍正常：
+
+- `period max=9.275 ms`；
+- `send max=0.235 ms`。
+
+但同步反馈退化为：
+
+- `feedback_read mean=11.278 ms`、`max=80.320 ms`；
+- `feedback_age max=117.502 ms`。
+
+反馈年龄逼近现有 `command_timeout_sec=0.12 s`，触发“关节反馈缺失或过期”
+安全停止，造成 Grip 按住期间控制反复退出和重新锁定。增大超时只会允许 QP
+继续使用更旧的关节状态，不解决 TCP 反馈阻塞。
+
+睿尔曼 MovejCANFD 示例使用三线程模式、`rm_set_realtime_push()` 和
+`rm_realtime_arm_state_call_back()`，通过 UDP 回调获取关节状态，而不是在
+CANFD 透传期间同步轮询关节角。
+
+## 数据流
+
+```text
+PICO -> ROS 125 Hz 控制回调 -> Placo QP -> TCP rm_movej_canfd
+
+RM75 控制器 -> UDP 5 ms 主动推送 -> SDK 第三线程回调
+             -> JointStateSnapshot 缓存 -> ROS 125 Hz 控制回调
+```
+
+TCP 仍承担 CANFD、慢停、安全配置和末端工具命令。UDP 只承担状态反馈，两条
+传输路径共用同一个机械臂句柄。
+
+## SDK 连接与反馈生命周期
+
+`RealManAdapter.connect()` 保持三线程模式和单次 `rm_create_robot_arm()`：
+
+1. 创建并检查机械臂句柄。
+2. 下发已有安全参数和可选初始位姿。
+3. 创建并保存 `rm_realtime_arm_state_callback_ptr`，避免 Python 回调被垃圾
+   回收。
+4. 使用 `rm_realtime_push_config_t` 配置 5 ms UDP 主动上报。
+5. 注册 `rm_realtime_arm_state_call_back()`。
+6. 等待第一帧有效 UDP 反馈，最长 2 秒。
+
+若配置接口返回非零，或 2 秒内没有有效反馈，连接初始化失败并删除已创建的
+机械臂句柄；不静默回退到 TCP 轮询。
+
+连接成功后不再创建反馈线程，也不再调用 `rm_get_joint_degree()`。
+`close()` 先停止接受回调更新，再执行现有慢停和句柄删除。控制器的 UDP 配置
+由下一次启动重新覆盖，不额外增加关闭阶段配置命令。
+
+## UDP 回调与缓存
+
+回调只执行有界、非阻塞工作：
+
+1. 检查回调对象、`errCode` 和来源机械臂 IP。
+2. 读取 7 个 `joint_position`，检查数量、数值类型及 NaN/Inf。
+3. 将厂商反馈的角度转换为弧度。
+4. 使用 `time.monotonic()` 记录接收时刻，并计算与上一帧的更新间隔。
+5. 在现有 `_joint_state_lock` 下替换 `JointStateSnapshot`。
+6. 第一帧有效数据唤醒连接初始化等待。
+
+无效 UDP 帧不覆盖上一帧缓存。若后续持续丢包，现有 120 ms 新鲜度检查自然
+触发安全停止。
+
+UDP 回调不执行 QP、ROS 发布、停止命令或其他 SDK 调用，避免阻塞 SDK 接收
+线程。
+
+## 参数与三份 YAML
+
+新增真机参数：
+
+- `realtime_push_host_ip`：机械臂可直接访问的上位机地址；
+- `realtime_push_port`：单臂 UDP 接收端口；
+- `realtime_push_cycle_ms`：主动上报周期，默认并配置为 `5`。
+
+本次现场配置：
+
+| 配置 | 节点 | host | port |
+|---|---|---|---:|
+| `right_arm_rm75.yaml` | 右臂 | `192.168.192.148` | 8090 |
+| `left_arm_rm75.yaml` | 左臂 | `192.168.192.148` | 8089 |
+| `dual_arm_rm75.yaml` | 左臂 | `192.168.192.148` | 8089 |
+| `dual_arm_rm75.yaml` | 右臂 | `192.168.192.148` | 8090 |
+
+左右臂端口必须不同。更换上位机或网络后，只需同步修改 YAML 中的
+`realtime_push_host_ip`。
+
+Mock 模式不导入厂商 SDK，也不要求 UDP 参数有效。
+
+## YAML 与 launch 参数所有权
+
+此前 `arm_debug.launch.py` 会用 launch 默认值覆盖 YAML 中的机械臂参数，
+导致 YAML 无法单独控制 `follow` 等行为。
+
+用户选择由 YAML 作为机械臂行为和硬件参数的唯一默认来源。
+
+以下参数只由 `left_arm_rm75.yaml`、`right_arm_rm75.yaml` 和
+`dual_arm_rm75.yaml` 管理，launch 不再声明或覆盖：
+
+- `robot_ip`、`robot_port`；
+- `avoid_singularity`；
+- `control_rate_hz`；
+- `follow`、`canfd_trajectory_mode`、`canfd_radio`；
+- `configure_safety_limits`；
+- `move_to_initial_pose_on_connect`；
+- `enable_tool_control`、`enable_trigger_gripper_control`；
+- `trigger_close_threshold`；
+- `configure_peripheral_on_connect`；
+- 本设计新增的 UDP 主动反馈参数。
+
+三份 YAML 补齐工具控制参数；删除其中不再生效的 `use_mock`，避免出现两个
+配置来源。
+
+`arm_debug.launch.py` 只保留：
+
+- `arm=left|right|both`，选择启动拓扑；
+- `use_mock=true|false`，作为显式安全运行模式，默认仍为 `true`；
+- PICO 输入节点的 `udp_host`、`udp_port`、`udp_timer_hz`；
+- launch 根据安装路径和左右臂生成的 `robot_urdf_path`、
+  `peripheral_config_file`、`peripheral_arm` 和 `tool_command_topic`。
+
+`launcher_ui.py` 和 README 中的启动命令同步删除机械臂 IP、初始化移动等已
+移交 YAML 的 launch 参数，只保留 `arm`、`use_mock` 和 PICO 输入覆盖。
+
+控制模式保持分阶段范围：
+
+- 三份 YAML 均使用 `follow: false`、`canfd_trajectory_mode: 2` 完成安全基线；
+- 基线验证通过前不启用高跟随或模式 0，不提高 `max_linear_speed`。
+
+## Timing 日志
+
+保留：
+
+- `period`；
+- `total`；
+- `qp`；
+- `send`；
+- `feedback_age`；
+- `feedback_interval`。
+
+`feedback_read` 表示同步 SDK 查询耗时；UDP 架构不存在该查询，因此该字段
+不再产生样本，现有条件日志逻辑会自动省略它，不新增同义统计项。
+
+## 测试
+
+使用 FakeArm 和伪造 SDK 模块覆盖：
+
+- 连接时使用正确 host、port、5 ms 周期配置 UDP 并注册回调；
+- 第一帧有效回调转换 7 个关节角为弧度并解除启动等待；
+- 连续回调正确记录 `feedback_interval`；
+- 错误码、来源 IP、长度或 NaN/Inf 无效帧不覆盖缓存；
+- UDP 配置失败或首帧超时会清理句柄并抛出明确异常；
+- 真机适配器不再启动轮询线程或调用 `rm_get_joint_degree()`；
+- Mock 模式无需厂商 SDK。
+- launch 不再覆盖 YAML 的机械臂行为和硬件参数；
+- `use_mock` 仍由 launch 默认设为 `true`；
+- `launcher_ui.py` 不再传递已经删除的 launch 参数。
+
+随后运行：
+
+```bash
+cd /home/robot/WS_xr
+source /opt/ros/humble/setup.bash
+python3 -m pytest -q src/xr_rm_teleop/test
+python3 -m pytest -q src/xr_rm_teleop/test/test_orientation_control.py
+colcon build --symlink-install --executor sequential
+```
+
+Codex 不连接真机。用户在右臂小范围测试中确认：
+
+- 启动日志显示收到 UDP 首帧；
+- 按住 Grip 不再出现反馈过期或 SDK `-2`；
+- 连续四个 timing 窗口 `period max < 10 ms`、`total max < 8 ms`；
+- `feedback_interval mean` 接近 5 ms；
+- `feedback_age mean < 5 ms`，且最大值不触发 120 ms 安全停止。