diff --git a/README.md b/README.md index b8e7924..a2e0584 100755 --- a/README.md +++ b/README.md @@ -12,7 +12,7 @@ PICO/XR 双手柄 UDP JSON -> /xr_rm//current_pose、raw_target_pose、target_pose、cmd_vel、target_clamped 调试话题 ``` -当前控制方式是“手柄相对位姿 + 单步 QP”遥操作：按住 `grip` 时锁定当前手柄位姿和机械臂 TCP 位姿，之后根据手柄相对位移和相对旋转生成目标 TCP，经过工作空间限幅、目标低通、姿态低通和单帧步长限制后，每个控制周期执行一次 Placo QP，并通过 `rm_movej_canfd` 下发 7 个关节目标。松开 `grip`、UDP 或关节反馈超时、节点退出时会请求机械臂慢停。 +当前控制方式是“手柄相对位姿 + 单步 QP”遥操作：按住 `grip` 时锁定当前手柄位姿和机械臂 TCP 位姿，之后根据手柄相对位移和相对旋转生成目标 TCP。姿态目标使用旋转矩阵和 SO(3) 最短路径完成死区、滤波与限速，不经过 RPY。每个控制周期执行一次 Placo QP，并通过 `rm_movej_canfd` 下发 7 个关节目标。松开 `grip`、UDP 或关节反馈超时、节点退出时会请求机械臂慢停。 ## 当前范围 @@ -69,7 +69,9 @@ src/ │ └── msg/ │ └── XrController.msg # hand/grip/trigger/pose └── xr_rm_teleop/ - ├── models/rm75/ # RM75 URDF 与网格 + ├── models/ + │ ├── rm75/ # 旧 RM75 模型资源（launch 不再选用） + │ └── rm75_omnipicker/ # RM75 + OmniPicker fixed URDF 与网格 └── xr_rm_teleop/ ├── placo_ik_solver.py # Placo 0.9.4 单步 QP 逆解 ├── single_arm_velocity_teleop.py @@ -239,7 +241,7 @@ ros2 launch xr_rm_bringup arm_debug.launch.py arm:=both use_mock:=false \ - `robot_port`：RM75 TCP 端口，默认 `8080`。 - `left_avoid_singularity` / `right_avoid_singularity`：左右臂避奇异参数，默认左 `0`、右 `1`。 - `avoid_singularity`：非空时覆盖左右臂避奇异参数。 -- `control_rate_hz`：同步关节反馈、执行一次 QP 并发送一次关节目标的频率，默认 `90.0`。 +- `control_rate_hz`：同步关节反馈、执行一次 QP 并发送一次关节目标的频率，默认 `125.0`。 - `follow`：传给 `rm_movej_canfd` 的跟随标志，默认 `false`。 - `configure_safety_limits`：连接真机后是否配置速度/加速度安全参数，默认 `true`。 - `enable_tool_control`：是否在遥操作节点内启用末端工具控制 topic，默认 `true`。 @@ -257,7 +259,7 @@ ros2 launch xr_rm_bringup arm_debug.launch.py arm:=both use_mock:=false \ `left_arm_rm75.yaml` 和 `right_arm_rm75.yaml` 用于 `arm_debug.launch.py arm:=left/right` 的单臂调试，因为单臂节点名是 `single_arm_velocity_teleop`。 -`xr_rm_bringup/config/peripherals_rm75.yaml` 保存末端工具坐标、负载和左右臂外设选择。当前左臂使用 `minisci`（沿工具局部 Z 偏移 `0.19 m`），右臂使用 `omnipic`（沿工具局部 Z 偏移 `0.16 m`）。URDF 仍只建模到法兰 `link_7`；QP 在 TCP 目标与法兰目标之间应用完整工具刚体变换。真机连接阶段会初始化外设，关节反馈、关节指令、慢停和开合命令复用该单臂节点的同一个 RealMan 连接。 +`xr_rm_bringup/config/peripherals_rm75.yaml` 保存真实控制器使用的末端工具坐标、负载和左右臂外设选择，文件内容保持原状。Placo 使用 `xr_rm_teleop/models/rm75_omnipicker` 中的一体化 fixed URDF，直接控制相对 `omnipicker_base_link` 沿 `+Z` 偏移 `0.16 m` 的 `omnipicker_tcp`，不再把外设 YAML 的工具位姿重复转换到 QP。真机连接阶段仍会初始化外设，关节反馈、关节指令、慢停和开合命令复用该单臂节点的同一个 RealMan 连接。 重点控制参数： @@ -267,8 +269,8 @@ ros2 launch xr_rm_bringup arm_debug.launch.py arm:=both use_mock:=false \ - `target_filter_fast_threshold_m`：进入快速滤波区间的目标变化阈值。 - `max_linear_speed`：目标位姿单帧步长限制对应的最大线速度。 - `enable_orientation_control`：是否把手柄相对旋转映射到 TCP 姿态。 -- `orientation_filter_alpha` / `orientation_deadband_rad`：目标 TCP 姿态低通和死区。 -- `max_orientation_speed`：目标姿态单帧步长限制对应的最大角速度。 +- `orientation_filter_alpha` / `orientation_deadband_rad`：按 SO(3) 最短旋转角处理的目标 TCP 姿态滤波和死区。 +- `max_orientation_speed`：目标 TCP 姿态沿 SO(3) 最短路径的最大角速度，当前为 `0.5 rad/s`。 - `workspace_min` / `workspace_max`：笛卡尔工作空间边界。 - `cyl_radius_limit`：基座圆柱半径限制。 - `xr_to_robot_matrix`：`/xr/*_controller` Project 位移到 RM75 base 坐标的映射矩阵。 diff --git a/docs/superpowers/plans/2026-07-28-rm75-control-timing-stats.md b/docs/superpowers/plans/2026-07-28-rm75-control-timing-stats.md new file mode 100644 index 0000000..8551124 --- /dev/null +++ b/docs/superpowers/plans/2026-07-28-rm75-control-timing-stats.md @@ -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。 diff --git a/docs/superpowers/plans/2026-07-28-rm75-so3-omnipicker-teleop.md b/docs/superpowers/plans/2026-07-28-rm75-so3-omnipicker-teleop.md new file mode 100644 index 0000000..f27af99 --- /dev/null +++ b/docs/superpowers/plans/2026-07-28-rm75-so3-omnipicker-teleop.md @@ -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 + + + + + + +``` + +- [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`、可操作度权重和所有既有安全 +限制未被改变。 diff --git a/docs/superpowers/specs/2026-07-28-rm75-control-timing-stats-design.md b/docs/superpowers/specs/2026-07-28-rm75-control-timing-stats-design.md new file mode 100644 index 0000000..894a1a6 --- /dev/null +++ b/docs/superpowers/specs/2026-07-28-rm75-control-timing-stats-design.md @@ -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`。 diff --git a/docs/superpowers/specs/2026-07-28-rm75-so3-omnipicker-teleop-design.md b/docs/superpowers/specs/2026-07-28-rm75-so3-omnipicker-teleop-design.md new file mode 100644 index 0000000..e860b80 --- /dev/null +++ b/docs/superpowers/specs/2026-07-28-rm75-so3-omnipicker-teleop-design.md @@ -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 + + + + + + + +``` + +该变换表示 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. 未改变工作空间、圆柱、关节限制、指令超时、安全停止或奇异点设置。 diff --git a/xr_rm_bringup/config/dual_arm_rm75.yaml b/xr_rm_bringup/config/dual_arm_rm75.yaml index 59c9379..00ab832 100755 --- a/xr_rm_bringup/config/dual_arm_rm75.yaml +++ b/xr_rm_bringup/config/dual_arm_rm75.yaml @@ -11,7 +11,7 @@ left_arm_teleop: ros__parameters: arm_name: left_rm75 controller_topic: /xr/left_controller - control_rate_hz: 90.0 + control_rate_hz: 125.0 command_timeout_sec: 0.12 # 位姿目标生成与平滑参数。 @@ -26,7 +26,7 @@ left_arm_teleop: enable_orientation_axes: [true, true, true] orientation_deadband_rad: 0.005 orientation_filter_alpha: 0.65 - max_orientation_speed: 0.6 + max_orientation_speed: 0.5 workspace_min: [-0.70, -0.60, 0.10] workspace_max: [0.70, 0.40, 0.70] cyl_radius_limit: [0.20, 0.60] @@ -62,7 +62,7 @@ right_arm_teleop: ros__parameters: arm_name: right_rm75 controller_topic: /xr/right_controller - control_rate_hz: 90.0 + control_rate_hz: 125.0 command_timeout_sec: 0.12 scale: 0.75 @@ -76,7 +76,7 @@ right_arm_teleop: enable_orientation_axes: [true, true, true] orientation_deadband_rad: 0.005 orientation_filter_alpha: 0.65 - max_orientation_speed: 0.6 + max_orientation_speed: 0.5 workspace_min: [-0.70, -0.60, 0.10] workspace_max: [0.70, 0.40, 0.70] cyl_radius_limit: [0.20, 0.60] diff --git a/xr_rm_bringup/config/left_arm_rm75.yaml b/xr_rm_bringup/config/left_arm_rm75.yaml index 5705fcf..1f01cd1 100644 --- a/xr_rm_bringup/config/left_arm_rm75.yaml +++ b/xr_rm_bringup/config/left_arm_rm75.yaml @@ -5,7 +5,7 @@ single_arm_velocity_teleop: ros__parameters: arm_name: left_rm75 controller_topic: /xr/left_controller - control_rate_hz: 90.0 + control_rate_hz: 125.0 command_timeout_sec: 0.12 # 手柄相对位姿 -> 目标 TCP 位姿；随后做目标低通、姿态低通和单帧步长限制。 @@ -20,7 +20,7 @@ single_arm_velocity_teleop: enable_orientation_axes: [true, true, true] orientation_deadband_rad: 0.005 orientation_filter_alpha: 0.65 - max_orientation_speed: 0.6 + max_orientation_speed: 0.5 workspace_min: [-0.70, -0.60, 0.10] workspace_max: [0.70, 0.40, 0.70] cyl_radius_limit: [0.20, 0.60] diff --git a/xr_rm_bringup/config/right_arm_rm75.yaml b/xr_rm_bringup/config/right_arm_rm75.yaml index 0acefa5..1ec3fc4 100644 --- a/xr_rm_bringup/config/right_arm_rm75.yaml +++ b/xr_rm_bringup/config/right_arm_rm75.yaml @@ -5,7 +5,7 @@ single_arm_velocity_teleop: ros__parameters: arm_name: right_rm75 controller_topic: /xr/right_controller - control_rate_hz: 90.0 + control_rate_hz: 125.0 command_timeout_sec: 0.12 scale: 0.7 @@ -20,9 +20,9 @@ single_arm_velocity_teleop: orientation_deadband_rad: 0.005 orientation_filter_alpha: 0.65 max_orientation_speed: 0.5 - workspace_min: [-0.60, -0.60, 0.10] - workspace_max: [0.60, 0.70, 0.55] - cyl_radius_limit: [0.10, 0.70] + workspace_min: [-0.70, -0.70, 0.10] + workspace_max: [0.70, 0.70, 0.75] + cyl_radius_limit: [0.10, 0.80] low_z_threshold: 0.1 low_z_min_radius: 0.1 @@ -35,7 +35,7 @@ single_arm_velocity_teleop: robot_ip: 192.168.192.19 robot_port: 8080 avoid_singularity: 1 - follow: false + follow: False canfd_trajectory_mode: 2 canfd_radio: 0 configure_safety_limits: true @@ -45,7 +45,7 @@ single_arm_velocity_teleop: max_angular_acc: 3.0 joint_max_speed: 180.0 joint_max_acc: 300.0 - move_to_initial_pose_on_connect: True + move_to_initial_pose_on_connect: false initial_joint_pose: [-90.14, 3.76, -86.89, 87.89, -96.53, -79.62, -90.04] init_move_speed: 20 debug_topic_prefix: /xr_rm diff --git a/xr_rm_bringup/launch/arm_debug.launch.py b/xr_rm_bringup/launch/arm_debug.launch.py index 1b40d0c..da5356c 100644 --- a/xr_rm_bringup/launch/arm_debug.launch.py +++ b/xr_rm_bringup/launch/arm_debug.launch.py @@ -35,8 +35,9 @@ def _rm75_urdf() -> PathJoinSubstitution: return PathJoinSubstitution([ FindPackageShare("xr_rm_teleop"), "models", - "rm75", - "RM75-B.urdf", + "rm75_omnipicker", + "urdf", + "RM75-B_OmniPicker_fixed.urdf", ]) @@ -286,7 +287,7 @@ def generate_launch_description() -> LaunchDescription: # 非空时作为左右臂全局覆盖，例如 avoid_singularity:=0。 DeclareLaunchArgument("avoid_singularity", default_value=""), # 每周期同步一次实际关节反馈、执行一次 Placo QP，再发送 rm_movej_canfd。 - DeclareLaunchArgument("control_rate_hz", default_value="90.0"), + DeclareLaunchArgument("control_rate_hz", default_value="125.0"), # 默认低跟随；高跟随请确认控制器和网络能稳定满足厂商周期要求后再打开。 DeclareLaunchArgument("follow", default_value="false"), DeclareLaunchArgument("configure_safety_limits", default_value="true"), diff --git a/xr_rm_teleop/models/rm75_omnipicker/meshes/omnipicker/base_link.STL b/xr_rm_teleop/models/rm75_omnipicker/meshes/omnipicker/base_link.STL new file mode 100644 index 0000000..fa96712 Binary files /dev/null and b/xr_rm_teleop/models/rm75_omnipicker/meshes/omnipicker/base_link.STL differ diff --git a/xr_rm_teleop/models/rm75_omnipicker/meshes/omnipicker/narrow1_Link.STL b/xr_rm_teleop/models/rm75_omnipicker/meshes/omnipicker/narrow1_Link.STL new file mode 100644 index 0000000..a0bc14c Binary files /dev/null and b/xr_rm_teleop/models/rm75_omnipicker/meshes/omnipicker/narrow1_Link.STL differ diff --git a/xr_rm_teleop/models/rm75_omnipicker/meshes/omnipicker/narrow2_Link.STL b/xr_rm_teleop/models/rm75_omnipicker/meshes/omnipicker/narrow2_Link.STL new file mode 100644 index 0000000..0c305ec Binary files /dev/null and b/xr_rm_teleop/models/rm75_omnipicker/meshes/omnipicker/narrow2_Link.STL differ diff --git a/xr_rm_teleop/models/rm75_omnipicker/meshes/omnipicker/narrow3_Link.STL b/xr_rm_teleop/models/rm75_omnipicker/meshes/omnipicker/narrow3_Link.STL new file mode 100644 index 0000000..51019ab Binary files /dev/null and b/xr_rm_teleop/models/rm75_omnipicker/meshes/omnipicker/narrow3_Link.STL differ diff --git a/xr_rm_teleop/models/rm75_omnipicker/meshes/omnipicker/narrow_loop_Link.STL b/xr_rm_teleop/models/rm75_omnipicker/meshes/omnipicker/narrow_loop_Link.STL new file mode 100644 index 0000000..0637782 Binary files /dev/null and b/xr_rm_teleop/models/rm75_omnipicker/meshes/omnipicker/narrow_loop_Link.STL differ diff --git a/xr_rm_teleop/models/rm75_omnipicker/meshes/omnipicker/wide1_Link.STL b/xr_rm_teleop/models/rm75_omnipicker/meshes/omnipicker/wide1_Link.STL new file mode 100644 index 0000000..b2afe36 Binary files /dev/null and b/xr_rm_teleop/models/rm75_omnipicker/meshes/omnipicker/wide1_Link.STL differ diff --git a/xr_rm_teleop/models/rm75_omnipicker/meshes/omnipicker/wide2_Link.STL b/xr_rm_teleop/models/rm75_omnipicker/meshes/omnipicker/wide2_Link.STL new file mode 100644 index 0000000..3f58cc5 Binary files /dev/null and b/xr_rm_teleop/models/rm75_omnipicker/meshes/omnipicker/wide2_Link.STL differ diff --git a/xr_rm_teleop/models/rm75_omnipicker/meshes/omnipicker/wide3_Link.STL b/xr_rm_teleop/models/rm75_omnipicker/meshes/omnipicker/wide3_Link.STL new file mode 100644 index 0000000..af9de69 Binary files /dev/null and b/xr_rm_teleop/models/rm75_omnipicker/meshes/omnipicker/wide3_Link.STL differ diff --git a/xr_rm_teleop/models/rm75_omnipicker/meshes/omnipicker/wide_loop_Link.STL b/xr_rm_teleop/models/rm75_omnipicker/meshes/omnipicker/wide_loop_Link.STL new file mode 100644 index 0000000..3ab9e86 Binary files /dev/null and b/xr_rm_teleop/models/rm75_omnipicker/meshes/omnipicker/wide_loop_Link.STL differ diff --git a/xr_rm_teleop/models/rm75_omnipicker/meshes/rm75/base_link.STL b/xr_rm_teleop/models/rm75_omnipicker/meshes/rm75/base_link.STL new file mode 100644 index 0000000..d5a35d5 Binary files /dev/null and b/xr_rm_teleop/models/rm75_omnipicker/meshes/rm75/base_link.STL differ diff --git a/xr_rm_teleop/models/rm75_omnipicker/meshes/rm75/link_1.STL b/xr_rm_teleop/models/rm75_omnipicker/meshes/rm75/link_1.STL new file mode 100644 index 0000000..8b2bc5f Binary files /dev/null and b/xr_rm_teleop/models/rm75_omnipicker/meshes/rm75/link_1.STL differ diff --git a/xr_rm_teleop/models/rm75_omnipicker/meshes/rm75/link_2.STL b/xr_rm_teleop/models/rm75_omnipicker/meshes/rm75/link_2.STL new file mode 100644 index 0000000..83d21b3 Binary files /dev/null and b/xr_rm_teleop/models/rm75_omnipicker/meshes/rm75/link_2.STL differ diff --git a/xr_rm_teleop/models/rm75_omnipicker/meshes/rm75/link_3.STL b/xr_rm_teleop/models/rm75_omnipicker/meshes/rm75/link_3.STL new file mode 100644 index 0000000..5f6baba Binary files /dev/null and b/xr_rm_teleop/models/rm75_omnipicker/meshes/rm75/link_3.STL differ diff --git a/xr_rm_teleop/models/rm75_omnipicker/meshes/rm75/link_4.STL b/xr_rm_teleop/models/rm75_omnipicker/meshes/rm75/link_4.STL new file mode 100644 index 0000000..949dc0f Binary files /dev/null and b/xr_rm_teleop/models/rm75_omnipicker/meshes/rm75/link_4.STL differ diff --git a/xr_rm_teleop/models/rm75_omnipicker/meshes/rm75/link_5.STL b/xr_rm_teleop/models/rm75_omnipicker/meshes/rm75/link_5.STL new file mode 100644 index 0000000..bfaf4f9 Binary files /dev/null and b/xr_rm_teleop/models/rm75_omnipicker/meshes/rm75/link_5.STL differ diff --git a/xr_rm_teleop/models/rm75_omnipicker/meshes/rm75/link_6.STL b/xr_rm_teleop/models/rm75_omnipicker/meshes/rm75/link_6.STL new file mode 100644 index 0000000..3ed9d50 Binary files /dev/null and b/xr_rm_teleop/models/rm75_omnipicker/meshes/rm75/link_6.STL differ diff --git a/xr_rm_teleop/models/rm75_omnipicker/meshes/rm75/link_7.STL b/xr_rm_teleop/models/rm75_omnipicker/meshes/rm75/link_7.STL new file mode 100644 index 0000000..412c292 Binary files /dev/null and b/xr_rm_teleop/models/rm75_omnipicker/meshes/rm75/link_7.STL differ diff --git a/xr_rm_teleop/models/rm75_omnipicker/urdf/RM75-B_OmniPicker_fixed.urdf b/xr_rm_teleop/models/rm75_omnipicker/urdf/RM75-B_OmniPicker_fixed.urdf new file mode 100644 index 0000000..f68fa8c --- /dev/null +++ b/xr_rm_teleop/models/rm75_omnipicker/urdf/RM75-B_OmniPicker_fixed.urdf @@ -0,0 +1,503 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/xr_rm_teleop/setup.py b/xr_rm_teleop/setup.py index e981e1f..b1ee9f4 100755 --- a/xr_rm_teleop/setup.py +++ b/xr_rm_teleop/setup.py @@ -24,6 +24,18 @@ setup( f"share/{package_name}/models/rm75/meshes", glob("models/rm75/meshes/*.STL"), ), + ( + f"share/{package_name}/models/rm75_omnipicker/urdf", + glob("models/rm75_omnipicker/urdf/*.urdf"), + ), + ( + f"share/{package_name}/models/rm75_omnipicker/meshes/rm75", + glob("models/rm75_omnipicker/meshes/rm75/*.STL"), + ), + ( + f"share/{package_name}/models/rm75_omnipicker/meshes/omnipicker", + glob("models/rm75_omnipicker/meshes/omnipicker/*.STL"), + ), ], install_requires=["setuptools"], zip_safe=True, diff --git a/xr_rm_teleop/test/placo_ik_smoke.py b/xr_rm_teleop/test/placo_ik_smoke.py index 0e8a6f0..95f982d 100644 --- a/xr_rm_teleop/test/placo_ik_smoke.py +++ b/xr_rm_teleop/test/placo_ik_smoke.py @@ -8,56 +8,67 @@ from pathlib import Path import numpy as np from xr_rm_teleop.placo_ik_solver import PlacoIkSolver -from xr_rm_teleop.realman_adapter import ArmPose CASES = { - "left": ( - [-79.55, -9.99, 71.01, 101.45, 95.07, -84.47, -74.52], - [0.0, 0.0, 0.19, 0.0, 0.0, 0.0, 1.0], - ), - "right": ( - [-90.14, 3.76, -86.89, 87.89, -96.53, -79.62, -90.04], - [0.0, 0.0, 0.16, 0.0, 0.0, 0.0, 1.0], - ), + "left": [-79.55, -9.99, 71.01, 101.45, 95.07, -84.47, -74.52], + "right": [-90.14, 3.76, -86.89, 87.89, -96.53, -79.62, -90.04], } -def angle_error(actual: list[float], target: list[float]) -> float: - deltas = [ - math.atan2(math.sin(a - b), math.cos(a - b)) - for a, b in zip(actual, target) - ] - return math.sqrt(sum(value * value for value in deltas)) +def rotation_z(angle: float) -> np.ndarray: + cosine = math.cos(angle) + sine = math.sin(angle) + return np.asarray( + [ + [cosine, -sine, 0.0], + [sine, cosine, 0.0], + [0.0, 0.0, 1.0], + ] + ) + + +def angle_error(actual: np.ndarray, target: np.ndarray) -> float: + cosine = np.clip((np.trace(target @ actual.T) - 1.0) * 0.5, -1.0, 1.0) + return float(math.acos(cosine)) def main() -> None: urdf_path = Path(sys.argv[1]).resolve() - for arm, (joint_degrees, tool_pose) in CASES.items(): - solver = PlacoIkSolver(str(urdf_path), tool_pose, 1.0 / 90.0) - joints = np.deg2rad(joint_degrees).tolist() - current = solver.update_joint_state(joints) - target = ArmPose( - current.x + 0.01, - current.y, - current.z, - current.rx, - current.ry, - current.rz + 0.05, + for arm, joint_degrees in CASES.items(): + initial_joints = np.deg2rad(joint_degrees) + drift_solver = PlacoIkSolver(str(urdf_path), 1.0 / 125.0) + joints = initial_joints.tolist() + stationary_target = drift_solver.update_joint_state(joints) + flange = drift_solver._robot.get_T_world_frame("link_7") + flange_to_tcp = np.linalg.inv(flange) @ stationary_target + assert np.allclose(flange_to_tcp[:3, 3], [0.0, 0.0, 0.16]) + assert np.allclose(flange_to_tcp[:3, :3], np.eye(3)) + for _ in range(250): + drift_solver.update_joint_state(joints) + joints = drift_solver.solve(stationary_target) + drift_degrees = float( + np.max(np.abs(np.rad2deg(np.asarray(joints) - initial_joints))) ) + solver = PlacoIkSolver(str(urdf_path), 1.0 / 125.0) + joints = initial_joints.tolist() + current = solver.update_joint_state(joints) + assert current.shape == (4, 4) + target = current.copy() + target[0, 3] += 0.01 + target[:3, :3] = rotation_z(0.05) @ target[:3, :3] + solve_durations = [] - for _ in range(45): + for _ in range(250): solver.update_joint_state(joints) started_at = time.perf_counter() joints = solver.solve(target) solve_durations.append(time.perf_counter() - started_at) actual = solver.update_joint_state(joints) - position_error = np.linalg.norm( - np.asarray(actual.xyz()) - np.asarray(target.xyz()) - ) - orientation_error = angle_error(actual.rpy(), target.rpy()) + position_error = np.linalg.norm(actual[:3, 3] - target[:3, 3]) + orientation_error = angle_error(actual[:3, :3], target[:3, :3]) assert len(joints) == 7 assert np.isfinite(joints).all() assert np.allclose( @@ -69,9 +80,10 @@ def main() -> None: print( f"{arm}: position_error={position_error:.6f}m, " f"orientation_error={math.degrees(orientation_error):.3f}deg, " + f"stationary_drift={drift_degrees:.3f}deg, " f"solve_avg={1000.0 * np.mean(solve_durations):.3f}ms, " f"solve_max={1000.0 * max(solve_durations):.3f}ms, " - f"solve_overruns={sum(value > 1.0 / 90.0 for value in solve_durations)}" + f"solve_overruns={sum(value > 1.0 / 125.0 for value in solve_durations)}" ) diff --git a/xr_rm_teleop/test/test_joint_control.py b/xr_rm_teleop/test/test_joint_control.py index 58d19ee..15dd3d2 100644 --- a/xr_rm_teleop/test/test_joint_control.py +++ b/xr_rm_teleop/test/test_joint_control.py @@ -1,13 +1,21 @@ import time from types import SimpleNamespace +import numpy as np import pytest -from xr_rm_teleop.realman_adapter import ArmPose, JointStateSnapshot -from xr_rm_teleop.single_arm_velocity_teleop import SingleArmVelocityTeleop +from xr_rm_teleop.realman_adapter import JointStateSnapshot +from xr_rm_teleop.single_arm_velocity_teleop import ( + SingleArmVelocityTeleop, + _make_transform, + _so3_exp, +) class FakeLogger: + def info(self, *args, **kwargs): + del args, kwargs + def warn(self, *args, **kwargs): del args, kwargs @@ -78,7 +86,9 @@ def test_first_feedback_initializes_last_valid_target_without_solving() -> None: def update_joint_state(self, joints): assert joints == [0.1] * 7 - return ArmPose(0.3, 0.0, 0.2) + transform = np.eye(4) + transform[:3, 3] = [0.3, 0.0, 0.2] + return transform def solve(self, target): del target @@ -95,7 +105,9 @@ def test_first_feedback_initializes_last_valid_target_without_solving() -> None: JointStateSnapshot([0.1] * 7, time.monotonic()) ) - assert pose == ArmPose(0.3, 0.0, 0.2) + assert pose == pytest.approx( + _make_transform([0.3, 0.0, 0.2], np.eye(3)) + ) assert teleop._last_valid_joint_target == [0.1] * 7 assert teleop._ik_solver.solve_calls == 0 @@ -112,7 +124,7 @@ def test_qp_failure_returns_last_known_good_target() -> None: teleop._arm_name = "right_rm75" teleop.get_logger = lambda: FakeLogger() - target = teleop._solve_joint_target(ArmPose(0.3, 0.0, 0.2)) + target = teleop._solve_joint_target(np.eye(4)) assert target == pytest.approx([0.1] * 7) assert teleop._last_valid_joint_target == pytest.approx([0.1] * 7) @@ -130,12 +142,88 @@ def test_qp_success_updates_last_known_good_target() -> None: teleop._arm_name = "left_rm75" teleop.get_logger = lambda: FakeLogger() - target = teleop._solve_joint_target(ArmPose(0.3, 0.0, 0.2)) + target = teleop._solve_joint_target(np.eye(4)) assert target == pytest.approx([0.2] * 7) assert teleop._last_valid_joint_target == pytest.approx([0.2] * 7) +def test_enter_active_control_initializes_se3_orientation_state() -> None: + teleop = object.__new__(SingleArmVelocityTeleop) + transform = _make_transform( + [0.3, -0.1, 0.2], + _so3_exp(np.asarray([0.1, -0.2, 0.3])), + ) + published = [] + teleop._arm_name = "right_rm75" + teleop.get_logger = lambda: FakeLogger() + teleop._publish_debug = lambda *args: published.append(args) + + teleop._enter_active_control( + [0.0, 0.0, 0.0], + (0.0, 0.0, 0.0, 1.0), + transform, + FakeTime(), + ) + + assert teleop._robot_start_transform == pytest.approx(transform) + assert teleop._filtered_target == pytest.approx(transform[:3, 3]) + assert teleop._filtered_orientation_target == pytest.approx(transform[:3, :3]) + assert teleop._last_sent_orientation == pytest.approx(transform[:3, :3]) + assert len(published) == 1 + + +def test_command_angular_velocity_uses_so3_rotation_vector() -> None: + teleop = object.__new__(SingleArmVelocityTeleop) + teleop._dt = 0.1 + teleop._last_sent_target = [0.0, 0.0, 0.0] + teleop._last_sent_orientation = np.eye(3) + teleop._last_command_time = None + + velocity = teleop._estimate_command_velocity( + [0.0, 0.0, 0.0], + _so3_exp(np.asarray([0.0, 0.0, 0.1])), + FakeTime(), + ) + + assert velocity == pytest.approx([0.0, 0.0, 0.0, 0.0, 0.0, 1.0]) + + +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()) + + def test_joint_send_failure_requests_slow_stop_and_resets_control() -> None: class FailingAdapter: def __init__(self) -> None: diff --git a/xr_rm_teleop/test/test_orientation_control.py b/xr_rm_teleop/test/test_orientation_control.py index 4e10409..7313e08 100644 --- a/xr_rm_teleop/test/test_orientation_control.py +++ b/xr_rm_teleop/test/test_orientation_control.py @@ -2,14 +2,19 @@ import math import time from types import SimpleNamespace +import numpy as np import pytest -from xr_rm_teleop.realman_adapter import ArmPose, JointStateSnapshot +from xr_rm_teleop.realman_adapter import JointStateSnapshot from xr_rm_teleop.single_arm_velocity_teleop import ( SingleArmVelocityTeleop, - _euler_to_quaternion, + _make_transform, + _matrix_to_quaternion, _normalize_quaternion, - _quaternion_to_euler, + _project_rotation, + _quaternion_to_matrix, + _so3_exp, + _so3_log, ) @@ -18,7 +23,10 @@ def _make_teleop_for_orientation() -> SingleArmVelocityTeleop: teleop._enable_orientation_control = True teleop._enable_orientation_axes = [True, True, True] teleop._controller_orientation_start = (0.0, 0.0, 0.0, 1.0) - teleop._robot_start_pose = ArmPose(0.3, 0.0, 0.2, 0.1, -0.2, 0.3) + teleop._robot_start_transform = _make_transform( + [0.3, 0.0, 0.2], + _so3_exp(np.asarray([0.1, -0.2, 0.3])), + ) teleop._xr_to_robot_matrix = [ 0.0, 1.0, 0.0, 0.0, 0.0, 1.0, @@ -27,47 +35,111 @@ def _make_teleop_for_orientation() -> SingleArmVelocityTeleop: return teleop -def assert_angles_close(actual: list[float] | tuple[float, ...], expected: list[float]) -> None: - assert len(actual) == len(expected) - for actual_value, expected_value in zip(actual, expected): - assert math.atan2(math.sin(actual_value - expected_value), math.cos(actual_value - expected_value)) == pytest.approx(0.0) - - def test_identity_controller_orientation_keeps_tcp_orientation() -> None: teleop = _make_teleop_for_orientation() target = teleop._raw_orientation_from_controller((0.0, 0.0, 0.0, 1.0)) - assert_angles_close(target, teleop._robot_start_pose.rpy()) + assert target == pytest.approx(teleop._robot_start_transform[:3, :3]) def test_xr_relative_rotation_maps_through_xr_to_robot_matrix() -> None: teleop = _make_teleop_for_orientation() - teleop._robot_start_pose = ArmPose(0.3, 0.0, 0.2, 0.0, 0.0, 0.0) - xr_roll = _euler_to_quaternion(0.2, 0.0, 0.0) + teleop._robot_start_transform = np.eye(4) + xr_roll = _matrix_to_quaternion(_so3_exp(np.asarray([0.2, 0.0, 0.0]))) target = teleop._raw_orientation_from_controller(xr_roll) - assert_angles_close(target, [0.0, 0.0, 0.2]) + assert _so3_log(target) == pytest.approx([0.0, 0.0, 0.2]) -def test_orientation_deadband_filter_and_speed_limit() -> None: +def test_quaternion_sign_does_not_change_rotation() -> None: + quaternion = _normalize_quaternion((0.2, -0.3, 0.1, 0.9)) + + assert _quaternion_to_matrix(quaternion) == pytest.approx( + _quaternion_to_matrix(tuple(-value for value in quaternion)) + ) + + +@pytest.mark.parametrize("pitch", [math.pi / 2.0 - 1e-5, -math.pi / 2.0 + 1e-5]) +def test_small_rotation_near_gimbal_lock_stays_small(pitch: float) -> None: + teleop = _make_teleop_for_orientation() + start_rotation = _so3_exp(np.asarray([0.0, pitch, 0.0])) + teleop._robot_start_transform = _make_transform([0.3, 0.0, 0.2], start_rotation) + teleop._xr_to_robot_matrix = np.eye(3).reshape(-1).tolist() + controller = _matrix_to_quaternion(_so3_exp(np.asarray([0.01, 0.0, 0.0]))) + + target = teleop._raw_orientation_from_controller(controller) + + error = _so3_log(target @ start_rotation.T) + assert np.linalg.norm(error) == pytest.approx(0.01) + + +def test_crossing_old_rpy_branch_uses_shortest_rotation() -> None: + teleop = _make_teleop_for_orientation() + start_rotation = _so3_exp(np.asarray([0.0, math.pi / 2.0 - 0.001, 0.0])) + teleop._robot_start_transform = _make_transform([0.3, 0.0, 0.2], start_rotation) + teleop._xr_to_robot_matrix = np.eye(3).reshape(-1).tolist() + controller = _matrix_to_quaternion(_so3_exp(np.asarray([0.0, 0.002, 0.0]))) + + target = teleop._raw_orientation_from_controller(controller) + + assert _so3_log(target @ start_rotation.T) == pytest.approx( + [0.0, 0.002, 0.0], + abs=1e-9, + ) + + +def test_disabled_orientation_axis_zeros_robot_rotation_vector_component() -> None: + teleop = _make_teleop_for_orientation() + teleop._robot_start_transform = np.eye(4) + teleop._xr_to_robot_matrix = np.eye(3).reshape(-1).tolist() + teleop._enable_orientation_axes = [True, False, True] + controller = _matrix_to_quaternion(_so3_exp(np.asarray([0.1, 0.2, 0.3]))) + + target = teleop._raw_orientation_from_controller(controller) + + assert _so3_log(target) == pytest.approx([0.1, 0.0, 0.3]) + + +def test_orientation_deadband_filter_and_speed_limit_use_so3_angle() -> None: teleop = object.__new__(SingleArmVelocityTeleop) teleop._orientation_deadband_rad = 0.01 teleop._orientation_filter_alpha = 0.5 teleop._max_orientation_speed = 0.5 - teleop._dt = 0.1 - teleop._last_sent_orientation = [0.0, 0.0, 0.0] - teleop._filtered_orientation_target = [0.0, 0.0, 0.0] + teleop._dt = 1.0 / 125.0 + teleop._last_sent_orientation = np.eye(3) + teleop._filtered_orientation_target = np.eye(3) - assert teleop._apply_orientation_deadband([0.001, 0.0, 0.0]) == [0.0, 0.0, 0.0] + inside_deadband = _so3_exp(np.asarray([0.006, 0.006, 0.0])) + assert teleop._apply_orientation_deadband(inside_deadband) == pytest.approx(np.eye(3)) - filtered = teleop._filter_orientation_target([0.2, 0.0, 0.0]) - assert_angles_close(filtered, [0.1, 0.0, 0.0]) + target = _so3_exp(np.asarray([0.2, 0.0, 0.0])) + filtered = teleop._filter_orientation_target(target) + assert _so3_log(filtered) == pytest.approx([0.1, 0.0, 0.0]) - limited, was_limited = teleop._limit_orientation_step([0.2, 0.0, 0.0]) + limited, was_limited = teleop._limit_orientation_step(target) assert was_limited - assert_angles_close(limited, [0.05, 0.0, 0.0]) + assert np.linalg.norm(_so3_log(limited)) == pytest.approx(0.5 / 125.0) + + +def test_rotation_matrix_to_debug_quaternion_is_normalized() -> None: + quaternion = _matrix_to_quaternion(_so3_exp(np.asarray([0.2, -0.1, 0.3]))) + + assert np.isfinite(quaternion).all() + assert np.linalg.norm(quaternion) == pytest.approx(1.0) + + +def test_rotation_projection_accepts_small_error_and_rejects_invalid_matrix() -> None: + near_rotation = np.eye(3) + near_rotation[0, 1] = 1e-5 + + projected = _project_rotation(near_rotation) + + assert projected.T @ projected == pytest.approx(np.eye(3)) + assert np.linalg.det(projected) == pytest.approx(1.0) + with pytest.raises(ValueError): + _project_rotation(np.diag([2.0, 1.0, 1.0])) def test_invalid_controller_quaternion_stops_current_tick() -> None: @@ -102,9 +174,7 @@ def test_invalid_controller_quaternion_stops_current_tick() -> None: time.monotonic(), ) ) - teleop._ik_solver = SimpleNamespace( - update_joint_state=lambda joints: ArmPose(0.3, 0.0, 0.2) - ) + teleop._ik_solver = SimpleNamespace(update_joint_state=lambda joints: np.eye(4)) teleop._active = False teleop._last_valid_joint_target = None teleop._last_current_pose = None @@ -119,11 +189,6 @@ def test_invalid_controller_quaternion_stops_current_tick() -> None: assert stopped == [True] -def test_quaternion_roundtrip_for_small_rpy() -> None: - quat = _normalize_quaternion(_euler_to_quaternion(0.2, -0.1, 0.3)) - assert_angles_close(_quaternion_to_euler(quat), [0.2, -0.1, 0.3]) - - def test_zero_quaternion_is_invalid() -> None: with pytest.raises(ValueError): _normalize_quaternion([0.0, 0.0, 0.0, 0.0]) diff --git a/xr_rm_teleop/test/test_placo_transforms.py b/xr_rm_teleop/test/test_placo_transforms.py index ed75908..8d1ec34 100644 --- a/xr_rm_teleop/test/test_placo_transforms.py +++ b/xr_rm_teleop/test/test_placo_transforms.py @@ -1,37 +1,72 @@ -import math +from pathlib import Path +from xml.etree import ElementTree import numpy as np import pytest from xr_rm_teleop.placo_ik_solver import ( PlacoIkSolver, - _arm_pose_to_transform, - _tool_pose_to_transform, - _transform_to_arm_pose, + _validated_transform, ) -from xr_rm_teleop.realman_adapter import ArmPose -def test_tool_offset_rotates_with_flange_and_roundtrips() -> None: - flange_pose = ArmPose(0.30, -0.10, 0.20, 0.0, math.pi / 2.0, 0.0) - tool_pose = [0.0, 0.0, 0.19, 0.0, 0.0, 0.0, 1.0] +def test_fixed_urdf_has_seven_moving_joints_and_omnipicker_tcp() -> None: + urdf_path = ( + Path(__file__).resolve().parents[1] + / "models" + / "rm75_omnipicker" + / "urdf" + / "RM75-B_OmniPicker_fixed.urdf" + ) + root = ElementTree.parse(urdf_path).getroot() + moving_joint_names = [ + joint.attrib["name"] + for joint in root.findall("joint") + if joint.attrib["type"] != "fixed" + ] + tcp_joint = root.find("joint[@name='omnipicker_tcp_joint']") + mesh_filenames = [ + mesh.attrib["filename"] + for mesh in root.findall(".//mesh") + ] - base_to_flange = _arm_pose_to_transform(flange_pose) - flange_to_tool = _tool_pose_to_transform(tool_pose) - base_to_tool = base_to_flange @ flange_to_tool - recovered_flange = base_to_tool @ np.linalg.inv(flange_to_tool) - - assert base_to_tool[:3, 3] == pytest.approx([0.49, -0.10, 0.20]) - assert recovered_flange == pytest.approx(base_to_flange) + assert moving_joint_names == [f"joint_{index}" for index in range(1, 8)] + assert all( + filename.startswith( + "package://xr_rm_teleop/models/rm75_omnipicker/meshes/" + ) + for filename in mesh_filenames + ) + assert tcp_joint is not None + 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" -def test_transform_to_arm_pose_roundtrip() -> None: - expected = ArmPose(0.25, -0.30, 0.40, 0.20, -0.30, 0.40) +def test_validated_transform_accepts_finite_se3_and_returns_a_copy() -> None: + transform = np.eye(4) + transform[:3, 3] = [0.3, -0.1, 0.2] - actual = _transform_to_arm_pose(_arm_pose_to_transform(expected)) + actual = _validated_transform(transform) - assert actual.xyz() == pytest.approx(expected.xyz()) - assert actual.rpy() == pytest.approx(expected.rpy()) + assert actual == pytest.approx(transform) + assert actual is not transform + + +@pytest.mark.parametrize( + "transform", + [ + np.eye(3), + np.full((4, 4), np.nan), + np.vstack([np.eye(3, 4), [0.0, 0.0, 0.0, 2.0]]), + np.diag([2.0, 1.0, 1.0, 1.0]), + ], +) +def test_validated_transform_rejects_invalid_se3(transform: np.ndarray) -> None: + with pytest.raises(ValueError): + _validated_transform(transform) def test_qp_result_rejects_nan_position_and_velocity_violations() -> None: diff --git a/xr_rm_teleop/xr_rm_teleop/placo_ik_solver.py b/xr_rm_teleop/xr_rm_teleop/placo_ik_solver.py index e66bf03..fbefd0e 100644 --- a/xr_rm_teleop/xr_rm_teleop/placo_ik_solver.py +++ b/xr_rm_teleop/xr_rm_teleop/placo_ik_solver.py @@ -2,102 +2,43 @@ from __future__ import annotations -import math from importlib.metadata import PackageNotFoundError, version from pathlib import Path import numpy as np -from .realman_adapter import ArmPose - - EXPECTED_PLACO_VERSION = "0.9.4" RM75_JOINT_NAMES = [f"joint_{index}" for index in range(1, 8)] RM75_Q_SLICE = slice(7, 14) -def _rpy_to_rotation(roll: float, pitch: float, yaw: float) -> np.ndarray: - cr, sr = math.cos(roll), math.sin(roll) - cp, sp = math.cos(pitch), math.sin(pitch) - cy, sy = math.cos(yaw), math.sin(yaw) - return np.array( - [ - [cy * cp, cy * sp * sr - sy * cr, cy * sp * cr + sy * sr], - [sy * cp, sy * sp * sr + cy * cr, sy * sp * cr - cy * sr], - [-sp, cp * sr, cp * cr], - ], - dtype=float, - ) +def _validated_transform(transform: np.ndarray) -> np.ndarray: + values = np.asarray(transform, dtype=float) + if values.shape != (4, 4) or not np.isfinite(values).all(): + raise ValueError("target transform must be a finite 4x4 matrix") + if not np.allclose(values[3], [0.0, 0.0, 0.0, 1.0], atol=1e-9): + raise ValueError("target transform must have a valid homogeneous row") + rotation = values[:3, :3] + if ( + np.linalg.norm(rotation.T @ rotation - np.eye(3)) > 1e-3 + or np.linalg.det(rotation) <= 0.0 + ): + raise ValueError("target transform must contain a valid rotation") + u, _, vt = np.linalg.svd(rotation) + projected = u @ vt + if np.linalg.det(projected) <= 0.0: + raise ValueError("target transform must contain a proper rotation") -def _rotation_to_rpy(rotation: np.ndarray) -> tuple[float, float, float]: - pitch = math.asin(-float(np.clip(rotation[2, 0], -1.0, 1.0))) - if abs(math.cos(pitch)) > 1e-9: - roll = math.atan2(float(rotation[2, 1]), float(rotation[2, 2])) - yaw = math.atan2(float(rotation[1, 0]), float(rotation[0, 0])) - else: - roll = math.atan2(-float(rotation[1, 2]), float(rotation[1, 1])) - yaw = 0.0 - return roll, pitch, yaw - - -def _arm_pose_to_transform(pose: ArmPose) -> np.ndarray: - transform = np.eye(4) - transform[:3, :3] = _rpy_to_rotation(pose.rx, pose.ry, pose.rz) - transform[:3, 3] = pose.xyz() - return transform - - -def _tool_pose_to_transform(tool_pose: list[float]) -> np.ndarray: - values = np.asarray(tool_pose, dtype=float) - if values.shape != (7,) or not np.isfinite(values).all(): - raise ValueError("tool pose must contain 7 finite values") - x, y, z, qx, qy, qz, qw = values - norm = math.sqrt(qx * qx + qy * qy + qz * qz + qw * qw) - if norm <= 1e-9: - raise ValueError("tool quaternion norm must be positive") - qx, qy, qz, qw = qx / norm, qy / norm, qz / norm, qw / norm - transform = np.eye(4) - transform[:3, :3] = np.array( - [ - [ - 1 - 2 * (qy * qy + qz * qz), - 2 * (qx * qy - qz * qw), - 2 * (qx * qz + qy * qw), - ], - [ - 2 * (qx * qy + qz * qw), - 1 - 2 * (qx * qx + qz * qz), - 2 * (qy * qz - qx * qw), - ], - [ - 2 * (qx * qz - qy * qw), - 2 * (qy * qz + qx * qw), - 1 - 2 * (qx * qx + qy * qy), - ], - ] - ) - transform[:3, 3] = [x, y, z] - return transform - - -def _transform_to_arm_pose(transform: np.ndarray) -> ArmPose: - roll, pitch, yaw = _rotation_to_rpy(transform[:3, :3]) - return ArmPose( - float(transform[0, 3]), - float(transform[1, 3]), - float(transform[2, 3]), - roll, - pitch, - yaw, - ) + result = values.copy() + result[:3, :3] = projected + return result class PlacoIkSolver: def __init__( self, urdf_path: str, - tool_pose: list[float], dt: float, ) -> None: if dt <= 0.0: @@ -147,15 +88,16 @@ class PlacoIkSolver: for index in velocity_offsets ] ) - self._tool_transform = _tool_pose_to_transform(tool_pose) - self._tool_inverse = np.linalg.inv(self._tool_transform) self._actual_joints: np.ndarray | None = None self._solver = placo.KinematicsSolver(self._robot) self._solver.dt = dt self._solver.mask_fbase(True) self._solver.enable_velocity_limits(True) - self._frame_task = self._solver.add_frame_task("link_7", np.eye(4)) + self._frame_task = self._solver.add_frame_task( + "omnipicker_tcp", + np.eye(4), + ) self._frame_task.configure("rm75_frame", "soft", 1.0) manipulability = self._solver.add_manipulability_task( "link_7", @@ -169,7 +111,7 @@ class PlacoIkSolver: def base_configuration(self) -> list[float]: return self._robot.state.q[:7].tolist() - def update_joint_state(self, joints: list[float]) -> ArmPose: + def update_joint_state(self, joints: list[float]) -> np.ndarray: values = np.asarray(joints, dtype=float) if values.shape != (7,) or not np.isfinite(values).all(): raise ValueError("joint state must contain 7 finite values") @@ -177,18 +119,15 @@ class PlacoIkSolver: self._actual_joints = values.copy() self._robot.state.q[RM75_Q_SLICE] = values self._robot.update_kinematics() - base_to_flange = self._robot.get_T_world_frame("link_7") + base_to_tool = self._robot.get_T_world_frame("omnipicker_tcp") if is_first_feedback: - self._frame_task.T_world_frame = base_to_flange.copy() - base_to_tool = base_to_flange @ self._tool_transform - return _transform_to_arm_pose(base_to_tool) + self._frame_task.T_world_frame = base_to_tool.copy() + return base_to_tool.copy() - def solve(self, target_tool_pose: ArmPose) -> list[float]: + def solve(self, target_tool_pose: np.ndarray) -> list[float]: if self._actual_joints is None: raise RuntimeError("joint state must be initialized before QP solve") - self._frame_task.T_world_frame = ( - _arm_pose_to_transform(target_tool_pose) @ self._tool_inverse - ) + self._frame_task.T_world_frame = _validated_transform(target_tool_pose) self._solver.solve(True) result = np.asarray( self._robot.state.q[RM75_Q_SLICE], diff --git a/xr_rm_teleop/xr_rm_teleop/single_arm_velocity_teleop.py b/xr_rm_teleop/xr_rm_teleop/single_arm_velocity_teleop.py index 3885416..657f3f7 100755 --- a/xr_rm_teleop/xr_rm_teleop/single_arm_velocity_teleop.py +++ b/xr_rm_teleop/xr_rm_teleop/single_arm_velocity_teleop.py @@ -12,6 +12,7 @@ import threading import time from typing import Iterable +import numpy as np import rclpy from geometry_msgs.msg import PoseStamped, TwistStamped from rclpy.node import Node @@ -23,7 +24,6 @@ from xr_rm_interfaces.msg import XrController from .fun_peripheral import load_peripheral_config from .placo_ik_solver import PlacoIkSolver from .realman_adapter import ( - ArmPose, JointStateSnapshot, MockRealManAdapter, RealManAdapter, @@ -38,14 +38,6 @@ def _clamp(value: float, low: float, high: float) -> float: return min(max(value, low), high) -def _wrap_angle(angle: float) -> float: - return math.atan2(math.sin(angle), math.cos(angle)) - - -def _angle_delta(target: float, current: float) -> float: - return _wrap_angle(target - current) - - def _normalize_quaternion(values: Iterable[float]) -> tuple[float, float, float, float]: x, y, z, w = [float(value) for value in values] norm = math.sqrt(x * x + y * y + z * z + w * w) @@ -54,31 +46,26 @@ def _normalize_quaternion(values: Iterable[float]) -> tuple[float, float, float, return x / norm, y / norm, z / norm, w / norm -def _quaternion_conjugate( +def _project_rotation(rotation: np.ndarray) -> np.ndarray: + values = np.asarray(rotation, dtype=float) + if values.shape != (3, 3) or not np.isfinite(values).all(): + raise ValueError("rotation must be a finite 3x3 matrix") + if ( + np.linalg.norm(values.T @ values - np.eye(3)) > 1e-3 + or np.linalg.det(values) <= 0.0 + ): + raise ValueError("rotation matrix is invalid") + u, _, vt = np.linalg.svd(values) + projected = u @ vt + if np.linalg.det(projected) <= 0.0: + raise ValueError("rotation matrix must be proper") + return projected + + +def _quaternion_to_matrix( quat: tuple[float, float, float, float], -) -> tuple[float, float, float, float]: - x, y, z, w = quat - return -x, -y, -z, w - - -def _quaternion_multiply( - left: tuple[float, float, float, float], - right: tuple[float, float, float, float], -) -> tuple[float, float, float, float]: - lx, ly, lz, lw = left - rx, ry, rz, rw = right - return _normalize_quaternion( - ( - lw * rx + lx * rw + ly * rz - lz * ry, - lw * ry - lx * rz + ly * rw + lz * rx, - lw * rz + lx * ry - ly * rx + lz * rw, - lw * rw - lx * rx - ly * ry - lz * rz, - ) - ) - - -def _quaternion_to_matrix(quat: tuple[float, float, float, float]) -> list[float]: - x, y, z, w = quat +) -> np.ndarray: + x, y, z, w = _normalize_quaternion(quat) xx = x * x yy = y * y zz = z * z @@ -88,69 +75,95 @@ def _quaternion_to_matrix(quat: tuple[float, float, float, float]) -> list[float wx = w * x wy = w * y wz = w * z - return [ - 1.0 - 2.0 * (yy + zz), - 2.0 * (xy - wz), - 2.0 * (xz + wy), - 2.0 * (xy + wz), - 1.0 - 2.0 * (xx + zz), - 2.0 * (yz - wx), - 2.0 * (xz - wy), - 2.0 * (yz + wx), - 1.0 - 2.0 * (xx + yy), - ] + return np.asarray( + [ + [1.0 - 2.0 * (yy + zz), 2.0 * (xy - wz), 2.0 * (xz + wy)], + [2.0 * (xy + wz), 1.0 - 2.0 * (xx + zz), 2.0 * (yz - wx)], + [2.0 * (xz - wy), 2.0 * (yz + wx), 1.0 - 2.0 * (xx + yy)], + ] + ) -def _matrix_multiply(left: list[float], right: list[float]) -> list[float]: - return [ - sum(left[row * 3 + k] * right[k * 3 + col] for k in range(3)) - for row in range(3) - for col in range(3) - ] - - -def _matrix_transpose(matrix: list[float]) -> list[float]: - return [ - matrix[0], matrix[3], matrix[6], - matrix[1], matrix[4], matrix[7], - matrix[2], matrix[5], matrix[8], - ] - - -def _euler_to_quaternion(roll: float, pitch: float, yaw: float) -> tuple[float, float, float, float]: - cy = math.cos(yaw * 0.5) - sy = math.sin(yaw * 0.5) - cp = math.cos(pitch * 0.5) - sp = math.sin(pitch * 0.5) - cr = math.cos(roll * 0.5) - sr = math.sin(roll * 0.5) - - qw = cr * cp * cy + sr * sp * sy - qx = sr * cp * cy - cr * sp * sy - qy = cr * sp * cy + sr * cp * sy - qz = cr * cp * sy - sr * sp * cy - return _normalize_quaternion((qx, qy, qz, qw)) - - -def _euler_to_matrix(roll: float, pitch: float, yaw: float) -> list[float]: - return _quaternion_to_matrix(_euler_to_quaternion(roll, pitch, yaw)) - - -def _matrix_to_euler(matrix: list[float]) -> tuple[float, float, float]: - sy = -_clamp(matrix[6], -1.0, 1.0) - pitch = math.asin(sy) - cp = math.cos(pitch) - if abs(cp) > 1e-9: - roll = math.atan2(matrix[7], matrix[8]) - yaw = math.atan2(matrix[3], matrix[0]) +def _matrix_to_quaternion( + rotation: np.ndarray, +) -> tuple[float, float, float, float]: + matrix = _project_rotation(rotation) + trace = float(np.trace(matrix)) + if trace > 0.0: + scale = math.sqrt(trace + 1.0) * 2.0 + quaternion = ( + (matrix[2, 1] - matrix[1, 2]) / scale, + (matrix[0, 2] - matrix[2, 0]) / scale, + (matrix[1, 0] - matrix[0, 1]) / scale, + 0.25 * scale, + ) + elif matrix[0, 0] > matrix[1, 1] and matrix[0, 0] > matrix[2, 2]: + scale = math.sqrt(1.0 + matrix[0, 0] - matrix[1, 1] - matrix[2, 2]) * 2.0 + quaternion = ( + 0.25 * scale, + (matrix[0, 1] + matrix[1, 0]) / scale, + (matrix[0, 2] + matrix[2, 0]) / scale, + (matrix[2, 1] - matrix[1, 2]) / scale, + ) + elif matrix[1, 1] > matrix[2, 2]: + scale = math.sqrt(1.0 + matrix[1, 1] - matrix[0, 0] - matrix[2, 2]) * 2.0 + quaternion = ( + (matrix[0, 1] + matrix[1, 0]) / scale, + 0.25 * scale, + (matrix[1, 2] + matrix[2, 1]) / scale, + (matrix[0, 2] - matrix[2, 0]) / scale, + ) else: - roll = math.atan2(-matrix[5], matrix[4]) - yaw = 0.0 - return _wrap_angle(roll), _wrap_angle(pitch), _wrap_angle(yaw) + scale = math.sqrt(1.0 + matrix[2, 2] - matrix[0, 0] - matrix[1, 1]) * 2.0 + quaternion = ( + (matrix[0, 2] + matrix[2, 0]) / scale, + (matrix[1, 2] + matrix[2, 1]) / scale, + 0.25 * scale, + (matrix[1, 0] - matrix[0, 1]) / scale, + ) + result = _normalize_quaternion(quaternion) + return tuple(-value for value in result) if result[3] < 0.0 else result -def _quaternion_to_euler(quat: tuple[float, float, float, float]) -> tuple[float, float, float]: - return _matrix_to_euler(_quaternion_to_matrix(quat)) +def _so3_log(rotation: np.ndarray) -> np.ndarray: + quaternion = _matrix_to_quaternion(rotation) + vector = np.asarray(quaternion[:3]) + sine_half = float(np.linalg.norm(vector)) + if sine_half <= 1e-12: + return 2.0 * vector + angle = 2.0 * math.atan2(sine_half, quaternion[3]) + return vector * (angle / sine_half) + + +def _so3_exp(rotation_vector: np.ndarray) -> np.ndarray: + vector = np.asarray(rotation_vector, dtype=float) + if vector.shape != (3,) or not np.isfinite(vector).all(): + raise ValueError("rotation vector must contain 3 finite values") + angle = float(np.linalg.norm(vector)) + skew = np.asarray( + [ + [0.0, -vector[2], vector[1]], + [vector[2], 0.0, -vector[0]], + [-vector[1], vector[0], 0.0], + ] + ) + if angle <= 1e-9: + return _project_rotation(np.eye(3) + skew + 0.5 * skew @ skew) + return _project_rotation( + np.eye(3) + + math.sin(angle) / angle * skew + + (1.0 - math.cos(angle)) / (angle * angle) * skew @ skew + ) + + +def _make_transform(position: Iterable[float], rotation: np.ndarray) -> np.ndarray: + xyz = np.asarray(list(position), dtype=float) + if xyz.shape != (3,) or not np.isfinite(xyz).all(): + raise ValueError("position must contain 3 finite values") + transform = np.eye(4) + transform[:3, :3] = _project_rotation(rotation) + transform[:3, 3] = xyz + return transform class SingleArmVelocityTeleop(Node): @@ -164,7 +177,7 @@ class SingleArmVelocityTeleop(Node): self.declare_parameter("arm_name", "rm75") self.declare_parameter("controller_topic", "/xr/right_controller") - self.declare_parameter("control_rate_hz", 90.0) + self.declare_parameter("control_rate_hz", 125.0) self.declare_parameter("command_timeout_sec", 0.12) self.declare_parameter("scale", 1.0) self.declare_parameter("deadband_m", 0.001) @@ -177,7 +190,7 @@ class SingleArmVelocityTeleop(Node): self.declare_parameter("enable_orientation_axes", [True, True, True]) self.declare_parameter("orientation_deadband_rad", 0.005) self.declare_parameter("orientation_filter_alpha", 0.65) - self.declare_parameter("max_orientation_speed", 0.6) + self.declare_parameter("max_orientation_speed", 0.5) self.declare_parameter("workspace_min", [0.20, -0.35, 0.10]) self.declare_parameter("workspace_max", [0.65, 0.35, 0.60]) self.declare_parameter("cyl_radius_limit", [0.20, 0.60]) @@ -252,13 +265,13 @@ class SingleArmVelocityTeleop(Node): self._active = False self._controller_start: list[float] | None = None self._controller_orientation_start: tuple[float, float, float, float] | None = None - self._robot_start_pose: ArmPose | None = None + self._robot_start_transform: np.ndarray | None = None self._filtered_target: list[float] | None = None - self._filtered_orientation_target: list[float] | None = None + self._filtered_orientation_target: np.ndarray | None = None self._last_sent_target: list[float] | None = None - self._last_sent_orientation: list[float] | None = None + self._last_sent_orientation: np.ndarray | None = None self._last_command_time: Time | None = None - self._last_current_pose: ArmPose | None = None + self._last_current_pose: np.ndarray | None = None self._last_valid_joint_target: list[float] | None = None self._joint_feedback_ready = False self._stop_sent = True @@ -267,6 +280,12 @@ class SingleArmVelocityTeleop(Node): self._tool_command_queue: queue.Queue[tuple[bool, str] | None] | None = None self._tool_worker_stop = threading.Event() self._tool_worker_thread: threading.Thread | None = None + 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 peripheral_arm = self._peripheral_arm_name() config_file = str(self.get_parameter("peripheral_config_file").value) @@ -276,7 +295,6 @@ class SingleArmVelocityTeleop(Node): ) self._ik_solver = PlacoIkSolver( str(self.get_parameter("robot_urdf_path").value), - self._peripheral_config.tool_pose, self._dt, ) self._adapter = self._make_adapter() @@ -454,6 +472,18 @@ class SingleArmVelocityTeleop(Node): self._enqueue_tool_command(self._trigger_tool_open, "trigger") def _control_tick(self) -> None: + tick_started_ns = time.perf_counter_ns() + last_tick_started_ns = getattr( + self, + "_last_control_tick_started_ns", + None, + ) + self._last_control_tick_started_ns = tick_started_ns + tick_period_ms = ( + None + if last_tick_started_ns is None + else (tick_started_ns - last_tick_started_ns) * 1e-6 + ) now = self.get_clock().now() snapshot = self._fresh_joint_state() if snapshot is None: @@ -522,67 +552,93 @@ class SingleArmVelocityTeleop(Node): ) return + feedback_age_ms = (time.monotonic() - snapshot.received_at) * 1000.0 assert self._controller_start is not None - assert self._robot_start_pose is not None + assert self._robot_start_transform is not None raw_target_xyz = self._raw_target_from_controller(controller_now) - raw_target_rpy = self._raw_orientation_from_controller(controller_quat) + raw_target_orientation = self._raw_orientation_from_controller( + controller_quat + ) workspace_target, workspace_clamped = self._clamp_workspace_with_flag(raw_target_xyz) desired_target = self._apply_deadband(workspace_target) filtered_target = self._filter_target(desired_target) sent_target, step_limited = self._limit_target_step(filtered_target) sent_target, final_clamped = self._clamp_workspace_with_flag(sent_target) - desired_orientation = self._apply_orientation_deadband(raw_target_rpy) + desired_orientation = self._apply_orientation_deadband( + raw_target_orientation + ) filtered_orientation = self._filter_orientation_target(desired_orientation) sent_orientation, orientation_step_limited = self._limit_orientation_step(filtered_orientation) velocity = self._estimate_command_velocity(sent_target, sent_orientation, now) target_clamped = workspace_clamped or step_limited or final_clamped or orientation_step_limited - raw_target_pose = ArmPose( - x=raw_target_xyz[0], - y=raw_target_xyz[1], - z=raw_target_xyz[2], - rx=raw_target_rpy[0], - ry=raw_target_rpy[1], - rz=raw_target_rpy[2], + raw_target_pose = _make_transform( + raw_target_xyz, + raw_target_orientation, ) - target_pose = ArmPose( - x=sent_target[0], - y=sent_target[1], - z=sent_target[2], - rx=sent_orientation[0], - ry=sent_orientation[1], - rz=sent_orientation[2], + target_pose = _make_transform( + sent_target, + sent_orientation, ) self._publish_debug(raw_target_pose, target_pose, velocity, target_clamped) + qp_started_ns = time.perf_counter_ns() joint_target = self._solve_joint_target(target_pose) - if self._send_joint_target(joint_target): + qp_ms = (time.perf_counter_ns() - qp_started_ns) * 1e-6 + send_started_ns = time.perf_counter_ns() + sent = self._send_joint_target(joint_target) + send_ms = (time.perf_counter_ns() - send_started_ns) * 1e-6 + if sent: self._last_sent_target = sent_target - self._last_sent_orientation = sent_orientation + self._last_sent_orientation = sent_orientation.copy() self._last_command_time = now self._stop_sent = False + total_ms = (time.perf_counter_ns() - tick_started_ns) * 1e-6 + try: + self._record_timing_sample( + tick_period_ms, + total_ms, + qp_ms, + send_ms, + feedback_age_ms, + ) + except Exception as exc: + self.get_logger().warn( + f"{self._arm_name} 控制周期统计失败：{exc}", + throttle_duration_sec=5.0, + ) def _enter_active_control( self, controller_now: list[float], controller_quat: tuple[float, float, float, float], - robot_pose: ArmPose, + robot_pose: np.ndarray, now: Time, ) -> None: - robot_xyz = robot_pose.xyz() + robot_transform = _make_transform( + robot_pose[:3, 3], + robot_pose[:3, :3], + ) + robot_xyz = robot_transform[:3, 3].tolist() + robot_orientation = robot_transform[:3, :3] self._active = True self._controller_start = controller_now self._controller_orientation_start = controller_quat - self._robot_start_pose = robot_pose + self._robot_start_transform = robot_transform self._filtered_target = robot_xyz - self._filtered_orientation_target = robot_pose.rpy() + self._filtered_orientation_target = robot_orientation.copy() self._last_sent_target = robot_xyz - self._last_sent_orientation = robot_pose.rpy() + self._last_sent_orientation = robot_orientation.copy() self._last_command_time = now self._stop_sent = True self.get_logger().info(f"{self._arm_name} Grip 按下，已锁定手柄和机械臂初始位姿。") - self._publish_debug(robot_pose, robot_pose, [0.0] * 6, False) + self._publish_debug( + robot_transform, + robot_transform, + [0.0] * 6, + False, + ) @staticmethod def _controller_xyz(msg: XrController) -> list[float]: @@ -602,15 +658,21 @@ class SingleArmVelocityTeleop(Node): def _raw_target_from_controller(self, controller_now: list[float]) -> list[float]: assert self._controller_start is not None - assert self._robot_start_pose is not None + assert self._robot_start_transform is not None + robot_start_xyz = self._robot_start_transform[:3, 3] controller_delta = [controller_now[i] - self._controller_start[i] for i in range(3)] robot_delta = self._map_xr_delta_to_robot(controller_delta) target = [ - self._robot_start_pose.x + self._scale * robot_delta[0], - self._robot_start_pose.y + self._scale * robot_delta[1], - self._robot_start_pose.z + self._scale * robot_delta[2], + robot_start_xyz[0] + self._scale * robot_delta[0], + robot_start_xyz[1] + self._scale * robot_delta[1], + robot_start_xyz[2] + self._scale * robot_delta[2], + ] + return [ + float(target[index]) + if self._enable_position_axes[index] + else float(robot_start_xyz[index]) + for index in range(3) ] - return [target[i] if self._enable_position_axes[i] else self._robot_start_pose.xyz()[i] for i in range(3)] def _map_xr_delta_to_robot(self, delta: list[float]) -> list[float]: matrix = self._xr_to_robot_matrix @@ -623,34 +685,33 @@ class SingleArmVelocityTeleop(Node): def _raw_orientation_from_controller( self, controller_quat: tuple[float, float, float, float], - ) -> list[float]: - assert self._robot_start_pose is not None + ) -> np.ndarray: + assert self._robot_start_transform is not None + robot_start_rotation = self._robot_start_transform[:3, :3] if not self._enable_orientation_control: - return self._robot_start_pose.rpy() + return robot_start_rotation.copy() assert self._controller_orientation_start is not None - xr_delta_quat = _quaternion_multiply( - controller_quat, - _quaternion_conjugate(self._controller_orientation_start), + xr_delta_matrix = ( + _quaternion_to_matrix(controller_quat) + @ _quaternion_to_matrix(self._controller_orientation_start).T ) - xr_delta_matrix = _quaternion_to_matrix(xr_delta_quat) - matrix = self._xr_to_robot_matrix - robot_delta_matrix = _matrix_multiply( - _matrix_multiply(matrix, xr_delta_matrix), - _matrix_transpose(matrix), + mapping = np.asarray(self._xr_to_robot_matrix, dtype=float).reshape(3, 3) + robot_delta = _project_rotation( + mapping @ xr_delta_matrix @ mapping.T ) - robot_start_matrix = _euler_to_matrix( - self._robot_start_pose.rx, - self._robot_start_pose.ry, - self._robot_start_pose.rz, + rotation_vector = _so3_log(robot_delta) + rotation_vector = np.asarray( + [ + rotation_vector[index] + if self._enable_orientation_axes[index] + else 0.0 + for index in range(3) + ] + ) + return _project_rotation( + _so3_exp(rotation_vector) @ robot_start_rotation ) - target_matrix = _matrix_multiply(robot_delta_matrix, robot_start_matrix) - target_rpy = list(_matrix_to_euler(target_matrix)) - start_rpy = self._robot_start_pose.rpy() - return [ - target_rpy[i] if self._enable_orientation_axes[i] else start_rpy[i] - for i in range(3) - ] def _apply_deadband(self, target: list[float]) -> list[float]: if self._deadband_m <= 0.0 or self._last_sent_target is None: @@ -699,50 +760,49 @@ class SingleArmVelocityTeleop(Node): for i in range(3) ], True - def _apply_orientation_deadband(self, target_rpy: list[float]) -> list[float]: + def _apply_orientation_deadband(self, target_rotation: np.ndarray) -> np.ndarray: if self._orientation_deadband_rad <= 0.0 or self._last_sent_orientation is None: - return [_wrap_angle(value) for value in target_rpy] - delta = [ - _angle_delta(target_rpy[i], self._last_sent_orientation[i]) - for i in range(3) - ] - if _norm(delta) < self._orientation_deadband_rad: - return list(self._last_sent_orientation) - return [_wrap_angle(value) for value in target_rpy] + return _project_rotation(target_rotation) + error = _so3_log( + target_rotation @ self._last_sent_orientation.T + ) + if np.linalg.norm(error) < self._orientation_deadband_rad: + return self._last_sent_orientation.copy() + return _project_rotation(target_rotation) - def _filter_orientation_target(self, target_rpy: list[float]) -> list[float]: + def _filter_orientation_target(self, target_rotation: np.ndarray) -> np.ndarray: if self._filtered_orientation_target is None: - self._filtered_orientation_target = [_wrap_angle(value) for value in target_rpy] - return list(self._filtered_orientation_target) + self._filtered_orientation_target = _project_rotation(target_rotation) + return self._filtered_orientation_target.copy() - self._filtered_orientation_target = [ - _wrap_angle( - self._filtered_orientation_target[i] - + self._orientation_filter_alpha - * _angle_delta(target_rpy[i], self._filtered_orientation_target[i]) - ) - for i in range(3) - ] - return list(self._filtered_orientation_target) + error = _so3_log( + target_rotation @ self._filtered_orientation_target.T + ) + self._filtered_orientation_target = _project_rotation( + _so3_exp(self._orientation_filter_alpha * error) + @ self._filtered_orientation_target + ) + return self._filtered_orientation_target.copy() - def _limit_orientation_step(self, target_rpy: list[float]) -> tuple[list[float], bool]: + def _limit_orientation_step( + self, + target_rotation: np.ndarray, + ) -> tuple[np.ndarray, bool]: if self._last_sent_orientation is None: - return [_wrap_angle(value) for value in target_rpy], False + return _project_rotation(target_rotation), False - delta = [ - _angle_delta(target_rpy[i], self._last_sent_orientation[i]) - for i in range(3) - ] - distance = _norm(delta) + error = _so3_log( + target_rotation @ self._last_sent_orientation.T + ) + distance = float(np.linalg.norm(error)) max_step = self._max_orientation_speed * self._dt if distance <= max_step or distance <= 1e-9: - return [_wrap_angle(value) for value in target_rpy], False + return _project_rotation(target_rotation), False scale = max_step / distance - return [ - _wrap_angle(self._last_sent_orientation[i] + delta[i] * scale) - for i in range(3) - ], True + return _project_rotation( + _so3_exp(scale * error) @ self._last_sent_orientation + ), True def _clamp_workspace_with_flag(self, target: list[float]) -> tuple[list[float], bool]: clamped = [ @@ -777,7 +837,7 @@ class SingleArmVelocityTeleop(Node): def _estimate_command_velocity( self, target_xyz: list[float], - target_rpy: list[float], + target_rotation: np.ndarray, now: Time, ) -> list[float]: if self._last_sent_target is None or self._last_sent_orientation is None: @@ -788,13 +848,78 @@ class SingleArmVelocityTeleop(Node): measured_dt = (now - self._last_command_time).nanoseconds * 1e-9 if measured_dt > 1e-6: dt = measured_dt + angular_velocity = ( + _so3_log(target_rotation @ self._last_sent_orientation.T) / dt + ) return [ (target_xyz[i] - self._last_sent_target[i]) / dt for i in range(3) - ] + [ - _angle_delta(target_rpy[i], self._last_sent_orientation[i]) / dt - for i in range(3) + ] + angular_velocity.tolist() + + @staticmethod + def _timing_summary( + 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={int(np.count_nonzero(values > deadline_ms))}" + ) + return result + "]" + + 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 + + sample_count = len(self._timing_samples["total"]) + 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"], + ), ] + message = ( + f"{self._arm_name} timing n={sample_count} " + f"deadline={deadline_ms:.3f} ms | " + + " | ".join(summaries) + ) + for samples in self._timing_samples.values(): + samples.clear() + self.get_logger().info(message) def _fresh_joint_state(self) -> JointStateSnapshot | None: snapshot = self._adapter.get_latest_joint_state() @@ -813,7 +938,7 @@ class SingleArmVelocityTeleop(Node): def _sync_joint_feedback( self, snapshot: JointStateSnapshot, - ) -> ArmPose: + ) -> np.ndarray: current_pose = self._ik_solver.update_joint_state( snapshot.positions ) @@ -822,7 +947,7 @@ class SingleArmVelocityTeleop(Node): self._last_valid_joint_target = list(snapshot.positions) return current_pose - def _solve_joint_target(self, target_pose: ArmPose) -> list[float]: + def _solve_joint_target(self, target_pose: np.ndarray) -> list[float]: if self._last_valid_joint_target is None: raise RuntimeError("valid joint feedback has not been initialized") try: @@ -843,7 +968,7 @@ class SingleArmVelocityTeleop(Node): self._active = False self._controller_start = None self._controller_orientation_start = None - self._robot_start_pose = None + self._robot_start_transform = None self._filtered_target = None self._filtered_orientation_target = None self._last_sent_target = None @@ -867,14 +992,19 @@ class SingleArmVelocityTeleop(Node): return self._publish_debug(pose, pose, [0.0] * 6, False) - def _debug_pose_fallback(self) -> ArmPose | None: + def _debug_pose_fallback(self) -> np.ndarray | None: if self._last_current_pose is not None: return self._last_current_pose - if self._robot_start_pose is not None: - return self._robot_start_pose - if self._last_sent_target is not None: - rpy = self._last_sent_orientation or [0.0, 0.0, 0.0] - return ArmPose(*self._last_sent_target, *rpy) + if self._robot_start_transform is not None: + return self._robot_start_transform + if ( + self._last_sent_target is not None + and self._last_sent_orientation is not None + ): + return _make_transform( + self._last_sent_target, + self._last_sent_orientation, + ) return None def _send_joint_target(self, joints: list[float]) -> bool: @@ -892,8 +1022,8 @@ class SingleArmVelocityTeleop(Node): def _publish_debug( self, - raw_target_pose: ArmPose, - target_pose: ArmPose, + raw_target_pose: np.ndarray, + target_pose: np.ndarray, command_velocity: list[float], target_clamped: bool, ) -> None: @@ -924,14 +1054,15 @@ class SingleArmVelocityTeleop(Node): self._target_clamped_pub.publish(clamped_msg) @staticmethod - def _pose_msg(stamp, pose: ArmPose) -> PoseStamped: - qx, qy, qz, qw = _euler_to_quaternion(pose.rx, pose.ry, pose.rz) + def _pose_msg(stamp, pose: np.ndarray) -> PoseStamped: + transform = _make_transform(pose[:3, 3], pose[:3, :3]) + qx, qy, qz, qw = _matrix_to_quaternion(transform[:3, :3]) msg = PoseStamped() msg.header.stamp = stamp msg.header.frame_id = "rm_base" - msg.pose.position.x = float(pose.x) - msg.pose.position.y = float(pose.y) - msg.pose.position.z = float(pose.z) + msg.pose.position.x = float(transform[0, 3]) + msg.pose.position.y = float(transform[1, 3]) + msg.pose.position.z = float(transform[2, 3]) msg.pose.orientation.x = qx msg.pose.orientation.y = qy msg.pose.orientation.z = qz