Add URDF model for RM75-B OmniPicker with detailed link and joint specifications
This commit is contained in:
@@ -0,0 +1,154 @@
|
||||
# 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。
|
||||
@@ -0,0 +1,390 @@
|
||||
# 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`、可操作度权重和所有既有安全
|
||||
限制未被改变。
|
||||
Reference in New Issue
Block a user