diff --git a/src/linker_hand_ros2_sdk/launch/test.bak b/src/linker_hand_ros2_sdk/launch/test.bak deleted file mode 100644 index a4d8d10..0000000 --- a/src/linker_hand_ros2_sdk/launch/test.bak +++ /dev/null @@ -1,31 +0,0 @@ -from launch import LaunchDescription -from launch_ros.actions import Node - -def generate_launch_description(): - return LaunchDescription([ - Node( - package='linker_hand_ros2_sdk', - executable='linker_hand_sdk', - name='linker_hand_sdk_left', - output='screen', - parameters=[{ - 'hand_type': 'left', - 'hand_joint': "L10", # 这里需要修改为实际Linker Hand的型号 L7、L10、L20、L21、L25 - 'is_touch': True, # 是否带有压力传感器 - 'can': 'can0', # 这里需要修改为实际的CAN总线名称 - }], - ), - - Node( - package='linker_hand_ros2_sdk', - executable='linker_hand_sdk', - name='linker_hand_sdk_right', - output='screen', - parameters=[{ - 'hand_type': 'right', - 'hand_joint': "L10", # 这里需要修改为实际Linker Hand的型号 L7、L10、L20、L21、L25 - 'is_touch': True, # 是否带有压力传感器 - 'can': 'can0', # 这里需要修改为实际的CAN总线名称 - }], - ), - ]) diff --git a/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/core/rs485/linker_hand_l6_rs485.py.bak b/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/core/rs485/linker_hand_l6_rs485.py.bak deleted file mode 100644 index d68b621..0000000 --- a/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/core/rs485/linker_hand_l6_rs485.py.bak +++ /dev/null @@ -1,444 +0,0 @@ -#!/usr/bin/env python3 -import os -import time -from pymodbus.client import ModbusSerialClient -from typing import List, Dict -import numpy as np - -_INTERVAL = 0.006 # 8 ms - -class LinkerHandL6RS485: - """L6机械手 Modbus-RTU 控制类""" - - # 6个关节名称 - JOINT_NAMES = ["thumb_pitch", "thumb_yaw", "index_pitch", - "middle_pitch", "ring_pitch", "little_pitch"] - - # 手指名称 - FINGER_NAMES = ["thumb", "index", "middle", "ring", "little"] - - def __init__(self, hand_id=0x27, modbus_port="/dev/ttyUSB0", baudrate=115200): - """ - 初始化L6机械手 - hand_id: 右手0x27(39), 左手0x28(40) - modbus_port: 串口设备路径 - baudrate: 波特率，固定115200 - """ - self.slave = hand_id - self.cli = ModbusSerialClient( - port=modbus_port, - baudrate=baudrate, - bytesize=8, - parity="N", - stopbits=1, - timeout=0.05 - ) - # pymodbus 3.5.1 需要显式连接 - self.connected = self.cli.connect() - if not self.connected: - raise ConnectionError(f"RS485连接失败，端口: {modbus_port}") - - def _read_input_registers(self, address: int, count: int) -> List[int]: - """读取输入寄存器""" - time.sleep(_INTERVAL) - result = self.cli.read_input_registers(address=address, count=count, slave=self.slave) - if result.isError(): - raise RuntimeError(f"读取输入寄存器失败: address={address}, count={count}") - return result.registers - - def _write_register(self, address: int, value: int): - """写入单个寄存器""" - time.sleep(_INTERVAL) - result = self.cli.write_register(address=address, value=value, slave=self.slave) - if result.isError(): - raise RuntimeError(f"写入寄存器失败: address={address}, value={value}") - - def _write_registers(self, address: int, values: List[int]): - """写入多个寄存器""" - time.sleep(_INTERVAL) - result = self.cli.write_registers(address=address, values=values, slave=self.slave) - if result.isError(): - raise RuntimeError(f"写入多个寄存器失败: address={address}, values={values}") - - # -------------------------------------------------- - # 基础读取接口 - # -------------------------------------------------- - - def read_angles(self) -> List[int]: - """读取6个关节角度 (输入寄存器 0-5)""" - return self._read_input_registers(0, 6) - - def read_torques(self) -> List[int]: - """读取6个关节转矩 (输入寄存器 6-11)""" - return self._read_input_registers(6, 6) - - def read_speeds(self) -> List[int]: - """读取6个关节速度 (输入寄存器 12-17)""" - return self._read_input_registers(12, 6) - - def read_temperatures(self) -> List[int]: - """读取6个关节温度 (输入寄存器 18-23)""" - return self._read_input_registers(18, 6) - - def read_error_codes(self) -> List[int]: - """读取6个关节错误码 (输入寄存器 24-29)""" - return self._read_input_registers(24, 6) - - # -------------------------------------------------- - # 压力传感器接口 - # -------------------------------------------------- - - # def _pressure(self, finger: int) -> List[int]: - # """内部：选手指 → 读压力数据""" - # # 选择手指 (保持寄存器 36) - # self._write_register(36, finger) - # time.sleep(_INTERVAL) - # # 读取压力数据 (输入寄存器 52-122) - # return np.array(self._read_input_registers(52, 71)) - def _pressure(self, finger: int) -> np.ndarray: - """ - 6x12 (72点) 矩阵尺寸。 - Modbus 地址 60/62。 - """ - rows = 12 # 12 行 - cols = 6 # 6 列 - finger_size = rows * cols # 72 个数据点 - - # modbus 地址和计数 - write_address = 60 # 写入手指选择 - read_address = 62 # 读取压力数据 - read_count = 96 # 读取 96 个寄存器 - skip_count = 10 # 跳过前 10 个校验点 - - # 0. 参数校验和手指写入值确定 - if finger < 1 or finger > 5: - raise ValueError(f"无效的手指编号: {finger}。手指编号应在 1 到 5 之间。") - - finger_write_value = finger - - # 1. 写入手指选择寄存器 (地址 60) - time.sleep(0.008) - wrsp = self.cli.write_register(address=write_address, value=finger_write_value, slave=self.slave) - if wrsp.isError(): - raise RuntimeError(f"写入手指选择 {finger} 到地址 {write_address} 失败: {wrsp}") - - # 写入后等待片刻 - time.sleep(0.008) - - # 2. 读取地址 62 的数据 - rrsp = self.cli.read_input_registers(address=read_address, count=read_count, slave=self.slave) - - if rrsp.isError(): - raise RuntimeError(f"读取地址 {read_address} 压力数据失败: {rrsp}") - - registers_16bit: List[int] = rrsp.registers - - # 3. 核心数据处理 - # a. 提取低 8 位数据 (得到 96 个 8 位数据点) - final_data_96 = [reg_value & 255 for reg_value in registers_16bit] - - # b. 跳过前 10 个校验/头部数据点 (得到 86 个有效数据点) - effective_data = np.array(final_data_96[skip_count:], dtype=np.uint8) - # c. 截取当前手指的矩阵数据 (从 86 个有效点中截取 72 个点) - start_idx = 0 - end_idx = finger_size # 72 - - finger_data_flat = effective_data[start_idx:end_idx] - - # d. 验证数据长度 - if finger_data_flat.size != finger_size: - raise ValueError( - f"数据提取失败。期望 {finger_size} 点 ({rows}x{cols})，" - f"但仅截取到 {finger_data_flat.size} 点。请检查协议，确认地址 62 是否一次性返回了所有手指数据。" - ) - - # e. 重塑为二维矩阵 (12 行 6 列) - finger_matrix = finger_data_flat.reshape((rows, cols)) - - return finger_matrix - - def read_pressure_thumb(self) -> np.ndarray: - """读取大拇指压力数据""" - return np.array(self._pressure(1), dtype=np.uint8) - - def read_pressure_index(self) -> np.ndarray: - """读取食指压力数据""" - return np.array(self._pressure(2), dtype=np.uint8) - - def read_pressure_middle(self) -> np.ndarray: - """读取中指压力数据""" - return np.array(self._pressure(3), dtype=np.uint8) - - def read_pressure_ring(self) -> np.ndarray: - """读取无名指压力数据""" - return np.array(self._pressure(4), dtype=np.uint8) - - def read_pressure_little(self) -> np.ndarray: - """读取小拇指压力数据""" - return np.array(self._pressure(5), dtype=np.uint8) - - # -------------------------------------------------- - # 版本信息接口 - # -------------------------------------------------- - - def read_versions(self) -> Dict[str, int]: - """读取版本信息 (输入寄存器 148-155)""" - result = self._read_input_registers(148, 8) - - return { - "hand_freedom": result[0], - "hand_version": result[1], - "hand_number": result[2], - "hand_direction": result[3], - "software_version_major": result[4], - "software_version_minor": result[5] if len(result) > 5 else 0, - "software_version_revision": result[6] if len(result) > 6 else 0, - "hardware_version": result[7] if len(result) > 7 else 0 - } - - # -------------------------------------------------- - # 写入接口 - # -------------------------------------------------- - - def write_angles(self, vals: List[int]): - """设置6个关节角度 (保持寄存器 0-5)""" - vals = [int(x) for x in vals] - if not self.is_valid_6xuint8(vals): - raise ValueError("需要6个0-255的整数") - self._write_registers(0, vals) - - def write_torques(self, vals: List[int]): - """设置6个关节转矩 (保持寄存器 6-11)""" - vals = [int(x) for x in vals] - if not self.is_valid_6xuint8(vals): - raise ValueError("需要6个0-255的整数") - self._write_registers(6, vals) - - def write_speeds(self, vals: List[int]): - """设置6个关节速度 (保持寄存器 12-17)""" - vals = [int(x) for x in vals] - if not self.is_valid_6xuint8(vals): - raise ValueError("需要6个0-255的整数") - self._write_registers(12, vals) - - # -------------------------------------------------- - # 上下文管理 - # -------------------------------------------------- - - def close(self): - """关闭连接""" - if self.connected: - self.cli.close() - self.connected = False - - def __enter__(self): - return self - - def __exit__(self, exc_type, exc_val, exc_tb): - self.close() - - # -------------------------------------------------- - # API固定接口函数 - # -------------------------------------------------- - - def is_valid_6xuint8(self, lst) -> bool: - """验证6个0-255的整数列表""" - if len(lst) != 6: - return False - return all(isinstance(x, int) and 0 <= x <= 255 for x in lst) - - def set_joint_positions(self, joint_angles=None): - """设置关节位置""" - joint_angles = joint_angles or [0] * 6 - self.write_angles(joint_angles) - - def set_speed(self, speed=None): - """设置速度""" - speed = speed or [200] * 6 - self.write_speeds(speed) - - def set_torque(self, torque=None): - """设置扭矩""" - torque = torque or [200] * 6 - self.write_torques(torque) - - def set_current(self, current=None): - """设置电流 (L6不支持)""" - print("当前L6不支持设置电流", flush=True) - - def get_version(self) -> list: - """获取版本信息""" - versions = self.read_versions() - return [ - versions.get("hand_freedom", 0), - versions.get("hand_version", 0), - versions.get("hand_number", 0), - versions.get("hand_direction", 0), - versions.get("software_version_major", 0), - versions.get("hardware_version", 0) - ] - - def get_current(self): - """获取电流 (L6不支持)""" - print("当前L6不支持获取电流", flush=True) - return [] - - def get_state(self) -> list: - """获取关节状态""" - return self.read_angles() - - def get_state_for_pub(self) -> list: - return self.get_state() - - def get_current_status(self) -> list: - return self.get_state() - - def get_speed(self) -> list: - """获取当前速度""" - return self.read_speeds() - - def get_joint_speed(self) -> list: - return self.get_speed() - - def get_touch_type(self) -> int: - """获取压感类型 (2=矩阵式)""" - return 2 - - def get_normal_force(self) -> list: - """获取压感数据：点式""" - return [-1] * 5 - - def get_tangential_force(self) -> list: - """获取压感数据：点式""" - return [-1] * 5 - - def get_approach_inc(self) -> list: - """获取压感数据：点式""" - return [-1] * 5 - - def get_touch(self) -> list: - return [-1] * 5 - - def get_thumb_matrix_touch(self,sleep_time=0): - return self._pressure(1) - - def get_index_matrix_touch(self,sleep_time=0): - return self._pressure(2) - - def get_middle_matrix_touch(self,sleep_time=0): - return self._pressure(3) - - def get_ring_matrix_touch(self,sleep_time=0): - return self._pressure(4) - - def get_little_matrix_touch(self,sleep_time=0): - return self._pressure(5) - - def get_matrix_touch(self) -> list: - """获取压感数据：矩阵式""" - return [self._pressure(1), self._pressure(2), self._pressure(3), - self._pressure(4), self._pressure(5)] - - def get_matrix_touch_v2(self) -> list: - """获取压感数据：矩阵式""" - return self.get_matrix_touch() - - def get_torque(self) -> list: - """获取当前扭矩""" - return self.read_torques() - - def get_temperature(self) -> list: - """获取当前电机温度""" - return self.read_temperatures() - - def get_fault(self) -> list: - """获取当前电机故障码""" - return self.read_error_codes() - - def get_serial_number(self): - return [0] * 6 - - # -------------------------------------------------- - # 便捷方法 - # -------------------------------------------------- - - def relax(self): - """所有手指伸直""" - self.set_joint_positions([255] * 6) - - def fist(self): - """所有手指握拳""" - self.set_joint_positions([0] * 6) - - def dump_status(self): - """打印状态信息""" - print("=" * 50) - print("L6机械手状态信息") - print("=" * 50) - - try: - # 关节状态 - angles = self.read_angles() - torques = self.read_torques() - speeds = self.read_speeds() - temps = self.read_temperatures() - errors = self.read_error_codes() - - print("关节状态:") - for i, name in enumerate(self.JOINT_NAMES): - print(f" {name:15s}: 角度={angles[i]:3d}, 扭矩={torques[i]:3d}, " - f"速度={speeds[i]:3d}, 温度={temps[i]:2d}℃, 错误={errors[i]:2d}") - - # 版本信息 - versions = self.read_versions() - print("\n版本信息:") - for key, value in versions.items(): - print(f" {key:20s}: {value}") - - # 压力传感器测试 - print("\n压力传感器测试:") - thumb_pressure = self.read_pressure_thumb() - print(f"大拇指压力数据长度: {len(thumb_pressure)}") - - except Exception as e: - print(f"读取状态时出错: {e}") - - print("=" * 50) - - -# ------------------- 演示程序 ------------------- -if __name__ == "__main__": - # 使用示例 - try: - with LinkerHandL6RS485(hand_id=0x27, modbus_port="/dev/ttyUSB0", baudrate=115200) as hand: - print("连接成功!") - - # 打印状态信息 - hand.dump_status() - - # 测试基本控制 - print("\n测试控制功能...") - print("伸直手指...") - hand.relax() - time.sleep(2) - - print("握拳...") - hand.fist() - time.sleep(2) - - print("恢复伸直...") - hand.relax() - - # 测试压力传感器 - print("\n测试压力传感器...") - thumb_matrix = hand.get_thumb_matrix_touch() - print(f"大拇指压力数据: {len(thumb_matrix)}个点") - - # 获取所有手指压力数据 - all_matrices = hand.get_matrix_touch() - for i, name in enumerate(hand.FINGER_NAMES): - matrix = all_matrices[i] - print(f"{name}手指压力数据长度: {len(matrix)}") - - except Exception as e: - print(f"错误: {e}") \ No newline at end of file diff --git a/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/core/rs485/linker_hand_o6_rs485.py.bak b/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/core/rs485/linker_hand_o6_rs485.py.bak deleted file mode 100644 index cb08dd9..0000000 --- a/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/core/rs485/linker_hand_o6_rs485.py.bak +++ /dev/null @@ -1,1157 +0,0 @@ -#!/usr/bin/env python3 -""" -O6 机械手 Modbus-RTU 控制类 (基于 pymodbus 3.5.1) -""" - -import os -import time -from typing import List, Dict, Any # 引入 Any 来表示灵活的输入类型 -import numpy as np -import logging -from threading import Lock # 用于线程安全和总线仲裁 - -# 导入 pymodbus 客户端 -from pymodbus.client import ModbusSerialClient -from struct import error as StructError - -logging.basicConfig( - level=logging.INFO, - format="[%(asctime)s] %(levelname)-8s %(message)s", - datefmt="%H:%M:%S" -) - -# ------------------------------------------------------------------ -# 读输入寄存器地址枚举（功能码 04，只读）- 保持原样 -# ------------------------------------------------------------------ -REG_RD_CURRENT_THUMB_PITCH = 0 # 大拇指弯曲角度（0-255，小=弯，大=伸） -REG_RD_CURRENT_THUMB_YAW = 1 # 大拇指横摆角度（0-255，小=靠掌心，大=远离） -REG_RD_CURRENT_INDEX_PITCH = 2 # 食指弯曲角度 -REG_RD_CURRENT_MIDDLE_PITCH = 3 # 中指弯曲角度 -REG_RD_CURRENT_RING_PITCH = 4 # 无名指弯曲角度 -REG_RD_CURRENT_LITTLE_PITCH = 5 # 小拇指弯曲角度 -REG_RD_CURRENT_THUMB_TORQUE = 6 # 大拇指弯曲转矩（0-255） -REG_RD_CURRENT_THUMB_YAW_TORQUE = 7 # 大拇指横摆转矩 -REG_RD_CURRENT_INDEX_TORQUE = 8 # 食指转矩 -REG_RD_CURRENT_MIDDLE_TORQUE = 9 # 中指转矩 -REG_RD_CURRENT_RING_TORQUE = 10 # 无名指转矩 -REG_RD_CURRENT_LITTLE_TORQUE = 11 # 小拇指转矩 -REG_RD_CURRENT_THUMB_SPEED = 12 # 大拇指弯曲速度（0-255） -REG_RD_CURRENT_THUMB_YAW_SPEED = 13 # 大拇指横摆速度 -REG_RD_CURRENT_INDEX_SPEED = 14 # 食指速度 -REG_RD_CURRENT_MIDDLE_SPEED = 15 # 中指速度 -REG_RD_CURRENT_RING_SPEED = 16 # 无名指速度 -REG_RD_CURRENT_LITTLE_SPEED = 17 # 小拇指速度 -REG_RD_THUMB_TEMP = 18 # 大拇指弯曲温度（0-70℃） -REG_RD_THUMB_YAW_TEMP = 19 # 大拇指横摆温度 -REG_RD_INDEX_TEMP = 20 # 食指温度 -REG_RD_MIDDLE_TEMP = 21 # 中指温度 -REG_RD_RING_TEMP = 22 # 无名指温度 -REG_RD_LITTLE_TEMP = 23 # 小拇指温度 -REG_RD_THUMB_ERROR = 24 # 大拇指错误码 -REG_RD_THUMB_YAW_ERROR = 25 # 大拇指横摆错误码 -REG_RD_INDEX_ERROR = 26 # 食指错误码 -REG_RD_MIDDLE_ERROR = 27 # 中指错误码 -REG_RD_RING_ERROR = 28 # 无名指错误码 -REG_RD_LITTLE_ERROR = 29 # 小拇指错误码 -REG_RD_HAND_FREEDOM = 30 # 版本号（与机械手标签相同） -REG_RD_HAND_VERSION = 31 # 手版本 -REG_RD_HAND_NUMBER = 32 # 手编号 -REG_RD_HAND_DIRECTION = 33 # 手方向（左/右） -REG_RD_SOFTWARE_VERSION = 34 # 软件版本 -REG_RD_HARDWARE_VERSION = 35 # 硬件版本 - - -# ------------------------------------------------------------------ -# 写保持寄存器地址枚举（功能码 16，读写）- 保持原样 -# ------------------------------------------------------------------ -REG_WR_THUMB_PITCH = 0 # 大拇指弯曲角度（0-255） -REG_WR_THUMB_YAW = 1 # 大拇指横摆角度 -REG_WR_INDEX_PITCH = 2 # 食指弯曲角度 -REG_WR_MIDDLE_PITCH = 3 # 中指弯曲角度 -REG_WR_RING_PITCH = 4 # 无名指弯曲角度 -REG_WR_LITTLE_PITCH = 5 # 小拇指弯曲角度 -REG_WR_THUMB_TORQUE = 6 # 大拇指弯曲转矩 -REG_WR_THUMB_YAW_TORQUE = 7 # 大拇指横摆转矩 -REG_WR_INDEX_TORQUE = 8 # 食指转矩 -REG_WR_MIDDLE_TORQUE = 9 # 中指转矩 -REG_WR_RING_TORQUE = 10 # 无名指转矩 -REG_WR_LITTLE_TORQUE = 11 # 小拇指转矩 -REG_WR_THUMB_SPEED = 12 # 大拇指弯曲速度 -REG_WR_THUMB_YAW_SPEED = 13 # 大拇指横摆速度 -REG_WR_INDEX_SPEED = 14 # 食指速度 -REG_WR_MIDDLE_SPEED = 15 # 中指速度 -REG_WR_RING_SPEED = 16 # 无名指速度 -REG_WR_LITTLE_SPEED = 17 # 小拇指速度 - - -class LinkerHandO6RS485: - """O6 机械手 Modbus-RTU 控制类，使用 pymodbus 3.5.1""" - - TTL_TIMEOUT = 0.15 # 串口超时 - FRAME_GAP = 0.030 # 30 ms - - # KEYS for easy indexing - JOINT_KEYS = ["thumb_pitch", "thumb_yaw", "index_pitch", - "middle_pitch", "ring_pitch", "little_pitch"] - - def __init__(self, hand_id=0x27, modbus_port="/dev/ttyUSB0", baudrate=115200): - self._id = hand_id - self._last_ts = 0.0 # 上一次帧结束时间 - self._lock = Lock() # 总线访问锁 - - # 使用 pymodbus 3.x 客户端 - self.cli = ModbusSerialClient( - port=modbus_port, - baudrate=baudrate, - bytesize=8, - parity="N", - stopbits=1, - timeout=self.TTL_TIMEOUT, - handle_local_echo=False - ) - - try: - logging.info(f"Connecting to Modbus RTU on {modbus_port}...") - self.connected = self.cli.connect() - if not self.connected: - raise ConnectionError(f"RS485 connect fail to {modbus_port}") - logging.info("Connection successful.") - except Exception as e: - logging.error(f"Initialization failed: {e}") - raise - - # ---------------------------------------------------------- - # 辅助方法 - # ---------------------------------------------------------- - def _bus_free(self): - """保证距离上一帧 ≥ 30 ms""" - with self._lock: - elapse = time.perf_counter() - self._last_ts - if elapse < self.FRAME_GAP: - time.sleep(self.FRAME_GAP - elapse) - - def _execute_read(self, address: int, count: int) -> List[int]: - """执行 Modbus 读取操作 (功能码 04), 带总线仲裁。""" - self._bus_free() - - rsp = self.cli.read_input_registers( - address=address, - count=count, - slave=self._id - ) - - self._last_ts = time.perf_counter() - - if rsp.isError(): - raise RuntimeError(f"Modbus Read Failed (Addr={address}, Count={count}): {rsp}") - - # 确保返回的值是 Python 原生整数 - return [int(reg) for reg in rsp.registers] - - def _execute_write(self, address: int, values: List[int]): - """执行 Modbus 批量写入操作 (功能码 16), 带总线仲裁。""" - self._bus_free() - - # values 必须是 Python 原生整数列表 - rsp = self.cli.write_registers( - address=address, - values=values, - slave=self._id - ) - - self._last_ts = time.perf_counter() - - if rsp.isError(): - raise RuntimeError(f"Modbus Write Failed (Addr={address}, Values={values}): {rsp}") - - # ---------------------------------------------------------- - # 批量读取和数据封装（优化通信效率） - # ---------------------------------------------------------- - def read_all_angles(self) -> List[int]: - return self._execute_read(REG_RD_CURRENT_THUMB_PITCH, 6) - - def read_all_torques(self) -> List[int]: - return self._execute_read(REG_RD_CURRENT_THUMB_TORQUE, 6) - - def read_all_speeds(self) -> List[int]: - return self._execute_read(REG_RD_CURRENT_THUMB_SPEED, 6) - - def read_all_temperatures(self) -> List[int]: - return self._execute_read(REG_RD_THUMB_TEMP, 6) - - def read_all_errors(self) -> List[int]: - return self._execute_read(REG_RD_THUMB_ERROR, 6) - - def read_all_versions(self) -> List[int]: - return self._execute_read(REG_RD_HAND_FREEDOM, 6) - - - # ---------------------------------------------------------- - # 只读属性（单个寄存器读取） - # ---------------------------------------------------------- - def _read_reg(self, addr: int) -> int: - """读单个输入寄存器（功能码 04），带 30 ms 帧间隔""" - return self._execute_read(addr, 1)[0] - - def get_thumb_pitch(self) -> int: return self._read_reg(REG_RD_CURRENT_THUMB_PITCH) - def get_thumb_yaw(self) -> int: return self._read_reg(REG_RD_CURRENT_THUMB_YAW) - def get_index_pitch(self) -> int: return self._read_reg(REG_RD_CURRENT_INDEX_PITCH) - def get_middle_pitch(self) -> int: return self._read_reg(REG_RD_CURRENT_MIDDLE_PITCH) - def get_ring_pitch(self) -> int: return self._read_reg(REG_RD_CURRENT_RING_PITCH) - def get_little_pitch(self) -> int: return self._read_reg(REG_RD_CURRENT_LITTLE_PITCH) - - def get_thumb_torque(self) -> int: return self._read_reg(REG_RD_CURRENT_THUMB_TORQUE) - def get_thumb_yaw_torque(self) -> int: return self._read_reg(REG_RD_CURRENT_THUMB_YAW_TORQUE) - def get_index_torque(self) -> int: return self._read_reg(REG_RD_CURRENT_INDEX_TORQUE) - def get_middle_torque(self) -> int: return self._read_reg(REG_RD_CURRENT_MIDDLE_TORQUE) - def get_ring_torque(self) -> int: return self._read_reg(REG_RD_CURRENT_RING_TORQUE) - def get_little_torque(self) -> int: return self._read_reg(REG_RD_CURRENT_LITTLE_TORQUE) - - def get_thumb_speed(self) -> int: return self._read_reg(REG_RD_CURRENT_THUMB_SPEED) - def get_thumb_yaw_speed(self) -> int: return self._read_reg(REG_RD_CURRENT_THUMB_YAW_SPEED) - def get_index_speed(self) -> int: return self._read_reg(REG_RD_CURRENT_INDEX_SPEED) - def get_middle_speed(self) -> int: return self._read_reg(REG_RD_CURRENT_MIDDLE_SPEED) - def get_ring_speed(self) -> int: return self._read_reg(REG_RD_CURRENT_RING_SPEED) - def get_little_speed(self) -> int: return self._read_reg(REG_RD_CURRENT_LITTLE_SPEED) - - def get_thumb_temp(self) -> int: return self._read_reg(REG_RD_THUMB_TEMP) - def get_thumb_yaw_temp(self) -> int: return self._read_reg(REG_RD_THUMB_YAW_TEMP) - def get_index_temp(self) -> int: return self._read_reg(REG_RD_INDEX_TEMP) - def get_middle_temp(self) -> int: return self._read_reg(REG_RD_MIDDLE_TEMP) - def get_ring_temp(self) -> int: return self._read_reg(REG_RD_RING_TEMP) - def get_little_temp(self) -> int: return self._read_reg(REG_RD_LITTLE_TEMP) - - def get_thumb_error(self) -> int: return self._read_reg(REG_RD_THUMB_ERROR) - def get_thumb_yaw_error(self) -> int: return self._read_reg(REG_RD_THUMB_YAW_ERROR) - def get_index_error(self) -> int: return self._read_reg(REG_RD_INDEX_ERROR) - def get_middle_error(self) -> int: return self._read_reg(REG_RD_MIDDLE_ERROR) - def get_ring_error(self) -> int: return self._read_reg(REG_RD_RING_ERROR) - def get_little_error(self) -> int: return self._read_reg(REG_RD_LITTLE_ERROR) - - def get_hand_freedom(self) -> int: return self._read_reg(REG_RD_HAND_FREEDOM) - def get_hand_version(self) -> int: return self._read_reg(REG_RD_HAND_VERSION) - def get_hand_number(self) -> int: return self._read_reg(REG_RD_HAND_NUMBER) - def get_hand_direction(self) -> int: return self._read_reg(REG_RD_HAND_DIRECTION) - def get_software_version(self) -> int: return self._read_reg(REG_RD_SOFTWARE_VERSION) - def get_hardware_version(self) -> int: return self._read_reg(REG_RD_HARDWARE_VERSION) - - # ---------------------------------------------------------- - # 批量 Getter (使用 read_all_... 方法) - # ---------------------------------------------------------- - def get_state(self) -> List[int]: - """获取手指电机状态 (角度)""" - return self.read_all_angles() - - def get_torque(self) -> List[int]: - """获取当前扭矩""" - return self.read_all_torques() - - def get_speed(self) -> List[int]: - """获取当前速度""" - return self.read_all_speeds() - - def get_temperature(self) -> List[int]: - """获取当前电机温度""" - return self.read_all_temperatures() - - def get_fault(self) -> List[int]: - """获取当前电机故障码""" - return self.read_all_errors() - - def get_version(self) -> list: - """获取当前固件版本号""" - return self.read_all_versions() - - - # ---------------------------------------------------------- - # 写保持寄存器 (单个寄存器写入) - # ---------------------------------------------------------- - def _write_reg(self, addr: int, value: int): - """写单个保持寄存器（功能码 16），带 30 ms 帧间隔""" - if not 0 <= value <= 255: - raise ValueError("value must be 0-255") - - # 确保 value 是 Python 原生 int - self._execute_write(addr, [int(value)]) - - def _write_regs(self, addr: int, values: List[int]): - """写多个保持寄存器（功能码 16），带 30 ms 帧间隔""" - # 此时 values 应该已经是经过 is_valid_6xuint8 验证并转换的 Python int 列表 - if not all(0 <= v <= 255 for v in values): - # 这行理论上不应触发，因为上层调用已校验 - raise ValueError("All values must be 0-255") - self._execute_write(addr, values) - - - def set_thumb_pitch(self, v: int): self._write_reg(REG_WR_THUMB_PITCH, v) - def set_thumb_yaw(self, v: int): self._write_reg(REG_WR_THUMB_YAW, v) - def set_index_pitch(self, v: int): self._write_reg(REG_WR_INDEX_PITCH, v) - def set_middle_pitch(self, v: int): self._write_reg(REG_WR_MIDDLE_PITCH, v) - def set_ring_pitch(self, v: int): self._write_reg(REG_WR_RING_PITCH, v) - def set_little_pitch(self, v: int): self._write_reg(REG_WR_LITTLE_PITCH, v) - - def set_thumb_torque(self, v: int): self._write_reg(REG_WR_THUMB_TORQUE, v) - def set_thumb_yaw_torque(self, v: int): self._write_reg(REG_WR_THUMB_YAW_TORQUE, v) - def set_index_torque(self, v: int): self._write_reg(REG_WR_INDEX_TORQUE, v) - def set_middle_torque(self, v: int): self._write_reg(REG_WR_MIDDLE_TORQUE, v) - def set_ring_torque(self, v: int): self._write_reg(REG_WR_RING_TORQUE, v) - def set_little_torque(self, v: int): self._write_reg(REG_WR_LITTLE_TORQUE, v) - - def set_thumb_speed(self, v: int): self._write_reg(REG_WR_THUMB_SPEED, v) - def set_thumb_yaw_speed(self, v: int): self._write_reg(REG_WR_THUMB_YAW_SPEED, v) - def set_index_speed(self, v: int): self._write_reg(REG_WR_INDEX_SPEED, v) - def set_middle_speed(self, v: int): self._write_reg(REG_WR_MIDDLE_SPEED, v) - def set_ring_speed(self, v: int): self._write_reg(REG_WR_RING_SPEED, v) - def set_little_speed(self, v: int): self._write_reg(REG_WR_LITTLE_SPEED, v) - - # ---------------------------------------------------------- - # 固定函数 (采用批量写入优化) - # ---------------------------------------------------------- - def is_valid_6xuint8(self, lst: List[Any]) -> bool: - """ - 验证6个0-255的整数列表。 - 允许输入包含浮点数、NumPy整数等可转换为 int 的类型，并进行范围校验。 - """ - if not (isinstance(lst, list) and len(lst) == 6): - return False - - try: - # 关键：尝试将所有元素转换为 Python 原生 int - int_values = [int(v) for v in lst] - except (ValueError, TypeError): - # 转换失败，列表中包含不可转换的元素 - return False - - # 校验转换后的整数列表是否在 0-255 范围内 - return all(0 <= x <= 255 for x in int_values) - - def set_joint_positions(self, joint_angles: List[Any] = None): - joint_angles = joint_angles or [0] * 6 - - if not self.is_valid_6xuint8(joint_angles): - logging.error(f"Invalid joint angles received: {joint_angles}") - raise ValueError("Joint angles must be a list of 6 values between 0 and 255 (convertible to int).") - - # 强制转换为 Modbus 兼容的 Python 原生 int 列表 - int_angles = [int(v) for v in joint_angles] - - # 批量写入 6 个角度寄存器 (从 REG_WR_THUMB_PITCH 地址 0 开始, count=6) - self._write_regs(REG_WR_THUMB_PITCH, int_angles) - - def set_speed(self, speed: List[Any] = None): - speed = speed or [200] * 6 - if not self.is_valid_6xuint8(speed): - logging.error(f"Invalid speed values received: {speed}") - raise ValueError("Speed values must be a list of 6 values between 0 and 255 (convertible to int).") - - int_speed = [int(v) for v in speed] - self._write_regs(REG_WR_THUMB_SPEED, int_speed) - - def set_torque(self, torque: List[Any] = None): - torque = torque or [200] * 6 - if not self.is_valid_6xuint8(torque): - logging.error(f"Invalid torque values received: {torque}") - raise ValueError("Torque values must be a list of 6 values between 0 and 255 (convertible to int).") - - int_torque = [int(v) for v in torque] - self._write_regs(REG_WR_THUMB_TORQUE, int_torque) - - # ... (其他固定函数保持不变) ... - - def set_current(self, current: List[int] = None): - print("当前O6不支持设置电流", flush=True) - pass - - def get_state_for_pub(self) -> list: - return self.get_state() - - def get_current_status(self) -> list: - return self.get_state() - - def get_joint_speed(self) -> list: - return self.get_speed() - - def get_touch_type(self) -> list: - return -1 - - def get_normal_force(self) -> list: - return [-1] * 5 - - def get_tangential_force(self) -> list: - return [-1] * 5 - - def get_approach_inc(self) -> list: - return [-1] * 5 - - def get_touch(self) -> list: - return [-1] * 5 - - def get_thumb_matrix_touch(self,sleep_time=0): - return np.full((12, 6), -1) - - def get_index_matrix_touch(self,sleep_time=0): - return np.full((12, 6), -1) - - def get_middle_matrix_touch(self,sleep_time=0): - return np.full((12, 6), -1) - - def get_ring_matrix_touch(self,sleep_time=0): - return np.full((12, 6), -1) - - def get_little_matrix_touch(self,sleep_time=0): - return np.full((12, 6), -1) - - def get_matrix_touch(self) -> list: - thumb_matrix = np.full((12, 6), -1) - index_matrix = np.full((12, 6), -1) - middle_matrix = np.full((12, 6), -1) - ring_matrix = np.full((12, 6), -1) - little_matrix = np.full((12, 6), -1) - return thumb_matrix , index_matrix , middle_matrix , ring_matrix , little_matrix - - def get_serial_number(self): - return [0] * 6 - - def get_matrix_touch_v2(self) -> list: - return self.get_matrix_touch() - - def get_finger_order(self): - return ["thumb_cmc_pitch", "thumb_cmc_yaw", "index_mcp_pitch", "middle_mcp_pitch", "ring_mcp_pitch", "pinky_mcp_pitch"] - - # ---------------------------------------------------------- - # 上下文管理 - # ---------------------------------------------------------- - def close(self): - if hasattr(self, 'connected') and self.connected: - self.cli.close() - self.connected = False - logging.info("Modbus connection closed.") - - def __enter__(self): - return self - - def __exit__(self, exc_type, exc_val, exc_tb): - self.close() - - # ---------------------------------------------------------- - # 便捷函数 - # ---------------------------------------------------------- - def set_all_fingers(self, pitch: int): - """同时设置五指弯曲角度（0-255），使用批量写入""" - # 允许传入 float/numpy int 等可转换为 int 的类型 - try: - pitch_int = int(pitch) - except (ValueError, TypeError): - raise ValueError("Pitch value must be a number convertible to int (0-255)") - - if not 0 <= pitch_int <= 255: - raise ValueError("Pitch value must be 0-255") - - # 批量设置所有 6 个关节的角度 - self.set_joint_positions([pitch_int] * 6) - - def relax(self): - """全部手指伸直（255）""" - self.set_all_fingers(255) - - def fist(self): - """全部手指弯曲（0）""" - self.set_all_fingers(0) - - def dump_status(self): - """打印当前所有可读状态 (使用批量读取优化)""" - print("--------- O6 Hand Status ---------") - - angles = self.get_state() - temps = self.get_temperature() - errors = self.get_fault() - versions = self.get_version() - - print(f"Joint Angles: {angles}") - print(f"Temperature: {temps}℃") - print(f"Error Codes: {errors}") - print(f"Versions: {versions}") - print("----------------------------------") - -# ------------------------------------------------------------------ -# 命令行快速测试 -# ------------------------------------------------------------------ -if __name__ == "__main__": - import argparse - - # 假设默认站号是 0x27 (39) - DEFAULT_HAND_ID = 0x27 - - parser = argparse.ArgumentParser(description="O6 Hand Modbus tester (using pymodbus 3.5.1)") - parser.add_argument("-p", "--port", required=True, help="串口, 如 /dev/ttyUSB0") - parser.add_argument("-l", "--left", action="store_const", const=0x28, default=DEFAULT_HAND_ID, dest='hand_id', help="左手 (0x28)，默认右手 (0x27)") - - args = parser.parse_args() - - try: - # 使用 with 语句确保连接关闭，这是 pymodbus 的推荐用法 - with LinkerHandO6RS485(hand_id=args.hand_id, modbus_port=args.port, baudrate=115200) as hand: - hand.dump_status() - print("执行 relax → 伸直") - hand.relax() - time.sleep(1) - print("执行 fist → 握拳") - hand.fist() - time.sleep(1) - hand.relax() - print("演示完成") - - except ConnectionError as e: - print(f"连接错误: {e}") - except RuntimeError as e: - print(f"Modbus 运行时错误: {e}") - except StructError as e: - print(f"数据结构错误 (请检查输入数据类型是否为原生int): {e}") - except Exception as e: - print(f"发生其他错误: {e}") - - -========================================================================================================================== - - -#!/usr/bin/env python3 -""" -O6 机械手 Modbus-RTU 控制类 (基于 pymodbus 3.5.1) -""" - -import os -import time -from typing import List, Dict, Any # 引入 Any 来表示灵活的输入类型 -import numpy as np -import logging -from threading import Lock # 用于线程安全和总线仲裁 - -# 导入 pymodbus 客户端 -from pymodbus.client import ModbusSerialClient -from struct import error as StructError - -logging.basicConfig( - level=logging.INFO, - format="[%(asctime)s] %(levelname)-8s %(message)s", - datefmt="%H:%M:%S" -) - -# ------------------------------------------------------------------ -# 读输入寄存器地址枚举（功能码 04，只读）- 按照 O6 协议文档定义 -# ------------------------------------------------------------------ -REG_RD_CURRENT_THUMB_PITCH = 0 # 大拇指弯曲角度（0-255，小=弯，大=伸） -REG_RD_CURRENT_THUMB_YAW = 1 # 大拇指横摆角度（0-255，小=靠掌心，大=远离） -REG_RD_CURRENT_INDEX_PITCH = 2 # 食指弯曲角度 -REG_RD_CURRENT_MIDDLE_PITCH = 3 # 中指弯曲角度 -REG_RD_CURRENT_RING_PITCH = 4 # 无名指弯曲角度 -REG_RD_CURRENT_LITTLE_PITCH = 5 # 小拇指弯曲角度 -REG_RD_CURRENT_THUMB_TORQUE = 6 # 大拇指弯曲转矩（0-255） -REG_RD_CURRENT_THUMB_YAW_TORQUE = 7 # 大拇指横摆转矩 -REG_RD_CURRENT_INDEX_TORQUE = 8 # 食指转矩 -REG_RD_CURRENT_MIDDLE_TORQUE = 9 # 中指转矩 -REG_RD_CURRENT_RING_TORQUE = 10 # 无名指转矩 -REG_RD_CURRENT_LITTLE_TORQUE = 11 # 小拇指转矩 -REG_RD_CURRENT_THUMB_SPEED = 12 # 大拇指弯曲速度（0-255） -REG_RD_CURRENT_THUMB_YAW_SPEED = 13 # 大拇指横摆速度 -REG_RD_CURRENT_INDEX_SPEED = 14 # 食指速度 -REG_RD_CURRENT_MIDDLE_SPEED = 15 # 中指速度 -REG_RD_CURRENT_RING_SPEED = 16 # 无名指速度 -REG_RD_CURRENT_LITTLE_SPEED = 17 # 小拇指速度 -REG_RD_THUMB_TEMP = 18 # 大拇指弯曲温度（0-70℃） -REG_RD_THUMB_YAW_TEMP = 19 # 大拇指横摆温度 -REG_RD_INDEX_TEMP = 20 # 食指温度 -REG_RD_MIDDLE_TEMP = 21 # 中指温度 -REG_RD_RING_TEMP = 22 # 无名指温度 -REG_RD_LITTLE_TEMP = 23 # 小拇指温度 -REG_RD_THUMB_ERROR = 24 # 大拇指错误码 -REG_RD_THUMB_YAW_ERROR = 25 # 大拇指横摆错误码 -REG_RD_INDEX_ERROR = 26 # 食指错误码 -REG_RD_MIDDLE_ERROR = 27 # 中指错误码 -REG_RD_RING_ERROR = 28 # 无名指错误码 -REG_RD_LITTLE_ERROR = 29 # 小拇指错误码 - -# 版本号/设备编号寄存器（地址 30-44，共15个寄存器） -REG_RD_HAND_FREEDOM = 30 # Hand_freedom - 设备编号 / 自由度（与机械手上标签相同） -REG_RD_HAND_VERSION = 31 # hand_version - 手版本 -REG_RD_HAND_NUMBER_HIGH = 32 # hand_number_高位 - 设备编号（高字节） -REG_RD_HAND_NUMBER_MID = 33 # hand_number_中位 - 设备编号（中字节） -REG_RD_HAND_NUMBER_LOW = 34 # hand_number_低位 - 设备编号（低字节） -REG_RD_HAND_DIRECTION = 35 # hand_direction - 手方向（左/右） -REG_RD_HARDWARE_VERSION_HIGH = 36 # hardware_version_高位 - 硬件版本（高字节） -REG_RD_HARDWARE_VERSION_MID = 37 # hardware_version_中位 - 硬件版本（中字节） -REG_RD_HARDWARE_VERSION_LOW = 38 # hardware_version_低位 - 硬件版本（低字节） -REG_RD_SOFTWARE_VERSION_HIGH = 39 # software_version_高位 - 软件版本（高字节） -REG_RD_SOFTWARE_VERSION_MID = 40 # software_version_中位 - 软件版本（中字节） -REG_RD_SOFTWARE_VERSION_LOW = 41 # software_version_低位 - 软件版本（低字节） -REG_RD_MECHANICAL_VERSION_HIGH = 42 # mechanical_version_高位 - 机械版本（高字节） -REG_RD_MECHANICAL_VERSION_MID = 43 # mechanical_version_中位 - 机械版本（中字节） -REG_RD_MECHANICAL_VERSION_LOW = 44 # mechanical_version_低位 - 机械版本（低字节） - -# 力传感器寄存器（地址 45-87+，动态范围） -REG_RD_PRESSURE_SENSING_ID = 45 # Pressure_Sensing_ID - 压力传感器ID (0-5) -REG_RD_PRESSURE_SENSING_SPEC = 46 # Pressure_Sensing_Specifications - 传感器数据规格 - - -# ------------------------------------------------------------------ -# 写保持寄存器地址枚举（功能码 16，读写）- 保持原样 -# ------------------------------------------------------------------ -REG_WR_THUMB_PITCH = 0 # 大拇指弯曲角度（0-255） -REG_WR_THUMB_YAW = 1 # 大拇指横摆角度 -REG_WR_INDEX_PITCH = 2 # 食指弯曲角度 -REG_WR_MIDDLE_PITCH = 3 # 中指弯曲角度 -REG_WR_RING_PITCH = 4 # 无名指弯曲角度 -REG_WR_LITTLE_PITCH = 5 # 小拇指弯曲角度 -REG_WR_THUMB_TORQUE = 6 # 大拇指弯曲转矩 -REG_WR_THUMB_YAW_TORQUE = 7 # 大拇指横摆转矩 -REG_WR_INDEX_TORQUE = 8 # 食指转矩 -REG_WR_MIDDLE_TORQUE = 9 # 中指转矩 -REG_WR_RING_TORQUE = 10 # 无名指转矩 -REG_WR_LITTLE_TORQUE = 11 # 小拇指转矩 -REG_WR_THUMB_SPEED = 12 # 大拇指弯曲速度 -REG_WR_THUMB_YAW_SPEED = 13 # 大拇指横摆速度 -REG_WR_INDEX_SPEED = 14 # 食指速度 -REG_WR_MIDDLE_SPEED = 15 # 中指速度 -REG_WR_RING_SPEED = 16 # 无名指速度 -REG_WR_LITTLE_SPEED = 17 # 小拇指速度 - - -class LinkerHandO6RS485: - """O6 机械手 Modbus-RTU 控制类，使用 pymodbus 3.5.1""" - - TTL_TIMEOUT = 0.15 # 串口超时 - FRAME_GAP = 0.030 # 30 ms - - # KEYS for easy indexing - JOINT_KEYS = ["thumb_pitch", "thumb_yaw", "index_pitch", - "middle_pitch", "ring_pitch", "little_pitch"] - - def __init__(self, hand_id=0x27, modbus_port="/dev/ttyUSB0", baudrate=115200): - self._id = hand_id - self._last_ts = 0.0 # 上一次帧结束时间 - self._lock = Lock() # 总线访问锁 - - # 使用 pymodbus 3.x 客户端 - self.cli = ModbusSerialClient( - port=modbus_port, - baudrate=baudrate, - bytesize=8, - parity="N", - stopbits=1, - timeout=self.TTL_TIMEOUT, - handle_local_echo=False - ) - - try: - logging.info(f"Connecting to Modbus RTU on {modbus_port}...") - self.connected = self.cli.connect() - if not self.connected: - raise ConnectionError(f"RS485 connect fail to {modbus_port}") - logging.info("Connection successful.") - except Exception as e: - logging.error(f"Initialization failed: {e}") - raise - - # ---------------------------------------------------------- - # 辅助方法 - # ---------------------------------------------------------- - def _bus_free(self): - """保证距离上一帧 ≥ 30 ms""" - with self._lock: - elapse = time.perf_counter() - self._last_ts - if elapse < self.FRAME_GAP: - time.sleep(self.FRAME_GAP - elapse) - - def _execute_read(self, address: int, count: int) -> List[int]: - """执行 Modbus 读取操作 (功能码 04), 带总线仲裁。""" - self._bus_free() - - rsp = self.cli.read_input_registers( - address=address, - count=count, - slave=self._id - ) - - self._last_ts = time.perf_counter() - - if rsp.isError(): - raise RuntimeError(f"Modbus Read Failed (Addr={address}, Count={count}): {rsp}") - - # 确保返回的值是 Python 原生整数 - return [int(reg) for reg in rsp.registers] - - def _execute_write(self, address: int, values: List[int]): - """执行 Modbus 批量写入操作 (功能码 16), 带总线仲裁。""" - self._bus_free() - - # values 必须是 Python 原生整数列表 - rsp = self.cli.write_registers( - address=address, - values=values, - slave=self._id - ) - - self._last_ts = time.perf_counter() - - if rsp.isError(): - raise RuntimeError(f"Modbus Write Failed (Addr={address}, Values={values}): {rsp}") - - # ---------------------------------------------------------- - # 批量读取和数据封装（优化通信效率） - # ---------------------------------------------------------- - def read_all_angles(self) -> List[int]: - return self._execute_read(REG_RD_CURRENT_THUMB_PITCH, 6) - - def read_all_torques(self) -> List[int]: - return self._execute_read(REG_RD_CURRENT_THUMB_TORQUE, 6) - - def read_all_speeds(self) -> List[int]: - return self._execute_read(REG_RD_CURRENT_THUMB_SPEED, 6) - - def read_all_temperatures(self) -> List[int]: - return self._execute_read(REG_RD_THUMB_TEMP, 6) - - def read_all_errors(self) -> List[int]: - return self._execute_read(REG_RD_THUMB_ERROR, 6) - - # ---------------------------------------------------------- - # 版本号/设备编号读取（按照协议文档：地址30-44，共15个寄存器） - # ---------------------------------------------------------- - def read_all_versions(self) -> str: - """一次性读取全部15个寄存器 (地址30-44)，返回以 '.' 连接的字符串。 - - 按 O6 协议文档的版本号格式返回： - hand_freedom.hand_version.hand_number_high.hand_number_mid.hand_number_low - .hand_direction.hardware_ver_hardware_ver_m.hardware_ver_l - .software_ver_h.software_ver_m.software_ver_l - .mechanical_ver_h.mechanical_ver_m.mechanical_ver_l - - 例如: "6.1.001.002.003.0.1.2.3.4.5.6.7.8.9" - """ - raw = self._execute_read(REG_RD_HAND_FREEDOM, 15) - return ".".join(str(v) for v in raw) - - # ---------------------------------------------------------- - # 基于 read_all_versions() 的设备编号解析方法 - # ---------------------------------------------------------- - def _parse_versions(self): - """解析 read_all_versions() 返回的字符串为字典""" - parts = self.read_all_versions().split(".") - if len(parts) < 15: - return {} - return { - "hand_freedom": int(parts[0]), - "hand_version": int(parts[1]), - "hand_number_high": int(parts[2]), - "hand_number_mid": int(parts[3]), - "hand_number_low": int(parts[4]), - "hand_direction": int(parts[5]), - "hw_ver_high": int(parts[6]), - "hw_ver_mid": int(parts[7]), - "hw_ver_low": int(parts[8]), - "sw_ver_high": int(parts[9]), - "sw_ver_mid": int(parts[10]), - "sw_ver_low": int(parts[11]), - "mech_ver_high": int(parts[12]), - "mech_ver_mid": int(parts[13]), - "mech_ver_low": int(parts[14]), - } - - def get_device_number(self) -> str: - """获取设备编号（与机械手上标签相同）。格式：高位+中位+低位 拼接的字符串。""" - v = self._parse_versions() - if not v: - return "0" - high, mid, low = v["hand_number_high"], v["hand_number_mid"], v["hand_number_low"] - # 将每个字节格式化为无前导零的整数（与标签显示一致） - return f"{high}{mid}{low}" - - def get_device_number_value(self) -> int: - """获取设备编号数值""" - v = self._parse_versions() - if not v: - return 0 - high, mid, low = v["hand_number_high"], v["hand_number_mid"], v["hand_number_low"] - return high * 65536 + mid * 256 + low - - def get_hardware_version(self) -> str: - """获取硬件版本号。格式：高.中.低""" - v = self._parse_versions() - if not v: - return "0.0.0" - return f"{v['hw_ver_high']}.{v['hw_ver_mid']}.{v['hw_ver_low']}" - - def get_software_version(self) -> str: - """获取软件版本号。格式：高.中.低""" - v = self._parse_versions() - if not v: - return "0.0.0" - return f"{v['sw_ver_high']}.{v['sw_ver_mid']}.{v['sw_ver_low']}" - - def get_mechanical_version(self) -> str: - """获取机械版本号。格式：高.中.低""" - v = self._parse_versions() - if not v: - return "0.0.0" - return f"{v['mech_ver_high']}.{v['mech_ver_mid']}.{v['mech_ver_low']}" - - def get_hand_freedom(self) -> int: - """获取自由度（与机械手上标签相同）""" - v = self._parse_versions() - return v.get("hand_freedom", 0) - - def get_hand_version_raw(self) -> int: - """获取手版本原始值""" - v = self._parse_versions() - return v.get("hand_version", 0) - - def get_hand_direction(self) -> str: - """获取手方向，转换为字符：76→'L', 82→'R'""" - v = self._parse_versions() - val = v.get("hand_direction", 0) - if val in (76, 82): - return chr(val) - # fallback: 直接转为字符（如果值在可打印范围内） - return chr(val) if 32 < val < 128 else f'Unknown({val})' - - - # ---------------------------------------------------------- - # 只读属性（单个寄存器读取） - # ---------------------------------------------------------- - def _read_reg(self, addr: int) -> int: - """读单个输入寄存器（功能码 04），带 30 ms 帧间隔""" - return self._execute_read(addr, 1)[0] - - def get_thumb_pitch(self) -> int: return self._read_reg(REG_RD_CURRENT_THUMB_PITCH) - def get_thumb_yaw(self) -> int: return self._read_reg(REG_RD_CURRENT_THUMB_YAW) - def get_index_pitch(self) -> int: return self._read_reg(REG_RD_CURRENT_INDEX_PITCH) - def get_middle_pitch(self) -> int: return self._read_reg(REG_RD_CURRENT_MIDDLE_PITCH) - def get_ring_pitch(self) -> int: return self._read_reg(REG_RD_CURRENT_RING_PITCH) - def get_little_pitch(self) -> int: return self._read_reg(REG_RD_CURRENT_LITTLE_PITCH) - - def get_thumb_torque(self) -> int: return self._read_reg(REG_RD_CURRENT_THUMB_TORQUE) - def get_thumb_yaw_torque(self) -> int: return self._read_reg(REG_RD_CURRENT_THUMB_YAW_TORQUE) - def get_index_torque(self) -> int: return self._read_reg(REG_RD_CURRENT_INDEX_TORQUE) - def get_middle_torque(self) -> int: return self._read_reg(REG_RD_CURRENT_MIDDLE_TORQUE) - def get_ring_torque(self) -> int: return self._read_reg(REG_RD_CURRENT_RING_TORQUE) - def get_little_torque(self) -> int: return self._read_reg(REG_RD_CURRENT_LITTLE_TORQUE) - - def get_thumb_speed(self) -> int: return self._read_reg(REG_RD_CURRENT_THUMB_SPEED) - def get_thumb_yaw_speed(self) -> int: return self._read_reg(REG_RD_CURRENT_THUMB_YAW_SPEED) - def get_index_speed(self) -> int: return self._read_reg(REG_RD_CURRENT_INDEX_SPEED) - def get_middle_speed(self) -> int: return self._read_reg(REG_RD_CURRENT_MIDDLE_SPEED) - def get_ring_speed(self) -> int: return self._read_reg(REG_RD_CURRENT_RING_SPEED) - def get_little_speed(self) -> int: return self._read_reg(REG_RD_CURRENT_LITTLE_SPEED) - - def get_thumb_temp(self) -> int: return self._read_reg(REG_RD_THUMB_TEMP) - def get_thumb_yaw_temp(self) -> int: return self._read_reg(REG_RD_THUMB_YAW_TEMP) - def get_index_temp(self) -> int: return self._read_reg(REG_RD_INDEX_TEMP) - def get_middle_temp(self) -> int: return self._read_reg(REG_RD_MIDDLE_TEMP) - def get_ring_temp(self) -> int: return self._read_reg(REG_RD_RING_TEMP) - def get_little_temp(self) -> int: return self._read_reg(REG_RD_LITTLE_TEMP) - - def get_thumb_error(self) -> int: return self._read_reg(REG_RD_THUMB_ERROR) - def get_thumb_yaw_error(self) -> int: return self._read_reg(REG_RD_THUMB_YAW_ERROR) - def get_index_error(self) -> int: return self._read_reg(REG_RD_INDEX_ERROR) - def get_middle_error(self) -> int: return self._read_reg(REG_RD_MIDDLE_ERROR) - def get_ring_error(self) -> int: return self._read_reg(REG_RD_RING_ERROR) - def get_little_error(self) -> int: return self._read_reg(REG_RD_LITTLE_ERROR) - - - # ---------------------------------------------------------- - # 批量 Getter (使用 read_all_... 方法) - # ---------------------------------------------------------- - def get_state(self) -> List[int]: - """获取手指电机状态 (角度)""" - return self.read_all_angles() - - def get_torque(self) -> List[int]: - """获取当前扭矩""" - return self.read_all_torques() - - def get_speed(self) -> List[int]: - """获取当前速度""" - return self.read_all_speeds() - - def get_temperature(self) -> List[int]: - """获取当前电机温度""" - return self.read_all_temperatures() - - def get_fault(self) -> List[int]: - """获取当前电机故障码""" - return self.read_all_errors() - - def get_version(self) -> str: - """获取当前固件版本号（已转换为字符串格式）""" - return self.read_all_versions() - - - # ---------------------------------------------------------- - # 写保持寄存器 (单个寄存器写入) - # ---------------------------------------------------------- - def _write_reg(self, addr: int, value: int): - """写单个保持寄存器（功能码 16），带 30 ms 帧间隔""" - if not 0 <= value <= 255: - raise ValueError("value must be 0-255") - - # 确保 value 是 Python 原生 int - self._execute_write(addr, [int(value)]) - - def _write_regs(self, addr: int, values: List[int]): - """写多个保持寄存器（功能码 16），带 30 ms 帧间隔""" - # 此时 values 应该已经是经过 is_valid_6xuint8 验证并转换的 Python int 列表 - if not all(0 <= v <= 255 for v in values): - # 这行理论上不应触发，因为上层调用已校验 - raise ValueError("All values must be 0-255") - self._execute_write(addr, values) - - - def set_thumb_pitch(self, v: int): self._write_reg(REG_WR_THUMB_PITCH, v) - def set_thumb_yaw(self, v: int): self._write_reg(REG_WR_THUMB_YAW, v) - def set_index_pitch(self, v: int): self._write_reg(REG_WR_INDEX_PITCH, v) - def set_middle_pitch(self, v: int): self._write_reg(REG_WR_MIDDLE_PITCH, v) - def set_ring_pitch(self, v: int): self._write_reg(REG_WR_RING_PITCH, v) - def set_little_pitch(self, v: int): self._write_reg(REG_WR_LITTLE_PITCH, v) - - def set_thumb_torque(self, v: int): self._write_reg(REG_WR_THUMB_TORQUE, v) - def set_thumb_yaw_torque(self, v: int): self._write_reg(REG_WR_THUMB_YAW_TORQUE, v) - def set_index_torque(self, v: int): self._write_reg(REG_WR_INDEX_TORQUE, v) - def set_middle_torque(self, v: int): self._write_reg(REG_WR_MIDDLE_TORQUE, v) - def set_ring_torque(self, v: int): self._write_reg(REG_WR_RING_TORQUE, v) - def set_little_torque(self, v: int): self._write_reg(REG_WR_LITTLE_TORQUE, v) - - def set_thumb_speed(self, v: int): self._write_reg(REG_WR_THUMB_SPEED, v) - def set_thumb_yaw_speed(self, v: int): self._write_reg(REG_WR_THUMB_YAW_SPEED, v) - def set_index_speed(self, v: int): self._write_reg(REG_WR_INDEX_SPEED, v) - def set_middle_speed(self, v: int): self._write_reg(REG_WR_MIDDLE_SPEED, v) - def set_ring_speed(self, v: int): self._write_reg(REG_WR_RING_SPEED, v) - def set_little_speed(self, v: int): self._write_reg(REG_WR_LITTLE_SPEED, v) - - # ---------------------------------------------------------- - # 固定函数 (采用批量写入优化) - # ---------------------------------------------------------- - def is_valid_6xuint8(self, lst: List[Any]) -> bool: - """ - 验证6个0-255的整数列表。 - 允许输入包含浮点数、NumPy整数等可转换为 int 的类型，并进行范围校验。 - """ - if not (isinstance(lst, list) and len(lst) == 6): - return False - - try: - # 关键：尝试将所有元素转换为 Python 原生 int - int_values = [int(v) for v in lst] - except (ValueError, TypeError): - # 转换失败，列表中包含不可转换的元素 - return False - - # 校验转换后的整数列表是否在 0-255 范围内 - return all(0 <= x <= 255 for x in int_values) - - def set_joint_positions(self, joint_angles: List[Any] = None): - joint_angles = joint_angles or [0] * 6 - - if not self.is_valid_6xuint8(joint_angles): - logging.error(f"Invalid joint angles received: {joint_angles}") - raise ValueError("Joint angles must be a list of 6 values between 0 and 255 (convertible to int).") - - # 强制转换为 Modbus 兼容的 Python 原生 int 列表 - int_angles = [int(v) for v in joint_angles] - - # 批量写入 6 个角度寄存器 (从 REG_WR_THUMB_PITCH 地址 0 开始, count=6) - self._write_regs(REG_WR_THUMB_PITCH, int_angles) - - def set_speed(self, speed: List[Any] = None): - speed = speed or [200] * 6 - if not self.is_valid_6xuint8(speed): - logging.error(f"Invalid speed values received: {speed}") - raise ValueError("Speed values must be a list of 6 values between 0 and 255 (convertible to int).") - - int_speed = [int(v) for v in speed] - self._write_regs(REG_WR_THUMB_SPEED, int_speed) - - def set_torque(self, torque: List[Any] = None): - torque = torque or [200] * 6 - if not self.is_valid_6xuint8(torque): - logging.error(f"Invalid torque values received: {torque}") - raise ValueError("Torque values must be a list of 6 values between 0 and 255 (convertible to int).") - - int_torque = [int(v) for v in torque] - self._write_regs(REG_WR_THUMB_TORQUE, int_torque) - - # ... (其他固定函数保持不变) ... - - def set_current(self, current: List[int] = None): - print("当前O6不支持设置电流", flush=True) - pass - - def get_state_for_pub(self) -> list: - return self.get_state() - - def get_current_status(self) -> list: - return self.get_state() - - def get_joint_speed(self) -> list: - return self.get_speed() - - def get_touch_type(self) -> list: - return -1 - - def get_normal_force(self) -> list: - return [-1] * 5 - - def get_tangential_force(self) -> list: - return [-1] * 5 - - def get_approach_inc(self) -> list: - return [-1] * 5 - - def get_touch(self) -> list: - return [-1] * 5 - - def get_thumb_matrix_touch(self,sleep_time=0): - return np.full((12, 6), -1) - - def get_index_matrix_touch(self,sleep_time=0): - return np.full((12, 6), -1) - - def get_middle_matrix_touch(self,sleep_time=0): - return np.full((12, 6), -1) - - def get_ring_matrix_touch(self,sleep_time=0): - return np.full((12, 6), -1) - - def get_little_matrix_touch(self,sleep_time=0): - return np.full((12, 6), -1) - - def get_matrix_touch(self) -> list: - thumb_matrix = np.full((12, 6), -1) - index_matrix = np.full((12, 6), -1) - middle_matrix = np.full((12, 6), -1) - ring_matrix = np.full((12, 6), -1) - little_matrix = np.full((12, 6), -1) - return thumb_matrix , index_matrix , middle_matrix , ring_matrix , little_matrix - - def get_serial_number(self): - - return "["+str(self.get_hand_freedom())+"-"+str(self.get_mechanical_version())+"-"+str(self.get_device_number())+"-"+str(self.get_hand_direction())+"]" - - def get_matrix_touch_v2(self) -> list: - return self.get_matrix_touch() - - def get_finger_order(self): - return ["thumb_cmc_pitch", "thumb_cmc_yaw", "index_mcp_pitch", "middle_mcp_pitch", "ring_mcp_pitch", "pinky_mcp_pitch"] - - def clear_faults(self): - pass - - def close(self): - if hasattr(self, 'connected') and self.connected: - self.cli.close() - self.connected = False - logging.info("Modbus connection closed.") - - def __enter__(self): - return self - - def __exit__(self, exc_type, exc_val, exc_tb): - self.close() - - # ---------------------------------------------------------- - # 便捷函数 - # ---------------------------------------------------------- - def set_all_fingers(self, pitch: int): - """同时设置五指弯曲角度（0-255），使用批量写入""" - # 允许传入 float/numpy int 等可转换为 int 的类型 - try: - pitch_int = int(pitch) - except (ValueError, TypeError): - raise ValueError("Pitch value must be a number convertible to int (0-255)") - - if not 0 <= pitch_int <= 255: - raise ValueError("Pitch value must be 0-255") - - # 批量设置所有 6 个关节的角度 - self.set_joint_positions([pitch_int] * 6) - - def relax(self): - """全部手指伸直（255）""" - self.set_all_fingers(255) - - def fist(self): - """全部手指弯曲（0）""" - self.set_all_fingers(0) - - def dump_status(self): - """打印当前所有可读状态 (使用批量读取优化)""" - print("--------- O6 Hand Status ---------") - - angles = self.get_state() - temps = self.get_temperature() - errors = self.get_fault() - - # 解析版本号字符串 - v = self._parse_versions() - - if v: - device_num_str = f"{v['hand_number_high']}{v['hand_number_mid']}{v['hand_number_low']}" - hw_ver = f"{v['hw_ver_high']}.{v['hw_ver_mid']}.{v['hw_ver_low']}" - sw_ver = f"{v['sw_ver_high']}.{v['sw_ver_mid']}.{v['sw_ver_low']}" - mech_ver = f"{v['mech_ver_high']}.{v['mech_ver_mid']}.{v['mech_ver_low']}" - - print(f"Device Number: {device_num_str}") - print(f"HWSW Version: HW={hw_ver} SW={sw_ver}") - print(f"Mechanical Ver: {mech_ver}") - print(f"Hand Freedom: {v['hand_freedom']}") - print(f"Full Version: {self.read_all_versions()}") - - print(f"Joint Angles: {angles}") - print(f"Temperature: {temps}℃") - print(f"Error Codes: {errors}") - print("----------------------------------") - - -# ------------------------------------------------------------------ -# 命令行快速测试 -# ------------------------------------------------------------------ -if __name__ == "__main__": - import argparse - - # 假设默认站号是 0x27 (39) - DEFAULT_HAND_ID = 0x27 - - parser = argparse.ArgumentParser(description="O6 Hand Modbus tester (using pymodbus 3.5.1)") - parser.add_argument("-p", "--port", required=True, help="串口, 如 /dev/ttyUSB0") - parser.add_argument("-l", "--left", action="store_const", const=0x28, default=DEFAULT_HAND_ID, dest='hand_id', help="左手 (0x28)，默认右手 (0x27)") - - args = parser.parse_args() - - try: - # 使用 with 语句确保连接关闭，这是 pymodbus 的推荐用法 - with LinkerHandO6RS485(hand_id=args.hand_id, modbus_port=args.port, baudrate=115200) as hand: - hand.dump_status() - - # 测试新增的设备编号读取方法 - print("\n--- 设备信息 ---") - print(f"设备编号（字符串）: {hand.get_device_number()}") - print(f"设备编号（数值）: {hand.get_device_number_value()}") - print(f"硬件版本号: {hand.get_hardware_version()}") - print(f"软件版本号: {hand.get_software_version()}") - print(f"机械版本号: {hand.get_mechanical_version()}") - - print("\n执行 relax → 伸直") - hand.relax() - time.sleep(1) - print("执行 fist → 握拳") - hand.fist() - time.sleep(1) - hand.relax() - print("演示完成") - - except ConnectionError as e: - print(f"连接错误: {e}") - except RuntimeError as e: - print(f"Modbus 运行时错误: {e}") - except StructError as e: - print(f"数据结构错误 (请检查输入数据类型是否为原生int): {e}") - except Exception as e: - print(f"发生其他错误: {e}") diff --git a/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/linker_hand.py.bak b/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/linker_hand.py.bak deleted file mode 100644 index 81c31a9..0000000 --- a/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/linker_hand.py.bak +++ /dev/null @@ -1,414 +0,0 @@ -#!/usr/bin/env python3 -# -*- coding: utf-8 -*- -''' -编译: colcon build --symlink-install -启动命令:ros2 run linker_hand_ros2_sdk linker_hand_sdk -''' -from re import A -import rclpy,sys # ROS2 Python接口库 -import time -import numpy as np -from rclpy.node import Node # ROS2 节点类 -from rclpy.clock import Clock -from std_msgs.msg import String, Header, Float32MultiArray -from sensor_msgs.msg import JointState, PointCloud2, PointField -import time, json, threading -from linker_hand_ros2_sdk.LinkerHand.linker_hand_api import LinkerHandApi -from linker_hand_ros2_sdk.LinkerHand.utils.color_msg import ColorMsg -from linker_hand_ros2_sdk.LinkerHand.utils.open_can import OpenCan - - -class LinkerHand(Node): - def __init__(self, name): - super().__init__(name) - # 声明参数（带默认值） - self.declare_parameter('hand_type', 'left') - self.declare_parameter('hand_joint', 'L6') - self.declare_parameter('is_touch', False) - self.declare_parameter('can', 'can0') - self.declare_parameter('modbus', "None") - - # ros时间获取 - self.stamp_clock = Clock() - # 获取参数值 - self.hand_type = self.get_parameter('hand_type').value - self.hand_joint = self.get_parameter('hand_joint').value - self.is_touch = self.get_parameter('is_touch').value - self.can = self.get_parameter('can').value - self.modbus = self.get_parameter('modbus').value - self.sdk_v = 2 - self.sleep_time = 0.005 - self.cmd_lock = False - self.last_hand_post_cmd = None # 最新手指位置命令 - self.last_hand_vel_cmd = None # 最新手指速度命令 - self.last_hand_eff_cmd = None # 最新手指力矩命令 - - self.last_hand_state = [-1] * 10 - self.last_hand_vel = [-1] * 10 - self.force = [[-1] * 5] * 4 - self.matrix_dic = { - "stamp":{ - "sec": 0, - "nanosec": 0, - }, - "thumb_matrix":[[-1] * 6 for _ in range(12)], - "index_matrix":[[-1] * 6 for _ in range(12)], - "middle_matrix":[[-1] * 6 for _ in range(12)], - "ring_matrix":[[-1] * 6 for _ in range(12)], - "little_matrix":[[-1] * 6 for _ in range(12)] - } - # 压感矩阵合值，单位g 克 - self.matrix_mass_dic = { - "stamp":{ - "secs": 0, - "nsecs": 0, - }, - "thumb_mass":[-1], - "index_mass":[-1], - "middle_mass":[-1], - "ring_mass":[-1], - "little_mass":[-1] - } - self.last_hand_info = { - "version": [-1], # Dexterous hand version number - "hand_joint": self.hand_joint, # Dexterous hand joint type - "speed": [-1] * 10, # Current speed threshold of the dexterous hand - "current": [-1] * 10, # Current of the dexterous hand - "fault": [-1] * 10, # Current fault of the dexterous hand - "motor_temperature": [-1] * 10, # Current motor temperature of the dexterous hand - "torque": [-1] * 10, # Current torque of the dexterous hand - "is_touch":self.is_touch, - "touch_type": -1, - "finger_order": None # Finger motor order - } - self.version = [] - self.touch_type = -1 - self.hz = 1.0/60.0 - - self.hand_setting_sub = self.create_subscription(String,'/cb_hand_setting_cmd', self.hand_setting_cb, 10) - self._init_hand() - time.sleep(1) - self.run_count = 0 # 计数器，用于记录运行次数 - self.timer = self.create_timer(0.01, self.run) # 100 Hz - self.thread_pub_state = threading.Thread(target=self.pub_state) - self.thread_pub_state.daemon = True - self.thread_pub_state.start() - - def _init_hand(self): - self.api = LinkerHandApi(hand_type=self.hand_type, hand_joint=self.hand_joint,modbus=self.modbus,can=self.can) - time.sleep(0.1) - self.touch_type = self.api.get_touch_type() - self.hand_cmd_sub = self.create_subscription(JointState, f'/cb_{self.hand_type}_hand_control_cmd', self.hand_control_cb,10) - self.hand_state_pub = self.create_publisher(JointState, f'/cb_{self.hand_type}_hand_state',10) - self.hand_info_pub = self.create_publisher(String, f'/cb_{self.hand_type}_hand_info', 10) - if self.is_touch == True: - if self.touch_type > 1: - ColorMsg(msg=f"{self.hand_type} {self.hand_joint} Equipped with matrix pressure sensing", color='green') - self.matrix_touch_pub = self.create_publisher(String, f'/cb_{self.hand_type}_hand_matrix_touch', 10) - self.matrix_touch_pub_pc = self.create_publisher(PointCloud2, f'/cb_{self.hand_type}_hand_matrix_touch_pc', 10) - self.matrix_touch_mass_pub = self.create_publisher(String, f'/cb_{self.hand_type}_hand_matrix_touch_mass', 10) - elif self.touch_type != -1: - ColorMsg(msg=f"{self.hand_type} {self.hand_joint} Equipped with pressure sensor", color="green") - self.touch_pub = self.create_publisher(Float32MultiArray, f'/cb_{self.hand_type}_hand_force', 10) - else: - ColorMsg(msg=f"{self.hand_type} {self.hand_joint} Not equipped with any pressure sensors", color="red") - self.is_touch = False - self.embedded_version = self.api.get_embedded_version() - pose = None - torque = [200, 200, 200, 200, 200] - speed = [200, 250, 250, 250, 250] - if self.hand_joint.upper() == "O6" or self.hand_joint.upper() == "L6" or self.hand_joint.upper() == "L6P": - pose = [200, 255, 255, 255, 255, 180] - torque = [250, 250, 250, 250, 250, 250] - # O6 最大速度阈值 - speed = [200, 250, 250, 250, 250, 250] - elif self.hand_joint == "L7": - # The data length of L7 is 7, reinitialize here - pose = [255, 200, 255, 255, 255, 255, 180] - torque = [250, 250, 250, 250, 250, 250, 250] - speed = [120, 250, 250, 250, 250, 250, 250] - elif self.hand_joint == "L10": - torque = [255] * 10 - pose = [255, 200, 255, 255, 255, 255, 180, 180, 180, 41] - speed = [200, 250, 250, 250, 250, 250, 250, 250, 250, 250] - elif self.hand_joint == "L20": - pose = [255,255,255,255,255,255,10,100,180,240,245,255,255,255,255,255,255,255,255,255] - elif self.hand_joint == "L21": - pose = [75, 255, 255, 255, 255, 176, 97, 81, 114, 147, 202, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255] - elif self.hand_joint == "L25": - pose = [75, 255, 255, 255, 255, 176, 97, 81, 114, 147, 202, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255] - if pose is not None: - for i in range(1): - self.api.set_speed(speed=speed) - time.sleep(0.1) - self.api.set_torque(torque=torque) - time.sleep(0.1) - self.api.finger_move(pose=pose) - time.sleep(0.1) - - def list_check(self,pose): - if isinstance(pose, list) == False: - return False - if len(self.last_hand_post_cmd) != len(pose): - return False - return any(abs(self.last_hand_post_cmd - pose) >= 3 for self.last_hand_post_cmd, pose in zip(self.last_hand_post_cmd, pose)) - - def hand_control_cb(self, msg): - if self.last_hand_post_cmd == None or self.list_check(msg.position) == True: - self.last_hand_post_cmd = msg.position - if self.last_hand_vel_cmd == None or self.list_check(msg.velocity) == True: - self.last_hand_vel_cmd = msg.velocity - if self.last_hand_eff_cmd == None or self.list_check(msg.effort) == True: - self.last_hand_eff_cmd = msg.effort - - def run(self): - if self.sdk_v == 1: - self.sleep_time = 0.009 - if self.hand_state_pub.get_subscription_count() > 0: - # 优先获取手指状态并且发布 - self.last_hand_state = self.api.get_state() - time.sleep(0.003) - self.last_hand_vel = self.api.get_joint_speed() - time.sleep(0.002) - if self.cmd_lock == False: - if self.last_hand_post_cmd != None: - self.api.finger_move(pose=self.last_hand_post_cmd) - self.last_hand_post_cmd = None - if self.last_hand_vel_cmd != None: - vel = list(self.last_hand_vel_cmd) - if all(x == 0 for x in vel): - pass - else: - if (str(self.hand_joint).upper() == "O6" or str(self.hand_joint).upper() == "L6" or str(self.hand_joint).upper() == "L6P") and len(vel) == 6: - speed = vel - self.api.set_joint_speed(speed=speed) - elif self.hand_joint == "L7" and len(vel) == 7: - speed = vel - self.api.set_joint_speed(speed=speed) - elif self.hand_joint == "L10" and len(vel) == 10: - speed = [vel[0],vel[2],vel[3],vel[4],vel[5]] - self.api.set_joint_speed(speed=speed) - elif self.hand_joint == "L20" and len(vel) == 20: - speed = [vel[10],vel[1],vel[2],vel[3],vel[4]] - self.api.set_joint_speed(speed=speed) - elif self.hand_joint == "L21" and len(vel) == 25: - speed = vel - self.api.set_joint_speed(speed=speed) - elif self.hand_joint == "L25" and len(vel) == 25: - speed = vel - self.api.set_joint_speed(speed=speed) - self.last_hand_vel_cmd = None - time.sleep(0.003) - if self.run_count == 3 and self.is_touch == True and self.touch_type == 1 and self.touch_pub.get_subscription_count() > 0: - """单点式压力传感器""" - self.force = self.api.get_force() - if self.is_touch == True and self.touch_type > 1 and (self.matrix_touch_pub.get_subscription_count() > 0 or self.matrix_touch_mass_pub.get_subscription_count() > 0 or self.matrix_touch_pub_pc.get_subscription_count() > 0): - """矩阵式压力传感器""" - if self.run_count == 3: - self.matrix_dic["thumb_matrix"] = self.api.get_thumb_matrix_touch(sleep_time=self.sleep_time).tolist() - if self.run_count == 4: - self.matrix_dic["index_matrix"] = self.api.get_index_matrix_touch(sleep_time=self.sleep_time).tolist() - if self.run_count == 5: - self.matrix_dic["middle_matrix"] = self.api.get_middle_matrix_touch(sleep_time=self.sleep_time).tolist() - if self.run_count == 6: - self.matrix_dic["ring_matrix"] = self.api.get_ring_matrix_touch(sleep_time=self.sleep_time).tolist() - if self.run_count == 7: - self.matrix_dic["little_matrix"] = self.api.get_little_matrix_touch(sleep_time=self.sleep_time).tolist() - time.sleep(0.005) - if self.run_count == 8 and self.hand_info_pub.get_subscription_count() > 0: - """手部信息""" - self.last_hand_info = { - "version": self.embedded_version, # Dexterous hand version number - "hand_joint": self.hand_joint, # Dexterous hand joint type - "speed": self.api.get_speed(), # Current speed threshold of the dexterous hand - "current": self.api.get_current(), # Current of the dexterous hand - "fault": self.api.get_fault(), # Current fault of the dexterous hand - "motor_temperature": self.api.get_temperature(), # Current motor temperature of the dexterous hand - "torque": self.api.get_torque(), # Current torque of the dexterous hand - "is_touch":self.is_touch, - "touch_type": self.touch_type, - "finger_order": self.api.get_finger_order() # Finger motor order - } - if self.run_count == 9: - self.run_count = 0 - self.run_count += 1 - time.sleep(0.003) - - - def pub_state(self): - while True: - if self.hand_state_pub.get_subscription_count() > 0: - msg = self.joint_state_msg(self.last_hand_state, self.last_hand_vel) - self.hand_state_pub.publish(msg) - if self.is_touch == True and self.touch_type == 1 and self.touch_pub.get_subscription_count() > 0: - msg = Float32MultiArray() - msg.data = [float(val) for sublist in self.force for val in sublist] - self.touch_pub.publish(msg) - if self.is_touch == True and self.touch_type > 1 and (self.matrix_touch_pub.get_subscription_count() > 0 or self.matrix_touch_mass_pub.get_subscription_count() > 0 or self.matrix_touch_pub_pc.get_subscription_count() > 0): - # 发布矩阵压感数据JSON格式 - self.pub_matrix_dic() - # 发布矩阵压感和值JSON格式 - self.pub_matrix_mass(dic=self.matrix_dic) - # 发布矩阵压感点云格式 - self.pub_matrix_point_cloud() - if self.hand_info_pub.get_subscription_count() > 0: - msg = String() - msg.data = json.dumps(self.last_hand_info) - self.hand_info_pub.publish(msg) - time.sleep(self.hz) - - def pub_matrix_mass(self, dic): - """发布矩阵数据合值 单位g 克 JSON格式""" - msg = String() - # 获取当前的 ROS 时间 - current_time = self.stamp_clock.now() - # 提取 secs 和 nsecs - t_secs = current_time.to_msg().sec - t_nsecs = current_time.to_msg().nanosec - self.matrix_mass_dic["stamp"]["secs"] = t_secs - self.matrix_mass_dic["stamp"]["nsecs"] = t_nsecs - self.matrix_mass_dic["unit"] = "g" - self.matrix_mass_dic["thumb_mass"] = sum(sum(row) for row in dic["thumb_matrix"]) - self.matrix_mass_dic["index_mass"] = sum(sum(row) for row in dic["index_matrix"]) - self.matrix_mass_dic["middle_mass"] = sum(sum(row) for row in dic["middle_matrix"]) - self.matrix_mass_dic["ring_mass"] = sum(sum(row) for row in dic["ring_matrix"]) - self.matrix_mass_dic["little_mass"] = sum(sum(row) for row in dic["little_matrix"]) - msg.data = json.dumps(self.matrix_mass_dic) - self.matrix_touch_mass_pub.publish(msg) - - def pub_matrix_point_cloud(self): - """发布矩阵数据点云格式""" - tmp_dic = self.matrix_dic.copy() - del tmp_dic['stamp'] # 去掉时间戳字段 - all_matrices = list(tmp_dic.values()) # 5 帧，每帧 6×12=72 个数 - # 摊平到一维：360 个 float - flat_list = [v for frame in all_matrices for v in frame] # 360 - flat = np.concatenate([np.asarray(np.clip(c, 0, 255), dtype=np.uint8) for c in flat_list]) - fields = [PointField( - name='val', - offset=0, - datatype=PointField.UINT8, - count=1 - )] - pc = PointCloud2() - pc.header.stamp = self.stamp_clock.now().to_msg() - pc.header.frame_id = '' - pc.height = 1 - pc.width = flat.size # 360 - pc.fields = fields - pc.is_bigendian = False - pc.point_step = 1 # 1 个 float32 - pc.row_step = pc.point_step * pc.width - pc.data = flat.tobytes() # 1440 字节 - self.matrix_touch_pub_pc.publish(pc) - - def pub_matrix_dic(self): - """发布矩阵数据JSON格式""" - msg = String() - # 获取当前的 ROS 时间 - current_time = self.stamp_clock.now() - # 提取 secs 和 nsecs - t_secs = current_time.to_msg().sec - t_nsecs = current_time.to_msg().nanosec - self.matrix_dic["stamp"]["secs"] = t_secs - self.matrix_dic["stamp"]["nsecs"] = t_nsecs - msg.data = json.dumps(self.matrix_dic) - self.matrix_touch_pub.publish(msg) - - def joint_state_msg(self, pose,vel=[]): - joint_state = JointState() - joint_state.header = Header() - joint_state.header.stamp = self.get_clock().now().to_msg() - joint_state.name = self.api.get_finger_order() - joint_state.position = [float(x) for x in pose] - if len(vel) > 1: - joint_state.velocity = [float(x) for x in vel] - else: - joint_state.velocity = [0.0] * len(pose) - joint_state.effort = [0.0] * len(pose) - return joint_state - - - - - - def hand_setting_cb(self,msg): - '''控制命令回调''' - data = json.loads(msg.data) - print(f"Received setting command: {data['setting_cmd']}",flush=True) - try: - if data["params"]["hand_type"] == "left": - hand = self.api - hand_left = True - elif data["params"]["hand_type"] == "right": - hand = self.api - hand_right = True - else: - print("Please specify the hand part to be set",flush=True) - return - self.cmd_lock = True - # Set maximum torque - if data["setting_cmd"] == "set_max_torque_limits": # Set maximum torque - torque = list(data["params"]["torque"]) - hand.set_torque(torque=torque) - - if data["setting_cmd"] == "set_speed": # Set speed - if isinstance(data["params"]["speed"], list) == True: - speed = data["params"]["speed"] - hand.set_speed(speed=speed) - else: - ColorMsg(msg=f"Speed parameter error, speed must be a list", color="red") - if data["setting_cmd"] == "clear_faults": # Clear faults - if hand_left == True and self.hand_joint == "L10" : - ColorMsg(msg=f"L10 left hand cannot clear faults") - elif hand_right == True and self.hand_joint == "L10" : - ColorMsg(msg=f"L10 right hand cannot clear faults") - else: - hand.clear_faults() - if data["setting_cmd"] == "get_faults": # Get faults - f = hand.get_fault() - ColorMsg(msg=f"Get faults: {f}") - if data["setting_cmd"] == "electric_current": # Get current - ColorMsg(msg=f"Get current: {hand.get_current()}") - if data["setting_cmd"] == "set_electric_current": # Set current - if isinstance(data["params"]["current"], list) == True: - hand.set_current(data["params"]["current"]) - if data["setting_cmd"] == "show_fun_table": # Get faults - f = hand.show_fun_table() - except: - print("命令参数错误") - self.cmd_lock = False - finally: - self.cmd_lock = False - - - def close_can(self): - self.api.open_can.close_can(can=self.can) - sys.exit(0) - - -def main(args=None): - try: - rclpy.init(args=args) - node = LinkerHand("linker_hand_sdk") - embedded_version = node.embedded_version - if len(embedded_version) == 3 or node.hand_joint.upper() == "O6" or node.hand_joint.upper() == "L6" or node.hand_joint.upper() == "G20": - ColorMsg(msg=f"New Matrix Touch For SDK V2", color="green") - node.sdk_v = 2 - elif len(embedded_version) == 6 and node.hand_joint == "L10": - ColorMsg(msg=f"New Matrix Touch For SDK V2", color="green") - node.sdk_v = 2 - elif len(embedded_version) > 4 and ((embedded_version[0]==10 and embedded_version[4]>35) or (embedded_version[0]==7 and embedded_version[4]>50) or (embedded_version[0] == 6)): - ColorMsg(msg=f"New Matrix Touch For SDK V2", color="green") - node.sdk_v = 2 - else: - ColorMsg(msg=f"SDK V1", color="green") - node.sdk_v = 1 - rclpy.spin(node) # 主循环，监听 ROS 回调 - except KeyboardInterrupt: - print("收到 Ctrl+C，准备退出...") - finally: - # node.close_can() # 关闭 CAN 或其他硬件资源 - # node.destroy_node() # 销毁 ROS 节点 - # rclpy.shutdown() # 关闭 ROS - print("程序已退出。")