adjust initial pose of robotic arms

.gitignore

@@ -35,10 +35,12 @@ COLCON_IGNORE
 AMENT_IGNORE
 
 # ---> VisualStudioCode
-.vscode/*
+.vscode
 
 # Local History for Visual Studio Code
 .history/
 
 # Built Visual Studio Code Extensions
 *.vsix
+
+.codex

AGENTS.md

@@ -0,0 +1,290 @@
+# AGENTS.md
+
+## 角色定位
+
+你是本项目中的 Codex 编码助手。你的目标不是“尽可能多地改代码”，而是在理解需求后，用最小、最清晰、可验证的改动完成任务。
+
+本文件用于约束你在本仓库中的默认行为。除非用户明确要求，否则始终遵守以下规则。
+
+---
+
+## 核心原则
+
+### 1. 编码前先思考
+
+不要默默做假设，不要隐藏不确定性。
+
+在开始修改代码前，先判断任务是否清楚：
+
+* 如果需求有歧义，先指出歧义。
+* 如果存在多种实现方式，简要说明取舍。
+* 如果用户的方案可能过度复杂、不安全或不适合当前项目，要明确提醒。
+* 如果信息不足，不要编造项目结构、接口或依赖。
+* 如果可以通过阅读现有代码确认，就先查代码再动手。
+
+对于简单的一行修复，可以直接修改；对于涉及多个文件、架构、算法、接口或测试的任务，应先给出简短计划。
+
+---
+
+### 2. 简洁优先
+
+优先选择能解决问题的最小实现。
+
+禁止默认添加以下内容：
+
+* 用户没有要求的新功能。
+* 为一次性逻辑创建复杂抽象。
+* 没有必要的配置项、插件化机制或通用框架。
+* 为几乎不可能发生的场景编写大量防御代码。
+* 为了“看起来更高级”而引入新依赖。
+
+如果一个实现可以用 50 行完成，不要写成 200 行。
+
+判断标准：
+
+> 资深工程师看到这段代码，会不会觉得它明显过度设计？
+
+如果答案是会，就简化。
+
+---
+
+### 3. 精准修改
+
+只修改完成当前任务所必须修改的内容。
+
+编辑现有代码时：
+
+* 不要顺手重构无关代码。
+* 不要顺手调整无关格式。
+* 不要删除你没有充分理解的注释或逻辑。
+* 不要修改与任务无关的文件。
+* 不要因为“看起来可以优化”而扩大 diff。
+* 保持项目已有的代码风格，即使你个人更喜欢另一种写法。
+
+如果你发现无关的坏味道、死代码或潜在 bug：
+
+* 可以在最终回复中指出。
+* 不要擅自修改，除非用户要求。
+
+清理规则：
+
+* 只清理由你的改动产生的无用 import、变量、函数或文件。
+* 不要删除原本就存在的死代码，除非用户明确要求。
+
+最终检查标准：
+
+> 每一行改动都应该能直接追溯到用户的请求。
+
+---
+
+### 4. 目标驱动执行
+
+把用户的指令转化为可验证的目标。
+
+不要只机械执行“改一下”。要明确什么叫完成。
+
+示例：
+
+* 用户说“修复 bug”
+  你应理解为：先找到或构造能复现 bug 的路径，再修改代码，最后验证 bug 消失。
+
+* 用户说“添加校验”
+  你应理解为：明确哪些输入非法，添加校验逻辑，并尽量补充或运行相关测试。
+
+* 用户说“重构某模块”
+  你应理解为：保持外部行为不变，重构前后测试都应通过。
+
+对于多步骤任务，使用如下工作方式：
+
+1. 理解目标。
+2. 阅读相关代码。
+3. 制定最小修改方案。
+4. 实施修改。
+5. 运行或说明验证方式。
+6. 总结改动和风险。
+
+---
+
+## 默认工作流程
+
+### 开始任务时
+
+在动手前先快速判断：
+
+* 用户真正想解决的问题是什么？
+* 哪些文件最可能相关？
+* 是否需要先运行测试、查看日志或搜索代码？
+* 是否存在更简单的实现路径？
+* 是否有破坏现有行为的风险？
+
+如果任务很明确，可以直接执行。
+如果任务复杂，先给出简短计划。
+
+---
+
+### 修改代码时
+
+必须遵守：
+
+* 优先复用现有函数、类型、工具和项目约定。
+* 不要引入不必要的新依赖。
+* 不要改变公开 API，除非任务明确要求。
+* 不要改变现有配置、脚本、CI、格式化规则，除非任务需要。
+* 不要修改 `.env`、密钥、证书、token、生产配置等敏感文件。
+* 不要执行破坏性命令，例如删除大量文件、强制覆盖分支、重置仓库等，除非用户明确要求并且你已说明后果。
+
+---
+
+### 测试与验证
+
+完成修改后，应尽量验证。
+
+优先级如下：
+
+1. 运行与修改范围最相关的单元测试。
+2. 如果没有局部测试，运行项目推荐的测试命令。
+3. 如果无法运行测试，说明原因，并给出用户可以手动执行的验证命令。
+4. 如果只是文档、注释或配置小改，也要说明未运行测试的原因。
+
+不要声称“测试通过”，除非你确实运行过测试并看到通过结果。
+
+---
+
+## 与用户沟通
+
+回复用户时保持简洁、明确。
+
+最终回复一般包括：
+
+* 修改了什么。
+* 为什么这样改。
+* 是否运行了测试。
+* 测试结果是什么。
+* 是否还有需要用户注意的风险。
+
+如果没有实际修改代码，而是给出方案，应说明：
+
+* 推荐方案。
+* 关键步骤。
+* 注意事项。
+* 可复制的命令或文件内容。
+
+---
+
+## 处理不确定性
+
+遇到不确定情况时，不要假装知道。
+
+应优先：
+
+* 搜索仓库中的相关代码。
+* 查看 README、文档、配置文件、测试文件。
+* 根据现有实现推断项目约定。
+* 在仍不确定时，明确指出不确定点。
+
+不要做以下事情：
+
+* 编造不存在的文件路径。
+* 编造不存在的函数、类、接口。
+* 编造测试结果。
+* 编造依赖版本。
+* 在不了解上下文时大范围重构。
+
+---
+
+## 代码风格
+
+遵守当前仓库已有风格。
+
+包括但不限于：
+
+* 命名风格。
+* 文件组织方式。
+* 错误处理方式。
+* 日志输出方式。
+* 类型标注方式。
+* 注释风格。
+* 测试组织方式。
+
+如果项目已有 lint、format 或 test 命令，优先使用项目已有命令，不要擅自更换工具链。
+
+---
+
+## 依赖管理
+
+添加依赖前必须谨慎。
+
+只有在满足以下条件时才考虑新增依赖：
+
+* 用户明确要求；或
+* 现有项目无法合理实现；且
+* 新依赖带来的复杂度明显小于手写实现；且
+* 已说明新增依赖的原因。
+
+不要为了少写几行代码而引入大型依赖。
+
+---
+
+## Git 与提交
+
+除非用户明确要求，否则不要自动提交、推送、创建分支或修改远程仓库。
+
+如果用户要求生成提交信息，提交信息应：
+
+* 简洁。
+* 准确描述实际改动。
+* 不夸大范围。
+* 不写没有完成的内容。
+
+推荐格式：
+
+```text
+fix: 修复 xxx 问题
+feat: 添加 xxx 功能
+refactor: 简化 xxx 实现
+docs: 更新 xxx 文档
+test: 添加 xxx 测试
+```
+
+---
+
+## 禁止行为
+
+除非用户明确要求，不要执行以下行为：
+
+* 大范围重构。
+* 重写整个模块。
+* 替换项目技术栈。
+* 修改无关文件。
+* 删除无关代码。
+* 改动锁文件，除非依赖确实变化。
+* 修改格式化配置，除非任务要求。
+* 添加与需求无关的测试框架或工具。
+* 使用破坏性 Git 命令。
+* 修改密钥、token、证书、生产配置文件。
+
+---
+
+## 判断本规则是否生效
+
+如果本文件生效，应该能看到以下结果：
+
+* diff 更小，只包含必要改动。
+* 代码更直接，不会过度抽象。
+* Codex 会在复杂任务前先说明计划。
+* Codex 会在不确定时提出问题或说明假设。
+* Codex 不会顺手重构无关代码。
+* Codex 会尽量运行测试或说明验证方式。
+* 最终回复会清楚说明改动、验证和风险。
+
+---
+
+## 项目专属规则
+
+* 本项目面向 Ubuntu 22.04 和 ROS2 Humble；构建、测试和运行命令应在工作空间根目录 `/home/robot/WS_xr` 执行，并先 `source /opt/ros/humble/setup.bash`。
+* 工作空间包含 `xr_rm_input`、`xr_rm_teleop` 两个 `ament_python` 包，以及 `xr_rm_interfaces`、`xr_rm_bringup` 两个 `ament_cmake` 包；优先使用现有 ROS2 包、节点和消息，不要另建重复入口。
+* 修改 ROS 节点、launch、消息定义或安装配置后，至少运行 `colcon build --symlink-install`；涉及遥操作姿态控制时，再运行 `pytest src/xr_rm_teleop/test/test_orientation_control.py`。
+* 遥操作统一使用 `xr_rm_bringup/launch/arm_debug.launch.py`；调试和验证默认使用 `use_mock:=true`，未经用户明确要求不得连接真机、移动机械臂或操作夹爪。
+* 所有机器人运动相关改动必须保留工作空间/圆柱限位、线速度与角速度限制、指令超时和安全停止逻辑；不得默认关闭 `configure_safety_limits`，不得把 `move_to_initial_pose_on_connect` 的默认值改为 `true`。
+* 修改左右臂控制参数时，检查 `dual_arm_rm75.yaml`、`left_arm_rm75.yaml` 和 `right_arm_rm75.yaml` 中对应配置是否需要同步；保留双臂模式下 `left_arm_teleop`、`right_arm_teleop` 的节点名。
+* 睿尔曼 Python API2 仅为真机模式依赖；不要让 mock 模式强制依赖厂商 SDK，也不要为同一机械臂新增并发 RealMan 连接。

CODEX.md

@@ -1,250 +0,0 @@
-# CODEX.md
-
-This file provides guidance to Codex, Claude Code, and other coding agents when working with this repository. Higher-priority system or developer instructions override this file.
-
-## Project Overview
-
-This repository is the `src/` layer of a ROS2 Humble workspace for PICO/XR controller teleoperation of left and right RealMan RM75 arms.
-
-The core behavior is relative Cartesian pose streaming:
-
-```text
-PICO/XR UDP JSON
-  -> xr_rm_input/udp_controller_receiver
-  -> /xr/left_controller and /xr/right_controller
-  -> xr_rm_teleop/single_arm_velocity_teleop
-  -> MockRealManAdapter or RealManAdapter
-  -> /xr_rm/<arm_name>/current_pose
-  -> /xr_rm/<arm_name>/raw_target_pose
-  -> /xr_rm/<arm_name>/target_pose
-  -> /xr_rm/<arm_name>/cmd_vel
-  -> /xr_rm/<arm_name>/target_clamped
-```
-
-When `grip=true`, the first valid frame locks both the XR controller origin and the robot TCP origin. Later controller translation and rotation deltas are converted into target TCP poses and sent through `rm_movep_canfd`. When `grip=false`, `pose_valid=false`, UDP data times out, an adapter exception occurs, or the node shuts down, motion must stop.
-
-## Architecture
-
-The project consists of:
-
-- **Interface package** (`xr_rm_interfaces`): defines the `XrController` message.
-- **Input package** (`xr_rm_input`): receives UDP controller JSON, normalizes controller payloads, publishes left/right XR controller topics, and provides `sample_udp_sender` for mock/debug input.
-- **Teleop package** (`xr_rm_teleop`): maps relative XR controller motion to RM75 Cartesian target poses and dispatches commands through mock or real adapters.
-- **Bringup package** (`xr_rm_bringup`): owns launch files, arm YAML configuration, and the local launcher UI.
-- **Unity/PICO sender** (`unity/XR_RM_PICO_UDP_Sender`): PICO 4 Ultra Unity project that sends controller pose, validity, source, tracking status, sequence, and timestamp fields over UDP.
-
-Key control facts:
-
-- Main launch file: `xr_rm_bringup/launch/arm_debug.launch.py`
-- Supported launch arm modes: `arm:=left|right|both`
-- Supported adapter modes: `use_mock:=true|false`
-- ROS2 workspace root: `/home/robot/WS_xr`
-- Repository/source root: `/home/robot/WS_xr/src`
-- `cmd_vel` is a debug estimate of target-pose change rate, not the real robot command topic.
-
-## Build Commands
-
-Run ROS2 commands from the workspace root:
-
-```bash
-cd /home/robot/WS_xr
-source /opt/ros/humble/setup.bash
-rosdep install --from-paths src -y --ignore-src
-colcon build --symlink-install
-source install/setup.bash
-```
-
-Git inspection commands:
-
-```bash
-cd /home/robot/WS_xr/src
-git status --short
-git diff
-git diff --check
-```
-
-## Run Commands
-
-### Mock Dual-Arm Debug
-
-```bash
-cd /home/robot/WS_xr
-source /opt/ros/humble/setup.bash
-source install/setup.bash
-ros2 launch xr_rm_bringup arm_debug.launch.py arm:=both use_mock:=true
-```
-
-In another terminal:
-
-```bash
-cd /home/robot/WS_xr
-source /opt/ros/humble/setup.bash
-source install/setup.bash
-ros2 run xr_rm_input sample_udp_sender --hand both --host 127.0.0.1 --port 15000 \
-  --pattern axis_sweep --seconds 60 --both-mode staggered
-```
-
-Use `--rotation-pattern rpy_steps` when checking orientation mapping.
-
-### Real Arm Debug
-
-```bash
-ros2 launch xr_rm_bringup arm_debug.launch.py arm:=left use_mock:=false
-ros2 launch xr_rm_bringup arm_debug.launch.py arm:=right use_mock:=false
-```
-
-Dual-arm real hardware:
-
-```bash
-ros2 launch xr_rm_bringup arm_debug.launch.py arm:=both use_mock:=false \
-  left_robot_ip:=192.168.192.18 \
-  right_robot_ip:=192.168.192.19
-```
-
-Launcher UI:
-
-```bash
-python3 src/xr_rm_bringup/tools/launcher_ui.py
-```
-
-## Debug Topics
-
-```bash
-ros2 topic echo /xr/left_controller
-ros2 topic echo /xr/right_controller
-ros2 topic echo /xr_rm/left_rm75/target_pose
-ros2 topic echo /xr_rm/right_rm75/target_pose
-ros2 topic echo /xr_rm/left_rm75/cmd_vel
-ros2 topic echo /xr_rm/right_rm75/cmd_vel
-```
-
-## UDP Protocol
-
-Preferred Unity packets contain top-level `controllers.left` and `controllers.right` objects plus `t`, `source_time`, `seq`, and `frame_id`.
-
-Each controller payload should include:
-
-- `grip`
-- `trigger`
-- `pos[3]`
-- `quat[4]`
-- `pose_valid`
-- `pose_source`
-- `tracking_state`
-- `controller_status`
-- `grip_value`
-- `axis[2]`
-- `buttons`
-
-The receiver also supports single-controller debug packets with `hand`, `pos`, and `quat`, plus aliases such as `position`, `p`, `pose.position`, `orientation`, and `q`. The default quaternion order is `xyzw`; use `quat_order:=wxyz` only when the sender really emits `wxyz`.
-
-When the PICO HUD shows `invalid none`, expect ROS-side `grip` to be forced false. Debug in this order: Unity pose source, `udp_controller_receiver` warnings, then teleop timeout or clamping topics.
-
-## Coordinate Notes
-
-PICO/OpenXR project coordinates are:
-
-- `+X`: right
-- `+Y`: up
-- `+Z`: back
-
-Current Unity `Project (+Z back)` output must remain in that project coordinate convention. PXR `pxr_predict` native values are converted to:
-
-```text
-project.x = native.z
-project.y = native.y
-project.z = -native.x
-```
-
-`Source raw` is only for field comparison and diagnostics. If `/xr/*_controller.pose.position` already matches the expected PICO/OpenXR coordinates but one arm moves in the wrong robot direction, prefer changing only that arm's YAML `xr_to_robot_matrix`.
-
-## Development Workflow
-
-Use the repository rule file as the project-level source of truth for coding-agent behavior:
-
-- Read relevant code and docs before editing.
-- Check `git status --short` before edits.
-- Treat existing uncommitted changes as user work; do not revert them unless explicitly requested.
-- Unless the user explicitly asks for direct code or file changes, first explain the proposed approach and wait for the user to decide whether to execute, continue, revise, or stop.
-- Before writing files, explain the goal understanding, files to touch, implementation plan, risks, and validation path, then wait for user confirmation.
-- Skip the confirmation gate only when the user's current request clearly authorizes direct modification, such as "可以直接修改", "无需确认", "直接执行", or equivalent wording.
-- Keep edits small and task-scoped.
-- Do not automatically run `git add`, commit, push, force push, delete branches, or merge branches.
-- After changes, summarize touched files, behavior changes, validation performed, and remaining risks.
-
-## Safety Rules
-
-This is a real robot teleoperation project. Keep all hardware-related changes conservative.
-
-Do not casually modify:
-
-- Left arm IP: `192.168.192.18`
-- Right arm IP: `192.168.192.19`
-- RM75 TCP port: `8080`
-- Workspace limits
-- Cylinder limits
-- `xr_to_robot_matrix`
-- Initial joint or TCP poses
-- Speed and acceleration limits
-- End-effector peripheral configuration
-
-Preserve stop behavior for:
-
-- `grip=false`
-- `pose_valid=false`
-- UDP timeout or stale controller data
-- Adapter exceptions
-- Node shutdown
-- PICO app pause, exit, or disabled sending
-
-Additional safety constraints:
-
-- Keep `move_to_initial_pose_on_connect` defaulting to `false`.
-- Do not bypass receiver behavior that forces `grip=false` when `pose_valid=false`.
-- Validate hardware-related behavior in this order: static checks, mock mode, single real arm, dual real arms.
-- Do not claim dual-arm collision detection exists unless code implements it.
-
-## Unity / PICO Notes
-
-- Current Unity project: `unity/XR_RM_PICO_UDP_Sender`
-- Current PICO SDK: `unity/PICO-Unity-Integration-SDK-release_3.4.0`
-- Treat the PICO SDK as third-party code unless the user explicitly targets it.
-- Treat Unity `Library/`, `Builds/`, `Logs/`, `UserSettings/`, APKs, and generated logs as local/generated artifacts.
-- The Unity package depends on TextMeshPro.
-- PICO panel fonts come from pregenerated `Assets/Resources/Fonts/Roboto-Regular SDF.asset` and `Roboto-Bold SDF.asset`.
-- Do not create TMP font assets dynamically at APK runtime.
-- The PICO UDP target IP must be the Ubuntu ROS host IPv4 address on the same LAN.
-- PICO app pause, exit, or disabled sending must send `grip=false`.
-- Invalid pose data must send `pose_valid=false` and allow the ROS receiver to force stop.
-
-## Documentation Rules
-
-- `README.md` is the main project document for humans.
-- `CODEX.md` is the coding-agent workflow and safety document.
-- Keep `docs/` for focused setup guides and report materials.
-- Do not add scattered Markdown files unless the user asks.
-- Do not document unimplemented features as complete.
-- Keep mock, real-arm, and Unity/PICO paths explicit.
-
-## Testing and Validation
-
-There is no single full-system automated test that proves real-hardware safety. Prefer layered validation:
-
-- Static checks: syntax, imports, formatting-sensitive checks, `git diff --check`.
-- ROS build: `colcon build --symlink-install`.
-- Mock validation: launch `arm_debug.launch.py` with `use_mock:=true`.
-- UDP validation: run `sample_udp_sender` with `axis_sweep` and, when needed, `rpy_steps`.
-- Real hardware validation: single arm first, then dual arm.
-
-## Important Notes
-
-- Do not implement or claim the following unless the user explicitly requests and code actually supports it:
-  - XRoboToolkit PC-Service bridge as a required runtime path
-  - D405/D435 video streaming or data recording
-  - Dual-arm collision detection
-  - Autonomous picking state machine
-  - QP IK, dexterous-hand retargeting, or whole-body tracker support
-  - Full time synchronization
-  - Robot state feedback to PICO
-- If motion direction is wrong, inspect `/xr/*_controller.pose.position` first before changing robot-side YAML.
-- The project should stay understandable enough for field debugging; avoid broad refactors during safety-sensitive fixes.

README.md

@@ -182,6 +182,9 @@ ros2 launch xr_rm_bringup arm_debug.launch.py arm:=left use_mock:=false
 ros2 launch xr_rm_bringup arm_debug.launch.py arm:=right use_mock:=false
 ```
 
+单臂真机默认执行配置文件中的 `movej(initial_joint_pose)`；现场需要跳过时，显式传入
+`move_to_initial_pose_on_connect:=false`。
+
 第四步：双臂真机。
 
 ```bash
@@ -190,7 +193,7 @@ ros2 launch xr_rm_bringup arm_debug.launch.py arm:=both use_mock:=false \
   right_robot_ip:=192.168.192.19
 ```
 
-默认不会自动移动到初始化点。只有在确认安全区清空后，才显式打开：
+双臂默认不会自动移动到初始化点。以后需要启用时，在确认安全区清空后显式打开：
 
 ```bash
 ros2 launch xr_rm_bringup arm_debug.launch.py arm:=both use_mock:=false \
@@ -221,7 +224,7 @@ ros2 launch xr_rm_bringup arm_debug.launch.py arm:=both use_mock:=false \
 - `enable_trigger_gripper_control`：是否允许用 `trigger` 点击切换对应夹爪状态，默认 `true`。
 - `trigger_close_threshold`：trigger 点击判定阈值，默认 `0.95`。
 - `configure_peripheral_on_connect`：遥操作节点连接真机后是否配置末端外设，默认 `true`；工具控制会复用同一个 RealMan 连接，避免两个进程同时抢占同一机械臂。
-- `move_to_initial_pose_on_connect`：连接后是否执行 `movej`/`movel` 初始化，默认 `false`。
+- `move_to_initial_pose_on_connect`：连接后是否执行 `movej(initial_joint_pose)`；默认 `auto`，单臂配置启用、双臂配置禁用，也可显式传 `true`/`false` 覆盖。
 
 ## 配置文件说明
 
@@ -249,7 +252,7 @@ ros2 launch xr_rm_bringup arm_debug.launch.py arm:=both use_mock:=false \
 - `xr_to_robot_matrix`：`/xr/*_controller` Project 位移到 RM75 base 坐标的映射矩阵。
 - `current_pose_poll_hz`：低频读取真机当前 TCP 的频率；控制中不再每帧阻塞读取状态。
 - `mock_initial_pose`：mock 模式初始 TCP 位姿。
-- `initial_joint_pose` / `initial_tcp_pose`：可选真机初始化点。
+- `initial_joint_pose`：可选真机初始关节角。
 
 当前 `/xr/*_controller` 的 Project 坐标约定：
 

openspec/config.yaml

@@ -0,0 +1,20 @@
+schema: spec-driven
+
+# Project context (optional)
+# This is shown to AI when creating artifacts.
+# Add your tech stack, conventions, style guides, domain knowledge, etc.
+# Example:
+#   context: |
+#     Tech stack: TypeScript, React, Node.js
+#     We use conventional commits
+#     Domain: e-commerce platform
+
+# Per-artifact rules (optional)
+# Add custom rules for specific artifacts.
+# Example:
+#   rules:
+#     proposal:
+#       - Keep proposals under 500 words
+#       - Always include a "Non-goals" section
+#     tasks:
+#       - Break tasks into chunks of max 2 hours

xr_rm_bringup/config/dual_arm_rm75.yaml

@@ -59,7 +59,6 @@ left_arm_teleop:
     joint_max_acc: 180.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_tcp_pose: [-0.2562, -0.2765, 0.1489, -3.0190, -0.1010, 3.1400]
     init_move_speed: 20
     debug_topic_prefix: /xr_rm
 
@@ -114,6 +113,5 @@ right_arm_teleop:
     joint_max_acc: 180.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_tcp_pose: [0.2663, -0.2606, 0.1027, 3.0330, 0.0000, 1.0910]
     init_move_speed: 20
     debug_topic_prefix: /xr_rm

xr_rm_bringup/config/left_arm_rm75.yaml

@@ -50,8 +50,7 @@ single_arm_velocity_teleop:
     max_angular_acc: 2.0
     joint_max_speed: 180.0
     joint_max_acc: 180.0
-    move_to_initial_pose_on_connect: false
+    move_to_initial_pose_on_connect: true
-    initial_joint_pose: [-167.21, 28.48, 28.21, 61.35, -14.40, 84.49, -124.51]
+    initial_joint_pose: [-79.55, -9.99, 71.01, 101.45, 95.07, -84.47, -74.52]
-    initial_tcp_pose: [-0.2562, -0.2765, 0.1489, -3.0190, -0.1010, 3.1400]
     init_move_speed: 20
     debug_topic_prefix: /xr_rm

xr_rm_bringup/config/right_arm_rm75.yaml

@@ -49,8 +49,7 @@ single_arm_velocity_teleop:
     max_angular_acc: 2.0
     joint_max_speed: 180.0
     joint_max_acc: 180.0
-    move_to_initial_pose_on_connect: false
+    move_to_initial_pose_on_connect: true
-    initial_joint_pose: [-25.60, 34.09, -19.55, 71.59, 16.97, 80.98, 59.67]
+    initial_joint_pose: [-90.14, 3.76, -86.89, 87.89, -96.53, -79.62, -90.04]
-    initial_tcp_pose: [0.2663, -0.2606, 0.1027, 3.0330, 0.0000, 1.0910]
     init_move_speed: 20
     debug_topic_prefix: /xr_rm

xr_rm_bringup/launch/arm_debug.launch.py

@@ -26,6 +26,10 @@ def _config_file(name: str) -> PathJoinSubstitution:
     ])
 
 
+def _initial_pose_override(value: str) -> dict[str, bool]:
+    return {} if value == "auto" else {"move_to_initial_pose_on_connect": _as_bool(value)}
+
+
 def _udp_receiver_node() -> Node:
     """接收 PICO/XR UDP 数据，并发布左右手柄 ROS2 话题。"""
     return Node(
@@ -46,7 +50,7 @@ def _udp_receiver_node() -> Node:
 def _single_arm_node(
     arm: str,
     use_mock: bool,
-    move_to_initial_pose: bool,
+    move_to_initial_pose: str,
     avoid_singularity: int,
     frame_type: int,
     control_rate_hz: float,
@@ -77,7 +81,7 @@ def _single_arm_node(
                 "control_rate_hz": control_rate_hz,
                 "follow": follow,
                 "configure_safety_limits": configure_safety_limits,
-                "move_to_initial_pose_on_connect": move_to_initial_pose,
+                **_initial_pose_override(move_to_initial_pose),
                 "enable_tool_control": enable_tool_control,
                 "enable_trigger_gripper_control": enable_trigger_gripper_control,
                 "trigger_close_threshold": trigger_close_threshold,
@@ -96,7 +100,7 @@ def _arm_name(arm: str) -> str:
 
 def _dual_arm_nodes(
     use_mock: bool,
-    move_to_initial_pose: bool,
+    move_to_initial_pose: str,
     left_avoid_singularity: int,
     right_avoid_singularity: int,
     frame_type: int,
@@ -127,7 +131,7 @@ def _dual_arm_nodes(
                     "control_rate_hz": control_rate_hz,
                     "follow": follow,
                     "configure_safety_limits": configure_safety_limits,
-                    "move_to_initial_pose_on_connect": move_to_initial_pose,
+                    **_initial_pose_override(move_to_initial_pose),
                     "enable_tool_control": enable_tool_control,
                     "enable_trigger_gripper_control": enable_trigger_gripper_control,
                     "trigger_close_threshold": trigger_close_threshold,
@@ -154,7 +158,7 @@ def _dual_arm_nodes(
                     "control_rate_hz": control_rate_hz,
                     "follow": follow,
                     "configure_safety_limits": configure_safety_limits,
-                    "move_to_initial_pose_on_connect": move_to_initial_pose,
+                    **_initial_pose_override(move_to_initial_pose),
                     "enable_tool_control": enable_tool_control,
                     "enable_trigger_gripper_control": enable_trigger_gripper_control,
                     "trigger_close_threshold": trigger_close_threshold,
@@ -173,9 +177,9 @@ def _launch_setup(context, *args, **kwargs):
     del args, kwargs
     arm = LaunchConfiguration("arm").perform(context).strip().lower()
     use_mock = _as_bool(LaunchConfiguration("use_mock").perform(context))
-    move_to_initial_pose = _as_bool(
+    move_to_initial_pose = LaunchConfiguration(
-        LaunchConfiguration("move_to_initial_pose_on_connect").perform(context)
+        "move_to_initial_pose_on_connect"
-    )
+    ).perform(context).strip().lower()
     avoid_override = LaunchConfiguration("avoid_singularity").perform(context).strip()
     left_avoid_singularity = int(
         avoid_override or LaunchConfiguration("left_avoid_singularity").perform(context)
@@ -277,8 +281,8 @@ def generate_launch_description() -> LaunchDescription:
         DeclareLaunchArgument("trigger_close_threshold", default_value="0.95"),
         # 连接成功后是否配置外设；关闭后仅订阅开合话题，但开合前需要另行完成外设配置。
         DeclareLaunchArgument("configure_peripheral_on_connect", default_value="true"),
-        # 默认不自动移动到初始点，确认安全区后再显式打开。
+        # auto 时由单/双臂 YAML 决定；也可显式传 true/false 覆盖。
-        DeclareLaunchArgument("move_to_initial_pose_on_connect", default_value="false"),
+        DeclareLaunchArgument("move_to_initial_pose_on_connect", default_value="auto"),
         # OpaqueFunction 允许根据 arm/use_mock 等运行时参数动态生成节点。
         OpaqueFunction(function=_launch_setup),
     ])

xr_rm_bringup/tools/launcher_ui.py

@@ -290,8 +290,7 @@ def build_commands_by_mode(mode: str) -> list[tuple[str, str]]:
             (
                 "Left Arm RealMan Launch",
                 "ros2 launch xr_rm_bringup arm_debug.launch.py arm:=left use_mock:=false "
-                f"left_robot_ip:={DEFAULT_LEFT_IP} "
+                f"left_robot_ip:={DEFAULT_LEFT_IP}",
-                "move_to_initial_pose_on_connect:=false",
             ),
             ("XRobotoolkit UDP Bridge (90 Hz)", _xrobotoolkit_bridge_command()),
             ("Left Tool Open", _tool_command("left", True)),
@@ -308,8 +307,7 @@ def build_commands_by_mode(mode: str) -> list[tuple[str, str]]:
             (
                 "Right Arm RealMan Launch",
                 "ros2 launch xr_rm_bringup arm_debug.launch.py arm:=right use_mock:=false "
-                f"right_robot_ip:={DEFAULT_RIGHT_IP} "
+                f"right_robot_ip:={DEFAULT_RIGHT_IP}",
-                "move_to_initial_pose_on_connect:=false",
             ),
             ("XRobotoolkit UDP Bridge (90 Hz)", _xrobotoolkit_bridge_command()),
             ("Right Tool Open", _tool_command("right", True)),

xr_rm_bringup/tools/realman_dual_arm_state_monitor.py

@@ -418,12 +418,12 @@ def format_arm_block(name: str, snap: ArmSnapshot, now_t: float) -> str:
     age = now_t - snap.last_update if snap.last_update > 0 else float("inf")
     stale_flag = "(stale)" if age > 2.0 else ""
 
-    j = ", ".join(f"{v:7.2f}" for v in snap.joint_deg)
+    j = ", ".join(f"{v:.2f}" for v in snap.joint_deg)
-    p = snap.pose_euler
+    p = ", ".join(f"{v:.4f}" for v in snap.pose_euler)
     return (
         f"[{name}] update_age={age:5.2f}s {stale_flag}, ret={snap.ret_code}\n"
         f"  Joint(deg): [{j}]\n"
-        f"  Pose[x y z rx ry rz]: [{p[0]: .4f}, {p[1]: .4f}, {p[2]: .4f}, {p[3]: .4f}, {p[4]: .4f}, {p[5]: .4f}]\n"
+        f"  Pose[x y z rx ry rz]: [{p}]\n"
         f"  Err: {snap.err_text}"
     )
 

xr_rm_teleop/test/test_initial_joint_pose.py

@@ -0,0 +1,19 @@
+from xr_rm_teleop.realman_adapter import RealManAdapter
+
+
+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, 1, initial_joint_pose=joints)
+    adapter._arm = FakeArm()
+
+    adapter._move_to_initial_pose()
+
+    assert adapter._arm.calls == [(joints, 20, 0, 0, 1)]

xr_rm_teleop/xr_rm_teleop/realman_adapter.py

@@ -91,7 +91,6 @@ class RealManAdapter:
         joint_max_acc: float = 180.0,
         move_to_initial_pose_on_connect: bool = False,
         initial_joint_pose: list[float] | None = None,
-        initial_tcp_pose: list[float] | None = None,
         init_move_speed: int = 20,
         canfd_trajectory_mode: int = 2,
         canfd_radio: int = 0,
@@ -110,7 +109,6 @@ class RealManAdapter:
         self._joint_max_acc = joint_max_acc
         self._move_to_initial_pose_on_connect = move_to_initial_pose_on_connect
         self._initial_joint_pose = initial_joint_pose
-        self._initial_tcp_pose = initial_tcp_pose
         self._init_move_speed = init_move_speed
         self._canfd_trajectory_mode = canfd_trajectory_mode
         self._canfd_radio = canfd_radio
@@ -219,13 +217,11 @@ class RealManAdapter:
             self._try_call("rm_set_joint_max_acc", joint_index, self._joint_max_acc)
 
     def _move_to_initial_pose(self) -> None:
-        if self._initial_joint_pose is None or self._initial_tcp_pose is None:
+        if self._initial_joint_pose is None:
-            raise RuntimeError("启用初始位姿移动时必须配置 initial_joint_pose 和 initial_tcp_pose")
+            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)")
-        ret = self._arm.rm_movel(self._initial_tcp_pose, self._init_move_speed, 0, 0, 1)
-        self._check_return(ret, "rm_movel(initial_tcp_pose)")
 
     def _try_call(self, name: str, *args: Any) -> None:
         func = getattr(self._arm, name, None)

xr_rm_teleop/xr_rm_teleop/single_arm_velocity_teleop.py

@@ -193,7 +193,6 @@ class SingleArmVelocityTeleop(Node):
         self.declare_parameter("joint_max_acc", 180.0)
         self.declare_parameter("move_to_initial_pose_on_connect", False)
         self.declare_parameter("initial_joint_pose", [0.0] * 7)
-        self.declare_parameter("initial_tcp_pose", [0.35, 0.0, 0.30, 0.0, 0.0, 0.0])
         self.declare_parameter("init_move_speed", 20)
         self.declare_parameter("canfd_trajectory_mode", 2)
         self.declare_parameter("canfd_radio", 0)
@@ -305,7 +304,6 @@ class SingleArmVelocityTeleop(Node):
             joint_max_acc=float(self.get_parameter("joint_max_acc").value),
             move_to_initial_pose_on_connect=self._bool_parameter("move_to_initial_pose_on_connect"),
             initial_joint_pose=self._float_list_parameter("initial_joint_pose", 7),
-            initial_tcp_pose=self._float_list_parameter("initial_tcp_pose", 6),
             init_move_speed=int(self.get_parameter("init_move_speed").value),
             canfd_trajectory_mode=int(self.get_parameter("canfd_trajectory_mode").value),
             canfd_radio=int(self.get_parameter("canfd_radio").value),