acRealman_xr/docs/superpowers/plans/2026-07-31-controller-primary-initial-pose.md

手柄主键回初始位姿实施计划

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 恢复初始关节角的测试：

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:

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 中保存初始弧度值并实现恢复：

self._initial_joint_positions = [
    math.radians(value) for value in initial_joint_degrees
]
self._joint_positions = list(self._initial_joint_positions)

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 改为公开方法，并保留原有阻塞式关节运动：

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() 中的启动回位调用改为：

if self._move_to_initial_pose_on_connect:
    self.move_to_initial_pose()

• Step 4: 运行测试并确认通过

Run:

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: 创建本地提交

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 增加测试辅助函数和成功路径测试：

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:

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 相同的首次采样保护：

self._last_primary_pressed: bool | None = None

在现有回调中接入主键处理：

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)
    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:

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: 先写失败路径测试

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:

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 重新使能状态：

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:

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: 创建本地提交

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: 只替换四处初始位姿

# 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:

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:

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: 创建本地提交

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: 运行遥操作相关测试

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: 构建工作空间

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: 检查最终范围

cd /home/robot/WS_xr/src
git status --short
git log -6 --oneline

Expected: 工作区干净；只有设计、计划、适配器、遥操作节点、两份测试和三份 YAML 配置的相关本地提交，不存在远程写操作。