feat: add peripherals control

xr_rm_teleop/package.xml

@@ -11,6 +11,7 @@
 
   <exec_depend>geometry_msgs</exec_depend>
   <exec_depend>rclpy</exec_depend>
+  <exec_depend>python3-yaml</exec_depend>
   <exec_depend>std_msgs</exec_depend>
   <exec_depend>xr_rm_interfaces</exec_depend>
 

xr_rm_teleop/xr_rm_teleop/fun_peripheral.py

@@ -0,0 +1,240 @@
+"""RealMan 机械臂末端外设配置工具。
+
+scissorgripper 参数约定：
+0：工具板 IO 控制的剪刀夹爪
+1：Modbus 控制的电动夹爪
+2：控制器 DO3/DO4 控制的小型剪刀夹爪
+
+当前 XR 工程只在真机连接阶段做外设初始化；主遥操作流程不调用开合命令。
+"""
+
+from __future__ import annotations
+
+import time
+from dataclasses import dataclass
+from pathlib import Path
+from typing import Any
+
+import yaml
+
+
+@dataclass(frozen=True)
+class PeripheralConfig:
+    scissorgripper: int
+    tools_in_ee: dict[str, list[list[float]]]
+    set_initial_tool_state: bool = False
+
+
+def load_peripheral_config(config_file: str, arm: str) -> PeripheralConfig:
+    """从 bringup YAML 读取指定左右臂的外设配置。"""
+    path = Path(config_file).expanduser()
+    if not path.is_file():
+        raise FileNotFoundError(f"外设配置文件不存在：{path}")
+
+    with path.open("r", encoding="utf-8") as stream:
+        data = yaml.safe_load(stream) or {}
+    if not isinstance(data, dict):
+        raise ValueError(f"外设配置文件格式错误：{path}")
+
+    tools_in_ee = _load_tools_in_ee(data.get("tools_in_ee"), path)
+    arm_configs = data.get("arms", {})
+    if not isinstance(arm_configs, dict) or arm not in arm_configs:
+        raise ValueError(f"{path} 中缺少 arms.{arm} 外设配置")
+
+    arm_config = arm_configs[arm]
+    if not isinstance(arm_config, dict):
+        raise ValueError(f"{path} 中 arms.{arm} 必须是映射")
+
+    scissorgripper = int(arm_config["scissorgripper"])
+    tool_count = len(tools_in_ee)
+    if not -tool_count <= scissorgripper < tool_count:
+        raise ValueError(
+            f"arms.{arm}.scissorgripper={scissorgripper} 超出工具列表范围"
+        )
+
+    set_initial_tool_state = _as_bool(
+        arm_config.get(
+            "set_initial_tool_state",
+            data.get("set_initial_tool_state", False),
+        )
+    )
+    return PeripheralConfig(scissorgripper, tools_in_ee, set_initial_tool_state)
+
+
+def _load_tools_in_ee(raw_tools: Any, path: Path) -> dict[str, list[list[float]]]:
+    if not isinstance(raw_tools, dict) or not raw_tools:
+        raise ValueError(f"{path} 中缺少 tools_in_ee 配置")
+
+    tools_in_ee: dict[str, list[list[float]]] = {}
+    for tool_name, tool_config in raw_tools.items():
+        if not isinstance(tool_config, dict):
+            raise ValueError(f"tools_in_ee.{tool_name} 必须是映射")
+        pose = _float_list(tool_config.get("pose"), 7, f"tools_in_ee.{tool_name}.pose")
+        load = _float_list(tool_config.get("load"), 7, f"tools_in_ee.{tool_name}.load")
+        tools_in_ee[str(tool_name)] = [pose, load]
+    return tools_in_ee
+
+
+def _float_list(value: Any, expected_length: int, name: str) -> list[float]:
+    if not isinstance(value, (list, tuple)) or len(value) != expected_length:
+        raise ValueError(f"{name} 必须包含 {expected_length} 个数值")
+    return [float(item) for item in value]
+
+
+def _as_bool(value: Any) -> bool:
+    if isinstance(value, bool):
+        return value
+    if isinstance(value, str):
+        return value.strip().lower() in ("1", "true", "yes", "on")
+    return bool(value)
+
+
+def _tool_name_for_index(tools_in_ee: dict[str, list[list[float]]], scissorgripper: int) -> str:
+    return list(tools_in_ee.keys())[scissorgripper]
+
+
+def cal_tool_frame(handle, scissorgripper, tools_in_ee):
+    """根据末端工具配置生成 RealMan 工具坐标系。"""
+    from Robotic_Arm.rm_robot_interface import rm_frame_t
+
+    tool_name = _tool_name_for_index(tools_in_ee, scissorgripper)
+    tool_pose = tools_in_ee[tool_name][0]
+    tool_load = tools_in_ee[tool_name][1]
+    eu = handle.rm_algo_quaternion2euler([tool_pose[6], tool_pose[3], tool_pose[4], tool_pose[5]])
+    tool_frame = rm_frame_t(tool_name, [tool_pose[0], tool_pose[1], tool_pose[2], eu[0], eu[1], eu[2]], tool_load[0],
+                            tool_load[1], tool_load[2], tool_load[3])
+    return tool_frame, tool_name
+
+
+def alg_init(scissorgripper, tools_in_ee):
+    """初始化算法模型、工具坐标系和关节运动限制。"""
+    from Robotic_Arm.rm_robot_interface import Algo, rm_force_type_e, rm_robot_arm_model_e
+
+    arm_model = rm_robot_arm_model_e.RM_MODEL_RM_75_E
+    force_type = rm_force_type_e.RM_MODEL_RM_B_E
+    algo_handle = Algo(arm_model, force_type)
+    tool_frame, tool_name = cal_tool_frame(algo_handle, scissorgripper, tools_in_ee)
+    algo_handle.rm_algo_set_toolframe(tool_frame)
+
+    algo_handle.rm_algo_set_joint_max_limit([150.0, 110.0, 170.0, 130, 175.0, 125.0, 179.0])
+    algo_handle.rm_algo_set_joint_min_limit([-150.0, -30.0, -170.0, -130, -175.0, -125.0, -179.0])
+    algo_handle.rm_algo_set_joint_max_speed([60.0, 60.0, 60.0, 60.0, 60.0, 60.0, 60.0])
+    algo_handle.rm_algo_set_joint_max_acc([
+        166.6666717529297,
+        166.6666717529297,
+        166.6666717529297,
+        166.6666717529297,
+        166.6666717529297,
+        166.6666717529297,
+        166.6666717529297,
+    ])
+    algo_handle.rm_algo_set_redundant_parameter_traversal_mode(False)
+
+    return algo_handle
+
+
+def peripheral_cfg(
+    robot,
+    scissorgripper,
+    tools_in_ee,
+    *,
+    set_initial_tool_state: bool = False,
+):
+    """配置机械臂控制器、当前工具坐标系和末端工具通信方式。"""
+    # 控制器输出 12V；IO1/IO2 配置为输入，IO3/IO4 配置为输出。
+    robot.rm_set_voltage(2, True)
+    time.sleep(0.2)
+    robot.rm_set_io_mode(1, 0, 50, 2)
+    robot.rm_set_io_mode(2, 0, 50, 2)
+    robot.rm_set_io_mode(3, 1, 50, 2)
+    robot.rm_set_io_mode(4, 1, 50, 2)
+
+    time.sleep(0.2)
+    tool_frame, tool_name = cal_tool_frame(robot, scissorgripper, tools_in_ee)
+    robot.rm_set_manual_tool_frame(frame=tool_frame)
+    robot.rm_change_tool_frame(tool_name)
+
+    if scissorgripper == 0:
+        # 剪刀夹爪通过工具板数字输出控制。
+        robot.rm_set_tool_voltage(voltage_type=2)
+        time.sleep(0.2)
+        robot.rm_set_tool_IO_mode(1, 1)
+        robot.rm_set_tool_IO_mode(2, 1)
+
+        if set_initial_tool_state:
+            robot.rm_set_tool_do_state(1, 0)
+            robot.rm_set_tool_do_state(2, 1)
+    elif scissorgripper == 1:
+        # 电动夹爪通过工具板 Modbus 寄存器控制。
+        from Robotic_Arm.rm_robot_interface import rm_peripheral_read_write_params_t
+
+        modbus_sts = robot.rm_set_modbus_mode(port=1, baudrate=115200, timeout=2)
+        if modbus_sts != 0:
+            print(f"警告：Modbus 模式设置失败：{modbus_sts}")
+        else:
+            print("Modbus 模式配置成功")
+
+        addr = 1
+        # 依次设置目标速度、目标力矩、目标加速度和目标减速度。
+        reg_value = [255, 60, 255, 255]
+        for i, reg_addr in enumerate([11, 12, 13, 14]):
+            write_params = rm_peripheral_read_write_params_t(1, reg_addr, addr, 1)
+            robot.rm_write_single_register(write_params, reg_value[i])
+            time.sleep(0.5)
+
+        if set_initial_tool_state:
+            set_tool_position(robot, percent=0.75, device=1, scissorgripper=scissorgripper)
+            time.sleep(1.5)
+            set_tool_position(robot, percent=0.15, device=1, scissorgripper=scissorgripper)
+
+    elif scissorgripper == 2:
+        # 小型剪刀夹爪通过控制器 DO3/DO4 控制。
+        if set_initial_tool_state:
+            robot.rm_set_do_state(io_num=3, state=1)
+            robot.rm_set_do_state(io_num=4, state=0)
+            time.sleep(2)
+            robot.rm_set_do_state(io_num=3, state=0)
+            robot.rm_set_do_state(io_num=4, state=1)
+
+    return robot
+
+
+def set_tool_position(robot, percent, device=1, scissorgripper=0):
+    """设置末端工具开合位置：0.0 表示闭合，1.0 表示完全张开。"""
+    if scissorgripper == 1:
+        from Robotic_Arm.rm_robot_interface import rm_peripheral_read_write_params_t
+
+        pos_value = int(percent * 255)
+        write_params = rm_peripheral_read_write_params_t(1, 10, device, 1)
+        robot.rm_write_single_register(write_params, pos_value)
+        # 写触发寄存器后夹爪才会执行目标位置。
+        write_params = rm_peripheral_read_write_params_t(1, 15, device, 1)
+        time.sleep(0.5)
+        robot.rm_write_single_register(write_params, 0x01)
+        time.sleep(0.5)
+
+    elif scissorgripper == 0:
+        # 剪刀夹爪只有全闭合和全张开两个状态。
+        if percent == 0:
+            robot.rm_set_tool_do_state(1, 1)
+            robot.rm_set_tool_do_state(2, 0)
+        elif percent == 1:
+            robot.rm_set_tool_do_state(1, 0)
+            robot.rm_set_tool_do_state(2, 1)
+    elif scissorgripper == 2:
+        # 小型剪刀夹爪同样只有全闭合和全张开两个状态。
+        if percent == 0:
+            robot.rm_set_do_state(3, 1)
+            robot.rm_set_do_state(4, 0)
+        elif percent == 1:
+            robot.rm_set_do_state(3, 0)
+            robot.rm_set_do_state(4, 1)
+
+
+def tool_exe(robot, scissorgripper, cmd):
+    """根据布尔命令执行末端工具开合。"""
+    switch_state = cmd
+    if switch_state:
+        set_tool_position(robot, 1, device=1, scissorgripper=scissorgripper)
+    else:
+        set_tool_position(robot, 0, device=1, scissorgripper=scissorgripper)

