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`、可操作度权重和所有既有安全
|
||||
限制未被改变。
|
||||
@@ -0,0 +1,30 @@
|
||||
# RM75 控制周期统计设计
|
||||
|
||||
## 目标
|
||||
|
||||
在不改变控制、QP、安全停止和真机通信行为的前提下，确认激活遥操作时的
|
||||
125 Hz 控制链路是否满足 `8 ms` 周期。
|
||||
|
||||
## 方案
|
||||
|
||||
在 `SingleArmVelocityTeleop` 内使用 `time.perf_counter_ns()` 采样，仅在
|
||||
Grip 激活并执行关节命令的周期记录：
|
||||
|
||||
- 相邻控制回调的实际周期；
|
||||
- 控制回调总执行时间；
|
||||
- Placo QP 求解时间；
|
||||
- `send_joint_target()` 调用时间；
|
||||
- 当前关节反馈年龄。
|
||||
|
||||
每累计约 5 秒激活样本，通过现有 ROS logger 输出一次汇总并清空窗口。每项
|
||||
输出样本数、mean、P95、P99 和 max；实际周期与总执行时间额外输出超过
|
||||
`self._dt` 的次数。首个激活周期没有可靠的相邻周期值，因此不记录周期。
|
||||
|
||||
统计只输出日志，不新增 ROS 消息、话题、参数、依赖或后台线程。计时与日志
|
||||
异常不得影响控制路径。
|
||||
|
||||
## 验证
|
||||
|
||||
先添加一个小单元测试，使用确定性样本验证百分位数、超限计数、日志输出和
|
||||
窗口清空。随后运行相关 pytest、姿态控制测试和
|
||||
`colcon build --symlink-install`。
|
||||
@@ -0,0 +1,241 @@
|
||||
# RM75 SO(3) 姿态跟随与 OmniPicker 一体化模型设计
|
||||
|
||||
日期：2026-07-28
|
||||
状态：已确认
|
||||
|
||||
## 1. 背景与目标
|
||||
|
||||
当前遥操作链路已经使用 Placo QP 将 TCP 目标转换为 RM75 七关节目标，但姿态目标在进入 QP 前会转换为 RPY，并按三个欧拉角分量进行滤波和限速。RM75 当前姿态接近俯仰角 `±90°` 时，同一个物理姿态可能切换到另一组等价 RPY，导致控制器把很小的手柄旋转解释为大角度路径，出现机械臂绕行和跟随时间过长。
|
||||
|
||||
本设计是
|
||||
`2026-07-27-rm75-placo-qp-ik-design.md`
|
||||
的增量修改；它覆盖旧设计中的模型、TCP 变换、姿态表示、控制频率和相关验收
|
||||
内容。现有的关节反馈、单步 QP、`rm_movej_canfd`、last-known-good 和停止链路
|
||||
继续保留。
|
||||
|
||||
本次修改的目标是：
|
||||
|
||||
1. 姿态控制全程使用旋转矩阵和 SE(3)，按 SO(3) 最短旋转路径滤波和限速。
|
||||
2. 左右臂统一使用用户提供的 `RM75-B_OmniPicker_fixed.urdf`。
|
||||
3. 在 URDF 内定义 `omnipicker_tcp`，取消 Placo 中重复的运行时末端变换。
|
||||
4. 首轮真机测试继续使用低跟随，将控制频率设为 `125 Hz`，TCP 目标角速度上限统一为 `0.5 rad/s`。
|
||||
5. 保留现有工作空间、圆柱、关节速度、指令超时和安全停止行为。
|
||||
|
||||
## 2. 本次不处理的内容
|
||||
|
||||
- 不开启高跟随，`follow` 继续为 `false`。
|
||||
- 不改变现有 Wi-Fi/有线混合网络拓扑。
|
||||
- 不新增高跟随周期看门狗。
|
||||
- 不修改 `avoid_singularity`，不新增奇异点检测或降速策略。
|
||||
- 首轮不删除或调整现有可操作度优化任务。
|
||||
- 不修改 `peripherals_rm75.yaml` 中的夹爪选择、工具位姿或负载。
|
||||
- 不新增 ROS2 节点、消息、求解器工厂或第三方依赖。
|
||||
- Codex 不连接或移动真实机械臂和夹爪。
|
||||
|
||||
## 3. 姿态数据模型
|
||||
|
||||
控制路径中的机器人位姿统一为有限的 `4×4` NumPy SE(3) 矩阵：
|
||||
|
||||
```text
|
||||
T = [ R p ]
|
||||
[ 0 1 ]
|
||||
```
|
||||
|
||||
其中 `R` 为 `3×3` 旋转矩阵，`p` 为 TCP 在机器人基坐标系下的位置。Placo 正解直接返回该矩阵，QP 目标也直接接收该矩阵。现有仅保存 `x/y/z/rx/ry/rz` 的 `ArmPose` 不再作为控制路径接口，避免在遥操作节点与 Placo 之间发生 RPY 往返转换。
|
||||
|
||||
ROS 调试边界按消息类型转换：
|
||||
|
||||
- `PoseStamped`：旋转矩阵转换为四元数后发布。
|
||||
- `TwistStamped.angular`：发布相邻两个目标旋转之间、在机器人基坐标系表达的 SO(3) 旋转向量速度。
|
||||
|
||||
## 4. XR 姿态到机器人姿态
|
||||
|
||||
Grip 按下时保存 XR 初始四元数和当前 `omnipicker_tcp` 初始旋转矩阵。每周期计算：
|
||||
|
||||
```text
|
||||
R_xr_delta = R_xr_now * transpose(R_xr_start)
|
||||
R_robot_delta = M * R_xr_delta * transpose(M)
|
||||
R_raw_target = R_robot_delta * R_robot_start
|
||||
```
|
||||
|
||||
`M` 为现有 `xr_to_robot_matrix`，其左右臂映射保持不变。`enable_orientation_axes` 继续保留；被关闭的机器人基坐标轴通过将对应 SO(3) 相对旋转向量分量置零实现，不再通过拼接 RPY 分量实现。
|
||||
|
||||
姿态处理统一使用基坐标系下的左乘增量：
|
||||
|
||||
```text
|
||||
R_error = R_target * transpose(R_current)
|
||||
r = Log(R_error)
|
||||
R_next = Exp(scale * r) * R_current
|
||||
```
|
||||
|
||||
处理顺序为：
|
||||
|
||||
1. 以 `norm(Log(R_target * R_lastᵀ))` 判断 `orientation_deadband_rad`。
|
||||
2. 以 `orientation_filter_alpha` 缩放从滤波状态到目标的最短旋转向量。
|
||||
3. 将从上一发送姿态到滤波姿态的旋转角限制在
|
||||
`max_orientation_speed / control_rate_hz` 以内。
|
||||
4. 将限速后的旋转和已通过现有工作空间限制的位置合成为 SE(3)。
|
||||
|
||||
该路径不使用 RPY 展开、分量插值或分量限速。相对旋转接近 `π` 时也必须选择物理最短路径；四元数 `q` 与 `-q` 必须得到相同目标。
|
||||
|
||||
位姿误差采用与 Placo `FrameTask` 一致的解耦 `3+3` 形式：
|
||||
|
||||
```text
|
||||
e_position = p_target - p_current
|
||||
e_orientation = Log(R_target * transpose(R_current))
|
||||
e_pose = [e_position; e_orientation]
|
||||
```
|
||||
|
||||
其中 `e_pose` 可视为六维任务误差，但不是
|
||||
`Log_SE3(T_target * inverse(T_current))`。本次不引入完整 SE(3) 对数映射中的
|
||||
平移—旋转耦合。
|
||||
|
||||
## 5. URDF 与 TCP
|
||||
|
||||
用户上传的
|
||||
`/home/robot/下载/Models/RM75-B_OmniPicker_Pinocchio.zip`
|
||||
作为模型来源。将 fixed URDF 和被引用的 RM75、OmniPicker mesh 放入
|
||||
`xr_rm_teleop/models/rm75_omnipicker`，由现有 `xr_rm_teleop` 包安装。
|
||||
|
||||
在 `RM75-B_OmniPicker_fixed.urdf` 中增加：
|
||||
|
||||
```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>
|
||||
```
|
||||
|
||||
该变换表示 TCP 相对 RM75 法兰坐标系沿 `+Z` 方向平移 `0.16 m`、坐标轴方向不变。上传模型中 `rm75_flange` 到 `omnipicker_base_link` 为零固定变换，因此上述定义与已确认的法兰到 TCP 变换一致。
|
||||
|
||||
`arm_debug.launch.py` 的左臂、右臂和双臂节点统一加载这一份 fixed URDF。模型仍只有 `joint_1` 至 `joint_7` 七个运动关节，OmniPicker 关节均保持 fixed。
|
||||
|
||||
## 6. Placo QP
|
||||
|
||||
`PlacoIkSolver` 改为：
|
||||
|
||||
```text
|
||||
构造参数：URDF 路径、dt
|
||||
当前位姿：get_T_world_frame("omnipicker_tcp")
|
||||
目标任务：add_frame_task("omnipicker_tcp", target_se3)
|
||||
```
|
||||
|
||||
删除仅供 Placo 使用的 `tool_pose` 构造参数、`_tool_transform`、
|
||||
`_tool_inverse` 和对应的法兰/TCP换算函数。`peripherals_rm75.yaml` 保持原状，
|
||||
仍供 `RealManAdapter` 配置真实控制器的工具坐标、负载和末端外设；其中的
|
||||
`pose` 不再传入 Placo，因此不会在 QP 中重复叠加末端偏移。
|
||||
|
||||
QP 配置首轮保持：
|
||||
|
||||
```text
|
||||
frame task: soft, 1.0
|
||||
manipulability task: soft, 5e-2
|
||||
kinetic energy regularizer: 1e-6
|
||||
```
|
||||
|
||||
可操作度任务已知会造成目标静止时的关节姿态变化，但按用户决定本轮保留，
|
||||
待确认 RPY 绕行消失后再单独评估。关节位置和单周期速度校验继续使用 URDF
|
||||
限制，固定虚拟基座和 `q[7:14]` 的七关节映射保持不变。
|
||||
|
||||
## 7. 参数与启动行为
|
||||
|
||||
以下配置在 `left_arm_rm75.yaml`、`right_arm_rm75.yaml` 和
|
||||
`dual_arm_rm75.yaml` 对应节点中同步：
|
||||
|
||||
```yaml
|
||||
control_rate_hz: 125.0
|
||||
orientation_deadband_rad: 0.005
|
||||
orientation_filter_alpha: 0.65
|
||||
max_orientation_speed: 0.5
|
||||
follow: false
|
||||
```
|
||||
|
||||
`arm_debug.launch.py` 的 `control_rate_hz` 默认值同步为 `125.0`，`follow`
|
||||
默认值继续为 `false`。`single_arm_velocity_teleop` 内部参数默认值同步，
|
||||
避免绕过 YAML 启动时回到旧频率或旧角速度。
|
||||
|
||||
`right_arm_rm75.yaml` 的 `move_to_initial_pose_on_connect` 从 `True` 改为
|
||||
`false`，与左臂、双臂以及 launch 默认安全行为一致。其他左右臂空间范围、
|
||||
线速度、关节速度、加速度、初始关节角和外设配置不变。
|
||||
|
||||
## 8. 异常与停止
|
||||
|
||||
现有异常策略保持：
|
||||
|
||||
- 非法 XR 四元数、Grip 松开、XR 超时、关节反馈过期或通信失败时执行现有慢停止并重置激活状态。
|
||||
- QP 失败或输出违反关节位置/速度限制时继续使用上一组有效关节目标。
|
||||
- 第一次有效关节反馈前不发送运动命令。
|
||||
- `configure_safety_limits` 保持启用。
|
||||
- Mock 模式不导入睿尔曼 SDK。
|
||||
|
||||
新增 SO(3) 运算必须拒绝非有限矩阵和零四元数。旋转矩阵若满足
|
||||
`norm(RᵀR-I) <= 1e-3` 且行列式为正，则使用 `3×3` SVD 投影到最近的合法旋转；
|
||||
超出该范围时停止输出，不能把明显无效的输入静默修正成运动目标。
|
||||
|
||||
## 9. 验证
|
||||
|
||||
### 9.1 自动测试
|
||||
|
||||
扩展 `test_orientation_control.py`，至少覆盖：
|
||||
|
||||
- 初始姿态俯仰接近 `+90°` 和 `-90°` 时，小手柄旋转仍产生相同量级的最短物理旋转。
|
||||
- 目标跨越原 RPY 表示分支时，不产生接近 `π` 的错误路径。
|
||||
- `q` 与 `-q` 产生相同旋转矩阵。
|
||||
- SO(3) 死区使用整体旋转角。
|
||||
- 每周期姿态步长不超过 `0.5 / 125 rad`。
|
||||
- SO(3) 滤波沿最短路径收敛。
|
||||
- 调试四元数有限且归一化。
|
||||
|
||||
更新 Placo 变换测试和 smoke test，验证：
|
||||
|
||||
- fixed URDF 可加载，运动关节仍严格为七个且顺序正确。
|
||||
- `omnipicker_tcp` 相对法兰的变换为 `[0, 0, 0.16]` 和单位旋转。
|
||||
- 当前/原始目标/发送目标均表示 `omnipicker_tcp`。
|
||||
- 一次 QP 输出七个有限关节角并满足位置与单周期速度限制。
|
||||
- 目标静止两秒时记录左右臂最大关节变化，但本轮不以 `≤0.5°` 作为通过条件。
|
||||
- 目标停止后 TCP 位置误差不超过 `5 mm`、姿态误差不超过 `2°`。
|
||||
|
||||
### 9.2 命令
|
||||
|
||||
所有命令从 `/home/robot/WS_xr` 执行：
|
||||
|
||||
```bash
|
||||
source /opt/ros/humble/setup.bash
|
||||
PYTHONPATH=src/xr_rm_teleop pytest -q \
|
||||
src/xr_rm_teleop/test/test_orientation_control.py
|
||||
|
||||
source /opt/ros/humble/setup.bash
|
||||
colcon build --symlink-install
|
||||
|
||||
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
|
||||
```
|
||||
|
||||
Placo smoke test继续使用已验证的
|
||||
`/home/robot/miniconda3/envs/xr/bin/python`，周期改为 `1/125 s`。
|
||||
|
||||
### 9.3 真机人工验收
|
||||
|
||||
Codex 只提供步骤，不执行真机操作。用户应分别测试左右臂：
|
||||
|
||||
1. 确认急停可用、周围无障碍物且 `move_to_initial_pose_on_connect=false`。
|
||||
2. 先保持手柄和目标姿态不变，记录 TCP 与关节变化。
|
||||
3. 仅改变手柄姿态，重点跨越原俯仰 `±90°` 附近的 RPY 分支。
|
||||
4. 确认 TCP 以最短物理旋转跟随，没有绕一大圈。
|
||||
5. 确认 `omnipicker_tcp` 位置保持在允许误差内。
|
||||
|
||||
## 10. 完成标准
|
||||
|
||||
1. 控制路径中的目标生成、滤波、限速和 QP 接口不再使用 RPY。
|
||||
2. 左右臂都由同一 fixed URDF 的 `omnipicker_tcp` 作为控制帧。
|
||||
3. Placo 不再使用 `peripherals_rm75.yaml` 的工具位姿进行矩阵换算。
|
||||
4. 可操作度任务保持现状，漂移数据被记录但不作为首轮阻断项。
|
||||
5. 三份配置使用 `125 Hz`、`0.5 rad/s` 和低跟随。
|
||||
6. 右臂连接时不再自动移动到初始关节姿态。
|
||||
7. 指定测试、构建和左右臂 mock 启动验证通过。
|
||||
8. 未改变工作空间、圆柱、关节限制、指令超时、安全停止或奇异点设置。
|
||||
Reference in New Issue
Block a user