Compare commits
8
Commits
d26ce7b945
...
75eff40fa2
| Author | SHA1 | Date | |
|---|---|---|---|
 YikaiFu-cart
|
75eff40fa2 | ||
|
YikaiFu-cart
|
21c444dcc8 | ||
|
YikaiFu-cart
|
4e068ce637 | ||
|
YikaiFu-cart
|
5267da14c2 | ||
|
YikaiFu-cart
|
3981c380ea | ||
|
YikaiFu-cart
|
80e823c097 | ||
|
YikaiFu-cart
|
b7eab7cc76 | ||
|
YikaiFu-cart
|
cf559f6d25 |
@@ -231,6 +231,7 @@
|
||||
除非用户明确要求，否则不要自动提交、推送或修改远程仓库。
|
||||
|
||||
使用 Superpowers 执行任务时，只允许按相关 skill 工作流创建本地 Git 提交；
|
||||
同一项变更生成的规格文档与实施计划必须合并为一次本地提交，不得分别提交。
|
||||
禁止执行 `git push`、合并本地分支、合并 PR 或其他远程写操作。相关 skill
|
||||
如需独立 worktree 或配套本地分支，可以创建，但不得将其合并到其他分支。
|
||||
其他情况下，除非用户明确要求，不要自动创建分支。
|
||||
|
||||
@@ -0,0 +1,486 @@
|
||||
# 手柄主键回初始位姿实施计划
|
||||
|
||||
> **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:** 左手 X 键和右手 A 键分别让对应机械臂安全回到配置的初始关节位姿，并同步三份机械臂配置中的新关节角。
|
||||
|
||||
**Architecture:** 继续使用现有左右手柄独立话题和单臂遥操作节点，不增加协调节点。遥操作节点检测自身 `XrController.primary` 的上升沿，先停止当前遥操作，再调用真机或 mock 适配器的同名回位方法并重新同步关节状态。
|
||||
|
||||
**Tech Stack:** Ubuntu 22.04、ROS2 Humble、Python 3、rclpy、pytest、ament/colcon、RealMan Python API2（仅真机运行时）。
|
||||
|
||||
## 全局约束
|
||||
|
||||
- 构建、测试和运行命令在 `/home/robot/WS_xr` 执行，并先运行 `source /opt/ros/humble/setup.bash`。
|
||||
- 所有自动验证使用 mock 或假对象，不连接真机、不移动机械臂、不操作夹爪。
|
||||
- 保留工作空间与圆柱限位、线速度与角速度限制、指令超时和安全停止逻辑。
|
||||
- `configure_safety_limits` 保持启用；`move_to_initial_pose_on_connect` 默认值保持 `false`。
|
||||
- mock 模式不得导入或依赖睿尔曼厂商 SDK，不新增 RealMan 连接。
|
||||
- 只修改完成本功能所需文件，不新增依赖、节点、话题、服务或配置项。
|
||||
- 左臂初始关节角（度）：`[-78.81, 3.22, 67.96, 97.12, 95.08, -81.11, -74.55]`。
|
||||
- 右臂初始关节角（度）：`[-86.10, 22.80, -89.57, 93.98, -91.82, -87.32, -89.35]`。
|
||||
|
||||
---
|
||||
|
||||
### Task 1: 复用适配器初始位姿运动
|
||||
|
||||
**Files:**
|
||||
- Modify: `src/xr_rm_teleop/xr_rm_teleop/realman_adapter.py:42-82`
|
||||
- Modify: `src/xr_rm_teleop/xr_rm_teleop/realman_adapter.py:178-182`
|
||||
- Modify: `src/xr_rm_teleop/xr_rm_teleop/realman_adapter.py:454-459`
|
||||
- Test: `src/xr_rm_teleop/test/test_initial_joint_pose.py:13-35`
|
||||
|
||||
**Interfaces:**
|
||||
- Consumes: 现有 `initial_joint_pose: list[float]`（度）和 `init_move_speed: int`。
|
||||
- Produces: `MockRealManAdapter.move_to_initial_pose() -> None`。
|
||||
- Produces: `RealManAdapter.move_to_initial_pose() -> None`。
|
||||
|
||||
- [ ] **Step 1: 先写失败测试**
|
||||
|
||||
将真机测试改为调用公开方法，并增加 mock 恢复初始关节角的测试：
|
||||
|
||||
```python
|
||||
def test_initial_pose_uses_joint_move_only() -> None:
|
||||
class FakeArm:
|
||||
def __init__(self) -> None:
|
||||
self.calls = []
|
||||
|
||||
def rm_movej(self, *args):
|
||||
self.calls.append(args)
|
||||
return 0
|
||||
|
||||
joints = [-167.21, 28.48, 28.21, 61.35, -14.40, 84.49, -124.51]
|
||||
adapter = RealManAdapter(
|
||||
"127.0.0.1",
|
||||
8080,
|
||||
0,
|
||||
"127.0.0.1",
|
||||
8090,
|
||||
initial_joint_pose=joints,
|
||||
)
|
||||
adapter._arm = FakeArm()
|
||||
|
||||
adapter.move_to_initial_pose()
|
||||
|
||||
assert adapter._arm.calls == [(joints, 20, 0, 0, 1)]
|
||||
|
||||
|
||||
def test_mock_initial_pose_restores_configured_joints() -> None:
|
||||
initial_degrees = [-78.81, 3.22, 67.96, 97.12, 95.08, -81.11, -74.55]
|
||||
adapter = MockRealManAdapter(initial_degrees)
|
||||
adapter.send_joint_target([0.0] * 7, follow=False)
|
||||
|
||||
adapter.move_to_initial_pose()
|
||||
|
||||
assert adapter.read_joint_state().positions == pytest.approx(
|
||||
[math.radians(value) for value in initial_degrees]
|
||||
)
|
||||
```
|
||||
|
||||
- [ ] **Step 2: 运行测试并确认按预期失败**
|
||||
|
||||
Run:
|
||||
|
||||
```bash
|
||||
cd /home/robot/WS_xr
|
||||
source /opt/ros/humble/setup.bash
|
||||
python3 -m pytest src/xr_rm_teleop/test/test_initial_joint_pose.py \
|
||||
-k 'initial_pose_uses_joint_move_only or mock_initial_pose_restores_configured_joints' -v
|
||||
```
|
||||
|
||||
Expected: FAIL，两个适配器都还没有公开的 `move_to_initial_pose` 方法。
|
||||
|
||||
- [ ] **Step 3: 写最小实现**
|
||||
|
||||
在 mock 中保存初始弧度值并实现恢复：
|
||||
|
||||
```python
|
||||
self._initial_joint_positions = [
|
||||
math.radians(value) for value in initial_joint_degrees
|
||||
]
|
||||
self._joint_positions = list(self._initial_joint_positions)
|
||||
```
|
||||
|
||||
```python
|
||||
def move_to_initial_pose(self) -> None:
|
||||
self._joint_positions = list(self._initial_joint_positions)
|
||||
self.last_joint_target = list(self._joint_positions)
|
||||
```
|
||||
|
||||
将真机 `_move_to_initial_pose` 改为公开方法，并保留原有阻塞式关节运动：
|
||||
|
||||
```python
|
||||
def move_to_initial_pose(self) -> None:
|
||||
self._require_arm()
|
||||
if self._initial_joint_pose is None:
|
||||
raise RuntimeError("启用初始位姿移动时必须配置 initial_joint_pose")
|
||||
|
||||
ret = self._arm.rm_movej(
|
||||
self._initial_joint_pose,
|
||||
self._init_move_speed,
|
||||
0,
|
||||
0,
|
||||
1,
|
||||
)
|
||||
self._check_return(ret, "rm_movej(initial_joint_pose)")
|
||||
```
|
||||
|
||||
同时把 `connect()` 中的启动回位调用改为：
|
||||
|
||||
```python
|
||||
if self._move_to_initial_pose_on_connect:
|
||||
self.move_to_initial_pose()
|
||||
```
|
||||
|
||||
- [ ] **Step 4: 运行测试并确认通过**
|
||||
|
||||
Run:
|
||||
|
||||
```bash
|
||||
cd /home/robot/WS_xr
|
||||
source /opt/ros/humble/setup.bash
|
||||
python3 -m pytest src/xr_rm_teleop/test/test_initial_joint_pose.py \
|
||||
-k 'initial_pose_uses_joint_move_only or mock_initial_pose_restores_configured_joints' -v
|
||||
```
|
||||
|
||||
Expected: PASS。
|
||||
|
||||
- [ ] **Step 5: 创建本地提交**
|
||||
|
||||
```bash
|
||||
cd /home/robot/WS_xr/src
|
||||
git add xr_rm_teleop/xr_rm_teleop/realman_adapter.py \
|
||||
xr_rm_teleop/test/test_initial_joint_pose.py
|
||||
git commit -m "feat: 复用适配器初始位姿运动"
|
||||
```
|
||||
|
||||
### Task 2: 在遥操作节点处理主键上升沿
|
||||
|
||||
**Files:**
|
||||
- Modify: `src/xr_rm_teleop/xr_rm_teleop/single_arm_velocity_teleop.py:276-299`
|
||||
- Modify: `src/xr_rm_teleop/xr_rm_teleop/single_arm_velocity_teleop.py:514-534`
|
||||
- Test: `src/xr_rm_teleop/test/test_joint_control.py:16-114`
|
||||
|
||||
**Interfaces:**
|
||||
- Consumes: `XrController.primary: bool`。
|
||||
- Consumes: Task 1 的 `adapter.move_to_initial_pose() -> None`。
|
||||
- Produces: `SingleArmVelocityTeleop._handle_initial_pose_button(msg: XrController) -> None`。
|
||||
|
||||
- [ ] **Step 1: 先写主键边沿失败测试**
|
||||
|
||||
在 `test_joint_control.py` 增加测试辅助函数和成功路径测试：
|
||||
|
||||
```python
|
||||
def _primary_button_teleop(*, move_error=None):
|
||||
events = []
|
||||
errors = []
|
||||
snapshot = JointStateSnapshot([0.2] * 7, time.monotonic())
|
||||
|
||||
class Adapter:
|
||||
def move_to_initial_pose(self):
|
||||
events.append("move")
|
||||
if move_error is not None:
|
||||
raise move_error
|
||||
|
||||
def read_joint_state(self):
|
||||
events.append("read")
|
||||
return snapshot
|
||||
|
||||
teleop = object.__new__(SingleArmVelocityTeleop)
|
||||
teleop._arm_name = "right_rm75"
|
||||
teleop._adapter = Adapter()
|
||||
teleop._last_primary_pressed = None
|
||||
teleop._grip_rearm_required = False
|
||||
teleop._safe_stop = lambda reset_active: events.append(
|
||||
("stop", reset_active)
|
||||
)
|
||||
teleop._reset_joint_state = lambda value: events.append(("sync", value))
|
||||
teleop._handle_trigger_gripper = lambda msg: None
|
||||
teleop.get_clock = lambda: SimpleNamespace(now=lambda: FakeTime())
|
||||
teleop.get_logger = lambda: SimpleNamespace(
|
||||
info=lambda message: None,
|
||||
error=lambda message: errors.append(message),
|
||||
)
|
||||
return teleop, events, errors, snapshot
|
||||
|
||||
|
||||
def test_primary_button_rising_edge_moves_once_and_resyncs() -> None:
|
||||
teleop, events, _, snapshot = _primary_button_teleop()
|
||||
released = SimpleNamespace(primary=False)
|
||||
pressed = SimpleNamespace(primary=True)
|
||||
|
||||
teleop._on_controller(released)
|
||||
teleop._on_controller(pressed)
|
||||
teleop._on_controller(pressed)
|
||||
teleop._on_controller(released)
|
||||
teleop._on_controller(pressed)
|
||||
|
||||
expected_once = [
|
||||
("stop", True),
|
||||
"move",
|
||||
"read",
|
||||
("sync", snapshot),
|
||||
]
|
||||
assert events == expected_once * 2
|
||||
assert teleop._grip_rearm_required
|
||||
```
|
||||
|
||||
- [ ] **Step 2: 运行测试并确认按预期失败**
|
||||
|
||||
Run:
|
||||
|
||||
```bash
|
||||
cd /home/robot/WS_xr
|
||||
source /opt/ros/humble/setup.bash
|
||||
python3 -m pytest src/xr_rm_teleop/test/test_joint_control.py \
|
||||
-k primary_button_rising_edge_moves_once_and_resyncs -v
|
||||
```
|
||||
|
||||
Expected: FAIL，因为 `_on_controller` 尚未处理 `primary`。
|
||||
|
||||
- [ ] **Step 3: 写最小成功实现**
|
||||
|
||||
在节点状态中增加与现有 trigger 相同的首次采样保护：
|
||||
|
||||
```python
|
||||
self._last_primary_pressed: bool | None = None
|
||||
```
|
||||
|
||||
在现有回调中接入主键处理：
|
||||
|
||||
```python
|
||||
def _on_controller(self, msg: XrController) -> None:
|
||||
self._last_msg = msg
|
||||
self._last_msg_time = self.get_clock().now()
|
||||
self._handle_initial_pose_button(msg)
|
||||
self._handle_trigger_gripper(msg)
|
||||
```
|
||||
|
||||
增加主键上升沿处理；首次采样只建立状态，避免节点启动时按键已经按住而意外运动：
|
||||
|
||||
```python
|
||||
def _handle_initial_pose_button(self, msg: XrController) -> None:
|
||||
if self._last_primary_pressed is None:
|
||||
self._last_primary_pressed = msg.primary
|
||||
return
|
||||
|
||||
rising_edge = msg.primary and not self._last_primary_pressed
|
||||
self._last_primary_pressed = msg.primary
|
||||
if not rising_edge:
|
||||
return
|
||||
|
||||
self._grip_rearm_required = True
|
||||
self._safe_stop(reset_active=True)
|
||||
self._adapter.move_to_initial_pose()
|
||||
self._reset_joint_state(self._adapter.read_joint_state())
|
||||
self.get_logger().info(f"{self._arm_name} 已回到初始位姿。")
|
||||
```
|
||||
|
||||
- [ ] **Step 4: 运行测试并确认通过**
|
||||
|
||||
Run:
|
||||
|
||||
```bash
|
||||
cd /home/robot/WS_xr
|
||||
source /opt/ros/humble/setup.bash
|
||||
python3 -m pytest src/xr_rm_teleop/test/test_joint_control.py \
|
||||
-k primary_button_rising_edge_moves_once_and_resyncs -v
|
||||
```
|
||||
|
||||
Expected: PASS。
|
||||
|
||||
- [ ] **Step 5: 先写失败路径测试**
|
||||
|
||||
```python
|
||||
def test_primary_button_move_failure_logs_and_stays_stopped() -> None:
|
||||
failure = RuntimeError("rm_movej failed")
|
||||
teleop, events, errors, _ = _primary_button_teleop(
|
||||
move_error=failure
|
||||
)
|
||||
|
||||
teleop._on_controller(SimpleNamespace(primary=False))
|
||||
teleop._on_controller(SimpleNamespace(primary=True))
|
||||
|
||||
assert events == [("stop", True), "move"]
|
||||
assert teleop._grip_rearm_required
|
||||
assert errors == [
|
||||
"right_rm75 回初始位姿失败：rm_movej failed"
|
||||
]
|
||||
```
|
||||
|
||||
- [ ] **Step 6: 运行失败路径测试并确认按预期失败**
|
||||
|
||||
Run:
|
||||
|
||||
```bash
|
||||
cd /home/robot/WS_xr
|
||||
source /opt/ros/humble/setup.bash
|
||||
python3 -m pytest src/xr_rm_teleop/test/test_joint_control.py \
|
||||
-k primary_button_move_failure_logs_and_stays_stopped -v
|
||||
```
|
||||
|
||||
Expected: FAIL，并抛出 `RuntimeError: rm_movej failed`。
|
||||
|
||||
- [ ] **Step 7: 增加最小异常处理**
|
||||
|
||||
用 `try/except` 包住回位和状态同步，失败时记录错误并保持已经设置的停止与 Grip
|
||||
重新使能状态：
|
||||
|
||||
```python
|
||||
try:
|
||||
self._adapter.move_to_initial_pose()
|
||||
self._reset_joint_state(self._adapter.read_joint_state())
|
||||
except Exception as exc:
|
||||
self.get_logger().error(
|
||||
f"{self._arm_name} 回初始位姿失败：{exc}"
|
||||
)
|
||||
return
|
||||
|
||||
self.get_logger().info(f"{self._arm_name} 已回到初始位姿。")
|
||||
```
|
||||
|
||||
- [ ] **Step 8: 运行两条主键测试并确认通过**
|
||||
|
||||
Run:
|
||||
|
||||
```bash
|
||||
cd /home/robot/WS_xr
|
||||
source /opt/ros/humble/setup.bash
|
||||
python3 -m pytest src/xr_rm_teleop/test/test_joint_control.py -k primary_button -v
|
||||
```
|
||||
|
||||
Expected: PASS。
|
||||
|
||||
- [ ] **Step 9: 创建本地提交**
|
||||
|
||||
```bash
|
||||
cd /home/robot/WS_xr/src
|
||||
git add xr_rm_teleop/xr_rm_teleop/single_arm_velocity_teleop.py \
|
||||
xr_rm_teleop/test/test_joint_control.py
|
||||
git commit -m "feat: 添加手柄主键回初始位姿"
|
||||
```
|
||||
|
||||
### Task 3: 同步三份初始位姿配置
|
||||
|
||||
**Files:**
|
||||
- Modify: `src/xr_rm_bringup/config/left_arm_rm75.yaml:57`
|
||||
- Modify: `src/xr_rm_bringup/config/right_arm_rm75.yaml:57`
|
||||
- Modify: `src/xr_rm_bringup/config/dual_arm_rm75.yaml:64`
|
||||
- Modify: `src/xr_rm_bringup/config/dual_arm_rm75.yaml:121`
|
||||
|
||||
**Interfaces:**
|
||||
- Consumes: 用户确认的左右臂 7 个关节角，单位为度。
|
||||
- Produces: 单臂和双臂模式一致的对应臂 `initial_joint_pose`。
|
||||
|
||||
- [ ] **Step 1: 只替换四处初始位姿**
|
||||
|
||||
```yaml
|
||||
# left_arm_rm75.yaml
|
||||
initial_joint_pose: [-78.81, 3.22, 67.96, 97.12, 95.08, -81.11, -74.55]
|
||||
|
||||
# right_arm_rm75.yaml
|
||||
initial_joint_pose: [-86.10, 22.80, -89.57, 93.98, -91.82, -87.32, -89.35]
|
||||
|
||||
# dual_arm_rm75.yaml / left_arm_teleop
|
||||
initial_joint_pose: [-78.81, 3.22, 67.96, 97.12, 95.08, -81.11, -74.55]
|
||||
|
||||
# dual_arm_rm75.yaml / right_arm_teleop
|
||||
initial_joint_pose: [-86.10, 22.80, -89.57, 93.98, -91.82, -87.32, -89.35]
|
||||
```
|
||||
|
||||
- [ ] **Step 2: 解析 YAML 并验证四处值**
|
||||
|
||||
Run:
|
||||
|
||||
```bash
|
||||
cd /home/robot/WS_xr
|
||||
source /opt/ros/humble/setup.bash
|
||||
python3 - <<'PY'
|
||||
from pathlib import Path
|
||||
import yaml
|
||||
|
||||
config_dir = Path("src/xr_rm_bringup/config")
|
||||
left = [-78.81, 3.22, 67.96, 97.12, 95.08, -81.11, -74.55]
|
||||
right = [-86.10, 22.80, -89.57, 93.98, -91.82, -87.32, -89.35]
|
||||
|
||||
left_single = yaml.safe_load((config_dir / "left_arm_rm75.yaml").read_text())
|
||||
right_single = yaml.safe_load((config_dir / "right_arm_rm75.yaml").read_text())
|
||||
dual = yaml.safe_load((config_dir / "dual_arm_rm75.yaml").read_text())
|
||||
|
||||
assert left_single["single_arm_velocity_teleop"]["ros__parameters"]["initial_joint_pose"] == left
|
||||
assert right_single["single_arm_velocity_teleop"]["ros__parameters"]["initial_joint_pose"] == right
|
||||
assert dual["left_arm_teleop"]["ros__parameters"]["initial_joint_pose"] == left
|
||||
assert dual["right_arm_teleop"]["ros__parameters"]["initial_joint_pose"] == right
|
||||
PY
|
||||
```
|
||||
|
||||
Expected: exit code 0，无输出。
|
||||
|
||||
- [ ] **Step 3: 确认没有改动其他 YAML 参数**
|
||||
|
||||
Run:
|
||||
|
||||
```bash
|
||||
cd /home/robot/WS_xr/src
|
||||
git diff --word-diff=plain -- \
|
||||
xr_rm_bringup/config/left_arm_rm75.yaml \
|
||||
xr_rm_bringup/config/right_arm_rm75.yaml \
|
||||
xr_rm_bringup/config/dual_arm_rm75.yaml
|
||||
```
|
||||
|
||||
Expected: 只有四个 `initial_joint_pose` 列表发生变化。
|
||||
|
||||
- [ ] **Step 4: 创建本地提交**
|
||||
|
||||
```bash
|
||||
cd /home/robot/WS_xr/src
|
||||
git add xr_rm_bringup/config/left_arm_rm75.yaml \
|
||||
xr_rm_bringup/config/right_arm_rm75.yaml \
|
||||
xr_rm_bringup/config/dual_arm_rm75.yaml
|
||||
git commit -m "config: 更新左右臂初始位姿"
|
||||
```
|
||||
|
||||
### Task 4: 完整验证
|
||||
|
||||
**Files:**
|
||||
- Verify: `src/xr_rm_teleop/test/test_initial_joint_pose.py`
|
||||
- Verify: `src/xr_rm_teleop/test/test_joint_control.py`
|
||||
- Verify: `src/xr_rm_teleop/test/test_orientation_control.py`
|
||||
- Verify: 全部四个 ROS2 包
|
||||
|
||||
**Interfaces:**
|
||||
- Consumes: Tasks 1–3 的本地提交。
|
||||
- Produces: mock 测试与 ROS2 构建通过的可验证结果。
|
||||
|
||||
- [ ] **Step 1: 运行遥操作相关测试**
|
||||
|
||||
```bash
|
||||
cd /home/robot/WS_xr
|
||||
source /opt/ros/humble/setup.bash
|
||||
python3 -m pytest src/xr_rm_teleop/test/test_initial_joint_pose.py \
|
||||
src/xr_rm_teleop/test/test_joint_control.py \
|
||||
src/xr_rm_teleop/test/test_orientation_control.py
|
||||
```
|
||||
|
||||
Expected: PASS，无 error 或 warning。
|
||||
|
||||
- [ ] **Step 2: 构建工作空间**
|
||||
|
||||
```bash
|
||||
cd /home/robot/WS_xr
|
||||
source /opt/ros/humble/setup.bash
|
||||
colcon build --symlink-install
|
||||
```
|
||||
|
||||
Expected: `xr_rm_interfaces`、`xr_rm_input`、`xr_rm_teleop` 和 `xr_rm_bringup`
|
||||
构建完成，无失败包。
|
||||
|
||||
- [ ] **Step 3: 检查最终范围**
|
||||
|
||||
```bash
|
||||
cd /home/robot/WS_xr/src
|
||||
git status --short
|
||||
git log -6 --oneline
|
||||
```
|
||||
|
||||
Expected: 工作区干净；只有设计、计划、适配器、遥操作节点、两份测试和三份 YAML
|
||||
配置的相关本地提交，不存在远程写操作。
|
||||
@@ -0,0 +1,73 @@
|
||||
# 手柄主键回初始位姿设计
|
||||
|
||||
## 背景与目标
|
||||
|
||||
有线连接已基本解决 UDP 超时和逆解失败问题。本次只增加一个明确操作：
|
||||
点击当前机械臂对应手柄的主键，使该机械臂按已配置的关节角回到初始位姿。
|
||||
|
||||
- 右臂模式：右手 A 键控制右臂，左手 X 键无效。
|
||||
- 左臂模式：左手 X 键控制左臂，右手 A 键无效。
|
||||
- 双臂模式：右手 A 键控制右臂，左手 X 键控制左臂。
|
||||
|
||||
## 最小方案
|
||||
|
||||
`XrController.primary` 已表示左手 X 键或右手 A 键。每个遥操作节点继续只订阅
|
||||
自身的手柄话题，并在 `primary` 上升沿调用适配器的初始位姿运动：
|
||||
|
||||
```text
|
||||
左手 X → /xr/left_controller → left_arm_teleop → 左臂初始位姿
|
||||
右手 A → /xr/right_controller → right_arm_teleop → 右臂初始位姿
|
||||
```
|
||||
|
||||
单臂模式只启动对应节点，因此另一只手柄天然无效；双臂模式下两个节点独立处理，
|
||||
无需新增协调节点、话题、服务或配置项。
|
||||
|
||||
## 运动与安全行为
|
||||
|
||||
- 只在按键从未按下变为按下时触发，持续按住不重复执行。
|
||||
- 回位前调用现有安全停止逻辑，退出当前相对位姿遥操作。
|
||||
- 使用更新后的 `initial_joint_pose` 和现有 `init_move_speed`。
|
||||
- 复用现有 `rm_movej(..., block=1)` 阻塞运动，完成后重新同步关节状态和 QP 状态。
|
||||
- 回位后必须先松开 Grip，才能重新进入遥操作。
|
||||
- 回位失败时记录错误并保持停止，不自动重试。
|
||||
- mock 模式只更新模拟关节状态，不导入或调用厂商 SDK。
|
||||
|
||||
## 初始位姿配置
|
||||
|
||||
关节角单位沿用现有 YAML，均为度：
|
||||
|
||||
- 左臂：`[-78.81, 3.22, 67.96, 97.12, 95.08, -81.11, -74.55]`
|
||||
- 右臂：`[-86.10, 22.80, -89.57, 93.98, -91.82, -87.32, -89.35]`
|
||||
|
||||
`left_arm_rm75.yaml` 使用左臂值，`right_arm_rm75.yaml` 使用右臂值；
|
||||
`dual_arm_rm75.yaml` 中左右节点分别使用对应值。三个文件中的 IP、坐标映射、
|
||||
工作空间、安全限制及其他控制参数保持不变。
|
||||
|
||||
## 代码范围
|
||||
|
||||
- 在 `realman_adapter.py` 为真机和 mock 提供同名的公开回位方法，真机实现复用现有
|
||||
私有初始位姿运动代码。
|
||||
- 在 `single_arm_velocity_teleop.py` 的现有手柄回调中增加主键上升沿处理。
|
||||
- 在现有测试文件中增加最小的适配器回位和按键边沿测试。
|
||||
- 同步 `left_arm_rm75.yaml`、`right_arm_rm75.yaml` 和 `dual_arm_rm75.yaml` 中的
|
||||
`initial_joint_pose`。
|
||||
|
||||
不修改消息定义、UDP 输入节点、launch、三个 YAML 中的其他参数、安全限位或
|
||||
夹爪控制。
|
||||
|
||||
## 验证
|
||||
|
||||
- 测试主键首次按下触发一次，持续按住不重复触发，松开后可以再次触发。
|
||||
- 测试 mock 回位后恢复配置的初始关节角。
|
||||
- 测试真机适配器仍只调用既有的关节运动命令；测试使用假 SDK 对象，不连接真机。
|
||||
- 检查单臂和双臂配置中的左右初始位姿与上述值一致。
|
||||
- 在 `/home/robot/WS_xr` 执行：
|
||||
|
||||
```bash
|
||||
source /opt/ros/humble/setup.bash
|
||||
python3 -m pytest src/xr_rm_teleop/test/test_initial_joint_pose.py
|
||||
python3 -m pytest src/xr_rm_teleop/test/test_orientation_control.py
|
||||
colcon build --symlink-install
|
||||
```
|
||||
|
||||
全部验证使用 mock 或假对象，不连接真机、不移动机械臂、不操作夹爪。
|
||||
@@ -16,23 +16,23 @@ left_arm_teleop:
|
||||
feedback_resync_timeout_sec: 0.5
|
||||
|
||||
# 位姿目标生成与平滑参数。
|
||||
scale: 0.75
|
||||
scale: 0.7
|
||||
deadband_m: 0.001
|
||||
target_filter_alpha: 0.65
|
||||
target_filter_alpha_fast: 0.9
|
||||
target_filter_fast_threshold_m: 0.03
|
||||
max_linear_speed: 0.2
|
||||
max_linear_speed: 0.15
|
||||
enable_position_axes: [true, true, true]
|
||||
enable_orientation_control: true
|
||||
enable_orientation_axes: [true, true, true]
|
||||
orientation_deadband_rad: 0.005
|
||||
orientation_filter_alpha: 0.65
|
||||
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]
|
||||
low_z_threshold: 0.20
|
||||
low_z_min_radius: 0.21
|
||||
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
|
||||
|
||||
# PICO/OpenXR 位置坐标：+X 向右，+Y 向上，+Z 向后。
|
||||
# 映射关系：机器人位移增量 = [-手柄y, 手柄z, -手柄x]。
|
||||
@@ -45,7 +45,7 @@ left_arm_teleop:
|
||||
realtime_push_host_ip: 192.168.192.148
|
||||
realtime_push_port: 8089
|
||||
realtime_push_cycle_ms: 5
|
||||
avoid_singularity: 0
|
||||
avoid_singularity: 1
|
||||
follow: false
|
||||
canfd_trajectory_mode: 2
|
||||
canfd_radio: 0
|
||||
@@ -54,14 +54,14 @@ left_arm_teleop:
|
||||
enable_trigger_gripper_control: true
|
||||
trigger_close_threshold: 0.95
|
||||
configure_peripheral_on_connect: true
|
||||
max_line_speed: 1.0
|
||||
max_angular_speed: 1.5
|
||||
max_line_acc: 1.0
|
||||
max_angular_acc: 2.0
|
||||
max_line_speed: 0.25
|
||||
max_angular_speed: 0.6
|
||||
max_line_acc: 1.3
|
||||
max_angular_acc: 3.0
|
||||
joint_max_speed: 180.0
|
||||
joint_max_acc: 180.0
|
||||
joint_max_acc: 300.0
|
||||
move_to_initial_pose_on_connect: false
|
||||
initial_joint_pose: [-167.21, 28.48, 28.21, 61.35, -14.40, 84.49, -124.51]
|
||||
initial_joint_pose: [-78.81, 3.22, 67.96, 97.12, 95.08, -81.11, -74.55]
|
||||
init_move_speed: 20
|
||||
debug_topic_prefix: /xr_rm
|
||||
|
||||
@@ -73,23 +73,23 @@ right_arm_teleop:
|
||||
command_timeout_sec: 0.12
|
||||
feedback_resync_timeout_sec: 0.5
|
||||
|
||||
scale: 0.75
|
||||
scale: 0.7
|
||||
deadband_m: 0.001
|
||||
target_filter_alpha: 0.65
|
||||
target_filter_alpha_fast: 0.9
|
||||
target_filter_fast_threshold_m: 0.03
|
||||
max_linear_speed: 0.2
|
||||
target_filter_fast_threshold_m: 0.05
|
||||
max_linear_speed: 0.15
|
||||
enable_position_axes: [true, true, true]
|
||||
enable_orientation_control: true
|
||||
enable_orientation_axes: [true, true, true]
|
||||
orientation_deadband_rad: 0.005
|
||||
orientation_filter_alpha: 0.65
|
||||
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]
|
||||
low_z_threshold: 0.20
|
||||
low_z_min_radius: 0.21
|
||||
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
|
||||
|
||||
# PICO/OpenXR 位置坐标：+X 向右，+Y 向上，+Z 向后。
|
||||
# 映射关系：机器人位移增量 = [手柄y, 手柄z, 手柄x]。
|
||||
@@ -111,13 +111,13 @@ right_arm_teleop:
|
||||
enable_trigger_gripper_control: true
|
||||
trigger_close_threshold: 0.95
|
||||
configure_peripheral_on_connect: true
|
||||
max_line_speed: 1.0
|
||||
max_angular_speed: 1.5
|
||||
max_line_acc: 1.0
|
||||
max_angular_acc: 2.0
|
||||
max_line_speed: 0.25
|
||||
max_angular_speed: 0.6
|
||||
max_line_acc: 1.3
|
||||
max_angular_acc: 3.0
|
||||
joint_max_speed: 180.0
|
||||
joint_max_acc: 180.0
|
||||
joint_max_acc: 300.0
|
||||
move_to_initial_pose_on_connect: false
|
||||
initial_joint_pose: [-25.60, 34.09, -19.55, 71.59, 16.97, 80.98, 59.67]
|
||||
initial_joint_pose: [-86.10, 22.80, -89.57, 93.98, -91.82, -87.32, -89.35]
|
||||
init_move_speed: 20
|
||||
debug_topic_prefix: /xr_rm
|
||||
|
||||
@@ -10,23 +10,23 @@ single_arm_velocity_teleop:
|
||||
feedback_resync_timeout_sec: 0.5
|
||||
|
||||
# 手柄相对位姿 -> 目标 TCP 位姿；随后做目标低通、姿态低通和单帧步长限制。
|
||||
scale: 1.0
|
||||
scale: 0.7
|
||||
deadband_m: 0.001
|
||||
target_filter_alpha: 0.65
|
||||
target_filter_alpha_fast: 0.9
|
||||
target_filter_fast_threshold_m: 0.03
|
||||
max_linear_speed: 0.3
|
||||
max_linear_speed: 0.15
|
||||
enable_position_axes: [true, true, true]
|
||||
enable_orientation_control: true
|
||||
enable_orientation_axes: [true, true, true]
|
||||
orientation_deadband_rad: 0.005
|
||||
orientation_filter_alpha: 0.65
|
||||
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]
|
||||
low_z_threshold: 0.20
|
||||
low_z_min_radius: 0.21
|
||||
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
|
||||
|
||||
# 映射关系：机器人位移增量 = [-手柄y, 手柄z, -手柄x]。
|
||||
xr_to_robot_matrix: [0.0, -1.0, 0.0,
|
||||
@@ -38,7 +38,7 @@ single_arm_velocity_teleop:
|
||||
realtime_push_host_ip: 192.168.192.148
|
||||
realtime_push_port: 8089
|
||||
realtime_push_cycle_ms: 5
|
||||
avoid_singularity: 0
|
||||
avoid_singularity: 1
|
||||
follow: false
|
||||
canfd_trajectory_mode: 2
|
||||
canfd_radio: 0
|
||||
@@ -47,13 +47,13 @@ single_arm_velocity_teleop:
|
||||
enable_trigger_gripper_control: true
|
||||
trigger_close_threshold: 0.95
|
||||
configure_peripheral_on_connect: true
|
||||
max_line_speed: 1.0
|
||||
max_angular_speed: 1.5
|
||||
max_line_acc: 1.0
|
||||
max_angular_acc: 2.0
|
||||
max_line_speed: 0.25
|
||||
max_angular_speed: 0.6
|
||||
max_line_acc: 1.3
|
||||
max_angular_acc: 3.0
|
||||
joint_max_speed: 180.0
|
||||
joint_max_acc: 180.0
|
||||
joint_max_acc: 300.0
|
||||
move_to_initial_pose_on_connect: false
|
||||
initial_joint_pose: [-79.55, -9.99, 71.01, 101.45, 95.07, -84.47, -74.52]
|
||||
initial_joint_pose: [-78.81, 3.22, 67.96, 97.12, 95.08, -81.11, -74.55]
|
||||
init_move_speed: 20
|
||||
debug_topic_prefix: /xr_rm
|
||||
|
||||
@@ -54,6 +54,6 @@ single_arm_velocity_teleop:
|
||||
joint_max_speed: 180.0
|
||||
joint_max_acc: 300.0
|
||||
move_to_initial_pose_on_connect: false
|
||||
initial_joint_pose: [-90.14, 3.76, -86.89, 87.89, -96.53, -79.62, -90.04]
|
||||
initial_joint_pose: [-86.10, 22.80, -89.57, 93.98, -91.82, -87.32, -89.35]
|
||||
init_move_speed: 20
|
||||
debug_topic_prefix: /xr_rm
|
||||
|
||||
@@ -30,11 +30,23 @@ def test_initial_pose_uses_joint_move_only() -> None:
|
||||
)
|
||||
adapter._arm = FakeArm()
|
||||
|
||||
adapter._move_to_initial_pose()
|
||||
adapter.move_to_initial_pose()
|
||||
|
||||
assert adapter._arm.calls == [(joints, 20, 0, 0, 1)]
|
||||
|
||||
|
||||
def test_mock_initial_pose_restores_configured_joints() -> None:
|
||||
initial_degrees = [-78.81, 3.22, 67.96, 97.12, 95.08, -81.11, -74.55]
|
||||
adapter = MockRealManAdapter(initial_degrees)
|
||||
adapter.send_joint_target([0.0] * 7, follow=False)
|
||||
|
||||
adapter.move_to_initial_pose()
|
||||
|
||||
assert adapter.read_joint_state().positions == pytest.approx(
|
||||
[math.radians(value) for value in initial_degrees]
|
||||
)
|
||||
|
||||
|
||||
def test_peripheral_config_exposes_selected_tool() -> None:
|
||||
config = PeripheralConfig(
|
||||
scissorgripper=1,
|
||||
|
||||
@@ -30,6 +30,76 @@ class FakeTime:
|
||||
return SimpleNamespace(nanoseconds=0)
|
||||
|
||||
|
||||
def _primary_button_teleop(*, move_error=None):
|
||||
events = []
|
||||
errors = []
|
||||
snapshot = JointStateSnapshot([0.2] * 7, time.monotonic())
|
||||
|
||||
class Adapter:
|
||||
def move_to_initial_pose(self):
|
||||
events.append("move")
|
||||
if move_error is not None:
|
||||
raise move_error
|
||||
|
||||
def read_joint_state(self):
|
||||
events.append("read")
|
||||
return snapshot
|
||||
|
||||
teleop = object.__new__(SingleArmVelocityTeleop)
|
||||
teleop._arm_name = "right_rm75"
|
||||
teleop._adapter = Adapter()
|
||||
teleop._last_primary_pressed = None
|
||||
teleop._grip_rearm_required = False
|
||||
teleop._safe_stop = lambda reset_active: events.append(
|
||||
("stop", reset_active)
|
||||
)
|
||||
teleop._reset_joint_state = lambda value: events.append(("sync", value))
|
||||
teleop._handle_trigger_gripper = lambda msg: None
|
||||
teleop.get_clock = lambda: SimpleNamespace(now=lambda: FakeTime())
|
||||
teleop.get_logger = lambda: SimpleNamespace(
|
||||
info=lambda message: None,
|
||||
error=lambda message: errors.append(message),
|
||||
)
|
||||
return teleop, events, errors, snapshot
|
||||
|
||||
|
||||
def test_primary_button_rising_edge_moves_once_and_resyncs() -> None:
|
||||
teleop, events, _, snapshot = _primary_button_teleop()
|
||||
released = SimpleNamespace(primary=False)
|
||||
pressed = SimpleNamespace(primary=True)
|
||||
|
||||
teleop._on_controller(released)
|
||||
teleop._on_controller(pressed)
|
||||
teleop._on_controller(pressed)
|
||||
teleop._on_controller(released)
|
||||
teleop._on_controller(pressed)
|
||||
|
||||
expected_once = [
|
||||
("stop", True),
|
||||
"move",
|
||||
"read",
|
||||
("sync", snapshot),
|
||||
]
|
||||
assert events == expected_once * 2
|
||||
assert teleop._grip_rearm_required
|
||||
|
||||
|
||||
def test_primary_button_move_failure_logs_and_stays_stopped() -> None:
|
||||
failure = RuntimeError("rm_movej failed")
|
||||
teleop, events, errors, _ = _primary_button_teleop(
|
||||
move_error=failure
|
||||
)
|
||||
|
||||
teleop._on_controller(SimpleNamespace(primary=False))
|
||||
teleop._on_controller(SimpleNamespace(primary=True))
|
||||
|
||||
assert events == [("stop", True), "move"]
|
||||
assert teleop._grip_rearm_required
|
||||
assert errors == [
|
||||
"right_rm75 回初始位姿失败：rm_movej failed"
|
||||
]
|
||||
|
||||
|
||||
def test_startup_joint_query_initializes_qp_and_command_history() -> None:
|
||||
positions = [0.1] * 7
|
||||
pose = np.eye(4)
|
||||
|
||||
@@ -44,9 +44,10 @@ class MockRealManAdapter:
|
||||
math.isfinite(value) for value in initial_joint_degrees
|
||||
):
|
||||
raise ValueError("initial joint pose must contain 7 finite values")
|
||||
self._joint_positions = [
|
||||
self._initial_joint_positions = [
|
||||
math.radians(value) for value in initial_joint_degrees
|
||||
]
|
||||
self._joint_positions = list(self._initial_joint_positions)
|
||||
self.last_joint_target: list[float] | None = None
|
||||
self.last_tool_open: bool | None = None
|
||||
|
||||
@@ -69,6 +70,10 @@ class MockRealManAdapter:
|
||||
self._joint_positions = list(joints)
|
||||
self.last_joint_target = list(joints)
|
||||
|
||||
def move_to_initial_pose(self) -> None:
|
||||
self._joint_positions = list(self._initial_joint_positions)
|
||||
self.last_joint_target = list(self._joint_positions)
|
||||
|
||||
def stop(self) -> None:
|
||||
return
|
||||
|
||||
@@ -178,7 +183,7 @@ class RealManAdapter:
|
||||
if self._configure_safety_limits:
|
||||
self._apply_safety_limits()
|
||||
if self._move_to_initial_pose_on_connect:
|
||||
self._move_to_initial_pose()
|
||||
self.move_to_initial_pose()
|
||||
self._feedback_ready.clear()
|
||||
self._accept_realtime_feedback = True
|
||||
self._realtime_callback = rm_realtime_arm_state_callback_ptr(
|
||||
@@ -451,11 +456,18 @@ class RealManAdapter:
|
||||
self._try_call("rm_set_joint_max_speed", joint_index, self._joint_max_speed)
|
||||
self._try_call("rm_set_joint_max_acc", joint_index, self._joint_max_acc)
|
||||
|
||||
def _move_to_initial_pose(self) -> None:
|
||||
def move_to_initial_pose(self) -> None:
|
||||
self._require_arm()
|
||||
if self._initial_joint_pose is None:
|
||||
raise RuntimeError("启用初始位姿移动时必须配置 initial_joint_pose")
|
||||
|
||||
ret = self._arm.rm_movej(self._initial_joint_pose, self._init_move_speed, 0, 0, 1)
|
||||
ret = self._arm.rm_movej(
|
||||
self._initial_joint_pose,
|
||||
self._init_move_speed,
|
||||
0,
|
||||
0,
|
||||
1,
|
||||
)
|
||||
self._check_return(ret, "rm_movej(initial_joint_pose)")
|
||||
|
||||
def _try_call(self, name: str, *args: Any) -> None:
|
||||
|
||||
@@ -295,6 +295,7 @@ class SingleArmVelocityTeleop(Node):
|
||||
self._control_fault_latched = False
|
||||
self._stop_sent = True
|
||||
self._trigger_tool_open = True
|
||||
self._last_primary_pressed: bool | None = None
|
||||
self._last_trigger_pressed: bool | None = None
|
||||
self._tool_command_queue: queue.Queue[tuple[bool, str] | None] | None = None
|
||||
self._tool_worker_stop = threading.Event()
|
||||
@@ -514,8 +515,32 @@ class SingleArmVelocityTeleop(Node):
|
||||
def _on_controller(self, msg: XrController) -> None:
|
||||
self._last_msg = msg
|
||||
self._last_msg_time = self.get_clock().now()
|
||||
self._handle_initial_pose_button(msg)
|
||||
self._handle_trigger_gripper(msg)
|
||||
|
||||
def _handle_initial_pose_button(self, msg: XrController) -> None:
|
||||
if self._last_primary_pressed is None:
|
||||
self._last_primary_pressed = msg.primary
|
||||
return
|
||||
|
||||
rising_edge = msg.primary and not self._last_primary_pressed
|
||||
self._last_primary_pressed = msg.primary
|
||||
if not rising_edge:
|
||||
return
|
||||
|
||||
self._grip_rearm_required = True
|
||||
self._safe_stop(reset_active=True)
|
||||
try:
|
||||
self._adapter.move_to_initial_pose()
|
||||
self._reset_joint_state(self._adapter.read_joint_state())
|
||||
except Exception as exc:
|
||||
self.get_logger().error(
|
||||
f"{self._arm_name} 回初始位姿失败：{exc}"
|
||||
)
|
||||
return
|
||||
|
||||
self.get_logger().info(f"{self._arm_name} 已回到初始位姿。")
|
||||
|
||||
def _handle_trigger_gripper(self, msg: XrController) -> None:
|
||||
if not self._enable_tool_control or not self._enable_trigger_gripper_control:
|
||||
return
|
||||
|
||||
Reference in New Issue
Block a user