xr_rm_teleop/xr_rm_teleop/realman_adapter.py

@@ -32,6 +32,7 @@ class MockRealManAdapter:
         self._pose = ArmPose(*initial_pose[:6])
         self._dt = dt
         self.last_velocity = [0.0] * 6
+        self.last_tool_open: bool | None = None
 
     def connect(self) -> None:
         return
@@ -56,6 +57,12 @@ class MockRealManAdapter:
     def close(self) -> None:
         self.stop()
 
+    def configure_peripheral(self, config_file: str, peripheral_arm: str) -> None:
+        del config_file, peripheral_arm
+
+    def set_tool_enabled(self, open_tool: bool) -> None:
+        self.last_tool_open = open_tool
+
 
 class RealManAdapter:
     """睿尔曼 Python API2 的笛卡尔速度透传适配层。"""
@@ -103,6 +110,7 @@ class RealManAdapter:
         self._command_mode = command_mode
         self._canfd_trajectory_mode = canfd_trajectory_mode
         self._canfd_radio = canfd_radio
+        self._scissorgripper: int | None = None
         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")
@@ -164,6 +172,35 @@ class RealManAdapter:
     def uses_pose_targets(self) -> bool:
         return self._command_mode == "pose_canfd"
 
+    def configure_peripheral(self, config_file: str, peripheral_arm: str) -> None:
+        self._require_arm()
+        from .fun_peripheral import load_peripheral_config, peripheral_cfg
+
+        config = load_peripheral_config(config_file, peripheral_arm)
+        self._scissorgripper = config.scissorgripper
+        tool_name = list(config.tools_in_ee.keys())[config.scissorgripper]
+        self._log_info(
+            "开始配置 RealMan 末端外设："
+            f"arm={peripheral_arm}, scissorgripper={config.scissorgripper}, "
+            f"tool={tool_name}, set_initial_tool_state={config.set_initial_tool_state}"
+        )
+        peripheral_cfg(
+            self._arm,
+            config.scissorgripper,
+            config.tools_in_ee,
+            set_initial_tool_state=config.set_initial_tool_state,
+        )
+        self._log_info(f"RealMan 末端外设配置完成：arm={peripheral_arm}, tool={tool_name}")
+
+    def set_tool_enabled(self, open_tool: bool) -> None:
+        self._require_arm()
+        if self._scissorgripper is None:
+            raise RuntimeError("末端外设尚未初始化，无法执行开合命令")
+
+        from .fun_peripheral import tool_exe
+
+        tool_exe(self._arm, self._scissorgripper, open_tool)
+
     def stop(self) -> None:
         if self._arm is None:
             return

