Add URDF model for RM75-B OmniPicker with detailed link and joint specifications
This commit is contained in:
@@ -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 姿态低通和死区。
|
||||
- `max_orientation_speed`：目标姿态单帧步长限制对应的最大角速度。
|
||||
- `orientation_filter_alpha` / `orientation_deadband_rad`：按 SO(3) 最短旋转角处理的目标 TCP 姿态滤波和死区。
|
||||
- `max_orientation_speed`：目标 TCP 姿态沿 SO(3) 最短路径的最大角速度，当前为 `0.5 rad/s`。
|
||||
- `workspace_min` / `workspace_max`：笛卡尔工作空间边界。
|
||||
- `cyl_radius_limit`：基座圆柱半径限制。
|
||||
- `xr_to_robot_matrix`：`/xr/*_controller` Project 位移到 RM75 base 坐标的映射矩阵。
|
||||
|
||||
@@ -0,0 +1,154 @@
|
||||
# RM75 Control Timing Stats Implementation Plan
|
||||
|
||||
> **For agentic workers:** REQUIRED SUB-SKILL: Use superpowers:subagent-driven-development (recommended) or superpowers:executing-plans to implement this plan task-by-task. Steps use checkbox (`- [ ]`) syntax for tracking.
|
||||
|
||||
**Goal:** 在 Grip 激活期间每约 5 秒向 `arm_debug.launch.py` 终端输出一次控制链路耗时统计。
|
||||
|
||||
**Architecture:** 在现有 `SingleArmVelocityTeleop` 控制回调内使用单调高精度时钟记录实际周期、控制路径总耗时、QP、关节发送和反馈年龄。节点保存一个固定长度样本窗口，满窗后用 NumPy 计算 mean/P95/P99/max，输出一条 ROS 日志并清空窗口。
|
||||
|
||||
**Tech Stack:** Python 3.10、ROS2 Humble `rclpy`、NumPy、pytest。
|
||||
|
||||
---
|
||||
|
||||
### Task 1: 控制周期统计
|
||||
|
||||
**Files:**
|
||||
- Modify: `xr_rm_teleop/test/test_joint_control.py`
|
||||
- Modify: `xr_rm_teleop/xr_rm_teleop/single_arm_velocity_teleop.py`
|
||||
|
||||
- [x] **Step 1: 写失败测试**
|
||||
|
||||
在 `test_joint_control.py` 添加确定性两样本窗口测试：
|
||||
|
||||
```python
|
||||
def test_timing_stats_logs_summary_and_clears_window() -> None:
|
||||
messages = []
|
||||
teleop = object.__new__(SingleArmVelocityTeleop)
|
||||
teleop._arm_name = "right_rm75"
|
||||
teleop._dt = 0.008
|
||||
teleop._timing_stats_window = 2
|
||||
teleop._timing_samples = {
|
||||
name: []
|
||||
for name in ("period", "total", "qp", "send", "feedback_age")
|
||||
}
|
||||
teleop.get_logger = lambda: SimpleNamespace(
|
||||
info=lambda message: messages.append(message)
|
||||
)
|
||||
|
||||
teleop._record_timing_sample(7.0, 6.0, 1.0, 0.5, 3.0)
|
||||
assert messages == []
|
||||
|
||||
teleop._record_timing_sample(9.0, 10.0, 2.0, 0.7, 4.0)
|
||||
|
||||
assert len(messages) == 1
|
||||
assert "right_rm75 timing n=2 deadline=8.000 ms" in messages[0]
|
||||
assert "period[n=2 mean=8.000 p95=8.900 p99=8.980 max=9.000 ms overruns=1]" in messages[0]
|
||||
assert "total[n=2 mean=8.000 p95=9.800 p99=9.960 max=10.000 ms overruns=1]" in messages[0]
|
||||
assert "qp[n=2" in messages[0]
|
||||
assert "send[n=2" in messages[0]
|
||||
assert "feedback_age[n=2" in messages[0]
|
||||
assert all(not samples for samples in teleop._timing_samples.values())
|
||||
```
|
||||
|
||||
- [x] **Step 2: 确认测试因功能缺失而失败**
|
||||
|
||||
在工作空间根目录运行：
|
||||
|
||||
```bash
|
||||
source /opt/ros/humble/setup.bash
|
||||
PYTHONPATH=src/xr_rm_teleop pytest -q \
|
||||
src/xr_rm_teleop/test/test_joint_control.py::test_timing_stats_logs_summary_and_clears_window
|
||||
```
|
||||
|
||||
预期：失败并提示 `SingleArmVelocityTeleop` 没有 `_record_timing_sample`。
|
||||
|
||||
- [x] **Step 3: 实现最小统计逻辑**
|
||||
|
||||
在节点初始化中创建约 5 秒的窗口：
|
||||
|
||||
```python
|
||||
self._timing_stats_window = max(1, int(round(5.0 / self._dt)))
|
||||
self._timing_samples = {
|
||||
name: []
|
||||
for name in ("period", "total", "qp", "send", "feedback_age")
|
||||
}
|
||||
self._last_control_tick_started_ns: int | None = None
|
||||
```
|
||||
|
||||
为每组样本计算统计摘要：
|
||||
|
||||
```python
|
||||
def _timing_summary(
|
||||
self,
|
||||
name: str,
|
||||
samples: list[float],
|
||||
deadline_ms: float | None = None,
|
||||
) -> str:
|
||||
values = np.asarray(samples)
|
||||
result = (
|
||||
f"{name}[n={len(samples)} mean={np.mean(values):.3f} "
|
||||
f"p95={np.percentile(values, 95):.3f} "
|
||||
f"p99={np.percentile(values, 99):.3f} "
|
||||
f"max={np.max(values):.3f} ms"
|
||||
)
|
||||
if deadline_ms is not None:
|
||||
result += f" overruns={np.count_nonzero(values > deadline_ms)}"
|
||||
return result + "]"
|
||||
```
|
||||
|
||||
满窗后输出并清空：
|
||||
|
||||
```python
|
||||
def _record_timing_sample(
|
||||
self,
|
||||
period_ms: float | None,
|
||||
total_ms: float,
|
||||
qp_ms: float,
|
||||
send_ms: float,
|
||||
feedback_age_ms: float,
|
||||
) -> None:
|
||||
if period_ms is not None:
|
||||
self._timing_samples["period"].append(period_ms)
|
||||
self._timing_samples["total"].append(total_ms)
|
||||
self._timing_samples["qp"].append(qp_ms)
|
||||
self._timing_samples["send"].append(send_ms)
|
||||
self._timing_samples["feedback_age"].append(feedback_age_ms)
|
||||
if len(self._timing_samples["total"]) < self._timing_stats_window:
|
||||
return
|
||||
|
||||
deadline_ms = self._dt * 1000.0
|
||||
summaries = [
|
||||
self._timing_summary("period", self._timing_samples["period"], deadline_ms),
|
||||
self._timing_summary("total", self._timing_samples["total"], deadline_ms),
|
||||
self._timing_summary("qp", self._timing_samples["qp"]),
|
||||
self._timing_summary("send", self._timing_samples["send"]),
|
||||
self._timing_summary("feedback_age", self._timing_samples["feedback_age"]),
|
||||
]
|
||||
self.get_logger().info(
|
||||
f"{self._arm_name} timing n={len(self._timing_samples['total'])} "
|
||||
f"deadline={deadline_ms:.3f} ms | " + " | ".join(summaries)
|
||||
)
|
||||
for samples in self._timing_samples.values():
|
||||
samples.clear()
|
||||
```
|
||||
|
||||
在 `_control_tick()` 中围绕 QP 和发送调用采样，并在关节命令处理完成后记录总耗时。早退周期不进入统计窗口，现有控制和安全逻辑保持不变。
|
||||
|
||||
- [x] **Step 4: 运行测试确认通过**
|
||||
|
||||
```bash
|
||||
source /opt/ros/humble/setup.bash
|
||||
PYTHONPATH=src/xr_rm_teleop pytest -q src/xr_rm_teleop/test/test_joint_control.py
|
||||
```
|
||||
|
||||
预期：全部通过。
|
||||
|
||||
- [x] **Step 5: 完整验证**
|
||||
|
||||
```bash
|
||||
source /opt/ros/humble/setup.bash
|
||||
pytest -q src/xr_rm_teleop/test/test_orientation_control.py
|
||||
colcon build --symlink-install
|
||||
```
|
||||
|
||||
预期：姿态测试和工作空间构建全部通过。根据仓库规则，不自动提交 Git。
|
||||
@@ -0,0 +1,390 @@
|
||||
# RM75 SO(3) 姿态跟随与 OmniPicker 模型 Implementation Plan
|
||||
|
||||
> **For Codex:** REQUIRED SUB-SKILL: Use `superpowers:executing-plans` to implement this plan task-by-task.
|
||||
|
||||
**Goal:** 去掉遥操作控制路径中的 RPY 往返转换，使 RM75 TCP 姿态始终沿 SO(3) 最短路径跟随，并让左右臂的 Placo QP 直接控制一体化模型中的 `omnipicker_tcp`。
|
||||
|
||||
**Architecture:** 保留现有单节点、单步 Placo QP、关节反馈、RealMan 连接和安全停止链路。XR 四元数映射为机器人旋转矩阵；平移使用直接位置差，姿态使用 SO(3) 对数误差，二者以解耦 `3+3` 形式处理。Placo 接收完整 `4×4` 目标矩阵并直接约束 URDF 的 `omnipicker_tcp`，不再读取外设工具位姿做 QP 末端换算。
|
||||
|
||||
**Tech Stack:** Ubuntu 22.04、ROS2 Humble、Python 3.10、NumPy、Placo 0.9.4、Pinocchio 3.7.0、pytest、URDF。
|
||||
|
||||
**Repository rule:** 不执行 `git commit`、`git push` 或真机命令。所有启动验证必须显式使用 `use_mock:=true`；`peripherals_rm75.yaml`、`avoid_singularity`、可操作度任务和既有安全限制保持不变。
|
||||
|
||||
---
|
||||
|
||||
## 文件范围
|
||||
|
||||
- Create: `xr_rm_teleop/models/rm75_omnipicker/urdf/RM75-B_OmniPicker_fixed.urdf`
|
||||
- Create: `xr_rm_teleop/models/rm75_omnipicker/meshes/rm75/*.STL`
|
||||
- Create: `xr_rm_teleop/models/rm75_omnipicker/meshes/omnipicker/*.STL`
|
||||
- Modify: `xr_rm_teleop/setup.py`
|
||||
- Modify: `xr_rm_teleop/xr_rm_teleop/placo_ik_solver.py`
|
||||
- Modify: `xr_rm_teleop/xr_rm_teleop/single_arm_velocity_teleop.py`
|
||||
- Modify: `xr_rm_teleop/test/test_orientation_control.py`
|
||||
- Modify: `xr_rm_teleop/test/test_placo_transforms.py`
|
||||
- Modify: `xr_rm_teleop/test/test_joint_control.py`
|
||||
- Modify: `xr_rm_teleop/test/placo_ik_smoke.py`
|
||||
- Modify: `xr_rm_bringup/launch/arm_debug.launch.py`
|
||||
- Modify: `xr_rm_bringup/config/left_arm_rm75.yaml`
|
||||
- Modify: `xr_rm_bringup/config/right_arm_rm75.yaml`
|
||||
- Modify: `xr_rm_bringup/config/dual_arm_rm75.yaml`
|
||||
- Modify: `README.md`
|
||||
|
||||
不删除旧 `xr_rm_teleop/models/rm75` 资源，只让 launch 停止选用它，避免扩大无关清理范围。
|
||||
|
||||
### Task 1: 导入 fixed 一体化模型并定义 TCP
|
||||
|
||||
**Files:**
|
||||
|
||||
- Create: `xr_rm_teleop/models/rm75_omnipicker/urdf/RM75-B_OmniPicker_fixed.urdf`
|
||||
- Create: `xr_rm_teleop/models/rm75_omnipicker/meshes/rm75/*.STL`
|
||||
- Create: `xr_rm_teleop/models/rm75_omnipicker/meshes/omnipicker/*.STL`
|
||||
- Modify: `xr_rm_teleop/setup.py`
|
||||
- Modify: `xr_rm_teleop/test/test_placo_transforms.py`
|
||||
|
||||
- [x] **Step 1: 先写模型结构失败测试**
|
||||
|
||||
在 `test_placo_transforms.py` 中用 `xml.etree.ElementTree` 读取 fixed URDF，断言：
|
||||
|
||||
```python
|
||||
assert moving_joint_names == [f"joint_{index}" for index in range(1, 8)]
|
||||
assert tcp_joint.attrib["type"] == "fixed"
|
||||
assert tcp_joint.find("parent").attrib["link"] == "omnipicker_base_link"
|
||||
assert tcp_joint.find("child").attrib["link"] == "omnipicker_tcp"
|
||||
assert tcp_joint.find("origin").attrib["xyz"] == "0 0 0.16"
|
||||
assert tcp_joint.find("origin").attrib["rpy"] == "0 0 0"
|
||||
```
|
||||
|
||||
运行：
|
||||
|
||||
```bash
|
||||
cd /home/robot/WS_xr
|
||||
source /opt/ros/humble/setup.bash
|
||||
PYTHONPATH=src/xr_rm_teleop pytest -q \
|
||||
src/xr_rm_teleop/test/test_placo_transforms.py
|
||||
```
|
||||
|
||||
Expected: FAIL，模型包尚不存在。
|
||||
|
||||
- [x] **Step 2: 从上传 ZIP 只导入运行所需资源**
|
||||
|
||||
从
|
||||
`/home/robot/下载/Models/RM75-B_OmniPicker_Pinocchio.zip`
|
||||
导入 fixed URDF 和两组 mesh 到
|
||||
`xr_rm_teleop/models/rm75_omnipicker`；不导入独立描述包元数据、示例脚本、
|
||||
活动式 URDF 或额外验证文档。保留上传模型的几何、惯量、关节限制和 fixed
|
||||
OmniPicker 关节，并在 `xr_rm_teleop/setup.py` 中安装这些资源。
|
||||
|
||||
- [x] **Step 3: 在 fixed URDF 增加已确认的 TCP**
|
||||
|
||||
```xml
|
||||
<link name="omnipicker_tcp"/>
|
||||
<joint name="omnipicker_tcp_joint" type="fixed">
|
||||
<parent link="omnipicker_base_link"/>
|
||||
<child link="omnipicker_tcp"/>
|
||||
<origin xyz="0 0 0.16" rpy="0 0 0"/>
|
||||
</joint>
|
||||
```
|
||||
|
||||
- [x] **Step 4: 重跑模型测试**
|
||||
|
||||
Expected: PASS；运动关节仍严格为 `joint_1` 至 `joint_7`，TCP 偏移为
|
||||
`+Z 0.16 m`。
|
||||
|
||||
### Task 2: 让 Placo 直接接收 SE(3) 并约束 `omnipicker_tcp`
|
||||
|
||||
**Files:**
|
||||
|
||||
- Modify: `xr_rm_teleop/xr_rm_teleop/placo_ik_solver.py`
|
||||
- Modify: `xr_rm_teleop/test/test_placo_transforms.py`
|
||||
- Modify: `xr_rm_teleop/test/placo_ik_smoke.py`
|
||||
|
||||
- [x] **Step 1: 先把变换和 smoke 测试改为矩阵接口**
|
||||
|
||||
测试改为：
|
||||
|
||||
```python
|
||||
current = solver.update_joint_state(joints)
|
||||
assert current.shape == (4, 4)
|
||||
target = current.copy()
|
||||
target[0, 3] += 0.01
|
||||
target[:3, :3] = rotation_delta @ target[:3, :3]
|
||||
joints = solver.solve(target)
|
||||
```
|
||||
|
||||
同时覆盖非法形状、NaN 和非 SE(3) 最后一行会被拒绝。smoke 使用
|
||||
`dt=1/125`，以旋转矩阵相对角度计算姿态误差，不再转换 RPY。
|
||||
|
||||
运行现有两项测试，确认它们先因旧 `ArmPose/tool_pose` 接口失败。
|
||||
|
||||
- [x] **Step 2: 最小化求解器接口**
|
||||
|
||||
将构造函数改为：
|
||||
|
||||
```python
|
||||
PlacoIkSolver(urdf_path: str, dt: float)
|
||||
```
|
||||
|
||||
并完成以下替换：
|
||||
|
||||
- 删除 `_rpy_to_rotation`、`_rotation_to_rpy`、`_arm_pose_to_transform`、
|
||||
`_transform_to_arm_pose`、`_tool_pose_to_transform`。
|
||||
- 删除 `_tool_transform` 和 `_tool_inverse`。
|
||||
- frame task 从 `link_7` 改为 `omnipicker_tcp`。
|
||||
- 可操作度任务继续作用于原来的 `link_7`，并保留原权重
|
||||
`soft, 5e-2`。
|
||||
- `update_joint_state()` 直接返回
|
||||
`get_T_world_frame("omnipicker_tcp").copy()`。
|
||||
- `solve()` 校验并直接设置传入的 `4×4` 目标矩阵。
|
||||
- frame task、动能正则、虚拟基座固定、关节位置/速度校验保持原状。
|
||||
|
||||
- [x] **Step 3: 运行纯单元测试**
|
||||
|
||||
```bash
|
||||
PYTHONPATH=src/xr_rm_teleop pytest -q \
|
||||
src/xr_rm_teleop/test/test_placo_transforms.py
|
||||
```
|
||||
|
||||
Expected: PASS。该命令不构造 Placo，不要求系统 Python 安装厂商 SDK。
|
||||
|
||||
### Task 3: 用 SO(3) 最短路径替换 RPY 姿态控制
|
||||
|
||||
**Files:**
|
||||
|
||||
- Modify: `xr_rm_teleop/test/test_orientation_control.py`
|
||||
- Modify: `xr_rm_teleop/xr_rm_teleop/single_arm_velocity_teleop.py`
|
||||
|
||||
- [x] **Step 1: 先写 SO(3) 回归测试**
|
||||
|
||||
保留零四元数停止测试，并增加以下最小覆盖：
|
||||
|
||||
- `q` 与 `-q` 得到同一旋转矩阵。
|
||||
- 初始 pitch 接近 `+90°`、`-90°` 时，小手柄旋转只产生同量级的小旋转。
|
||||
- 跨过旧 RPY 分支时，相对旋转仍取最短路径。
|
||||
- 死区按 `norm(Log(R_target R_currentᵀ))` 判断。
|
||||
- `alpha=0.5` 时 SO(3) 误差角减半。
|
||||
- `dt=1/125`、`max_orientation_speed=0.5` 时单步不超过 `0.004 rad`。
|
||||
- 关闭某姿态轴时，在机器人基坐标系将对应旋转向量分量清零。
|
||||
- 矩阵转调试四元数后有限且单位化。
|
||||
|
||||
运行：
|
||||
|
||||
```bash
|
||||
PYTHONPATH=src/xr_rm_teleop pytest -q \
|
||||
src/xr_rm_teleop/test/test_orientation_control.py
|
||||
```
|
||||
|
||||
Expected: FAIL，旧代码仍返回和处理 RPY。
|
||||
|
||||
- [x] **Step 2: 实现最少的 NumPy SO(3) 运算**
|
||||
|
||||
在现有遥操作模块中加入并只加入实际调用的函数：
|
||||
|
||||
```text
|
||||
quaternion -> rotation matrix
|
||||
rotation matrix -> normalized quaternion
|
||||
Log_SO3(rotation) -> 3D rotation vector
|
||||
Exp_SO3(rotation vector) -> rotation matrix
|
||||
position + rotation -> 4×4 transform
|
||||
```
|
||||
|
||||
输入必须有限。近似旋转矩阵仅在
|
||||
`norm(RᵀR-I) <= 1e-3` 且行列式为正时用 SVD 投影；明显无效输入抛出
|
||||
`ValueError`。`Log_SO3` 在接近 `π` 时仍返回最短的有限旋转向量。
|
||||
|
||||
- [x] **Step 3: 替换姿态目标、滤波和限速**
|
||||
|
||||
控制路径统一为：
|
||||
|
||||
```python
|
||||
R_xr_delta = R_xr_now @ R_xr_start.T
|
||||
R_robot_delta = mapping @ R_xr_delta @ mapping.T
|
||||
axis_delta = log_so3(R_robot_delta)
|
||||
axis_delta[disabled_axes] = 0.0
|
||||
R_raw = exp_so3(axis_delta) @ R_robot_start
|
||||
|
||||
error = log_so3(R_target @ R_current.T)
|
||||
R_next = exp_so3(scale * error) @ R_current
|
||||
```
|
||||
|
||||
继续分别保存平移列表和旋转矩阵状态，但构造 QP 目标与调试目标时合成为
|
||||
`4×4` 矩阵。删除控制路径中的 `_matrix_to_euler`、
|
||||
`_quaternion_to_euler`、分量 `_angle_delta` 及 RPY
|
||||
死区/滤波/限速；位置死区、滤波、工作空间和圆柱限位原样保留。
|
||||
|
||||
- [x] **Step 4: 重跑姿态测试**
|
||||
|
||||
Expected: PASS。
|
||||
|
||||
### Task 4: 把节点状态、QP 和调试话题贯通为 SE(3)
|
||||
|
||||
**Files:**
|
||||
|
||||
- Modify: `xr_rm_teleop/xr_rm_teleop/single_arm_velocity_teleop.py`
|
||||
- Modify: `xr_rm_teleop/test/test_joint_control.py`
|
||||
|
||||
- [x] **Step 1: 先将关节控制测试改为 `4×4` 矩阵**
|
||||
|
||||
Fake solver 的 `update_joint_state()` 返回有限齐次矩阵；QP
|
||||
成功、失败和首帧反馈测试均断言矩阵接口。运行测试，确认旧类型假设失败。
|
||||
|
||||
- [x] **Step 2: 完成节点矩阵状态迁移**
|
||||
|
||||
- `_robot_start_pose`、`_last_current_pose` 和调试 fallback 改存 `4×4`
|
||||
矩阵。
|
||||
- `PlacoIkSolver` 初始化不再接收
|
||||
`self._peripheral_config.tool_pose`；外设配置仍只传给
|
||||
`RealManAdapter.configure_peripheral()`。
|
||||
- 原始目标与发送目标均合成为 `omnipicker_tcp` 的 SE(3)。
|
||||
- `TwistStamped.angular` 使用
|
||||
`Log(R_sent R_previousᵀ) / dt`，表达在 `rm_base`。
|
||||
- `PoseStamped` 只在发布边界把旋转矩阵转四元数。
|
||||
- QP 异常继续返回 last-known-good；Grip 松开、超时、反馈错误和发送错误继续
|
||||
走现有慢停与状态重置。
|
||||
|
||||
- [x] **Step 3: 运行相关单元测试**
|
||||
|
||||
```bash
|
||||
PYTHONPATH=src/xr_rm_teleop pytest -q \
|
||||
src/xr_rm_teleop/test/test_orientation_control.py \
|
||||
src/xr_rm_teleop/test/test_joint_control.py \
|
||||
src/xr_rm_teleop/test/test_placo_transforms.py \
|
||||
src/xr_rm_teleop/test/test_initial_joint_pose.py
|
||||
```
|
||||
|
||||
Expected: PASS。
|
||||
|
||||
### Task 5: 切换 launch 模型并同步已确认参数
|
||||
|
||||
**Files:**
|
||||
|
||||
- Modify: `xr_rm_bringup/launch/arm_debug.launch.py`
|
||||
- Modify: `xr_rm_bringup/config/left_arm_rm75.yaml`
|
||||
- Modify: `xr_rm_bringup/config/right_arm_rm75.yaml`
|
||||
- Modify: `xr_rm_bringup/config/dual_arm_rm75.yaml`
|
||||
- Modify: `xr_rm_teleop/setup.py`
|
||||
- Modify: `xr_rm_teleop/xr_rm_teleop/single_arm_velocity_teleop.py`
|
||||
- Modify: `README.md`
|
||||
|
||||
- [x] **Step 1: 修改模型来源**
|
||||
|
||||
`_rm75_urdf()` 改为：
|
||||
|
||||
```python
|
||||
PathJoinSubstitution([
|
||||
FindPackageShare("xr_rm_teleop"),
|
||||
"models",
|
||||
"rm75_omnipicker",
|
||||
"urdf",
|
||||
"RM75-B_OmniPicker_fixed.urdf",
|
||||
])
|
||||
```
|
||||
|
||||
并让 `xr_rm_teleop/setup.py` 安装该目录下的 fixed URDF 和两组 mesh。
|
||||
|
||||
- [x] **Step 2: 只修改已确认参数**
|
||||
|
||||
节点默认值、launch 默认值和三份 YAML 对应项同步：
|
||||
|
||||
```yaml
|
||||
control_rate_hz: 125.0
|
||||
orientation_deadband_rad: 0.005
|
||||
orientation_filter_alpha: 0.65
|
||||
max_orientation_speed: 0.5
|
||||
follow: false
|
||||
```
|
||||
|
||||
其中右臂 YAML 的
|
||||
`move_to_initial_pose_on_connect: True`
|
||||
改为 `false`。不修改任何工作空间、圆柱、线速度、关节速度、初始角、
|
||||
`avoid_singularity`、安全配置或外设配置。
|
||||
|
||||
- [x] **Step 3: 更新 README 中已失真的运行说明**
|
||||
|
||||
只更新：
|
||||
|
||||
- 默认控制频率 `90.0 -> 125.0`。
|
||||
- QP 模型改为一体化 fixed URDF，并直接控制 `omnipicker_tcp`。
|
||||
- 姿态死区、滤波和限速使用 SO(3) 最短路径，不使用 RPY。
|
||||
- `peripherals_rm75.yaml` 仍只用于真实控制器工具坐标、负载和外设选择，不再
|
||||
参与 Placo TCP 矩阵换算。
|
||||
|
||||
### Task 6: 构建、数值 smoke 与 mock 启动验证
|
||||
|
||||
**Files:**
|
||||
|
||||
- Modify: `xr_rm_teleop/test/placo_ik_smoke.py`
|
||||
|
||||
- [x] **Step 1: 构建整个工作空间**
|
||||
|
||||
```bash
|
||||
cd /home/robot/WS_xr
|
||||
source /opt/ros/humble/setup.bash
|
||||
colcon build --symlink-install
|
||||
```
|
||||
|
||||
Expected: `xr_rm_teleop` 和 `xr_rm_bringup` 构建成功。
|
||||
|
||||
- [x] **Step 2: 运行指定姿态测试和相关回归测试**
|
||||
|
||||
```bash
|
||||
source /opt/ros/humble/setup.bash
|
||||
PYTHONPATH=src/xr_rm_teleop pytest -q \
|
||||
src/xr_rm_teleop/test/test_orientation_control.py
|
||||
|
||||
PYTHONPATH=src/xr_rm_teleop pytest -q \
|
||||
src/xr_rm_teleop/test/test_joint_control.py \
|
||||
src/xr_rm_teleop/test/test_placo_transforms.py \
|
||||
src/xr_rm_teleop/test/test_initial_joint_pose.py
|
||||
```
|
||||
|
||||
Expected: PASS。
|
||||
|
||||
- [x] **Step 3: 使用固定 XR Python 运行 Placo 数值 smoke**
|
||||
|
||||
```bash
|
||||
source install/setup.bash
|
||||
/home/robot/miniconda3/envs/xr/bin/python \
|
||||
src/xr_rm_teleop/test/placo_ik_smoke.py \
|
||||
install/xr_rm_teleop/share/xr_rm_teleop/models/rm75_omnipicker/urdf/RM75-B_OmniPicker_fixed.urdf
|
||||
```
|
||||
|
||||
对左右初始关节姿态分别验证：
|
||||
|
||||
- 七个运动关节及顺序正确。
|
||||
- `omnipicker_tcp` 相对 `link_7` 为 `[0, 0, 0.16]`、单位旋转。
|
||||
- QP 输出七个有限关节角并满足位置与单周期速度限制。
|
||||
- 目标停止两秒时打印最大关节变化，但不把漂移设为失败条件。
|
||||
- 运动目标最终 TCP 位置误差 `<= 5 mm`，姿态误差 `<= 2°`。
|
||||
- 打印平均/最大求解耗时及超过 `8 ms` 周期预算的次数，只记录、不设机器相关
|
||||
的硬失败阈值。
|
||||
|
||||
- [x] **Step 4: 只启动 mock**
|
||||
|
||||
分别短时启动：
|
||||
|
||||
```bash
|
||||
source /opt/ros/humble/setup.bash
|
||||
source install/setup.bash
|
||||
ros2 launch xr_rm_bringup arm_debug.launch.py arm:=left use_mock:=true
|
||||
ros2 launch xr_rm_bringup arm_debug.launch.py arm:=right use_mock:=true
|
||||
ros2 launch xr_rm_bringup arm_debug.launch.py arm:=both use_mock:=true
|
||||
```
|
||||
|
||||
确认 fixed URDF、Placo 和左右节点名加载成功，无 RealMan SDK 导入或网络连接。
|
||||
由人工结束 mock launch；Codex 不执行任何 `use_mock:=false` 命令。
|
||||
|
||||
- [x] **Step 5: 最终范围检查**
|
||||
|
||||
```bash
|
||||
git diff --check
|
||||
git status --short
|
||||
git diff -- \
|
||||
src/xr_rm_teleop \
|
||||
src/xr_rm_bringup \
|
||||
src/README.md \
|
||||
src/docs/superpowers
|
||||
```
|
||||
|
||||
确认 `peripherals_rm75.yaml`、`avoid_singularity`、可操作度权重和所有既有安全
|
||||
限制未被改变。
|
||||
@@ -0,0 +1,30 @@
|
||||
# RM75 控制周期统计设计
|
||||
|
||||
## 目标
|
||||
|
||||
在不改变控制、QP、安全停止和真机通信行为的前提下，确认激活遥操作时的
|
||||
125 Hz 控制链路是否满足 `8 ms` 周期。
|
||||
|
||||
## 方案
|
||||
|
||||
在 `SingleArmVelocityTeleop` 内使用 `time.perf_counter_ns()` 采样，仅在
|
||||
Grip 激活并执行关节命令的周期记录：
|
||||
|
||||
- 相邻控制回调的实际周期；
|
||||
- 控制回调总执行时间；
|
||||
- Placo QP 求解时间；
|
||||
- `send_joint_target()` 调用时间；
|
||||
- 当前关节反馈年龄。
|
||||
|
||||
每累计约 5 秒激活样本，通过现有 ROS logger 输出一次汇总并清空窗口。每项
|
||||
输出样本数、mean、P95、P99 和 max；实际周期与总执行时间额外输出超过
|
||||
`self._dt` 的次数。首个激活周期没有可靠的相邻周期值，因此不记录周期。
|
||||
|
||||
统计只输出日志，不新增 ROS 消息、话题、参数、依赖或后台线程。计时与日志
|
||||
异常不得影响控制路径。
|
||||
|
||||
## 验证
|
||||
|
||||
先添加一个小单元测试，使用确定性样本验证百分位数、超限计数、日志输出和
|
||||
窗口清空。随后运行相关 pytest、姿态控制测试和
|
||||
`colcon build --symlink-install`。
|
||||
@@ -0,0 +1,241 @@
|
||||
# RM75 SO(3) 姿态跟随与 OmniPicker 一体化模型设计
|
||||
|
||||
日期：2026-07-28
|
||||
状态：已确认
|
||||
|
||||
## 1. 背景与目标
|
||||
|
||||
当前遥操作链路已经使用 Placo QP 将 TCP 目标转换为 RM75 七关节目标，但姿态目标在进入 QP 前会转换为 RPY，并按三个欧拉角分量进行滤波和限速。RM75 当前姿态接近俯仰角 `±90°` 时，同一个物理姿态可能切换到另一组等价 RPY，导致控制器把很小的手柄旋转解释为大角度路径，出现机械臂绕行和跟随时间过长。
|
||||
|
||||
本设计是
|
||||
`2026-07-27-rm75-placo-qp-ik-design.md`
|
||||
的增量修改；它覆盖旧设计中的模型、TCP 变换、姿态表示、控制频率和相关验收
|
||||
内容。现有的关节反馈、单步 QP、`rm_movej_canfd`、last-known-good 和停止链路
|
||||
继续保留。
|
||||
|
||||
本次修改的目标是：
|
||||
|
||||
1. 姿态控制全程使用旋转矩阵和 SE(3)，按 SO(3) 最短旋转路径滤波和限速。
|
||||
2. 左右臂统一使用用户提供的 `RM75-B_OmniPicker_fixed.urdf`。
|
||||
3. 在 URDF 内定义 `omnipicker_tcp`，取消 Placo 中重复的运行时末端变换。
|
||||
4. 首轮真机测试继续使用低跟随，将控制频率设为 `125 Hz`，TCP 目标角速度上限统一为 `0.5 rad/s`。
|
||||
5. 保留现有工作空间、圆柱、关节速度、指令超时和安全停止行为。
|
||||
|
||||
## 2. 本次不处理的内容
|
||||
|
||||
- 不开启高跟随，`follow` 继续为 `false`。
|
||||
- 不改变现有 Wi-Fi/有线混合网络拓扑。
|
||||
- 不新增高跟随周期看门狗。
|
||||
- 不修改 `avoid_singularity`，不新增奇异点检测或降速策略。
|
||||
- 首轮不删除或调整现有可操作度优化任务。
|
||||
- 不修改 `peripherals_rm75.yaml` 中的夹爪选择、工具位姿或负载。
|
||||
- 不新增 ROS2 节点、消息、求解器工厂或第三方依赖。
|
||||
- Codex 不连接或移动真实机械臂和夹爪。
|
||||
|
||||
## 3. 姿态数据模型
|
||||
|
||||
控制路径中的机器人位姿统一为有限的 `4×4` NumPy SE(3) 矩阵：
|
||||
|
||||
```text
|
||||
T = [ R p ]
|
||||
[ 0 1 ]
|
||||
```
|
||||
|
||||
其中 `R` 为 `3×3` 旋转矩阵，`p` 为 TCP 在机器人基坐标系下的位置。Placo 正解直接返回该矩阵，QP 目标也直接接收该矩阵。现有仅保存 `x/y/z/rx/ry/rz` 的 `ArmPose` 不再作为控制路径接口，避免在遥操作节点与 Placo 之间发生 RPY 往返转换。
|
||||
|
||||
ROS 调试边界按消息类型转换：
|
||||
|
||||
- `PoseStamped`：旋转矩阵转换为四元数后发布。
|
||||
- `TwistStamped.angular`：发布相邻两个目标旋转之间、在机器人基坐标系表达的 SO(3) 旋转向量速度。
|
||||
|
||||
## 4. XR 姿态到机器人姿态
|
||||
|
||||
Grip 按下时保存 XR 初始四元数和当前 `omnipicker_tcp` 初始旋转矩阵。每周期计算：
|
||||
|
||||
```text
|
||||
R_xr_delta = R_xr_now * transpose(R_xr_start)
|
||||
R_robot_delta = M * R_xr_delta * transpose(M)
|
||||
R_raw_target = R_robot_delta * R_robot_start
|
||||
```
|
||||
|
||||
`M` 为现有 `xr_to_robot_matrix`，其左右臂映射保持不变。`enable_orientation_axes` 继续保留；被关闭的机器人基坐标轴通过将对应 SO(3) 相对旋转向量分量置零实现，不再通过拼接 RPY 分量实现。
|
||||
|
||||
姿态处理统一使用基坐标系下的左乘增量：
|
||||
|
||||
```text
|
||||
R_error = R_target * transpose(R_current)
|
||||
r = Log(R_error)
|
||||
R_next = Exp(scale * r) * R_current
|
||||
```
|
||||
|
||||
处理顺序为：
|
||||
|
||||
1. 以 `norm(Log(R_target * R_lastᵀ))` 判断 `orientation_deadband_rad`。
|
||||
2. 以 `orientation_filter_alpha` 缩放从滤波状态到目标的最短旋转向量。
|
||||
3. 将从上一发送姿态到滤波姿态的旋转角限制在
|
||||
`max_orientation_speed / control_rate_hz` 以内。
|
||||
4. 将限速后的旋转和已通过现有工作空间限制的位置合成为 SE(3)。
|
||||
|
||||
该路径不使用 RPY 展开、分量插值或分量限速。相对旋转接近 `π` 时也必须选择物理最短路径；四元数 `q` 与 `-q` 必须得到相同目标。
|
||||
|
||||
位姿误差采用与 Placo `FrameTask` 一致的解耦 `3+3` 形式：
|
||||
|
||||
```text
|
||||
e_position = p_target - p_current
|
||||
e_orientation = Log(R_target * transpose(R_current))
|
||||
e_pose = [e_position; e_orientation]
|
||||
```
|
||||
|
||||
其中 `e_pose` 可视为六维任务误差，但不是
|
||||
`Log_SE3(T_target * inverse(T_current))`。本次不引入完整 SE(3) 对数映射中的
|
||||
平移—旋转耦合。
|
||||
|
||||
## 5. URDF 与 TCP
|
||||
|
||||
用户上传的
|
||||
`/home/robot/下载/Models/RM75-B_OmniPicker_Pinocchio.zip`
|
||||
作为模型来源。将 fixed URDF 和被引用的 RM75、OmniPicker mesh 放入
|
||||
`xr_rm_teleop/models/rm75_omnipicker`，由现有 `xr_rm_teleop` 包安装。
|
||||
|
||||
在 `RM75-B_OmniPicker_fixed.urdf` 中增加：
|
||||
|
||||
```xml
|
||||
<link name="omnipicker_tcp"/>
|
||||
|
||||
<joint name="omnipicker_tcp_joint" type="fixed">
|
||||
<parent link="omnipicker_base_link"/>
|
||||
<child link="omnipicker_tcp"/>
|
||||
<origin xyz="0 0 0.16" rpy="0 0 0"/>
|
||||
</joint>
|
||||
```
|
||||
|
||||
该变换表示 TCP 相对 RM75 法兰坐标系沿 `+Z` 方向平移 `0.16 m`、坐标轴方向不变。上传模型中 `rm75_flange` 到 `omnipicker_base_link` 为零固定变换，因此上述定义与已确认的法兰到 TCP 变换一致。
|
||||
|
||||
`arm_debug.launch.py` 的左臂、右臂和双臂节点统一加载这一份 fixed URDF。模型仍只有 `joint_1` 至 `joint_7` 七个运动关节，OmniPicker 关节均保持 fixed。
|
||||
|
||||
## 6. Placo QP
|
||||
|
||||
`PlacoIkSolver` 改为：
|
||||
|
||||
```text
|
||||
构造参数：URDF 路径、dt
|
||||
当前位姿：get_T_world_frame("omnipicker_tcp")
|
||||
目标任务：add_frame_task("omnipicker_tcp", target_se3)
|
||||
```
|
||||
|
||||
删除仅供 Placo 使用的 `tool_pose` 构造参数、`_tool_transform`、
|
||||
`_tool_inverse` 和对应的法兰/TCP换算函数。`peripherals_rm75.yaml` 保持原状，
|
||||
仍供 `RealManAdapter` 配置真实控制器的工具坐标、负载和末端外设；其中的
|
||||
`pose` 不再传入 Placo，因此不会在 QP 中重复叠加末端偏移。
|
||||
|
||||
QP 配置首轮保持：
|
||||
|
||||
```text
|
||||
frame task: soft, 1.0
|
||||
manipulability task: soft, 5e-2
|
||||
kinetic energy regularizer: 1e-6
|
||||
```
|
||||
|
||||
可操作度任务已知会造成目标静止时的关节姿态变化，但按用户决定本轮保留，
|
||||
待确认 RPY 绕行消失后再单独评估。关节位置和单周期速度校验继续使用 URDF
|
||||
限制，固定虚拟基座和 `q[7:14]` 的七关节映射保持不变。
|
||||
|
||||
## 7. 参数与启动行为
|
||||
|
||||
以下配置在 `left_arm_rm75.yaml`、`right_arm_rm75.yaml` 和
|
||||
`dual_arm_rm75.yaml` 对应节点中同步：
|
||||
|
||||
```yaml
|
||||
control_rate_hz: 125.0
|
||||
orientation_deadband_rad: 0.005
|
||||
orientation_filter_alpha: 0.65
|
||||
max_orientation_speed: 0.5
|
||||
follow: false
|
||||
```
|
||||
|
||||
`arm_debug.launch.py` 的 `control_rate_hz` 默认值同步为 `125.0`，`follow`
|
||||
默认值继续为 `false`。`single_arm_velocity_teleop` 内部参数默认值同步，
|
||||
避免绕过 YAML 启动时回到旧频率或旧角速度。
|
||||
|
||||
`right_arm_rm75.yaml` 的 `move_to_initial_pose_on_connect` 从 `True` 改为
|
||||
`false`，与左臂、双臂以及 launch 默认安全行为一致。其他左右臂空间范围、
|
||||
线速度、关节速度、加速度、初始关节角和外设配置不变。
|
||||
|
||||
## 8. 异常与停止
|
||||
|
||||
现有异常策略保持：
|
||||
|
||||
- 非法 XR 四元数、Grip 松开、XR 超时、关节反馈过期或通信失败时执行现有慢停止并重置激活状态。
|
||||
- QP 失败或输出违反关节位置/速度限制时继续使用上一组有效关节目标。
|
||||
- 第一次有效关节反馈前不发送运动命令。
|
||||
- `configure_safety_limits` 保持启用。
|
||||
- Mock 模式不导入睿尔曼 SDK。
|
||||
|
||||
新增 SO(3) 运算必须拒绝非有限矩阵和零四元数。旋转矩阵若满足
|
||||
`norm(RᵀR-I) <= 1e-3` 且行列式为正，则使用 `3×3` SVD 投影到最近的合法旋转；
|
||||
超出该范围时停止输出，不能把明显无效的输入静默修正成运动目标。
|
||||
|
||||
## 9. 验证
|
||||
|
||||
### 9.1 自动测试
|
||||
|
||||
扩展 `test_orientation_control.py`，至少覆盖：
|
||||
|
||||
- 初始姿态俯仰接近 `+90°` 和 `-90°` 时，小手柄旋转仍产生相同量级的最短物理旋转。
|
||||
- 目标跨越原 RPY 表示分支时，不产生接近 `π` 的错误路径。
|
||||
- `q` 与 `-q` 产生相同旋转矩阵。
|
||||
- SO(3) 死区使用整体旋转角。
|
||||
- 每周期姿态步长不超过 `0.5 / 125 rad`。
|
||||
- SO(3) 滤波沿最短路径收敛。
|
||||
- 调试四元数有限且归一化。
|
||||
|
||||
更新 Placo 变换测试和 smoke test，验证：
|
||||
|
||||
- fixed URDF 可加载，运动关节仍严格为七个且顺序正确。
|
||||
- `omnipicker_tcp` 相对法兰的变换为 `[0, 0, 0.16]` 和单位旋转。
|
||||
- 当前/原始目标/发送目标均表示 `omnipicker_tcp`。
|
||||
- 一次 QP 输出七个有限关节角并满足位置与单周期速度限制。
|
||||
- 目标静止两秒时记录左右臂最大关节变化，但本轮不以 `≤0.5°` 作为通过条件。
|
||||
- 目标停止后 TCP 位置误差不超过 `5 mm`、姿态误差不超过 `2°`。
|
||||
|
||||
### 9.2 命令
|
||||
|
||||
所有命令从 `/home/robot/WS_xr` 执行：
|
||||
|
||||
```bash
|
||||
source /opt/ros/humble/setup.bash
|
||||
PYTHONPATH=src/xr_rm_teleop pytest -q \
|
||||
src/xr_rm_teleop/test/test_orientation_control.py
|
||||
|
||||
source /opt/ros/humble/setup.bash
|
||||
colcon build --symlink-install
|
||||
|
||||
source /opt/ros/humble/setup.bash
|
||||
source install/setup.bash
|
||||
ros2 launch xr_rm_bringup arm_debug.launch.py arm:=left use_mock:=true
|
||||
ros2 launch xr_rm_bringup arm_debug.launch.py arm:=right use_mock:=true
|
||||
```
|
||||
|
||||
Placo smoke test继续使用已验证的
|
||||
`/home/robot/miniconda3/envs/xr/bin/python`，周期改为 `1/125 s`。
|
||||
|
||||
### 9.3 真机人工验收
|
||||
|
||||
Codex 只提供步骤，不执行真机操作。用户应分别测试左右臂：
|
||||
|
||||
1. 确认急停可用、周围无障碍物且 `move_to_initial_pose_on_connect=false`。
|
||||
2. 先保持手柄和目标姿态不变，记录 TCP 与关节变化。
|
||||
3. 仅改变手柄姿态，重点跨越原俯仰 `±90°` 附近的 RPY 分支。
|
||||
4. 确认 TCP 以最短物理旋转跟随，没有绕一大圈。
|
||||
5. 确认 `omnipicker_tcp` 位置保持在允许误差内。
|
||||
|
||||
## 10. 完成标准
|
||||
|
||||
1. 控制路径中的目标生成、滤波、限速和 QP 接口不再使用 RPY。
|
||||
2. 左右臂都由同一 fixed URDF 的 `omnipicker_tcp` 作为控制帧。
|
||||
3. Placo 不再使用 `peripherals_rm75.yaml` 的工具位姿进行矩阵换算。
|
||||
4. 可操作度任务保持现状，漂移数据被记录但不作为首轮阻断项。
|
||||
5. 三份配置使用 `125 Hz`、`0.5 rad/s` 和低跟随。
|
||||
6. 右臂连接时不再自动移动到初始关节姿态。
|
||||
7. 指定测试、构建和左右臂 mock 启动验证通过。
|
||||
8. 未改变工作空间、圆柱、关节限制、指令超时、安全停止或奇异点设置。
|
||||
@@ -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]
|
||||
|
||||
@@ -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]
|
||||
|
||||
@@ -5,7 +5,7 @@ single_arm_velocity_teleop:
|
||||
ros__parameters:
|
||||
arm_name: right_rm75
|
||||
controller_topic: /xr/right_controller
|
||||
control_rate_hz: 90.0
|
||||
control_rate_hz: 125.0
|
||||
command_timeout_sec: 0.12
|
||||
|
||||
scale: 0.7
|
||||
@@ -20,9 +20,9 @@ single_arm_velocity_teleop:
|
||||
orientation_deadband_rad: 0.005
|
||||
orientation_filter_alpha: 0.65
|
||||
max_orientation_speed: 0.5
|
||||
workspace_min: [-0.60, -0.60, 0.10]
|
||||
workspace_max: [0.60, 0.70, 0.55]
|
||||
cyl_radius_limit: [0.10, 0.70]
|
||||
workspace_min: [-0.70, -0.70, 0.10]
|
||||
workspace_max: [0.70, 0.70, 0.75]
|
||||
cyl_radius_limit: [0.10, 0.80]
|
||||
low_z_threshold: 0.1
|
||||
low_z_min_radius: 0.1
|
||||
|
||||
@@ -35,7 +35,7 @@ single_arm_velocity_teleop:
|
||||
robot_ip: 192.168.192.19
|
||||
robot_port: 8080
|
||||
avoid_singularity: 1
|
||||
follow: false
|
||||
follow: False
|
||||
canfd_trajectory_mode: 2
|
||||
canfd_radio: 0
|
||||
configure_safety_limits: true
|
||||
@@ -45,7 +45,7 @@ single_arm_velocity_teleop:
|
||||
max_angular_acc: 3.0
|
||||
joint_max_speed: 180.0
|
||||
joint_max_acc: 300.0
|
||||
move_to_initial_pose_on_connect: True
|
||||
move_to_initial_pose_on_connect: false
|
||||
initial_joint_pose: [-90.14, 3.76, -86.89, 87.89, -96.53, -79.62, -90.04]
|
||||
init_move_speed: 20
|
||||
debug_topic_prefix: /xr_rm
|
||||
|
||||
@@ -35,8 +35,9 @@ def _rm75_urdf() -> PathJoinSubstitution:
|
||||
return PathJoinSubstitution([
|
||||
FindPackageShare("xr_rm_teleop"),
|
||||
"models",
|
||||
"rm75",
|
||||
"RM75-B.urdf",
|
||||
"rm75_omnipicker",
|
||||
"urdf",
|
||||
"RM75-B_OmniPicker_fixed.urdf",
|
||||
])
|
||||
|
||||
|
||||
@@ -286,7 +287,7 @@ def generate_launch_description() -> LaunchDescription:
|
||||
# 非空时作为左右臂全局覆盖，例如 avoid_singularity:=0。
|
||||
DeclareLaunchArgument("avoid_singularity", default_value=""),
|
||||
# 每周期同步一次实际关节反馈、执行一次 Placo QP，再发送 rm_movej_canfd。
|
||||
DeclareLaunchArgument("control_rate_hz", default_value="90.0"),
|
||||
DeclareLaunchArgument("control_rate_hz", default_value="125.0"),
|
||||
# 默认低跟随；高跟随请确认控制器和网络能稳定满足厂商周期要求后再打开。
|
||||
DeclareLaunchArgument("follow", default_value="false"),
|
||||
DeclareLaunchArgument("configure_safety_limits", default_value="true"),
|
||||
|
||||
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
@@ -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>
|
||||
@@ -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,
|
||||
|
||||
@@ -8,56 +8,67 @@ from pathlib import Path
|
||||
import numpy as np
|
||||
|
||||
from xr_rm_teleop.placo_ik_solver import PlacoIkSolver
|
||||
from xr_rm_teleop.realman_adapter import ArmPose
|
||||
|
||||
|
||||
CASES = {
|
||||
"left": (
|
||||
[-79.55, -9.99, 71.01, 101.45, 95.07, -84.47, -74.52],
|
||||
[0.0, 0.0, 0.19, 0.0, 0.0, 0.0, 1.0],
|
||||
),
|
||||
"right": (
|
||||
[-90.14, 3.76, -86.89, 87.89, -96.53, -79.62, -90.04],
|
||||
[0.0, 0.0, 0.16, 0.0, 0.0, 0.0, 1.0],
|
||||
),
|
||||
"left": [-79.55, -9.99, 71.01, 101.45, 95.07, -84.47, -74.52],
|
||||
"right": [-90.14, 3.76, -86.89, 87.89, -96.53, -79.62, -90.04],
|
||||
}
|
||||
|
||||
|
||||
def angle_error(actual: list[float], target: list[float]) -> float:
|
||||
deltas = [
|
||||
math.atan2(math.sin(a - b), math.cos(a - b))
|
||||
for a, b in zip(actual, target)
|
||||
def rotation_z(angle: float) -> np.ndarray:
|
||||
cosine = math.cos(angle)
|
||||
sine = math.sin(angle)
|
||||
return np.asarray(
|
||||
[
|
||||
[cosine, -sine, 0.0],
|
||||
[sine, cosine, 0.0],
|
||||
[0.0, 0.0, 1.0],
|
||||
]
|
||||
return math.sqrt(sum(value * value for value in deltas))
|
||||
)
|
||||
|
||||
|
||||
def angle_error(actual: np.ndarray, target: np.ndarray) -> float:
|
||||
cosine = np.clip((np.trace(target @ actual.T) - 1.0) * 0.5, -1.0, 1.0)
|
||||
return float(math.acos(cosine))
|
||||
|
||||
|
||||
def main() -> None:
|
||||
urdf_path = Path(sys.argv[1]).resolve()
|
||||
for arm, (joint_degrees, tool_pose) in CASES.items():
|
||||
solver = PlacoIkSolver(str(urdf_path), tool_pose, 1.0 / 90.0)
|
||||
joints = np.deg2rad(joint_degrees).tolist()
|
||||
current = solver.update_joint_state(joints)
|
||||
target = ArmPose(
|
||||
current.x + 0.01,
|
||||
current.y,
|
||||
current.z,
|
||||
current.rx,
|
||||
current.ry,
|
||||
current.rz + 0.05,
|
||||
for arm, joint_degrees in CASES.items():
|
||||
initial_joints = np.deg2rad(joint_degrees)
|
||||
drift_solver = PlacoIkSolver(str(urdf_path), 1.0 / 125.0)
|
||||
joints = initial_joints.tolist()
|
||||
stationary_target = drift_solver.update_joint_state(joints)
|
||||
flange = drift_solver._robot.get_T_world_frame("link_7")
|
||||
flange_to_tcp = np.linalg.inv(flange) @ stationary_target
|
||||
assert np.allclose(flange_to_tcp[:3, 3], [0.0, 0.0, 0.16])
|
||||
assert np.allclose(flange_to_tcp[:3, :3], np.eye(3))
|
||||
for _ in range(250):
|
||||
drift_solver.update_joint_state(joints)
|
||||
joints = drift_solver.solve(stationary_target)
|
||||
drift_degrees = float(
|
||||
np.max(np.abs(np.rad2deg(np.asarray(joints) - initial_joints)))
|
||||
)
|
||||
|
||||
solver = PlacoIkSolver(str(urdf_path), 1.0 / 125.0)
|
||||
joints = initial_joints.tolist()
|
||||
current = solver.update_joint_state(joints)
|
||||
assert current.shape == (4, 4)
|
||||
target = current.copy()
|
||||
target[0, 3] += 0.01
|
||||
target[:3, :3] = rotation_z(0.05) @ target[:3, :3]
|
||||
|
||||
solve_durations = []
|
||||
for _ in range(45):
|
||||
for _ in range(250):
|
||||
solver.update_joint_state(joints)
|
||||
started_at = time.perf_counter()
|
||||
joints = solver.solve(target)
|
||||
solve_durations.append(time.perf_counter() - started_at)
|
||||
|
||||
actual = solver.update_joint_state(joints)
|
||||
position_error = np.linalg.norm(
|
||||
np.asarray(actual.xyz()) - np.asarray(target.xyz())
|
||||
)
|
||||
orientation_error = angle_error(actual.rpy(), target.rpy())
|
||||
position_error = np.linalg.norm(actual[:3, 3] - target[:3, 3])
|
||||
orientation_error = angle_error(actual[:3, :3], target[:3, :3])
|
||||
assert len(joints) == 7
|
||||
assert np.isfinite(joints).all()
|
||||
assert np.allclose(
|
||||
@@ -69,9 +80,10 @@ def main() -> None:
|
||||
print(
|
||||
f"{arm}: position_error={position_error:.6f}m, "
|
||||
f"orientation_error={math.degrees(orientation_error):.3f}deg, "
|
||||
f"stationary_drift={drift_degrees:.3f}deg, "
|
||||
f"solve_avg={1000.0 * np.mean(solve_durations):.3f}ms, "
|
||||
f"solve_max={1000.0 * max(solve_durations):.3f}ms, "
|
||||
f"solve_overruns={sum(value > 1.0 / 90.0 for value in solve_durations)}"
|
||||
f"solve_overruns={sum(value > 1.0 / 125.0 for value in solve_durations)}"
|
||||
)
|
||||
|
||||
|
||||
|
||||
@@ -1,13 +1,21 @@
|
||||
import time
|
||||
from types import SimpleNamespace
|
||||
|
||||
import numpy as np
|
||||
import pytest
|
||||
|
||||
from xr_rm_teleop.realman_adapter import ArmPose, JointStateSnapshot
|
||||
from xr_rm_teleop.single_arm_velocity_teleop import SingleArmVelocityTeleop
|
||||
from xr_rm_teleop.realman_adapter import JointStateSnapshot
|
||||
from xr_rm_teleop.single_arm_velocity_teleop import (
|
||||
SingleArmVelocityTeleop,
|
||||
_make_transform,
|
||||
_so3_exp,
|
||||
)
|
||||
|
||||
|
||||
class FakeLogger:
|
||||
def info(self, *args, **kwargs):
|
||||
del args, kwargs
|
||||
|
||||
def warn(self, *args, **kwargs):
|
||||
del args, kwargs
|
||||
|
||||
@@ -78,7 +86,9 @@ def test_first_feedback_initializes_last_valid_target_without_solving() -> None:
|
||||
|
||||
def update_joint_state(self, joints):
|
||||
assert joints == [0.1] * 7
|
||||
return ArmPose(0.3, 0.0, 0.2)
|
||||
transform = np.eye(4)
|
||||
transform[:3, 3] = [0.3, 0.0, 0.2]
|
||||
return transform
|
||||
|
||||
def solve(self, target):
|
||||
del target
|
||||
@@ -95,7 +105,9 @@ def test_first_feedback_initializes_last_valid_target_without_solving() -> None:
|
||||
JointStateSnapshot([0.1] * 7, time.monotonic())
|
||||
)
|
||||
|
||||
assert pose == ArmPose(0.3, 0.0, 0.2)
|
||||
assert pose == pytest.approx(
|
||||
_make_transform([0.3, 0.0, 0.2], np.eye(3))
|
||||
)
|
||||
assert teleop._last_valid_joint_target == [0.1] * 7
|
||||
assert teleop._ik_solver.solve_calls == 0
|
||||
|
||||
@@ -112,7 +124,7 @@ def test_qp_failure_returns_last_known_good_target() -> None:
|
||||
teleop._arm_name = "right_rm75"
|
||||
teleop.get_logger = lambda: FakeLogger()
|
||||
|
||||
target = teleop._solve_joint_target(ArmPose(0.3, 0.0, 0.2))
|
||||
target = teleop._solve_joint_target(np.eye(4))
|
||||
|
||||
assert target == pytest.approx([0.1] * 7)
|
||||
assert teleop._last_valid_joint_target == pytest.approx([0.1] * 7)
|
||||
@@ -130,12 +142,88 @@ def test_qp_success_updates_last_known_good_target() -> None:
|
||||
teleop._arm_name = "left_rm75"
|
||||
teleop.get_logger = lambda: FakeLogger()
|
||||
|
||||
target = teleop._solve_joint_target(ArmPose(0.3, 0.0, 0.2))
|
||||
target = teleop._solve_joint_target(np.eye(4))
|
||||
|
||||
assert target == pytest.approx([0.2] * 7)
|
||||
assert teleop._last_valid_joint_target == pytest.approx([0.2] * 7)
|
||||
|
||||
|
||||
def test_enter_active_control_initializes_se3_orientation_state() -> None:
|
||||
teleop = object.__new__(SingleArmVelocityTeleop)
|
||||
transform = _make_transform(
|
||||
[0.3, -0.1, 0.2],
|
||||
_so3_exp(np.asarray([0.1, -0.2, 0.3])),
|
||||
)
|
||||
published = []
|
||||
teleop._arm_name = "right_rm75"
|
||||
teleop.get_logger = lambda: FakeLogger()
|
||||
teleop._publish_debug = lambda *args: published.append(args)
|
||||
|
||||
teleop._enter_active_control(
|
||||
[0.0, 0.0, 0.0],
|
||||
(0.0, 0.0, 0.0, 1.0),
|
||||
transform,
|
||||
FakeTime(),
|
||||
)
|
||||
|
||||
assert teleop._robot_start_transform == pytest.approx(transform)
|
||||
assert teleop._filtered_target == pytest.approx(transform[:3, 3])
|
||||
assert teleop._filtered_orientation_target == pytest.approx(transform[:3, :3])
|
||||
assert teleop._last_sent_orientation == pytest.approx(transform[:3, :3])
|
||||
assert len(published) == 1
|
||||
|
||||
|
||||
def test_command_angular_velocity_uses_so3_rotation_vector() -> None:
|
||||
teleop = object.__new__(SingleArmVelocityTeleop)
|
||||
teleop._dt = 0.1
|
||||
teleop._last_sent_target = [0.0, 0.0, 0.0]
|
||||
teleop._last_sent_orientation = np.eye(3)
|
||||
teleop._last_command_time = None
|
||||
|
||||
velocity = teleop._estimate_command_velocity(
|
||||
[0.0, 0.0, 0.0],
|
||||
_so3_exp(np.asarray([0.0, 0.0, 0.1])),
|
||||
FakeTime(),
|
||||
)
|
||||
|
||||
assert velocity == pytest.approx([0.0, 0.0, 0.0, 0.0, 0.0, 1.0])
|
||||
|
||||
|
||||
def test_timing_stats_logs_summary_and_clears_window() -> None:
|
||||
messages = []
|
||||
teleop = object.__new__(SingleArmVelocityTeleop)
|
||||
teleop._arm_name = "right_rm75"
|
||||
teleop._dt = 0.008
|
||||
teleop._timing_stats_window = 2
|
||||
teleop._timing_samples = {
|
||||
name: []
|
||||
for name in ("period", "total", "qp", "send", "feedback_age")
|
||||
}
|
||||
teleop.get_logger = lambda: SimpleNamespace(
|
||||
info=lambda message: messages.append(message)
|
||||
)
|
||||
|
||||
teleop._record_timing_sample(7.0, 6.0, 1.0, 0.5, 3.0)
|
||||
assert messages == []
|
||||
|
||||
teleop._record_timing_sample(9.0, 10.0, 2.0, 0.7, 4.0)
|
||||
|
||||
assert len(messages) == 1
|
||||
assert "right_rm75 timing n=2 deadline=8.000 ms" in messages[0]
|
||||
assert (
|
||||
"period[n=2 mean=8.000 p95=8.900 p99=8.980 "
|
||||
"max=9.000 ms overruns=1]"
|
||||
) in messages[0]
|
||||
assert (
|
||||
"total[n=2 mean=8.000 p95=9.800 p99=9.960 "
|
||||
"max=10.000 ms overruns=1]"
|
||||
) in messages[0]
|
||||
assert "qp[n=2" in messages[0]
|
||||
assert "send[n=2" in messages[0]
|
||||
assert "feedback_age[n=2" in messages[0]
|
||||
assert all(not samples for samples in teleop._timing_samples.values())
|
||||
|
||||
|
||||
def test_joint_send_failure_requests_slow_stop_and_resets_control() -> None:
|
||||
class FailingAdapter:
|
||||
def __init__(self) -> None:
|
||||
|
||||
@@ -2,14 +2,19 @@ import math
|
||||
import time
|
||||
from types import SimpleNamespace
|
||||
|
||||
import numpy as np
|
||||
import pytest
|
||||
|
||||
from xr_rm_teleop.realman_adapter import ArmPose, JointStateSnapshot
|
||||
from xr_rm_teleop.realman_adapter import JointStateSnapshot
|
||||
from xr_rm_teleop.single_arm_velocity_teleop import (
|
||||
SingleArmVelocityTeleop,
|
||||
_euler_to_quaternion,
|
||||
_make_transform,
|
||||
_matrix_to_quaternion,
|
||||
_normalize_quaternion,
|
||||
_quaternion_to_euler,
|
||||
_project_rotation,
|
||||
_quaternion_to_matrix,
|
||||
_so3_exp,
|
||||
_so3_log,
|
||||
)
|
||||
|
||||
|
||||
@@ -18,7 +23,10 @@ def _make_teleop_for_orientation() -> SingleArmVelocityTeleop:
|
||||
teleop._enable_orientation_control = True
|
||||
teleop._enable_orientation_axes = [True, True, True]
|
||||
teleop._controller_orientation_start = (0.0, 0.0, 0.0, 1.0)
|
||||
teleop._robot_start_pose = ArmPose(0.3, 0.0, 0.2, 0.1, -0.2, 0.3)
|
||||
teleop._robot_start_transform = _make_transform(
|
||||
[0.3, 0.0, 0.2],
|
||||
_so3_exp(np.asarray([0.1, -0.2, 0.3])),
|
||||
)
|
||||
teleop._xr_to_robot_matrix = [
|
||||
0.0, 1.0, 0.0,
|
||||
0.0, 0.0, 1.0,
|
||||
@@ -27,47 +35,111 @@ def _make_teleop_for_orientation() -> SingleArmVelocityTeleop:
|
||||
return teleop
|
||||
|
||||
|
||||
def assert_angles_close(actual: list[float] | tuple[float, ...], expected: list[float]) -> None:
|
||||
assert len(actual) == len(expected)
|
||||
for actual_value, expected_value in zip(actual, expected):
|
||||
assert math.atan2(math.sin(actual_value - expected_value), math.cos(actual_value - expected_value)) == pytest.approx(0.0)
|
||||
|
||||
|
||||
def test_identity_controller_orientation_keeps_tcp_orientation() -> None:
|
||||
teleop = _make_teleop_for_orientation()
|
||||
|
||||
target = teleop._raw_orientation_from_controller((0.0, 0.0, 0.0, 1.0))
|
||||
|
||||
assert_angles_close(target, teleop._robot_start_pose.rpy())
|
||||
assert target == pytest.approx(teleop._robot_start_transform[:3, :3])
|
||||
|
||||
|
||||
def test_xr_relative_rotation_maps_through_xr_to_robot_matrix() -> None:
|
||||
teleop = _make_teleop_for_orientation()
|
||||
teleop._robot_start_pose = ArmPose(0.3, 0.0, 0.2, 0.0, 0.0, 0.0)
|
||||
xr_roll = _euler_to_quaternion(0.2, 0.0, 0.0)
|
||||
teleop._robot_start_transform = np.eye(4)
|
||||
xr_roll = _matrix_to_quaternion(_so3_exp(np.asarray([0.2, 0.0, 0.0])))
|
||||
|
||||
target = teleop._raw_orientation_from_controller(xr_roll)
|
||||
|
||||
assert_angles_close(target, [0.0, 0.0, 0.2])
|
||||
assert _so3_log(target) == pytest.approx([0.0, 0.0, 0.2])
|
||||
|
||||
|
||||
def test_orientation_deadband_filter_and_speed_limit() -> None:
|
||||
def test_quaternion_sign_does_not_change_rotation() -> None:
|
||||
quaternion = _normalize_quaternion((0.2, -0.3, 0.1, 0.9))
|
||||
|
||||
assert _quaternion_to_matrix(quaternion) == pytest.approx(
|
||||
_quaternion_to_matrix(tuple(-value for value in quaternion))
|
||||
)
|
||||
|
||||
|
||||
@pytest.mark.parametrize("pitch", [math.pi / 2.0 - 1e-5, -math.pi / 2.0 + 1e-5])
|
||||
def test_small_rotation_near_gimbal_lock_stays_small(pitch: float) -> None:
|
||||
teleop = _make_teleop_for_orientation()
|
||||
start_rotation = _so3_exp(np.asarray([0.0, pitch, 0.0]))
|
||||
teleop._robot_start_transform = _make_transform([0.3, 0.0, 0.2], start_rotation)
|
||||
teleop._xr_to_robot_matrix = np.eye(3).reshape(-1).tolist()
|
||||
controller = _matrix_to_quaternion(_so3_exp(np.asarray([0.01, 0.0, 0.0])))
|
||||
|
||||
target = teleop._raw_orientation_from_controller(controller)
|
||||
|
||||
error = _so3_log(target @ start_rotation.T)
|
||||
assert np.linalg.norm(error) == pytest.approx(0.01)
|
||||
|
||||
|
||||
def test_crossing_old_rpy_branch_uses_shortest_rotation() -> None:
|
||||
teleop = _make_teleop_for_orientation()
|
||||
start_rotation = _so3_exp(np.asarray([0.0, math.pi / 2.0 - 0.001, 0.0]))
|
||||
teleop._robot_start_transform = _make_transform([0.3, 0.0, 0.2], start_rotation)
|
||||
teleop._xr_to_robot_matrix = np.eye(3).reshape(-1).tolist()
|
||||
controller = _matrix_to_quaternion(_so3_exp(np.asarray([0.0, 0.002, 0.0])))
|
||||
|
||||
target = teleop._raw_orientation_from_controller(controller)
|
||||
|
||||
assert _so3_log(target @ start_rotation.T) == pytest.approx(
|
||||
[0.0, 0.002, 0.0],
|
||||
abs=1e-9,
|
||||
)
|
||||
|
||||
|
||||
def test_disabled_orientation_axis_zeros_robot_rotation_vector_component() -> None:
|
||||
teleop = _make_teleop_for_orientation()
|
||||
teleop._robot_start_transform = np.eye(4)
|
||||
teleop._xr_to_robot_matrix = np.eye(3).reshape(-1).tolist()
|
||||
teleop._enable_orientation_axes = [True, False, True]
|
||||
controller = _matrix_to_quaternion(_so3_exp(np.asarray([0.1, 0.2, 0.3])))
|
||||
|
||||
target = teleop._raw_orientation_from_controller(controller)
|
||||
|
||||
assert _so3_log(target) == pytest.approx([0.1, 0.0, 0.3])
|
||||
|
||||
|
||||
def test_orientation_deadband_filter_and_speed_limit_use_so3_angle() -> None:
|
||||
teleop = object.__new__(SingleArmVelocityTeleop)
|
||||
teleop._orientation_deadband_rad = 0.01
|
||||
teleop._orientation_filter_alpha = 0.5
|
||||
teleop._max_orientation_speed = 0.5
|
||||
teleop._dt = 0.1
|
||||
teleop._last_sent_orientation = [0.0, 0.0, 0.0]
|
||||
teleop._filtered_orientation_target = [0.0, 0.0, 0.0]
|
||||
teleop._dt = 1.0 / 125.0
|
||||
teleop._last_sent_orientation = np.eye(3)
|
||||
teleop._filtered_orientation_target = np.eye(3)
|
||||
|
||||
assert teleop._apply_orientation_deadband([0.001, 0.0, 0.0]) == [0.0, 0.0, 0.0]
|
||||
inside_deadband = _so3_exp(np.asarray([0.006, 0.006, 0.0]))
|
||||
assert teleop._apply_orientation_deadband(inside_deadband) == pytest.approx(np.eye(3))
|
||||
|
||||
filtered = teleop._filter_orientation_target([0.2, 0.0, 0.0])
|
||||
assert_angles_close(filtered, [0.1, 0.0, 0.0])
|
||||
target = _so3_exp(np.asarray([0.2, 0.0, 0.0]))
|
||||
filtered = teleop._filter_orientation_target(target)
|
||||
assert _so3_log(filtered) == pytest.approx([0.1, 0.0, 0.0])
|
||||
|
||||
limited, was_limited = teleop._limit_orientation_step([0.2, 0.0, 0.0])
|
||||
limited, was_limited = teleop._limit_orientation_step(target)
|
||||
assert was_limited
|
||||
assert_angles_close(limited, [0.05, 0.0, 0.0])
|
||||
assert np.linalg.norm(_so3_log(limited)) == pytest.approx(0.5 / 125.0)
|
||||
|
||||
|
||||
def test_rotation_matrix_to_debug_quaternion_is_normalized() -> None:
|
||||
quaternion = _matrix_to_quaternion(_so3_exp(np.asarray([0.2, -0.1, 0.3])))
|
||||
|
||||
assert np.isfinite(quaternion).all()
|
||||
assert np.linalg.norm(quaternion) == pytest.approx(1.0)
|
||||
|
||||
|
||||
def test_rotation_projection_accepts_small_error_and_rejects_invalid_matrix() -> None:
|
||||
near_rotation = np.eye(3)
|
||||
near_rotation[0, 1] = 1e-5
|
||||
|
||||
projected = _project_rotation(near_rotation)
|
||||
|
||||
assert projected.T @ projected == pytest.approx(np.eye(3))
|
||||
assert np.linalg.det(projected) == pytest.approx(1.0)
|
||||
with pytest.raises(ValueError):
|
||||
_project_rotation(np.diag([2.0, 1.0, 1.0]))
|
||||
|
||||
|
||||
def test_invalid_controller_quaternion_stops_current_tick() -> None:
|
||||
@@ -102,9 +174,7 @@ def test_invalid_controller_quaternion_stops_current_tick() -> None:
|
||||
time.monotonic(),
|
||||
)
|
||||
)
|
||||
teleop._ik_solver = SimpleNamespace(
|
||||
update_joint_state=lambda joints: ArmPose(0.3, 0.0, 0.2)
|
||||
)
|
||||
teleop._ik_solver = SimpleNamespace(update_joint_state=lambda joints: np.eye(4))
|
||||
teleop._active = False
|
||||
teleop._last_valid_joint_target = None
|
||||
teleop._last_current_pose = None
|
||||
@@ -119,11 +189,6 @@ def test_invalid_controller_quaternion_stops_current_tick() -> None:
|
||||
assert stopped == [True]
|
||||
|
||||
|
||||
def test_quaternion_roundtrip_for_small_rpy() -> None:
|
||||
quat = _normalize_quaternion(_euler_to_quaternion(0.2, -0.1, 0.3))
|
||||
assert_angles_close(_quaternion_to_euler(quat), [0.2, -0.1, 0.3])
|
||||
|
||||
|
||||
def test_zero_quaternion_is_invalid() -> None:
|
||||
with pytest.raises(ValueError):
|
||||
_normalize_quaternion([0.0, 0.0, 0.0, 0.0])
|
||||
|
||||
@@ -1,37 +1,72 @@
|
||||
import math
|
||||
from pathlib import Path
|
||||
from xml.etree import ElementTree
|
||||
|
||||
import numpy as np
|
||||
import pytest
|
||||
|
||||
from xr_rm_teleop.placo_ik_solver import (
|
||||
PlacoIkSolver,
|
||||
_arm_pose_to_transform,
|
||||
_tool_pose_to_transform,
|
||||
_transform_to_arm_pose,
|
||||
_validated_transform,
|
||||
)
|
||||
from xr_rm_teleop.realman_adapter import ArmPose
|
||||
|
||||
|
||||
def test_tool_offset_rotates_with_flange_and_roundtrips() -> None:
|
||||
flange_pose = ArmPose(0.30, -0.10, 0.20, 0.0, math.pi / 2.0, 0.0)
|
||||
tool_pose = [0.0, 0.0, 0.19, 0.0, 0.0, 0.0, 1.0]
|
||||
def test_fixed_urdf_has_seven_moving_joints_and_omnipicker_tcp() -> None:
|
||||
urdf_path = (
|
||||
Path(__file__).resolve().parents[1]
|
||||
/ "models"
|
||||
/ "rm75_omnipicker"
|
||||
/ "urdf"
|
||||
/ "RM75-B_OmniPicker_fixed.urdf"
|
||||
)
|
||||
root = ElementTree.parse(urdf_path).getroot()
|
||||
moving_joint_names = [
|
||||
joint.attrib["name"]
|
||||
for joint in root.findall("joint")
|
||||
if joint.attrib["type"] != "fixed"
|
||||
]
|
||||
tcp_joint = root.find("joint[@name='omnipicker_tcp_joint']")
|
||||
mesh_filenames = [
|
||||
mesh.attrib["filename"]
|
||||
for mesh in root.findall(".//mesh")
|
||||
]
|
||||
|
||||
base_to_flange = _arm_pose_to_transform(flange_pose)
|
||||
flange_to_tool = _tool_pose_to_transform(tool_pose)
|
||||
base_to_tool = base_to_flange @ flange_to_tool
|
||||
recovered_flange = base_to_tool @ np.linalg.inv(flange_to_tool)
|
||||
|
||||
assert base_to_tool[:3, 3] == pytest.approx([0.49, -0.10, 0.20])
|
||||
assert recovered_flange == pytest.approx(base_to_flange)
|
||||
assert moving_joint_names == [f"joint_{index}" for index in range(1, 8)]
|
||||
assert all(
|
||||
filename.startswith(
|
||||
"package://xr_rm_teleop/models/rm75_omnipicker/meshes/"
|
||||
)
|
||||
for filename in mesh_filenames
|
||||
)
|
||||
assert tcp_joint is not None
|
||||
assert tcp_joint.attrib["type"] == "fixed"
|
||||
assert tcp_joint.find("parent").attrib["link"] == "omnipicker_base_link"
|
||||
assert tcp_joint.find("child").attrib["link"] == "omnipicker_tcp"
|
||||
assert tcp_joint.find("origin").attrib["xyz"] == "0 0 0.16"
|
||||
assert tcp_joint.find("origin").attrib["rpy"] == "0 0 0"
|
||||
|
||||
|
||||
def test_transform_to_arm_pose_roundtrip() -> None:
|
||||
expected = ArmPose(0.25, -0.30, 0.40, 0.20, -0.30, 0.40)
|
||||
def test_validated_transform_accepts_finite_se3_and_returns_a_copy() -> None:
|
||||
transform = np.eye(4)
|
||||
transform[:3, 3] = [0.3, -0.1, 0.2]
|
||||
|
||||
actual = _transform_to_arm_pose(_arm_pose_to_transform(expected))
|
||||
actual = _validated_transform(transform)
|
||||
|
||||
assert actual.xyz() == pytest.approx(expected.xyz())
|
||||
assert actual.rpy() == pytest.approx(expected.rpy())
|
||||
assert actual == pytest.approx(transform)
|
||||
assert actual is not transform
|
||||
|
||||
|
||||
@pytest.mark.parametrize(
|
||||
"transform",
|
||||
[
|
||||
np.eye(3),
|
||||
np.full((4, 4), np.nan),
|
||||
np.vstack([np.eye(3, 4), [0.0, 0.0, 0.0, 2.0]]),
|
||||
np.diag([2.0, 1.0, 1.0, 1.0]),
|
||||
],
|
||||
)
|
||||
def test_validated_transform_rejects_invalid_se3(transform: np.ndarray) -> None:
|
||||
with pytest.raises(ValueError):
|
||||
_validated_transform(transform)
|
||||
|
||||
|
||||
def test_qp_result_rejects_nan_position_and_velocity_violations() -> None:
|
||||
|
||||
@@ -2,102 +2,43 @@
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import math
|
||||
from importlib.metadata import PackageNotFoundError, version
|
||||
from pathlib import Path
|
||||
|
||||
import numpy as np
|
||||
|
||||
from .realman_adapter import ArmPose
|
||||
|
||||
|
||||
EXPECTED_PLACO_VERSION = "0.9.4"
|
||||
RM75_JOINT_NAMES = [f"joint_{index}" for index in range(1, 8)]
|
||||
RM75_Q_SLICE = slice(7, 14)
|
||||
|
||||
|
||||
def _rpy_to_rotation(roll: float, pitch: float, yaw: float) -> np.ndarray:
|
||||
cr, sr = math.cos(roll), math.sin(roll)
|
||||
cp, sp = math.cos(pitch), math.sin(pitch)
|
||||
cy, sy = math.cos(yaw), math.sin(yaw)
|
||||
return np.array(
|
||||
[
|
||||
[cy * cp, cy * sp * sr - sy * cr, cy * sp * cr + sy * sr],
|
||||
[sy * cp, sy * sp * sr + cy * cr, sy * sp * cr - cy * sr],
|
||||
[-sp, cp * sr, cp * cr],
|
||||
],
|
||||
dtype=float,
|
||||
)
|
||||
def _validated_transform(transform: np.ndarray) -> np.ndarray:
|
||||
values = np.asarray(transform, dtype=float)
|
||||
if values.shape != (4, 4) or not np.isfinite(values).all():
|
||||
raise ValueError("target transform must be a finite 4x4 matrix")
|
||||
if not np.allclose(values[3], [0.0, 0.0, 0.0, 1.0], atol=1e-9):
|
||||
raise ValueError("target transform must have a valid homogeneous row")
|
||||
|
||||
rotation = values[:3, :3]
|
||||
if (
|
||||
np.linalg.norm(rotation.T @ rotation - np.eye(3)) > 1e-3
|
||||
or np.linalg.det(rotation) <= 0.0
|
||||
):
|
||||
raise ValueError("target transform must contain a valid rotation")
|
||||
u, _, vt = np.linalg.svd(rotation)
|
||||
projected = u @ vt
|
||||
if np.linalg.det(projected) <= 0.0:
|
||||
raise ValueError("target transform must contain a proper rotation")
|
||||
|
||||
def _rotation_to_rpy(rotation: np.ndarray) -> tuple[float, float, float]:
|
||||
pitch = math.asin(-float(np.clip(rotation[2, 0], -1.0, 1.0)))
|
||||
if abs(math.cos(pitch)) > 1e-9:
|
||||
roll = math.atan2(float(rotation[2, 1]), float(rotation[2, 2]))
|
||||
yaw = math.atan2(float(rotation[1, 0]), float(rotation[0, 0]))
|
||||
else:
|
||||
roll = math.atan2(-float(rotation[1, 2]), float(rotation[1, 1]))
|
||||
yaw = 0.0
|
||||
return roll, pitch, yaw
|
||||
|
||||
|
||||
def _arm_pose_to_transform(pose: ArmPose) -> np.ndarray:
|
||||
transform = np.eye(4)
|
||||
transform[:3, :3] = _rpy_to_rotation(pose.rx, pose.ry, pose.rz)
|
||||
transform[:3, 3] = pose.xyz()
|
||||
return transform
|
||||
|
||||
|
||||
def _tool_pose_to_transform(tool_pose: list[float]) -> np.ndarray:
|
||||
values = np.asarray(tool_pose, dtype=float)
|
||||
if values.shape != (7,) or not np.isfinite(values).all():
|
||||
raise ValueError("tool pose must contain 7 finite values")
|
||||
x, y, z, qx, qy, qz, qw = values
|
||||
norm = math.sqrt(qx * qx + qy * qy + qz * qz + qw * qw)
|
||||
if norm <= 1e-9:
|
||||
raise ValueError("tool quaternion norm must be positive")
|
||||
qx, qy, qz, qw = qx / norm, qy / norm, qz / norm, qw / norm
|
||||
transform = np.eye(4)
|
||||
transform[:3, :3] = np.array(
|
||||
[
|
||||
[
|
||||
1 - 2 * (qy * qy + qz * qz),
|
||||
2 * (qx * qy - qz * qw),
|
||||
2 * (qx * qz + qy * qw),
|
||||
],
|
||||
[
|
||||
2 * (qx * qy + qz * qw),
|
||||
1 - 2 * (qx * qx + qz * qz),
|
||||
2 * (qy * qz - qx * qw),
|
||||
],
|
||||
[
|
||||
2 * (qx * qz - qy * qw),
|
||||
2 * (qy * qz + qx * qw),
|
||||
1 - 2 * (qx * qx + qy * qy),
|
||||
],
|
||||
]
|
||||
)
|
||||
transform[:3, 3] = [x, y, z]
|
||||
return transform
|
||||
|
||||
|
||||
def _transform_to_arm_pose(transform: np.ndarray) -> ArmPose:
|
||||
roll, pitch, yaw = _rotation_to_rpy(transform[:3, :3])
|
||||
return ArmPose(
|
||||
float(transform[0, 3]),
|
||||
float(transform[1, 3]),
|
||||
float(transform[2, 3]),
|
||||
roll,
|
||||
pitch,
|
||||
yaw,
|
||||
)
|
||||
result = values.copy()
|
||||
result[:3, :3] = projected
|
||||
return result
|
||||
|
||||
|
||||
class PlacoIkSolver:
|
||||
def __init__(
|
||||
self,
|
||||
urdf_path: str,
|
||||
tool_pose: list[float],
|
||||
dt: float,
|
||||
) -> None:
|
||||
if dt <= 0.0:
|
||||
@@ -147,15 +88,16 @@ class PlacoIkSolver:
|
||||
for index in velocity_offsets
|
||||
]
|
||||
)
|
||||
self._tool_transform = _tool_pose_to_transform(tool_pose)
|
||||
self._tool_inverse = np.linalg.inv(self._tool_transform)
|
||||
self._actual_joints: np.ndarray | None = None
|
||||
|
||||
self._solver = placo.KinematicsSolver(self._robot)
|
||||
self._solver.dt = dt
|
||||
self._solver.mask_fbase(True)
|
||||
self._solver.enable_velocity_limits(True)
|
||||
self._frame_task = self._solver.add_frame_task("link_7", np.eye(4))
|
||||
self._frame_task = self._solver.add_frame_task(
|
||||
"omnipicker_tcp",
|
||||
np.eye(4),
|
||||
)
|
||||
self._frame_task.configure("rm75_frame", "soft", 1.0)
|
||||
manipulability = self._solver.add_manipulability_task(
|
||||
"link_7",
|
||||
@@ -169,7 +111,7 @@ class PlacoIkSolver:
|
||||
def base_configuration(self) -> list[float]:
|
||||
return self._robot.state.q[:7].tolist()
|
||||
|
||||
def update_joint_state(self, joints: list[float]) -> ArmPose:
|
||||
def update_joint_state(self, joints: list[float]) -> np.ndarray:
|
||||
values = np.asarray(joints, dtype=float)
|
||||
if values.shape != (7,) or not np.isfinite(values).all():
|
||||
raise ValueError("joint state must contain 7 finite values")
|
||||
@@ -177,18 +119,15 @@ class PlacoIkSolver:
|
||||
self._actual_joints = values.copy()
|
||||
self._robot.state.q[RM75_Q_SLICE] = values
|
||||
self._robot.update_kinematics()
|
||||
base_to_flange = self._robot.get_T_world_frame("link_7")
|
||||
base_to_tool = self._robot.get_T_world_frame("omnipicker_tcp")
|
||||
if is_first_feedback:
|
||||
self._frame_task.T_world_frame = base_to_flange.copy()
|
||||
base_to_tool = base_to_flange @ self._tool_transform
|
||||
return _transform_to_arm_pose(base_to_tool)
|
||||
self._frame_task.T_world_frame = base_to_tool.copy()
|
||||
return base_to_tool.copy()
|
||||
|
||||
def solve(self, target_tool_pose: ArmPose) -> list[float]:
|
||||
def solve(self, target_tool_pose: np.ndarray) -> list[float]:
|
||||
if self._actual_joints is None:
|
||||
raise RuntimeError("joint state must be initialized before QP solve")
|
||||
self._frame_task.T_world_frame = (
|
||||
_arm_pose_to_transform(target_tool_pose) @ self._tool_inverse
|
||||
)
|
||||
self._frame_task.T_world_frame = _validated_transform(target_tool_pose)
|
||||
self._solver.solve(True)
|
||||
result = np.asarray(
|
||||
self._robot.state.q[RM75_Q_SLICE],
|
||||
|
||||
@@ -12,6 +12,7 @@ import threading
|
||||
import time
|
||||
from typing import Iterable
|
||||
|
||||
import numpy as np
|
||||
import rclpy
|
||||
from geometry_msgs.msg import PoseStamped, TwistStamped
|
||||
from rclpy.node import Node
|
||||
@@ -23,7 +24,6 @@ from xr_rm_interfaces.msg import XrController
|
||||
from .fun_peripheral import load_peripheral_config
|
||||
from .placo_ik_solver import PlacoIkSolver
|
||||
from .realman_adapter import (
|
||||
ArmPose,
|
||||
JointStateSnapshot,
|
||||
MockRealManAdapter,
|
||||
RealManAdapter,
|
||||
@@ -38,14 +38,6 @@ def _clamp(value: float, low: float, high: float) -> float:
|
||||
return min(max(value, low), high)
|
||||
|
||||
|
||||
def _wrap_angle(angle: float) -> float:
|
||||
return math.atan2(math.sin(angle), math.cos(angle))
|
||||
|
||||
|
||||
def _angle_delta(target: float, current: float) -> float:
|
||||
return _wrap_angle(target - current)
|
||||
|
||||
|
||||
def _normalize_quaternion(values: Iterable[float]) -> tuple[float, float, float, float]:
|
||||
x, y, z, w = [float(value) for value in values]
|
||||
norm = math.sqrt(x * x + y * y + z * z + w * w)
|
||||
@@ -54,31 +46,26 @@ def _normalize_quaternion(values: Iterable[float]) -> tuple[float, float, float,
|
||||
return x / norm, y / norm, z / norm, w / norm
|
||||
|
||||
|
||||
def _quaternion_conjugate(
|
||||
def _project_rotation(rotation: np.ndarray) -> np.ndarray:
|
||||
values = np.asarray(rotation, dtype=float)
|
||||
if values.shape != (3, 3) or not np.isfinite(values).all():
|
||||
raise ValueError("rotation must be a finite 3x3 matrix")
|
||||
if (
|
||||
np.linalg.norm(values.T @ values - np.eye(3)) > 1e-3
|
||||
or np.linalg.det(values) <= 0.0
|
||||
):
|
||||
raise ValueError("rotation matrix is invalid")
|
||||
u, _, vt = np.linalg.svd(values)
|
||||
projected = u @ vt
|
||||
if np.linalg.det(projected) <= 0.0:
|
||||
raise ValueError("rotation matrix must be proper")
|
||||
return projected
|
||||
|
||||
|
||||
def _quaternion_to_matrix(
|
||||
quat: tuple[float, float, float, float],
|
||||
) -> tuple[float, float, float, float]:
|
||||
x, y, z, w = quat
|
||||
return -x, -y, -z, w
|
||||
|
||||
|
||||
def _quaternion_multiply(
|
||||
left: tuple[float, float, float, float],
|
||||
right: tuple[float, float, float, float],
|
||||
) -> tuple[float, float, float, float]:
|
||||
lx, ly, lz, lw = left
|
||||
rx, ry, rz, rw = right
|
||||
return _normalize_quaternion(
|
||||
(
|
||||
lw * rx + lx * rw + ly * rz - lz * ry,
|
||||
lw * ry - lx * rz + ly * rw + lz * rx,
|
||||
lw * rz + lx * ry - ly * rx + lz * rw,
|
||||
lw * rw - lx * rx - ly * ry - lz * rz,
|
||||
)
|
||||
)
|
||||
|
||||
|
||||
def _quaternion_to_matrix(quat: tuple[float, float, float, float]) -> list[float]:
|
||||
x, y, z, w = quat
|
||||
) -> np.ndarray:
|
||||
x, y, z, w = _normalize_quaternion(quat)
|
||||
xx = x * x
|
||||
yy = y * y
|
||||
zz = z * z
|
||||
@@ -88,69 +75,95 @@ def _quaternion_to_matrix(quat: tuple[float, float, float, float]) -> list[float
|
||||
wx = w * x
|
||||
wy = w * y
|
||||
wz = w * z
|
||||
return [
|
||||
1.0 - 2.0 * (yy + zz),
|
||||
2.0 * (xy - wz),
|
||||
2.0 * (xz + wy),
|
||||
2.0 * (xy + wz),
|
||||
1.0 - 2.0 * (xx + zz),
|
||||
2.0 * (yz - wx),
|
||||
2.0 * (xz - wy),
|
||||
2.0 * (yz + wx),
|
||||
1.0 - 2.0 * (xx + yy),
|
||||
return np.asarray(
|
||||
[
|
||||
[1.0 - 2.0 * (yy + zz), 2.0 * (xy - wz), 2.0 * (xz + wy)],
|
||||
[2.0 * (xy + wz), 1.0 - 2.0 * (xx + zz), 2.0 * (yz - wx)],
|
||||
[2.0 * (xz - wy), 2.0 * (yz + wx), 1.0 - 2.0 * (xx + yy)],
|
||||
]
|
||||
)
|
||||
|
||||
|
||||
def _matrix_multiply(left: list[float], right: list[float]) -> list[float]:
|
||||
return [
|
||||
sum(left[row * 3 + k] * right[k * 3 + col] for k in range(3))
|
||||
for row in range(3)
|
||||
for col in range(3)
|
||||
]
|
||||
|
||||
|
||||
def _matrix_transpose(matrix: list[float]) -> list[float]:
|
||||
return [
|
||||
matrix[0], matrix[3], matrix[6],
|
||||
matrix[1], matrix[4], matrix[7],
|
||||
matrix[2], matrix[5], matrix[8],
|
||||
]
|
||||
|
||||
|
||||
def _euler_to_quaternion(roll: float, pitch: float, yaw: float) -> tuple[float, float, float, float]:
|
||||
cy = math.cos(yaw * 0.5)
|
||||
sy = math.sin(yaw * 0.5)
|
||||
cp = math.cos(pitch * 0.5)
|
||||
sp = math.sin(pitch * 0.5)
|
||||
cr = math.cos(roll * 0.5)
|
||||
sr = math.sin(roll * 0.5)
|
||||
|
||||
qw = cr * cp * cy + sr * sp * sy
|
||||
qx = sr * cp * cy - cr * sp * sy
|
||||
qy = cr * sp * cy + sr * cp * sy
|
||||
qz = cr * cp * sy - sr * sp * cy
|
||||
return _normalize_quaternion((qx, qy, qz, qw))
|
||||
|
||||
|
||||
def _euler_to_matrix(roll: float, pitch: float, yaw: float) -> list[float]:
|
||||
return _quaternion_to_matrix(_euler_to_quaternion(roll, pitch, yaw))
|
||||
|
||||
|
||||
def _matrix_to_euler(matrix: list[float]) -> tuple[float, float, float]:
|
||||
sy = -_clamp(matrix[6], -1.0, 1.0)
|
||||
pitch = math.asin(sy)
|
||||
cp = math.cos(pitch)
|
||||
if abs(cp) > 1e-9:
|
||||
roll = math.atan2(matrix[7], matrix[8])
|
||||
yaw = math.atan2(matrix[3], matrix[0])
|
||||
def _matrix_to_quaternion(
|
||||
rotation: np.ndarray,
|
||||
) -> tuple[float, float, float, float]:
|
||||
matrix = _project_rotation(rotation)
|
||||
trace = float(np.trace(matrix))
|
||||
if trace > 0.0:
|
||||
scale = math.sqrt(trace + 1.0) * 2.0
|
||||
quaternion = (
|
||||
(matrix[2, 1] - matrix[1, 2]) / scale,
|
||||
(matrix[0, 2] - matrix[2, 0]) / scale,
|
||||
(matrix[1, 0] - matrix[0, 1]) / scale,
|
||||
0.25 * scale,
|
||||
)
|
||||
elif matrix[0, 0] > matrix[1, 1] and matrix[0, 0] > matrix[2, 2]:
|
||||
scale = math.sqrt(1.0 + matrix[0, 0] - matrix[1, 1] - matrix[2, 2]) * 2.0
|
||||
quaternion = (
|
||||
0.25 * scale,
|
||||
(matrix[0, 1] + matrix[1, 0]) / scale,
|
||||
(matrix[0, 2] + matrix[2, 0]) / scale,
|
||||
(matrix[2, 1] - matrix[1, 2]) / scale,
|
||||
)
|
||||
elif matrix[1, 1] > matrix[2, 2]:
|
||||
scale = math.sqrt(1.0 + matrix[1, 1] - matrix[0, 0] - matrix[2, 2]) * 2.0
|
||||
quaternion = (
|
||||
(matrix[0, 1] + matrix[1, 0]) / scale,
|
||||
0.25 * scale,
|
||||
(matrix[1, 2] + matrix[2, 1]) / scale,
|
||||
(matrix[0, 2] - matrix[2, 0]) / scale,
|
||||
)
|
||||
else:
|
||||
roll = math.atan2(-matrix[5], matrix[4])
|
||||
yaw = 0.0
|
||||
return _wrap_angle(roll), _wrap_angle(pitch), _wrap_angle(yaw)
|
||||
scale = math.sqrt(1.0 + matrix[2, 2] - matrix[0, 0] - matrix[1, 1]) * 2.0
|
||||
quaternion = (
|
||||
(matrix[0, 2] + matrix[2, 0]) / scale,
|
||||
(matrix[1, 2] + matrix[2, 1]) / scale,
|
||||
0.25 * scale,
|
||||
(matrix[1, 0] - matrix[0, 1]) / scale,
|
||||
)
|
||||
result = _normalize_quaternion(quaternion)
|
||||
return tuple(-value for value in result) if result[3] < 0.0 else result
|
||||
|
||||
|
||||
def _quaternion_to_euler(quat: tuple[float, float, float, float]) -> tuple[float, float, float]:
|
||||
return _matrix_to_euler(_quaternion_to_matrix(quat))
|
||||
def _so3_log(rotation: np.ndarray) -> np.ndarray:
|
||||
quaternion = _matrix_to_quaternion(rotation)
|
||||
vector = np.asarray(quaternion[:3])
|
||||
sine_half = float(np.linalg.norm(vector))
|
||||
if sine_half <= 1e-12:
|
||||
return 2.0 * vector
|
||||
angle = 2.0 * math.atan2(sine_half, quaternion[3])
|
||||
return vector * (angle / sine_half)
|
||||
|
||||
|
||||
def _so3_exp(rotation_vector: np.ndarray) -> np.ndarray:
|
||||
vector = np.asarray(rotation_vector, dtype=float)
|
||||
if vector.shape != (3,) or not np.isfinite(vector).all():
|
||||
raise ValueError("rotation vector must contain 3 finite values")
|
||||
angle = float(np.linalg.norm(vector))
|
||||
skew = np.asarray(
|
||||
[
|
||||
[0.0, -vector[2], vector[1]],
|
||||
[vector[2], 0.0, -vector[0]],
|
||||
[-vector[1], vector[0], 0.0],
|
||||
]
|
||||
)
|
||||
if angle <= 1e-9:
|
||||
return _project_rotation(np.eye(3) + skew + 0.5 * skew @ skew)
|
||||
return _project_rotation(
|
||||
np.eye(3)
|
||||
+ math.sin(angle) / angle * skew
|
||||
+ (1.0 - math.cos(angle)) / (angle * angle) * skew @ skew
|
||||
)
|
||||
|
||||
|
||||
def _make_transform(position: Iterable[float], rotation: np.ndarray) -> np.ndarray:
|
||||
xyz = np.asarray(list(position), dtype=float)
|
||||
if xyz.shape != (3,) or not np.isfinite(xyz).all():
|
||||
raise ValueError("position must contain 3 finite values")
|
||||
transform = np.eye(4)
|
||||
transform[:3, :3] = _project_rotation(rotation)
|
||||
transform[:3, 3] = xyz
|
||||
return transform
|
||||
|
||||
|
||||
class SingleArmVelocityTeleop(Node):
|
||||
@@ -164,7 +177,7 @@ class SingleArmVelocityTeleop(Node):
|
||||
|
||||
self.declare_parameter("arm_name", "rm75")
|
||||
self.declare_parameter("controller_topic", "/xr/right_controller")
|
||||
self.declare_parameter("control_rate_hz", 90.0)
|
||||
self.declare_parameter("control_rate_hz", 125.0)
|
||||
self.declare_parameter("command_timeout_sec", 0.12)
|
||||
self.declare_parameter("scale", 1.0)
|
||||
self.declare_parameter("deadband_m", 0.001)
|
||||
@@ -177,7 +190,7 @@ class SingleArmVelocityTeleop(Node):
|
||||
self.declare_parameter("enable_orientation_axes", [True, True, True])
|
||||
self.declare_parameter("orientation_deadband_rad", 0.005)
|
||||
self.declare_parameter("orientation_filter_alpha", 0.65)
|
||||
self.declare_parameter("max_orientation_speed", 0.6)
|
||||
self.declare_parameter("max_orientation_speed", 0.5)
|
||||
self.declare_parameter("workspace_min", [0.20, -0.35, 0.10])
|
||||
self.declare_parameter("workspace_max", [0.65, 0.35, 0.60])
|
||||
self.declare_parameter("cyl_radius_limit", [0.20, 0.60])
|
||||
@@ -252,13 +265,13 @@ class SingleArmVelocityTeleop(Node):
|
||||
self._active = False
|
||||
self._controller_start: list[float] | None = None
|
||||
self._controller_orientation_start: tuple[float, float, float, float] | None = None
|
||||
self._robot_start_pose: ArmPose | None = None
|
||||
self._robot_start_transform: np.ndarray | None = None
|
||||
self._filtered_target: list[float] | None = None
|
||||
self._filtered_orientation_target: list[float] | None = None
|
||||
self._filtered_orientation_target: np.ndarray | None = None
|
||||
self._last_sent_target: list[float] | None = None
|
||||
self._last_sent_orientation: list[float] | None = None
|
||||
self._last_sent_orientation: np.ndarray | None = None
|
||||
self._last_command_time: Time | None = None
|
||||
self._last_current_pose: ArmPose | None = None
|
||||
self._last_current_pose: np.ndarray | None = None
|
||||
self._last_valid_joint_target: list[float] | None = None
|
||||
self._joint_feedback_ready = False
|
||||
self._stop_sent = True
|
||||
@@ -267,6 +280,12 @@ class SingleArmVelocityTeleop(Node):
|
||||
self._tool_command_queue: queue.Queue[tuple[bool, str] | None] | None = None
|
||||
self._tool_worker_stop = threading.Event()
|
||||
self._tool_worker_thread: threading.Thread | None = None
|
||||
self._timing_stats_window = max(1, int(round(5.0 / self._dt)))
|
||||
self._timing_samples = {
|
||||
name: []
|
||||
for name in ("period", "total", "qp", "send", "feedback_age")
|
||||
}
|
||||
self._last_control_tick_started_ns: int | None = None
|
||||
|
||||
peripheral_arm = self._peripheral_arm_name()
|
||||
config_file = str(self.get_parameter("peripheral_config_file").value)
|
||||
@@ -276,7 +295,6 @@ class SingleArmVelocityTeleop(Node):
|
||||
)
|
||||
self._ik_solver = PlacoIkSolver(
|
||||
str(self.get_parameter("robot_urdf_path").value),
|
||||
self._peripheral_config.tool_pose,
|
||||
self._dt,
|
||||
)
|
||||
self._adapter = self._make_adapter()
|
||||
@@ -454,6 +472,18 @@ class SingleArmVelocityTeleop(Node):
|
||||
self._enqueue_tool_command(self._trigger_tool_open, "trigger")
|
||||
|
||||
def _control_tick(self) -> None:
|
||||
tick_started_ns = time.perf_counter_ns()
|
||||
last_tick_started_ns = getattr(
|
||||
self,
|
||||
"_last_control_tick_started_ns",
|
||||
None,
|
||||
)
|
||||
self._last_control_tick_started_ns = tick_started_ns
|
||||
tick_period_ms = (
|
||||
None
|
||||
if last_tick_started_ns is None
|
||||
else (tick_started_ns - last_tick_started_ns) * 1e-6
|
||||
)
|
||||
now = self.get_clock().now()
|
||||
snapshot = self._fresh_joint_state()
|
||||
if snapshot is None:
|
||||
@@ -522,67 +552,93 @@ class SingleArmVelocityTeleop(Node):
|
||||
)
|
||||
return
|
||||
|
||||
feedback_age_ms = (time.monotonic() - snapshot.received_at) * 1000.0
|
||||
assert self._controller_start is not None
|
||||
assert self._robot_start_pose is not None
|
||||
assert self._robot_start_transform is not None
|
||||
|
||||
raw_target_xyz = self._raw_target_from_controller(controller_now)
|
||||
raw_target_rpy = self._raw_orientation_from_controller(controller_quat)
|
||||
raw_target_orientation = self._raw_orientation_from_controller(
|
||||
controller_quat
|
||||
)
|
||||
workspace_target, workspace_clamped = self._clamp_workspace_with_flag(raw_target_xyz)
|
||||
desired_target = self._apply_deadband(workspace_target)
|
||||
filtered_target = self._filter_target(desired_target)
|
||||
sent_target, step_limited = self._limit_target_step(filtered_target)
|
||||
sent_target, final_clamped = self._clamp_workspace_with_flag(sent_target)
|
||||
desired_orientation = self._apply_orientation_deadband(raw_target_rpy)
|
||||
desired_orientation = self._apply_orientation_deadband(
|
||||
raw_target_orientation
|
||||
)
|
||||
filtered_orientation = self._filter_orientation_target(desired_orientation)
|
||||
sent_orientation, orientation_step_limited = self._limit_orientation_step(filtered_orientation)
|
||||
|
||||
velocity = self._estimate_command_velocity(sent_target, sent_orientation, now)
|
||||
target_clamped = workspace_clamped or step_limited or final_clamped or orientation_step_limited
|
||||
raw_target_pose = ArmPose(
|
||||
x=raw_target_xyz[0],
|
||||
y=raw_target_xyz[1],
|
||||
z=raw_target_xyz[2],
|
||||
rx=raw_target_rpy[0],
|
||||
ry=raw_target_rpy[1],
|
||||
rz=raw_target_rpy[2],
|
||||
raw_target_pose = _make_transform(
|
||||
raw_target_xyz,
|
||||
raw_target_orientation,
|
||||
)
|
||||
target_pose = ArmPose(
|
||||
x=sent_target[0],
|
||||
y=sent_target[1],
|
||||
z=sent_target[2],
|
||||
rx=sent_orientation[0],
|
||||
ry=sent_orientation[1],
|
||||
rz=sent_orientation[2],
|
||||
target_pose = _make_transform(
|
||||
sent_target,
|
||||
sent_orientation,
|
||||
)
|
||||
|
||||
self._publish_debug(raw_target_pose, target_pose, velocity, target_clamped)
|
||||
qp_started_ns = time.perf_counter_ns()
|
||||
joint_target = self._solve_joint_target(target_pose)
|
||||
if self._send_joint_target(joint_target):
|
||||
qp_ms = (time.perf_counter_ns() - qp_started_ns) * 1e-6
|
||||
send_started_ns = time.perf_counter_ns()
|
||||
sent = self._send_joint_target(joint_target)
|
||||
send_ms = (time.perf_counter_ns() - send_started_ns) * 1e-6
|
||||
if sent:
|
||||
self._last_sent_target = sent_target
|
||||
self._last_sent_orientation = sent_orientation
|
||||
self._last_sent_orientation = sent_orientation.copy()
|
||||
self._last_command_time = now
|
||||
self._stop_sent = False
|
||||
total_ms = (time.perf_counter_ns() - tick_started_ns) * 1e-6
|
||||
try:
|
||||
self._record_timing_sample(
|
||||
tick_period_ms,
|
||||
total_ms,
|
||||
qp_ms,
|
||||
send_ms,
|
||||
feedback_age_ms,
|
||||
)
|
||||
except Exception as exc:
|
||||
self.get_logger().warn(
|
||||
f"{self._arm_name} 控制周期统计失败：{exc}",
|
||||
throttle_duration_sec=5.0,
|
||||
)
|
||||
|
||||
def _enter_active_control(
|
||||
self,
|
||||
controller_now: list[float],
|
||||
controller_quat: tuple[float, float, float, float],
|
||||
robot_pose: ArmPose,
|
||||
robot_pose: np.ndarray,
|
||||
now: Time,
|
||||
) -> None:
|
||||
robot_xyz = robot_pose.xyz()
|
||||
robot_transform = _make_transform(
|
||||
robot_pose[:3, 3],
|
||||
robot_pose[:3, :3],
|
||||
)
|
||||
robot_xyz = robot_transform[:3, 3].tolist()
|
||||
robot_orientation = robot_transform[:3, :3]
|
||||
self._active = True
|
||||
self._controller_start = controller_now
|
||||
self._controller_orientation_start = controller_quat
|
||||
self._robot_start_pose = robot_pose
|
||||
self._robot_start_transform = robot_transform
|
||||
self._filtered_target = robot_xyz
|
||||
self._filtered_orientation_target = robot_pose.rpy()
|
||||
self._filtered_orientation_target = robot_orientation.copy()
|
||||
self._last_sent_target = robot_xyz
|
||||
self._last_sent_orientation = robot_pose.rpy()
|
||||
self._last_sent_orientation = robot_orientation.copy()
|
||||
self._last_command_time = now
|
||||
self._stop_sent = True
|
||||
self.get_logger().info(f"{self._arm_name} Grip 按下，已锁定手柄和机械臂初始位姿。")
|
||||
self._publish_debug(robot_pose, robot_pose, [0.0] * 6, False)
|
||||
self._publish_debug(
|
||||
robot_transform,
|
||||
robot_transform,
|
||||
[0.0] * 6,
|
||||
False,
|
||||
)
|
||||
|
||||
@staticmethod
|
||||
def _controller_xyz(msg: XrController) -> list[float]:
|
||||
@@ -602,15 +658,21 @@ class SingleArmVelocityTeleop(Node):
|
||||
|
||||
def _raw_target_from_controller(self, controller_now: list[float]) -> list[float]:
|
||||
assert self._controller_start is not None
|
||||
assert self._robot_start_pose is not None
|
||||
assert self._robot_start_transform is not None
|
||||
robot_start_xyz = self._robot_start_transform[:3, 3]
|
||||
controller_delta = [controller_now[i] - self._controller_start[i] for i in range(3)]
|
||||
robot_delta = self._map_xr_delta_to_robot(controller_delta)
|
||||
target = [
|
||||
self._robot_start_pose.x + self._scale * robot_delta[0],
|
||||
self._robot_start_pose.y + self._scale * robot_delta[1],
|
||||
self._robot_start_pose.z + self._scale * robot_delta[2],
|
||||
robot_start_xyz[0] + self._scale * robot_delta[0],
|
||||
robot_start_xyz[1] + self._scale * robot_delta[1],
|
||||
robot_start_xyz[2] + self._scale * robot_delta[2],
|
||||
]
|
||||
return [
|
||||
float(target[index])
|
||||
if self._enable_position_axes[index]
|
||||
else float(robot_start_xyz[index])
|
||||
for index in range(3)
|
||||
]
|
||||
return [target[i] if self._enable_position_axes[i] else self._robot_start_pose.xyz()[i] for i in range(3)]
|
||||
|
||||
def _map_xr_delta_to_robot(self, delta: list[float]) -> list[float]:
|
||||
matrix = self._xr_to_robot_matrix
|
||||
@@ -623,34 +685,33 @@ class SingleArmVelocityTeleop(Node):
|
||||
def _raw_orientation_from_controller(
|
||||
self,
|
||||
controller_quat: tuple[float, float, float, float],
|
||||
) -> list[float]:
|
||||
assert self._robot_start_pose is not None
|
||||
) -> np.ndarray:
|
||||
assert self._robot_start_transform is not None
|
||||
robot_start_rotation = self._robot_start_transform[:3, :3]
|
||||
if not self._enable_orientation_control:
|
||||
return self._robot_start_pose.rpy()
|
||||
return robot_start_rotation.copy()
|
||||
|
||||
assert self._controller_orientation_start is not None
|
||||
xr_delta_quat = _quaternion_multiply(
|
||||
controller_quat,
|
||||
_quaternion_conjugate(self._controller_orientation_start),
|
||||
xr_delta_matrix = (
|
||||
_quaternion_to_matrix(controller_quat)
|
||||
@ _quaternion_to_matrix(self._controller_orientation_start).T
|
||||
)
|
||||
xr_delta_matrix = _quaternion_to_matrix(xr_delta_quat)
|
||||
matrix = self._xr_to_robot_matrix
|
||||
robot_delta_matrix = _matrix_multiply(
|
||||
_matrix_multiply(matrix, xr_delta_matrix),
|
||||
_matrix_transpose(matrix),
|
||||
mapping = np.asarray(self._xr_to_robot_matrix, dtype=float).reshape(3, 3)
|
||||
robot_delta = _project_rotation(
|
||||
mapping @ xr_delta_matrix @ mapping.T
|
||||
)
|
||||
robot_start_matrix = _euler_to_matrix(
|
||||
self._robot_start_pose.rx,
|
||||
self._robot_start_pose.ry,
|
||||
self._robot_start_pose.rz,
|
||||
)
|
||||
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)
|
||||
rotation_vector = _so3_log(robot_delta)
|
||||
rotation_vector = np.asarray(
|
||||
[
|
||||
rotation_vector[index]
|
||||
if self._enable_orientation_axes[index]
|
||||
else 0.0
|
||||
for index in range(3)
|
||||
]
|
||||
)
|
||||
return _project_rotation(
|
||||
_so3_exp(rotation_vector) @ robot_start_rotation
|
||||
)
|
||||
|
||||
def _apply_deadband(self, target: list[float]) -> list[float]:
|
||||
if self._deadband_m <= 0.0 or self._last_sent_target is None:
|
||||
@@ -699,50 +760,49 @@ class SingleArmVelocityTeleop(Node):
|
||||
for i in range(3)
|
||||
], True
|
||||
|
||||
def _apply_orientation_deadband(self, target_rpy: list[float]) -> list[float]:
|
||||
def _apply_orientation_deadband(self, target_rotation: np.ndarray) -> np.ndarray:
|
||||
if self._orientation_deadband_rad <= 0.0 or self._last_sent_orientation is None:
|
||||
return [_wrap_angle(value) for value in target_rpy]
|
||||
delta = [
|
||||
_angle_delta(target_rpy[i], self._last_sent_orientation[i])
|
||||
for i in range(3)
|
||||
]
|
||||
if _norm(delta) < self._orientation_deadband_rad:
|
||||
return list(self._last_sent_orientation)
|
||||
return [_wrap_angle(value) for value in target_rpy]
|
||||
|
||||
def _filter_orientation_target(self, target_rpy: list[float]) -> list[float]:
|
||||
if self._filtered_orientation_target is None:
|
||||
self._filtered_orientation_target = [_wrap_angle(value) for value in target_rpy]
|
||||
return list(self._filtered_orientation_target)
|
||||
|
||||
self._filtered_orientation_target = [
|
||||
_wrap_angle(
|
||||
self._filtered_orientation_target[i]
|
||||
+ self._orientation_filter_alpha
|
||||
* _angle_delta(target_rpy[i], self._filtered_orientation_target[i])
|
||||
return _project_rotation(target_rotation)
|
||||
error = _so3_log(
|
||||
target_rotation @ self._last_sent_orientation.T
|
||||
)
|
||||
for i in range(3)
|
||||
]
|
||||
return list(self._filtered_orientation_target)
|
||||
if np.linalg.norm(error) < self._orientation_deadband_rad:
|
||||
return self._last_sent_orientation.copy()
|
||||
return _project_rotation(target_rotation)
|
||||
|
||||
def _limit_orientation_step(self, target_rpy: list[float]) -> tuple[list[float], bool]:
|
||||
def _filter_orientation_target(self, target_rotation: np.ndarray) -> np.ndarray:
|
||||
if self._filtered_orientation_target is None:
|
||||
self._filtered_orientation_target = _project_rotation(target_rotation)
|
||||
return self._filtered_orientation_target.copy()
|
||||
|
||||
error = _so3_log(
|
||||
target_rotation @ self._filtered_orientation_target.T
|
||||
)
|
||||
self._filtered_orientation_target = _project_rotation(
|
||||
_so3_exp(self._orientation_filter_alpha * error)
|
||||
@ self._filtered_orientation_target
|
||||
)
|
||||
return self._filtered_orientation_target.copy()
|
||||
|
||||
def _limit_orientation_step(
|
||||
self,
|
||||
target_rotation: np.ndarray,
|
||||
) -> tuple[np.ndarray, bool]:
|
||||
if self._last_sent_orientation is None:
|
||||
return [_wrap_angle(value) for value in target_rpy], False
|
||||
return _project_rotation(target_rotation), False
|
||||
|
||||
delta = [
|
||||
_angle_delta(target_rpy[i], self._last_sent_orientation[i])
|
||||
for i in range(3)
|
||||
]
|
||||
distance = _norm(delta)
|
||||
error = _so3_log(
|
||||
target_rotation @ self._last_sent_orientation.T
|
||||
)
|
||||
distance = float(np.linalg.norm(error))
|
||||
max_step = self._max_orientation_speed * self._dt
|
||||
if distance <= max_step or distance <= 1e-9:
|
||||
return [_wrap_angle(value) for value in target_rpy], False
|
||||
return _project_rotation(target_rotation), False
|
||||
|
||||
scale = max_step / distance
|
||||
return [
|
||||
_wrap_angle(self._last_sent_orientation[i] + delta[i] * scale)
|
||||
for i in range(3)
|
||||
], True
|
||||
return _project_rotation(
|
||||
_so3_exp(scale * error) @ self._last_sent_orientation
|
||||
), True
|
||||
|
||||
def _clamp_workspace_with_flag(self, target: list[float]) -> tuple[list[float], bool]:
|
||||
clamped = [
|
||||
@@ -777,7 +837,7 @@ class SingleArmVelocityTeleop(Node):
|
||||
def _estimate_command_velocity(
|
||||
self,
|
||||
target_xyz: list[float],
|
||||
target_rpy: list[float],
|
||||
target_rotation: np.ndarray,
|
||||
now: Time,
|
||||
) -> list[float]:
|
||||
if self._last_sent_target is None or self._last_sent_orientation is None:
|
||||
@@ -788,13 +848,78 @@ class SingleArmVelocityTeleop(Node):
|
||||
measured_dt = (now - self._last_command_time).nanoseconds * 1e-9
|
||||
if measured_dt > 1e-6:
|
||||
dt = measured_dt
|
||||
angular_velocity = (
|
||||
_so3_log(target_rotation @ self._last_sent_orientation.T) / dt
|
||||
)
|
||||
return [
|
||||
(target_xyz[i] - self._last_sent_target[i]) / dt
|
||||
for i in range(3)
|
||||
] + [
|
||||
_angle_delta(target_rpy[i], self._last_sent_orientation[i]) / dt
|
||||
for i in range(3)
|
||||
] + angular_velocity.tolist()
|
||||
|
||||
@staticmethod
|
||||
def _timing_summary(
|
||||
name: str,
|
||||
samples: list[float],
|
||||
deadline_ms: float | None = None,
|
||||
) -> str:
|
||||
values = np.asarray(samples)
|
||||
result = (
|
||||
f"{name}[n={len(samples)} mean={np.mean(values):.3f} "
|
||||
f"p95={np.percentile(values, 95):.3f} "
|
||||
f"p99={np.percentile(values, 99):.3f} "
|
||||
f"max={np.max(values):.3f} ms"
|
||||
)
|
||||
if deadline_ms is not None:
|
||||
result += (
|
||||
f" overruns={int(np.count_nonzero(values > deadline_ms))}"
|
||||
)
|
||||
return result + "]"
|
||||
|
||||
def _record_timing_sample(
|
||||
self,
|
||||
period_ms: float | None,
|
||||
total_ms: float,
|
||||
qp_ms: float,
|
||||
send_ms: float,
|
||||
feedback_age_ms: float,
|
||||
) -> None:
|
||||
if period_ms is not None:
|
||||
self._timing_samples["period"].append(period_ms)
|
||||
self._timing_samples["total"].append(total_ms)
|
||||
self._timing_samples["qp"].append(qp_ms)
|
||||
self._timing_samples["send"].append(send_ms)
|
||||
self._timing_samples["feedback_age"].append(feedback_age_ms)
|
||||
if len(self._timing_samples["total"]) < self._timing_stats_window:
|
||||
return
|
||||
|
||||
sample_count = len(self._timing_samples["total"])
|
||||
deadline_ms = self._dt * 1000.0
|
||||
summaries = [
|
||||
self._timing_summary(
|
||||
"period",
|
||||
self._timing_samples["period"],
|
||||
deadline_ms,
|
||||
),
|
||||
self._timing_summary(
|
||||
"total",
|
||||
self._timing_samples["total"],
|
||||
deadline_ms,
|
||||
),
|
||||
self._timing_summary("qp", self._timing_samples["qp"]),
|
||||
self._timing_summary("send", self._timing_samples["send"]),
|
||||
self._timing_summary(
|
||||
"feedback_age",
|
||||
self._timing_samples["feedback_age"],
|
||||
),
|
||||
]
|
||||
message = (
|
||||
f"{self._arm_name} timing n={sample_count} "
|
||||
f"deadline={deadline_ms:.3f} ms | "
|
||||
+ " | ".join(summaries)
|
||||
)
|
||||
for samples in self._timing_samples.values():
|
||||
samples.clear()
|
||||
self.get_logger().info(message)
|
||||
|
||||
def _fresh_joint_state(self) -> JointStateSnapshot | None:
|
||||
snapshot = self._adapter.get_latest_joint_state()
|
||||
@@ -813,7 +938,7 @@ class SingleArmVelocityTeleop(Node):
|
||||
def _sync_joint_feedback(
|
||||
self,
|
||||
snapshot: JointStateSnapshot,
|
||||
) -> ArmPose:
|
||||
) -> np.ndarray:
|
||||
current_pose = self._ik_solver.update_joint_state(
|
||||
snapshot.positions
|
||||
)
|
||||
@@ -822,7 +947,7 @@ class SingleArmVelocityTeleop(Node):
|
||||
self._last_valid_joint_target = list(snapshot.positions)
|
||||
return current_pose
|
||||
|
||||
def _solve_joint_target(self, target_pose: ArmPose) -> list[float]:
|
||||
def _solve_joint_target(self, target_pose: np.ndarray) -> list[float]:
|
||||
if self._last_valid_joint_target is None:
|
||||
raise RuntimeError("valid joint feedback has not been initialized")
|
||||
try:
|
||||
@@ -843,7 +968,7 @@ class SingleArmVelocityTeleop(Node):
|
||||
self._active = False
|
||||
self._controller_start = None
|
||||
self._controller_orientation_start = None
|
||||
self._robot_start_pose = None
|
||||
self._robot_start_transform = None
|
||||
self._filtered_target = None
|
||||
self._filtered_orientation_target = None
|
||||
self._last_sent_target = None
|
||||
@@ -867,14 +992,19 @@ class SingleArmVelocityTeleop(Node):
|
||||
return
|
||||
self._publish_debug(pose, pose, [0.0] * 6, False)
|
||||
|
||||
def _debug_pose_fallback(self) -> ArmPose | None:
|
||||
def _debug_pose_fallback(self) -> np.ndarray | None:
|
||||
if self._last_current_pose is not None:
|
||||
return self._last_current_pose
|
||||
if self._robot_start_pose is not None:
|
||||
return self._robot_start_pose
|
||||
if self._last_sent_target is not None:
|
||||
rpy = self._last_sent_orientation or [0.0, 0.0, 0.0]
|
||||
return ArmPose(*self._last_sent_target, *rpy)
|
||||
if self._robot_start_transform is not None:
|
||||
return self._robot_start_transform
|
||||
if (
|
||||
self._last_sent_target is not None
|
||||
and self._last_sent_orientation is not None
|
||||
):
|
||||
return _make_transform(
|
||||
self._last_sent_target,
|
||||
self._last_sent_orientation,
|
||||
)
|
||||
return None
|
||||
|
||||
def _send_joint_target(self, joints: list[float]) -> bool:
|
||||
@@ -892,8 +1022,8 @@ class SingleArmVelocityTeleop(Node):
|
||||
|
||||
def _publish_debug(
|
||||
self,
|
||||
raw_target_pose: ArmPose,
|
||||
target_pose: ArmPose,
|
||||
raw_target_pose: np.ndarray,
|
||||
target_pose: np.ndarray,
|
||||
command_velocity: list[float],
|
||||
target_clamped: bool,
|
||||
) -> None:
|
||||
@@ -924,14 +1054,15 @@ class SingleArmVelocityTeleop(Node):
|
||||
self._target_clamped_pub.publish(clamped_msg)
|
||||
|
||||
@staticmethod
|
||||
def _pose_msg(stamp, pose: ArmPose) -> PoseStamped:
|
||||
qx, qy, qz, qw = _euler_to_quaternion(pose.rx, pose.ry, pose.rz)
|
||||
def _pose_msg(stamp, pose: np.ndarray) -> PoseStamped:
|
||||
transform = _make_transform(pose[:3, 3], pose[:3, :3])
|
||||
qx, qy, qz, qw = _matrix_to_quaternion(transform[:3, :3])
|
||||
msg = PoseStamped()
|
||||
msg.header.stamp = stamp
|
||||
msg.header.frame_id = "rm_base"
|
||||
msg.pose.position.x = float(pose.x)
|
||||
msg.pose.position.y = float(pose.y)
|
||||
msg.pose.position.z = float(pose.z)
|
||||
msg.pose.position.x = float(transform[0, 3])
|
||||
msg.pose.position.y = float(transform[1, 3])
|
||||
msg.pose.position.z = float(transform[2, 3])
|
||||
msg.pose.orientation.x = qx
|
||||
msg.pose.orientation.y = qy
|
||||
msg.pose.orientation.z = qz
|
||||
|
||||
Reference in New Issue
Block a user