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