xr_rm_teleop/xr_rm_teleop/single_arm_velocity_teleop.py

@@ -12,6 +12,7 @@ from typing import Iterable
 import rclpy
 from geometry_msgs.msg import PoseStamped, TwistStamped
 from rclpy.node import Node
+from std_msgs.msg import Bool
 
 from xr_rm_interfaces.msg import XrController
 
@@ -70,6 +71,11 @@ class SingleArmVelocityTeleop(Node):
         self.declare_parameter("command_mode", "velocity")
         self.declare_parameter("canfd_trajectory_mode", 2)
         self.declare_parameter("canfd_radio", 0)
+        self.declare_parameter("enable_tool_control", True)
+        self.declare_parameter("configure_peripheral_on_connect", True)
+        self.declare_parameter("peripheral_config_file", "")
+        self.declare_parameter("peripheral_arm", "")
+        self.declare_parameter("tool_command_topic", "")
         self.declare_parameter("debug_topic_prefix", "/xr_rm")
 
         self._arm_name = str(self.get_parameter("arm_name").value)
@@ -107,6 +113,7 @@ class SingleArmVelocityTeleop(Node):
 
         self._adapter = self._make_adapter()
         self._adapter.connect()
+        self._setup_tool_control()
 
         debug_ns = f"{self._debug_topic_prefix}/{self._arm_name}"
         self._current_pose_pub = self.create_publisher(PoseStamped, f"{debug_ns}/current_pose", 10)
