feat: Implement UDP feedback for RM75 robot arms

README.md

@@ -207,23 +207,18 @@ 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
 ```
 
-所有配置默认都不会执行 `movej(initial_joint_pose)`；只有确认安全区清空后，才可显式传入
-`move_to_initial_pose_on_connect:=true`。
+所有 YAML 默认都不会执行 `movej(initial_joint_pose)`。只有确认安全区清空后，才可在当前使用的
+`left_arm_rm75.yaml`、`right_arm_rm75.yaml` 或 `dual_arm_rm75.yaml` 中将
+`move_to_initial_pose_on_connect` 改为 `true`。
 
 第四步：双臂真机。
 
 ```bash
-ros2 launch xr_rm_bringup arm_debug.launch.py arm:=both use_mock:=false \
-  left_robot_ip:=192.168.192.18 \
-  right_robot_ip:=192.168.192.19
+ros2 launch xr_rm_bringup arm_debug.launch.py arm:=both use_mock:=false
 ```
 
-双臂默认不会自动移动到初始化点。以后需要启用时，在确认安全区清空后显式打开：
-
-```bash
-ros2 launch xr_rm_bringup arm_debug.launch.py arm:=both use_mock:=false \
-  move_to_initial_pose_on_connect:=true
-```
+双臂默认不会自动移动到初始化点；机器人地址、控制参数和初始化移动开关均从
+`dual_arm_rm75.yaml` 读取。
 
 ## Launch 入口说明
 
@@ -236,19 +231,9 @@ ros2 launch xr_rm_bringup arm_debug.launch.py arm:=both use_mock:=false \
 - `udp_host`：UDP 监听地址，默认 `0.0.0.0`。
 - `udp_port`：UDP 监听端口，默认 `15000`。
 - `udp_timer_hz`：UDP receiver 轮询频率，默认 `200.0`。
-- `left_robot_ip`：左臂 IP，默认 `192.168.192.18`。
-- `right_robot_ip`：右臂 IP，默认 `192.168.192.19`。
-- `robot_port`：RM75 TCP 端口，默认 `8080`。
-- `left_avoid_singularity` / `right_avoid_singularity`：左右臂避奇异参数，默认左 `0`、右 `1`。
-- `avoid_singularity`：非空时覆盖左右臂避奇异参数。
-- `control_rate_hz`：同步关节反馈、执行一次 QP 并发送一次关节目标的频率，默认 `125.0`。
-- `follow`：传给 `rm_movej_canfd` 的跟随标志，默认 `false`。
-- `configure_safety_limits`：连接真机后是否配置速度/加速度安全参数，默认 `true`。
-- `enable_tool_control`：是否在遥操作节点内启用末端工具控制 topic，默认 `true`。
-- `enable_trigger_gripper_control`：是否允许用 `trigger` 点击切换对应夹爪状态，默认 `true`。
-- `trigger_close_threshold`：trigger 点击判定阈值，默认 `0.95`。
-- `configure_peripheral_on_connect`：遥操作节点连接真机后是否配置末端外设，默认 `true`；工具控制会复用同一个 RealMan 连接，避免两个进程同时抢占同一机械臂。
-- `move_to_initial_pose_on_connect`：连接后是否执行 `movej(initial_joint_pose)`；默认 `auto`，沿用 YAML 中的 `false`，也可显式传 `true`/`false` 覆盖。
+
+机器人 IP/端口、控制频率、CANFD、限速、工具和初始化位姿等行为参数只由对应 YAML
+配置，launch 不再提供同名覆盖项。
 
 ## 配置文件说明
 
@@ -274,6 +259,13 @@ ros2 launch xr_rm_bringup arm_debug.launch.py arm:=both use_mock:=false \
 - `workspace_min` / `workspace_max`：笛卡尔工作空间边界。
 - `cyl_radius_limit`：基座圆柱半径限制。
 - `xr_to_robot_matrix`：`/xr/*_controller` Project 位移到 RM75 base 坐标的映射矩阵。
+- `robot_ip` / `robot_port`：RM75 TCP 控制连接地址。
+- `realtime_push_host_ip`：连接机械臂 Wi-Fi 后本机实际 IPv4；可用
+  `ip -4 route get 192.168.192.19` 查看输出中的 `src`，当前为 `192.168.192.148`。
+- `realtime_push_port`：UDP 主动反馈端口；左臂 `8089`、右臂 `8090`，同机双臂不能重复。
+- `realtime_push_cycle_ms`：UDP 主动反馈周期，当前为厂商支持的 `5 ms`。
+- `follow` / `canfd_trajectory_mode`：`rm_movej_canfd` 的高跟随和轨迹模式参数。
+- 当前三份 YAML 默认均使用 `follow: false` 完成安全基线验证；确认关节加速度与反馈稳定后，再单独测试高跟随。
 - `initial_joint_pose`：mock 的初始关节反馈，以及显式开启初始化移动时的真机初始关节角。
 
 当前 `/xr/*_controller` 的 Project 坐标约定：
@@ -432,7 +424,7 @@ ros2 topic echo /xr/right_controller --field trigger
 
 1. 确认急停、网络、机械臂工作区和人员位置。
 2. `launcher_ui.py` 中先 `Ping Left RM75` 或 `Ping Right RM75`。
-3. 单臂启动，`move_to_initial_pose_on_connect:=false`。
+3. 确认对应 YAML 中 `move_to_initial_pose_on_connect: false` 后单臂启动。
 4. 手握急停，按住 `grip` 后只做小幅单轴移动。
 5. 逐个确认上/下、前/后、左/右方向。
 6. 小角度转动手柄，确认 `/xr_rm/<arm>/target_pose` 姿态和 `/xr_rm/<arm>/cmd_vel.twist.angular` 变化符合预期。

docs/superpowers/plans/2026-07-29-rm75-canfd-udp-feedback.md

@@ -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.

docs/superpowers/plans/2026-07-29-rm75-high-follow-yaml-defaults.md

@@ -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.

docs/superpowers/specs/2026-07-29-rm75-canfd-udp-feedback-design.md

@@ -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 安全停止。

xr_rm_bringup/config/dual_arm_rm75.yaml

@@ -39,14 +39,20 @@ left_arm_teleop:
                          0.0, 0.0, 1.0,
                          -1.0, 0.0, 0.0]
 
-    use_mock: false
     robot_ip: 192.168.192.18
     robot_port: 8080
+    realtime_push_host_ip: 192.168.192.148
+    realtime_push_port: 8089
+    realtime_push_cycle_ms: 5
     avoid_singularity: 0
     follow: false
     canfd_trajectory_mode: 2
     canfd_radio: 0
     configure_safety_limits: true
+    enable_tool_control: true
+    enable_trigger_gripper_control: true
+    trigger_close_threshold: 0.95
+    configure_peripheral_on_connect: true
     max_line_speed: 1.0
     max_angular_speed: 1.5
     max_line_acc: 1.0
@@ -89,14 +95,20 @@ right_arm_teleop:
                          0.0, 0.0, 1.0,
                          1.0, 0.0, 0.0]
 
-    use_mock: false
     robot_ip: 192.168.192.19
     robot_port: 8080
+    realtime_push_host_ip: 192.168.192.148
+    realtime_push_port: 8090
+    realtime_push_cycle_ms: 5
     avoid_singularity: 1
     follow: false
     canfd_trajectory_mode: 2
     canfd_radio: 0
     configure_safety_limits: true
+    enable_tool_control: true
+    enable_trigger_gripper_control: true
+    trigger_close_threshold: 0.95
+    configure_peripheral_on_connect: true
     max_line_speed: 1.0
     max_angular_speed: 1.5
     max_line_acc: 1.0

xr_rm_bringup/config/left_arm_rm75.yaml

@@ -32,14 +32,20 @@ single_arm_velocity_teleop:
                          0.0, 0.0, 1.0,
                          -1.0, 0.0, 0.0]
 
-    use_mock: false
     robot_ip: 192.168.192.18
     robot_port: 8080
+    realtime_push_host_ip: 192.168.192.148
+    realtime_push_port: 8089
+    realtime_push_cycle_ms: 5
     avoid_singularity: 0
     follow: false
     canfd_trajectory_mode: 2
     canfd_radio: 0
     configure_safety_limits: true
+    enable_tool_control: true
+    enable_trigger_gripper_control: true
+    trigger_close_threshold: 0.95
+    configure_peripheral_on_connect: true
     max_line_speed: 1.0
     max_angular_speed: 1.5
     max_line_acc: 1.0

xr_rm_bringup/config/right_arm_rm75.yaml

@@ -31,14 +31,21 @@ single_arm_velocity_teleop:
                          0.0, 0.0, 1.0,
                          1.0, 0.0, 0.0]
 
-    use_mock: false
     robot_ip: 192.168.192.19
     robot_port: 8080
+    realtime_push_host_ip: 192.168.192.148
+    realtime_push_port: 8090
+    realtime_push_cycle_ms: 5
     avoid_singularity: 1
-    follow: False
+    # 厂商 MovejCANFD 示例默认低跟随；高跟随仅在验证规划轨迹后单独开启。
+    follow: false
     canfd_trajectory_mode: 2
     canfd_radio: 0
     configure_safety_limits: true
+    enable_tool_control: true
+    enable_trigger_gripper_control: true
+    trigger_close_threshold: 0.95
+    configure_peripheral_on_connect: true
     max_line_speed: 0.25
     max_angular_speed: 0.6
     max_line_acc: 1.3

xr_rm_bringup/launch/arm_debug.launch.py

@@ -41,10 +41,6 @@ def _rm75_urdf() -> PathJoinSubstitution:
     ])
 
 
-def _initial_pose_override(value: str) -> dict[str, bool]:
-    return {} if value == "auto" else {"move_to_initial_pose_on_connect": _as_bool(value)}
-
-
 def _udp_receiver_node() -> Node:
     """接收 PICO/XR UDP 数据，并发布左右手柄 ROS2 话题。"""
     return Node(
@@ -65,19 +61,9 @@ def _udp_receiver_node() -> Node:
 def _single_arm_node(
     arm: str,
     use_mock: bool,
-    move_to_initial_pose: str,
-    avoid_singularity: int,
-    control_rate_hz: float,
-    follow: bool,
-    configure_safety_limits: bool,
-    enable_tool_control: bool,
-    enable_trigger_gripper_control: bool,
-    trigger_close_threshold: float,
-    configure_peripheral_on_connect: bool,
 ) -> Node:
     """创建单臂调试节点；左/右臂分别使用独立 YAML，节点名保持单臂默认名。"""
     config_name = "left_arm_rm75.yaml" if arm == "left" else "right_arm_rm75.yaml"
-    robot_ip = LaunchConfiguration("left_robot_ip" if arm == "left" else "right_robot_ip")
     arm_name = _arm_name(arm)
     return Node(
         package="xr_rm_teleop",
@@ -89,18 +75,7 @@ def _single_arm_node(
             _config_file(config_name),
             {
                 "use_mock": use_mock,
-                "robot_ip": robot_ip,
-                "robot_port": LaunchConfiguration("robot_port"),
-                "avoid_singularity": avoid_singularity,
                 "robot_urdf_path": _rm75_urdf(),
-                "control_rate_hz": control_rate_hz,
-                "follow": follow,
-                "configure_safety_limits": configure_safety_limits,
-                **_initial_pose_override(move_to_initial_pose),
-                "enable_tool_control": enable_tool_control,
-                "enable_trigger_gripper_control": enable_trigger_gripper_control,
-                "trigger_close_threshold": trigger_close_threshold,
-                "configure_peripheral_on_connect": configure_peripheral_on_connect,
                 "peripheral_config_file": _config_file("peripherals_rm75.yaml"),
                 "peripheral_arm": arm,
                 "tool_command_topic": f"/xr_rm/{arm_name}/tool_enable",
@@ -113,19 +88,7 @@ def _arm_name(arm: str) -> str:
     return "left_rm75" if arm == "left" else "right_rm75"
 
 
-def _dual_arm_nodes(
-    use_mock: bool,
-    move_to_initial_pose: str,
-    left_avoid_singularity: int,
-    right_avoid_singularity: int,
-    control_rate_hz: float,
-    follow: bool,
-    configure_safety_limits: bool,
-    enable_tool_control: bool,
-    enable_trigger_gripper_control: bool,
-    trigger_close_threshold: float,
-    configure_peripheral_on_connect: bool,
-) -> list[Node]:
+def _dual_arm_nodes(use_mock: bool) -> list[Node]:
     """创建双臂节点；两个节点共用双臂 YAML，但节点名区分左右臂参数命名空间。"""
     config_file = _config_file("dual_arm_rm75.yaml")
     return [
@@ -139,18 +102,7 @@ def _dual_arm_nodes(
                 config_file,
                 {
                     "use_mock": use_mock,
-                    "robot_ip": LaunchConfiguration("left_robot_ip"),
-                    "robot_port": LaunchConfiguration("robot_port"),
-                    "avoid_singularity": left_avoid_singularity,
                     "robot_urdf_path": _rm75_urdf(),
-                    "control_rate_hz": control_rate_hz,
-                    "follow": follow,
-                    "configure_safety_limits": configure_safety_limits,
-                    **_initial_pose_override(move_to_initial_pose),
-                    "enable_tool_control": enable_tool_control,
-                    "enable_trigger_gripper_control": enable_trigger_gripper_control,
-                    "trigger_close_threshold": trigger_close_threshold,
-                    "configure_peripheral_on_connect": configure_peripheral_on_connect,
                     "peripheral_config_file": _config_file("peripherals_rm75.yaml"),
                     "peripheral_arm": "left",
                     "tool_command_topic": "/xr_rm/left_rm75/tool_enable",
@@ -167,18 +119,7 @@ def _dual_arm_nodes(
                 config_file,
                 {
                     "use_mock": use_mock,
-                    "robot_ip": LaunchConfiguration("right_robot_ip"),
-                    "robot_port": LaunchConfiguration("robot_port"),
-                    "avoid_singularity": right_avoid_singularity,
                     "robot_urdf_path": _rm75_urdf(),
-                    "control_rate_hz": control_rate_hz,
-                    "follow": follow,
-                    "configure_safety_limits": configure_safety_limits,
-                    **_initial_pose_override(move_to_initial_pose),
-                    "enable_tool_control": enable_tool_control,
-                    "enable_trigger_gripper_control": enable_trigger_gripper_control,
-                    "trigger_close_threshold": trigger_close_threshold,
-                    "configure_peripheral_on_connect": configure_peripheral_on_connect,
                     "peripheral_config_file": _config_file("peripherals_rm75.yaml"),
                     "peripheral_arm": "right",
                     "tool_command_topic": "/xr_rm/right_rm75/tool_enable",
@@ -198,71 +139,15 @@ def _launch_setup(context, *args, **kwargs):
         )
     arm = LaunchConfiguration("arm").perform(context).strip().lower()
     use_mock = _as_bool(LaunchConfiguration("use_mock").perform(context))
-    move_to_initial_pose = LaunchConfiguration(
-        "move_to_initial_pose_on_connect"
-    ).perform(context).strip().lower()
-    avoid_override = LaunchConfiguration("avoid_singularity").perform(context).strip()
-    left_avoid_singularity = int(
-        avoid_override or LaunchConfiguration("left_avoid_singularity").perform(context)
-    )
-    right_avoid_singularity = int(
-        avoid_override or LaunchConfiguration("right_avoid_singularity").perform(context)
-    )
-    control_rate_hz = float(LaunchConfiguration("control_rate_hz").perform(context))
-    follow = _as_bool(LaunchConfiguration("follow").perform(context))
-    configure_safety_limits = _as_bool(
-        LaunchConfiguration("configure_safety_limits").perform(context)
-    )
-    configure_peripheral_on_connect = _as_bool(
-        LaunchConfiguration("configure_peripheral_on_connect").perform(context)
-    )
-    enable_tool_control = _as_bool(
-        LaunchConfiguration("enable_tool_control").perform(context)
-    )
-    enable_trigger_gripper_control = _as_bool(
-        LaunchConfiguration("enable_trigger_gripper_control").perform(context)
-    )
-    trigger_close_threshold = float(
-        LaunchConfiguration("trigger_close_threshold").perform(context)
-    )
 
     if arm not in ("left", "right", "both"):
         raise ValueError("arm must be one of: left, right, both")
 
     nodes = [_udp_receiver_node()]
     if arm == "both":
-        nodes.extend(
-            _dual_arm_nodes(
-                use_mock,
-                move_to_initial_pose,
-                left_avoid_singularity,
-                right_avoid_singularity,
-                control_rate_hz,
-                follow,
-                configure_safety_limits,
-                enable_tool_control,
-                enable_trigger_gripper_control,
-                trigger_close_threshold,
-                configure_peripheral_on_connect,
-            )
-        )
+        nodes.extend(_dual_arm_nodes(use_mock))
     else:
-        avoid_singularity = left_avoid_singularity if arm == "left" else right_avoid_singularity
-        nodes.append(
-            _single_arm_node(
-                arm,
-                use_mock,
-                move_to_initial_pose,
-                avoid_singularity,
-                control_rate_hz,
-                follow,
-                configure_safety_limits,
-                enable_tool_control,
-                enable_trigger_gripper_control,
-                trigger_close_threshold,
-                configure_peripheral_on_connect,
-            )
-        )
+        nodes.append(_single_arm_node(arm, use_mock))
     return nodes
 
 
@@ -277,29 +162,6 @@ def generate_launch_description() -> LaunchDescription:
         DeclareLaunchArgument("udp_port", default_value="15000"),
         # UDP receiver 轮询频率高于 PICO 发送频率，减少 socket 中等待时间。
         DeclareLaunchArgument("udp_timer_hz", default_value="200.0"),
-        # 左右 RM75 默认 IP，可在命令行中按现场网络覆盖。
-        DeclareLaunchArgument("left_robot_ip", default_value="192.168.192.18"),
-        DeclareLaunchArgument("right_robot_ip", default_value="192.168.192.19"),
-        DeclareLaunchArgument("robot_port", default_value="8080"),
-        # 真机位姿透传与安全配置参数。
-        DeclareLaunchArgument("left_avoid_singularity", default_value="0"),
-        DeclareLaunchArgument("right_avoid_singularity", default_value="1"),
-        # 非空时作为左右臂全局覆盖，例如 avoid_singularity:=0。
-        DeclareLaunchArgument("avoid_singularity", default_value=""),
-        # 每周期同步一次实际关节反馈、执行一次 Placo QP，再发送 rm_movej_canfd。
-        DeclareLaunchArgument("control_rate_hz", default_value="125.0"),
-        # 默认低跟随；高跟随请确认控制器和网络能稳定满足厂商周期要求后再打开。
-        DeclareLaunchArgument("follow", default_value="false"),
-        DeclareLaunchArgument("configure_safety_limits", default_value="true"),
-        # 工具控制通过遥操作节点复用同一个 RealMan 连接，避免两个进程抢同一机械臂连接。
-        DeclareLaunchArgument("enable_tool_control", default_value="true"),
-        # trigger 上升沿切换夹爪开/关；grip 仍只控制机械臂运动。
-        DeclareLaunchArgument("enable_trigger_gripper_control", default_value="true"),
-        DeclareLaunchArgument("trigger_close_threshold", default_value="0.95"),
-        # 连接成功后是否配置外设；关闭后仅订阅开合话题，但开合前需要另行完成外设配置。
-        DeclareLaunchArgument("configure_peripheral_on_connect", default_value="true"),
-        # auto 时由单/双臂 YAML 决定；也可显式传 true/false 覆盖。
-        DeclareLaunchArgument("move_to_initial_pose_on_connect", default_value="auto"),
         # OpaqueFunction 允许根据 arm/use_mock 等运行时参数动态生成节点。
         OpaqueFunction(function=_launch_setup),
     ])

xr_rm_bringup/test/test_launcher_ui_cleanup.py

@@ -0,0 +1,94 @@
+import importlib.util
+import signal
+import subprocess
+import unittest
+from pathlib import Path
+from unittest import mock
+
+
+MODULE_PATH = Path(__file__).parents[1] / "tools" / "launcher_ui.py"
+SPEC = importlib.util.spec_from_file_location("launcher_ui", MODULE_PATH)
+launcher_ui = importlib.util.module_from_spec(SPEC)
+assert SPEC.loader is not None
+SPEC.loader.exec_module(launcher_ui)
+
+
+class LauncherCleanupTest(unittest.TestCase):
+    def test_xrobotoolkit_cleanup_policy_only_stops_service_on_window_close(self) -> None:
+        stop_all_patterns = set(
+            launcher_ui._xrobotoolkit_cleanup_patterns(stop_pc_service=False)
+        )
+        window_close_patterns = set(
+            launcher_ui._xrobotoolkit_cleanup_patterns(stop_pc_service=True)
+        )
+
+        self.assertLessEqual(
+            {"RobotLinuxDemo.x86_64", "PXREAClientUnity"},
+            stop_all_patterns,
+        )
+        self.assertNotIn("RoboticsServiceProcess", stop_all_patterns)
+        self.assertIn("RoboticsServiceProcess", window_close_patterns)
+
+    def test_close_and_stop_all_select_different_pc_service_policies(self) -> None:
+        app = object.__new__(launcher_ui.LauncherApp)
+        calls = []
+
+        class Root:
+            destroyed = False
+
+            def destroy(self) -> None:
+                self.destroyed = True
+
+        app.root = Root()
+        app.stop_launched_processes = lambda **kwargs: calls.append(kwargs) or True
+
+        app.kill_launched_processes()
+        app.on_close_requested()
+
+        self.assertEqual(
+            calls,
+            [
+                {"confirm": True, "notify": True, "stop_pc_service": False},
+                {"confirm": True, "notify": False, "stop_pc_service": True},
+            ],
+        )
+        self.assertTrue(app.root.destroyed)
+
+    def test_stop_all_keeps_oldest_pc_service_and_stops_duplicates(self) -> None:
+        app = object.__new__(launcher_ui.LauncherApp)
+        app.status = mock.Mock()
+        app.close_related_terminal_windows = lambda: 0
+
+        def fake_check_output(command, **_kwargs):
+            if command == ["pgrep", "-o", "-f", "RoboticsServiceProcess"]:
+                return "101\n"
+            if command == ["pgrep", "-f", "RoboticsServiceProcess"]:
+                return "101\n202\n303\n"
+            raise subprocess.CalledProcessError(1, command)
+
+        with (
+            mock.patch.object(
+                launcher_ui.subprocess,
+                "check_output",
+                side_effect=fake_check_output,
+            ),
+            mock.patch.object(launcher_ui.os, "kill") as kill,
+            mock.patch.object(launcher_ui.time, "sleep"),
+        ):
+            app.stop_launched_processes(
+                confirm=False,
+                notify=False,
+                stop_pc_service=False,
+            )
+
+        self.assertEqual(
+            kill.call_args_list,
+            [
+                mock.call(202, signal.SIGTERM),
+                mock.call(303, signal.SIGTERM),
+            ],
+        )
+
+
+if __name__ == "__main__":
+    unittest.main()

xr_rm_bringup/tools/launcher_ui.py

@@ -121,6 +121,17 @@ def _xrobotoolkit_bridge_command() -> str:
     )
 
 
+def _xrobotoolkit_cleanup_patterns(*, stop_pc_service: bool) -> tuple[str, ...]:
+    patterns = (
+        XROBOTOOLKIT_BRIDGE_EXECUTABLE,
+        "RobotLinuxDemo.x86_64",
+        "PXREAClientUnity",
+    )
+    if stop_pc_service:
+        return (*patterns, "RoboticsServiceProcess")
+    return patterns
+
+
 def _tool_command(arm: str, open_tool: bool) -> str:
     arm_name = "left_rm75" if arm == "left" else "right_rm75"
     value = "true" if open_tool else "false"
@@ -289,8 +300,7 @@ def build_commands_by_mode(mode: str) -> list[tuple[str, str]]:
             ("Ping Left RM75", f"ping -c 4 {DEFAULT_LEFT_IP}"),
             (
                 "Left Arm RealMan Launch",
-                "ros2 launch xr_rm_bringup arm_debug.launch.py arm:=left use_mock:=false "
-                f"left_robot_ip:={DEFAULT_LEFT_IP}",
+                "ros2 launch xr_rm_bringup arm_debug.launch.py arm:=left use_mock:=false",
             ),
             ("XRobotoolkit UDP Bridge (90 Hz)", _xrobotoolkit_bridge_command()),
             ("Left Tool Open", _tool_command("left", True)),
@@ -306,8 +316,7 @@ def build_commands_by_mode(mode: str) -> list[tuple[str, str]]:
             ("Ping Right RM75", f"ping -c 4 {DEFAULT_RIGHT_IP}"),
             (
                 "Right Arm RealMan Launch",
-                "ros2 launch xr_rm_bringup arm_debug.launch.py arm:=right use_mock:=false "
-                f"right_robot_ip:={DEFAULT_RIGHT_IP}",
+                "ros2 launch xr_rm_bringup arm_debug.launch.py arm:=right use_mock:=false",
             ),
             ("XRobotoolkit UDP Bridge (90 Hz)", _xrobotoolkit_bridge_command()),
             ("Right Tool Open", _tool_command("right", True)),
@@ -324,9 +333,7 @@ def build_commands_by_mode(mode: str) -> list[tuple[str, str]]:
             ("Ping Right RM75", f"ping -c 4 {DEFAULT_RIGHT_IP}"),
             (
                 "Dual Arm RealMan Launch",
-                "ros2 launch xr_rm_bringup arm_debug.launch.py arm:=both use_mock:=false "
-                f"left_robot_ip:={DEFAULT_LEFT_IP} right_robot_ip:={DEFAULT_RIGHT_IP} "
-                "move_to_initial_pose_on_connect:=false",
+                "ros2 launch xr_rm_bringup arm_debug.launch.py arm:=both use_mock:=false",
             ),
             ("XRobotoolkit UDP Bridge (90 Hz)", _xrobotoolkit_bridge_command()),
             (
@@ -1283,13 +1290,27 @@ class LauncherApp:
         return closed
 
     def on_close_requested(self) -> None:
-        if self.stop_launched_processes(confirm=True, notify=False):
+        if self.stop_launched_processes(
+            confirm=True,
+            notify=False,
+            stop_pc_service=True,
+        ):
             self.root.destroy()
 
     def kill_launched_processes(self) -> None:
-        self.stop_launched_processes(confirm=True, notify=True)
+        self.stop_launched_processes(
+            confirm=True,
+            notify=True,
+            stop_pc_service=False,
+        )
 
-    def stop_launched_processes(self, *, confirm: bool, notify: bool) -> bool:
+    def stop_launched_processes(
+        self,
+        *,
+        confirm: bool,
+        notify: bool,
+        stop_pc_service: bool,
+    ) -> bool:
         if confirm and not messagebox.askyesno(
             "Confirm Stop",
             "Stop XR-RM launcher terminals, topic monitors, ROS nodes, and bridge processes started from this workspace?",
@@ -1302,7 +1323,7 @@ class LauncherApp:
             "ros2 run xr_rm_input",
             "ros2 run xr_rm_teleop",
             "XR_RM_LAUNCHER_SESSION=1",
-            XROBOTOOLKIT_BRIDGE_EXECUTABLE,
+            *_xrobotoolkit_cleanup_patterns(stop_pc_service=stop_pc_service),
             TERMINAL_TITLE_PREFIX,
             TOPIC_MONITOR_TITLE,
             CMD_VEL_MONITOR_TITLE,
@@ -1326,6 +1347,20 @@ class LauncherApp:
             "ros2 topic list",
             "ros2 node list",
         ]
+        pc_service_keep_pid: int | None = None
+        if not stop_pc_service:
+            try:
+                output = subprocess.check_output(
+                    ["pgrep", "-o", "-f", "RoboticsServiceProcess"],
+                    text=True,
+                )
+                pc_service_keep_pid = int(output.strip())
+                patterns.append("RoboticsServiceProcess")
+            except (subprocess.CalledProcessError, ValueError):
+                pass
+            except Exception as exc:
+                print(f"Failed to find the oldest RoboticsServiceProcess: {exc}")
+
         protected = {os.getpid(), os.getppid()}
         killed: set[int] = set()
 
@@ -1345,6 +1380,8 @@ class LauncherApp:
                     continue
                 if pid in protected or pid in killed:
                     continue
+                if pattern == "RoboticsServiceProcess" and pid == pc_service_keep_pid:
+                    continue
                 try:
                     os.kill(pid, signal.SIGTERM)
                     killed.add(pid)

xr_rm_teleop/test/placo_ik_smoke.py

@@ -50,6 +50,9 @@ def main() -> None:
         drift_degrees = float(
             np.max(np.abs(np.rad2deg(np.asarray(joints) - initial_joints)))
         )
+        assert drift_degrees <= 0.05, (
+            f"{arm} stationary target drifted {drift_degrees:.3f}deg"
+        )
 
         solver = PlacoIkSolver(str(urdf_path), 1.0 / 125.0)
         joints = initial_joints.tolist()

xr_rm_teleop/test/test_initial_joint_pose.py

@@ -1,4 +1,6 @@
 import math
+import sys
+from types import ModuleType, SimpleNamespace
 
 import pytest
 
@@ -18,7 +20,14 @@ def test_initial_pose_uses_joint_move_only() -> None:
             return 0
 
     joints = [-167.21, 28.48, 28.21, 61.35, -14.40, 84.49, -124.51]
-    adapter = RealManAdapter("127.0.0.1", 8080, 0, 1, initial_joint_pose=joints)
+    adapter = RealManAdapter(
+        "127.0.0.1",
+        8080,
+        0,
+        "127.0.0.1",
+        8090,
+        initial_joint_pose=joints,
+    )
     adapter._arm = FakeArm()
 
     adapter._move_to_initial_pose()
@@ -111,80 +120,299 @@ def test_tool_frame_sdk_failures_are_reported(existing, failure, operation) -> N
         _configure_tool_frame(FakeArm(), object(), "omnipic")
 
 
-def test_joint_feedback_is_cached_in_radians(monkeypatch) -> None:
-    class FakeArm:
-        def rm_get_joint_degree(self):
-            return 0, [0.0, 10.0, -20.0, 30.0, -40.0, 50.0, -60.0]
-
-    perf_counter_ns = iter(
-        [1_000_000_000, 1_002_000_000, 2_000_000_000, 2_003_000_000]
-    )
-    monotonic = iter([10.0, 10.011])
-    monkeypatch.setattr(
-        realman_adapter,
-        "time",
-        type(
-            "FakeTime",
-            (),
-            {
-                "perf_counter_ns": staticmethod(lambda: next(perf_counter_ns)),
-                "monotonic": staticmethod(lambda: next(monotonic)),
-            },
+def _udp_state(
+    *,
+    robot_ip: str = "127.0.0.1",
+    joints=None,
+    error_code: int = 0,
+    joint_enabled=None,
+    joint_error_codes=None,
+    arm_error_codes=None,
+    arm_current_status: int = 0,
+):
+    if joints is None:
+        joints = [0.0, 10.0, -20.0, 30.0, -40.0, 50.0, -60.0]
+    if joint_enabled is None:
+        joint_enabled = [True] * 7
+    if joint_error_codes is None:
+        joint_error_codes = [0] * 7
+    if arm_error_codes is None:
+        arm_error_codes = []
+    return SimpleNamespace(
+        errCode=error_code,
+        arm_ip=robot_ip.encode(),
+        joint_status=SimpleNamespace(
+            joint_position=joints,
+            joint_en_flag=joint_enabled,
+            joint_err_code=joint_error_codes,
         ),
+        err=SimpleNamespace(
+            err_len=len(arm_error_codes),
+            err=list(arm_error_codes),
+        ),
+        arm_current_status=arm_current_status,
     )
-    adapter = RealManAdapter("127.0.0.1", 8080, 0, 0.01)
-    adapter._arm = FakeArm()
 
-    adapter._read_joint_state_once()
+
+def _install_fake_sdk(monkeypatch, *, push_return=0, send_feedback=True):
+    class FakeThreadMode:
+        RM_TRIPLE_MODE_E = 2
+
+    class FakePushConfig:
+        def __init__(self, *args):
+            self.args = args
+
+    class FakeArm:
+        instance = None
+
+        def __init__(self, mode):
+            self.mode = mode
+            self.callback = None
+            self.config = None
+            self.delete_calls = 0
+            FakeArm.instance = self
+
+        def rm_create_robot_arm(self, robot_ip, robot_port):
+            self.robot_ip = robot_ip
+            self.robot_port = robot_port
+            return SimpleNamespace(id=1)
+
+        def rm_realtime_arm_state_call_back(self, callback):
+            self.callback = callback
+
+        def rm_set_realtime_push(self, config):
+            self.config = config
+            if push_return == 0 and send_feedback:
+                self.callback(_udp_state())
+            return push_return
+
+        def rm_delete_robot_arm(self):
+            self.delete_calls += 1
+            return 0
+
+    sdk = ModuleType("Robotic_Arm.rm_robot_interface")
+    sdk.RoboticArm = FakeArm
+    sdk.rm_thread_mode_e = FakeThreadMode
+    sdk.rm_realtime_push_config_t = FakePushConfig
+    sdk.rm_realtime_arm_state_callback_ptr = lambda callback: callback
+    package = ModuleType("Robotic_Arm")
+    package.rm_robot_interface = sdk
+    monkeypatch.setitem(sys.modules, "Robotic_Arm", package)
+    monkeypatch.setitem(sys.modules, "Robotic_Arm.rm_robot_interface", sdk)
+    return SimpleNamespace(RoboticArm=FakeArm)
+
+
+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._read_joint_state_once()
+    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 == pytest.approx(2.0)
+    assert first.read_duration_ms is None
     assert first.update_interval_ms is None
     assert second is not None
-    assert second.read_duration_ms == pytest.approx(3.0)
-    assert second.update_interval_ms == pytest.approx(11.0)
+    assert second.read_duration_ms is None
+    assert second.update_interval_ms == pytest.approx(5.0)
 
 
-def test_feedback_loop_uses_absolute_schedule_without_catch_up(monkeypatch) -> None:
-    class FakeStopEvent:
-        def __init__(self) -> None:
-            self.checks = 0
-            self.waits = []
-
-        def is_set(self):
-            self.checks += 1
-            return self.checks > 3
-
-        def wait(self, timeout):
-            self.waits.append(timeout)
-            return False
-
-    monotonic = iter([0.0, 0.005, 0.018, 0.018, 0.023])
-    monkeypatch.setattr(
-        realman_adapter,
-        "time",
-        type(
-            "FakeTime",
-            (),
-            {"monotonic": staticmethod(lambda: next(monotonic))},
-        ),
+@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 = RealManAdapter("127.0.0.1", 8080, 0, 0.008)
-    stop_event = FakeStopEvent()
-    reads = []
-    adapter._feedback_stop = stop_event
-    adapter._read_joint_state_once = lambda: reads.append(None)
+    adapter._accept_realtime_feedback = True
+    adapter._on_realtime_arm_state(_udp_state(joints=[1.0] * 7))
+    before = adapter.get_latest_joint_state()
 
-    adapter._feedback_loop()
+    adapter._on_realtime_arm_state(state)
 
-    assert len(reads) == 3
-    assert stop_event.waits == pytest.approx([0.003, 0.003])
+    assert adapter.get_latest_joint_state() == before
+
+
+def test_udp_joint_fault_marks_snapshot_unready() -> 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(
+            joint_enabled=[True, True, False, True, True, True, True],
+            joint_error_codes=[0, 0, 17, 0, 0, 0, 0],
+            arm_error_codes=[42],
+            arm_current_status=9,
+        )
+    )
+
+    snapshot = adapter.get_latest_joint_state()
+    assert snapshot is not None
+    assert snapshot.motion_ready is False
+
+
+def test_udp_stop_status_marks_snapshot_unready_without_joint_error() -> 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(arm_current_status=9))
+
+    snapshot = adapter.get_latest_joint_state()
+    assert snapshot is not None
+    assert snapshot.motion_ready is False
+
+
+def test_udp_zero_arm_error_code_is_motion_ready() -> 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(arm_error_codes=[0]))
+
+    snapshot = adapter.get_latest_joint_state()
+    assert snapshot is not None
+    assert snapshot.motion_ready is True
+
+
+def test_udp_fault_and_recovery_are_logged_once_per_transition() -> None:
+    class FakeLogger:
+        def __init__(self) -> None:
+            self.infos = []
+            self.warnings = []
+
+        def info(self, message):
+            self.infos.append(message)
+
+        def warn(self, message):
+            self.warnings.append(message)
+
+    logger = FakeLogger()
+    adapter = RealManAdapter(
+        "127.0.0.1",
+        8080,
+        0,
+        "127.0.0.1",
+        8090,
+        logger=logger,
+    )
+    adapter._accept_realtime_feedback = True
+    adapter._on_realtime_arm_state(_udp_state())
+    fault = _udp_state(
+        joint_enabled=[False] * 7,
+        joint_error_codes=[17, 0, 0, 0, 0, 0, 0],
+        arm_error_codes=[42],
+        arm_current_status=9,
+    )
+
+    adapter._on_realtime_arm_state(fault)
+    adapter._on_realtime_arm_state(fault)
+    adapter._on_realtime_arm_state(_udp_state())
+
+    assert len(logger.warnings) == 1
+    assert "joint_errors=[17, 0, 0, 0, 0, 0, 0]" in logger.warnings[0]
+    assert len([message for message in logger.infos if "恢复正常" in message]) == 1
+
+
+def test_connect_configures_udp_feedback_and_waits_for_first_frame(monkeypatch) -> None:
+    fake_sdk = _install_fake_sdk(monkeypatch)
+    adapter = RealManAdapter(
+        "127.0.0.1",
+        8080,
+        0,
+        "192.168.192.148",
+        8090,
+        configure_safety_limits=False,
+    )
+
+    adapter.connect()
+
+    arm = fake_sdk.RoboticArm.instance
+    assert arm is not None
+    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")
+
+
+def test_udp_configuration_failure_cleans_up_robot_handle(monkeypatch) -> None:
+    fake_sdk = _install_fake_sdk(monkeypatch, push_return=1)
+    adapter = RealManAdapter(
+        "127.0.0.1",
+        8080,
+        0,
+        "192.168.192.148",
+        8090,
+        configure_safety_limits=False,
+    )
+
+    with pytest.raises(RuntimeError, match="rm_set_realtime_push"):
+        adapter.connect()
+
+    assert fake_sdk.RoboticArm.instance.delete_calls == 1
+    assert adapter._arm is None
+
+
+def test_udp_first_frame_timeout_cleans_up_robot_handle(monkeypatch) -> None:
+    fake_sdk = _install_fake_sdk(monkeypatch, send_feedback=False)
+    adapter = RealManAdapter(
+        "127.0.0.1",
+        8080,
+        0,
+        "192.168.192.148",
+        8090,
+        configure_safety_limits=False,
+    )
+    adapter._feedback_ready = SimpleNamespace(
+        clear=lambda: None,
+        set=lambda: None,
+        wait=lambda timeout: False,
+    )
+
+    with pytest.raises(RuntimeError, match="within 2 seconds"):
+        adapter.connect()
+
+    assert fake_sdk.RoboticArm.instance.delete_calls == 1
+    assert adapter._arm is None
 
 
 def test_joint_target_uses_movej_canfd_in_degrees() -> None:
@@ -196,7 +424,13 @@ def test_joint_target_uses_movej_canfd_in_degrees() -> None:
             self.calls.append(args)
             return 0
 
-    adapter = RealManAdapter("127.0.0.1", 8080, 0, 0.01)
+    adapter = RealManAdapter(
+        "127.0.0.1",
+        8080,
+        0,
+        "127.0.0.1",
+        8090,
+    )
     adapter._arm = FakeArm()
     target = [math.radians(value) for value in [1, 2, 3, 4, 5, 6, 7]]
 

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)
 

xr_rm_teleop/xr_rm_teleop/placo_ik_solver.py

@@ -99,12 +99,6 @@ class PlacoIkSolver:
             np.eye(4),
         )
         self._frame_task.configure("rm75_frame", "soft", 1.0)
-        manipulability = self._solver.add_manipulability_task(
-            "link_7",
-            "both",
-            1.0,
-        )
-        manipulability.configure("rm75_manipulability", "soft", 5e-2)
         self._solver.add_kinetic_energy_regularization_task(1e-6)
 
     @property

xr_rm_teleop/xr_rm_teleop/realman_adapter.py

@@ -2,6 +2,7 @@
 
 from __future__ import annotations
 
+import ipaddress
 import math
 import threading
 import time
@@ -32,6 +33,7 @@ class JointStateSnapshot:
     received_at: float
     read_duration_ms: float | None = None
     update_interval_ms: float | None = None
+    motion_ready: bool = True
 
 
 class MockRealManAdapter:
@@ -85,7 +87,9 @@ class RealManAdapter:
         robot_ip: str,
         robot_port: int,
         avoid_singularity: int,
-        feedback_period: float,
+        realtime_push_host_ip: str,
+        realtime_push_port: int,
+        realtime_push_cycle_ms: int = 5,
         logger: Any | None = None,
         configure_safety_limits: bool = True,
         max_line_speed: float = 1.0,
@@ -103,9 +107,22 @@ class RealManAdapter:
         self._robot_ip = robot_ip
         self._robot_port = robot_port
         self._avoid_singularity = avoid_singularity
-        if feedback_period <= 0.0:
-            raise ValueError("feedback_period must be positive")
-        self._feedback_period = feedback_period
+        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"
+            )
+        self._realtime_push_port = realtime_push_port
+        self._realtime_push_cycle_ms = realtime_push_cycle_ms
         self._logger = logger
         self._configure_safety_limits = configure_safety_limits
         self._max_line_speed = max_line_speed
@@ -123,20 +140,31 @@ class RealManAdapter:
         self._arm: Any | None = None
         self._joint_state_lock = threading.Lock()
         self._latest_joint_state: JointStateSnapshot | None = None
-        self._feedback_stop = threading.Event()
-        self._feedback_thread: threading.Thread | None = None
+        self._feedback_ready = threading.Event()
+        self._realtime_callback: Any | None = None
+        self._accept_realtime_feedback = False
         self._feedback_fault_logged = False
+        self._last_motion_status: tuple[Any, ...] | None = None
 
     def connect(self) -> None:
         try:
-            from Robotic_Arm.rm_robot_interface import RoboticArm, rm_thread_mode_e
+            from Robotic_Arm.rm_robot_interface import (
+                RoboticArm,
+                rm_realtime_arm_state_callback_ptr,
+                rm_realtime_push_config_t,
+                rm_thread_mode_e,
+            )
         except ImportError as exc:
             raise RuntimeError(
                 "未安装睿尔曼 Python API2。请安装厂商 SDK，或用 use_mock:=true 先跑模拟模式。"
             ) from exc
 
         self._arm = RoboticArm(rm_thread_mode_e.RM_TRIPLE_MODE_E)
-        handle = self._arm.rm_create_robot_arm(self._robot_ip, self._robot_port)
+        try:
+            handle = self._arm.rm_create_robot_arm(
+                self._robot_ip,
+                self._robot_port,
+            )
             self._check_robot_handle(handle)
             self._log_info(
                 "RealMan connected: "
@@ -148,13 +176,45 @@ class RealManAdapter:
                 self._apply_safety_limits()
             if self._move_to_initial_pose_on_connect:
                 self._move_to_initial_pose()
-        self._feedback_stop.clear()
-        self._feedback_thread = threading.Thread(
-            target=self._feedback_loop,
-            name=f"rm75_feedback_{self._robot_ip}",
-            daemon=True,
+            self._feedback_ready.clear()
+            self._accept_realtime_feedback = True
+            self._realtime_callback = rm_realtime_arm_state_callback_ptr(
+                self._on_realtime_arm_state
             )
-        self._feedback_thread.start()
+            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"
+                )
+            self._log_info(
+                "RealMan UDP realtime feedback ready: "
+                f"host={self._realtime_push_host_ip}:"
+                f"{self._realtime_push_port}, "
+                f"cycle={self._realtime_push_cycle_ms} ms"
+            )
+        except Exception:
+            self._accept_realtime_feedback = False
+            try:
+                self._arm.rm_delete_robot_arm()
+            except Exception:
+                pass
+            self._arm = None
+            self._realtime_callback = None
+            raise
 
     def get_latest_joint_state(self) -> JointStateSnapshot | None:
         with self._joint_state_lock:
@@ -165,6 +225,7 @@ class RealManAdapter:
                 self._latest_joint_state.received_at,
                 self._latest_joint_state.read_duration_ms,
                 self._latest_joint_state.update_interval_ms,
+                self._latest_joint_state.motion_ready,
             )
 
     def send_joint_target(self, joints: list[float], follow: bool) -> None:
@@ -219,70 +280,118 @@ class RealManAdapter:
     def close(self) -> None:
         if self._arm is None:
             return
+        self._accept_realtime_feedback = False
         self.stop()
-        self._feedback_stop.set()
-        if self._feedback_thread is not None:
-            self._feedback_thread.join(timeout=3.0)
-            if self._feedback_thread.is_alive():
-                self._log_warn("RealMan 关节反馈线程未在 3 秒内退出。")
-            self._feedback_thread = None
         try:
             self._arm.rm_delete_robot_arm()
         finally:
             self._arm = None
+            self._realtime_callback = None
 
     def _require_arm(self) -> None:
         if self._arm is None:
             raise RuntimeError("睿尔曼机械臂尚未连接")
 
-    def _feedback_loop(self) -> None:
-        next_read_at = time.monotonic()
-        while not self._feedback_stop.is_set():
+    def _on_realtime_arm_state(self, data: Any) -> None:
+        if not self._accept_realtime_feedback:
+            return
         try:
-                self._read_joint_state_once()
-                self._feedback_fault_logged = False
-            except Exception as exc:
-                if not self._feedback_fault_logged:
-                    self._log_warn(f"RealMan 关节反馈读取失败：{exc}")
-                    self._feedback_fault_logged = True
-            next_read_at += self._feedback_period
-            remaining = next_read_at - time.monotonic()
-            if remaining <= 0.0:
-                next_read_at = time.monotonic()
-                continue
-            self._feedback_stop.wait(remaining)
-
-    def _read_joint_state_once(self) -> None:
-        self._require_arm()
-        read_started_ns = time.perf_counter_ns()
-        result = self._arm.rm_get_joint_degree()
-        read_duration_ms = (time.perf_counter_ns() - read_started_ns) * 1e-6
-        self._check_return(result, "rm_get_joint_degree")
-        if not isinstance(result, tuple) or len(result) < 2:
-            raise RuntimeError(f"rm_get_joint_degree 返回格式错误：{result!r}")
-        degrees = result[1]
+            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 (
-            not isinstance(degrees, (list, tuple))
-            or len(degrees) != 7
+                len(degrees) != 7
                 or not all(isinstance(value, Number) for value in degrees)
             ):
-            raise RuntimeError(f"RM75 关节反馈必须包含 7 个数值：{degrees!r}")
+                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 RuntimeError("RM75 关节反馈包含 NaN/Inf")
+                raise ValueError("RM75 UDP feedback contains NaN/Inf")
+            joint_enabled = [
+                bool(value) for value in data.joint_status.joint_en_flag
+            ]
+            joint_errors = [
+                int(value) for value in data.joint_status.joint_err_code
+            ]
+            if len(joint_enabled) != 7 or len(joint_errors) != 7:
+                raise ValueError(
+                    "RM75 UDP feedback must contain 7 joint states"
+                )
+            arm_error_count = int(data.err.err_len)
+            arm_errors = [
+                int(value) for value in list(data.err.err)[:arm_error_count]
+            ]
+            arm_errors = [code for code in arm_errors if code != 0]
+            arm_current_status = int(data.arm_current_status)
+            motion_ready = (
+                0 <= arm_current_status <= 8
+                and all(joint_enabled)
+                and not any(joint_errors)
+                and not arm_errors
+            )
+            motion_status = (
+                arm_current_status,
+                tuple(joint_enabled),
+                tuple(joint_errors),
+                tuple(arm_errors),
+                motion_ready,
+            )
             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
+                    else (
+                        received_at
+                        - self._latest_joint_state.received_at
+                    )
+                    * 1000.0
                 )
                 self._latest_joint_state = JointStateSnapshot(
                     positions,
                     received_at,
-                read_duration_ms,
+                    None,
                     update_interval_ms,
+                    motion_ready,
                 )
+            self._log_motion_status_transition(motion_status)
+            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
+
+    def _log_motion_status_transition(
+        self,
+        status: tuple[Any, ...],
+    ) -> None:
+        previous = self._last_motion_status
+        if status == previous:
+            return
+        self._last_motion_status = status
+        arm_status, joint_enabled, joint_errors, arm_errors, ready = status
+        details = (
+            f"arm_status={arm_status}, "
+            f"joint_enabled={list(joint_enabled)}, "
+            f"joint_errors={list(joint_errors)}, "
+            f"arm_errors={list(arm_errors)}"
+        )
+        if not ready:
+            self._log_warn(f"RealMan UDP 报警或掉使能：{details}")
+        elif previous is not None and not previous[-1]:
+            self._log_info(f"RealMan UDP 运动状态恢复正常：{details}")
 
     def _apply_safety_limits(self) -> None:
         # 真机安全限幅尽量下发到控制器；不支持的 SDK 接口会在 _try_call 中降级为警告。

xr_rm_teleop/xr_rm_teleop/single_arm_velocity_teleop.py

@@ -201,6 +201,9 @@ class SingleArmVelocityTeleop(Node):
         self.declare_parameter("robot_urdf_path", "")
         self.declare_parameter("robot_ip", "192.168.1.18")
         self.declare_parameter("robot_port", 8080)
+        self.declare_parameter("realtime_push_host_ip", "")
+        self.declare_parameter("realtime_push_port", 0)
+        self.declare_parameter("realtime_push_cycle_ms", 5)
         self.declare_parameter("avoid_singularity", 1)
         self.declare_parameter("follow", False)
         self.declare_parameter("configure_safety_limits", True)
@@ -255,6 +258,12 @@ class SingleArmVelocityTeleop(Node):
         self._enable_tool_control = self._bool_parameter("enable_tool_control")
         self._enable_trigger_gripper_control = self._bool_parameter("enable_trigger_gripper_control")
         self._trigger_close_threshold = float(self.get_parameter("trigger_close_threshold").value)
+        self._joint_command_max_speed = math.radians(
+            float(self.get_parameter("joint_max_speed").value)
+        )
+        self._joint_command_max_acceleration = math.radians(
+            float(self.get_parameter("joint_max_acc").value)
+        )
         self._debug_topic_prefix = str(self.get_parameter("debug_topic_prefix").value).rstrip("/")
         if not self._debug_topic_prefix:
             self._debug_topic_prefix = "/xr_rm"
@@ -273,7 +282,11 @@ class SingleArmVelocityTeleop(Node):
         self._last_command_time: Time | None = None
         self._last_current_pose: np.ndarray | None = None
         self._last_valid_joint_target: list[float] | None = None
+        self._latest_joint_positions: list[float] | None = None
+        self._last_joint_command_target: list[float] | None = None
+        self._last_joint_command_velocity: list[float] | None = None
         self._joint_feedback_ready = False
+        self._grip_rearm_required = False
         self._stop_sent = True
         self._trigger_tool_open = True
         self._last_trigger_pressed: bool | None = None
@@ -337,7 +350,15 @@ class SingleArmVelocityTeleop(Node):
             robot_ip=self.get_parameter("robot_ip").value,
             robot_port=int(self.get_parameter("robot_port").value),
             avoid_singularity=int(self.get_parameter("avoid_singularity").value),
-            feedback_period=self._dt,
+            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
+            ),
             logger=self.get_logger(),
             configure_safety_limits=self._bool_parameter("configure_safety_limits"),
             max_line_speed=float(self.get_parameter("max_line_speed").value),
@@ -496,9 +517,10 @@ class SingleArmVelocityTeleop(Node):
         now = self.get_clock().now()
         snapshot = self._fresh_joint_state()
         if snapshot is None:
+            self._grip_rearm_required = True
             if self._joint_feedback_ready:
                 self.get_logger().warn(
-                    f"{self._arm_name} 关节反馈缺失或过期，机械臂停止。",
+                    f"{self._arm_name} 关节反馈缺失、过期或机械臂未就绪，机械臂停止。",
                     throttle_duration_sec=1.0,
                 )
             self._joint_feedback_ready = False
@@ -512,6 +534,7 @@ class SingleArmVelocityTeleop(Node):
                 throttle_duration_sec=1.0,
             )
             self._joint_feedback_ready = False
+            self._grip_rearm_required = True
             self._safe_stop(reset_active=True)
             return
         if not self._joint_feedback_ready:
@@ -537,10 +560,18 @@ class SingleArmVelocityTeleop(Node):
             return
 
         if not self._last_msg.grip:
+            self._grip_rearm_required = False
             if self._active:
                 self.get_logger().info(f"{self._arm_name} Grip 松开，退出相对位姿遥操。")
             self._safe_stop(reset_active=True)
             return
+        if getattr(self, "_grip_rearm_required", False):
+            self.get_logger().warn(
+                f"{self._arm_name} 反馈故障后等待 Grip 松开，禁止自动恢复运动。",
+                throttle_duration_sec=1.0,
+            )
+            self._safe_stop(reset_active=True)
+            return
 
         controller_now = self._controller_xyz(self._last_msg)
         try:
@@ -957,6 +988,7 @@ class SingleArmVelocityTeleop(Node):
         if (
             len(snapshot.positions) != 7
             or not all(math.isfinite(value) for value in snapshot.positions)
+            or not snapshot.motion_ready
         ):
             return None
         return snapshot
@@ -968,6 +1000,7 @@ class SingleArmVelocityTeleop(Node):
         current_pose = self._ik_solver.update_joint_state(
             snapshot.positions
         )
+        self._latest_joint_positions = list(snapshot.positions)
         self._last_current_pose = current_pose
         if not self._active or self._last_valid_joint_target is None:
             self._last_valid_joint_target = list(snapshot.positions)
@@ -1000,6 +1033,8 @@ class SingleArmVelocityTeleop(Node):
             self._last_sent_target = None
             self._last_sent_orientation = None
             self._last_command_time = None
+            self._last_joint_command_target = None
+            self._last_joint_command_velocity = None
         self._publish_stop_debug()
 
     def _send_stop_once(self) -> None:
@@ -1034,8 +1069,22 @@ class SingleArmVelocityTeleop(Node):
         return None
 
     def _send_joint_target(self, joints: list[float]) -> bool:
+        previous_target = self._last_joint_command_target
+        if previous_target is None:
+            previous_target = self._latest_joint_positions
+        if previous_target is None:
+            raise RuntimeError("valid joint feedback has not been initialized")
+        previous_velocity = self._last_joint_command_velocity or [0.0] * 7
+        limited_target, limited_velocity = self._limit_joint_command_step(
+            target=joints,
+            previous_target=previous_target,
+            previous_velocity=previous_velocity,
+            max_speed=self._joint_command_max_speed,
+            max_acceleration=self._joint_command_max_acceleration,
+            dt=self._dt,
+        )
         try:
-            self._adapter.send_joint_target(joints, self._follow)
+            self._adapter.send_joint_target(limited_target, self._follow)
         except Exception as exc:
             self.get_logger().error(
                 f"{self._arm_name} 发送关节透传命令失败：{exc}",
@@ -1044,8 +1093,43 @@ class SingleArmVelocityTeleop(Node):
             self._send_stop_once()
             self._safe_stop(reset_active=True)
             return False
+        self._last_joint_command_target = limited_target
+        self._last_joint_command_velocity = limited_velocity
         return True
 
+    @staticmethod
+    def _limit_joint_command_step(
+        target: list[float],
+        previous_target: list[float],
+        previous_velocity: list[float],
+        max_speed: float,
+        max_acceleration: float,
+        dt: float,
+    ) -> tuple[list[float], list[float]]:
+        if (
+            len(target) != 7
+            or len(previous_target) != 7
+            or len(previous_velocity) != 7
+        ):
+            raise ValueError("joint command state must contain 7 values")
+        if max_speed <= 0.0 or max_acceleration <= 0.0 or dt <= 0.0:
+            raise ValueError("joint command limits and dt must be positive")
+        desired_velocity = np.clip(
+            (np.asarray(target) - np.asarray(previous_target)) / dt,
+            -max_speed,
+            max_speed,
+        )
+        velocity_step = max_acceleration * dt
+        velocity = np.clip(
+            desired_velocity,
+            np.asarray(previous_velocity) - velocity_step,
+            np.asarray(previous_velocity) + velocity_step,
+        )
+        limited_target = np.asarray(previous_target) + velocity * dt
+        if not np.isfinite(limited_target).all():
+            raise ValueError("joint command contains NaN/Inf")
+        return limited_target.tolist(), velocity.tolist()
+
     def _publish_debug(
         self,
         raw_target_pose: np.ndarray,
@@ -1151,6 +1235,10 @@ class SingleArmVelocityTeleop(Node):
             raise ValueError("cyl_radius_limit[1] must be > cyl_radius_limit[0]")
         if self._low_z_min_radius < 0.0:
             raise ValueError("low_z_min_radius must be >= 0")
+        if self._joint_command_max_speed <= 0.0:
+            raise ValueError("joint_max_speed must be > 0")
+        if self._joint_command_max_acceleration <= 0.0:
+            raise ValueError("joint_max_acc must be > 0")
 
     def _shutdown_tool_worker(self) -> None:
         if self._tool_worker_thread is None or self._tool_command_queue is None: