YikaiFu-cart/acRealman_xr
Template
1
1
Fork 1
Code Issues Pull Requests Packages Projects Releases Wiki Activity

adjust initial pose of robotic arms

Browse Source
This commit is contained in:
YikaiFu-cart
2026-07-14 17:20:50 +08:00
parent 8aed1687f3
commit 33734524ed
12 changed files with 72 additions and 36 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
.gitignore vendored
View File

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

9
README.md
View File

@ -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 坐标约定：

20
openspec/config.yaml Normal file
View File

@ -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

2
xr_rm_bringup/config/dual_arm_rm75.yaml
View File

@ -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

5
xr_rm_bringup/config/left_arm_rm75.yaml
View File

@ -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

5
xr_rm_bringup/config/right_arm_rm75.yaml
View File

@ -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

24
xr_rm_bringup/launch/arm_debug.launch.py
View File

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

6
xr_rm_bringup/tools/launcher_ui.py
View File

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

6
xr_rm_bringup/tools/realman_dual_arm_state_monitor.py
View File

@ -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}"
)

19
xr_rm_teleop/test/test_initial_joint_pose.py Normal file
View File

@ -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)]

8
xr_rm_teleop/xr_rm_teleop/realman_adapter.py
View File

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

2
xr_rm_teleop/xr_rm_teleop/single_arm_velocity_teleop.py
View File

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