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

dual arm control from sample_udp_sender.py

Browse Source
This commit is contained in:
YikaiFu-cart
2026-05-25 17:02:37 +08:00
parent 340bd9138d
commit 7551f1e8ea
18 changed files with 976 additions and 74 deletions
Download Patch File Download Diff File Expand all files Collapse all files

6
xr_rm_teleop/setup.py
View File

@ -1,3 +1,9 @@
"""xr_rm_teleop 包安装配置。
该包提供基于 XR 相对位移的 RM75 笛卡尔速度遥操作节点，以及把
手柄 trigger 映射到夹爪力控命令的桥接节点。
"""
from setuptools import setup
package_name = "xr_rm_teleop"

3
xr_rm_teleop/xr_rm_teleop/__init__.py
View File

@ -1 +1,4 @@
"""XR 到 RM75 遥操作节点包。
核心模块包括单臂速度遥操作、RealMan SDK 适配层和夹爪 trigger 桥接。
"""

7
xr_rm_teleop/xr_rm_teleop/gripper_trigger_bridge.py
View File

@ -1,3 +1,9 @@
"""手柄 trigger 到夹爪力控指令的桥接节点。
订阅 XR 手柄消息，将 trigger 压力映射为 OmniPicker 可使用的归一化
闭合力比例，并在消息超时或未握持时自动回到开爪指令。
"""
from __future__ import annotations
import rclpy
@ -62,6 +68,7 @@ class GripperTriggerBridge(Node):
def _publish_command(self) -> None:
command = self._open_command
# 手柄消息超时后立即退回开爪，避免夹爪在通信中断时保持闭合力。
if self._last_msg is not None and self._last_msg_time is not None:
age = (self.get_clock().now() - self._last_msg_time).nanoseconds * 1e-9
if age <= self._stale_timeout_sec:

114
xr_rm_teleop/xr_rm_teleop/realman_adapter.py
View File

