Add URDF model for RM75-B OmniPicker with detailed link and joint specifications

README.md

@@ -12,7 +12,7 @@ PICO/XR 双手柄 UDP JSON
   -> /xr_rm/<arm_name>/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 姿态低通和死区。
+- `orientation_filter_alpha` / `orientation_deadband_rad`：按 SO(3) 最短旋转角处理的目标 TCP 姿态滤波和死区。
-- `max_orientation_speed`：目标姿态单帧步长限制对应的最大角速度。
+- `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 坐标的映射矩阵。

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。

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
+<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`、可操作度权重和所有既有安全
+限制未被改变。

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`。

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
+<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. 未改变工作空间、圆柱、关节限制、指令超时、安全停止或奇异点设置。

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]

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]

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_min: [-0.70, -0.70, 0.10]
-    workspace_max: [0.60, 0.70, 0.55]
+    workspace_max: [0.70, 0.70, 0.75]
-    cyl_radius_limit: [0.10, 0.70]
+    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

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_omnipicker",
-        "RM75-B.urdf",
+        "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"),

xr_rm_teleop/models/rm75_omnipicker/urdf/RM75-B_OmniPicker_fixed.urdf

@@ -0,0 +1,503 @@
+<?xml version='1.0' encoding='UTF-8'?>
+<robot name="RM75_B_OmniPicker_fixed">
+  <link name="base_link">
+    <inertial>
+      <origin xyz="0.00049987 5.2709E-05 0.060019" rpy="0 0 0"/>
+      <mass value="1.862"/>
+      <inertia ixx="0.0017232" ixy="-3.1058E-06" ixz="-3.7924E-05" iyy="0.0017051" iyz="1.3691E-06" izz="0.00090158"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="package://xr_rm_teleop/models/rm75_omnipicker/meshes/rm75/base_link.STL"/>
+      </geometry>
+      <material name="">
+        <color rgba="1 1 1 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="package://xr_rm_teleop/models/rm75_omnipicker/meshes/rm75/base_link.STL"/>
+      </geometry>
+    </collision>
+  </link>
+  <link name="link_1">
+    <inertial>
+      <origin xyz="0.000241 -0.013273 -0.00995" rpy="0 0 0"/>
+      <mass value="1.574"/>
+      <inertia ixx="0.002487573" ixy="0.000009663" ixz="-0.000007909" iyy="0.002321038" iyz="0.000179393" izz="0.001450554"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="package://xr_rm_teleop/models/rm75_omnipicker/meshes/rm75/link_1.STL"/>
+      </geometry>
+      <material name="">
+        <color rgba="1 1 1 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="package://xr_rm_teleop/models/rm75_omnipicker/meshes/rm75/link_1.STL"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="joint_1" type="revolute">
+    <origin xyz="0 0 0.2405" rpy="0 0 0"/>
+    <parent link="base_link"/>
+    <child link="link_1"/>
+    <axis xyz="0 0 1"/>
+    <limit lower="-3.106" upper="3.106" effort="60" velocity="3.14"/>
+  </joint>
+  <link name="link_2">
+    <inertial>
+      <origin xyz="-0.000357 -0.106789 0.005329" rpy="0 0 0"/>
+      <mass value="1.217"/>
+      <inertia ixx="0.003494121" ixy="0.000002921" ixz="-0.000005613" iyy="0.000892721" iyz="-0.000583884" izz="0.003444080"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="package://xr_rm_teleop/models/rm75_omnipicker/meshes/rm75/link_2.STL"/>
+      </geometry>
+      <material name="">
+        <color rgba="1 1 1 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="package://xr_rm_teleop/models/rm75_omnipicker/meshes/rm75/link_2.STL"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="joint_2" type="revolute">
+    <origin xyz="0 0 0" rpy="-1.5708 0 0"/>
+    <parent link="link_1"/>
+    <child link="link_2"/>
+    <axis xyz="0 0 1"/>
+    <limit lower="-2.2689" upper="2.2689" effort="60" velocity="3.14"/>
+  </joint>
+  <link name="link_3">
+    <inertial>
+      <origin xyz="0.000003 -0.01398 -0.011324" rpy="0 0 0"/>
+      <mass value="1.11"/>
+      <inertia ixx="0.001836663" ixy="0.000002259" ixz="-0.000004216" iyy="0.001498875" iyz="0.000037167" izz="0.001062545"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="package://xr_rm_teleop/models/rm75_omnipicker/meshes/rm75/link_3.STL"/>
+      </geometry>
+      <material name="">
+        <color rgba="1 1 1 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="package://xr_rm_teleop/models/rm75_omnipicker/meshes/rm75/link_3.STL"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="joint_3" type="revolute">
+    <origin xyz="0 -0.256 0" rpy="1.5708 0 0"/>
+    <parent link="link_2"/>
+    <child link="link_3"/>
+    <axis xyz="0 0 1"/>
+    <limit lower="-3.106" upper="3.106" effort="30" velocity="3.14"/>
+  </joint>
+  <link name="link_4">
+    <inertial>
+      <origin xyz="-0.000005 -0.084658 0.004747" rpy="0 0 0"/>
+      <mass value="0.685"/>
+      <inertia ixx="0.001282444" ixy="-0.000000551" ixz="-0.000000630" iyy="0.000373013" iyz="-0.000232084" izz="0.001256177"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="package://xr_rm_teleop/models/rm75_omnipicker/meshes/rm75/link_4.STL"/>
+      </geometry>
+      <material name="">
+        <color rgba="1 1 1 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="package://xr_rm_teleop/models/rm75_omnipicker/meshes/rm75/link_4.STL"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="joint_4" type="revolute">
+    <origin xyz="0 0 0" rpy="-1.5708 0 0"/>
+    <parent link="link_3"/>
+    <child link="link_4"/>
+    <axis xyz="0 0 1"/>
+    <limit lower="-2.356" upper="2.356" effort="30" velocity="3.14"/>
+  </joint>
+  <link name="link_5">
+    <inertial>
+      <origin xyz="0.000078 -0.012937 -0.008781" rpy="0 0 0"/>
+      <mass value="0.619"/>
+      <inertia ixx="0.000627336" ixy="0.000001636" ixz="-0.000001345" iyy="0.000542455" iyz="0.000034970" izz="0.000370291"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="package://xr_rm_teleop/models/rm75_omnipicker/meshes/rm75/link_5.STL"/>
+      </geometry>
+      <material name="">
+        <color rgba="1 1 1 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="package://xr_rm_teleop/models/rm75_omnipicker/meshes/rm75/link_5.STL"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="joint_5" type="revolute">
+    <origin xyz="0 -0.21 0" rpy="1.5708 0 0"/>
+    <parent link="link_4"/>
+    <child link="link_5"/>
+    <axis xyz="0 0 1"/>
+    <limit lower="-3.106" upper="3.106" effort="10" velocity="3.14"/>
+  </joint>
+  <link name="link_6">
+    <inertial>
+      <origin xyz="-0.000014 -0.078524 0.002819" rpy="0 0 0"/>
+      <mass value="0.602"/>
+      <inertia ixx="0.000780774" ixy="-0.000000121" ixz="-0.000000469" iyy="0.000289973" iyz="-0.000120513" izz="0.000763955"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="package://xr_rm_teleop/models/rm75_omnipicker/meshes/rm75/link_6.STL"/>
+      </geometry>
+      <material name="">
+        <color rgba="1 1 1 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="package://xr_rm_teleop/models/rm75_omnipicker/meshes/rm75/link_6.STL"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="joint_6" type="revolute">
+    <origin xyz="0 0 0" rpy="-1.5708 0 0"/>
+    <parent link="link_5"/>
+    <child link="link_6"/>
+    <axis xyz="0 0 1"/>
+    <limit lower="-2.234" upper="2.234" effort="10" velocity="3.14"/>
+  </joint>
+  <link name="link_7">
+    <inertial>
+      <origin xyz="0.001094 -0.000077 -0.010119" rpy="0 0 0"/>
+      <mass value="0.107"/>
+      <inertia ixx="0.000044123" ixy="-0.000000064" ixz="0.0000003" iyy="0.000035078" iyz="-0.000000029" izz="0.000065445"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="package://xr_rm_teleop/models/rm75_omnipicker/meshes/rm75/link_7.STL"/>
+      </geometry>
+      <material name="">
+        <color rgba="1 1 1 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="package://xr_rm_teleop/models/rm75_omnipicker/meshes/rm75/link_7.STL"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="joint_7" type="revolute">
+    <origin xyz="0 -0.144 0" rpy="1.5708 0 0"/>
+    <parent link="link_6"/>
+    <child link="link_7"/>
+    <axis xyz="0 0 1"/>
+    <limit lower="-6.28" upper="6.28" effort="10" velocity="3.14"/>
+  </joint>
+  <!-- RM75 end-flange alias. link_7 is treated as the tool mounting frame. -->
+  <link name="rm75_flange"/>
+  <joint name="rm75_link7_to_flange" type="fixed">
+    <origin xyz="0 0 0" rpy="0 0 0"/>
+    <parent link="link_7"/>
+    <child link="rm75_flange"/>
+  </joint>
+  <!-- OmniPicker mounting transform. Adjust xyz/rpy here if an adapter plate or different clocking is used. -->
+  <joint name="rm75_flange_to_omnipicker" type="fixed">
+    <origin xyz="0 0 0" rpy="0 0 0"/>
+    <parent link="rm75_flange"/>
+    <child link="omnipicker_base_link"/>
+  </joint>
+  <link name="omnipicker_base_link">
+    <inertial>
+      <origin xyz="-0.00005520 1.2341E-05 0.03296193" rpy="0 0 0"/>
+      <mass value="0.25641368"/>
+      <inertia ixx="4.6351E-04" ixy="-1.0E-08" ixz="-1.04E-06" iyy="4.4525E-04" iyz="-5.00E-08" izz="1.0438E-04"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://xr_rm_teleop/models/rm75_omnipicker/meshes/omnipicker/base_link.STL"/>
+      </geometry>
+      <material name="">
+        <color rgba="0.89804 0.91765 0.92941 1"/>
+      </material>
+    </visual>
+  </link>
+  <link name="omnipicker_hand_narrow1_Link">
+    <inertial>
+      <origin xyz="0.0094685 0.0068806 6.5437E-05" rpy="0 0 0"/>
+      <mass value="0.025428"/>
+      <inertia ixx="1.9574E-06" ixy="-4.2911E-07" ixz="-3.7111E-11" iyy="2.3919E-06" iyz="-3.008E-10" izz="1.5501E-06"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="package://xr_rm_teleop/models/rm75_omnipicker/meshes/omnipicker/narrow1_Link.STL"/>
+      </geometry>
+      <material name="">
+        <color rgba="0.75294 0.75294 0.75294 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="package://xr_rm_teleop/models/rm75_omnipicker/meshes/omnipicker/narrow1_Link.STL"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="omnipicker_hand_narrow1_joint" type="fixed">
+    <origin xyz="0 -0.0195 0.0565" rpy="-2.9951 -1.5708 -0.15964"/>
+    <parent link="omnipicker_base_link"/>
+    <child link="omnipicker_hand_narrow1_Link"/>
+  </joint>
+  <link name="omnipicker_hand_narrow2_Link">
+    <inertial>
+      <origin xyz="0.0088027 -0.007035 1.6424E-05" rpy="0 0 0"/>
+      <mass value="0.0040132"/>
+      <inertia ixx="1.0307E-07" ixy="5.7851E-08" ixz="9.5801E-11" iyy="1.1385E-07" iyz="-2.81E-11" izz="1.7009E-07"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="package://xr_rm_teleop/models/rm75_omnipicker/meshes/omnipicker/narrow2_Link.STL"/>
+      </geometry>
+      <material name="">
+        <color rgba="0.75294 0.75294 0.75294 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="package://xr_rm_teleop/models/rm75_omnipicker/meshes/omnipicker/narrow2_Link.STL"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="omnipicker_hand_narrow2_joint" type="fixed">
+    <origin xyz="0.030852 0.018551 0" rpy="0 0 0"/>
+    <parent link="omnipicker_hand_narrow1_Link"/>
+    <child link="omnipicker_hand_narrow2_Link"/>
+  </joint>
+  <link name="omnipicker_hand_narrow3_Link">
+    <inertial>
+      <origin xyz="0.012508 -0.0079729 9.4339E-05" rpy="0 0 0"/>
+      <mass value="0.018029"/>
+      <inertia ixx="1.1403E-06" ixy="5.5159E-07" ixz="-4.0096E-13" iyy="2.5704E-06" iyz="1.0951E-12" izz="2.3252E-06"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="package://xr_rm_teleop/models/rm75_omnipicker/meshes/omnipicker/narrow3_Link.STL"/>
+      </geometry>
+      <material name="">
+        <color rgba="0.75294 0.75294 0.75294 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="package://xr_rm_teleop/models/rm75_omnipicker/meshes/omnipicker/narrow3_Link.STL"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="omnipicker_hand_narrow3_joint" type="fixed">
+    <origin xyz="0.018118 -0.01574 0" rpy="0 0 0"/>
+    <parent link="omnipicker_hand_narrow2_Link"/>
+    <child link="omnipicker_hand_narrow3_Link"/>
+  </joint>
+  <link name="omnipicker_hand_narrow4_Link">
+  </link>
+  <joint name="omnipicker_hand_narrow4_joint" type="fixed">
+    <origin xyz="0 -0.0104 0" rpy="0 0 0"/>
+    <parent link="omnipicker_hand_narrow3_Link"/>
+    <child link="omnipicker_hand_narrow4_Link"/>
+    <axis xyz="0 0 1"/>
+    <limit lower="-3.14" upper="3.14" effort="0" velocity="0"/>
+  </joint>
+  <link name="omnipicker_hand_narrow_loop_Link">
+    <inertial>
+      <origin xyz="0.014869 -0.0036066 0.00029307" rpy="0 0 0"/>
+      <mass value="0.022591"/>
+      <inertia ixx="4.3916E-06" ixy="1.114E-07" ixz="-4.9655E-12" iyy="4.737E-06" iyz="1.7121E-11" izz="6.0445E-07"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="package://xr_rm_teleop/models/rm75_omnipicker/meshes/omnipicker/narrow_loop_Link.STL"/>
+      </geometry>
+      <material name="">
+        <color rgba="0.75294 0.75294 0.75294 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="package://xr_rm_teleop/models/rm75_omnipicker/meshes/omnipicker/narrow_loop_Link.STL"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="omnipicker_hand_narrow_loop_joint" type="fixed">
+    <origin xyz="0 -0.021633 0.07387" rpy="-2.9951 -1.5708 -0.15964"/>
+    <parent link="omnipicker_base_link"/>
+    <child link="omnipicker_hand_narrow_loop_Link"/>
+  </joint>
+  <link name="omnipicker_hand_wide1_Link">
+    <inertial>
+      <origin xyz="0.0095051 -0.0068479 6.8268E-05" rpy="0 0 0"/>
+      <mass value="0.025428"/>
+      <inertia ixx="1.9565E-06" ixy="4.2798E-07" ixz="2.3844E-10" iyy="2.3928E-06" iyz="-1.4454E-10" izz="1.5501E-06"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="package://xr_rm_teleop/models/rm75_omnipicker/meshes/omnipicker/wide1_Link.STL"/>
+      </geometry>
+      <material name="">
+        <color rgba="0.75294 0.75294 0.75294 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="package://xr_rm_teleop/models/rm75_omnipicker/meshes/omnipicker/wide1_Link.STL"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="omnipicker_hand_wide1_joint" type="fixed">
+    <origin xyz="0 0.0195 0.0565" rpy="-2.9951 -1.5708 -0.15964"/>
+    <parent link="omnipicker_base_link"/>
+    <child link="omnipicker_hand_wide1_Link"/>
+  </joint>
+  <link name="omnipicker_hand_wide2_Link">
+    <inertial>
+      <origin xyz="0.0088027 0.007035 -1.6424E-05" rpy="0 0 0"/>
+      <mass value="0.0040132"/>
+      <inertia ixx="1.0307E-07" ixy="-5.7851E-08" ixz="-9.58E-11" iyy="1.1385E-07" iyz="-2.81E-11" izz="1.7009E-07"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="package://xr_rm_teleop/models/rm75_omnipicker/meshes/omnipicker/wide2_Link.STL"/>
+      </geometry>
+      <material name="">
+        <color rgba="0.75294 0.75294 0.75294 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="package://xr_rm_teleop/models/rm75_omnipicker/meshes/omnipicker/wide2_Link.STL"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="omnipicker_hand_wide2_joint" type="fixed">
+    <origin xyz="0.030852 -0.018551 0" rpy="0 0 0"/>
+    <parent link="omnipicker_hand_wide1_Link"/>
+    <child link="omnipicker_hand_wide2_Link"/>
+  </joint>
+  <link name="omnipicker_hand_wide3_Link">
+    <inertial>
+      <origin xyz="0.016206 0.0094593 4.7668E-05" rpy="0 0 0"/>
+      <mass value="0.035835"/>
+      <inertia ixx="8.5056E-06" ixy="-1.1363E-06" ixz="-4.2908E-11" iyy="1.1235E-05" iyz="-2.9251E-11" izz="4.6309E-06"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="package://xr_rm_teleop/models/rm75_omnipicker/meshes/omnipicker/wide3_Link.STL"/>
+      </geometry>
+      <material name="">
+        <color rgba="0.75294 0.75294 0.75294 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="package://xr_rm_teleop/models/rm75_omnipicker/meshes/omnipicker/wide3_Link.STL"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="omnipicker_hand_wide3_joint" type="fixed">
+    <origin xyz="0.018118 0.01574 0" rpy="0 0 0"/>
+    <parent link="omnipicker_hand_wide2_Link"/>
+    <child link="omnipicker_hand_wide3_Link"/>
+  </joint>
+  <link name="omnipicker_hand_wide4_Link">
+  </link>
+  <joint name="omnipicker_hand_wide4_joint" type="fixed">
+    <origin xyz="0 0.0104 0" rpy="0 0 0"/>
+    <parent link="omnipicker_hand_wide3_Link"/>
+    <child link="omnipicker_hand_wide4_Link"/>
+    <axis xyz="0 0 1"/>
+    <limit lower="-3.14" upper="3.14" effort="0" velocity="0"/>
+  </joint>
+  <link name="omnipicker_hand_wide_loop_Link">
+    <inertial>
+      <origin xyz="0.016268 0.0040555 0.00030323" rpy="0 0 0"/>
+      <mass value="0.025142"/>
+      <inertia ixx="5.887E-06" ixy="-1.1234E-07" ixz="2.1954E-11" iyy="6.236E-06" iyz="-9.473E-12" izz="6.2389E-07"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="package://xr_rm_teleop/models/rm75_omnipicker/meshes/omnipicker/wide_loop_Link.STL"/>
+      </geometry>
+      <material name="">
+        <color rgba="0.75294 0.75294 0.75294 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="package://xr_rm_teleop/models/rm75_omnipicker/meshes/omnipicker/wide_loop_Link.STL"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="omnipicker_hand_wide_loop_joint" type="fixed">
+    <origin xyz="0 0.021633 0.07387" rpy="-2.9951 -1.5708 -0.15964"/>
+    <parent link="omnipicker_base_link"/>
+    <child link="omnipicker_hand_wide_loop_Link"/>
+  </joint>
+  <link name="omnipicker_mount_frame"/>
+  <joint name="omnipicker_base_to_mount_frame" type="fixed">
+    <origin xyz="0 0 0" rpy="0 0 0"/>
+    <parent link="omnipicker_base_link"/>
+    <child link="omnipicker_mount_frame"/>
+  </joint>
+  <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>
+</robot>

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,

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": (
+    "left": [-79.55, -9.99, 71.01, 101.45, 95.07, -84.47, -74.52],
-        [-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],
-        [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],
-    ),
 }
 
 
-def angle_error(actual: list[float], target: list[float]) -> float:
+def rotation_z(angle: float) -> np.ndarray:
-    deltas = [
+    cosine = math.cos(angle)
-        math.atan2(math.sin(a - b), math.cos(a - b))
+    sine = math.sin(angle)
-        for a, b in zip(actual, target)
+    return np.asarray(
-    ]
+        [
-    return math.sqrt(sum(value * value for value in deltas))
+            [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():
+    for arm, joint_degrees in CASES.items():
-        solver = PlacoIkSolver(str(urdf_path), tool_pose, 1.0 / 90.0)
+        initial_joints = np.deg2rad(joint_degrees)
-        joints = np.deg2rad(joint_degrees).tolist()
+        drift_solver = PlacoIkSolver(str(urdf_path), 1.0 / 125.0)
-        current = solver.update_joint_state(joints)
+        joints = initial_joints.tolist()
-        target = ArmPose(
+        stationary_target = drift_solver.update_joint_state(joints)
-            current.x + 0.01,
+        flange = drift_solver._robot.get_T_world_frame("link_7")
-            current.y,
+        flange_to_tcp = np.linalg.inv(flange) @ stationary_target
-            current.z,
+        assert np.allclose(flange_to_tcp[:3, 3], [0.0, 0.0, 0.16])
-            current.rx,
+        assert np.allclose(flange_to_tcp[:3, :3], np.eye(3))
-            current.ry,
+        for _ in range(250):
-            current.rz + 0.05,
+            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(
+        position_error = np.linalg.norm(actual[:3, 3] - target[:3, 3])
-            np.asarray(actual.xyz()) - np.asarray(target.xyz())
+        orientation_error = angle_error(actual[:3, :3], target[:3, :3])
-        )
-        orientation_error = angle_error(actual.rpy(), target.rpy())
         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)}"
         )
 
 

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.realman_adapter import JointStateSnapshot
-from xr_rm_teleop.single_arm_velocity_teleop import SingleArmVelocityTeleop
+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:

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)
+    teleop._robot_start_transform = np.eye(4)
-    xr_roll = _euler_to_quaternion(0.2, 0.0, 0.0)
+    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._dt = 1.0 / 125.0
-    teleop._last_sent_orientation = [0.0, 0.0, 0.0]
+    teleop._last_sent_orientation = np.eye(3)
-    teleop._filtered_orientation_target = [0.0, 0.0, 0.0]
+    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])
+    target = _so3_exp(np.asarray([0.2, 0.0, 0.0]))
-    assert_angles_close(filtered, [0.1, 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(
+    teleop._ik_solver = SimpleNamespace(update_joint_state=lambda joints: np.eye(4))
-        update_joint_state=lambda joints: ArmPose(0.3, 0.0, 0.2)
-    )
     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])

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,
+    _validated_transform,
-    _tool_pose_to_transform,
-    _transform_to_arm_pose,
 )
-from xr_rm_teleop.realman_adapter import ArmPose
 
 
-def test_tool_offset_rotates_with_flange_and_roundtrips() -> None:
+def test_fixed_urdf_has_seven_moving_joints_and_omnipicker_tcp() -> None:
-    flange_pose = ArmPose(0.30, -0.10, 0.20, 0.0, math.pi / 2.0, 0.0)
+    urdf_path = (
-    tool_pose = [0.0, 0.0, 0.19, 0.0, 0.0, 0.0, 1.0]
+        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)
+    assert moving_joint_names == [f"joint_{index}" for index in range(1, 8)]
-    flange_to_tool = _tool_pose_to_transform(tool_pose)
+    assert all(
-    base_to_tool = base_to_flange @ flange_to_tool
+        filename.startswith(
-    recovered_flange = base_to_tool @ np.linalg.inv(flange_to_tool)
+            "package://xr_rm_teleop/models/rm75_omnipicker/meshes/"
-
+        )
-    assert base_to_tool[:3, 3] == pytest.approx([0.49, -0.10, 0.20])
+        for filename in mesh_filenames
-    assert recovered_flange == pytest.approx(base_to_flange)
+    )
+    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:
+def test_validated_transform_accepts_finite_se3_and_returns_a_copy() -> None:
-    expected = ArmPose(0.25, -0.30, 0.40, 0.20, -0.30, 0.40)
+    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 == pytest.approx(transform)
-    assert actual.rpy() == pytest.approx(expected.rpy())
+    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:

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:
+def _validated_transform(transform: np.ndarray) -> np.ndarray:
-    cr, sr = math.cos(roll), math.sin(roll)
+    values = np.asarray(transform, dtype=float)
-    cp, sp = math.cos(pitch), math.sin(pitch)
+    if values.shape != (4, 4) or not np.isfinite(values).all():
-    cy, sy = math.cos(yaw), math.sin(yaw)
+        raise ValueError("target transform must be a finite 4x4 matrix")
-    return np.array(
+    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")
-            [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,
-    )
 
+    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]:
+    result = values.copy()
-    pitch = math.asin(-float(np.clip(rotation[2, 0], -1.0, 1.0)))
+    result[:3, :3] = projected
-    if abs(math.cos(pitch)) > 1e-9:
+    return result
-        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,
-    )
 
 
 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()
+            self._frame_task.T_world_frame = base_to_tool.copy()
-        base_to_tool = base_to_flange @ self._tool_transform
+        return base_to_tool.copy()
-        return _transform_to_arm_pose(base_to_tool)
 
-    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 = (
+        self._frame_task.T_world_frame = _validated_transform(target_tool_pose)
-            _arm_pose_to_transform(target_tool_pose) @ self._tool_inverse
-        )
         self._solver.solve(True)
         result = np.asarray(
             self._robot.state.q[RM75_Q_SLICE],

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]:
+) -> np.ndarray:
-    x, y, z, w = quat
+    x, y, z, w = _normalize_quaternion(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
     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 [
+    return np.asarray(
-        1.0 - 2.0 * (yy + zz),
+        [
-        2.0 * (xy - wz),
+            [1.0 - 2.0 * (yy + zz), 2.0 * (xy - wz), 2.0 * (xz + wy)],
-        2.0 * (xz + wy),
+            [2.0 * (xy + wz), 1.0 - 2.0 * (xx + zz), 2.0 * (yz - wx)],
-        2.0 * (xy + wz),
+            [2.0 * (xz - wy), 2.0 * (yz + wx), 1.0 - 2.0 * (xx + yy)],
-        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]:
+def _matrix_to_quaternion(
-    return [
+    rotation: np.ndarray,
-        sum(left[row * 3 + k] * right[k * 3 + col] for k in range(3))
+) -> tuple[float, float, float, float]:
-        for row in range(3)
+    matrix = _project_rotation(rotation)
-        for col in range(3)
+    trace = float(np.trace(matrix))
-    ]
+    if trace > 0.0:
-
+        scale = math.sqrt(trace + 1.0) * 2.0
-
+        quaternion = (
-def _matrix_transpose(matrix: list[float]) -> list[float]:
+            (matrix[2, 1] - matrix[1, 2]) / scale,
-    return [
+            (matrix[0, 2] - matrix[2, 0]) / scale,
-        matrix[0], matrix[3], matrix[6],
+            (matrix[1, 0] - matrix[0, 1]) / scale,
-        matrix[1], matrix[4], matrix[7],
+            0.25 * scale,
-        matrix[2], matrix[5], matrix[8],
+        )
-    ]
+    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 = (
-def _euler_to_quaternion(roll: float, pitch: float, yaw: float) -> tuple[float, float, float, float]:
+            0.25 * scale,
-    cy = math.cos(yaw * 0.5)
+            (matrix[0, 1] + matrix[1, 0]) / scale,
-    sy = math.sin(yaw * 0.5)
+            (matrix[0, 2] + matrix[2, 0]) / scale,
-    cp = math.cos(pitch * 0.5)
+            (matrix[2, 1] - matrix[1, 2]) / scale,
-    sp = math.sin(pitch * 0.5)
+        )
-    cr = math.cos(roll * 0.5)
+    elif matrix[1, 1] > matrix[2, 2]:
-    sr = math.sin(roll * 0.5)
+        scale = math.sqrt(1.0 + matrix[1, 1] - matrix[0, 0] - matrix[2, 2]) * 2.0
-
+        quaternion = (
-    qw = cr * cp * cy + sr * sp * sy
+            (matrix[0, 1] + matrix[1, 0]) / scale,
-    qx = sr * cp * cy - cr * sp * sy
+            0.25 * scale,
-    qy = cr * sp * cy + sr * cp * sy
+            (matrix[1, 2] + matrix[2, 1]) / scale,
-    qz = cr * cp * sy - sr * sp * cy
+            (matrix[0, 2] - matrix[2, 0]) / scale,
-    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])
     else:
-        roll = math.atan2(-matrix[5], matrix[4])
+        scale = math.sqrt(1.0 + matrix[2, 2] - matrix[0, 0] - matrix[1, 1]) * 2.0
-        yaw = 0.0
+        quaternion = (
-    return _wrap_angle(roll), _wrap_angle(pitch), _wrap_angle(yaw)
+            (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]:
+def _so3_log(rotation: np.ndarray) -> np.ndarray:
-    return _matrix_to_euler(_quaternion_to_matrix(quat))
+    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(
+        raw_target_pose = _make_transform(
-            x=raw_target_xyz[0],
+            raw_target_xyz,
-            y=raw_target_xyz[1],
+            raw_target_orientation,
-            z=raw_target_xyz[2],
-            rx=raw_target_rpy[0],
-            ry=raw_target_rpy[1],
-            rz=raw_target_rpy[2],
         )
-        target_pose = ArmPose(
+        target_pose = _make_transform(
-            x=sent_target[0],
+            sent_target,
-            y=sent_target[1],
+            sent_orientation,
-            z=sent_target[2],
-            rx=sent_orientation[0],
-            ry=sent_orientation[1],
-            rz=sent_orientation[2],
         )
 
         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],
+            robot_start_xyz[0] + self._scale * robot_delta[0],
-            self._robot_start_pose.y + self._scale * robot_delta[1],
+            robot_start_xyz[1] + self._scale * robot_delta[1],
-            self._robot_start_pose.z + self._scale * robot_delta[2],
+            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]:
+    ) -> np.ndarray:
-        assert self._robot_start_pose is not None
+        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(
+        xr_delta_matrix = (
-            controller_quat,
+            _quaternion_to_matrix(controller_quat)
-            _quaternion_conjugate(self._controller_orientation_start),
+            @ _quaternion_to_matrix(self._controller_orientation_start).T
         )
-        xr_delta_matrix = _quaternion_to_matrix(xr_delta_quat)
+        mapping = np.asarray(self._xr_to_robot_matrix, dtype=float).reshape(3, 3)
-        matrix = self._xr_to_robot_matrix
+        robot_delta = _project_rotation(
-        robot_delta_matrix = _matrix_multiply(
+            mapping @ xr_delta_matrix @ mapping.T
-            _matrix_multiply(matrix, xr_delta_matrix),
-            _matrix_transpose(matrix),
         )
-        robot_start_matrix = _euler_to_matrix(
+        rotation_vector = _so3_log(robot_delta)
-            self._robot_start_pose.rx,
+        rotation_vector = np.asarray(
-            self._robot_start_pose.ry,
+            [
-            self._robot_start_pose.rz,
+                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]
+            return _project_rotation(target_rotation)
-        delta = [
+        error = _so3_log(
-            _angle_delta(target_rpy[i], self._last_sent_orientation[i])
+            target_rotation @ self._last_sent_orientation.T
-            for i in range(3)
+        )
-        ]
+        if np.linalg.norm(error) < self._orientation_deadband_rad:
-        if _norm(delta) < self._orientation_deadband_rad:
+            return self._last_sent_orientation.copy()
-            return list(self._last_sent_orientation)
+        return _project_rotation(target_rotation)
-        return [_wrap_angle(value) for value in target_rpy]
 
-    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]
+            self._filtered_orientation_target = _project_rotation(target_rotation)
-            return list(self._filtered_orientation_target)
+            return self._filtered_orientation_target.copy()
 
-        self._filtered_orientation_target = [
+        error = _so3_log(
-            _wrap_angle(
+            target_rotation @ self._filtered_orientation_target.T
-                self._filtered_orientation_target[i]
+        )
-                + self._orientation_filter_alpha
+        self._filtered_orientation_target = _project_rotation(
-                * _angle_delta(target_rpy[i], self._filtered_orientation_target[i])
+            _so3_exp(self._orientation_filter_alpha * error)
-            )
+            @ self._filtered_orientation_target
-            for i in range(3)
+        )
-        ]
+        return self._filtered_orientation_target.copy()
-        return list(self._filtered_orientation_target)
 
-    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 = [
+        error = _so3_log(
-            _angle_delta(target_rpy[i], self._last_sent_orientation[i])
+            target_rotation @ self._last_sent_orientation.T
-            for i in range(3)
+        )
-        ]
+        distance = float(np.linalg.norm(error))
-        distance = _norm(delta)
         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 [
+        return _project_rotation(
-            _wrap_angle(self._last_sent_orientation[i] + delta[i] * scale)
+            _so3_exp(scale * error) @ self._last_sent_orientation
-            for i in range(3)
+        ), True
-        ], 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)
-        ] + [
+        ] + angular_velocity.tolist()
-            _angle_delta(target_rpy[i], self._last_sent_orientation[i]) / dt
+
-            for i in range(3)
+    @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:
+        if self._robot_start_transform is not None:
-            return self._robot_start_pose
+            return self._robot_start_transform
-        if self._last_sent_target is not None:
+        if (
-            rpy = self._last_sent_orientation or [0.0, 0.0, 0.0]
+            self._last_sent_target is not None
-            return ArmPose(*self._last_sent_target, *rpy)
+            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,
+        raw_target_pose: np.ndarray,
-        target_pose: ArmPose,
+        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:
+    def _pose_msg(stamp, pose: np.ndarray) -> PoseStamped:
-        qx, qy, qz, qw = _euler_to_quaternion(pose.rx, pose.ry, pose.rz)
+        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.x = float(transform[0, 3])
-        msg.pose.position.y = float(pose.y)
+        msg.pose.position.y = float(transform[1, 3])
-        msg.pose.position.z = float(pose.z)
+        msg.pose.position.z = float(transform[2, 3])
         msg.pose.orientation.x = qx
         msg.pose.orientation.y = qy
         msg.pose.orientation.z = qz