Files

T

YikaiFu-cart 08996434e5 feat: Implement UDP feedback for RM75 robot arms

2026-07-29 15:26:59 +08:00

15 KiB

Raw Blame History

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:

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:

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:

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:

realtime_push_host_ip: str,
realtime_push_port: int,
realtime_push_cycle_ms: int = 5,

Validate with stdlib ipaddress.IPv4Address:

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:

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:

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:

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:

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:

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:

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:

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:

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:

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:

# 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:

{
    "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:

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:

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:

"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:

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:

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

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

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

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

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.

15 KiB Raw Blame History Unescape Escape

RM75 CANFD UDP Feedback Implementation Plan

Task 1: Add failing UDP feedback adapter tests

Task 2: Implement single-handle UDP feedback

Task 3: Move robot defaults into YAML and simplify launch

Task 4: Synchronize launcher UI and README

Task 5: Full verification

15 KiB

Raw Blame History