Remove outdated design documents for RM75 control and feedback systems
This commit is contained in:
File diff suppressed because it is too large
Load Diff
@@ -1,154 +0,0 @@
|
||||
# RM75 Control Timing Stats 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:** 在 Grip 激活期间每约 5 秒向 `arm_debug.launch.py` 终端输出一次控制链路耗时统计。
|
||||
|
||||
**Architecture:** 在现有 `SingleArmVelocityTeleop` 控制回调内使用单调高精度时钟记录实际周期、控制路径总耗时、QP、关节发送和反馈年龄。节点保存一个固定长度样本窗口，满窗后用 NumPy 计算 mean/P95/P99/max，输出一条 ROS 日志并清空窗口。
|
||||
|
||||
**Tech Stack:** Python 3.10、ROS2 Humble `rclpy`、NumPy、pytest。
|
||||
|
||||
---
|
||||
|
||||
### Task 1: 控制周期统计
|
||||
|
||||
**Files:**
|
||||
- Modify: `xr_rm_teleop/test/test_joint_control.py`
|
||||
- Modify: `xr_rm_teleop/xr_rm_teleop/single_arm_velocity_teleop.py`
|
||||
|
||||
- [x] **Step 1: 写失败测试**
|
||||
|
||||
在 `test_joint_control.py` 添加确定性两样本窗口测试：
|
||||
|
||||
```python
|
||||
def test_timing_stats_logs_summary_and_clears_window() -> None:
|
||||
messages = []
|
||||
teleop = object.__new__(SingleArmVelocityTeleop)
|
||||
teleop._arm_name = "right_rm75"
|
||||
teleop._dt = 0.008
|
||||
teleop._timing_stats_window = 2
|
||||
teleop._timing_samples = {
|
||||
name: []
|
||||
for name in ("period", "total", "qp", "send", "feedback_age")
|
||||
}
|
||||
teleop.get_logger = lambda: SimpleNamespace(
|
||||
info=lambda message: messages.append(message)
|
||||
)
|
||||
|
||||
teleop._record_timing_sample(7.0, 6.0, 1.0, 0.5, 3.0)
|
||||
assert messages == []
|
||||
|
||||
teleop._record_timing_sample(9.0, 10.0, 2.0, 0.7, 4.0)
|
||||
|
||||
assert len(messages) == 1
|
||||
assert "right_rm75 timing n=2 deadline=8.000 ms" in messages[0]
|
||||
assert "period[n=2 mean=8.000 p95=8.900 p99=8.980 max=9.000 ms overruns=1]" in messages[0]
|
||||
assert "total[n=2 mean=8.000 p95=9.800 p99=9.960 max=10.000 ms overruns=1]" in messages[0]
|
||||
assert "qp[n=2" in messages[0]
|
||||
assert "send[n=2" in messages[0]
|
||||
assert "feedback_age[n=2" in messages[0]
|
||||
assert all(not samples for samples in teleop._timing_samples.values())
|
||||
```
|
||||
|
||||
- [x] **Step 2: 确认测试因功能缺失而失败**
|
||||
|
||||
在工作空间根目录运行：
|
||||
|
||||
```bash
|
||||
source /opt/ros/humble/setup.bash
|
||||
PYTHONPATH=src/xr_rm_teleop pytest -q \
|
||||
src/xr_rm_teleop/test/test_joint_control.py::test_timing_stats_logs_summary_and_clears_window
|
||||
```
|
||||
|
||||
预期：失败并提示 `SingleArmVelocityTeleop` 没有 `_record_timing_sample`。
|
||||
|
||||
- [x] **Step 3: 实现最小统计逻辑**
|
||||
|
||||
在节点初始化中创建约 5 秒的窗口：
|
||||
|
||||
```python
|
||||
self._timing_stats_window = max(1, int(round(5.0 / self._dt)))
|
||||
self._timing_samples = {
|
||||
name: []
|
||||
for name in ("period", "total", "qp", "send", "feedback_age")
|
||||
}
|
||||
self._last_control_tick_started_ns: int | None = None
|
||||
```
|
||||
|
||||
为每组样本计算统计摘要：
|
||||
|
||||
```python
|
||||
def _timing_summary(
|
||||
self,
|
||||
name: str,
|
||||
samples: list[float],
|
||||
deadline_ms: float | None = None,
|
||||
) -> str:
|
||||
values = np.asarray(samples)
|
||||
result = (
|
||||
f"{name}[n={len(samples)} mean={np.mean(values):.3f} "
|
||||
f"p95={np.percentile(values, 95):.3f} "
|
||||
f"p99={np.percentile(values, 99):.3f} "
|
||||
f"max={np.max(values):.3f} ms"
|
||||
)
|
||||
if deadline_ms is not None:
|
||||
result += f" overruns={np.count_nonzero(values > deadline_ms)}"
|
||||
return result + "]"
|
||||
```
|
||||
|
||||
满窗后输出并清空：
|
||||
|
||||
```python
|
||||
def _record_timing_sample(
|
||||
self,
|
||||
period_ms: float | None,
|
||||
total_ms: float,
|
||||
qp_ms: float,
|
||||
send_ms: float,
|
||||
feedback_age_ms: float,
|
||||
) -> None:
|
||||
if period_ms is not None:
|
||||
self._timing_samples["period"].append(period_ms)
|
||||
self._timing_samples["total"].append(total_ms)
|
||||
self._timing_samples["qp"].append(qp_ms)
|
||||
self._timing_samples["send"].append(send_ms)
|
||||
self._timing_samples["feedback_age"].append(feedback_age_ms)
|
||||
if len(self._timing_samples["total"]) < self._timing_stats_window:
|
||||
return
|
||||
|
||||
deadline_ms = self._dt * 1000.0
|
||||
summaries = [
|
||||
self._timing_summary("period", self._timing_samples["period"], deadline_ms),
|
||||
self._timing_summary("total", self._timing_samples["total"], deadline_ms),
|
||||
self._timing_summary("qp", self._timing_samples["qp"]),
|
||||
self._timing_summary("send", self._timing_samples["send"]),
|
||||
self._timing_summary("feedback_age", self._timing_samples["feedback_age"]),
|
||||
]
|
||||
self.get_logger().info(
|
||||
f"{self._arm_name} timing n={len(self._timing_samples['total'])} "
|
||||
f"deadline={deadline_ms:.3f} ms | " + " | ".join(summaries)
|
||||
)
|
||||
for samples in self._timing_samples.values():
|
||||
samples.clear()
|
||||
```
|
||||
|
||||
在 `_control_tick()` 中围绕 QP 和发送调用采样，并在关节命令处理完成后记录总耗时。早退周期不进入统计窗口，现有控制和安全逻辑保持不变。
|
||||
|
||||
- [x] **Step 4: 运行测试确认通过**
|
||||
|
||||
```bash
|
||||
source /opt/ros/humble/setup.bash
|
||||
PYTHONPATH=src/xr_rm_teleop pytest -q src/xr_rm_teleop/test/test_joint_control.py
|
||||
```
|
||||
|
||||
预期：全部通过。
|
||||
|
||||
- [x] **Step 5: 完整验证**
|
||||
|
||||
```bash
|
||||
source /opt/ros/humble/setup.bash
|
||||
pytest -q src/xr_rm_teleop/test/test_orientation_control.py
|
||||
colcon build --symlink-install
|
||||
```
|
||||
|
||||
预期：姿态测试和工作空间构建全部通过。根据仓库规则，不自动提交 Git。
|
||||
@@ -1,109 +0,0 @@
|
||||
# RM75 Feedback Absolute Scheduling 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:** 将关节反馈线程从“读取后固定等待 8 ms”改为无历史周期补跑的绝对起始周期调度。
|
||||
|
||||
**Architecture:** `RealManAdapter._feedback_loop()` 保留现有读取、告警和停止结构，只把固定 `Event.wait(feedback_period)` 替换为下一截止时间计算。读取提前完成时等待剩余时间；读取超期时重置调度基准并立即进入下一周期。
|
||||
|
||||
**Tech Stack:** Python 3.10、threading、time.monotonic、pytest、ROS2 Humble、colcon
|
||||
|
||||
---
|
||||
|
||||
### Task 1: 反馈绝对周期调度
|
||||
|
||||
**Files:**
|
||||
- Modify: `xr_rm_teleop/xr_rm_teleop/realman_adapter.py`
|
||||
- Test: `xr_rm_teleop/test/test_initial_joint_pose.py`
|
||||
|
||||
- [ ] **Step 1: 写失败测试**
|
||||
|
||||
用确定性的 FakeTime 和 FakeStopEvent 运行 `_feedback_loop()` 三次读取：
|
||||
|
||||
- 第一次读取在 5 ms 完成，应只等待剩余 3 ms；
|
||||
- 第二次在 18 ms 完成，超过 16 ms 截止时间，应不等待并把基准重置为
|
||||
18 ms；
|
||||
- 第三次在 23 ms 完成，应等待到新基准的 26 ms，即再次等待 3 ms。
|
||||
|
||||
断言读取三次且 `wait()` 参数为 `[0.003, 0.003]`。该结果同时证明没有补跑
|
||||
旧的 8 ms 和 16 ms 截止点。
|
||||
|
||||
- [ ] **Step 2: 确认测试失败**
|
||||
|
||||
```bash
|
||||
source /opt/ros/humble/setup.bash
|
||||
python3 -m pytest -q src/xr_rm_teleop/test/test_initial_joint_pose.py
|
||||
```
|
||||
|
||||
预期：当前固定等待实现记录 `[0.008, 0.008, 0.008]`，测试失败。
|
||||
|
||||
- [ ] **Step 3: 写最小实现**
|
||||
|
||||
把 `_feedback_loop()` 改为：
|
||||
|
||||
```python
|
||||
def _feedback_loop(self) -> None:
|
||||
next_read_at = time.monotonic()
|
||||
while not self._feedback_stop.is_set():
|
||||
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)
|
||||
```
|
||||
|
||||
- [ ] **Step 4: 确认局部测试通过**
|
||||
|
||||
重复 Step 2 命令。预期：全部 PASS。
|
||||
|
||||
### Task 2: 调度回归验证
|
||||
|
||||
**Files:**
|
||||
- Verify: `xr_rm_teleop`
|
||||
- Verify: ROS2 workspace
|
||||
|
||||
- [ ] **Step 1: 运行遥操作包测试**
|
||||
|
||||
```bash
|
||||
source /opt/ros/humble/setup.bash
|
||||
python3 -m pytest -q src/xr_rm_teleop/test
|
||||
```
|
||||
|
||||
预期：全部 PASS。
|
||||
|
||||
- [ ] **Step 2: 运行姿态控制指定测试**
|
||||
|
||||
```bash
|
||||
source /opt/ros/humble/setup.bash
|
||||
python3 -m pytest -q src/xr_rm_teleop/test/test_orientation_control.py
|
||||
```
|
||||
|
||||
预期：全部 PASS。
|
||||
|
||||
- [ ] **Step 3: 构建工作空间**
|
||||
|
||||
```bash
|
||||
source /opt/ros/humble/setup.bash
|
||||
colcon build --symlink-install
|
||||
```
|
||||
|
||||
预期：四个包构建成功。
|
||||
|
||||
- [ ] **Step 4: 检查最终差异**
|
||||
|
||||
```bash
|
||||
git diff --check
|
||||
git status --short
|
||||
```
|
||||
|
||||
预期：只包含已确认的统计、工具坐标系幂等修复、反馈绝对周期调度、对应测试
|
||||
及 Superpowers 文档。按仓库规则不自动提交。
|
||||
@@ -1,167 +0,0 @@
|
||||
# RM75 Feedback Thread Timing 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:** 在不改变反馈轮询和机械臂控制行为的前提下，统计 `rm_get_joint_degree()` 调用耗时与成功反馈更新间隔。
|
||||
|
||||
**Architecture:** `RealManAdapter` 在反馈读取边界测量时间，并把可选计时值随 `JointStateSnapshot` 放入现有缓存。`SingleArmVelocityTeleop` 复用现有 timing 窗口，只对新的反馈时间戳记录一次并输出汇总。
|
||||
|
||||
**Tech Stack:** Python 3.10、ROS2 Humble、pytest、NumPy、colcon
|
||||
|
||||
---
|
||||
|
||||
### Task 1: 在反馈缓存中携带真实读取计时
|
||||
|
||||
**Files:**
|
||||
- Modify: `xr_rm_teleop/xr_rm_teleop/realman_adapter.py`
|
||||
- Test: `xr_rm_teleop/test/test_initial_joint_pose.py`
|
||||
|
||||
- [ ] **Step 1: 写失败测试**
|
||||
|
||||
在 `test_joint_feedback_is_cached_in_radians` 中通过 `monkeypatch` 固定
|
||||
`perf_counter_ns()` 和 `monotonic()`，连续读取两次，并验证：
|
||||
|
||||
```python
|
||||
assert first.read_duration_ms == pytest.approx(2.0)
|
||||
assert first.update_interval_ms is None
|
||||
assert second.read_duration_ms == pytest.approx(3.0)
|
||||
assert second.update_interval_ms == pytest.approx(11.0)
|
||||
```
|
||||
|
||||
- [ ] **Step 2: 确认测试失败**
|
||||
|
||||
在 `/home/robot/WS_xr` 执行：
|
||||
|
||||
```bash
|
||||
source /opt/ros/humble/setup.bash
|
||||
python3 -m pytest -q src/xr_rm_teleop/test/test_initial_joint_pose.py::test_joint_feedback_is_cached_in_radians
|
||||
```
|
||||
|
||||
预期：因 `JointStateSnapshot` 尚无计时字段而失败。
|
||||
|
||||
- [ ] **Step 3: 最小实现反馈计时**
|
||||
|
||||
给快照增加可选字段，保持现有两参数构造兼容：
|
||||
|
||||
```python
|
||||
@dataclass(frozen=True)
|
||||
class JointStateSnapshot:
|
||||
positions: list[float]
|
||||
received_at: float
|
||||
read_duration_ms: float | None = None
|
||||
update_interval_ms: float | None = None
|
||||
```
|
||||
|
||||
在 `_read_joint_state_once()` 中只包围 SDK 调用测量 `read_duration_ms`；数据校验
|
||||
成功后取得 `received_at`，并在缓存锁内根据上一快照计算
|
||||
`update_interval_ms`。`get_latest_joint_state()` 同步复制两个字段。Mock 使用
|
||||
字段默认值，不伪造计时。
|
||||
|
||||
- [ ] **Step 4: 确认局部测试通过**
|
||||
|
||||
重复 Step 2 命令。预期：PASS。
|
||||
|
||||
### Task 2: 将唯一反馈样本加入现有 timing 汇总
|
||||
|
||||
**Files:**
|
||||
- Modify: `xr_rm_teleop/xr_rm_teleop/single_arm_velocity_teleop.py`
|
||||
- Test: `xr_rm_teleop/test/test_joint_control.py`
|
||||
|
||||
- [ ] **Step 1: 写失败测试**
|
||||
|
||||
扩展 `test_timing_stats_logs_summary_and_clears_window`：在三个控制样本中传入
|
||||
“快照 A、重复快照 A、快照 B”，并断言日志包含：
|
||||
|
||||
```python
|
||||
assert "feedback_read[n=2" in messages[0]
|
||||
assert "feedback_interval[n=1" in messages[0]
|
||||
```
|
||||
|
||||
这样同时验证新反馈只计一次、重复缓存不重复计数、首次反馈无更新间隔。
|
||||
|
||||
- [ ] **Step 2: 确认测试失败**
|
||||
|
||||
在 `/home/robot/WS_xr` 执行：
|
||||
|
||||
```bash
|
||||
source /opt/ros/humble/setup.bash
|
||||
python3 -m pytest -q src/xr_rm_teleop/test/test_joint_control.py::test_timing_stats_logs_summary_and_clears_window
|
||||
```
|
||||
|
||||
预期：因 timing 样本尚不支持新字段而失败。
|
||||
|
||||
- [ ] **Step 3: 最小实现唯一反馈统计**
|
||||
|
||||
在节点初始化时：
|
||||
|
||||
```python
|
||||
self._timing_samples = {
|
||||
name: []
|
||||
for name in (
|
||||
"period",
|
||||
"total",
|
||||
"qp",
|
||||
"send",
|
||||
"feedback_age",
|
||||
"feedback_read",
|
||||
"feedback_interval",
|
||||
)
|
||||
}
|
||||
self._last_timing_feedback_received_at: float | None = None
|
||||
```
|
||||
|
||||
让 `_record_timing_sample()` 接收当前 `JointStateSnapshot`。仅当
|
||||
`received_at` 与 `_last_timing_feedback_received_at` 不同时，追加非 `None`
|
||||
的读取耗时和更新间隔。汇总时仅输出非空的新数组，避免 mock 模式对空数组
|
||||
求百分位数。控制循环把已有 `snapshot` 传入该函数。
|
||||
|
||||
- [ ] **Step 4: 确认相关测试通过**
|
||||
|
||||
```bash
|
||||
source /opt/ros/humble/setup.bash
|
||||
python3 -m pytest -q src/xr_rm_teleop/test/test_joint_control.py src/xr_rm_teleop/test/test_initial_joint_pose.py
|
||||
```
|
||||
|
||||
预期：全部 PASS。
|
||||
|
||||
### Task 3: 回归验证
|
||||
|
||||
**Files:**
|
||||
- Verify: `xr_rm_teleop`
|
||||
- Verify: ROS2 workspace
|
||||
|
||||
- [ ] **Step 1: 运行遥操作包测试**
|
||||
|
||||
```bash
|
||||
source /opt/ros/humble/setup.bash
|
||||
python3 -m pytest -q src/xr_rm_teleop/test
|
||||
```
|
||||
|
||||
预期：全部 PASS。
|
||||
|
||||
- [ ] **Step 2: 运行姿态控制指定测试**
|
||||
|
||||
```bash
|
||||
source /opt/ros/humble/setup.bash
|
||||
python3 -m pytest -q src/xr_rm_teleop/test/test_orientation_control.py
|
||||
```
|
||||
|
||||
预期：全部 PASS。
|
||||
|
||||
- [ ] **Step 3: 构建工作空间**
|
||||
|
||||
```bash
|
||||
source /opt/ros/humble/setup.bash
|
||||
colcon build --symlink-install
|
||||
```
|
||||
|
||||
预期：四个包构建成功。
|
||||
|
||||
- [ ] **Step 4: 检查差异**
|
||||
|
||||
```bash
|
||||
git diff --check
|
||||
git status --short
|
||||
```
|
||||
|
||||
预期：只包含设计、计划、反馈计时实现及相关测试。按仓库规则不自动提交。
|
||||
@@ -1,107 +0,0 @@
|
||||
# RM75 Idempotent Tool Frame 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:** 让 RealMan 工具坐标系在首次启动时创建、后续启动时更新，并检查所有相关 SDK 返回值。
|
||||
|
||||
**Architecture:** `fun_peripheral.py` 增加一个只负责工具坐标系的内部函数，先查询名称列表，再选择创建或更新，最后切换。现有 `peripheral_cfg()` 继续负责 IO 和夹爪初始化，只把原来的两次无检查调用替换为该函数。
|
||||
|
||||
**Tech Stack:** Python 3.10、RealMan Python API2、pytest、ROS2 Humble、colcon
|
||||
|
||||
---
|
||||
|
||||
### Task 1: 工具坐标系幂等配置
|
||||
|
||||
**Files:**
|
||||
- Modify: `xr_rm_teleop/xr_rm_teleop/fun_peripheral.py`
|
||||
- Test: `xr_rm_teleop/test/test_initial_joint_pose.py`
|
||||
|
||||
- [ ] **Step 1: 写失败测试**
|
||||
|
||||
导入新的 `_configure_tool_frame`，用 FakeArm 分别返回包含和不包含 `omnipic`
|
||||
的名称列表。断言不存在时调用 `rm_set_manual_tool_frame`，存在时调用
|
||||
`rm_update_tool_frame`，两条路径最后都调用 `rm_change_tool_frame`。
|
||||
|
||||
再用参数化失败返回码验证查询、创建/更新和切换失败均抛出包含 SDK 操作名称
|
||||
的 `RuntimeError`。
|
||||
|
||||
- [ ] **Step 2: 确认测试失败**
|
||||
|
||||
```bash
|
||||
source /opt/ros/humble/setup.bash
|
||||
python3 -m pytest -q src/xr_rm_teleop/test/test_initial_joint_pose.py
|
||||
```
|
||||
|
||||
预期：因 `_configure_tool_frame` 尚不存在而在测试收集阶段失败。
|
||||
|
||||
- [ ] **Step 3: 写最小实现**
|
||||
|
||||
在 `fun_peripheral.py` 增加：
|
||||
|
||||
```python
|
||||
def _check_sdk_return(result: Any, operation: str) -> None:
|
||||
if result != 0:
|
||||
raise RuntimeError(f"{operation} failed with code {result}: {result!r}")
|
||||
|
||||
|
||||
def _configure_tool_frame(robot, tool_frame, tool_name: str) -> None:
|
||||
frames = robot.rm_get_total_tool_frame()
|
||||
if not isinstance(frames, dict):
|
||||
raise RuntimeError(
|
||||
f"rm_get_total_tool_frame returned invalid data: {frames!r}"
|
||||
)
|
||||
_check_sdk_return(
|
||||
frames.get("return_code"),
|
||||
"rm_get_total_tool_frame",
|
||||
)
|
||||
tool_names = frames.get("tool_names")
|
||||
if not isinstance(tool_names, (list, tuple)):
|
||||
raise RuntimeError(
|
||||
f"rm_get_total_tool_frame returned invalid tool_names: {tool_names!r}"
|
||||
)
|
||||
|
||||
if tool_name in tool_names:
|
||||
operation = "rm_update_tool_frame"
|
||||
result = robot.rm_update_tool_frame(frame=tool_frame)
|
||||
else:
|
||||
operation = "rm_set_manual_tool_frame"
|
||||
result = robot.rm_set_manual_tool_frame(frame=tool_frame)
|
||||
_check_sdk_return(result, operation)
|
||||
_check_sdk_return(
|
||||
robot.rm_change_tool_frame(tool_name),
|
||||
"rm_change_tool_frame",
|
||||
)
|
||||
```
|
||||
|
||||
在 `peripheral_cfg()` 中用
|
||||
`_configure_tool_frame(robot, tool_frame, tool_name)` 替换原来的创建和切换
|
||||
调用。
|
||||
|
||||
- [ ] **Step 4: 确认测试通过**
|
||||
|
||||
重复 Step 2 命令。预期：全部 PASS。
|
||||
|
||||
### Task 2: 工具修复回归验证
|
||||
|
||||
**Files:**
|
||||
- Verify: `xr_rm_teleop`
|
||||
- Verify: ROS2 workspace
|
||||
|
||||
- [ ] **Step 1: 运行遥操作包测试**
|
||||
|
||||
```bash
|
||||
source /opt/ros/humble/setup.bash
|
||||
python3 -m pytest -q src/xr_rm_teleop/test
|
||||
```
|
||||
|
||||
预期：全部 PASS。
|
||||
|
||||
- [ ] **Step 2: 构建工作空间**
|
||||
|
||||
```bash
|
||||
source /opt/ros/humble/setup.bash
|
||||
colcon build --symlink-install
|
||||
```
|
||||
|
||||
预期：四个包构建成功。
|
||||
|
||||
@@ -1,390 +0,0 @@
|
||||
# RM75 SO(3) 姿态跟随与 OmniPicker 模型 Implementation Plan
|
||||
|
||||
> **For Codex:** REQUIRED SUB-SKILL: Use `superpowers:executing-plans` to implement this plan task-by-task.
|
||||
|
||||
**Goal:** 去掉遥操作控制路径中的 RPY 往返转换，使 RM75 TCP 姿态始终沿 SO(3) 最短路径跟随，并让左右臂的 Placo QP 直接控制一体化模型中的 `omnipicker_tcp`。
|
||||
|
||||
**Architecture:** 保留现有单节点、单步 Placo QP、关节反馈、RealMan 连接和安全停止链路。XR 四元数映射为机器人旋转矩阵；平移使用直接位置差，姿态使用 SO(3) 对数误差，二者以解耦 `3+3` 形式处理。Placo 接收完整 `4×4` 目标矩阵并直接约束 URDF 的 `omnipicker_tcp`，不再读取外设工具位姿做 QP 末端换算。
|
||||
|
||||
**Tech Stack:** Ubuntu 22.04、ROS2 Humble、Python 3.10、NumPy、Placo 0.9.4、Pinocchio 3.7.0、pytest、URDF。
|
||||
|
||||
**Repository rule:** 不执行 `git commit`、`git push` 或真机命令。所有启动验证必须显式使用 `use_mock:=true`；`peripherals_rm75.yaml`、`avoid_singularity`、可操作度任务和既有安全限制保持不变。
|
||||
|
||||
---
|
||||
|
||||
## 文件范围
|
||||
|
||||
- Create: `xr_rm_teleop/models/rm75_omnipicker/urdf/RM75-B_OmniPicker_fixed.urdf`
|
||||
- Create: `xr_rm_teleop/models/rm75_omnipicker/meshes/rm75/*.STL`
|
||||
- Create: `xr_rm_teleop/models/rm75_omnipicker/meshes/omnipicker/*.STL`
|
||||
- Modify: `xr_rm_teleop/setup.py`
|
||||
- Modify: `xr_rm_teleop/xr_rm_teleop/placo_ik_solver.py`
|
||||
- Modify: `xr_rm_teleop/xr_rm_teleop/single_arm_velocity_teleop.py`
|
||||
- Modify: `xr_rm_teleop/test/test_orientation_control.py`
|
||||
- Modify: `xr_rm_teleop/test/test_placo_transforms.py`
|
||||
- Modify: `xr_rm_teleop/test/test_joint_control.py`
|
||||
- Modify: `xr_rm_teleop/test/placo_ik_smoke.py`
|
||||
- Modify: `xr_rm_bringup/launch/arm_debug.launch.py`
|
||||
- Modify: `xr_rm_bringup/config/left_arm_rm75.yaml`
|
||||
- Modify: `xr_rm_bringup/config/right_arm_rm75.yaml`
|
||||
- Modify: `xr_rm_bringup/config/dual_arm_rm75.yaml`
|
||||
- Modify: `README.md`
|
||||
|
||||
不删除旧 `xr_rm_teleop/models/rm75` 资源，只让 launch 停止选用它，避免扩大无关清理范围。
|
||||
|
||||
### Task 1: 导入 fixed 一体化模型并定义 TCP
|
||||
|
||||
**Files:**
|
||||
|
||||
- Create: `xr_rm_teleop/models/rm75_omnipicker/urdf/RM75-B_OmniPicker_fixed.urdf`
|
||||
- Create: `xr_rm_teleop/models/rm75_omnipicker/meshes/rm75/*.STL`
|
||||
- Create: `xr_rm_teleop/models/rm75_omnipicker/meshes/omnipicker/*.STL`
|
||||
- Modify: `xr_rm_teleop/setup.py`
|
||||
- Modify: `xr_rm_teleop/test/test_placo_transforms.py`
|
||||
|
||||
- [x] **Step 1: 先写模型结构失败测试**
|
||||
|
||||
在 `test_placo_transforms.py` 中用 `xml.etree.ElementTree` 读取 fixed URDF，断言：
|
||||
|
||||
```python
|
||||
assert moving_joint_names == [f"joint_{index}" for index in range(1, 8)]
|
||||
assert tcp_joint.attrib["type"] == "fixed"
|
||||
assert tcp_joint.find("parent").attrib["link"] == "omnipicker_base_link"
|
||||
assert tcp_joint.find("child").attrib["link"] == "omnipicker_tcp"
|
||||
assert tcp_joint.find("origin").attrib["xyz"] == "0 0 0.16"
|
||||
assert tcp_joint.find("origin").attrib["rpy"] == "0 0 0"
|
||||
```
|
||||
|
||||
运行：
|
||||
|
||||
```bash
|
||||
cd /home/robot/WS_xr
|
||||
source /opt/ros/humble/setup.bash
|
||||
PYTHONPATH=src/xr_rm_teleop pytest -q \
|
||||
src/xr_rm_teleop/test/test_placo_transforms.py
|
||||
```
|
||||
|
||||
Expected: FAIL，模型包尚不存在。
|
||||
|
||||
- [x] **Step 2: 从上传 ZIP 只导入运行所需资源**
|
||||
|
||||
从
|
||||
`/home/robot/下载/Models/RM75-B_OmniPicker_Pinocchio.zip`
|
||||
导入 fixed URDF 和两组 mesh 到
|
||||
`xr_rm_teleop/models/rm75_omnipicker`；不导入独立描述包元数据、示例脚本、
|
||||
活动式 URDF 或额外验证文档。保留上传模型的几何、惯量、关节限制和 fixed
|
||||
OmniPicker 关节，并在 `xr_rm_teleop/setup.py` 中安装这些资源。
|
||||
|
||||
- [x] **Step 3: 在 fixed URDF 增加已确认的 TCP**
|
||||
|
||||
```xml
|
||||
<link name="omnipicker_tcp"/>
|
||||
<joint name="omnipicker_tcp_joint" type="fixed">
|
||||
<parent link="omnipicker_base_link"/>
|
||||
<child link="omnipicker_tcp"/>
|
||||
<origin xyz="0 0 0.16" rpy="0 0 0"/>
|
||||
</joint>
|
||||
```
|
||||
|
||||
- [x] **Step 4: 重跑模型测试**
|
||||
|
||||
Expected: PASS；运动关节仍严格为 `joint_1` 至 `joint_7`，TCP 偏移为
|
||||
`+Z 0.16 m`。
|
||||
|
||||
### Task 2: 让 Placo 直接接收 SE(3) 并约束 `omnipicker_tcp`
|
||||
|
||||
**Files:**
|
||||
|
||||
- Modify: `xr_rm_teleop/xr_rm_teleop/placo_ik_solver.py`
|
||||
- Modify: `xr_rm_teleop/test/test_placo_transforms.py`
|
||||
- Modify: `xr_rm_teleop/test/placo_ik_smoke.py`
|
||||
|
||||
- [x] **Step 1: 先把变换和 smoke 测试改为矩阵接口**
|
||||
|
||||
测试改为：
|
||||
|
||||
```python
|
||||
current = solver.update_joint_state(joints)
|
||||
assert current.shape == (4, 4)
|
||||
target = current.copy()
|
||||
target[0, 3] += 0.01
|
||||
target[:3, :3] = rotation_delta @ target[:3, :3]
|
||||
joints = solver.solve(target)
|
||||
```
|
||||
|
||||
同时覆盖非法形状、NaN 和非 SE(3) 最后一行会被拒绝。smoke 使用
|
||||
`dt=1/125`，以旋转矩阵相对角度计算姿态误差，不再转换 RPY。
|
||||
|
||||
运行现有两项测试，确认它们先因旧 `ArmPose/tool_pose` 接口失败。
|
||||
|
||||
- [x] **Step 2: 最小化求解器接口**
|
||||
|
||||
将构造函数改为：
|
||||
|
||||
```python
|
||||
PlacoIkSolver(urdf_path: str, dt: float)
|
||||
```
|
||||
|
||||
并完成以下替换：
|
||||
|
||||
- 删除 `_rpy_to_rotation`、`_rotation_to_rpy`、`_arm_pose_to_transform`、
|
||||
`_transform_to_arm_pose`、`_tool_pose_to_transform`。
|
||||
- 删除 `_tool_transform` 和 `_tool_inverse`。
|
||||
- frame task 从 `link_7` 改为 `omnipicker_tcp`。
|
||||
- 可操作度任务继续作用于原来的 `link_7`，并保留原权重
|
||||
`soft, 5e-2`。
|
||||
- `update_joint_state()` 直接返回
|
||||
`get_T_world_frame("omnipicker_tcp").copy()`。
|
||||
- `solve()` 校验并直接设置传入的 `4×4` 目标矩阵。
|
||||
- frame task、动能正则、虚拟基座固定、关节位置/速度校验保持原状。
|
||||
|
||||
- [x] **Step 3: 运行纯单元测试**
|
||||
|
||||
```bash
|
||||
PYTHONPATH=src/xr_rm_teleop pytest -q \
|
||||
src/xr_rm_teleop/test/test_placo_transforms.py
|
||||
```
|
||||
|
||||
Expected: PASS。该命令不构造 Placo，不要求系统 Python 安装厂商 SDK。
|
||||
|
||||
### Task 3: 用 SO(3) 最短路径替换 RPY 姿态控制
|
||||
|
||||
**Files:**
|
||||
|
||||
- Modify: `xr_rm_teleop/test/test_orientation_control.py`
|
||||
- Modify: `xr_rm_teleop/xr_rm_teleop/single_arm_velocity_teleop.py`
|
||||
|
||||
- [x] **Step 1: 先写 SO(3) 回归测试**
|
||||
|
||||
保留零四元数停止测试，并增加以下最小覆盖：
|
||||
|
||||
- `q` 与 `-q` 得到同一旋转矩阵。
|
||||
- 初始 pitch 接近 `+90°`、`-90°` 时，小手柄旋转只产生同量级的小旋转。
|
||||
- 跨过旧 RPY 分支时，相对旋转仍取最短路径。
|
||||
- 死区按 `norm(Log(R_target R_currentᵀ))` 判断。
|
||||
- `alpha=0.5` 时 SO(3) 误差角减半。
|
||||
- `dt=1/125`、`max_orientation_speed=0.5` 时单步不超过 `0.004 rad`。
|
||||
- 关闭某姿态轴时，在机器人基坐标系将对应旋转向量分量清零。
|
||||
- 矩阵转调试四元数后有限且单位化。
|
||||
|
||||
运行：
|
||||
|
||||
```bash
|
||||
PYTHONPATH=src/xr_rm_teleop pytest -q \
|
||||
src/xr_rm_teleop/test/test_orientation_control.py
|
||||
```
|
||||
|
||||
Expected: FAIL，旧代码仍返回和处理 RPY。
|
||||
|
||||
- [x] **Step 2: 实现最少的 NumPy SO(3) 运算**
|
||||
|
||||
在现有遥操作模块中加入并只加入实际调用的函数：
|
||||
|
||||
```text
|
||||
quaternion -> rotation matrix
|
||||
rotation matrix -> normalized quaternion
|
||||
Log_SO3(rotation) -> 3D rotation vector
|
||||
Exp_SO3(rotation vector) -> rotation matrix
|
||||
position + rotation -> 4×4 transform
|
||||
```
|
||||
|
||||
输入必须有限。近似旋转矩阵仅在
|
||||
`norm(RᵀR-I) <= 1e-3` 且行列式为正时用 SVD 投影；明显无效输入抛出
|
||||
`ValueError`。`Log_SO3` 在接近 `π` 时仍返回最短的有限旋转向量。
|
||||
|
||||
- [x] **Step 3: 替换姿态目标、滤波和限速**
|
||||
|
||||
控制路径统一为：
|
||||
|
||||
```python
|
||||
R_xr_delta = R_xr_now @ R_xr_start.T
|
||||
R_robot_delta = mapping @ R_xr_delta @ mapping.T
|
||||
axis_delta = log_so3(R_robot_delta)
|
||||
axis_delta[disabled_axes] = 0.0
|
||||
R_raw = exp_so3(axis_delta) @ R_robot_start
|
||||
|
||||
error = log_so3(R_target @ R_current.T)
|
||||
R_next = exp_so3(scale * error) @ R_current
|
||||
```
|
||||
|
||||
继续分别保存平移列表和旋转矩阵状态，但构造 QP 目标与调试目标时合成为
|
||||
`4×4` 矩阵。删除控制路径中的 `_matrix_to_euler`、
|
||||
`_quaternion_to_euler`、分量 `_angle_delta` 及 RPY
|
||||
死区/滤波/限速；位置死区、滤波、工作空间和圆柱限位原样保留。
|
||||
|
||||
- [x] **Step 4: 重跑姿态测试**
|
||||
|
||||
Expected: PASS。
|
||||
|
||||
### Task 4: 把节点状态、QP 和调试话题贯通为 SE(3)
|
||||
|
||||
**Files:**
|
||||
|
||||
- Modify: `xr_rm_teleop/xr_rm_teleop/single_arm_velocity_teleop.py`
|
||||
- Modify: `xr_rm_teleop/test/test_joint_control.py`
|
||||
|
||||
- [x] **Step 1: 先将关节控制测试改为 `4×4` 矩阵**
|
||||
|
||||
Fake solver 的 `update_joint_state()` 返回有限齐次矩阵；QP
|
||||
成功、失败和首帧反馈测试均断言矩阵接口。运行测试，确认旧类型假设失败。
|
||||
|
||||
- [x] **Step 2: 完成节点矩阵状态迁移**
|
||||
|
||||
- `_robot_start_pose`、`_last_current_pose` 和调试 fallback 改存 `4×4`
|
||||
矩阵。
|
||||
- `PlacoIkSolver` 初始化不再接收
|
||||
`self._peripheral_config.tool_pose`；外设配置仍只传给
|
||||
`RealManAdapter.configure_peripheral()`。
|
||||
- 原始目标与发送目标均合成为 `omnipicker_tcp` 的 SE(3)。
|
||||
- `TwistStamped.angular` 使用
|
||||
`Log(R_sent R_previousᵀ) / dt`，表达在 `rm_base`。
|
||||
- `PoseStamped` 只在发布边界把旋转矩阵转四元数。
|
||||
- QP 异常继续返回 last-known-good；Grip 松开、超时、反馈错误和发送错误继续
|
||||
走现有慢停与状态重置。
|
||||
|
||||
- [x] **Step 3: 运行相关单元测试**
|
||||
|
||||
```bash
|
||||
PYTHONPATH=src/xr_rm_teleop pytest -q \
|
||||
src/xr_rm_teleop/test/test_orientation_control.py \
|
||||
src/xr_rm_teleop/test/test_joint_control.py \
|
||||
src/xr_rm_teleop/test/test_placo_transforms.py \
|
||||
src/xr_rm_teleop/test/test_initial_joint_pose.py
|
||||
```
|
||||
|
||||
Expected: PASS。
|
||||
|
||||
### Task 5: 切换 launch 模型并同步已确认参数
|
||||
|
||||
**Files:**
|
||||
|
||||
- Modify: `xr_rm_bringup/launch/arm_debug.launch.py`
|
||||
- Modify: `xr_rm_bringup/config/left_arm_rm75.yaml`
|
||||
- Modify: `xr_rm_bringup/config/right_arm_rm75.yaml`
|
||||
- Modify: `xr_rm_bringup/config/dual_arm_rm75.yaml`
|
||||
- Modify: `xr_rm_teleop/setup.py`
|
||||
- Modify: `xr_rm_teleop/xr_rm_teleop/single_arm_velocity_teleop.py`
|
||||
- Modify: `README.md`
|
||||
|
||||
- [x] **Step 1: 修改模型来源**
|
||||
|
||||
`_rm75_urdf()` 改为：
|
||||
|
||||
```python
|
||||
PathJoinSubstitution([
|
||||
FindPackageShare("xr_rm_teleop"),
|
||||
"models",
|
||||
"rm75_omnipicker",
|
||||
"urdf",
|
||||
"RM75-B_OmniPicker_fixed.urdf",
|
||||
])
|
||||
```
|
||||
|
||||
并让 `xr_rm_teleop/setup.py` 安装该目录下的 fixed URDF 和两组 mesh。
|
||||
|
||||
- [x] **Step 2: 只修改已确认参数**
|
||||
|
||||
节点默认值、launch 默认值和三份 YAML 对应项同步：
|
||||
|
||||
```yaml
|
||||
control_rate_hz: 125.0
|
||||
orientation_deadband_rad: 0.005
|
||||
orientation_filter_alpha: 0.65
|
||||
max_orientation_speed: 0.5
|
||||
follow: false
|
||||
```
|
||||
|
||||
其中右臂 YAML 的
|
||||
`move_to_initial_pose_on_connect: True`
|
||||
改为 `false`。不修改任何工作空间、圆柱、线速度、关节速度、初始角、
|
||||
`avoid_singularity`、安全配置或外设配置。
|
||||
|
||||
- [x] **Step 3: 更新 README 中已失真的运行说明**
|
||||
|
||||
只更新：
|
||||
|
||||
- 默认控制频率 `90.0 -> 125.0`。
|
||||
- QP 模型改为一体化 fixed URDF，并直接控制 `omnipicker_tcp`。
|
||||
- 姿态死区、滤波和限速使用 SO(3) 最短路径，不使用 RPY。
|
||||
- `peripherals_rm75.yaml` 仍只用于真实控制器工具坐标、负载和外设选择，不再
|
||||
参与 Placo TCP 矩阵换算。
|
||||
|
||||
### Task 6: 构建、数值 smoke 与 mock 启动验证
|
||||
|
||||
**Files:**
|
||||
|
||||
- Modify: `xr_rm_teleop/test/placo_ik_smoke.py`
|
||||
|
||||
- [x] **Step 1: 构建整个工作空间**
|
||||
|
||||
```bash
|
||||
cd /home/robot/WS_xr
|
||||
source /opt/ros/humble/setup.bash
|
||||
colcon build --symlink-install
|
||||
```
|
||||
|
||||
Expected: `xr_rm_teleop` 和 `xr_rm_bringup` 构建成功。
|
||||
|
||||
- [x] **Step 2: 运行指定姿态测试和相关回归测试**
|
||||
|
||||
```bash
|
||||
source /opt/ros/humble/setup.bash
|
||||
PYTHONPATH=src/xr_rm_teleop pytest -q \
|
||||
src/xr_rm_teleop/test/test_orientation_control.py
|
||||
|
||||
PYTHONPATH=src/xr_rm_teleop pytest -q \
|
||||
src/xr_rm_teleop/test/test_joint_control.py \
|
||||
src/xr_rm_teleop/test/test_placo_transforms.py \
|
||||
src/xr_rm_teleop/test/test_initial_joint_pose.py
|
||||
```
|
||||
|
||||
Expected: PASS。
|
||||
|
||||
- [x] **Step 3: 使用固定 XR Python 运行 Placo 数值 smoke**
|
||||
|
||||
```bash
|
||||
source install/setup.bash
|
||||
/home/robot/miniconda3/envs/xr/bin/python \
|
||||
src/xr_rm_teleop/test/placo_ik_smoke.py \
|
||||
install/xr_rm_teleop/share/xr_rm_teleop/models/rm75_omnipicker/urdf/RM75-B_OmniPicker_fixed.urdf
|
||||
```
|
||||
|
||||
对左右初始关节姿态分别验证：
|
||||
|
||||
- 七个运动关节及顺序正确。
|
||||
- `omnipicker_tcp` 相对 `link_7` 为 `[0, 0, 0.16]`、单位旋转。
|
||||
- QP 输出七个有限关节角并满足位置与单周期速度限制。
|
||||
- 目标停止两秒时打印最大关节变化，但不把漂移设为失败条件。
|
||||
- 运动目标最终 TCP 位置误差 `<= 5 mm`，姿态误差 `<= 2°`。
|
||||
- 打印平均/最大求解耗时及超过 `8 ms` 周期预算的次数，只记录、不设机器相关
|
||||
的硬失败阈值。
|
||||
|
||||
- [x] **Step 4: 只启动 mock**
|
||||
|
||||
分别短时启动：
|
||||
|
||||
```bash
|
||||
source /opt/ros/humble/setup.bash
|
||||
source install/setup.bash
|
||||
ros2 launch xr_rm_bringup arm_debug.launch.py arm:=left use_mock:=true
|
||||
ros2 launch xr_rm_bringup arm_debug.launch.py arm:=right use_mock:=true
|
||||
ros2 launch xr_rm_bringup arm_debug.launch.py arm:=both use_mock:=true
|
||||
```
|
||||
|
||||
确认 fixed URDF、Placo 和左右节点名加载成功，无 RealMan SDK 导入或网络连接。
|
||||
由人工结束 mock launch；Codex 不执行任何 `use_mock:=false` 命令。
|
||||
|
||||
- [x] **Step 5: 最终范围检查**
|
||||
|
||||
```bash
|
||||
git diff --check
|
||||
git status --short
|
||||
git diff -- \
|
||||
src/xr_rm_teleop \
|
||||
src/xr_rm_bringup \
|
||||
src/README.md \
|
||||
src/docs/superpowers
|
||||
```
|
||||
|
||||
确认 `peripherals_rm75.yaml`、`avoid_singularity`、可操作度权重和所有既有安全
|
||||
限制未被改变。
|
||||
@@ -1,508 +0,0 @@
|
||||
# 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.
|
||||
@@ -1,42 +0,0 @@
|
||||
# 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