@ -1,5 +1,12 @@
"""RM75 机械臂适配层。
对上提供统一的当前位姿读取、笛卡尔速度发送和停止接口；对下根据配置
选择 mock 积分模拟器或睿尔曼 Python API2 真机通信。
"""
from __future__ import annotations
import time
from dataclasses import dataclass
from numbers import Number
from typing import Any
@ -72,6 +79,9 @@ class RealManAdapter:
initial_joint_pose: list[float] | None = None,
initial_tcp_pose: list[float] | None = None,
init_move_speed: int = 20,
command_mode: str = "velocity",
canfd_trajectory_mode: int = 2,
canfd_radio: int = 0,
) -> None:
self._robot_ip = robot_ip
self._robot_port = robot_port
@ -90,7 +100,12 @@ class RealManAdapter:
self._initial_joint_pose = initial_joint_pose
self._initial_tcp_pose = initial_tcp_pose
self._init_move_speed = init_move_speed
self._command_mode = command_mode
self._canfd_trajectory_mode = canfd_trajectory_mode
self._canfd_radio = canfd_radio
self._arm: Any | None = None
if self._command_mode not in ("velocity", "pose_canfd"):
raise ValueError("command_mode must be one of: velocity, pose_canfd")
def connect(self) -> None:
try:
@ -103,17 +118,21 @@ class RealManAdapter:
self._arm = RoboticArm(rm_thread_mode_e.RM_TRIPLE_MODE_E)
handle = self._arm.rm_create_robot_arm(self._robot_ip, self._robot_port)
self._check_robot_handle(handle)
self._log_info(
"RealMan connected: "
f"ip={self._robot_ip}, port={self._robot_port}, "
f"avoid_singularity={self._avoid_singularity}, "
f"frame_type={self._frame_type}, dt_ms={self._dt_ms}, "
f"command_mode={self._command_mode}"
)
if self._configure_safety_limits:
self._apply_safety_limits()
if self._move_to_initial_pose_on_connect:
self._move_to_initial_pose()
# 速度透传初始化必须和控制循环周期一致，避免真实机械臂出现周期不稳定。
ret = self._arm.rm_set_movev_canfd_init(
self._avoid_singularity,
self._frame_type,
self._dt_ms,
)
self._check_return(ret, "rm_set_movev_canfd_init")
if self._command_mode == "velocity":
self._init_movev_canfd()
else:
self._log_info("command_mode=pose_canfd，跳过 rm_set_movev_canfd_init。")
def get_current_pose(self) -> ArmPose:
self._require_arm()
@ -125,14 +144,32 @@ class RealManAdapter:
def send_cartesian_velocity(self, velocity: list[float], follow: bool) -> None:
self._require_arm()
if self._command_mode == "pose_canfd":
return
ret = self._arm.rm_movev_canfd(velocity, follow, 0, 0)
self._check_return(ret, "rm_movev_canfd")
def send_cartesian_target(self, pose: ArmPose, follow: bool) -> None:
self._require_arm()
if self._command_mode != "pose_canfd":
raise RuntimeError("send_cartesian_target requires command_mode=pose_canfd")
ret = self._arm.rm_movep_canfd(
[pose.x, pose.y, pose.z, pose.rx, pose.ry, pose.rz],
follow,
self._canfd_trajectory_mode,
self._canfd_radio,
)
self._check_return(ret, "rm_movep_canfd")
def uses_pose_targets(self) -> bool:
return self._command_mode == "pose_canfd"
def stop(self) -> None:
if self._arm is None:
return
try:
self._arm.rm_movev_canfd([0.0] * 6, False, 0, 0)
if self._command_mode == "velocity":
self._arm.rm_movev_canfd([0.0] * 6, False, 0, 0)
self._arm.rm_set_arm_slow_stop()
except Exception:
pass
@ -151,7 +188,8 @@ class RealManAdapter:
raise RuntimeError("睿尔曼机械臂尚未连接")
def _apply_safety_limits(self) -> None:
self._try_call("rm_set_avoid_singularity_mode", True)
# 真机安全限幅尽量下发到控制器；不支持的 SDK 接口会在 _try_call 中降级为警告。
self._try_call("rm_set_avoid_singularity_mode", int(self._avoid_singularity))
self._try_call("rm_set_arm_max_line_speed", self._max_line_speed)
self._try_call("rm_set_arm_max_angular_speed", self._max_angular_speed)
self._try_call("rm_set_arm_max_line_acc", self._max_line_acc)
@ -169,6 +207,46 @@ class RealManAdapter:
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 _init_movev_canfd(self) -> None:
attempts = 3
candidates = [self._avoid_singularity]
if self._avoid_singularity != 0:
candidates.append(0)
# 某些现场控制器初始化避奇异模式会超时，失败后自动降级到 0 再重试。
last_ret: Any = None
for avoid_singularity in candidates:
if avoid_singularity != self._avoid_singularity:
self._log_warn("rm_set_movev_canfd_init 降级为 avoid_singularity=0 后重试。")
for attempt in range(1, attempts + 1):
ret = self._arm.rm_set_movev_canfd_init(
avoid_singularity,
self._frame_type,
self._dt_ms,
)
last_ret = ret
code = self._return_code(ret)
if code == 0:
self._avoid_singularity = avoid_singularity
self._log_info(
"rm_set_movev_canfd_init 成功："
f"avoid_singularity={avoid_singularity}, "
f"frame_type={self._frame_type}, dt_ms={self._dt_ms}"
)
return
self._log_warn(
"rm_set_movev_canfd_init 失败："
f"avoid_singularity={avoid_singularity}, "
f"frame_type={self._frame_type}, dt_ms={self._dt_ms}, "
f"attempt={attempt}/{attempts}, ret={ret!r}"
)
if attempt < attempts:
time.sleep(0.2)
self._check_return(last_ret, "rm_set_movev_canfd_init")
def _try_call(self, name: str, *args: Any) -> None:
func = getattr(self._arm, name, None)
if func is None:
@ -185,18 +263,28 @@ class RealManAdapter:
if self._logger is not None:
self._logger.warn(message)
@staticmethod
def _check_robot_handle(handle: Any) -> None:
def _log_info(self, message: str) -> None:
if self._logger is not None:
self._logger.info(message)
def _check_robot_handle(self, handle: Any) -> None:
handle_id = getattr(handle, "id", None)
if handle_id == -1:
raise RuntimeError("rm_create_robot_arm failed: socket error or robot unreachable")
raise RuntimeError(
"rm_create_robot_arm failed: TCP may be reachable, but the RealMan "
f"controller did not return robot info from {self._robot_ip}:{self._robot_port}"
)
@staticmethod
def _check_return(ret: Any, name: str) -> None:
code = ret[0] if isinstance(ret, tuple) and ret else ret
code = RealManAdapter._return_code(ret)
if isinstance(code, int) and code != 0:
raise RuntimeError(f"{name} failed with code {code}: {ret!r}")
@staticmethod
def _return_code(ret: Any) -> Any:
return ret[0] if isinstance(ret, tuple) and ret else ret
@classmethod
def _find_pose(cls, obj: Any) -> list[float] | None:
# 不同 SDK 版本返回字段可能略有差异，因此递归查找常见 TCP 位姿字段。

