From 33734524edd291e78a32540525396cb632aea873 Mon Sep 17 00:00:00 2001 From: YikaiFu-cart Date: Tue, 14 Jul 2026 17:20:50 +0800 Subject: [PATCH] adjust initial pose of robotic arms --- .gitignore | 2 ++ README.md | 9 ++++--- openspec/config.yaml | 20 ++++++++++++++++ xr_rm_bringup/config/dual_arm_rm75.yaml | 2 -- xr_rm_bringup/config/left_arm_rm75.yaml | 5 ++-- xr_rm_bringup/config/right_arm_rm75.yaml | 5 ++-- xr_rm_bringup/launch/arm_debug.launch.py | 24 +++++++++++-------- xr_rm_bringup/tools/launcher_ui.py | 6 ++--- .../tools/realman_dual_arm_state_monitor.py | 6 ++--- xr_rm_teleop/test/test_initial_joint_pose.py | 19 +++++++++++++++ xr_rm_teleop/xr_rm_teleop/realman_adapter.py | 8 ++----- .../single_arm_velocity_teleop.py | 2 -- 12 files changed, 72 insertions(+), 36 deletions(-) create mode 100644 openspec/config.yaml create mode 100644 xr_rm_teleop/test/test_initial_joint_pose.py diff --git a/.gitignore b/.gitignore index 742d772..b9bd1a8 100644 --- a/.gitignore +++ b/.gitignore @@ -42,3 +42,5 @@ AMENT_IGNORE # Built Visual Studio Code Extensions *.vsix + +.codex \ No newline at end of file diff --git a/README.md b/README.md index b152525..4b7cb4b 100755 --- a/README.md +++ b/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 坐标约定： diff --git a/openspec/config.yaml b/openspec/config.yaml new file mode 100644 index 0000000..392946c --- /dev/null +++ b/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 diff --git a/xr_rm_bringup/config/dual_arm_rm75.yaml b/xr_rm_bringup/config/dual_arm_rm75.yaml index 52a8006..634c7b2 100755 --- a/xr_rm_bringup/config/dual_arm_rm75.yaml +++ b/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 diff --git a/xr_rm_bringup/config/left_arm_rm75.yaml b/xr_rm_bringup/config/left_arm_rm75.yaml index f7e663a..bbe721b 100644 --- a/xr_rm_bringup/config/left_arm_rm75.yaml +++ b/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 - 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] + move_to_initial_pose_on_connect: true + initial_joint_pose: [-79.55, -9.99, 71.01, 101.45, 95.07, -84.47, -74.52] init_move_speed: 20 debug_topic_prefix: /xr_rm diff --git a/xr_rm_bringup/config/right_arm_rm75.yaml b/xr_rm_bringup/config/right_arm_rm75.yaml index 2cbe4e8..b088d18 100644 --- a/xr_rm_bringup/config/right_arm_rm75.yaml +++ b/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 - 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] + move_to_initial_pose_on_connect: true + 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 diff --git a/xr_rm_bringup/launch/arm_debug.launch.py b/xr_rm_bringup/launch/arm_debug.launch.py index fa3eae9..b2d2483 100644 --- a/xr_rm_bringup/launch/arm_debug.launch.py +++ b/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( - LaunchConfiguration("move_to_initial_pose_on_connect").perform(context) - ) + move_to_initial_pose = LaunchConfiguration( + "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"), - # 默认不自动移动到初始点，确认安全区后再显式打开。 - DeclareLaunchArgument("move_to_initial_pose_on_connect", default_value="false"), + # auto 时由单/双臂 YAML 决定；也可显式传 true/false 覆盖。 + DeclareLaunchArgument("move_to_initial_pose_on_connect", default_value="auto"), # OpaqueFunction 允许根据 arm/use_mock 等运行时参数动态生成节点。 OpaqueFunction(function=_launch_setup), ]) diff --git a/xr_rm_bringup/tools/launcher_ui.py b/xr_rm_bringup/tools/launcher_ui.py index 314ed24..3409be7 100755 --- a/xr_rm_bringup/tools/launcher_ui.py +++ b/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} " - "move_to_initial_pose_on_connect:=false", + f"left_robot_ip:={DEFAULT_LEFT_IP}", ), ("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} " - "move_to_initial_pose_on_connect:=false", + f"right_robot_ip:={DEFAULT_RIGHT_IP}", ), ("XRobotoolkit UDP Bridge (90 Hz)", _xrobotoolkit_bridge_command()), ("Right Tool Open", _tool_command("right", True)), diff --git a/xr_rm_bringup/tools/realman_dual_arm_state_monitor.py b/xr_rm_bringup/tools/realman_dual_arm_state_monitor.py index 4e623c6..299cb20 100755 --- a/xr_rm_bringup/tools/realman_dual_arm_state_monitor.py +++ b/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) - p = snap.pose_euler + j = ", ".join(f"{v:.2f}" for v in snap.joint_deg) + 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}" ) diff --git a/xr_rm_teleop/test/test_initial_joint_pose.py b/xr_rm_teleop/test/test_initial_joint_pose.py new file mode 100644 index 0000000..8355108 --- /dev/null +++ b/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)] diff --git a/xr_rm_teleop/xr_rm_teleop/realman_adapter.py b/xr_rm_teleop/xr_rm_teleop/realman_adapter.py index 3933951..8b6c962 100755 --- a/xr_rm_teleop/xr_rm_teleop/realman_adapter.py +++ b/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: - raise RuntimeError("启用初始位姿移动时必须配置 initial_joint_pose 和 initial_tcp_pose") + 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)") - 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) diff --git a/xr_rm_teleop/xr_rm_teleop/single_arm_velocity_teleop.py b/xr_rm_teleop/xr_rm_teleop/single_arm_velocity_teleop.py index df5439a..74440d0 100755 --- a/xr_rm_teleop/xr_rm_teleop/single_arm_velocity_teleop.py +++ b/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),