adjust initial pose of robotic arms

.gitignore

@@ -42,3 +42,5 @@ AMENT_IGNORE
 
 # Built Visual Studio Code Extensions
 *.vsix
+
+.codex

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