@@ -148,6 +155,42 @@ class SingleArmVelocityTeleop(Node):
             canfd_radio=int(self.get_parameter("canfd_radio").value),
         )
 
+    def _setup_tool_control(self) -> None:
+        if not self._bool_parameter("enable_tool_control"):
+            return
+
+        peripheral_arm = self._peripheral_arm_name()
+        config_file = str(self.get_parameter("peripheral_config_file").value)
+        if self._bool_parameter("configure_peripheral_on_connect"):
+            self._adapter.configure_peripheral(config_file, peripheral_arm)
+
+        topic = str(self.get_parameter("tool_command_topic").value).strip()
+        if not topic:
+            topic = f"{self._debug_topic_prefix}/{self._arm_name}/tool_enable"
+        self.create_subscription(Bool, topic, self._on_tool_command, 10)
+        self.get_logger().info(f"{self._arm_name} 工具控制已启用，监听：{topic} (true=open, false=close)")
+
+    def _on_tool_command(self, msg: Bool) -> None:
+        open_tool = bool(msg.data)
+        action = "open" if open_tool else "close"
+        try:
+            self._adapter.set_tool_enabled(open_tool)
+        except Exception as exc:
+            self.get_logger().error(f"{self._arm_name} tool {action} failed: {exc}")
+            return
+        self.get_logger().info(f"{self._arm_name} tool {action} command sent")
+
+    def _peripheral_arm_name(self) -> str:
+        configured = str(self.get_parameter("peripheral_arm").value).strip().lower()
+        if configured:
+            return configured
+        arm_name = self._arm_name.lower()
+        if arm_name.startswith("left"):
+            return "left"
+        if arm_name.startswith("right"):
+            return "right"
+        return arm_name
+
     def _on_controller(self, msg: XrController) -> None:
         self._last_msg = msg
         self._last_msg_time = self.get_clock().now()