49
xr_rm_teleop/xr_rm_teleop/single_arm_velocity_teleop.py
View File

@ -1,3 +1,9 @@
"""RM75 单臂 XR 相对位移遥操作节点。
节点订阅左/右手柄位姿，在 grip 按下时锁定手柄和 TCP 起点，把手柄相对位移
映射到机器人坐标系中的目标 TCP，并通过笛卡尔速度命令跟随该目标。
"""
from __future__ import annotations
import math
@ -60,6 +66,9 @@ class SingleArmVelocityTeleop(Node):
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("command_mode", "velocity")
self.declare_parameter("canfd_trajectory_mode", 2)
self.declare_parameter("canfd_radio", 0)
self.declare_parameter("debug_topic_prefix", "/xr_rm")
self._arm_name = str(self.get_parameter("arm_name").value)
@ -107,6 +116,7 @@ class SingleArmVelocityTeleop(Node):
self.get_logger().info(f"{self._arm_name} 速度遥操节点已启动，监听话题：{topic}")
def _make_adapter(self):
# mock 和真机共享同一控制逻辑，只在适配层切换运动命令的执行方式。
if self._bool_parameter("use_mock"):
return MockRealManAdapter(
[float(v) for v in self.get_parameter("mock_initial_pose").value],
@ -131,6 +141,9 @@ class SingleArmVelocityTeleop(Node):
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),
command_mode=str(self.get_parameter("command_mode").value),
canfd_trajectory_mode=int(self.get_parameter("canfd_trajectory_mode").value),
canfd_radio=int(self.get_parameter("canfd_radio").value),
)
def _on_controller(self, msg: XrController) -> None:
@ -206,7 +219,24 @@ class SingleArmVelocityTeleop(Node):
]
cartesian_velocity = self._filtered_velocity + [0.0, 0.0, 0.0]
self._publish_debug(robot_pose, target, cartesian_velocity)
self._send_cartesian_velocity(cartesian_velocity)
if self._adapter_uses_pose_targets():
step_target = [
robot_now[i] + cartesian_velocity[i] * self._dt
for i in range(3)
]
step_target = self._clamp_workspace(step_target)
self._send_cartesian_target(
ArmPose(
x=step_target[0],
y=step_target[1],
z=step_target[2],
rx=robot_pose.rx,
ry=robot_pose.ry,
rz=robot_pose.rz,
)
)
else:
self._send_cartesian_velocity(cartesian_velocity)
@staticmethod
def _controller_xyz(msg: XrController) -> list[float]:
@ -221,6 +251,7 @@ class SingleArmVelocityTeleop(Node):
]
def _clamp_workspace(self, target: list[float]) -> list[float]:
# 先做盒状工作空间限幅，再叠加底座圆柱半径限制。
clamped = [
_clamp(target[i], self._workspace_min[i], self._workspace_max[i])
for i in range(3)
@ -259,6 +290,8 @@ class SingleArmVelocityTeleop(Node):
def _safe_stop(self) -> None:
# 所有断连、松手、退出路径都走这里，确保发送零速度。
self._filtered_velocity = [0.0, 0.0, 0.0]
if self._adapter_uses_pose_targets():
return
self._send_cartesian_velocity([0.0] * 6)
def _send_cartesian_velocity(self, velocity: list[float]) -> None:
@ -271,6 +304,20 @@ class SingleArmVelocityTeleop(Node):
)
self._active = False
def _send_cartesian_target(self, pose: ArmPose) -> None:
try:
self._adapter.send_cartesian_target(pose, self._follow)
except Exception as exc:
self.get_logger().error(
f"{self._arm_name} 发送位姿透传命令失败：{exc}",
throttle_duration_sec=1.0,
)
self._active = False
def _adapter_uses_pose_targets(self) -> bool:
uses_pose_targets = getattr(self._adapter, "uses_pose_targets", None)
return bool(uses_pose_targets is not None and uses_pose_targets())
def _publish_debug(self, current_pose: ArmPose, target_xyz: list[float], velocity: list[float]) -> None:
stamp = self.get_clock().now().to_msg()
current_msg = self._pose_msg(stamp, current_pose)