feat: Implement UDP feedback for RM75 robot arms
This commit is contained in:
@@ -0,0 +1,508 @@
|
||||
# RM75 CANFD UDP Feedback Implementation Plan
|
||||
|
||||
> **For agentic workers:** REQUIRED SUB-SKILL: Use superpowers:subagent-driven-development (recommended) or superpowers:executing-plans to implement this plan task-by-task. Steps use checkbox (`- [ ]`) syntax for tracking.
|
||||
|
||||
**Goal:** Replace synchronous TCP joint polling with the vendor UDP realtime callback while making YAML the source of robot behavior and hardware defaults.
|
||||
|
||||
**Architecture:** Keep one `RoboticArm(RM_TRIPLE_MODE_E)` handle per arm. TCP sends CANFD and safety/tool commands; a 5 ms controller UDP push invokes a minimal callback that updates the existing locked joint snapshot. Launch keeps only topology, mock safety mode, PICO input, and generated paths/topics.
|
||||
|
||||
**Tech Stack:** Python 3.10, ROS2 Humble, RealMan Python API2, YAML, pytest, colcon
|
||||
|
||||
---
|
||||
|
||||
### Task 1: Add failing UDP feedback adapter tests
|
||||
|
||||
**Files:**
|
||||
- Modify: `xr_rm_teleop/test/test_initial_joint_pose.py`
|
||||
|
||||
- [ ] **Step 1: Replace polling-specific tests with UDP callback tests**
|
||||
|
||||
Add `sys`, `types`, and `SimpleNamespace` imports. Replace
|
||||
`test_joint_feedback_is_cached_in_radians` and
|
||||
`test_feedback_loop_uses_absolute_schedule_without_catch_up` with helpers and
|
||||
tests equivalent to:
|
||||
|
||||
```python
|
||||
def _udp_state(robot_ip="127.0.0.1", joints=None, error_code=0):
|
||||
return SimpleNamespace(
|
||||
errCode=error_code,
|
||||
arm_ip=robot_ip.encode(),
|
||||
joint_status=SimpleNamespace(
|
||||
joint_position=joints or [0.0, 10.0, -20.0, 30.0, -40.0, 50.0, -60.0]
|
||||
),
|
||||
)
|
||||
|
||||
|
||||
def test_udp_feedback_is_cached_in_radians(monkeypatch) -> None:
|
||||
monotonic = iter([10.0, 10.005])
|
||||
monkeypatch.setattr(realman_adapter.time, "monotonic", lambda: next(monotonic))
|
||||
adapter = RealManAdapter(
|
||||
"127.0.0.1",
|
||||
8080,
|
||||
0,
|
||||
"127.0.0.1",
|
||||
8090,
|
||||
)
|
||||
|
||||
adapter._accept_realtime_feedback = True
|
||||
adapter._on_realtime_arm_state(_udp_state())
|
||||
first = adapter.get_latest_joint_state()
|
||||
adapter._on_realtime_arm_state(_udp_state())
|
||||
second = adapter.get_latest_joint_state()
|
||||
|
||||
assert first is not None
|
||||
assert first.positions == pytest.approx(
|
||||
[math.radians(value) for value in [0, 10, -20, 30, -40, 50, -60]]
|
||||
)
|
||||
assert first.read_duration_ms is None
|
||||
assert first.update_interval_ms is None
|
||||
assert second is not None
|
||||
assert second.update_interval_ms == pytest.approx(5.0)
|
||||
|
||||
|
||||
@pytest.mark.parametrize(
|
||||
"state",
|
||||
[
|
||||
_udp_state(error_code=-3),
|
||||
_udp_state(robot_ip="192.168.192.18"),
|
||||
_udp_state(joints=[0.0] * 6),
|
||||
_udp_state(joints=[0.0, 0.0, 0.0, math.nan, 0.0, 0.0, 0.0]),
|
||||
],
|
||||
)
|
||||
def test_invalid_udp_feedback_does_not_replace_snapshot(state) -> None:
|
||||
adapter = RealManAdapter(
|
||||
"127.0.0.1",
|
||||
8080,
|
||||
0,
|
||||
"127.0.0.1",
|
||||
8090,
|
||||
)
|
||||
adapter._accept_realtime_feedback = True
|
||||
adapter._on_realtime_arm_state(_udp_state(joints=[1.0] * 7))
|
||||
before = adapter.get_latest_joint_state()
|
||||
|
||||
adapter._on_realtime_arm_state(state)
|
||||
|
||||
assert adapter.get_latest_joint_state() == before
|
||||
```
|
||||
|
||||
Add a fake vendor module that records callback registration and push config.
|
||||
Its `rm_set_realtime_push()` invokes the registered callback with `_udp_state()`.
|
||||
Assert:
|
||||
|
||||
```python
|
||||
adapter.connect()
|
||||
arm = fake_module.RoboticArm.instance
|
||||
assert arm.config.args == (5, True, 8090, 0, "192.168.192.148")
|
||||
assert arm.callback is adapter._realtime_callback
|
||||
assert adapter.get_latest_joint_state() is not None
|
||||
assert not hasattr(adapter, "_feedback_thread")
|
||||
```
|
||||
|
||||
Add failure cases where `rm_set_realtime_push()` returns `1`, and where
|
||||
`adapter._feedback_ready.wait` returns `False`. Both must raise `RuntimeError`;
|
||||
the fake arm must record one `rm_delete_robot_arm()` call.
|
||||
|
||||
- [ ] **Step 2: Run the focused tests and verify RED**
|
||||
|
||||
Run:
|
||||
|
||||
```bash
|
||||
cd /home/robot/WS_xr
|
||||
source /opt/ros/humble/setup.bash
|
||||
python3 -m pytest -q src/xr_rm_teleop/test/test_initial_joint_pose.py
|
||||
```
|
||||
|
||||
Expected: FAIL because `RealManAdapter` does not accept realtime push
|
||||
parameters and has no `_on_realtime_arm_state`.
|
||||
|
||||
---
|
||||
|
||||
### Task 2: Implement single-handle UDP feedback
|
||||
|
||||
**Files:**
|
||||
- Modify: `xr_rm_teleop/xr_rm_teleop/realman_adapter.py`
|
||||
- Modify: `xr_rm_teleop/xr_rm_teleop/single_arm_velocity_teleop.py`
|
||||
- Modify: `xr_rm_teleop/test/test_initial_joint_pose.py`
|
||||
|
||||
- [ ] **Step 1: Replace polling constructor state with realtime push state**
|
||||
|
||||
Change `RealManAdapter.__init__` positional parameters from `feedback_period`
|
||||
to:
|
||||
|
||||
```python
|
||||
realtime_push_host_ip: str,
|
||||
realtime_push_port: int,
|
||||
realtime_push_cycle_ms: int = 5,
|
||||
```
|
||||
|
||||
Validate with stdlib `ipaddress.IPv4Address`:
|
||||
|
||||
```python
|
||||
try:
|
||||
self._realtime_push_host_ip = str(
|
||||
ipaddress.IPv4Address(realtime_push_host_ip)
|
||||
)
|
||||
except ipaddress.AddressValueError as exc:
|
||||
raise ValueError("realtime_push_host_ip must be a valid IPv4 address") from exc
|
||||
if not 1 <= realtime_push_port <= 65535:
|
||||
raise ValueError("realtime_push_port must be between 1 and 65535")
|
||||
if realtime_push_cycle_ms <= 0 or realtime_push_cycle_ms % 5 != 0:
|
||||
raise ValueError("realtime_push_cycle_ms must be a positive multiple of 5")
|
||||
```
|
||||
|
||||
Store the port and cycle, then replace feedback thread members with:
|
||||
|
||||
```python
|
||||
self._feedback_ready = threading.Event()
|
||||
self._realtime_callback: Any | None = None
|
||||
self._accept_realtime_feedback = False
|
||||
self._feedback_fault_logged = False
|
||||
```
|
||||
|
||||
- [ ] **Step 2: Configure callback and UDP push during connect**
|
||||
|
||||
Import these SDK symbols inside `connect()` so mock mode stays SDK-free:
|
||||
|
||||
```python
|
||||
from Robotic_Arm.rm_robot_interface import (
|
||||
RoboticArm,
|
||||
rm_realtime_arm_state_callback_ptr,
|
||||
rm_realtime_push_config_t,
|
||||
rm_thread_mode_e,
|
||||
)
|
||||
```
|
||||
|
||||
After existing safety and optional initial-pose configuration:
|
||||
|
||||
```python
|
||||
self._feedback_ready.clear()
|
||||
self._accept_realtime_feedback = True
|
||||
self._realtime_callback = rm_realtime_arm_state_callback_ptr(
|
||||
self._on_realtime_arm_state
|
||||
)
|
||||
self._arm.rm_realtime_arm_state_call_back(self._realtime_callback)
|
||||
config = rm_realtime_push_config_t(
|
||||
self._realtime_push_cycle_ms,
|
||||
True,
|
||||
self._realtime_push_port,
|
||||
0,
|
||||
self._realtime_push_host_ip,
|
||||
)
|
||||
self._check_return(
|
||||
self._arm.rm_set_realtime_push(config),
|
||||
"rm_set_realtime_push",
|
||||
)
|
||||
if not self._feedback_ready.wait(timeout=2.0):
|
||||
raise RuntimeError(
|
||||
"RealMan UDP realtime feedback did not receive a valid frame within 2 seconds"
|
||||
)
|
||||
```
|
||||
|
||||
Wrap post-handle initialization so any exception disables callback acceptance,
|
||||
deletes the handle, sets `_arm = None`, and re-raises.
|
||||
|
||||
- [ ] **Step 3: Implement the bounded callback**
|
||||
|
||||
Replace `_feedback_loop()` and `_read_joint_state_once()` with:
|
||||
|
||||
```python
|
||||
def _on_realtime_arm_state(self, data: Any) -> None:
|
||||
if not self._accept_realtime_feedback:
|
||||
return
|
||||
try:
|
||||
if data is None or int(data.errCode) != 0:
|
||||
raise ValueError("invalid realtime feedback error code")
|
||||
arm_ip = data.arm_ip
|
||||
if isinstance(arm_ip, bytes):
|
||||
arm_ip = arm_ip.decode("utf-8").split("\x00", 1)[0]
|
||||
if str(arm_ip) != self._robot_ip:
|
||||
raise ValueError(f"unexpected realtime feedback source: {arm_ip}")
|
||||
degrees = list(data.joint_status.joint_position)
|
||||
if (
|
||||
len(degrees) != 7
|
||||
or not all(isinstance(value, Number) for value in degrees)
|
||||
):
|
||||
raise ValueError("RM75 UDP feedback must contain 7 numeric joints")
|
||||
positions = [math.radians(float(value)) for value in degrees]
|
||||
if not all(math.isfinite(value) for value in positions):
|
||||
raise ValueError("RM75 UDP feedback contains NaN/Inf")
|
||||
received_at = time.monotonic()
|
||||
with self._joint_state_lock:
|
||||
update_interval_ms = (
|
||||
None
|
||||
if self._latest_joint_state is None
|
||||
else (received_at - self._latest_joint_state.received_at) * 1000.0
|
||||
)
|
||||
self._latest_joint_state = JointStateSnapshot(
|
||||
positions,
|
||||
received_at,
|
||||
None,
|
||||
update_interval_ms,
|
||||
)
|
||||
self._feedback_fault_logged = False
|
||||
self._feedback_ready.set()
|
||||
except Exception as exc:
|
||||
if not self._feedback_fault_logged:
|
||||
self._log_warn(f"RealMan UDP realtime feedback invalid: {exc}")
|
||||
self._feedback_fault_logged = True
|
||||
```
|
||||
|
||||
In `close()`, set `_accept_realtime_feedback = False` before slow-stop and
|
||||
handle deletion. Remove feedback thread stop/join logic. Keep the callback
|
||||
reference alive until after `rm_delete_robot_arm()`.
|
||||
|
||||
- [ ] **Step 4: Declare and pass ROS parameters**
|
||||
|
||||
In `SingleArmVelocityTeleop`, declare:
|
||||
|
||||
```python
|
||||
self.declare_parameter("realtime_push_host_ip", "")
|
||||
self.declare_parameter("realtime_push_port", 0)
|
||||
self.declare_parameter("realtime_push_cycle_ms", 5)
|
||||
```
|
||||
|
||||
Replace `feedback_period=self._dt` in `_make_adapter()` with:
|
||||
|
||||
```python
|
||||
realtime_push_host_ip=str(
|
||||
self.get_parameter("realtime_push_host_ip").value
|
||||
),
|
||||
realtime_push_port=int(
|
||||
self.get_parameter("realtime_push_port").value
|
||||
),
|
||||
realtime_push_cycle_ms=int(
|
||||
self.get_parameter("realtime_push_cycle_ms").value
|
||||
),
|
||||
```
|
||||
|
||||
Update all direct `RealManAdapter(...)` calls in tests to pass a host and port.
|
||||
|
||||
- [ ] **Step 5: Run focused tests and verify GREEN**
|
||||
|
||||
Run:
|
||||
|
||||
```bash
|
||||
cd /home/robot/WS_xr
|
||||
source /opt/ros/humble/setup.bash
|
||||
python3 -m pytest -q src/xr_rm_teleop/test/test_initial_joint_pose.py
|
||||
```
|
||||
|
||||
Expected: all focused tests pass, with no real SDK connection.
|
||||
|
||||
---
|
||||
|
||||
### Task 3: Move robot defaults into YAML and simplify launch
|
||||
|
||||
**Files:**
|
||||
- Modify: `xr_rm_bringup/config/right_arm_rm75.yaml`
|
||||
- Modify: `xr_rm_bringup/config/left_arm_rm75.yaml`
|
||||
- Modify: `xr_rm_bringup/config/dual_arm_rm75.yaml`
|
||||
- Modify: `xr_rm_bringup/launch/arm_debug.launch.py`
|
||||
|
||||
- [ ] **Step 1: Run a failing ownership assertion**
|
||||
|
||||
Run a one-off Python assertion that requires the three YAMLs to contain UDP
|
||||
and tool parameters, and requires launch not to declare robot behavior
|
||||
arguments:
|
||||
|
||||
```python
|
||||
from pathlib import Path
|
||||
import yaml
|
||||
|
||||
config_dir = Path("xr_rm_bringup/config")
|
||||
for name in ("left_arm_rm75.yaml", "right_arm_rm75.yaml"):
|
||||
params = yaml.safe_load((config_dir / name).read_text())
|
||||
params = params["single_arm_velocity_teleop"]["ros__parameters"]
|
||||
assert "use_mock" not in params
|
||||
assert params["realtime_push_host_ip"] == "192.168.192.148"
|
||||
assert params["realtime_push_cycle_ms"] == 5
|
||||
assert params["enable_tool_control"] is True
|
||||
|
||||
source = Path("xr_rm_bringup/launch/arm_debug.launch.py").read_text()
|
||||
for name in (
|
||||
"left_robot_ip",
|
||||
"right_robot_ip",
|
||||
"robot_port",
|
||||
"avoid_singularity",
|
||||
"control_rate_hz",
|
||||
"follow",
|
||||
"configure_safety_limits",
|
||||
"move_to_initial_pose_on_connect",
|
||||
):
|
||||
assert f'DeclareLaunchArgument("{name}"' not in source
|
||||
```
|
||||
|
||||
Expected: FAIL because the YAML parameters are missing and launch still
|
||||
declares overrides.
|
||||
|
||||
- [ ] **Step 2: Update all YAML nodes**
|
||||
|
||||
Remove `use_mock`. Add:
|
||||
|
||||
```yaml
|
||||
realtime_push_host_ip: 192.168.192.148
|
||||
realtime_push_cycle_ms: 5
|
||||
enable_tool_control: true
|
||||
enable_trigger_gripper_control: true
|
||||
trigger_close_threshold: 0.95
|
||||
configure_peripheral_on_connect: true
|
||||
```
|
||||
|
||||
Use `realtime_push_port: 8089` for left-arm nodes and `8090` for right-arm
|
||||
nodes. Keep:
|
||||
|
||||
```yaml
|
||||
# all single-arm and dual-arm nodes
|
||||
follow: false
|
||||
canfd_trajectory_mode: 2
|
||||
```
|
||||
|
||||
The right-arm high-follow default was reverted after the first hardware test
|
||||
exposed an unplanned stationary null-space trajectory. Do not change speeds,
|
||||
workspace limits, timeouts, safety limits, or initial pose defaults.
|
||||
|
||||
- [ ] **Step 3: Reduce launch overrides**
|
||||
|
||||
Make `_single_arm_node(arm, use_mock)` and `_dual_arm_nodes(use_mock)` load
|
||||
their YAML first, then pass only:
|
||||
|
||||
```python
|
||||
{
|
||||
"use_mock": use_mock,
|
||||
"robot_urdf_path": _rm75_urdf(),
|
||||
"peripheral_config_file": _config_file("peripherals_rm75.yaml"),
|
||||
"peripheral_arm": arm,
|
||||
"tool_command_topic": f"/xr_rm/{_arm_name(arm)}/tool_enable",
|
||||
}
|
||||
```
|
||||
|
||||
Keep equivalent per-side generated values in dual mode. Remove
|
||||
`_initial_pose_override`, robot IP/port, avoid-singularity, control-rate,
|
||||
follow, safety, tool-control and initial-pose parsing from `_launch_setup`.
|
||||
|
||||
Keep only these launch arguments:
|
||||
|
||||
```python
|
||||
DeclareLaunchArgument("arm", default_value="right")
|
||||
DeclareLaunchArgument("use_mock", default_value="true")
|
||||
DeclareLaunchArgument("udp_host", default_value="0.0.0.0")
|
||||
DeclareLaunchArgument("udp_port", default_value="15000")
|
||||
DeclareLaunchArgument("udp_timer_hz", default_value="200.0")
|
||||
```
|
||||
|
||||
- [ ] **Step 4: Re-run ownership assertion and inspect launch arguments**
|
||||
|
||||
Run the assertion from Step 1, then:
|
||||
|
||||
```bash
|
||||
cd /home/robot/WS_xr
|
||||
source /opt/ros/humble/setup.bash
|
||||
source install/setup.bash
|
||||
ros2 launch xr_rm_bringup arm_debug.launch.py --show-args
|
||||
```
|
||||
|
||||
Expected: the assertion passes; launch lists only `arm`, `use_mock`,
|
||||
`udp_host`, `udp_port`, and `udp_timer_hz`.
|
||||
|
||||
---
|
||||
|
||||
### Task 4: Synchronize launcher UI and README
|
||||
|
||||
**Files:**
|
||||
- Modify: `xr_rm_bringup/tools/launcher_ui.py`
|
||||
- Modify: `README.md`
|
||||
|
||||
- [ ] **Step 1: Remove deleted launch arguments from UI commands**
|
||||
|
||||
Keep ping targets unchanged. Change real launch commands to:
|
||||
|
||||
```python
|
||||
"ros2 launch xr_rm_bringup arm_debug.launch.py arm:=left use_mock:=false"
|
||||
"ros2 launch xr_rm_bringup arm_debug.launch.py arm:=right use_mock:=false"
|
||||
"ros2 launch xr_rm_bringup arm_debug.launch.py arm:=both use_mock:=false"
|
||||
```
|
||||
|
||||
- [ ] **Step 2: Update README ownership and commands**
|
||||
|
||||
Remove examples and launch-argument descriptions for robot IP/port,
|
||||
avoid-singularity, control-rate, follow, safety/tool flags, and initial-pose
|
||||
overrides. State that these values live in the selected YAML. Add the UDP
|
||||
feedback parameters, host `192.168.192.148`, ports `8089/8090`, 5 ms cycle,
|
||||
and the command used after Wi-Fi changes:
|
||||
|
||||
```bash
|
||||
ip -4 route get 192.168.192.19
|
||||
```
|
||||
|
||||
Keep `arm`, `use_mock`, and PICO UDP arguments documented as launch
|
||||
arguments. Keep the warning that checked-in default `use_mock=true` prevents
|
||||
an accidental real connection.
|
||||
|
||||
- [ ] **Step 3: Check syntax and stale references**
|
||||
|
||||
Run:
|
||||
|
||||
```bash
|
||||
python3 -m py_compile \
|
||||
xr_rm_bringup/launch/arm_debug.launch.py \
|
||||
xr_rm_bringup/tools/launcher_ui.py
|
||||
rg -n "left_robot_ip:=|right_robot_ip:=|move_to_initial_pose_on_connect:=" \
|
||||
README.md xr_rm_bringup/tools/launcher_ui.py
|
||||
```
|
||||
|
||||
Expected: compilation passes; `rg` returns no stale command-line overrides.
|
||||
|
||||
---
|
||||
|
||||
### Task 5: Full verification
|
||||
|
||||
**Files:**
|
||||
- Verify all files changed by Tasks 1–4
|
||||
|
||||
- [ ] **Step 1: Run teleop tests**
|
||||
|
||||
```bash
|
||||
cd /home/robot/WS_xr
|
||||
source /opt/ros/humble/setup.bash
|
||||
python3 -m pytest -q src/xr_rm_teleop/test
|
||||
python3 -m pytest -q src/xr_rm_teleop/test/test_orientation_control.py
|
||||
```
|
||||
|
||||
Expected: all tests pass.
|
||||
|
||||
- [ ] **Step 2: Build all workspace packages**
|
||||
|
||||
```bash
|
||||
cd /home/robot/WS_xr
|
||||
source /opt/ros/humble/setup.bash
|
||||
colcon build --symlink-install --executor sequential
|
||||
```
|
||||
|
||||
Expected: `xr_rm_interfaces`, `xr_rm_input`, `xr_rm_teleop`, and
|
||||
`xr_rm_bringup` all finish successfully.
|
||||
|
||||
- [ ] **Step 3: Verify mock launch without vendor hardware**
|
||||
|
||||
```bash
|
||||
cd /home/robot/WS_xr
|
||||
source /opt/ros/humble/setup.bash
|
||||
source install/setup.bash
|
||||
timeout 8s ros2 launch xr_rm_bringup arm_debug.launch.py \
|
||||
arm:=right use_mock:=true
|
||||
```
|
||||
|
||||
Expected: the mock teleop and UDP input nodes start; timeout ends the launch.
|
||||
No RealMan SDK connection is attempted.
|
||||
|
||||
- [ ] **Step 4: Inspect final diff**
|
||||
|
||||
```bash
|
||||
cd /home/robot/WS_xr/src
|
||||
git diff --check
|
||||
git status --short
|
||||
git diff --stat
|
||||
```
|
||||
|
||||
Expected: no whitespace errors and no unrelated files. Do not commit, push,
|
||||
or connect to the real robot unless the user explicitly requests it.
|
||||
@@ -0,0 +1,42 @@
|
||||
# RM75 Right-Arm High-Follow YAML Defaults Implementation Plan
|
||||
|
||||
> **For agentic workers:** REQUIRED SUB-SKILL: Use superpowers:subagent-driven-development (recommended) or superpowers:executing-plans to implement this plan task-by-task. Steps use checkbox (`- [ ]`) syntax for tracking.
|
||||
|
||||
**Goal:** Make right-arm single-arm debugging default to RealMan high-follow complete passthrough for phase-two testing.
|
||||
|
||||
**Architecture:** Change only the existing right-arm YAML parameters. Keep launch, left-arm, dual-arm, speed limits, safety limits, timeouts, and stop behavior unchanged.
|
||||
|
||||
**Tech Stack:** ROS2 Humble, YAML, pytest, colcon
|
||||
|
||||
---
|
||||
|
||||
### Task 1: Change right-arm CANFD defaults
|
||||
|
||||
**Files:**
|
||||
- Modify: `xr_rm_bringup/config/right_arm_rm75.yaml`
|
||||
|
||||
- [ ] **Step 1: Run a failing configuration assertion**
|
||||
|
||||
Run a Python YAML assertion requiring `follow is True` and
|
||||
`canfd_trajectory_mode == 0`.
|
||||
|
||||
Expected: FAIL because the current values are `false` and `2`.
|
||||
|
||||
- [ ] **Step 2: Apply the minimal configuration change**
|
||||
|
||||
```yaml
|
||||
follow: true
|
||||
canfd_trajectory_mode: 0
|
||||
```
|
||||
|
||||
- [ ] **Step 3: Verify configuration and regressions**
|
||||
|
||||
Run the same YAML assertion and expect PASS. Then run:
|
||||
|
||||
```bash
|
||||
source /opt/ros/humble/setup.bash
|
||||
python3 -m pytest -q src/xr_rm_teleop/test
|
||||
colcon build --symlink-install --executor sequential
|
||||
```
|
||||
|
||||
Expected: all tests and all four workspace packages pass.
|
||||
Reference in New Issue
Block a user