From 85dbf428e93aba0573a84b7f2515438d6bb6b968 Mon Sep 17 00:00:00 2001 From: lxp <2770281812@qq.com> Date: Tue, 30 Jun 2026 13:39:48 +0800 Subject: [PATCH] =?UTF-8?q?=E6=A0=87=E5=AE=9A=E6=94=AF=E6=8C=81=E5=8D=95?= =?UTF-8?q?=E6=89=8B=E8=BF=9E=E6=8E=A5=E3=80=82=E5=8F=AA=E6=8E=A5=E5=8F=B3?= =?UTF-8?q?=E6=89=8B=E6=97=B6=E4=BC=9A=E8=B7=B3=E8=BF=87=E5=B7=A6=E6=89=8B?= =?UTF-8?q?=E9=87=87=E6=A0=B7?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .gitignore | 21 + .../launch/linker_hand.launch.py | 21 + .../launch/linker_hand_double.launch.py | 35 + src/linker_hand_ros2_sdk/launch/test.bak | 31 + .../LinkerHand/__init__.py | 0 .../LinkerHand/config/L10_positions.yaml | 146 ++ .../LinkerHand/config/L20_positions.yaml | 0 .../LinkerHand/config/L21_positions.yaml | 110 ++ .../LinkerHand/config/L25_positions.yaml | 83 ++ .../LinkerHand/config/L6_positions.yaml | 26 + .../LinkerHand/config/L7_positions.yaml | 29 + .../LinkerHand/config/O6_positions.yaml | 26 + .../LinkerHand/config/__init__.py | 0 .../LinkerHand/config/setting.yaml | 58 + .../LinkerHand/core/__init__.py | 0 .../LinkerHand/core/can/__init__.py | 0 .../core/can/linker_hand_g20_can.py | 1269 +++++++++++++++++ .../core/can/linker_hand_l10_can.py | 531 +++++++ .../core/can/linker_hand_l20_can.py | 478 +++++++ .../core/can/linker_hand_l21_can.py | 822 +++++++++++ .../core/can/linker_hand_l24_can.py | 448 ++++++ .../core/can/linker_hand_l25_can.py | 848 +++++++++++ .../LinkerHand/core/can/linker_hand_l6_can.py | 426 ++++++ .../LinkerHand/core/can/linker_hand_l7_can.py | 419 ++++++ .../LinkerHand/core/can/linker_hand_o6_can.py | 447 ++++++ .../core/rs485/linker_hand_l10_rs485.py | 345 +++++ .../core/rs485/linker_hand_l6_rs485.py | 460 ++++++ .../core/rs485/linker_hand_l6_rs485.py.bak | 444 ++++++ .../core/rs485/linker_hand_l7_rs485.py | 423 ++++++ .../core/rs485/linker_hand_o6_rs485.py | 671 +++++++++ .../core/rs485/linker_hand_o6_rs485.py.bak | 1157 +++++++++++++++ .../LinkerHand/linker_hand_api.py | 355 +++++ .../LinkerHand/utils/__init__.py | 0 .../LinkerHand/utils/color_msg.py | 27 + .../LinkerHand/utils/init_linker_hand.py | 81 ++ .../LinkerHand/utils/load_write_yaml.py | 101 ++ .../LinkerHand/utils/mapping.py | 383 +++++ .../LinkerHand/utils/open_can.py | 145 ++ .../linker_hand_ros2_sdk/__init__.py | 0 .../linker_hand_ros2_sdk/linker_hand.py | 421 ++++++ .../linker_hand_ros2_sdk/linker_hand.py.bak | 414 ++++++ .../linker_hand_advanced_g20.py | 258 ++++ .../linker_hand_advanced_l10.py | 246 ++++ .../linker_hand_advanced_l6.py | 251 ++++ .../linker_hand_advanced_l7.py | 281 ++++ .../linker_hand_advanced_o6.py | 252 ++++ .../linker_hand_g20_palm_touch.py | 268 ++++ src/linker_hand_ros2_sdk/package.xml | 21 + src/linker_hand_ros2_sdk/pyproject.toml | 3 + .../resource/linker_hand_ros2_sdk | 0 src/linker_hand_ros2_sdk/setup.cfg | 4 + src/linker_hand_ros2_sdk/setup.py | 50 + .../test/test_copyright.py | 25 + src/linker_hand_ros2_sdk/test/test_flake8.py | 25 + src/linker_hand_ros2_sdk/test/test_pep257.py | 23 + .../config/base_config.yml | 2 +- .../motion/linkerforce/retarget.py | 323 +++-- 57 files changed, 13619 insertions(+), 114 deletions(-) create mode 100644 .gitignore create mode 100644 src/linker_hand_ros2_sdk/launch/linker_hand.launch.py create mode 100644 src/linker_hand_ros2_sdk/launch/linker_hand_double.launch.py create mode 100644 src/linker_hand_ros2_sdk/launch/test.bak create mode 100644 src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/__init__.py create mode 100644 src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/config/L10_positions.yaml create mode 100644 src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/config/L20_positions.yaml create mode 100644 src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/config/L21_positions.yaml create mode 100644 src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/config/L25_positions.yaml create mode 100644 src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/config/L6_positions.yaml create mode 100644 src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/config/L7_positions.yaml create mode 100644 src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/config/O6_positions.yaml create mode 100644 src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/config/__init__.py create mode 100644 src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/config/setting.yaml create mode 100644 src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/core/__init__.py create mode 100644 src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/core/can/__init__.py create mode 100644 src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/core/can/linker_hand_g20_can.py create mode 100644 src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/core/can/linker_hand_l10_can.py create mode 100644 src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/core/can/linker_hand_l20_can.py create mode 100644 src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/core/can/linker_hand_l21_can.py create mode 100644 src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/core/can/linker_hand_l24_can.py create mode 100644 src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/core/can/linker_hand_l25_can.py create mode 100644 src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/core/can/linker_hand_l6_can.py create mode 100644 src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/core/can/linker_hand_l7_can.py create mode 100644 src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/core/can/linker_hand_o6_can.py create mode 100644 src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/core/rs485/linker_hand_l10_rs485.py create mode 100644 src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/core/rs485/linker_hand_l6_rs485.py create mode 100644 src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/core/rs485/linker_hand_l6_rs485.py.bak create mode 100644 src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/core/rs485/linker_hand_l7_rs485.py create mode 100644 src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/core/rs485/linker_hand_o6_rs485.py create mode 100644 src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/core/rs485/linker_hand_o6_rs485.py.bak create mode 100644 src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/linker_hand_api.py create mode 100644 src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/utils/__init__.py create mode 100644 src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/utils/color_msg.py create mode 100644 src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/utils/init_linker_hand.py create mode 100644 src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/utils/load_write_yaml.py create mode 100644 src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/utils/mapping.py create mode 100644 src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/utils/open_can.py create mode 100644 src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/__init__.py create mode 100644 src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/linker_hand.py create mode 100644 src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/linker_hand.py.bak create mode 100644 src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/linker_hand_advanced_g20.py create mode 100644 src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/linker_hand_advanced_l10.py create mode 100644 src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/linker_hand_advanced_l6.py create mode 100644 src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/linker_hand_advanced_l7.py create mode 100644 src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/linker_hand_advanced_o6.py create mode 100644 src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/linker_hand_g20_palm_touch.py create mode 100644 src/linker_hand_ros2_sdk/package.xml create mode 100644 src/linker_hand_ros2_sdk/pyproject.toml create mode 100644 src/linker_hand_ros2_sdk/resource/linker_hand_ros2_sdk create mode 100644 src/linker_hand_ros2_sdk/setup.cfg create mode 100644 src/linker_hand_ros2_sdk/setup.py create mode 100644 src/linker_hand_ros2_sdk/test/test_copyright.py create mode 100644 src/linker_hand_ros2_sdk/test/test_flake8.py create mode 100644 src/linker_hand_ros2_sdk/test/test_pep257.py diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..f7832d8 --- /dev/null +++ b/.gitignore @@ -0,0 +1,21 @@ +# ROS 2 / colcon build outputs +/build/ +/install/ +/log/ + +# Python bytecode and caches +__pycache__/ +*.py[cod] +*$py.class + +# Test and tooling caches +.pytest_cache/ +.coverage +htmlcov/ +.mypy_cache/ +.ruff_cache/ + +# Runtime and calibration scratch files +*.tmp +*.log + diff --git a/src/linker_hand_ros2_sdk/launch/linker_hand.launch.py b/src/linker_hand_ros2_sdk/launch/linker_hand.launch.py new file mode 100644 index 0000000..7c090a6 --- /dev/null +++ b/src/linker_hand_ros2_sdk/launch/linker_hand.launch.py @@ -0,0 +1,21 @@ +#!/usr/bin/env python3 +# -*- coding: utf-8 -*- +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', + output='screen', + parameters=[{ + 'hand_type': 'left', # 配置Linker Hand灵巧手类型 left | right 字母为小写 + 'hand_joint': "O6", # O6\L6P\L6\L7\L10\L20\G20(工业版)\L21 字母为大写 + 'is_touch': True, # 配置Linker Hand灵巧手是否有压力传感器 True | False + 'can': 'can0', # 这里需要修改为实际的CAN总线名称 如果是win系统则类似于 PCAN_USBBUS1。注：蓝色盒子为Linux下can0，WIN下位PCAN_USBBUS1。透明盒子Linux下为can0，WIN下为0 + "modbus": "None" # "None" | "/dev/ttyUSB0" 这里需要修改为实际的Modbus总线名称 如果是win系统则 COM* Ubuntu则为/dev/ttyUSB* 注意添加sudo chmod 777 /dev/ttyUSB*权限 + }], + ), + ]) diff --git a/src/linker_hand_ros2_sdk/launch/linker_hand_double.launch.py b/src/linker_hand_ros2_sdk/launch/linker_hand_double.launch.py new file mode 100644 index 0000000..4d81d46 --- /dev/null +++ b/src/linker_hand_ros2_sdk/launch/linker_hand_double.launch.py @@ -0,0 +1,35 @@ +#!/usr/bin/env python3 +# -*- coding: utf-8 -*- +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', # 配置Linker Hand灵巧手类型 left | right 字母为小写 + 'hand_joint': "G20", # O6\L6P\L6\L7\L10\L20\G20(工业版)\L21 字母为大写 + 'is_touch': True, # 配置Linker Hand灵巧手是否有压力传感器 True | False + 'can': 'can0', # 这里需要修改为实际的CAN总线名称 如果是win系统则类似于 PCAN_USBBUS1 + "modbus": "None" # "None" | "/dev/ttyUSB0" 这里需要修改为实际的Modbus总线名称 如果是win系统则 COM* Ubuntu则为/dev/ttyUSB* + }], + ), + + Node( + package='linker_hand_ros2_sdk', + executable='linker_hand_sdk', + name='linker_hand_sdk_right', + output='screen', + parameters=[{ + 'hand_type': 'right', # 配置Linker Hand灵巧手类型 left | right 字母为小写 + 'hand_joint': "G20", # O6\L6P\L6\L7\L10\L20\G20(工业版)\L21 字母为大写 + 'is_touch': True, # 配置Linker Hand灵巧手是否有压力传感器 True | False + 'can': 'can1', # 这里需要修改为实际的CAN总线名称 如果是win系统则类似于 PCAN_USBBUS1 + "modbus": "None" # "None" | "/dev/ttyUSB0" 这里需要修改为实际的Modbus总线名称 如果是win系统则 COM* Ubuntu则为/dev/ttyUSB* + }], + ), + ]) diff --git a/src/linker_hand_ros2_sdk/launch/test.bak b/src/linker_hand_ros2_sdk/launch/test.bak new file mode 100644 index 0000000..a4d8d10 --- /dev/null +++ b/src/linker_hand_ros2_sdk/launch/test.bak @@ -0,0 +1,31 @@ +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/__init__.py b/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/config/L10_positions.yaml b/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/config/L10_positions.yaml new file mode 100644 index 0000000..ea7439d --- /dev/null +++ b/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/config/L10_positions.yaml @@ -0,0 +1,146 @@ +LEFT_HAND: +- ACTION_NAME: 张开 + POSITION: + - 255 + - 70 + - 255 + - 255 + - 255 + - 255 + - 255 + - 255 + - 255 + - 255 +- ACTION_NAME: 捏合5CM + POSITION: + - 165 + - 70 + - 165 + - 165 + - 255 + - 255 + - 113 + - 255 + - 255 + - 88 +- ACTION_NAME: 捏合1CM + POSITION: + - 150 + - 70 + - 155 + - 155 + - 255 + - 255 + - 113 + - 255 + - 255 + - 88 +- ACTION_NAME: 握3CM物品 + POSITION: + - 113 + - 70 + - 85 + - 85 + - 85 + - 85 + - 85 + - 255 + - 255 + - 88 +- ACTION_NAME: 准备抓握 + POSITION: + - 255 + - 70 + - 255 + - 255 + - 255 + - 255 + - 255 + - 255 + - 255 + - 121 +- ACTION_NAME: 拇指弯曲 + POSITION: + - 35 + - 140 + - 255 + - 255 + - 255 + - 255 + - 255 + - 255 + - 255 + - 30 +- ACTION_NAME: 食指弯曲 + POSITION: + - 255 + - 70 + - 0 + - 255 + - 255 + - 255 + - 255 + - 255 + - 255 + - 255 +- ACTION_NAME: shishi + POSITION: + - 85 + - 30 + - 255 + - 0 + - 0 + - 255 + - 0 + - 0 + - 0 + - 66 +RIGHT_HAND: +- ACTION_NAME: 张开 + POSITION: + - 255 + - 70 + - 255 + - 255 + - 255 + - 255 + - 255 + - 255 + - 255 + - 255 +- ACTION_NAME: 捏合5CM + POSITION: + - 165 + - 70 + - 165 + - 165 + - 255 + - 255 + - 113 + - 255 + - 255 + - 88 +- ACTION_NAME: 捏合1CM + POSITION: + - 150 + - 70 + - 155 + - 155 + - 255 + - 255 + - 113 + - 255 + - 255 + - 88 +- ACTION_NAME: 准备抓握 + POSITION: + - 255 + - 70 + - 255 + - 255 + - 255 + - 255 + - 255 + - 255 + - 255 + - 121 diff --git a/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/config/L20_positions.yaml b/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/config/L20_positions.yaml new file mode 100644 index 0000000..e69de29 diff --git a/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/config/L21_positions.yaml b/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/config/L21_positions.yaml new file mode 100644 index 0000000..a23101c --- /dev/null +++ b/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/config/L21_positions.yaml @@ -0,0 +1,110 @@ +LEFT_HAND: +- ACTION_NAME: 张开 + POSITION: + - 96 + - 255 + - 255 + - 255 + - 255 + - 150 + - 114 + - 151 + - 189 + - 255 + - 180 + - 255 + - 255 + - 255 + - 255 + - 255 + - 255 + - 255 + - 255 + - 255 + - 255 + - 255 + - 255 + - 255 + - 255 +- ACTION_NAME: 握拳 + POSITION: + - 177 + - 0 + - 0 + - 0 + - 0 + - 51 + - 114 + - 151 + - 189 + - 255 + - 79 + - 255 + - 255 + - 255 + - 255 + - 131 + - 222 + - 244 + - 255 + - 255 + - 0 + - 0 + - 0 + - 0 + - 0 +RIGHT_HAND: +- ACTION_NAME: 张开 + POSITION: + - 96 + - 255 + - 255 + - 255 + - 255 + - 150 + - 114 + - 151 + - 189 + - 255 + - 180 + - 255 + - 255 + - 255 + - 255 + - 255 + - 255 + - 255 + - 255 + - 255 + - 255 + - 255 + - 255 + - 255 + - 255 +- ACTION_NAME: 握拳 + POSITION: + - 230 + - 80 + - 51 + - 42 + - 7 + - 35 + - 114 + - 151 + - 189 + - 255 + - 58 + - 255 + - 255 + - 255 + - 255 + - 133 + - 5 + - 0 + - 0 + - 0 + - 30 + - 0 + - 0 + - 0 + - 0 diff --git a/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/config/L25_positions.yaml b/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/config/L25_positions.yaml new file mode 100644 index 0000000..709b0c3 --- /dev/null +++ b/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/config/L25_positions.yaml @@ -0,0 +1,83 @@ +LEFT_HAND: +- ACTION_NAME: 张开 + POSITION: + - 96 + - 255 + - 255 + - 255 + - 255 + - 150 + - 114 + - 151 + - 189 + - 255 + - 180 + - 255 + - 255 + - 255 + - 255 + - 255 + - 255 + - 255 + - 255 + - 255 + - 255 + - 255 + - 255 + - 255 + - 255 +RIGHT_HAND: +- ACTION_NAME: 张开 + POSITION: + - 96 + - 255 + - 255 + - 255 + - 255 + - 150 + - 114 + - 151 + - 189 + - 255 + - 180 + - 255 + - 255 + - 255 + - 255 + - 255 + - 255 + - 255 + - 255 + - 255 + - 255 + - 255 + - 255 + - 255 + - 255 +- ACTION_NAME: 握拳 + POSITION: + - 230 + - 80 + - 51 + - 42 + - 7 + - 35 + - 114 + - 151 + - 189 + - 255 + - 58 + - 255 + - 255 + - 255 + - 255 + - 133 + - 5 + - 0 + - 0 + - 0 + - 30 + - 0 + - 0 + - 0 + - 0 diff --git a/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/config/L6_positions.yaml b/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/config/L6_positions.yaml new file mode 100644 index 0000000..9caaf4a --- /dev/null +++ b/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/config/L6_positions.yaml @@ -0,0 +1,26 @@ +LEFT_HAND: +- ACTION_NAME: 握拳 + POSITION: + - 67 + - 151 + - 0 + - 0 + - 0 + - 0 +- ACTION_NAME: 张开 + POSITION: + - 255 + - 179 + - 255 + - 255 + - 255 + - 255 +RIGHT_HAND: +- ACTION_NAME: 张开 + POSITION: + - 255 + - 70 + - 255 + - 255 + - 255 + - 255 diff --git a/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/config/L7_positions.yaml b/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/config/L7_positions.yaml new file mode 100644 index 0000000..d573967 --- /dev/null +++ b/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/config/L7_positions.yaml @@ -0,0 +1,29 @@ +LEFT_HAND: +- ACTION_NAME: 握拳 + POSITION: + - 67 + - 151 + - 0 + - 0 + - 0 + - 0 + - 37 +- ACTION_NAME: 张开 + POSITION: + - 255 + - 179 + - 255 + - 255 + - 255 + - 255 + - 83 +RIGHT_HAND: +- ACTION_NAME: 张开 + POSITION: + - 255 + - 70 + - 255 + - 255 + - 255 + - 255 + - 255 diff --git a/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/config/O6_positions.yaml b/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/config/O6_positions.yaml new file mode 100644 index 0000000..9caaf4a --- /dev/null +++ b/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/config/O6_positions.yaml @@ -0,0 +1,26 @@ +LEFT_HAND: +- ACTION_NAME: 握拳 + POSITION: + - 67 + - 151 + - 0 + - 0 + - 0 + - 0 +- ACTION_NAME: 张开 + POSITION: + - 255 + - 179 + - 255 + - 255 + - 255 + - 255 +RIGHT_HAND: +- ACTION_NAME: 张开 + POSITION: + - 255 + - 70 + - 255 + - 255 + - 255 + - 255 diff --git a/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/config/__init__.py b/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/config/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/config/setting.yaml b/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/config/setting.yaml new file mode 100644 index 0000000..76f6860 --- /dev/null +++ b/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/config/setting.yaml @@ -0,0 +1,58 @@ +VERSION: 3.1.1 # 支持O6、L6在RS485模式 +LINKER_HAND: # 手部配置信息 + LEFT_HAND: + EXISTS: True # 是否存在左手 + TOUCH: True # 是否有压力传感器 + CAN: "can0" # 配置CAN端口 默认can0 如果MODUBS不为"None",则CAN配置失效。 如果是win系统则类似于 PCAN_USBBUS1。注：蓝色盒子为Linux下can0，WIN下位PCAN_USBBUS1。透明盒子Linux下为can0，WIN下为0 + MODBUS: "None" # 通讯协议是否为485 默认None 如果启动485，则是设备端口 /dev/ttyUSB* CAN配置失效 当前只支持O6/L6.后续版本正在努力增加中 + JOINT: L6 # 左手型号 O6/L6/L7/L10/L20/G20/L21/L25/ + NAME: # 默认值，不用修改 + - joint41 + - joint42 + - joint43 + - joint44 + - joint45 + - joint46 + - joint47 + - joint48 + - joint49 + - joint50 + - joint51 + - joint52 + - joint53 + - joint54 + - joint55 + - joint56 + - joint57 + - joint58 + - joint59 + - joint60 + + RIGHT_HAND: + EXISTS: False # 是否存在右手 + TOUCH: False # 是否有压力传感器 + CAN: "can0" # 配置CAN端口 默认can0 如果MODUBS不为"None",则CAN配置失效。 如果是win系统则类似于 PCAN_USBBUS1。注：蓝色盒子为Linux下can0，WIN下位PCAN_USBBUS1。透明盒子Linux下为can0，WIN下为0 + MODBUS: "None" # 通讯协议是否为485 默认None 如果启动485，则是设备端口 /dev/ttyUSB* CAN配置失效 当前只支持O6/L6.后续版本正在努力增加中 + JOINT: L6 # 右手型号 O6/L6/L7/L10/L20/G20/L21/L25/ + NAME: # 默认值，不用修改 + - joint71 + - joint72 + - joint73 + - joint77 + - joint75 + - joint76 + - joint77 + - joint78 + - joint79 + - joint80 + - joint81 + - joint82 + - joint83 + - joint84 + - joint88 + - joint86 + - joint87 + - joint88 + - joint89 + - joint90 +PASSWORD: "12345678" # 由于与can通讯，需要激活通讯接口用到系统管理员密码。只有Linux系统需要，windows系统不需要。RS485不需要修改，RS485需要给/dev/ttyUSB* 777权限 diff --git a/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/core/__init__.py b/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/core/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/core/can/__init__.py b/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/core/can/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/core/can/linker_hand_g20_can.py b/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/core/can/linker_hand_g20_can.py new file mode 100644 index 0000000..9553aed --- /dev/null +++ b/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/core/can/linker_hand_g20_can.py @@ -0,0 +1,1269 @@ +#!/usr/bin/env python3 +import can +import time, sys, os +import threading +import numpy as np +from enum import Enum +from utils.open_can import OpenCan +from can.exceptions import CanError +from utils.color_msg import ColorMsg +current_dir = os.path.dirname(os.path.abspath(__file__)) +target_dir = os.path.abspath(os.path.join(current_dir, "..")) +sys.path.append(target_dir) +""" +拇指41: [拇指侧摆, 拇指横摆, 拇指根部, 预留, 预留, 拇指尖部] +食指42: [食指侧摆, 预留, 食指根部, 预留, 预留, 食指末端] +中指43: [中指侧摆, 预留, 中指根部, 预留, 预留, 中指末端] +无名指44: [无名指侧摆, 预留, 无名指根部, 预留, 预留, 无名指末端] +小指45: [小指侧摆, 预留, 小指根部, 预留, 预留, 小指末端] +""" +CMD_MAP = [ + "拇指根部", + "食指根部", + "中指根部", + "无名指根部", + "小指根部", + "拇指侧摆", + "食指侧摆", + "中指侧摆", + "无名指侧摆", + "小指侧摆", + "拇指横摆", + "预留", + "预留", + "预留", + "预留", + "拇指尖部", + "食指末端", + "中指末端", + "无名指末端", + "小指末端" +] + +class FrameProperty(Enum): + # 手指运动控制 - 并联型控制指令（控制所有手指同一关节） + ROLL_POS = 0x01 # 横滚关节位置 + YAW_POS = 0x02 # 航向关节位置 + ROOT1_POS = 0x03 # 指根1关节位置 + ROOT2_POS = 0x04 # 指根2关节位置 + ROOT3_POS = 0x05 # 指根3关节位置 + TIP_POS = 0x06 # 指尖关节位置 + + # 关节速度指令 + ROLL_SPEED = 0x09 # 横滚关节速度 + YAW_SPEED = 0x0A # 航向关节速度 + ROOT1_SPEED = 0x0B # 指根1关节速度 + ROOT2_SPEED = 0x0C # 指根2关节速度 + ROOT3_SPEED = 0x0D # 指根3关节速度 + TIP_SPEED = 0x0E # 指尖关节速度 + + # 关节扭矩指令 + ROLL_TORQUE = 0x11 # 横滚关节扭矩 + YAW_TORQUE = 0x12 # 航向关节扭矩 + ROOT1_TORQUE = 0x13 # 指根1关节扭矩 + ROOT2_TORQUE = 0x14 # 指根2关节扭矩 + ROOT3_TORQUE = 0x15 # 指根3关节扭矩 + TIP_TORQUE = 0x16 # 指尖关节扭矩 + + # 关节故障码 + ROLL_FAULT = 0x19 # 横滚关节故障码 + YAW_FAULT = 0x1A # 航向关节故障码 + ROOT1_FAULT = 0x1B # 指根1关节故障码 + ROOT2_FAULT = 0x1C # 指根2关节故障码 + ROOT3_FAULT = 0x1D # 指根3关节故障码 + TIP_FAULT = 0x1E # 指尖关节故障码 + + # 关节温度 + ROLL_TEMPERATURE = 0x21 # 横滚关节过温保护阈值 + YAW_TEMPERATURE = 0x22 # 航向关节过温保护阈值 + ROOT1_TEMPERATURE = 0x23 # 指根1关节过温保护阈值 + ROOT2_TEMPERATURE = 0x24 # 指根2关节过温保护阈值 + ROOT3_TEMPERATURE = 0x25 # 指根3关节过温保护阈值 + TIP_TEMPERATURE = 0x26 # 指尖关节过温保护阈值 + + # 手指运动控制 - 串联型控制指令（控制同一手指所有关节） + THUMB_POS = 0x41 # 大拇指指关节位置 + INDEX_POS = 0x42 # 食指关节位置 + MIDDLE_POS = 0x43 # 中指关节位置 + RING_POS = 0x44 # 无名指关节位置 + LITTLE_POS = 0x45 # 小拇指关节位置 + + # 手指速度 + THUMB_SPEED = 0x49 # 大拇指速度 + INDEX_SPEED = 0x4A # 食指速度 + MIDDLE_SPEED = 0x4B # 中指速度 + RING_SPEED = 0x4C # 无名指速度 + LITTLE_SPEED = 0x4D # 小拇指速度 + + # 手指扭矩 + THUMB_TORQUE = 0x51 # 大拇指扭矩 + INDEX_TORQUE = 0x52 # 食指扭矩 + MIDDLE_TORQUE = 0x53 # 中指扭矩 + RING_TORQUE = 0x54 # 无名指扭矩 + LITTLE_TORQUE = 0x55 # 小拇指扭矩 + + # 手指故障码 + THUMB_FAULT = 0x59 # 大拇指故障码 + INDEX_FAULT = 0x5A # 食指故障码 + MIDDLE_FAULT = 0x5B # 中指故障码 + RING_FAULT = 0x5C # 无名指故障码 + LITTLE_FAULT = 0x5D # 小拇指故障码 + + # 手指温度 + THUMB_TEMPERATURE = 0x61 # 大拇指过温保护阈值 + INDEX_TEMPERATURE = 0x62 # 食指过温保护阈值 + MIDDLE_TEMPERATURE = 0x63 # 中指过温保护阈值 + RING_TEMPERATURE = 0x64 # 无名指过温保护阈值 + LITTLE_TEMPERATURE = 0x65 # 小拇指过温保护阈值 + + # 手指运动控制 - 合并指令区域 + FINGER_SPEED = 0x81 # 设置手指速度 + FINGER_TORQUE = 0x82 # 设置手指输出扭矩 + FINGER_FAULT = 0x83 # 清除手指故障及故障码 + FINGER_TEMPERATURE = 0x84 # 手指各关节温度 + + # 指尖传感器数据 + HAND_NORMAL_FORCE = 0x90 # 五指法向压力 + HAND_TANGENTIAL_FORCE = 0x91 # 五指切向压力 + HAND_TANGENTIAL_FORCE_DIR = 0x92 # 五指切向方向 + HAND_APPROACH_INC = 0x93 # 五指接近感应 + + # 手指所有数据 + THUMB_ALL_DATA = 0x98 # 大拇指所有数据 + INDEX_ALL_DATA = 0x99 # 食指所有数据 + MIDDLE_ALL_DATA = 0x9A # 中指所有数据 + RING_ALL_DATA = 0x9B # 无名指所有数据 + LITTLE_ALL_DATA = 0x9C # 小拇指所有数据 + + # 触觉传感器 + TOUCH_SENSOR_TYPE = 0xB0 # 触觉传感器类型 + THUMB_TOUCH = 0xB1 # 大拇指触觉传感 + INDEX_TOUCH = 0xB2 # 食指触觉传感 + MIDDLE_TOUCH = 0xB3 # 中指触觉传感 + RING_TOUCH = 0xB4 # 无名指触觉传感 + LITTLE_TOUCH = 0xB5 # 小拇指触觉传感 + PALM_TOUCH = 0xB6 # 手掌指触觉传感 + + # 查询指令 + HAND_UID_GET = 0xC0 # 唯一标识码查询 + HAND_HARDWARE_VERSION_GET = 0xC1 # 硬件版本查询 + HAND_SOFTWARE_VERSION_GET = 0xC2 # 软件版本查询 + HAND_COMM_ID_GET = 0xC3 # 设备id查询 + HAND_STRUCT_VERSION_GET = 0xC4 # 结构版本号查询 + + # 出厂指令 + HOST_CMD_HAND_ERASE_POS_CALI = 0xCD # 擦除位置校准值 + HAND_COMM_ID_SET = 0xD1 # 通信ID设置 + HAND_UID_SET = 0xF0 # 唯一标识码设置 + +class LinkerHandG20Can: + def __init__(self, can_channel='can0', baudrate=1000000, can_id=0x28, yaml=""): + self.can_id = can_id + self.can_channel = can_channel + self.baudrate = baudrate + self.open_can = OpenCan(load_yaml=yaml) + + self.running = True + + # 初始化数据存储变量 + self.last_thumb_pos, self.last_index_pos, self.last_ring_pos, self.last_middle_pos, self.last_little_pos = None, None, None, None, None + self.last_root1, self.last_yaw, self.last_roll, self.last_root2, self.last_tip = None, None, None, None, None + + # 并联控制数据存储 + self.x01, self.x02, self.x03, self.x04, self.x05, self.x06 = [], [], [], [], [], [] + self.x09, self.x0A, self.x0B, self.x0C, self.x0D, self.x0E = [], [], [], [], [], [] + self.x11, self.x12, self.x13, self.x14, self.x15, self.x16 = [], [], [], [], [], [] + self.x19, self.x1A, self.x1B, self.x1C, self.x1D, self.x1E = [], [], [], [], [], [] + self.x21, self.x22, self.x23, self.x24, self.x25, self.x26 = [], [], [], [], [], [] + + # 串联控制数据存储 + self.x41, self.x42, self.x43, self.x44, self.x45 = [], [], [], [], [] + self.x49, self.x4A, self.x4B, self.x4C, self.x4D = [0] * 6, [0] * 6, [0] * 6, [0] * 6, [0] * 6 + self.x51, self.x52, self.x53, self.x54, self.x55 = [], [], [], [], [] + self.x59, self.x5A, self.x5B, self.x5C, self.x5D = [], [], [], [], [] + self.x61, self.x62, self.x63, self.x64, self.x65 = [], [], [], [], [] + + # 合并指令区域数据存储 + self.x81, self.x82, self.x83, self.x84 = [], [], [], [] + + # 传感器数据存储 + self.x90, self.x91, self.x92, self.x93 = [], [], [], [] + self.x98, self.x99, self.x9A, self.x9B, self.x9C = [], [], [], [], [] + self.xB0, self.xB1, self.xB2, self.xB3, self.xB4, self.xB5, self.xB6 = [], [], [], [], [], [], [] + self.normal_force, self.tangential_force, self.tangential_force_dir, self.approach_inc = [[-1] * 5 for _ in range(4)] + + # 查询指令数据存储 + self.xC0, self.xC1, self.xC2, self.xC3, self.xC4 = [], [], [], [], [] + self.serial_number = [] + self.serial_number_map = { + 0: 0, + 1: 1, + 2: 2, + 3: 3, + } + # 触觉传感器矩阵数据 + self.thumb_matrix = np.full((12, 6), -1) + self.index_matrix = np.full((12, 6), -1) + self.middle_matrix = np.full((12, 6), -1) + self.ring_matrix = np.full((12, 6), -1) + self.little_matrix = np.full((12, 6), -1) + self.matrix_map = { + 0: 0, 16: 1, 32: 2, 48: 3, 64: 4, 80: 5, + 96: 6, 112: 7, 128: 8, 144: 9, 160: 10, 176: 11, + } + # 全掌触觉数据缓存 + self.thumb_matrix_palm = np.full((23, 9), -1) + self.thumb_matrix_palm_tmp = [] + self.thumb_matrix_palm_mass = [-1, -1, -1] + + self.index_matrix_palm = np.full((23, 9), -1) + self.index_matrix_palm_tmp = [] + self.index_matrix_palm_mass = [-1, -1, -1] + + self.middle_matrix_palm = np.full((23, 9), -1) + self.middle_matrix_palm_tmp = [] + self.middle_matrix_palm_mass = [-1, -1, -1] + + self.ring_matrix_palm = np.full((23, 9), -1) + self.ring_matrix_palm_tmp = [] + self.ring_matrix_palm_mass = [-1, -1, -1] + + self.little_matrix_palm = np.full((23, 9), -1) + self.little_matrix_palm_tmp = [] + self.little_matrix_palm_mass = [-1, -1, -1] + + self.palm_matrix_palm = np.full((28, 20), -1) + self.palm_matrix_palm_tmp = [] + self.palm_matrix_palm_mass = [-1, -1] + + self.bus = self.init_can_bus(channel=self.can_channel, baudrate=baudrate) + # 启动接收线程 + self.receive_thread = threading.Thread(target=self.receive_response) + self.receive_thread.daemon = True + self.receive_thread.start() + self._check_touch_type() + + self.xB0 = self.get_touch_sensor_type() # 获取触觉传感器类型,如果返回值为5：TSSP_JZG(全手掌，指尖11x9,指中6x9,指根6x9,数据以23行9列形式返回，五指各有3个合力值。手掌20x28，手掌有两个合力值，分为上掌上半部分和下半部分。) + + def _check_touch_type(self): + '''根据SN编码判断压感类型''' + self.sn = self.get_serial_number() + time.sleep(0.1) + if self.sn != "-1": + parts = self.sn.split("-") + if parts[4] == "A": + self.touch_type = 1 + elif parts[4] == "B": + self.touch_type = 2 + self.touch_code = 0xC6 # 6*12 + elif parts[4] == "J": + self.touch_type = 3 + elif parts[4] == "F": + self.touch_type = 4 + self.touch_code = 0xA4 # 4*10 + elif parts[4] == "Z": + self.touch_type = -1 + else: + # 如果没有SN编码则根据返回数据进行判断 + self.touch_type = self.get_touch_type() + + def init_can_bus(self, channel, baudrate): + """ + 尝试按优先级连接 CAN 总线，并实现回退机制。 + """ + # --- 统一异常处理块开始 --- + try: + if sys.platform == "linux": + # Linux 优先级：1. socketcan + try: + self.open_can.open_can(self.can_channel) + # 尝试 socketcan + bus = can.interface.Bus(channel=channel, interface="socketcan", bitrate=baudrate) + ColorMsg(msg=f"成功连接: interface='socketcan', channel='{channel}'", color="green") + return bus + except CanError as e: + # 如果 socketcan 失败，可以考虑在这里尝试其他 Linux 接口 (如 'pcan') + ColorMsg(msg=f"socketcan 接口连接失败: {e}", color="yellow") + raise # 重新抛出异常，让外层 try 捕获 + elif sys.platform == "win32": + # Windows 优先级：1. pcan + try: + bus = can.interface.Bus(channel=channel, interface='pcan', bitrate=baudrate) + ColorMsg(msg=f"成功连接: interface='pcan', channel='{channel}'", color="green") + return bus + except CanError as e: + ColorMsg(msg=f"pcan 接口连接失败，尝试回退到 'candle': {e}", color="yellow") + # Windows 优先级：2. candle (回退方法) + try: + bus = can.Bus(interface="candle", channel=channel, bitrate=baudrate) + ColorMsg(msg=f"成功连接: interface='candle', channel='{channel}'", color="green") + return bus + except CanError as e: + ColorMsg(msg=f"candle 接口连接失败: {e}", color="yellow") + raise # 两个接口都失败，抛出异常 + else: + raise EnvironmentError("Unsupported platform for CAN interface") + # --- 统一异常处理块结束 --- + except Exception as e: + # 如果任何一个接口尝试失败并抛出异常（包括 EnvironmentError） + ColorMsg(msg=f"致命错误：所有 CAN 接口连接尝试均失败或平台不受支持。请检查设备连接或驱动安装和配置文件中CAN参数的配置。\n错误详情: {e}", color="red") + # 保持 raise 动作，将错误信息传递给调用者，避免程序继续运行 + raise + + def send_command(self, frame_property, data_list, sleep_time=0.003): + """ + 发送指令到CAN总线 + :param frame_property: 数据帧属性 + :param data_list: 数据载荷 + """ + frame_property_value = int(frame_property.value) if hasattr(frame_property, 'value') else frame_property + data = [frame_property_value] + [int(val) for val in data_list] + msg = can.Message(arbitration_id=self.can_id, data=data, is_extended_id=False) + try: + self.bus.send(msg) + except can.CanError as e: + print(f"Failed to send message: {e}") + self.open_can.open_can(self.can_channel) + time.sleep(1) + self.is_can = self.open_can.is_can_up_sysfs(interface=self.can_channel) + time.sleep(1) + if self.is_can: + self.bus = can.interface.Bus(channel=self.can_channel, interface="socketcan", bitrate=self.baudrate) + else: + print("Reconnecting CAN devices ....") + time.sleep(sleep_time) + + def receive_response(self): + """ + 接收并处理CAN总线响应消息 + """ + while self.running: + try: + msg = self.bus.recv(timeout=1.0) + if msg: + self.process_response(msg) + except can.CanError as e: + print(f"Error receiving message: {e}") + + def process_response(self, msg): + """ + 处理CAN响应消息 + """ + if msg.arbitration_id == self.can_id: + frame_type = msg.data[0] + response_data = msg.data[1:] + if len(list(response_data)) == 0: + return + # 并联控制指令响应 + if frame_type == 0x01: self.x01 = list(response_data) + elif frame_type == 0x02: self.x02 = list(response_data) + elif frame_type == 0x03: self.x03 = list(response_data) + elif frame_type == 0x04: self.x04 = list(response_data) + elif frame_type == 0x05: self.x05 = list(response_data) + elif frame_type == 0x06: self.x06 = list(response_data) + elif frame_type == 0x09: self.x09 = list(response_data) + elif frame_type == 0x0A: self.x0A = list(response_data) + elif frame_type == 0x0B: self.x0B = list(response_data) + elif frame_type == 0x0C: self.x0C = list(response_data) + elif frame_type == 0x0D: self.x0D = list(response_data) + elif frame_type == 0x0E: self.x0E = list(response_data) + elif frame_type == 0x11: self.x11 = list(response_data) + elif frame_type == 0x12: self.x12 = list(response_data) + elif frame_type == 0x13: self.x13 = list(response_data) + elif frame_type == 0x14: self.x14 = list(response_data) + elif frame_type == 0x15: self.x15 = list(response_data) + elif frame_type == 0x16: self.x16 = list(response_data) + elif frame_type == 0x19: self.x19 = list(response_data) + elif frame_type == 0x1A: self.x1A = list(response_data) + elif frame_type == 0x1B: self.x1B = list(response_data) + elif frame_type == 0x1C: self.x1C = list(response_data) + elif frame_type == 0x1D: self.x1D = list(response_data) + elif frame_type == 0x1E: self.x1E = list(response_data) + elif frame_type == 0x21: self.x21 = list(response_data) + elif frame_type == 0x22: self.x22 = list(response_data) + elif frame_type == 0x23: self.x23 = list(response_data) + elif frame_type == 0x24: self.x24 = list(response_data) + elif frame_type == 0x25: self.x25 = list(response_data) + elif frame_type == 0x26: self.x26 = list(response_data) + + # 串联控制指令响应 + elif frame_type == 0x41: self.x41 = list(response_data) + elif frame_type == 0x42: self.x42 = list(response_data) + elif frame_type == 0x43: self.x43 = list(response_data) + elif frame_type == 0x44: self.x44 = list(response_data) + elif frame_type == 0x45: self.x45 = list(response_data) + elif frame_type == 0x49: self.x49 = list(response_data) + elif frame_type == 0x4A: self.x4A = list(response_data) + elif frame_type == 0x4B: self.x4B = list(response_data) + elif frame_type == 0x4C: self.x4C = list(response_data) + elif frame_type == 0x4D: self.x4D = list(response_data) + elif frame_type == 0x51: self.x51 = list(response_data) + elif frame_type == 0x52: self.x52 = list(response_data) + elif frame_type == 0x53: self.x53 = list(response_data) + elif frame_type == 0x54: self.x54 = list(response_data) + elif frame_type == 0x55: self.x55 = list(response_data) + elif frame_type == 0x59: self.x59 = list(response_data) + elif frame_type == 0x5A: self.x5A = list(response_data) + elif frame_type == 0x5B: self.x5B = list(response_data) + elif frame_type == 0x5C: self.x5C = list(response_data) + elif frame_type == 0x5D: self.x5D = list(response_data) + elif frame_type == 0x61: self.x61 = list(response_data) + elif frame_type == 0x62: self.x62 = list(response_data) + elif frame_type == 0x63: self.x63 = list(response_data) + elif frame_type == 0x64: self.x64 = list(response_data) + elif frame_type == 0x65: self.x65 = list(response_data) + + # 合并指令区域响应 + elif frame_type == 0x81: self.x81 = list(response_data) + elif frame_type == 0x82: self.x82 = list(response_data) + elif frame_type == 0x83: self.x83 = list(response_data) + elif frame_type == 0x84: self.x84 = list(response_data) + + # 传感器数据响应 + elif frame_type == 0x90: self.x90 = list(response_data) + elif frame_type == 0x91: self.x91 = list(response_data) + elif frame_type == 0x92: self.x92 = list(response_data) + elif frame_type == 0x93: self.x93 = list(response_data) + elif frame_type == 0x98: self.x98 = list(response_data) + elif frame_type == 0x99: self.x99 = list(response_data) + elif frame_type == 0x9A: self.x9A = list(response_data) + elif frame_type == 0x9B: self.x9B = list(response_data) + elif frame_type == 0x9C: self.x9C = list(response_data) + + # 触觉传感器响应 + elif frame_type == 0xB0: self.xB0 = list(response_data) + elif frame_type == 0xB1: + d = list(response_data) + if self.xB0[0] == 5: + """全掌矩阵""" + self.thumb_matrix_palm_tmp.append(d) # 将返回帧存储到缓存 + if len(d) == 4 and d[0] == 22 and d[1] == 7: # 如果是最后一帧 + self.thumb_matrix_palm = self.build_matrix(self.thumb_matrix_palm_tmp) # 将帧数据排列成23行9列矩阵 + self.thumb_matrix_palm_tmp=[] # 重置缓存数据 + if len(d) == 7 and d[0] == 255: + self.thumb_matrix_palm_mass = self.build_matrix_mass(d) # [指尖合力值, 指中合力值, 指根合力值] + else: + if len(d) == 2: + self.xB1 = d + elif len(d) == 7: + index = self.matrix_map.get(d[0]) + if index is not None: + self.thumb_matrix[index] = d[1:] + + elif frame_type == 0xB2: + d = list(response_data) + if self.xB0[0] == 5: + """全掌矩阵""" + self.index_matrix_palm_tmp.append(d) # 将返回帧存储到缓存 + if len(d) == 4 and d[0] == 22 and d[1] == 7: # 如果是最后一帧 + self.index_matrix_palm = self.build_matrix(self.index_matrix_palm_tmp) # 将帧数据排列成23行9列矩阵 + self.index_matrix_palm_tmp=[] # 重置缓存数据 + if len(d) == 7 and d[0] == 255: + self.index_matrix_palm_mass = self.build_matrix_mass(d) # [指尖合力值, 指中合力值, 指根合力值] + else: + if len(d) == 2: + self.xB2 = d + elif len(d) == 7: + index = self.matrix_map.get(d[0]) + if index is not None: + self.index_matrix[index] = d[1:] + elif frame_type == 0xB3: + d = list(response_data) + if self.xB0[0] == 5: + """全掌矩阵""" + self.middle_matrix_palm_tmp.append(d) # 将返回帧存储到缓存 + if len(d) == 4 and d[0] == 22 and d[1] == 7: # 如果是最后一帧 + self.middle_matrix_palm = self.build_matrix(self.middle_matrix_palm_tmp) # 将帧数据排列成23行9列矩阵 + self.middle_matrix_palm_tmp=[] # 重置缓存数据 + if len(d) == 7 and d[0] == 255: + self.middle_matrix_palm_mass = self.build_matrix_mass(d) # [指尖合力值, 指中合力值, 指根合力值] + else: + if len(d) == 2: + self.xB3 = d + elif len(d) == 7: + index = self.matrix_map.get(d[0]) + if index is not None: + self.middle_matrix[index] = d[1:] + elif frame_type == 0xB4: + d = list(response_data) + if self.xB0[0] == 5: + """全掌矩阵""" + self.ring_matrix_palm_tmp.append(d) # 将返回帧存储到缓存 + if len(d) == 4 and d[0] == 22 and d[1] == 7: # 如果是最后一帧 + self.ring_matrix_palm = self.build_matrix(self.ring_matrix_palm_tmp) # 将帧数据排列成23行9列矩阵 + self.ring_matrix_palm_tmp=[] # 重置缓存数据 + if len(d) == 7 and d[0] == 255: + self.ring_matrix_palm_mass = self.build_matrix_mass(d) # [指尖合力值, 指中合力值, 指根合力值] + else: + if len(d) == 2: + self.xB4 = d + elif len(d) == 7: + index = self.matrix_map.get(d[0]) + if index is not None: + self.ring_matrix[index] = d[1:] + elif frame_type == 0xB5: + d = list(response_data) + if self.xB0[0] == 5: + """全掌矩阵""" + self.little_matrix_palm_tmp.append(d) # 将返回帧存储到缓存 + if len(d) == 4 and d[0] == 22 and d[1] == 7: # 如果是最后一帧 + self.little_matrix_palm = self.build_matrix(self.little_matrix_palm_tmp) # 将帧数据排列成23行9列矩阵 + self.little_matrix_palm_tmp=[] # 重置缓存数据 + if len(d) == 7 and d[0] == 255: + self.little_matrix_palm_mass = self.build_matrix_mass(d) # [指尖合力值, 指中合力值, 指根合力值] + else: + if len(d) == 2: + self.xB5 = d + elif len(d) == 7: + index = self.matrix_map.get(d[0]) + if index is not None: + self.little_matrix[index] = d[1:] + elif frame_type == 0xB6: + d = list(response_data) + if self.xB0[0] == 5: + """全掌矩阵""" + self.palm_matrix_palm_tmp.append(d) # 将返回帧存储到缓存 + if len(d) == 7 and d[0] == 27 and d[1] == 5: # 如果是最后一帧 + self.palm_matrix_palm = self.build_matrix(self.palm_matrix_palm_tmp) # 将帧数据排列成23行9列矩阵 + self.palm_matrix_palm_tmp=[] # 重置缓存数据 + if len(d) == 5 and d[0] == 255: + self.palm_matrix_palm_mass = self.build_matrix_mass(d) + else: + self.xB6 = d + + + # 查询指令响应 + # elif frame_type == 0xC0: self.xC0 = list(response_data) + elif frame_type == 0xC0: + d = list(response_data) + index = self.serial_number_map.get(d[0]) + if index is not None: + self.serial_number=self.serial_number + d[1:] + else: + self.serial_number=self.serial_number + [-1] * 6 + elif frame_type == 0xC1: self.xC1 = list(response_data) + elif frame_type == 0xC2: self.xC2 = list(response_data) + elif frame_type == 0xC3: self.xC3 = list(response_data) + elif frame_type == 0xC4: self.xC4 = list(response_data) + + + + def build_matrix(self, data, r=23, c=9): + rows, cols = r, c + matrix = np.full((rows, cols), -1) + + for item in data: + if len(item) == 4: + # 最后一行：从列坐标开始放 + row, col, v1, v2 = item + start_col = col # 改为 col，而不是 col+1 + values = [v1, v2] + + cur_row, cur_col = row, start_col + for val in values: + if cur_col >= cols: + cur_row += 1 + cur_col = 0 + if cur_row < rows: + matrix[cur_row][cur_col] = val + cur_col += 1 + break + + elif len(item) == 7: + # 普通行：从列坐标开始放 + row, col, v1, v2, v3, v4, v5 = item + start_col = col # 改为 col，而不是 col+1 + values = [v1, v2, v3, v4, v5] + + cur_row, cur_col = row, start_col + for val in values: + if cur_col >= cols: + cur_row += 1 + cur_col = 0 + if cur_row < rows: + matrix[cur_row][cur_col] = val + cur_col += 1 + + return matrix + + def build_matrix_mass(self, hex_data): + """ + 处理返回的和力值的帧数据,手指返回合力值长度为3，[指尖，指中，指根] + 手掌返回为长度为2。[上半部,下半部] + 解析 CAN 数据（支持 5 字节或 7 字节） + + 参数: + hex_data: 十六进制列表，如 [0xFF, 0x27, 0x02, 0x51, 0x05, 0x20, 0x04] (7字节) + 或 [0xFF, 0x92, 0x09, 0xF8, 0x00] (5字节) + + 返回: + (id, values) 其中 id 是 int，values 是包含 int 的列表（2个或3个） + """ + if len(hex_data) not in [5, 7]: + raise ValueError(f"数据长度不支持，需要 5 或 7 字节，实际: {len(hex_data)}") + + # 获取 ID（第一个字节） + can_id = hex_data[0] + + # 计算有多少组数据（每组2字节） + data_bytes = hex_data[1:] # 去掉 ID + num_values = len(data_bytes) // 2 + + # 解析数据组（小端序） + values = [] + for i in range(num_values): + low_byte = data_bytes[i*2] # 低位字节 + high_byte = data_bytes[i*2 + 1] # 高位字节 + # 小端拼接：低位 + 高位<<8 + value = low_byte | (high_byte << 8) + values.append(value) + + return values + + + # 并联控制指令方法 + def set_roll_positions(self, joint_ranges): + """设置所有手指横滚关节位置""" + self.send_command(FrameProperty.ROLL_POS, joint_ranges) + + def set_yaw_positions(self, joint_ranges): + """设置所有手指航向关节位置""" + self.send_command(FrameProperty.YAW_POS, joint_ranges) + + def set_root1_positions(self, joint_ranges): + """设置所有手指指根1关节位置""" + self.send_command(FrameProperty.ROOT1_POS, joint_ranges) + + def set_root2_positions(self, joint_ranges): + """设置所有手指指根2关节位置""" + self.send_command(FrameProperty.ROOT2_POS, joint_ranges) + + def set_root3_positions(self, joint_ranges): + """设置所有手指指根3关节位置""" + self.send_command(FrameProperty.ROOT3_POS, joint_ranges) + + def set_tip_positions(self, joint_ranges=[80]*5): + """设置所有手指指尖关节位置""" + self.send_command(FrameProperty.TIP_POS, joint_ranges) + + # 串联控制指令方法 + def set_thumb_positions(self, joint_ranges): + """设置大拇指所有关节位置""" + self.send_command(FrameProperty.THUMB_POS, joint_ranges) + + def set_index_positions(self, joint_ranges): + """设置食指所有关节位置""" + self.send_command(FrameProperty.INDEX_POS, joint_ranges) + + def set_middle_positions(self, joint_ranges): + """设置中指所有关节位置""" + self.send_command(FrameProperty.MIDDLE_POS, joint_ranges) + + def set_ring_positions(self, joint_ranges): + """设置无名指所有关节位置""" + self.send_command(FrameProperty.RING_POS, joint_ranges) + + def set_little_positions(self, joint_ranges): + """设置小拇指所有关节位置""" + self.send_command(FrameProperty.LITTLE_POS, joint_ranges) + + # 扭矩设置方法 + def set_thumb_torque(self, torque_values): + """设置大拇指扭矩""" + self.send_command(FrameProperty.THUMB_TORQUE, torque_values) + + def set_index_torque(self, torque_values): + """设置食指扭矩""" + self.send_command(FrameProperty.INDEX_TORQUE, torque_values) + + def set_middle_torque(self, torque_values): + """设置中指扭矩""" + self.send_command(FrameProperty.MIDDLE_TORQUE, torque_values) + + def set_ring_torque(self, torque_values): + """设置无名指扭矩""" + self.send_command(FrameProperty.RING_TORQUE, torque_values) + + def set_little_torque(self, torque_values): + """设置小拇指扭矩""" + self.send_command(FrameProperty.LITTLE_TORQUE, torque_values) + + # 速度设置方法 + def set_thumb_speed(self, speed_values): + """设置大拇指速度""" + self.send_command(FrameProperty.THUMB_SPEED, speed_values) + + def set_index_speed(self, speed_values): + """设置食指速度""" + self.send_command(FrameProperty.INDEX_SPEED, speed_values) + + def set_middle_speed(self, speed_values): + """设置中指速度""" + self.send_command(FrameProperty.MIDDLE_SPEED, speed_values) + + def set_ring_speed(self, speed_values): + """设置无名指速度""" + self.send_command(FrameProperty.RING_SPEED, speed_values) + + def set_little_speed(self, speed_values): + """设置小拇指速度""" + self.send_command(FrameProperty.LITTLE_SPEED, speed_values) + + # 查询方法 + def get_thumb_positions(self): + """获取大拇指所有关节当前位置""" + self.send_command(FrameProperty.THUMB_POS, []) + return self.x41 + + def get_index_positions(self): + """获取食指所有关节当前位置""" + self.send_command(FrameProperty.INDEX_POS, []) + return self.x42 + + def get_middle_positions(self): + """获取中指所有关节当前位置""" + self.send_command(FrameProperty.MIDDLE_POS, []) + return self.x43 + + def get_ring_positions(self): + """获取无名指所有关节当前位置""" + self.send_command(FrameProperty.RING_POS, []) + return self.x44 + + def get_little_positions(self): + """获取小拇指所有关节当前位置""" + self.send_command(FrameProperty.LITTLE_POS, []) + return self.x45 + + + def get_thumb_speed(self): + """获取大拇指速度""" + self.send_command(FrameProperty.THUMB_SPEED, []) + + def get_index_speed(self): + """获取食指速度""" + self.send_command(FrameProperty.INDEX_SPEED, []) + + def get_middle_speed(self): + """获取中指速度""" + self.send_command(FrameProperty.MIDDLE_SPEED, []) + + def get_ring_speed(self): + """获取无名指速度""" + self.send_command(FrameProperty.RING_SPEED, []) + + def get_little_speed(self): + """获取小拇指速度""" + self.send_command(FrameProperty.LITTLE_SPEED, []) + + def get_thumb_torque(self): + """获取大拇指扭矩""" + self.send_command(FrameProperty.THUMB_TORQUE, []) + + def get_index_torque(self): + """获取食指扭矩""" + self.send_command(FrameProperty.INDEX_TORQUE, []) + + def get_middle_torque(self): + """获取中指扭矩""" + self.send_command(FrameProperty.MIDDLE_TORQUE, []) + + def get_ring_torque(self): + """获取无名指扭矩""" + self.send_command(FrameProperty.RING_TORQUE, []) + + def get_little_torque(self): + """获取小拇指扭矩""" + self.send_command(FrameProperty.LITTLE_TORQUE, []) + + def get_thumb_fault(self): + """获取大拇指所有关节故障码""" + self.send_command(FrameProperty.THUMB_FAULT, []) + return self.x59 + + def get_index_fault(self): + """获取食指所有关节故障码""" + self.send_command(FrameProperty.INDEX_FAULT, []) + return self.x5A + + def get_middle_fault(self): + """获取中指所有关节故障码""" + self.send_command(FrameProperty.MIDDLE_FAULT, []) + return self.x5B + + def get_ring_fault(self): + """获取无名指所有关节故障码""" + self.send_command(FrameProperty.RING_FAULT, []) + return self.x5C + + def get_little_fault(self): + """获取小拇指所有关节故障码""" + self.send_command(FrameProperty.LITTLE_FAULT, []) + return self.x5D + + def get_thumb_temperature(self): + """获取大拇指所有关节当前温度""" + self.send_command(FrameProperty.THUMB_TEMPERATURE, []) + return self.x61 + + def get_index_temperature(self): + """获取食指所有关节当前温度""" + self.send_command(FrameProperty.INDEX_TEMPERATURE, []) + return self.x62 + + def get_middle_temperature(self): + """获取中指所有关节当前温度""" + self.send_command(FrameProperty.MIDDLE_TEMPERATURE, []) + return self.x63 + + def get_ring_temperature(self): + """获取无名指所有关节当前温度""" + self.send_command(FrameProperty.RING_TEMPERATURE, []) + return self.x64 + + def get_little_temperature(self): + """获取小拇指所有关节当前温度""" + self.send_command(FrameProperty.LITTLE_TEMPERATURE, []) + return self.x65 + + # 合并指令区域方法 + def set_finger_speed(self, speed_values): + """设置手指速度""" + self.send_command(FrameProperty.FINGER_SPEED, speed_values) + + def set_finger_torque(self, torque_values): + """设置手指输出扭矩""" + self.send_command(FrameProperty.FINGER_TORQUE, torque_values) + + def clear_finger_faults(self, finger_mask=[1, 1, 1, 1, 1]): + """清除手指故障及故障码""" + self.send_command(FrameProperty.FINGER_FAULT, finger_mask) + return self.x83 + + def get_finger_temperature(self): + """获取手指各关节温度""" + self.send_command(FrameProperty.FINGER_TEMPERATURE, []) + return self.x84 + + # 传感器数据获取方法 + def get_normal_force(self): + """获取五指法向力""" + self.send_command(FrameProperty.HAND_NORMAL_FORCE, []) + return self.x90 + + def get_tangential_force(self): + """获取五指切向力""" + self.send_command(FrameProperty.HAND_TANGENTIAL_FORCE, []) + return self.x91 + + def get_tangential_force_dir(self): + """获取五指切向力方向""" + self.send_command(FrameProperty.HAND_TANGENTIAL_FORCE_DIR, []) + return self.x92 + + def get_approach_inc(self): + """获取五指接近感应""" + self.send_command(FrameProperty.HAND_APPROACH_INC, []) + return self.x93 + + def get_force(self): + '''Get pressure sensor data''' + return [self.x90,self.x91,self.x92,self.x93] + + # 触觉传感器方法 + def get_touch_sensor_type(self): + """获取触觉传感器类型 暂仅支持G20""" + self.send_command(FrameProperty.TOUCH_SENSOR_TYPE, []) + return self.xB0[0] + + def get_thumb_touch(self): + """获取大拇指触觉传感数据""" + if self.xB0[0] == 5: + d = [23, 9, 1] + sleep_time = 0.015 + else: + d = [0xC6] + sleep_time = 0.007 + self.send_command(FrameProperty.THUMB_TOUCH, d, sleep_time=sleep_time) + #return self.thumb_matrix + + def get_index_touch(self): + """获取食指触觉传感数据""" + if self.xB0[0] == 5: + d = [23, 9, 1] + sleep_time = 0.015 + else: + d = [0xC6] + sleep_time = 0.007 + self.send_command(FrameProperty.INDEX_TOUCH, d, sleep_time=sleep_time) + #return self.xB2 + + def get_middle_touch(self): + """获取中指触觉传感数据""" + if self.xB0[0] == 5: + d = [23, 9, 1] + sleep_time = 0.015 + else: + d = [0xC6] + sleep_time = 0.007 + self.send_command(FrameProperty.MIDDLE_TOUCH, d, sleep_time=sleep_time) + #33333return self.xB3 + + def get_ring_touch(self): + """获取无名指触觉传感数据""" + if self.xB0[0] == 5: + d = [23, 9, 1] + sleep_time = 0.015 + else: + d = [0xC6] + sleep_time = 0.007 + self.send_command(FrameProperty.RING_TOUCH, d, sleep_time=sleep_time) + #return self.xB4 + + def get_little_touch(self): + """获取小拇指触觉传感数据""" + if self.xB0[0] == 5: + d = [23, 9, 1] + sleep_time = 0.015 + else: + d = [0xC6] + sleep_time = 0.007 + self.send_command(FrameProperty.LITTLE_TOUCH, d, sleep_time=sleep_time) + #return self.xB5 + + def get_palm_touch(self): + """获取手掌触觉传感数据""" + if self.xB0[0] == 5: + d = [28, 20, 1] + sleep_time = 0.035 + else: + d = [0xC6] + sleep_time = 0.007 + self.send_command(FrameProperty.PALM_TOUCH, d, sleep_time=sleep_time) + #return self.xB6 + + # 查询指令方法 + def get_uid(self): + """获取设备唯一标识码""" + self.send_command(FrameProperty.HAND_UID_GET, []) + return self.xC0 + + def get_hardware_version(self): + """获取硬件版本""" + self.send_command(FrameProperty.HAND_HARDWARE_VERSION_GET, []) + return self.xC1 + + def get_software_version(self): + """获取软件版本""" + self.send_command(FrameProperty.HAND_SOFTWARE_VERSION_GET, []) + return self.xC2 + + def get_comm_id(self): + """获取设备通信ID""" + self.send_command(FrameProperty.HAND_COMM_ID_GET, []) + return self.xC3 + + def get_struct_version(self): + """获取结构版本号""" + self.send_command(FrameProperty.HAND_STRUCT_VERSION_GET, []) + return self.xC4 + + # 出厂指令方法 + def erase_position_calibration(self): + """擦除位置校准值""" + self.send_command(FrameProperty.HOST_CMD_HAND_ERASE_POS_CALI, []) + + def set_comm_id(self, new_id): + """设置通信ID""" + self.send_command(FrameProperty.HAND_COMM_ID_SET, [new_id]) + + def set_uid(self, uid_data): + """设置唯一标识码（内部出厂使用）""" + self.send_command(FrameProperty.HAND_UID_SET, uid_data) + + # 辅助方法 + def slice_list(self, input_list, slice_size): + """将列表按指定大小切片""" + return [input_list[i:i + slice_size] for i in range(0, len(input_list), slice_size)] + # ----------------------------------------------------- + # API指令区域 + #------------------------------------------------------ + def set_joint_positions(self, joint_ranges): + """API接口:设置手指所有关节位置""" + j = self.cmd_range_to_joint_range(cmd_list=joint_ranges) + self.set_thumb_positions(j[0]) + self.set_index_positions(j[1]) + self.set_middle_positions(j[2]) + self.set_ring_positions(j[3]) + self.set_little_positions(j[4]) + + def set_speed(self, speed=[250] * 5): + """API接口:设置手指速度""" + self.set_thumb_speed(speed_values=[speed[0]] * 6) + self.set_index_speed(speed_values=[speed[1]] * 6) + self.set_middle_speed(speed_values=[speed[2]] * 6) + self.set_ring_speed(speed_values=[speed[3]] * 6) + self.set_little_speed(speed_values=[speed[4]] * 6) + + def set_torque(self, torque=[250] * 5): + """API接口:设置手指最大扭矩""" + self.set_thumb_torque(torque_values=[torque[0]] * 6) + self.set_index_torque(torque_values=[torque[1]] * 6) + self.set_middle_torque(torque_values=[torque[2]] * 6) + self.set_ring_torque(torque_values=[torque[3]] * 6) + self.set_little_torque(torque_values=[torque[4]] * 6) + + + def get_version(self): + """API接口:获取手指嵌入式版本信息""" + return self.get_software_version() + + def get_current_status(self): + """API接口:获取手指当前状态""" + self.get_thumb_positions() + self.get_index_positions() + self.get_middle_positions() + self.get_ring_positions() + self.get_little_positions() + time.sleep(0.002) + s = [self.x41, self.x42, self.x43, self.x44, self.x45] + cmd_state = self.joint_state_to_cmd_state(state=s) + return cmd_state + + def get_current_pub_status(self): + """API接口:获取手指当前状态""" + self.get_current_status() + + def get_speed(self): + """API接口:获取手指速度""" + self.get_thumb_speed() + self.get_index_speed() + self.get_middle_speed() + self.get_ring_speed() + self.get_little_speed() + time.sleep(0.002) + + joint_speed = [self.x49, self.x4A, self.x4B, self.x4C, self.x4D] + state_speed = self.joint_state_to_cmd_state(state=joint_speed) + return state_speed + + def get_touch_type(self): + """API接口:获取手指触觉传感器类型""" + self.send_command(0xb0,[],sleep_time=0.03) + self.send_command(0xb1,[],sleep_time=0.03) + t = [] + for i in range(3): + t = self.xB1 + time.sleep(0.01) + if len(t) == 2: + return 2 + else: + self.send_command(0x20,[],sleep_time=0.03) + time.sleep(0.01) + if self.normal_force[0] == -1: + return -1 + else: + return 1 + + def get_matrix_touch(self): + """API接口:获取手指触摸传感器数据""" + self.get_thumb_touch() + self.get_index_touch() + self.get_middle_touch() + self.get_ring_touch() + self.get_little_touch() + return self.thumb_matrix , self.index_matrix , self.middle_matrix , self.ring_matrix , self.little_matrix + + def get_matrix_touch_v2(self): + """API接口:获取手指触摸传感器数据""" + return self.get_matrix_touch() + + def get_thumb_matrix_touch(self,sleep_time=0): + """API接口:获取[大拇指]指触摸传感器数据""" + self.get_thumb_touch() + if self.xB0[0] == 5: + data = self.thumb_matrix_palm + else: + data = self.thumb_matrix + return data + + def get_index_matrix_touch(self,sleep_time=0): + """API接口:获取[食指]指触摸传感器数据""" + self.get_index_touch() + if self.xB0[0] == 5: + data = self.index_matrix_palm + else: + data = self.index_matrix + return data + + def get_middle_matrix_touch(self,sleep_time=0): + """API接口:获取[中指]指触摸传感器数据""" + self.get_middle_touch() + if self.xB0[0] == 5: + data = self.middle_matrix_palm + else: + data = self.middle_matrix + return data + + def get_ring_matrix_touch(self,sleep_time=0): + """API接口:获取[无名指]指触摸传感器数据""" + self.get_ring_touch() + if self.xB0[0] == 5: + data = self.ring_matrix_palm + else: + data = self.ring_matrix + return data + + def get_little_matrix_touch(self,sleep_time=0): + """API接口:获取[小指]指触摸传感器数据""" + self.get_little_touch() + if self.xB0[0] == 5: + data = self.little_matrix_palm + else: + data = self.little_matrix + return data + + def get_palm_matrix_touch(self,sleep_time=0): + """API接口:获取[小指]指触摸传感器数据""" + self.get_palm_touch() + if self.xB0[0] == 5: + data = self.palm_matrix_palm + else: + data = self.palm_matrix + return data + + def get_torque(self): + """API接口:获取手指最大扭矩""" + self.get_thumb_torque() + self.get_index_torque() + self.get_middle_torque() + self.get_ring_torque() + self.get_little_torque() + time.sleep(0.003) + t = [self.x51, self.x52, self.x53, self.x54, self.x55] + cmd_torque = self.joint_state_to_cmd_state(state=t) + return cmd_torque + + def get_current(self): + """API接口:获取手指电流""" + return [-1] * 20 + + def get_temperature(self): + """API接口:获取手指温度""" + joint_temperature = [self.get_thumb_temperature(), self.get_index_temperature(), self.get_middle_temperature(), self.get_ring_temperature(), self.get_little_temperature()] + cmd_temperature = self.joint_state_to_cmd_state(state=joint_temperature) + return cmd_temperature + + + def get_fault(self): + """API接口:获取手指故障代码""" + joint_fault = [self.get_thumb_fault(), self.get_index_fault(), self.get_middle_fault(), self.get_ring_fault(), self.get_little_fault()] + cmd_fault = self.joint_state_to_cmd_state(state=joint_fault) + return cmd_fault + + def clear_faults(self): + """API接口:清除手指故障代码""" + self.clear_finger_faults(finger_mask=[1, 1, 1, 1, 1]) + + def cmd_range_to_joint_range(self,cmd_list): + """根据手指映射关系，将手指控制命令列表转换为手指分组数据形式""" + # 定义手指映射规则 + finger_mapping = { + '拇指': [10, 5, 0, 11, 12, 15], + '食指': [6, 11, 1, 13, 14, 16], + '中指': [7, 12, 2, 13, 14, 17], + '无名指': [8, 13, 3, 14, 15, 18], + '小指': [9, 14, 4, 15, 16, 19] + } + + result = [] + + for finger, indices in finger_mapping.items(): + finger_data = [cmd_list[i] for i in indices] + result.append(finger_data) + + return result + + + def joint_state_to_cmd_state(self, state): + """ + 将关节状态转换为命令状态 + :param state: list2 格式的数据，5×6 的二维列表 + :return: list1 格式的 20 维列表 + """ + # 初始化结果列表，20个位置，预留位默认为0 + result = [0] * 20 + + # list1 索引映射: + # 0:拇指根部, 1:食指根部, 2:中指根部, 3:无名指根部, 4:小指根部 + # 5:拇指侧摆, 6:食指侧摆, 7:中指侧摆, 8:无名指侧摆, 9:小指侧摆 + # 10:拇指横摆, 11-14:预留, 15:拇指尖部, 16:食指末端, 17:中指末端, 18:无名指末端, 19:小指末端 + + # list2 每行结构: [侧摆/横摆, 0, 根部, 0, 0, 末端/尖部] + # 拇指行: [横摆, 侧摆, 根部, 0, 0, 尖部] — 注意拇指特殊，第1列是横摆，第2列是侧摆 + # 其他指: [侧摆, 0, 根部, 0, 0, 末端] + + # 拇指 (第0行) — 特殊处理 + result[10] = state[0][0] # 拇指横摆 + result[5] = state[0][1] # 拇指侧摆 + result[0] = state[0][2] # 拇指根部 + result[15] = state[0][5] # 拇指尖部 + + # 食指 (第1行) + result[6] = state[1][0] # 食指侧摆 + result[1] = state[1][2] # 食指根部 + result[16] = state[1][5] # 食指末端 + + # 中指 (第2行) + result[7] = state[2][0] # 中指侧摆 + result[2] = state[2][2] # 中指根部 + result[17] = state[2][5] # 中指末端 + + # 无名指 (第3行) + result[8] = state[3][0] # 无名指侧摆 + result[3] = state[3][2] # 无名指根部 + result[18] = state[3][5] # 无名指末端 + + # 小指 (第4行) + result[9] = state[4][0] # 小指侧摆 + result[4] = state[4][2] # 小指根部 + result[19] = state[4][5] # 小指末端 + + # 预留位 11-14 保持为 0 + + return result + + + def _list_d_value(self, list1, list2): + """检查两个列表的值是否有显著差异""" + if list1 is None: + return True + for a, b in zip(list1, list2): + if abs(b - a) > 2: + return True + return False + + def close_can_interface(self): + """关闭CAN接口""" + if self.bus: + self.bus.shutdown() + self.running = False + + def get_serial_number(self): + try: + self.send_command(0xC0,[],sleep_time=0.005) + # 1. 使用 bytes() 函数将整数列表转换为字节对象 + # bytes() 接收一个由 0-255 之间的整数组成的列表。 + byte_data = bytes(self.serial_number) + # 2. 使用 .decode() 方法将字节对象解码为 ASCII 字符串 + result_string = byte_data.decode('ascii') + if result_string == "": + return "-1" + else: + # print(f"原始 ASCII 码列表: {self.serial_number}") + # print(f"解码后的字符串: {result_string}") + return result_string + except: + return "-1" + def get_finger_order(self): + return ["Thumb Base", "Index Finger Base", "Middle Finger Base", "Ring Finger Base", "Pinky Finger Base", "Thumb Abduction", "Index Finger Abduction", "Middle Finger Abduction", "Ring Finger Abduction", "Pinky Finger Abduction", "Thumb Horizontal Abduction", "Reserved", "Reserved", "Reserved", "Reserved", "Thumb Tip", "Index Finger Tip", "Middle Finger Tip", "Ring Finger Tip", "Pinky Finger Tip"] diff --git a/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/core/can/linker_hand_l10_can.py b/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/core/can/linker_hand_l10_can.py new file mode 100644 index 0000000..f445a58 --- /dev/null +++ b/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/core/can/linker_hand_l10_can.py @@ -0,0 +1,531 @@ +#!/usr/bin/env python3 +# -*- coding: utf-8 -*- +import can +import time,sys +import threading +import numpy as np +#from tabulate import tabulate +from enum import Enum +from utils.open_can import OpenCan +from utils.color_msg import ColorMsg +from can.exceptions import CanError + + + +class FrameProperty(Enum): + INVALID_FRAME_PROPERTY = 0x00 + JOINT_POSITION_RCO = 0x01 + MAX_PRESS_RCO = 0x02 + MAX_PRESS_RCO2 = 0x03 + JOINT_POSITION2_RCO = 0x04 + JOINT_SPEED = 0x05 + JOINT_SPEED2 = 0x06 + REQUEST_DATA_RETURN = 0x09 + JOINT_POSITION_N = 0x11 + MAX_PRESS_N = 0x12 + HAND_NORMAL_FORCE = 0X20 + HAND_TANGENTIAL_FORCE = 0X21 + HAND_TANGENTIAL_FORCE_DIR = 0X22 + HAND_APPROACH_INC = 0X23 + MOTOR_TEMPERATURE_1 = 0x33 + MOTOR_TEMPERATURE_2 = 0x34 + +class LinkerHandL10Can: + def __init__(self,can_id, can_channel='can0', baudrate=1000000, yaml=""): + self.can_id = can_id + self.can_channel = can_channel + self.baudrate = baudrate + self.open_can = OpenCan(load_yaml=yaml) + self.is_cmd = False + self.x01 = [-1] * 5 + self.x02 = [-1] * 5 + self.x03 = [-1] * 5 + self.x04 = [-1] * 5 + self.x05 = [-1] * 5 + self.x06 = [-1] * 5 + self.x33 = self.x34 = [0] * 5 + # Fault codes + self.x35,self.x36 = [0] * 5,[0] * 5 + # New pressure sensors + self.xb0,self.xb1,self.xb2,self.xb3,self.xb4,self.xb5 = [-1] * 5,[-1] * 5,[-1] * 5,[-1] * 5,[-1] * 5,[-1] * 5 + + self.thumb_matrix = np.full((12, 6), -1) + self.index_matrix = np.full((12, 6), -1) + self.middle_matrix = np.full((12, 6), -1) + self.ring_matrix = np.full((12, 6), -1) + self.little_matrix = np.full((12, 6), -1) + self.matrix_map = { + 0: 0, + 16: 1, + 32: 2, + 48: 3, + 64: 4, + 80: 5, + 96: 6, + 112: 7, + 128: 8, + 144: 9, + 160: 10, + 176: 11, + } + self.serial_number = [] + self.serial_number_map = { + 0: 0, + 1: 1, + 2: 2, + 3: 3, + } + self.can_id = can_id + self.joint_angles = [0] * 10 + self.pressures = [200] * 5 # Default torque 200 + self.bus = self.init_can_bus(can_channel, baudrate) + self.normal_force, self.tangential_force, self.tangential_force_dir, self.approach_inc = [[-1] * 5 for _ in range(4)] + self.version = None + # Start receiving thread + self.running = True + self.receive_thread = threading.Thread(target=self.receive_response) + self.receive_thread.daemon = True + self.receive_thread.start() + self.version = self.get_version() + + def init_can_bus(self, channel, baudrate): + """ + 尝试按优先级连接 CAN 总线，并实现回退机制。 + """ + # --- 统一异常处理块开始 --- + try: + if sys.platform == "linux": + # Linux 优先级：1. socketcan + try: + self.open_can.open_can(self.can_channel) + # 尝试 socketcan + bus = can.interface.Bus(channel=channel, interface="socketcan", bitrate=baudrate) + ColorMsg(msg=f"成功连接: interface='socketcan', channel='{channel}'", color="green") + return bus + except CanError as e: + # 如果 socketcan 失败，可以考虑在这里尝试其他 Linux 接口 (如 'pcan') + ColorMsg(msg=f"socketcan 接口连接失败: {e}", color="yellow") + raise # 重新抛出异常，让外层 try 捕获 + elif sys.platform == "win32": + # Windows 优先级：1. pcan + try: + bus = can.interface.Bus(channel=channel, interface='pcan', bitrate=baudrate) + ColorMsg(msg=f"成功连接: interface='pcan', channel='{channel}'", color="green") + return bus + except CanError as e: + ColorMsg(msg=f"pcan 接口连接失败，尝试回退到 'candle': {e}", color="yellow") + # Windows 优先级：2. candle (回退方法) + try: + bus = can.Bus(interface="candle", channel=channel, bitrate=baudrate) + ColorMsg(msg=f"成功连接: interface='candle', channel='{channel}'", color="green") + return bus + except CanError as e: + ColorMsg(msg=f"candle 接口连接失败: {e}", color="yellow") + raise # 两个接口都失败，抛出异常 + else: + raise EnvironmentError("Unsupported platform for CAN interface") + # --- 统一异常处理块结束 --- + except Exception as e: + # 如果任何一个接口尝试失败并抛出异常（包括 EnvironmentError） + ColorMsg(msg=f"致命错误：所有 CAN 接口连接尝试均失败或平台不受支持。请检查设备连接或驱动安装和配置文件中CAN参数的配置。\n错误详情: {e}", color="red") + # 保持 raise 动作，将错误信息传递给调用者，避免程序继续运行 + raise + + def send_frame(self, frame_property, data_list,sleep=0.002): + """Send a single CAN frame with specified properties and data.""" + frame_property_value = int(frame_property.value) if hasattr(frame_property, 'value') else frame_property + data = [frame_property_value] + [int(val) for val in data_list] + msg = can.Message(arbitration_id=self.can_id, data=data, is_extended_id=False) + try: + self.bus.send(msg) + except can.CanError as e: + print(f"Failed to send message: {e}") + self.open_can.open_can(self.can_channel) + time.sleep(1) + self.is_can = self.open_can.is_can_up_sysfs(interface=self.can_channel) + time.sleep(1) + if self.is_can: + self.bus = can.interface.Bus(channel=self.can_channel, interface="socketcan", bitrate=self.baudrate) + else: + print("Reconnecting CAN devices ....") + # time.sleep(1) + # + time.sleep(sleep) + + def set_joint_positions(self, joint_angles): + """Set the positions of 10 joints (joint_angles: list of 10 values).""" + self.joint_angles = joint_angles + self.is_cmd = True + # Send angle control in frames, L10 protocol splits into first 6 and last 4 + self.send_frame(FrameProperty.JOINT_POSITION2_RCO, self.joint_angles[6:]) + #time.sleep(0.001) + self.send_frame(FrameProperty.JOINT_POSITION_RCO, self.joint_angles[:6]) + #time.sleep(0.002) + self.is_cmd = False + + + def set_max_torque_limits(self, pressures,type="get"): + """Set maximum torque limits""" + if type == "get": + self.pressures = [0.0] + else: + self.pressures = pressures[:5] + #self.send_frame(FrameProperty.MAX_PRESS_RCO, self.pressures) + + + def set_joint_speed_l10(self,speed=[180]*5): + self.x05 = speed + for i in range(2): + time.sleep(0.01) + self.send_frame(0x05, speed) + def set_speed(self,speed=[180]*5): + if len(speed) == 5: + self.x05 = speed + for i in range(2): + time.sleep(0.01) + self.send_frame(0x05, speed) + elif len(speed) == 10: + for i in range(2): + time.sleep(0.01) + self.send_frame(0x05, speed[:5]) + self.send_frame(0x06, speed[5:]) + else: + raise ValueError("Speed list must have 10 elements.") + def request_all_status(self): + """Get all joint positions and pressures.""" + self.send_frame(FrameProperty.REQUEST_DATA_RETURN, []) + ''' -------------------Pressure Sensors---------------------- ''' + def get_normal_force(self): + self.send_frame(FrameProperty.HAND_NORMAL_FORCE,[],sleep=0.004) + + def get_tangential_force(self): + self.send_frame(FrameProperty.HAND_TANGENTIAL_FORCE,[],sleep=0.004) + + def get_tangential_force_dir(self): + self.send_frame(FrameProperty.HAND_TANGENTIAL_FORCE_DIR,[],sleep=0.004) + def get_approach_inc(self): + self.send_frame(FrameProperty.HAND_APPROACH_INC,[],sleep=0.004) + ''' -------------------Motor Temperature---------------------- ''' + def get_motor_temperature(self): + self.send_frame(FrameProperty.MOTOR_TEMPERATURE_1,[],sleep=0.01) + self.send_frame(FrameProperty.MOTOR_TEMPERATURE_2,[],sleep=0.01) + # Motor fault codes + def get_motor_fault_code(self): + self.send_frame(0x35,[],sleep=0.1) + self.send_frame(0x36,[],sleep=0.1) + def receive_response(self): + """Receive CAN responses and process them.""" + while self.running: + try: + msg = self.bus.recv(timeout=1.0) + if msg: + self.process_response(msg) + except can.CanError as e: + print(f"Error receiving CAN message: {e}") + + def process_response(self, msg): + """Process received CAN messages.""" + if msg.arbitration_id == self.can_id: + frame_type = msg.data[0] + response_data = msg.data[1:] + if len(list(response_data)) == 0: + return + if frame_type == FrameProperty.JOINT_POSITION_RCO.value: # 0x01 + self.x01 = list(response_data) + elif frame_type == FrameProperty.MAX_PRESS_RCO.value: # 0x02 + self.x02 = list(response_data) + elif frame_type == FrameProperty.MAX_PRESS_RCO2.value: # 0x03 + self.x03 = list(response_data) + elif frame_type == FrameProperty.JOINT_POSITION2_RCO.value: # 0x04 + self.x04 = list(response_data) + elif frame_type == 0x05: + self.x05 = list(response_data) + elif frame_type == 0x06: + self.x06 = list(response_data) + elif frame_type == 0x20: + # Five-finger normal force + d = list(response_data) + self.normal_force = [float(i) for i in d] + elif frame_type == 0x21: + # Five-finger tangential force + d = list(response_data) + self.tangential_force = [float(i) for i in d] + elif frame_type == 0x22: + # Five-finger tangential force direction + d = list(response_data) + self.tangential_force_dir = [float(i) for i in d] + elif frame_type == 0x23: + # Five-finger approach increment + d = list(response_data) + self.approach_inc = [float(i) for i in d] + elif frame_type == 0x33: + self.x33 = list(response_data) + elif frame_type == 0x34: + self.x34 = list(response_data) + elif frame_type == 0x35: + self.x35 = list(response_data) + elif frame_type == 0x36: + self.x36 = list(response_data) + elif frame_type == 0xb0: + self.xb0 = list(response_data) + elif frame_type == 0xb1: + d = list(response_data) + if len(d) == 2: + self.xb1 = d + elif len(d) == 7: + index = self.matrix_map.get(d[0]) + if index is not None: + self.thumb_matrix[index] = d[1:] # Remove the first flag bit + elif frame_type == 0xb2: + d = list(response_data) + if len(d) == 2: + self.xb2 = d + elif len(d) == 7: + index = self.matrix_map.get(d[0]) + if index is not None: + self.index_matrix[index] = d[1:] # Remove the first flag bit + elif frame_type == 0xb3: + d = list(response_data) + if len(d) == 2: + self.xb3 = d + elif len(d) == 7: + index = self.matrix_map.get(d[0]) + if index is not None: + self.middle_matrix[index] = d[1:] # Remove the first flag bit + elif frame_type == 0xb4: + d = list(response_data) + if len(d) == 2: + self.xb4 = d + elif len(d) == 7: + index = self.matrix_map.get(d[0]) + if index is not None: + self.ring_matrix[index] = d[1:] # Remove the first flag bit + elif frame_type == 0xb5: + d = list(response_data) + if len(d) == 2: + self.xb5 = d + elif len(d) == 7: + index = self.matrix_map.get(d[0]) + if index is not None: + self.little_matrix[index] = d[1:] # Remove the first flag bit + elif frame_type == 0x64: + self.version = list(response_data) + elif frame_type == 0xC2: # version number + self.version = list(response_data) + elif frame_type == 0xC0: + d = list(response_data) + index = self.serial_number_map.get(d[0]) + if index is not None: + self.serial_number=self.serial_number + d[1:] + else: + self.serial_number=self.serial_number + [-1] * 6 + + def get_version(self): + self.send_frame(0x64, [], sleep=0.1) + time.sleep(0.1) + if self.version is None: + self.send_frame(0xC2, [], sleep=0.1) + time.sleep(0.1) + return self.version + + def set_torque(self,torque=[]): + '''Set maximum torque''' + if len(torque) == 5: + self.send_frame(0x02, torque) + time.sleep(0.002) + self.send_frame(0x03,torque) + elif len(torque) > 5: + self.send_frame(0x02, torque[:5]) + time.sleep(0.002) + self.send_frame(0x03,torque[5:]) + + + def get_current_status(self): + '''Get current joint status''' + if self.is_cmd == False: + #if self.version != None and self.version[4] > 35: + self.send_frame(0x01,[],sleep=0.003) + self.send_frame(0x04,[],sleep=0.003) + state = self.x01 + self.x04 + return state + else: + state = self.x01 + self.x04 + return state + + def get_current_pub_status(self): + state = self.x01 + self.x04 + return state + + def get_speed(self): + '''Get current speed''' + self.send_frame(0x05,[],sleep=0.003) + self.send_frame(0x06,[],sleep=0.003) + return self.x05 + self.x06 + + def get_force(self): + '''Get pressure sensor data''' + return [self.normal_force,self.tangential_force , self.tangential_force_dir , self.approach_inc] + def get_temperature(self): + '''Get current motor temperature''' + self.get_motor_temperature() + return self.x33+self.x34 + + def get_touch_type(self): + '''Get touch type''' + self.send_frame(0xb0,[],sleep=0.03) + self.send_frame(0xb1,[],sleep=0.03) + t = [] + for i in range(3): + t = self.xb1 + time.sleep(0.01) + if len(t) == 2: + return 2 + else: + self.send_frame(0x20,[],sleep=0.03) + time.sleep(0.01) + if self.normal_force[0] == -1: + return -1 + else: + return 1 + + def get_touch(self): + '''Get touch data''' + self.send_frame(0xb1,[],sleep=0.03) + self.send_frame(0xb2,[],sleep=0.03) + self.send_frame(0xb3,[],sleep=0.03) + self.send_frame(0xb4,[],sleep=0.03) + self.send_frame(0xb5,[],sleep=0.03) + return [self.xb1[1],self.xb2[1],self.xb3[1],self.xb4[1],self.xb5[1],0] # The last digit is palm, currently not available + + def get_matrix_touch(self): + self.send_frame(0xb1,[0xc6],sleep=0.06) + self.send_frame(0xb2,[0xc6],sleep=0.06) + self.send_frame(0xb3,[0xc6],sleep=0.06) + self.send_frame(0xb4,[0xc6],sleep=0.06) + self.send_frame(0xb5,[0xc6],sleep=0.06) + return self.thumb_matrix , self.index_matrix , self.middle_matrix , self.ring_matrix , self.little_matrix + + def get_matrix_touch_v2(self): + self.send_frame(0xb1,[0xc6],sleep=0.005) + self.send_frame(0xb2,[0xc6],sleep=0.005) + self.send_frame(0xb3,[0xc6],sleep=0.005) + self.send_frame(0xb4,[0xc6],sleep=0.005) + self.send_frame(0xb5,[0xc6],sleep=0.005) + return self.thumb_matrix , self.index_matrix , self.middle_matrix , self.ring_matrix , self.little_matrix + + def get_thumb_matrix_touch(self,sleep_time=0.005): + self.send_frame(0xb1,[0xc6],sleep=sleep_time) + return self.thumb_matrix + + def get_index_matrix_touch(self,sleep_time=0.005): + self.send_frame(0xb2,[0xc6],sleep=sleep_time) + return self.index_matrix + + def get_middle_matrix_touch(self,sleep_time=0.005): + self.send_frame(0xb3,[0xc6],sleep=sleep_time) + return self.middle_matrix + + def get_ring_matrix_touch(self,sleep_time=0.005): + self.send_frame(0xb4,[0xc6],sleep=sleep_time) + return self.ring_matrix + + def get_little_matrix_touch(self,sleep_time=0.005): + self.send_frame(0xb5,[0xc6],sleep=sleep_time) + return self.little_matrix + + + def get_torque(self): + '''Get current motor torque''' + if self.version != None and self.version[4]< 36: + return [-1] * 5 + else: + self.send_frame(0x02, []) + time.sleep(0.002) + self.send_frame(0x03,[]) + time.sleep(0.002) + return self.x02+self.x03 + + def get_fault(self): + '''Get motor fault''' + self.get_motor_fault_code() + return self.x35+self.x36 + + def get_current(self): + '''Get current''' + #return [-1] * 5 + self.send_frame(0x02, []) + time.sleep(0.002) + self.send_frame(0x03,[]) + return self.x02+self.x03 + + def get_serial_number(self): + try: + self.send_frame(0xC0,[],sleep=0.005) + # 1. 使用 bytes() 函数将整数列表转换为字节对象 + # bytes() 接收一个由 0-255 之间的整数组成的列表。 + byte_data = bytes(self.serial_number) + # 2. 使用 .decode() 方法将字节对象解码为 ASCII 字符串 + result_string = byte_data.decode('ascii') + if result_string == "": + return "-1" + else: + # print(f"原始 ASCII 码列表: {self.serial_number}") + # print(f"解码后的字符串: {result_string}") + return result_string + except: + return "-1" + + def get_finger_order(self): + return ["thumb_cmc_pitch", "thumb_cmc_yaw", "index_mcp_pitch", "middle_mcp_pitch", "ring_mcp_pitch", "pinky_mcp_pitch", + "index_mcp_roll", "ring_mcp_roll", "pinky_mcp_roll", "thumb_cmc_roll"] + + def clear_faults(self, finger_mask=[1, 1, 1, 1, 1]): + """L10 暂不支持清除故障码""" + pass + + def show_fun_table(self): + # if len(data) != 8 or data[0] != 0x64: + # raise ValueError("数据格式不正确") + data = self.version + result = { + "自由度": data[0], + "机械版本": data[1], + "版本序号": data[2], + "手方向": chr(data[3]), # ASCII 转字符 + "软件版本": f"V{data[4] >> 4}.{data[4] & 0x0F}", + "硬件版本": f"V{data[5] >> 4}.{data[5] & 0x0F}", + "修订标志": data[6], + "set_position": "Y", + "set_torque": "Y", + "set_speed": "Y", + "get_version": "Y", + "get_current_status": "Y", + "get_speed": "Y", + "get_temperature": "Y", + "get_touch_type": "Y", + "get_matrix_touch": "Y", + "get_fault": "Y", + "get_current": "current == torque" + } + + #return [data[0],data[1],data[2],chr(data[3]),f"V{data[4] >> 4}.{data[4] & 0x0F}",f"V{data[5] >> 4}.{data[5] & 0x0F}",data[6]] + table = [[k, v] for k, v in result.items()] + #print(tabulate(table, tablefmt="grid"), flush=True) + + + # # 示例数据 + # data = [0x64, 0x15, 0x03, 0x0A, 0x4C, 0x11, 0x22, 0x01] + # parsed = parse_version_data(data) + + # # 打印结果 + # for k, v in parsed.items(): + # print(f"{k}: {v}") + + + def close_can_interface(self): + """Stop the CAN communication.""" + self.running = False + if self.receive_thread.is_alive(): + self.receive_thread.join() + if self.bus: + self.bus.shutdown() diff --git a/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/core/can/linker_hand_l20_can.py b/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/core/can/linker_hand_l20_can.py new file mode 100644 index 0000000..5b5f36e --- /dev/null +++ b/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/core/can/linker_hand_l20_can.py @@ -0,0 +1,478 @@ +import sys +import time +import can +import threading +from enum import Enum +import numpy as np +from utils.open_can import OpenCan +from utils.color_msg import ColorMsg +from can.exceptions import CanError + +class FrameProperty(Enum): + INVALID_FRAME_PROPERTY = 0x00 # Invalid CAN frame property | No return + JOINT_PITCH_R = 0x01 # Short frame pitch angle - finger base flexion | Returns this type of data + JOINT_YAW_R = 0x02 # Short frame yaw angle - finger abduction/adduction | Returns this type of data + JOINT_ROLL_R = 0x03 # Short frame roll angle - only used for thumb | Returns this type of data + JOINT_TIP_R = 0x04 # Short frame fingertip angle control | Returns this type of data + JOINT_SPEED_R = 0x05 # Short frame speed - motor running speed control | Returns this type of data + JOINT_CURRENT_R = 0x06 # Short frame current - motor running current feedback | Returns this type of data + JOINT_FAULT_R = 0x07 # Short frame fault - motor running fault feedback | Returns this type of data + REQUEST_DATA_RETURN = 0x09 # Request data return | Returns all data + JOINT_PITCH_NR = 0x11 # Pitch angle - finger base flexion | No return for this type of data + JOINT_YAW_NR = 0x12 # Yaw angle - finger abduction/adduction | No return for this type of data + JOINT_ROLL_NR = 0x13 # Roll angle - only used for thumb | No return for this type of data + JOINT_TIP_NR = 0x14 # Fingertip angle control | No return for this type of data + JOINT_SPEED_NR = 0x15 # Speed - motor running speed control | No return for this type of data + JOINT_CURRENT_NR = 0x16 # Current - motor running current feedback | No return for this type of data + JOINT_FAULT_NR = 0x17 # Fault - motor running fault feedback | No return for this type of data + HAND_UID = 0xC0 # Device unique identifier Read only -------- + HAND_HARDWARE_VERSION = 0xC1 # Hardware version Read only -------- + HAND_SOFTWARE_VERSION = 0xC2 # Software version Read only -------- + HAND_COMM_ID = 0xC3 # Device ID Read/Write 1 byte + HAND_SAVE_PARAMETER = 0xCF # Save parameters Write only -------- + + +class LinkerHandL20Can: + def __init__(self, can_channel='can0', baudrate=1000000, can_id=0x28,yaml=""): + self.can_id = can_id + self.can_channel = can_channel + self.baudrate = baudrate + self.open_can = OpenCan(load_yaml=yaml) + + self.running = True + self.x05 = [255] * 5 + self.x06, self.x07 = [],[] + # New pressure sensors + self.xb0,self.xb1,self.xb2,self.xb3,self.xb4,self.xb5 = [-1] * 5,[-1] * 5,[-1] * 5,[-1] * 5,[-1] * 5,[-1] * 5 + self.x09 = self.x0b = self.x0c = self.x0d = [-1] * 5 + self.thumb_matrix = np.full((12, 6), -1) + self.index_matrix = np.full((12, 6), -1) + self.middle_matrix = np.full((12, 6), -1) + self.ring_matrix = np.full((12, 6), -1) + self.little_matrix = np.full((12, 6), -1) + self.matrix_map = { + 0: 0, + 16: 1, + 32: 2, + 48: 3, + 64: 4, + 80: 5, + 96: 6, + 112: 7, + 128: 8, + 144: 9, + 160: 10, + 176: 11, + } + + # Initialize CAN bus according to operating system + # try: + # if sys.platform == "linux": + # self.open_can.open_can(self.can_channel) + # time.sleep(0.1) + # self.bus = can.interface.Bus( + # channel=can_channel, interface="socketcan", bitrate=baudrate, + # can_filters=[{"can_id": can_id, "can_mask": 0x7FF}] + # ) + # elif sys.platform == "win32": + # self.bus = can.interface.Bus( + # channel=can_channel, interface='pcan', bitrate=baudrate, + # can_filters=[{"can_id": can_id, "can_mask": 0x7FF}] + # ) + # else: + # raise EnvironmentError("Unsupported platform for CAN interface") + # except: + # print("Please insert CAN device",flush=True) + self.bus = self.init_can_bus(channel=self.can_channel, baudrate=baudrate) + # Initialize data storage + self.x01, self.x02, self.x03, self.x04 = [[-1] * 5 for _ in range(4)] + self.normal_force, self.tangential_force, self.tangential_force_dir, self.approach_inc = \ + [[-1] * 5 for _ in range(4)] + + # Start receive thread + self.get_touch_type() + time.sleep(0.1) + self.receive_thread = threading.Thread(target=self.receive_response) + self.receive_thread.daemon = True + self.receive_thread.start() + + def init_can_bus(self, channel, baudrate): + """ + 尝试按优先级连接 CAN 总线，并实现回退机制。 + """ + # --- 统一异常处理块开始 --- + try: + if sys.platform == "linux": + # Linux 优先级：1. socketcan + try: + self.open_can.open_can(self.can_channel) + # 尝试 socketcan + bus = can.interface.Bus(channel=channel, interface="socketcan", bitrate=baudrate) + ColorMsg(msg=f"成功连接: interface='socketcan', channel='{channel}'", color="green") + return bus + except CanError as e: + # 如果 socketcan 失败，可以考虑在这里尝试其他 Linux 接口 (如 'pcan') + ColorMsg(msg=f"socketcan 接口连接失败: {e}", color="yellow") + raise # 重新抛出异常，让外层 try 捕获 + elif sys.platform == "win32": + # Windows 优先级：1. pcan + try: + bus = can.interface.Bus(channel=channel, interface='pcan', bitrate=baudrate) + ColorMsg(msg=f"成功连接: interface='pcan', channel='{channel}'", color="green") + return bus + except CanError as e: + ColorMsg(msg=f"pcan 接口连接失败，尝试回退到 'candle': {e}", color="yellow") + # Windows 优先级：2. candle (回退方法) + try: + bus = can.Bus(interface="candle", channel=channel, bitrate=baudrate) + ColorMsg(msg=f"成功连接: interface='candle', channel='{channel}'", color="green") + return bus + except CanError as e: + ColorMsg(msg=f"candle 接口连接失败: {e}", color="yellow") + raise # 两个接口都失败，抛出异常 + else: + raise EnvironmentError("Unsupported platform for CAN interface") + # --- 统一异常处理块结束 --- + except Exception as e: + # 如果任何一个接口尝试失败并抛出异常（包括 EnvironmentError） + ColorMsg(msg=f"致命错误：所有 CAN 接口连接尝试均失败或平台不受支持。请检查设备连接或驱动安装和配置文件中CAN参数的配置。\n错误详情: {e}", color="red") + # 保持 raise 动作，将错误信息传递给调用者，避免程序继续运行 + raise + # def send_command(self, frame_property, data_list): + # print("66666") + # """ + # Send command to CAN bus + # :param frame_property: Data frame property + # :param data_list: Data payload + # """ + # frame_property_value = int(frame_property.value) if hasattr(frame_property, 'value') else frame_property + # data = [frame_property_value] + [int(val) for val in data_list] + # msg = can.Message(arbitration_id=self.can_id, data=data, is_extended_id=False) + # try: + # self.bus.send(msg) + # print(f"Message sent: ID={hex(self.can_id)}, Data={data}") + # except can.CanError as e: + # print(f"Failed to send message: {e}") + + def receive_response(self): + """ + Receive and process CAN bus response messages + """ + while self.running: + try: + msg = self.bus.recv(timeout=1.0) # Blocking receive, 1 second timeout + if msg: + self.process_response(msg) + except can.CanError as e: + print(f"Error receiving message666: {e}",flush=True) + + + def set_finger_base(self, angles): + self.send_command(FrameProperty.JOINT_PITCH_NR, angles) + + def set_finger_tip(self, angles): + self.send_command(FrameProperty.JOINT_TIP_NR, angles) + + def set_finger_middle(self, angles): + self.send_command(FrameProperty.JOINT_YAW_NR, angles) + + def set_thumb_roll(self, angle): + self.send_command(FrameProperty.JOINT_ROLL_NR, angle) + + def send_command(self, frame_property, data_list,sleep=0.002): + frame_property_value = int(frame_property.value) if hasattr(frame_property, 'value') else frame_property + data = [frame_property_value] + [int(val) for val in data_list] + + msg = can.Message(arbitration_id=self.can_id, data=data, is_extended_id=False) + try: + self.bus.send(msg) + except can.CanError: + print("Message NOT sent") + self.open_can.open_can(self.can_channel) + time.sleep(1) + self.is_can = self.open_can.is_can_up_sysfs(interface=self.can_channel) + time.sleep(1) + if self.is_can: + self.bus = can.interface.Bus(channel=self.can_channel, interface="socketcan", bitrate=self.baudrate) + else: + print("Reconnecting CAN devices ....",flush=True) + time.sleep(sleep) + + def set_joint_pitch(self, frame, angles): + self.send_command(frame, angles) + + def set_joint_yaw(self, angles): + self.send_command(0x02, angles) + + def set_joint_roll(self, thumb_roll): + self.send_command(0x03, [thumb_roll, 0, 0, 0, 0]) + + def set_joint_speed(self, speed): + self.x05 = speed + self.send_command(0x05, speed) + def set_electric_current(self, e_c=[]): + self.send_command(0x06, e_c) + + def get_normal_force(self): + self.send_command(0x20,[]) + + def get_tangential_force(self): + self.send_command(0x21,[]) + + + def get_tangential_force_dir(self): + self.send_command(0x22,[]) + + def get_approach_inc(self): + self.send_command(0x23,[]) + + + + + def get_electric_current(self, e_c=[]): + self.send_command(0x06, e_c) + + def request_device_info(self): + self.send_command(0xC0, [0]) + self.send_command(0xC1, [0]) + self.send_command(0xC2, [0]) + + def save_parameters(self): + self.send_command(0xCF, []) + def process_response(self, msg): + if msg.arbitration_id == self.can_id: + frame_type = msg.data[0] + response_data = msg.data[1:] + if len(list(response_data)) == 0: + return + if frame_type == 0x01: + self.x01 = list(response_data) + elif frame_type == 0x02: + self.x02 = list(response_data) + elif frame_type == 0x03: + self.x03 = list(response_data) + elif frame_type == 0x04: + self.x04 = list(response_data) + elif frame_type == 0xC0: + print(f"Device ID info: {response_data}") + if self.can_id == 0x28: + self.right_hand_info = response_data + elif self.can_id == 0x27: + self.left_hand_info = response_data + elif frame_type == 0x05: + self.x05 = list(response_data) + elif frame_type == 0x06: + self.x06 = list(response_data) + elif frame_type == 0x07: + self.x07 = list(response_data) + elif frame_type == 0x09: + self.x09 = list(response_data) + elif frame_type == 0x0B: + self.x0b = list(response_data) + elif frame_type == 0x0C: + self.x0c = list(response_data) + elif frame_type == 0x0D: + self.x0d = list(response_data) + elif frame_type == 0x20: + d = list(response_data) + self.normal_force = [float(i) for i in d] + elif frame_type == 0x21: + d = list(response_data) + self.tangential_force = [float(i) for i in d] + elif frame_type == 0x22: + d = list(response_data) + self.tangential_force_dir = [float(i) for i in d] + elif frame_type == 0x23: + d = list(response_data) + self.approach_inc = [float(i) for i in d] + elif frame_type == 0xb0: + self.xb0 = list(response_data) + elif frame_type == 0xb1: + d = list(response_data) + if len(d) == 2: + self.xb1 = d + elif len(d) == 7: + index = self.matrix_map.get(d[0]) + if index is not None: + self.thumb_matrix[index] = d[1:] # Remove the first flag bit + elif frame_type == 0xb2: + d = list(response_data) + if len(d) == 2: + self.xb2 = d + elif len(d) == 7: + index = self.matrix_map.get(d[0]) + if index is not None: + self.index_matrix[index] = d[1:] # Remove the first flag bit + elif frame_type == 0xb3: + d = list(response_data) + if len(d) == 2: + self.xb3 = d + elif len(d) == 7: + index = self.matrix_map.get(d[0]) + if index is not None: + self.middle_matrix[index] = d[1:] # Remove the first flag bit + elif frame_type == 0xb4: + d = list(response_data) + if len(d) == 2: + self.xb4 = d + elif len(d) == 7: + index = self.matrix_map.get(d[0]) + if index is not None: + self.ring_matrix[index] = d[1:] # Remove the first flag bit + elif frame_type == 0xb5: + d = list(response_data) + if len(d) == 2: + self.xb5 = d + elif len(d) == 7: + index = self.matrix_map.get(d[0]) + if index is not None: + self.little_matrix[index] = d[1:] # Remove the first flag bit + def pose_slice(self, p): + """Slice the joint array into finger action arrays""" + try: + finger_base = [int(val) for val in p[0:5]] # Finger base + yaw_angles = [int(val) for val in p[5:10]] # Yaw + thumb_yaw = [int(val) for val in p[10:15]] # Thumb yaw to palm, others are 0 + finger_tip = [int(val) for val in p[15:20]] # Fingertip flexion + return finger_base, yaw_angles, thumb_yaw, finger_tip + except Exception as e: + print(e) + def set_joint_positions(self, position): + if len(position) != 20: + print("L20 finger joint length is incorrect") + return + finger_base, yaw_angles, thumb_yaw, finger_tip = self.pose_slice(position) + self.set_thumb_roll(thumb_yaw) # Thumb yaw to palm movement + self.set_finger_tip(finger_tip) # Fingertip movement + self.set_finger_base(finger_base) # Finger base movement + self.set_finger_middle(yaw_angles) # Yaw movement + def set_speed(self, speed=[]): + if len(speed) != 5: + raise ValueError("Speed list must have 5 elements.") + return + self.send_command(0x05,speed) + def set_torque(self, torque=[]): + '''Set torque, not supported for L20''' + print("Set torque, not supported for L20") + def set_current(self, current=[]): + '''Set current''' + self.set_electric_current(e_c=current) + def get_version(self): + '''Get version, currently not supported''' + return [0] * 5 + def get_current_status(self): + '''Get current finger joint status''' + self.send_command(0x01,[],sleep=0.01) + self.send_command(0x02,[],sleep=0.01) + self.send_command(0x03,[],sleep=0.01) + self.send_command(0x04,[],sleep=0.01) + return self.x01 + self.x02 + self.x03 + self.x04 + + def get_current_pub_status(self): + time.sleep(0.01) + return self.x01 + self.x02 + self.x03 + self.x04 + + def get_speed(self): + '''Get current motor speed''' + self.send_command(0x05, [0]) + time.sleep(0.001) + return self.x05 + def get_current(self): + '''Get current threshold''' + self.send_command(0x06, [0]) + return self.x06 + def get_torque(self): + '''Get current motor torque, not supported for L20''' + return [0] * 5 + def get_fault(self): + self.send_command(0x07,[]) + time.sleep(0.01) + return self.x07 + + def get_temperature(self): + '''Get motor temperature''' + self.send_command(0x09,[]) + self.send_command(0x0b,[]) + self.send_command(0x0c,[]) + self.send_command(0x0d,[]) + + return self.x09+self.x0b+self.x0c+self.x0d + + def clear_faults(self): + '''Clear motor faults''' + self.send_command(0x07, [1, 1, 1, 1, 1]) + + def get_touch_type(self): + '''Get touch type''' + t = [] + for i in range(3): + self.send_command(0xb0,[],sleep=0.03) + if self.xb0 == [2]: + return 2 + elif self.xb0 == [1]: + return 1 + else: + self.send_command(0x20,[],sleep=0.03) + time.sleep(0.01) + if self.normal_force[0] == -1: + return -1 + + def get_touch(self): + '''Get touch data''' + self.send_command(0xb1,[],sleep=0.03) + self.send_command(0xb2,[],sleep=0.03) + self.send_command(0xb3,[],sleep=0.03) + self.send_command(0xb4,[],sleep=0.03) + self.send_command(0xb5,[],sleep=0.03) + return [self.xb1[1],self.xb2[1],self.xb3[1],self.xb4[1],self.xb5[1],0] # The last digit is palm, currently not available + + def get_matrix_touch(self): + self.send_command(0xb1,[0xc6],sleep=0.04) + self.send_command(0xb2,[0xc6],sleep=0.04) + self.send_command(0xb3,[0xc6],sleep=0.04) + self.send_command(0xb4,[0xc6],sleep=0.04) + self.send_command(0xb5,[0xc6],sleep=0.04) + return self.thumb_matrix , self.index_matrix , self.middle_matrix , self.ring_matrix , self.little_matrix + + + def get_thumb_matrix_touch(self,sleep_time=0.009): + self.send_command(0xb1,[0xc6],sleep=sleep_time) + return self.thumb_matrix + + def get_index_matrix_touch(self,sleep_time=0.009): + self.send_command(0xb2,[0xc6],sleep=sleep_time) + return self.index_matrix + + def get_middle_matrix_touch(self,sleep_time=0.009): + self.send_command(0xb3,[0xc6],sleep=sleep_time) + return self.middle_matrix + + def get_ring_matrix_touch(self,sleep_time=0.009): + self.send_command(0xb4,[0xc6],sleep=sleep_time) + return self.ring_matrix + + def get_little_matrix_touch(self,sleep_time=0.009): + self.send_command(0xb5,[0xc6],sleep=sleep_time) + return self.little_matrix + + + def get_faults(self): + '''Get motor fault codes''' + self.send_command(0x07, []) + return self.x07 + def get_force(self): + '''Get pressure sensor data''' + return [self.normal_force,self.tangential_force,self.tangential_force_dir,self.approach_inc] + + def get_serial_number(self): + return [0] * 6 + + def show_fun_table(self): + pass + + def get_finger_order(self): + return [] + + def close_can_interface(self): + if self.bus: + self.bus.shutdown() # Close CAN bus diff --git a/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/core/can/linker_hand_l21_can.py b/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/core/can/linker_hand_l21_can.py new file mode 100644 index 0000000..66ba97a --- /dev/null +++ b/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/core/can/linker_hand_l21_can.py @@ -0,0 +1,822 @@ +#!/usr/bin/env python3 +import can +import time, sys, os +import threading +import numpy as np +from enum import Enum +from utils.open_can import OpenCan +from utils.color_msg import ColorMsg +from can.exceptions import CanError +current_dir = os.path.dirname(os.path.abspath(__file__)) +target_dir = os.path.abspath(os.path.join(current_dir, "..")) +sys.path.append(target_dir) + +class FrameProperty(Enum): + # Finger motion control - parallel control commands + ROLL_POS = 0x01 # Roll joint position + YAWPOS = 0x02 # Yaw joint position + ROOT1_POS = 0x03 # Root joint 1 position + ROOT2_POS = 0x04 # Root joint 2 position + ROOT3_POS = 0x05 # Root joint 3 position + TIP_POS = 0x06 # Fingertip joint position + # Finger motion control - serial control commands + THUMB_POS = 0x41 # Thumb joint position + INDEX_POS = 0x42 # Index finger joint position + MIDDLE_POS = 0x43 # Middle finger joint position + RING_POS = 0x44 # Ring finger joint position + LITTLE_POS = 0x45 # Little finger joint position + + # Finger motion control - speed + ROLL_SPEED = 0x09 # Roll joint speed + YAW_SPEED = 0x0A # Yaw joint speed + ROOT1_SPEED = 0x0B # Root joint 1 speed + ROOT2_SPEED = 0x0C # Root joint 2 speed + ROOT3_SPEED = 0x0D # Root joint 3 speed + TIP_SPEED = 0x0E # Fingertip joint speed + THUMB_SPEED = 0x49 # Thumb speed + INDEX_SPEED = 0x4A # Index finger speed + MIDDLE_SPEED = 0x4B # Middle finger speed + RING_SPEED = 0x4C # Ring finger speed + LITTLE_SPEED = 0x4D # Little finger speed + + # Finger motion control - torque + ROLL_TORQUE = 0x11 # Roll joint torque + YAW_TORQUE = 0x12 # Yaw joint torque + ROOT1_TORQUE = 0x13 # Root joint 1 torque + ROOT2_TORQUE = 0x14 # Root joint 2 torque + ROOT3_TORQUE = 0x15 # Root joint 3 torque + TIP_TORQUE = 0x16 # Fingertip joint torque + THUMB_TORQUE = 0x51 # Thumb torque + INDEX_TORQUE = 0x52 # Index finger torque + MIDDLE_TORQUE = 0x53 # Middle finger torque + RING_TORQUE = 0x54 # Ring finger torque + LITTLE_TORQUE = 0x55 # Little finger torque + + THUMB_FAULT = 0x59 # Thumb fault code | Returns this type of data + INDEX_FAULT = 0x5A # Index finger fault code | Returns this type of data + MIDDLE_FAULT = 0x5B # Middle finger fault code | Returns this type of data + RING_FAULT = 0x5C # Ring finger fault code | Returns this type of data + LITTLE_FAULT = 0x5D # Little finger fault code | Returns this type of data + + # Finger faults and temperature + ROLL_FAULT = 0x19 # Roll joint fault code + YAW_FAULT = 0x1A # Yaw joint fault code + ROOT1_FAULT = 0x1B # Root joint 1 fault code + ROOT2_FAULT = 0x1C # Root joint 2 fault code + ROOT3_FAULT = 0x1D # Root joint 3 fault code + TIP_FAULT = 0x1E # Fingertip joint fault code + ROLL_TEMPERATURE = 0x21 # Roll joint over-temperature protection threshold + YAW_TEMPERATURE = 0x22 # Yaw joint over-temperature protection threshold + ROOT1_TEMPERATURE = 0x23 # Root joint 1 over-temperature protection threshold + ROOT2_TEMPERATURE = 0x24 # Root joint 2 over-temperature protection threshold + ROOT3_TEMPERATURE = 0x25 # Root joint 3 over-temperature protection threshold + TIP_TEMPERATURE = 0x26 # Fingertip joint over-temperature protection threshold + THUMB_TEMPERATURE = 0x61 # Thumb over-temperature protection threshold + INDEX_TEMPERATURE = 0x62 # Index finger over-temperature protection threshold + MIDDLE_TEMPERATURE = 0x63 # Middle finger over-temperature protection threshold + RING_TEMPERATURE = 0x64 # Ring finger over-temperature protection threshold + LITTLE_TEMPERATURE = 0x65 # Little finger over-temperature protection threshold + + # Configuration and preset actions + HAND_UID = 0xC0 # Device unique identifier + HAND_HARDWARE_VERSION = 0xC1 # Hardware version + HAND_SOFTWARE_VERSION = 0xC2 # Software version + HAND_COMM_ID = 0xC3 # Device ID + HAND_FACTORY_RESET = 0xCE # Restore factory settings + HAND_SAVE_PARAMETER = 0xCF # Save parameters + + # Tactile sensor data + HAND_NORMAL_FORCE = 0x90 # Normal force of five fingers + HAND_TANGENTIAL_FORCE = 0x91 # Tangential force of five fingers + HAND_TANGENTIAL_FORCE_DIR = 0x92 # Tangential direction of five fingers + HAND_APPROACH_INC = 0x93 # Approach sensing of five fingers + + TOUCH_SENSOR_TYPE = 0xB0 # Sensor type + THUMB_TOUCH = 0xB1 # Thumb tactile sensing + INDEX_TOUCH = 0xB2 # Index finger tactile sensing + MIDDLE_TOUCH = 0xB3 # Middle finger tactile sensing + RING_TOUCH = 0xB4 # Ring finger tactile sensing + LITTLE_TOUCH = 0xB5 # Little finger tactile sensing + PALM_TOUCH = 0xB6 # Palm tactile sensing + + # Action control + ACTION_PLAY = 0xA0 # Action + + # Combined command area + FINGER_SPEED = 0x81 # Set maximum finger speed + FINGER_TORQUE = 0x82 # Set maximum finger torque + FINGER_FAULT = 0x83 # Clear finger faults and fault codes + FINGER_TEMPERATURE = 0x84 # Finger joint temperatures + +class LinkerHandL21Can: + def __init__(self, can_channel='can0', baudrate=1000000, can_id=0x28,yaml=""): + self.can_id = can_id + self.can_channel = can_channel + self.baudrate = baudrate + self.open_can = OpenCan(load_yaml=yaml) + + self.running = True + self.last_thumb_pos, self.last_index_pos,self.last_ring_pos,self.last_middle_pos, self.last_little_pos = None,None,None,None,None + self.x01, self.x02, self.x03, self.x04,self.x05,self.x06,self.x07, self.x08,self.x09,self.x0A,self.x0B,self.x0C,self.x0D,self.x0E,self.speed = [],[],[],[],[],[],[],[],[],[],[],[],[],[],[] + self.last_root1,self.last_yaw,self.last_roll,self.last_root2,self.last_tip = None,None,None,None,None + # Speed + self.x49, self.x4a, self.x4b, self.x4c, self.x4d,self.xc1 = [],[],[],[],[],[] + self.x41,self.x42,self.x43,self.x44,self.x45 = [],[],[],[],[] + self.x83 = [-1] * 5 + # Torque + self.x51, self.x52, self.x53, self.x54,self.x55 = [],[],[],[],[] + # Fault codes + self.x59,self.x5a,self.x5b,self.x5c,self.x5d = [-1] * 5,[-1] * 5,[-1] * 5,[-1] * 5,[-1] * 5 + # Temperature thresholds + self.x61,self.x62,self.x63,self.x64,self.x65 = [],[],[],[],[] + # Pressure sensors + self.x90,self.x91,self.x92,self.x93 = [],[],[],[] + # New pressure sensors + self.xb0,self.xb1,self.xb2,self.xb3,self.xb4,self.xb5,self.xb6 = [-1] * 5,[-1] * 5,[-1] * 5,[-1] * 5,[-1] * 5,[-1] * 5,[-1] * 5 + self.thumb_matrix = np.full((12, 6), -1) + self.index_matrix = np.full((12, 6), -1) + self.middle_matrix = np.full((12, 6), -1) + self.ring_matrix = np.full((12, 6), -1) + self.little_matrix = np.full((12, 6), -1) + self.matrix_map = { + 0: 0, + 16: 1, + 32: 2, + 48: 3, + 64: 4, + 80: 5, + 96: 6, + 112: 7, + 128: 8, + 144: 9, + 160: 10, + 176: 11, + } + # Initialize CAN bus according to operating system + # try: + # if sys.platform == "linux": + # self.open_can.open_can(self.can_channel) + # time.sleep(0.1) + # self.bus = can.interface.Bus( + # channel=can_channel, interface="socketcan", bitrate=baudrate, + # can_filters=[{"can_id": can_id, "can_mask": 0x7FF}] + # ) + # elif sys.platform == "win32": + # self.bus = can.interface.Bus( + # channel=can_channel, interface='pcan', bitrate=baudrate, + # can_filters=[{"can_id": can_id, "can_mask": 0x7FF}] + # ) + # else: + # raise EnvironmentError("Unsupported platform for CAN interface") + # except: + # print("Please insert CAN device") + self.bus = self.init_can_bus(channel=self.can_channel, baudrate=baudrate) + + # Start receive thread + self.receive_thread = threading.Thread(target=self.receive_response) + self.receive_thread.daemon = True + self.receive_thread.start() + + def init_can_bus(self, channel, baudrate): + """ + 尝试按优先级连接 CAN 总线，并实现回退机制。 + """ + # --- 统一异常处理块开始 --- + try: + if sys.platform == "linux": + # Linux 优先级：1. socketcan + try: + self.open_can.open_can(self.can_channel) + # 尝试 socketcan + bus = can.interface.Bus(channel=channel, interface="socketcan", bitrate=baudrate) + ColorMsg(msg=f"成功连接: interface='socketcan', channel='{channel}'", color="green") + return bus + except CanError as e: + # 如果 socketcan 失败，可以考虑在这里尝试其他 Linux 接口 (如 'pcan') + ColorMsg(msg=f"socketcan 接口连接失败: {e}", color="yellow") + raise # 重新抛出异常，让外层 try 捕获 + elif sys.platform == "win32": + # Windows 优先级：1. pcan + try: + bus = can.interface.Bus(channel=channel, interface='pcan', bitrate=baudrate) + ColorMsg(msg=f"成功连接: interface='pcan', channel='{channel}'", color="green") + return bus + except CanError as e: + ColorMsg(msg=f"pcan 接口连接失败，尝试回退到 'candle': {e}", color="yellow") + # Windows 优先级：2. candle (回退方法) + try: + bus = can.Bus(interface="candle", channel=channel, bitrate=baudrate) + ColorMsg(msg=f"成功连接: interface='candle', channel='{channel}'", color="green") + return bus + except CanError as e: + ColorMsg(msg=f"candle 接口连接失败: {e}", color="yellow") + raise # 两个接口都失败，抛出异常 + else: + raise EnvironmentError("Unsupported platform for CAN interface") + # --- 统一异常处理块结束 --- + except Exception as e: + # 如果任何一个接口尝试失败并抛出异常（包括 EnvironmentError） + ColorMsg(msg=f"致命错误：所有 CAN 接口连接尝试均失败或平台不受支持。请检查设备连接或驱动安装和配置文件中CAN参数的配置。\n错误详情: {e}", color="red") + # 保持 raise 动作，将错误信息传递给调用者，避免程序继续运行 + raise + + def send_command(self, frame_property, data_list,sleep_time=0.003): + """ + Send command to CAN bus + :param frame_property: Data frame property + :param data_list: Data payload + """ + frame_property_value = int(frame_property.value) if hasattr(frame_property, 'value') else frame_property + data = [frame_property_value] + [int(val) for val in data_list] + msg = can.Message(arbitration_id=self.can_id, data=data, is_extended_id=False) + try: + self.bus.send(msg) + except can.CanError as e: + print(f"Failed to send message: {e}") + self.open_can.open_can(self.can_channel) + time.sleep(1) + self.is_can = self.open_can.is_can_up_sysfs(interface=self.can_channel) + time.sleep(1) + if self.is_can: + self.bus = can.interface.Bus(channel=self.can_channel, interface="socketcan", bitrate=self.baudrate) + else: + print("Reconnecting CAN devices ....") + time.sleep(sleep_time) + + def receive_response(self): + """ + Receive and process CAN bus response messages + """ + while self.running: + try: + msg = self.bus.recv(timeout=1.0) + if msg: + self.process_response(msg) + except can.CanError as e: + print(f"Error receiving message: {e}") + + + def set_joint_positions(self, joint_ranges): + if len(joint_ranges) == 25: + l21_pose = self.joint_map(joint_ranges) + # Use list comprehension to split the list into subarrays of 6 elements each + chunks = [l21_pose[i:i+6] for i in range(0, 30, 6)] + for i in range(3): + self.send_command(FrameProperty.THUMB_POS, chunks[0]) + time.sleep(0.001) + self.send_command(FrameProperty.INDEX_POS, chunks[1]) + time.sleep(0.001) + self.send_command(FrameProperty.MIDDLE_POS, chunks[2]) + time.sleep(0.001) + self.send_command(FrameProperty.RING_POS, chunks[3]) + time.sleep(0.001) + self.send_command(FrameProperty.LITTLE_POS, chunks[4]) + time.sleep(0.001) + + def set_joint_positions_by_topic(self, joint_ranges): + if len(joint_ranges) == 25: + l21_pose = self.slice_list(joint_ranges,5) + if self._list_d_value(self.last_root1, l21_pose[0]): + self.set_root1_positions(l21_pose[0]) + self.last_root1 = l21_pose[0] + if self._list_d_value(self.last_yaw, l21_pose[1]): + self.set_yaw_positions(l21_pose[1]) + self.last_yaw = l21_pose[1] + if self._list_d_value(self.last_roll, l21_pose[2]): + self.set_roll_positions(l21_pose[2]) + self.last_roll = l21_pose[2] + if self._list_d_value(self.last_root2, l21_pose[3]): + self.set_root2_positions(l21_pose[3]) + self.last_root2 = l21_pose[3] + if self._list_d_value(self.last_tip, l21_pose[4]): + self.set_tip_positions(l21_pose[4]) + self.last_tip = l21_pose[4] + + + def slice_list(self, input_list, slice_size): + """ + Slice a list into pieces of specified size. + + Args: + input_list (list): The list to be sliced. + slice_size (int): Number of elements per slice. + + Returns: + list of lists: The sliced list. + """ + sliced_list = [input_list[i:i + slice_size] for i in range(0, len(input_list), slice_size)] + return sliced_list + + def _list_d_value(self,list1, list2): + if list1 == None: + return True + for a, b in zip(list1, list2): + if abs(b - a) > 2: + return True + break + return False + # Set all finger roll joint positions + def set_roll_positions(self, joint_ranges): + self.send_command(FrameProperty.ROLL_POS, joint_ranges) + # Set all finger yaw joint positions + def set_yaw_positions(self, joint_ranges): + self.send_command(FrameProperty.YAW_POS, joint_ranges) + # Set all finger root1 joint positions + def set_root1_positions(self, joint_ranges): + self.send_command(FrameProperty.ROOT1_POS, joint_ranges) + # Set all finger root2 joint positions + def set_root2_positions(self, joint_ranges): + self.send_command(FrameProperty.ROOT2_POS, joint_ranges) + # Set all finger root3 joint positions + def set_root3_positions(self, joint_ranges): + self.send_command(FrameProperty.ROOT3_POS, joint_ranges) + # Set all finger tip joint positions + def set_tip_positions(self, joint_ranges=[80]*5): + self.send_command(FrameProperty.TIP_POS, joint_ranges) + # Set thumb torque + def set_thumb_torque(self, j=[]): + self.send_command(FrameProperty.THUMB_TORQUE, j) + # Set index finger torque + def set_index_torque(self, j=[]): + self.send_command(FrameProperty.INDEX_TORQUE, j) + # Set middle finger torque + def set_middle_torque(self, j=[]): + self.send_command(FrameProperty.MIDDLE_TORQUE, j) + # Set ring finger torque + def set_ring_torque(self, j=[]): + self.send_command(FrameProperty.RING_TORQUE, j) + # Set little finger torque + def set_little_torque(self, j=[]): + self.send_command(FrameProperty.LITTLE_TORQUE, j) + + # Get thumb joint positions + def get_thumb_positions(self,j=[0]): + self.send_command(FrameProperty.THUMB_POS, j) + # Get index finger joint positions + def get_index_positions(self, j=[0]): + self.send_command(FrameProperty.INDEX_POS,j) + # Get middle finger joint positions + def get_middle_positions(self, j=[0]): + self.send_command(FrameProperty.MIDDLE_POS,j) + # Get ring finger joint positions + def get_ring_positions(self, j=[0]): + self.send_command(FrameProperty.RING_POS,j) + # Get little finger joint positions + def get_little_positions(self, j=[0]): + self.send_command(FrameProperty.LITTLE_POS, j) + # Get all thumb motor fault codes + def get_thumbn_fault(self,j=[]): + self.send_command(FrameProperty.THUMB_FAULT,j) + # Get all index finger motor fault codes + def get_index_fault(self,j=[]): + self.send_command(FrameProperty.INDEX_FAULT,j) + # Get all middle finger motor fault codes + def get_middle_fault(self,j=[]): + self.send_command(FrameProperty.MIDDLE_FAULT,j) + # Get all ring finger motor fault codes + def get_ring_fault(self,j=[]): + self.send_command(FrameProperty.RING_FAULT,j) + # Get all little finger motor fault codes + def get_little_fault(self,j=[]): + self.send_command(FrameProperty.LITTLE_FAULT,j) + # Get thumb temperature threshold + def get_thumb_threshold(self,j=[]): + self.send_command(FrameProperty.THUMB_TEMPERATURE, '') + # Get index finger temperature threshold + def get_index_threshold(self,j=[]): + self.send_command(FrameProperty.INDEX_TEMPERATURE, j) + # Get middle finger temperature threshold + def get_middle_threshold(self,j=[]): + self.send_command(FrameProperty.MIDDLE_TEMPERATURE, j) + # Get ring finger temperature threshold + def get_ring_threshold(self,j=[]): + self.send_command(FrameProperty.RING_TEMPERATURE, j) + # Get little finger temperature threshold + def get_little_threshold(self,j=[]): + self.send_command(FrameProperty.LITTLE_TEMPERATURE, j) + + # Disable mode 01 + def set_disability_mode(self, j=[1,1,1,1,1]): + self.send_command(0x85,j) + # Enable mode 00 + def set_enable_mode(self, j=[00,00,00,00,00]): + self.send_command(0x85,j) + + # Set all finger torques + def set_torque(self,torque=[250]*5): + t = torque[0] + i = torque[1] + m = torque[2] + r = torque[3] + l = torque[4] + self.set_thumb_torque(j=[t]*5) + self.set_index_torque(j=[i]*5) + self.set_middle_torque(j=[m]*5) + self.set_ring_torque(j=[r]*5) + self.set_little_torque(j=[l]*5) + + def set_speed(self, speed): + self.speed = speed + if len(speed) < 25: + thumb_speed = [self.speed[0]]*5 + index_speed = [self.speed[1]]*5 + middle_speed = [self.speed[2]]*5 + ring_speed = [self.speed[3]]*5 + little_speed = [self.speed[4]]*5 + else: + thumb_speed = [self.speed[0],self.speed[1],self.speed[2],self.speed[3],self.speed[4]] + index_speed = [self.speed[5],self.speed[6],self.speed[7],self.speed[8],self.speed[9]] + middle_speed = [self.speed[10],self.speed[11],self.speed[12],self.speed[13],self.speed[14]] + ring_speed = [self.speed[15],self.speed[16],self.speed[17],self.speed[18],self.speed[19]] + little_speed = [self.speed[20],self.speed[21],self.speed[22],self.speed[23],self.speed[24]] + self.send_command(FrameProperty.THUMB_SPEED, thumb_speed) + self.send_command(FrameProperty.INDEX_SPEED, index_speed) + self.send_command(FrameProperty.MIDDLE_SPEED, middle_speed) + self.send_command(FrameProperty.RING_SPEED, ring_speed) + self.send_command(FrameProperty.LITTLE_SPEED, little_speed) + + def set_finger_torque(self, torque): + self.send_command(0x42, torque) + + def request_device_info(self): + self.send_command(0xC0, [0]) + self.send_command(0xC1, [0]) + self.send_command(0xC2, [0]) + + def save_parameters(self): + self.send_command(0xCF, []) + def process_response(self, msg): + if msg.arbitration_id == self.can_id: + frame_type = msg.data[0] + response_data = msg.data[1:] + if len(list(response_data)) == 0: + return + if frame_type == 0x01: + self.x01 = list(response_data) + elif frame_type == 0x02: + self.x02 = list(response_data) + elif frame_type == 0x03: + self.x03 = list(response_data) + elif frame_type == 0x04: + self.x04 = list(response_data) + elif frame_type == 0x05: + self.x05 = list(response_data) + elif frame_type == 0x06: + self.x06 = list(response_data) + elif frame_type == 0xC0: + print(f"Device ID info: {response_data}") + if self.can_id == 0x28: + self.right_hand_info = response_data + elif self.can_id == 0x27: + self.left_hand_info = response_data + elif frame_type == 0x08: + self.x08 = list(response_data) + elif frame_type == 0x09: + self.x09 = list(response_data) + elif frame_type == 0x0A: + self.x0A = list(response_data) + elif frame_type == 0x0B: + self.x0B = list(response_data) + elif frame_type == 0x0C: + self.x0C = list(response_data) + elif frame_type == 0x0D: + self.x0D = list(response_data) + elif frame_type == 0x22: + d = list(response_data) + self.tangential_force_dir = [float(i) for i in d] + elif frame_type == 0x23: + d = list(response_data) + self.approach_inc = [float(i) for i in d] + elif frame_type == 0x41: + self.x41 = list(response_data) + elif frame_type == 0x42: + self.x42 = list(response_data) + elif frame_type == 0x43: + self.x43 = list(response_data) + elif frame_type == 0x44: + self.x44 = list(response_data) + elif frame_type == 0x45: + self.x45 = list(response_data) + elif frame_type == 0x49: + self.x49 = list(response_data) + elif frame_type == 0x4a: + self.x4a = list(response_data) + elif frame_type == 0x4b: + self.x4b = list(response_data) + elif frame_type == 0x4c: + self.x4c = list(response_data) + elif frame_type == 0x4d: + self.x4d = list(response_data) + elif frame_type == 0xc1: + self.xc1 = list(response_data) + elif frame_type == 0x51: + self.x51 = list(response_data) + elif frame_type == 0x52: + self.x52 = list(response_data) + elif frame_type == 0x53: + self.x53 = list(response_data) + elif frame_type == 0x54: + self.x54 = list(response_data) + elif frame_type == 0x55: + self.x55 = list(response_data) + elif frame_type == 0x59: + self.x59 = list(response_data) + elif frame_type == 0x5a: + self.x5a = list(response_data) + elif frame_type == 0x5b: + self.x5b = list(response_data) + elif frame_type == 0x5c: + self.x5c = list(response_data) + elif frame_type == 0x5d: + self.x5d = list(response_data) + elif frame_type == 0x61: + self.x61 = list(response_data) + elif frame_type == 0x62: + self.x62 = list(response_data) + elif frame_type == 0x63: + self.x63 = list(response_data) + elif frame_type == 0x64: + self.x64 = list(response_data) + elif frame_type == 0x65: + self.x65 = list(response_data) + elif frame_type == 0x83: + self.x83 = list(response_data) + elif frame_type == 0x90: + self.x90 = list(response_data) + elif frame_type == 0x91: + self.x91 = list(response_data) + elif frame_type == 0x92: + self.x92 = list(response_data) + elif frame_type == 0x93: + self.x93 = list(response_data) + elif frame_type == 0xb0: + self.xb0 = list(response_data) + elif frame_type == 0xb1: + d = list(response_data) + if len(d) == 2: + self.xb1 = d + elif len(d) == 7: + index = self.matrix_map.get(d[0]) + if index is not None: + self.thumb_matrix[index] = d[1:] + elif frame_type == 0xb2: + d = list(response_data) + if len(d) == 2: + self.xb2 = d + elif len(d) == 7: + index = self.matrix_map.get(d[0]) + if index is not None: + self.index_matrix[index] = d[1:] + elif frame_type == 0xb3: + d = list(response_data) + if len(d) == 2: + self.xb3 = d + elif len(d) == 7: + index = self.matrix_map.get(d[0]) + if index is not None: + self.middle_matrix[index] = d[1:] + elif frame_type == 0xb4: + d = list(response_data) + if len(d) == 2: + self.xb4 = d + elif len(d) == 7: + index = self.matrix_map.get(d[0]) + if index is not None: + self.ring_matrix[index] = d[1:] + elif frame_type == 0xb5: + d = list(response_data) + if len(d) == 2: + self.xb5 = d + elif len(d) == 7: + index = self.matrix_map.get(d[0]) + if index is not None: + self.little_matrix[index] = d[1:] + elif frame_type == 0xb6: + self.xb6 = list(response_data) + + def joint_map(self, pose): + # l21 CAN data by default receives 30 data + l21_pose = [0.0] * 30 + + mapping = { + 0: 10, 1: 5, 2: 0, 3: 15, 4: None, 5: 20, + 6: None, 7: 6, 8: 1, 9: 16, 10: None, 11: 21, + 12: None, 13: 7, 14: 2, 15: 17, 16: None, 17: 22, + 18: None, 19: 8, 20: 3, 21: 18, 22: None, 23: 23, + 24: None, 25: 9, 26: 4, 27: 19, 28: None, 29: 24 + } + + for l21_idx, pose_idx in mapping.items(): + if pose_idx is not None: + l21_pose[l21_idx] = pose[pose_idx] + + return l21_pose + + def state_to_cmd(self, l21_state): + pose = [0.0] * 25 + + mapping = { + 0: 10, 1: 5, 2: 0, 3: 15, 5: 20, 7: 6, + 8: 1, 9: 16, 11: 21, 13:7, 14: 2, 15: 17, 17: 22, + 19: 8, 20: 3, 21: 18, 23: 23, 25: 9, 26: 4, + 27: 19, 29: 24 + } + for l21_idx, pose_idx in mapping.items(): + pose[pose_idx] = l21_state[l21_idx] + return pose + def action_play(self): + self.send_command(0xA0,[]) + def get_current_status(self, j=''): + self.send_command(FrameProperty.THUMB_POS, j,sleep_time=0.001) + self.send_command(FrameProperty.INDEX_POS,j,sleep_time=0.001) + self.send_command(FrameProperty.MIDDLE_POS,j,sleep_time=0.001) + self.send_command(FrameProperty.RING_POS,j,sleep_time=0.001) + self.send_command(FrameProperty.LITTLE_POS, j,sleep_time=0.001) + state= self.x41+ self.x42+ self.x43+ self.x44+ self.x45 + if len(state) == 30: + l21_state = self.state_to_cmd(l21_state=state) + return l21_state + + def get_current_pub_status(self): + state= self.x41+ self.x42+ self.x43+ self.x44+ self.x45 + if len(state) == 30: + l21_state = self.state_to_cmd(l21_state=state) + return l21_state + + def get_current_state_topic(self): + self.send_command(0x01,[]) + self.send_command(0x02,[]) + self.send_command(0x03,[]) + self.send_command(0x04,[]) + self.send_command(0x06,[]) + state = self.x03+self.x02+self.x01+self.x04+self.x06 + return state + + def get_speed(self,j=''): + self.send_command(FrameProperty.THUMB_SPEED, j) + self.send_command(FrameProperty.INDEX_SPEED, j) + self.send_command(FrameProperty.MIDDLE_SPEED, j) + self.send_command(FrameProperty.RING_SPEED, j) + self.send_command(FrameProperty.LITTLE_SPEED, j) + speed = self.x49+ self.x4a+ self.x4b+ self.x4c+ self.x4d + if len(speed) == 30: + l21_speed = self.state_to_cmd(l21_state=speed) + return l21_speed + + # def get_finger_torque(self): + # return self.finger_torque() + def get_fault(self): + self.get_thumbn_fault() + self.get_index_fault() + self.get_middle_fault() + self.get_ring_fault() + self.get_little_fault() + return [self.x59]+[self.x5a]+[self.x5b]+[self.x5c]+[self.x5d] + def get_threshold(self): + self.get_thumb_threshold() + self.get_index_threshold() + self.get_middle_threshold() + self.get_ring_threshold() + self.get_little_threshold() + return [self.x61]+[self.x62]+[self.x63]+[self.x64]+[self.x65] + def get_version(self): + if self.xc1 == []: + self.send_command(FrameProperty.HAND_HARDWARE_VERSION,[]) + return self.xc1 + def get_normal_force(self): + self.send_command(FrameProperty.HAND_NORMAL_FORCE,[]) + return self.x90 + def get_tangential_force(self): + self.send_command(FrameProperty.HAND_TANGENTIAL_FORCE,[]) + return self.x91 + def get_tangential_force_dir(self): + self.send_command(FrameProperty.HAND_TANGENTIAL_FORCE_DIR,[]) + return self.x92 + def get_approach_inc(self): + self.send_command(FrameProperty.HAND_APPROACH_INC,[]) + return self.x93 + + def get_touch_type(self): + '''Get tactile sensor type data''' + self.send_command(FrameProperty.TOUCH_SENSOR_TYPE,[]) + try: + return self.xb0[0] + except: + pass + def get_finger_torque(self): + self.send_command(FrameProperty.THUMB_TORQUE,[]) + self.send_command(FrameProperty.INDEX_TORQUE,[]) + self.send_command(FrameProperty.MIDDLE_TORQUE,[]) + self.send_command(FrameProperty.RING_TORQUE,[]) + self.send_command(FrameProperty.LITTLE_TORQUE,[]) + return self.x51+self.x52+self.x53+self.x54+self.x55 + + def get_torque(self): + return self.get_finger_torque() + + def get_thumb_touch(self): + '''Get thumb tactile sensor data''' + self.send_command(FrameProperty.THUMB_TOUCH,[],sleep_time=0.015) + return self.xb1 + + def get_index_touch(self): + '''Get index finger tactile sensor data''' + self.send_command(FrameProperty.INDEX_TOUCH,[0xc6],sleep_time=0.015) + return self.xb2 + + def get_middle_touch(self): + '''Get middle finger tactile sensor data''' + self.send_command(FrameProperty.MIDDLE_TOUCH,[],sleep_time=0.015) + return self.xb3 + + def get_ring_touch(self): + '''Get ring finger tactile sensor data''' + self.send_command(FrameProperty.RING_TOUCH,[],sleep_time=0.015) + return self.xb4 + + def get_little_touch(self): + '''Get little finger tactile sensor data''' + self.send_command(FrameProperty.LITTLE_TOUCH,[],sleep_time=0.015) + return self.xb5 + + def get_palm_touch(self): + '''Get palm tactile sensor data''' + self.send_command(FrameProperty.PALM_TOUCH,[],sleep_time=0.015) + return self.xb6 + + def get_force(self): + '''Get pressure sensor data''' + return [self.x90,self.x91 , self.x92 , self.x93] + + def get_touch(self): + '''Get tactile sensor data''' + self.get_thumb_touch() + self.get_index_touch() + self.get_middle_touch() + self.get_ring_touch() + self.get_little_touch() + self.get_palm_touch() + try: + return [self.xb1[1],self.xb2[1] , self.xb3[1] , self.xb4[1],self.xb5[1],self.xb6[1]] + except: + pass + + def get_matrix_touch(self): + self.send_command(0xb1,[0xc6],sleep_time=0.04) + self.send_command(0xb2,[0xc6],sleep_time=0.04) + self.send_command(0xb3,[0xc6],sleep_time=0.04) + self.send_command(0xb4,[0xc6],sleep_time=0.04) + self.send_command(0xb5,[0xc6],sleep_time=0.04) + return self.thumb_matrix , self.index_matrix , self.middle_matrix , self.ring_matrix , self.little_matrix + + def get_current(self): + '''Not supported yet''' + return [0] * 21 + def get_temperature(self): + self.get_thumb_threshold() + self.get_index_threshold() + self.get_middle_threshold() + self.get_ring_threshold() + self.get_little_threshold() + return self.x61+self.x62+self.x63+self.x64+self.x65 + + def get_serial_number(self): + return [0] * 6 + + def get_finger_order(self): + return [ + "thumb_root", + "index_finger_root", + "middle_finger_root", + "ring_finger_root", + "little_finger_root", + "thumb_abduction", + "index_finger_abduction", + "middle_finger_abduction", + "ring_finger_abduction", + "little_finger_abduction", + "thumb_roll", + "reserved", + "reserved", + "reserved", + "reserved", + "thumb_middle_joint", + "reserved", + "reserved", + "reserved", + "reserved", + "thumb_tip", + "index_finger_tip", + "middle_finger_tip", + "ring_finger_tip", + "little_finger_tip" + ] + + def clear_faults(self): + '''Clear motor faults''' + self.send_command(0x83, [1, 1, 1, 1, 1],sleep_time=0.003) + return self.x83 + + def close_can_interface(self): + if self.bus: + self.bus.shutdown() # Close CAN bus diff --git a/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/core/can/linker_hand_l24_can.py b/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/core/can/linker_hand_l24_can.py new file mode 100644 index 0000000..d5192f6 --- /dev/null +++ b/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/core/can/linker_hand_l24_can.py @@ -0,0 +1,448 @@ +#!/usr/bin/env python3 +import can +import time,sys,os +import threading +import numpy as np +from enum import Enum +current_dir = os.path.dirname(os.path.abspath(__file__)) +target_dir = os.path.abspath(os.path.join(current_dir, "..")) +sys.path.append(target_dir) +from utils.color_msg import ColorMsg + +class FrameProperty(Enum): + INVALID_FRAME_PROPERTY = 0x00 # 无效的can帧属性 | 无返回 + # 并行指令区域 + ROLL_POS = 0x01 # 横滚关节位置 | 坐标系建在每个手指的指根部位，按手指伸直的状态去定义旋转角度 + YAW_POS = 0x02 # 航向关节位置 | 坐标系建在每个手指的指根部位，按手指伸直的状态去定义旋转角度 + ROOT1_POS = 0x03 # 指根1关节位置 | 最接近手掌的指根关节 + ROOT2_POS = 0x04 # 指根2关节位置 | 最接近手掌的指根关节 + ROOT3_POS = 0x05 # 指根3关节位置 | 最接近手掌的指根关节 + TIP_POS = 0x06 # 指尖关节位置 | 最接近手掌的指根关节 + + ROLL_SPEED = 0x09 # 横滚关节速度 | 坐标系建在每个手指的指根部位，按手指伸直的状态去定义旋转角度 + YAW_SPEED = 0x0A # 航向关节速度 | 坐标系建在每个手指的指根部位，按手指伸直的状态去定义旋转角度 + ROOT1_SPEED = 0x0B # 指根1关节速度 | 最接近手掌的指根关节 + ROOT2_SPEED = 0x0C # 指根2关节速度 | 最接近手掌的指根关节 + ROOT3_SPEED = 0x0D # 指根3关节速度 | 最接近手掌的指根关节 + TIP_SPEED = 0x0E # 指尖关节速度 | 最接近手掌的指根关节 + + ROLL_TORQUE = 0x11 # 横滚关节扭矩 | 坐标系建在每个手指的指根部位，按手指伸直的状态去定义旋转角度 + YAW_TORQUE = 0x12 # 航向关节扭矩 | 坐标系建在每个手指的指根部位，按手指伸直的状态去定义旋转角度 + ROOT1_TORQUE = 0x13 # 指根1关节扭矩 | 最接近手掌的指根关节 + ROOT2_TORQUE = 0x14 # 指根2关节扭矩 | 最接近手掌的指根关节 + ROOT3_TORQUE = 0x15 # 指根3关节扭矩 | 最接近手掌的指根关节 + TIP_TORQUE = 0x16 # 指尖关节扭矩 | 最接近手掌的指根关节 + + ROLL_FAULT = 0x19 # 横滚关节故障码 | 坐标系建在每个手指的指根部位，按手指伸直的状态去定义旋转角度 + YAW_FAULT = 0x1A # 航向关节故障码 | 坐标系建在每个手指的指根部位，按手指伸直的状态去定义旋转角度 + ROOT1_FAULT = 0x1B # 指根1关节故障码 | 最接近手掌的指根关节 + ROOT2_FAULT = 0x1C # 指根2关节故障码 | 最接近手掌的指根关节 + ROOT3_FAULT = 0x1D # 指根3关节故障码 | 最接近手掌的指根关节 + TIP_FAULT = 0x1E # 指尖关节故障码 | 最接近手掌的指根关节 + + ROLL_TEMPERATURE = 0x21 # 横滚关节温度 | 坐标系建在每个手指的指根部位，按手指伸直的状态去定义旋转角度 + YAW_TEMPERATURE = 0x22 # 航向关节温度 | 坐标系建在每个手指的指根部位，按手指伸直的状态去定义旋转角度 + ROOT1_TEMPERATURE = 0x23 # 指根1关节温度 | 最接近手掌的指根关节 + ROOT2_TEMPERATURE = 0x24 # 指根2关节温度 | 最接近手掌的指根关节 + ROOT3_TEMPERATURE = 0x25 # 指根3关节温度 | 最接近手掌的指根关节 + TIP_TEMPERATURE = 0x26 # 指尖关节温度 | 最接近手掌的指根关节 + # 并行指令区域 + + # 串行指令区域 + THUMB_POS = 0x41 # 大拇指指关节位置 | 返回本类型数据 + INDEX_POS = 0x42 # 食指关节位置 | 返回本类型数据 + MIDDLE_POS = 0x43 # 中指关节位置 | 返回本类型数据 + RING_POS = 0x44 # 无名指关节位置 | 返回本类型数据 + LITTLE_POS = 0x45 # 小拇指关节位置 | 返回本类型数据 + + THUMB_SPEED = 0x49 # 大拇指速度 | 返回本类型数据 + INDEX_SPEED = 0x4A # 食指速度 | 返回本类型数据 + MIDDLE_SPEED = 0x4B # 中指速度 | 返回本类型数据 + RING_SPEED = 0x4C # 无名指速度 | 返回本类型数据 + LITTLE_SPEED = 0x4D # 小拇指速度 | 返回本类型数据 + + THUMB_TORQUE = 0x51 # 大拇指扭矩 | 返回本类型数据 + INDEX_TORQUE = 0x52 # 食指扭矩 | 返回本类型数据 + MIDDLE_TORQUE = 0x53 # 中指扭矩 | 返回本类型数据 + RING_TORQUE = 0x54 # 无名指扭矩 | 返回本类型数据 + LITTLE_TORQUE = 0x55 # 小拇指扭矩 | 返回本类型数据 + + THUMB_FAULT = 0x59 # 大拇指故障码 | 返回本类型数据 + INDEX_FAULT = 0x5A # 食指故障码 | 返回本类型数据 + MIDDLE_FAULT = 0x5B # 中指故障码 | 返回本类型数据 + RING_FAULT = 0x5C # 无名指故障码 | 返回本类型数据 + LITTLE_FAULT = 0x5D # 小拇指故障码 | 返回本类型数据 + + THUMB_TEMPERATURE = 0x61 # 大拇指温度 | 返回本类型数据 + INDEX_TEMPERATURE = 0x62 # 食指温度 | 返回本类型数据 + MIDDLE_TEMPERATURE = 0x63 # 中指温度 | 返回本类型数据 + RING_TEMPERATURE = 0x64 # 无名指温度 | 返回本类型数据 + LITTLE_TEMPERATURE = 0x65 # 小拇指温度 | 返回本类型数据 + # 串行指令区域 + + # 合并指令区域，同一手指非必要单控数据合并 + FINGER_SPEED = 0x81 # 手指速度 | 返回本类型数据 + FINGER_TORQUE = 0x82 # 转矩 | 返回本类型数据 + FINGER_FAULT = 0x83 # 手指故障码 | 返回本类型数据 + + # 指尖传感器数据组 + HAND_NORMAL_FORCE = 0x90 # 五指法向压力 + HAND_TANGENTIAL_FORCE = 0x91 # 五指切向压力 + HAND_TANGENTIAL_FORCE_DIR = 0x92 # 五指切向方向 + HAND_APPROACH_INC = 0x93 # 五指接近感应 + + THUMB_ALL_DATA = 0x98 # 大拇指所有数据 + INDEX_ALL_DATA = 0x99 # 食指所有数据 + MIDDLE_ALL_DATA = 0x9A # 中指所有数据 + RING_ALL_DATA = 0x9B # 无名指所有数据 + LITTLE_ALL_DATA = 0x9C # 小拇指所有数据 + # 动作指令 ·ACTION + ACTION_PLAY = 0xA0 # 动作 + + # 配置命令·CONFIG + HAND_UID = 0xC0 # 设备唯一标识码 + HAND_HARDWARE_VERSION = 0xC1 # 硬件版本 + HAND_SOFTWARE_VERSION = 0xC2 # 软件版本 + HAND_COMM_ID = 0xC3 # 设备id + HAND_FACTORY_RESET = 0xCE # 恢复出厂设置 + HAND_SAVE_PARAMETER = 0xCF # 保存参数 + + WHOLE_FRAME = 0xF0 # 整帧传输 | 返回一字节帧属性+整个结构体485及网络传输专属 + +class LinkerHandL24Can: + def __init__(self, config, can_channel='can0', baudrate=1000000, can_id=0x28): + self.config = config + self.can_id = can_id + self.running = True + self.x01, self.x02, self.x03, self.x04,self.x05,self.x06,self.x07, self.x08,self.x09,self.x0A,self.x0B,self.x0C,self.x0D,self.x0E,self.speed = [],[],[],[],[],[],[],[],[],[],[],[],[],[],[] + # 速度 + self.x49, self.x4a, self.x4b, self.x4c, self.x4d = [],[],[],[],[] + self.x41,self.x42,self.x43,self.x44,self.x45 = [],[],[],[],[] + # 根据操作系统初始化 CAN 总线 + if sys.platform == "linux": + self.bus = can.interface.Bus( + channel=can_channel, interface="socketcan", bitrate=baudrate, + can_filters=[{"can_id": can_id, "can_mask": 0x7FF}] + ) + elif sys.platform == "win32": + self.bus = can.interface.Bus( + channel=can_channel, interface='pcan', bitrate=baudrate, + can_filters=[{"can_id": can_id, "can_mask": 0x7FF}] + ) + else: + raise EnvironmentError("Unsupported platform for CAN interface") + + # 根据 can_id 初始化 publisher 和相关参数 + if can_id == 0x28: # 左手 + self.hand_exists = config['LINKER_HAND']['LEFT_HAND']['EXISTS'] + self.hand_joint = config['LINKER_HAND']['LEFT_HAND']['JOINT'] + self.hand_names = config['LINKER_HAND']['LEFT_HAND']['NAME'] + elif can_id == 0x27: # 右手 + + self.hand_exists = config['LINKER_HAND']['RIGHT_HAND']['EXISTS'] + self.hand_joint = config['LINKER_HAND']['RIGHT_HAND']['JOINT'] + self.hand_names = config['LINKER_HAND']['RIGHT_HAND']['NAME'] + + + # 启动接收线程 + self.receive_thread = threading.Thread(target=self.receive_response) + self.receive_thread.daemon = True + self.receive_thread.start() + + def send_command(self, frame_property, data_list): + """ + 发送命令到 CAN 总线 + :param frame_property: 数据帧属性 + :param data_list: 数据载荷 + """ + frame_property_value = int(frame_property.value) if hasattr(frame_property, 'value') else frame_property + data = [frame_property_value] + [int(val) for val in data_list] + msg = can.Message(arbitration_id=self.can_id, data=data, is_extended_id=False) + try: + self.bus.send(msg) + #print(f"Message sent: ID={hex(self.can_id)}, Data={data}") + except can.CanError as e: + print(f"Failed to send message: {e}") + time.sleep(0.002) + + def receive_response(self): + """ + 接收并处理 CAN 总线的响应消息 + """ + while self.running: + try: + msg = self.bus.recv(timeout=1.0) # 阻塞接收，1 秒超时 + if msg: + self.process_response(msg) + except can.CanError as e: + print(f"Error receiving message: {e}") + + + def set_joint_positions(self, joint_ranges): + if len(joint_ranges) == 25: + l24_pose = self.joint_map(joint_ranges) + # 使用列表推导式将列表每6个元素切成一个子数组 + chunks = [l24_pose[i:i+6] for i in range(0, 30, 6)] + self.send_command(FrameProperty.THUMB_POS, chunks[0]) + self.send_command(FrameProperty.INDEX_POS, chunks[1]) + self.send_command(FrameProperty.MIDDLE_POS, chunks[2]) + self.send_command(FrameProperty.RING_POS, chunks[3]) + self.send_command(FrameProperty.LITTLE_POS, chunks[4]) + #self.set_tip_positions(joint_ranges[:5]) + #print(l24_pose) + + # 设置所有手指横滚关节位置 + def set_roll_positions(self, joint_ranges): + self.send_command(FrameProperty.ROLL_POS, joint_ranges) + # 设置所有手指航向关节位置 + def set_yaw_positions(self, joint_ranges): + self.send_command(FrameProperty.YAW_POS, joint_ranges) + # 设置所有手指指根1关节位置 + def set_root1_positions(self, joint_ranges): + self.send_command(FrameProperty.ROOT1_POS, joint_ranges) + # 设置所有手指指根2关节位置 + def set_root2_positions(self, joint_ranges): + self.send_command(FrameProperty.ROOT2_POS, joint_ranges) + # 设置所有手指指根3关节位置 + def set_root3_positions(self, joint_ranges): + self.send_command(FrameProperty.ROOT3_POS, joint_ranges) + # 设置所有手指指尖关节位置 + def set_tip_positions(self, joint_ranges=[80]*5): + self.send_command(FrameProperty.TIP_POS, joint_ranges) + # 获取大拇指指关节位置 + def get_thumb_positions(self,j=[0]): + self.send_command(FrameProperty.THUMB_POS, j) + # 获取食指关节位置 + def get_index_positions(self, j=[0]): + self.send_command(FrameProperty.INDEX_POS,j) + # 获取中指关节位置 + def get_middle_positions(self, j=[0]): + self.send_command(FrameProperty.MIDDLE_POS,j) + # 获取无名指关节位置 + def get_ring_positions(self, j=[0]): + self.send_command(FrameProperty.RING_POS,j) + # 获取小拇指关节位置 + def get_little_positions(self, j=[0]): + self.send_command(FrameProperty.LITTLE_POS, j) + # 失能01模式 + def set_disability_mode(self, j=[1,1,1,1,1]): + self.send_command(0x85,j) + # 使能00模式 + def set_enable_mode(self, j=[00,00,00,00,00]): + self.send_command(0x85,j) + + + def set_speed(self, speed): + self.speed = [speed]*6 + ColorMsg(msg=f"L24设置速度为:{self.speed}", color="yellow") + self.send_command(FrameProperty.THUMB_SPEED, self.speed) + self.send_command(FrameProperty.INDEX_SPEED, self.speed) + self.send_command(FrameProperty.MIDDLE_SPEED, self.speed) + self.send_command(FrameProperty.RING_SPEED, self.speed) + self.send_command(FrameProperty.LITTLE_SPEED, self.speed) + + def set_finger_torque(self, torque): + self.send_command(0x42, torque) + + def request_device_info(self): + self.send_command(0xC0, [0]) + self.send_command(0xC1, [0]) + self.send_command(0xC2, [0]) + + def save_parameters(self): + self.send_command(0xCF, []) + def process_response(self, msg): + if msg.arbitration_id == self.can_id: + frame_type = msg.data[0] + response_data = msg.data[1:] + if frame_type == 0x01: + self.x01 = list(response_data) + elif frame_type == 0x02: + self.x02 = list(response_data) + elif frame_type == 0x03: + self.x03 = list(response_data) + elif frame_type == 0x04: + self.x04 = list(response_data) + elif frame_type == 0x05: + self.x05 = list(response_data) + elif frame_type == 0x06: + self.x06 = list(response_data) + print("_-"*20) + print(self.x06) + elif frame_type == 0xC0: + print(f"Device ID info: {response_data}") + if self.can_id == 0x28: + self.right_hand_info = response_data + elif self.can_id == 0x27: + self.left_hand_info = response_data + elif frame_type == 0x08: + self.x08 = list(response_data) + elif frame_type == 0x09: + self.x09 = list(response_data) + elif frame_type == 0x0A: + self.x0A = list(response_data) + elif frame_type == 0x0B: + self.x0B = list(response_data) + elif frame_type == 0x0C: + self.x0C = list(response_data) + elif frame_type == 0x0D: + self.x0D = list(response_data) + elif frame_type == 0x22: + #ColorMsg(msg=f"五指切向压力方向：{list(response_data)}") + d = list(response_data) + self.tangential_force_dir = [float(i) for i in d] + elif frame_type == 0x23: + #ColorMsg(msg=f"五指接近度：{list(response_data)}") + d = list(response_data) + self.approach_inc = [float(i) for i in d] + elif frame_type == 0x41: # 拇指关节位置返回值 + self.x41 = list(response_data) + elif frame_type == 0x42: # 食指关节位置返回值 + self.x42 = list(response_data) + elif frame_type == 0x43: # 中指关节位置返回值 + self.x43 = list(response_data) + elif frame_type == 0x44: # 无名指关节位置返回值 + self.x44 = list(response_data) + elif frame_type == 0x45: # 小拇指关节位置返回值 + self.x45 = list(response_data) + elif frame_type == 0x49: # 拇指速度返回值 + self.x49 = list(response_data) + elif frame_type == 0x4a: # 食指速度返回值 + self.x4a = list(response_data) + elif frame_type == 0x4b: # 中指速度返回值 + self.x4b = list(response_data) + elif frame_type == 0x4c: # 无名指速度返回值 + self.x4c = list(response_data) + elif frame_type == 0x4d: # 小拇指速度返回值 + self.x4d = list(response_data) + + # topic映射L24 + def joint_map(self, pose): + # L24 CAN数据默认接收30个数据 + l24_pose = [0.0] * 30 # 初始化l24_pose为30个0.0 + + # 映射表，通过字典简化映射关系 + mapping = { + 0: 10, 1: 5, 2: 0, 3: 15, 4: None, 5: 20, + 6: None, 7: 6, 8: 1, 9: 16, 10: None, 11: 21, + 12: None, 13: None, 14: 2, 15: 17, 16: None, 17: 22, + 18: None, 19: 8, 20: 3, 21: 18, 22: None, 23: 23, + 24: None, 25: 9, 26: 4, 27: 19, 28: None, 29: 24 + } + + # 遍历映射字典，进行值的映射 + for l24_idx, pose_idx in mapping.items(): + if pose_idx is not None: + l24_pose[l24_idx] = pose[pose_idx] + + return l24_pose + + # 将L24的状态值转换为CMD格式的状态值 + def state_to_cmd(self, l24_state): + # L24 CAN默认接收30个数据，初始化pose为25个0.0 + pose = [0.0] * 25 # 原来控制L24的指令数据为25个 + + # 映射关系，字典中存储l24_state索引和pose索引之间的映射关系 + mapping = { + 0: 10, 1: 5, 2: 0, 3: 15, 5: 20, 7: 6, + 8: 1, 9: 16, 11: 21, 14: 2, 15: 17, 17: 22, + 19: 8, 20: 3, 21: 18, 23: 23, 25: 9, 26: 4, + 27: 19, 29: 24 + } + # 遍历映射字典，更新pose的值 + for l24_idx, pose_idx in mapping.items(): + pose[pose_idx] = l24_state[l24_idx] + return pose + + # 获取所有关节数据 + def get_current_status(self, j=''): + time.sleep(0.01) + self.send_command(FrameProperty.THUMB_POS, j) + self.send_command(FrameProperty.INDEX_POS,j) + self.send_command(FrameProperty.MIDDLE_POS,j) + self.send_command(FrameProperty.RING_POS,j) + self.send_command(FrameProperty.LITTLE_POS, j) + #return self.x41, self.x42, self.x43, self.x44, self.x45 + time.sleep(0.1) + state= self.x41+ self.x42+ self.x43+ self.x44+ self.x45 + if len(state) == 30: + l24_state = self.state_to_cmd(l24_state=state) + return l24_state + + def get_speed(self,j=''): + time.sleep(0.1) + self.send_command(FrameProperty.THUMB_SPEED, j) # 大拇指速度 + self.send_command(FrameProperty.INDEX_SPEED, j) # 食指速度 + self.send_command(FrameProperty.MIDDLE_SPEED, j) # 中指速度 + self.send_command(FrameProperty.RING_SPEED, j) # 无名指速度 + self.send_command(FrameProperty.LITTLE_SPEED, j) # 小拇指速度 + speed = self.x49+ self.x4a+ self.x4b+ self.x4c+ self.x4d + if len(speed) == 30: + l24_speed = self.state_to_cmd(l24_state=speed) + return l24_speed + + def get_finger_torque(self): + return self.finger_torque + # def get_current(self): + # return self.x06 + # def get_fault(self): + # return self.x07 + + def clear_faults(self, finger_mask=[1, 1, 1, 1, 1]): + """L24 暂不支持清除故障码""" + pass + + def close_can_interface(self): + if self.bus: + self.bus.shutdown() # 关闭 CAN 总线 + + ''' + 这个方法只用于展示数据关系映射，使用的话最好使用上面的方法 + ''' + def joint_map_2(self, pose): + l24_pose = [0.0]*30 #L24 CAN默认接收30个数据 pose控制L24发送的指令数据默认25个，这里进行映射 + ''' + 需要进行映射 + # L24 CAN数据格式 + #["拇指横摆0-10", "拇指侧摆1-5", "拇指根部2-0", "拇指中部3-15", "预留4-", "拇指指尖5-20", "预留6-", "食指侧摆7-6", "食指根部8-1", "食指中部9-16", "预留10-", "食指指尖11-21", "预留12-", "预留13-", "中指根部14-2", "中指中部15-17", "预留16-", "中指指尖17-22", "预留18-", "无名指侧摆19-8", "无名指根部20-3", "无名指中部21-18", "预留22-", "无名指指尖23-23", "预留24-", "小指侧摆25-9", "小指根部26-4", "小指中部27-19", "预留28-", "小指指尖29-24"] + # CMD 接收到的数据格式 + #["拇指根部0", "食指根部1", "中指根部2", "无名指根部3","小指根部4","拇指侧摆5","食指侧摆6","中指侧摆","无名指侧摆8","小指侧摆9","拇指横摆10","预留","预留","预留","预留","拇指中部15","食指中部16","中指中部17","无名指中部18","小指中部19","拇指指尖20","食指指尖21","中指指尖22","无名指指尖23","小指指尖24"] + ''' + l24_pose[0] = pose[10] + l24_pose[1] = pose[5] + l24_pose[2] = pose[0] + l24_pose[3] = pose[15] + l24_pose[4] = 0.0 + l24_pose[5] = pose[20] + l24_pose[6] = 0.0 + l24_pose[7] = pose[6] + l24_pose[8] = pose[1] + l24_pose[9] = pose[16] + l24_pose[10] = 0.0 + l24_pose[11] = pose[21] + l24_pose[12] = 0.0 + l24_pose[13] = 0.0 + l24_pose[14] = pose[2] + l24_pose[15] = pose[17] + l24_pose[16] = 0.0 + l24_pose[17] = pose[22] + l24_pose[18] = 0.0 + l24_pose[19] = pose[8] + l24_pose[20] = pose[3] + l24_pose[21] = pose[18] + l24_pose[22] = 0.0 + l24_pose[23] = pose[23] + l24_pose[24] = 0.0 + l24_pose[25] = pose[9] + l24_pose[26] = pose[4] + l24_pose[27] = pose[19] + l24_pose[28] = 0.0 + l24_pose[29] = pose[24] + return l24_pose + + def get_finger_order(self): + return [] + def get_serial_number(self): + return [0] * 6 + def show_fun_table(self): + pass \ No newline at end of file diff --git a/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/core/can/linker_hand_l25_can.py b/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/core/can/linker_hand_l25_can.py new file mode 100644 index 0000000..97f91b4 --- /dev/null +++ b/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/core/can/linker_hand_l25_can.py @@ -0,0 +1,848 @@ +#!/usr/bin/env python3 +import can +import time,sys,os +import threading +import numpy as np +from enum import Enum +from utils.open_can import OpenCan +from utils.color_msg import ColorMsg +from can.exceptions import CanError +current_dir = os.path.dirname(os.path.abspath(__file__)) +target_dir = os.path.abspath(os.path.join(current_dir, "..")) +sys.path.append(target_dir) + +class FrameProperty(Enum): + INVALID_FRAME_PROPERTY = 0x00 # Invalid CAN frame property | No response + # Parallel command area + ROLL_POS = 0x01 # Roll joint position | The coordinate system is built at the root of each finger, and the rotation angle is defined according to the straightened state of the finger [10,11,12,13,14] + YAW_POS = 0x02 # Yaw joint position | The coordinate system is built at the root of each finger, and the rotation angle is defined according to the straightened state of the finger [5,6,7,8,9] + ROOT1_POS = 0x03 # Root1 joint position | The root joint closest to the palm [0,1,2,3,4] + ROOT2_POS = 0x04 # Root2 joint position | The root joint closest to the palm [15, 16,17,18,19] + ROOT3_POS = 0x05 # Root3 joint position | The root joint closest to the palm Not available + TIP_POS = 0x06 # Fingertip joint position | The root joint closest to the palm [20,21,22,23,24] + + ROLL_SPEED = 0x09 # Roll joint speed | The coordinate system is built at the root of each finger, and the rotation angle is defined according to the straightened state of the finger + YAW_SPEED = 0x0A # Yaw joint speed | The coordinate system is built at the root of each finger, and the rotation angle is defined according to the straightened state of the finger + ROOT1_SPEED = 0x0B # Root1 joint speed | The root joint closest to the palm + ROOT2_SPEED = 0x0C # Root2 joint speed | The root joint closest to the palm + ROOT3_SPEED = 0x0D # Root3 joint speed | The root joint closest to the palm + TIP_SPEED = 0x0E # Fingertip joint speed | The root joint closest to the palm + + ROLL_TORQUE = 0x11 # Roll joint torque | The coordinate system is built at the root of each finger, and the rotation angle is defined according to the straightened state of the finger + YAW_TORQUE = 0x12 # Yaw joint torque | The coordinate system is built at the root of each finger, and the rotation angle is defined according to the straightened state of the finger + ROOT1_TORQUE = 0x13 # Root1 joint torque | The root joint closest to the palm + ROOT2_TORQUE = 0x14 # Root2 joint torque | The root joint closest to the palm + ROOT3_TORQUE = 0x15 # Root3 joint torque | The root joint closest to the palm + TIP_TORQUE = 0x16 # Fingertip joint torque | The root joint closest to the palm + + ROLL_FAULT = 0x19 # Roll joint fault code | The coordinate system is built at the root of each finger, and the rotation angle is defined according to the straightened state of the finger + YAW_FAULT = 0x1A # Yaw joint fault code | The coordinate system is built at the root of each finger, and the rotation angle is defined according to the straightened state of the finger + ROOT1_FAULT = 0x1B # Root1 joint fault code | The root joint closest to the palm + ROOT2_FAULT = 0x1C # Root2 joint fault code | The root joint closest to the palm + ROOT3_FAULT = 0x1D # Root3 joint fault code | The root joint closest to the palm + TIP_FAULT = 0x1E # Fingertip joint fault code | The root joint closest to the palm + + ROLL_TEMPERATURE = 0x21 # Roll joint temperature | The coordinate system is built at the root of each finger, and the rotation angle is defined according to the straightened state of the finger + YAW_TEMPERATURE = 0x22 # Yaw joint temperature | The coordinate system is built at the root of each finger, and the rotation angle is defined according to the straightened state of the finger + ROOT1_TEMPERATURE = 0x23 # Root1 joint temperature | The root joint closest to the palm + ROOT2_TEMPERATURE = 0x24 # Root2 joint temperature | The root joint closest to the palm + ROOT3_TEMPERATURE = 0x25 # Root3 joint temperature | The root joint closest to the palm + TIP_TEMPERATURE = 0x26 # Fingertip joint temperature | The root joint closest to the palm + # Parallel command area + + # Serial command area + THUMB_POS = 0x41 # Thumb joint position | Returns this type of data + INDEX_POS = 0x42 # Index finger joint position | Returns this type of data + MIDDLE_POS = 0x43 # Middle finger joint position | Returns this type of data + RING_POS = 0x44 # Ring finger joint position | Returns this type of data + LITTLE_POS = 0x45 # Little finger joint position | Returns this type of data + + THUMB_SPEED = 0x49 # Thumb speed | Returns this type of data + INDEX_SPEED = 0x4A # Index finger speed | Returns this type of data + MIDDLE_SPEED = 0x4B # Middle finger speed | Returns this type of data + RING_SPEED = 0x4C # Ring finger speed | Returns this type of data + LITTLE_SPEED = 0x4D # Little finger speed | Returns this type of data + + THUMB_TORQUE = 0x51 # Thumb torque | Returns this type of data + INDEX_TORQUE = 0x52 # Index finger torque | Returns this type of data + MIDDLE_TORQUE = 0x53 # Middle finger torque | Returns this type of data + RING_TORQUE = 0x54 # Ring finger torque | Returns this type of data + LITTLE_TORQUE = 0x55 # Little finger torque | Returns this type of data + + THUMB_FAULT = 0x59 # Thumb fault code | Returns this type of data + INDEX_FAULT = 0x5A # Index finger fault code | Returns this type of data + MIDDLE_FAULT = 0x5B # Middle finger fault code | Returns this type of data + RING_FAULT = 0x5C # Ring finger fault code | Returns this type of data + LITTLE_FAULT = 0x5D # Little finger fault code | Returns this type of data + + THUMB_TEMPERATURE = 0x61 # Thumb temperature | Returns this type of data + INDEX_TEMPERATURE = 0x62 # Index finger temperature | Returns this type of data + MIDDLE_TEMPERATURE = 0x63 # Middle finger temperature | Returns this type of data + RING_TEMPERATURE = 0x64 # Ring finger temperature | Returns this type of data + LITTLE_TEMPERATURE = 0x65 # Little finger temperature | Returns this type of data + # Serial command area + + # Merged command area, non-essential single control data of the same finger is merged + FINGER_SPEED = 0x81 # Finger speed | Returns this type of data + FINGER_TORQUE = 0x82 # Torque | Returns this type of data + FINGER_FAULT = 0x83 # Finger fault code | Returns this type of data + + # Fingertip sensor data group + HAND_NORMAL_FORCE = 0x90 # Normal force of five fingers + HAND_TANGENTIAL_FORCE = 0x91 # Tangential force of five fingers + HAND_TANGENTIAL_FORCE_DIR = 0x92 # Tangential direction of five fingers + HAND_APPROACH_INC = 0x93 # Proximity sensing of five fingers + + THUMB_ALL_DATA = 0x98 # All data of thumb + INDEX_ALL_DATA = 0x99 # All data of index finger + MIDDLE_ALL_DATA = 0x9A # All data of middle finger + RING_ALL_DATA = 0x9B # All data of ring finger + LITTLE_ALL_DATA = 0x9C # All data of little finger + # Action command ·ACTION + ACTION_PLAY = 0xA0 # Action + + # Configuration command ·CONFIG + HAND_UID = 0xC0 # Device unique identifier + HAND_HARDWARE_VERSION = 0xC1 # Hardware version + HAND_SOFTWARE_VERSION = 0xC2 # Software version + HAND_COMM_ID = 0xC3 # Device id + HAND_FACTORY_RESET = 0xCE # Restore factory settings + HAND_SAVE_PARAMETER = 0xCF # Save parameters + + WHOLE_FRAME = 0xF0 # Whole frame transmission | Returns one byte frame property + the entire structure for 485 and network transmission only + +class LinkerHandL25Can: + def __init__(self, can_channel='can0', baudrate=1000000, can_id=0x28,yaml=""): + self.can_id = can_id + self.can_channel = can_channel + self.baudrate = baudrate + self.open_can = OpenCan(load_yaml=yaml) + + self.running = True + self.last_thumb_pos, self.last_index_pos,self.last_ring_pos,self.last_middle_pos, self.last_little_pos = None,None,None,None,None + self.x01, self.x02, self.x03, self.x04,self.x05,self.x06,self.x07, self.x08,self.x09,self.x0A,self.x0B,self.x0C,self.x0D,self.x0E,self.speed = [],[],[],[],[],[],[],[],[],[],[],[],[],[],[] + self.last_root1,self.last_yaw,self.last_roll,self.last_root2,self.last_tip = None,None,None,None,None + # 速度 + self.x49, self.x4a, self.x4b, self.x4c, self.x4d,self.xc1 = [],[],[],[],[],[] + self.x41,self.x42,self.x43,self.x44,self.x45 = [],[],[],[],[] + # 扭矩 + self.x51, self.x52, self.x53, self.x54,self.x55 = [],[],[],[],[] + # 故障码 + self.x59,self.x5a,self.x5b,self.x5c,self.x5d = [],[],[],[],[] + # 温度阈值 + self.x61,self.x62,self.x63,self.x64,self.x65 = [],[],[],[],[] + # 压感 + self.x90,self.x91,self.x92,self.x93 = [],[],[],[] + # 新压感 + self.xb0,self.xb1,self.xb2,self.xb3,self.xb4,self.xb5 = [-1] * 5,[-1] * 5,[-1] * 5,[-1] * 5,[-1] * 5,[-1] * 5 + self.thumb_matrix = np.full((12, 6), -1) + self.index_matrix = np.full((12, 6), -1) + self.middle_matrix = np.full((12, 6), -1) + self.ring_matrix = np.full((12, 6), -1) + self.little_matrix = np.full((12, 6), -1) + self.matrix_map = { + 0: 0, + 16: 1, + 32: 2, + 48: 3, + 64: 4, + 80: 5, + 96: 6, + 112: 7, + 128: 8, + 144: 9, + 160: 10, + 176: 11, + } + # 根据操作系统初始化 CAN 总线 + # try: + # if sys.platform == "linux": + # self.open_can.open_can(self.can_channel) + # time.sleep(0.1) + # self.bus = can.interface.Bus( + # channel=can_channel, interface="socketcan", bitrate=baudrate, + # can_filters=[{"can_id": can_id, "can_mask": 0x7FF}] + # ) + # elif sys.platform == "win32": + # self.bus = can.interface.Bus( + # channel=can_channel, interface='pcan', bitrate=baudrate, + # can_filters=[{"can_id": can_id, "can_mask": 0x7FF}] + # ) + # else: + # raise EnvironmentError("Unsupported platform for CAN interface") + # except: + # print("Please insert CAN device") + self.bus = self.init_can_bus(channel=self.can_channel, baudrate=baudrate) + # 启动接收线程 + self.receive_thread = threading.Thread(target=self.receive_response) + self.receive_thread.daemon = True + self.receive_thread.start() + + def init_can_bus(self, channel, baudrate): + """ + 尝试按优先级连接 CAN 总线，并实现回退机制。 + """ + # --- 统一异常处理块开始 --- + try: + if sys.platform == "linux": + # Linux 优先级：1. socketcan + try: + self.open_can.open_can(self.can_channel) + # 尝试 socketcan + bus = can.interface.Bus(channel=channel, interface="socketcan", bitrate=baudrate) + ColorMsg(msg=f"成功连接: interface='socketcan', channel='{channel}'", color="green") + return bus + except CanError as e: + # 如果 socketcan 失败，可以考虑在这里尝试其他 Linux 接口 (如 'pcan') + ColorMsg(msg=f"socketcan 接口连接失败: {e}", color="yellow") + raise # 重新抛出异常，让外层 try 捕获 + elif sys.platform == "win32": + # Windows 优先级：1. pcan + try: + bus = can.interface.Bus(channel=channel, interface='pcan', bitrate=baudrate) + ColorMsg(msg=f"成功连接: interface='pcan', channel='{channel}'", color="green") + return bus + except CanError as e: + ColorMsg(msg=f"pcan 接口连接失败，尝试回退到 'candle': {e}", color="yellow") + # Windows 优先级：2. candle (回退方法) + try: + bus = can.Bus(interface="candle", channel=channel, bitrate=baudrate) + ColorMsg(msg=f"成功连接: interface='candle', channel='{channel}'", color="green") + return bus + except CanError as e: + ColorMsg(msg=f"candle 接口连接失败: {e}", color="yellow") + raise # 两个接口都失败，抛出异常 + else: + raise EnvironmentError("Unsupported platform for CAN interface") + # --- 统一异常处理块结束 --- + except Exception as e: + # 如果任何一个接口尝试失败并抛出异常（包括 EnvironmentError） + ColorMsg(msg=f"致命错误：所有 CAN 接口连接尝试均失败或平台不受支持。请检查设备连接或驱动安装和配置文件中CAN参数的配置。\n错误详情: {e}", color="red") + # 保持 raise 动作，将错误信息传递给调用者，避免程序继续运行 + raise + + def send_command(self, frame_property, data_list): + """ + Send command to CAN bus + :param frame_property: Data frame properties + :param data_list: Data payload + """ + frame_property_value = int(frame_property.value) if hasattr(frame_property, 'value') else frame_property + data = [frame_property_value] + [int(val) for val in data_list] + msg = can.Message(arbitration_id=self.can_id, data=data, is_extended_id=False) + try: + self.bus.send(msg) + #print(f"Message sent: ID={hex(self.can_id)}, Data={data}") + except can.CanError as e: + print(f"Failed to send message: {e}") + self.open_can.open_can(self.can_channel) + time.sleep(1) + self.is_can = self.open_can.is_can_up_sysfs(interface=self.can_channel) + time.sleep(1) + if self.is_can: + self.bus = can.interface.Bus(channel=self.can_channel, interface="socketcan", bitrate=self.baudrate) + else: + print("Reconnecting CAN devices ....") + time.sleep(0.001) + + def receive_response(self): + """ + Receive and process response messages from CAN bus + """ + while self.running: + try: + msg = self.bus.recv(timeout=1.0) # 阻塞接收，1 秒超时 + if msg: + self.process_response(msg) + except can.CanError as e: + print(f"Error receiving message: {e}") + + + def set_joint_positions(self, joint_ranges): + if len(joint_ranges) == 25: + l25_pose = self.joint_map(joint_ranges) + # 使用列表推导式将列表每6个元素切成一个子数组 + chunks = [l25_pose[i:i+6] for i in range(0, 30, 6)] + self.send_command(FrameProperty.THUMB_POS, chunks[0]) + #time.sleep(0.001) + self.send_command(FrameProperty.INDEX_POS, chunks[1]) + #time.sleep(0.001) + self.send_command(FrameProperty.MIDDLE_POS, chunks[2]) + #time.sleep(0.001) + self.send_command(FrameProperty.RING_POS, chunks[3]) + #time.sleep(0.001) + self.send_command(FrameProperty.LITTLE_POS, chunks[4]) + #time.sleep(0.001) + + def set_joint_positions_by_topic(self, joint_ranges): + if len(joint_ranges) == 25: + # Finger Joint Position Constants + #ROLL_POS = 0x01 # Roll joint position | Coordinate system based on finger base, rotation angle defined when finger is straight [10,11,12,13,14] + #YAW_POS = 0x02 # Yaw joint position | Coordinate system based on finger base, rotation angle defined when finger is straight [5,6,7,8,9] + #ROOT1_POS = 0x03 # Root1 joint position | Joint closest to the palm [0,1,2,3,4] + #ROOT2_POS = 0x04 # Root2 joint position | Joint closest to the palm [15,16,17,18,19] + #ROOT3_POS = 0x05 # Root3 joint position | Joint closest to the palm (currently unused) + #TIP_POS = 0x06 # Tip joint position | Joint closest to the palm [20,21,22,23,24] + + # Finger joint names mapping (Chinese to English translation): + # ["Thumb root", "Index root", "Middle root", "Ring root", "Pinky root", + # "Thumb yaw", "Index yaw", "Middle yaw", "Ring yaw", "Pinky yaw", + # "Thumb roll", "Reserved", "Reserved", "Reserved", "Reserved", + # "Thumb middle", "Index middle", "Middle middle", "Ring middle", "Pinky middle", + # "Thumb tip", "Index tip", "Middle tip", "Ring tip", "Pinky tip"] + + + l25_pose = self.slice_list(joint_ranges,5) + if self._list_d_value(self.last_root1, l25_pose[0]): + self.set_root1_positions(l25_pose[0]) + self.last_root1 = l25_pose[0] + if self._list_d_value(self.last_yaw, l25_pose[1]): + self.set_yaw_positions(l25_pose[1]) + self.last_yaw = l25_pose[1] + if self._list_d_value(self.last_roll, l25_pose[2]): + self.set_roll_positions(l25_pose[2]) + self.last_roll = l25_pose[2] + if self._list_d_value(self.last_root2, l25_pose[3]): + self.set_root2_positions(l25_pose[3]) + self.last_root2 = l25_pose[3] + if self._list_d_value(self.last_tip, l25_pose[4]): + self.set_tip_positions(l25_pose[4]) + self.last_tip = l25_pose[4] + + + def slice_list(self, input_list, slice_size): + """ + Split a list into chunks of specified size. + + Parameters: + input_list (list): The list to be chunked. + slice_size (int): Number of elements in each chunk. + + Returns: + list of lists: The chunked list. + """ + # Implementation using list comprehension + sliced_list = [input_list[i:i + slice_size] for i in range(0, len(input_list), slice_size)] + return sliced_list + + def _list_d_value(self,list1, list2): + if list1 == None: + return True + for a, b in zip(list1, list2): + if abs(b - a) > 2: + return True + break + return False + # Set roll joint positions for all fingers + def set_roll_positions(self, joint_ranges): + self.send_command(FrameProperty.ROLL_POS, joint_ranges) + # Set yaw joint positions for all fingers + def set_yaw_positions(self, joint_ranges): + print(joint_ranges) + self.send_command(FrameProperty.YAW_POS, joint_ranges) + # Set base joint 1 positions for all fingers + def set_root1_positions(self, joint_ranges): + self.send_command(FrameProperty.ROOT1_POS, joint_ranges) + # Set base joint 2 positions for all fingers + def set_root2_positions(self, joint_ranges): + self.send_command(FrameProperty.ROOT2_POS, joint_ranges) + # Set base joint 3 positions for all fingers + def set_root3_positions(self, joint_ranges): + self.send_command(FrameProperty.ROOT3_POS, joint_ranges) + # Set fingertip joint positions for all fingers + def set_tip_positions(self, joint_ranges=[80]*5): + self.send_command(FrameProperty.TIP_POS, joint_ranges) + # Set thumb torque parameters + def set_thumb_torque(self, j=[]): + self.send_command(FrameProperty.THUMB_TORQUE, j) + # Set index finger torque + def set_index_torque(self, j=[]): + self.send_command(FrameProperty.INDEX_TORQUE, j) + # Set middle finger torque + def set_middle_torque(self, j=[]): + self.send_command(FrameProperty.MIDDLE_TORQUE, j) + # Set ring finger torque + def set_ring_torque(self, j=[]): + self.send_command(FrameProperty.RING_TORQUE, j) + # Set little finger torque + def set_little_torque(self, j=[]): + self.send_command(FrameProperty.LITTLE_TORQUE, j) + + # Get thumb joint position + def get_thumb_positions(self,j=[0]): + self.send_command(FrameProperty.THUMB_POS, j) + # Get index finger joint positions + def get_index_positions(self, j=[0]): + self.send_command(FrameProperty.INDEX_POS,j) + # Get middle finger joint position + def get_middle_positions(self, j=[0]): + self.send_command(FrameProperty.MIDDLE_POS,j) + # Retrieve the position of the ring finger joint + def get_ring_positions(self, j=[0]): + self.send_command(FrameProperty.RING_POS,j) + # Retrieve the position of the little finger joint + def get_little_positions(self, j=[0]): + self.send_command(FrameProperty.LITTLE_POS, j) + # All fault codes of motors in the thumb + def get_thumbn_fault(self,j=[]): + self.send_command(FrameProperty.THUMB_FAULT,j) + # All motor fault codes for the index finger + def get_index_fault(self,j=[]): + self.send_command(FrameProperty.INDEX_FAULT,j) + # All motor fault codes for the middle finger + def get_middle_fault(self,j=[]): + self.send_command(FrameProperty.MIDDLE_FAULT,j) + # All motor fault codes for the ring finger + def get_ring_fault(self,j=[]): + self.send_command(FrameProperty.RING_FAULT,j) + # All motor fault codes for the little finger + def get_little_fault(self,j=[]): + self.send_command(FrameProperty.LITTLE_FAULT,j) + # Temperature threshold for the thumb motors + def get_thumb_threshold(self,j=[]): + self.send_command(FrameProperty.THUMB_TEMPERATURE, '') + # Temperature threshold for the index finger motors + def get_index_threshold(self,j=[]): + self.send_command(FrameProperty.INDEX_TEMPERATURE, j) + # Temperature threshold for the middle finger motors + def get_middle_threshold(self,j=[]): + self.send_command(FrameProperty.MIDDLE_TEMPERATURE, j) + # Temperature threshold for the ring finger motors + def get_ring_threshold(self,j=[]): + self.send_command(FrameProperty.RING_TEMPERATURE, j) + # Little finger temperature threshold + def get_little_threshold(self,j=[]): + self.send_command(FrameProperty.LITTLE_TEMPERATURE, j) + + + def set_disability_mode(self, j=[1,1,1,1,1]): + self.send_command(0x85,j) + + def set_enable_mode(self, j=[00,00,00,00,00]): + self.send_command(0x85,j) + + # Set torque for all fingers + def set_torque(self,torque=[250]*5): + t = torque[0] + i = torque[1] + m = torque[2] + r = torque[3] + l = torque[4] + self.set_thumb_torque(j=[t]*5) + self.set_index_torque(j=[i]*5) + self.set_middle_torque(j=[m]*5) + self.set_ring_torque(j=[r]*5) + self.set_little_torque(j=[l]*5) + + def set_speed(self, speed): + self.speed = speed + if len(speed) < 25: + thumb_speed = [self.speed[0]]*5 + index_speed = [self.speed[1]]*5 + middle_speed = [self.speed[2]]*5 + ring_speed = [self.speed[3]]*5 + little_speed = [self.speed[4]]*5 + else: + thumb_speed = [self.speed[0],self.speed[1],self.speed[2],self.speed[3],self.speed[4]] + index_speed = [self.speed[5],self.speed[6],self.speed[7],self.speed[8],self.speed[9]] + middle_speed = [self.speed[10],self.speed[11],self.speed[12],self.speed[13],self.speed[14]] + ring_speed = [self.speed[15],self.speed[16],self.speed[17],self.speed[18],self.speed[19]] + little_speed = [self.speed[20],self.speed[21],self.speed[22],self.speed[23],self.speed[24]] + self.send_command(FrameProperty.THUMB_SPEED, thumb_speed) + self.send_command(FrameProperty.INDEX_SPEED, index_speed) + self.send_command(FrameProperty.MIDDLE_SPEED, middle_speed) + self.send_command(FrameProperty.RING_SPEED, ring_speed) + self.send_command(FrameProperty.LITTLE_SPEED, little_speed) + + def set_finger_torque(self, torque): + self.send_command(0x42, torque) + + def request_device_info(self): + self.send_command(0xC0, [0]) + self.send_command(0xC1, [0]) + self.send_command(0xC2, [0]) + + def save_parameters(self): + self.send_command(0xCF, []) + def process_response(self, msg): + if msg.arbitration_id == self.can_id: + frame_type = msg.data[0] + response_data = msg.data[1:] + if len(list(response_data)) == 0: + return + if frame_type == 0x01: + self.x01 = list(response_data) + elif frame_type == 0x02: + self.x02 = list(response_data) + elif frame_type == 0x03: + self.x03 = list(response_data) + elif frame_type == 0x04: + self.x04 = list(response_data) + elif frame_type == 0x05: + self.x05 = list(response_data) + elif frame_type == 0x06: + self.x06 = list(response_data) + elif frame_type == 0xC0: + print(f"Device ID info: {response_data}") + if self.can_id == 0x28: + self.right_hand_info = response_data + elif self.can_id == 0x27: + self.left_hand_info = response_data + elif frame_type == 0x08: + self.x08 = list(response_data) + elif frame_type == 0x09: + self.x09 = list(response_data) + elif frame_type == 0x0A: + self.x0A = list(response_data) + elif frame_type == 0x0B: + self.x0B = list(response_data) + elif frame_type == 0x0C: + self.x0C = list(response_data) + elif frame_type == 0x0D: + self.x0D = list(response_data) + elif frame_type == 0x22: + d = list(response_data) + self.tangential_force_dir = [float(i) for i in d] + elif frame_type == 0x23: + d = list(response_data) + self.approach_inc = [float(i) for i in d] + elif frame_type == 0x41: + self.x41 = list(response_data) + elif frame_type == 0x42: + + self.x42 = list(response_data) + elif frame_type == 0x43: + self.x43 = list(response_data) + elif frame_type == 0x44: + + self.x44 = list(response_data) + elif frame_type == 0x45: + self.x45 = list(response_data) + elif frame_type == 0x49: + self.x49 = list(response_data) + elif frame_type == 0x4a: + self.x4a = list(response_data) + elif frame_type == 0x4b: + self.x4b = list(response_data) + elif frame_type == 0x4c: + self.x4c = list(response_data) + elif frame_type == 0x4d: + self.x4d = list(response_data) + elif frame_type == 0xc1: + self.xc1 = list(response_data) + elif frame_type == 0x51: + self.x51 = list(response_data) + elif frame_type == 0x52: + self.x52 = list(response_data) + elif frame_type == 0x53: + self.x53 = list(response_data) + elif frame_type == 0x54: + self.x54 = list(response_data) + elif frame_type == 0x55: + self.x55 = list(response_data) + elif frame_type == 0x59: + self.x59 = list(response_data) + elif frame_type == 0x5a: + self.x5a = list(response_data) + elif frame_type == 0x5b: + self.x5b = list(response_data) + elif frame_type == 0x5c: + self.x5c = list(response_data) + elif frame_type == 0x5d: + self.x5d = list(response_data) + elif frame_type == 0x61: + self.x61 = list(response_data) + elif frame_type == 0x62: + self.x62 = list(response_data) + elif frame_type == 0x63: + self.x63 = list(response_data) + elif frame_type == 0x64: + self.x64 = list(response_data) + elif frame_type == 0x65: + self.x65 = list(response_data) + elif frame_type == 0x90: + self.x90 = list(response_data) + elif frame_type == 0x91: + self.x91 = list(response_data) + elif frame_type == 0x92: + self.x92 = list(response_data) + elif frame_type == 0x93: + self.x93 = list(response_data) + elif frame_type == 0xb0: + self.xb0 = list(response_data) + elif frame_type == 0xb1: + d = list(response_data) + if len(d) == 2: + self.xb1 = d + elif len(d) == 7: + index = self.matrix_map.get(d[0]) + if index is not None: + self.thumb_matrix[index] = d[1:] + elif frame_type == 0xb2: + d = list(response_data) + if len(d) == 2: + self.xb2 = d + elif len(d) == 7: + index = self.matrix_map.get(d[0]) + if index is not None: + self.index_matrix[index] = d[1:] + elif frame_type == 0xb3: + d = list(response_data) + if len(d) == 2: + self.xb3 = d + elif len(d) == 7: + index = self.matrix_map.get(d[0]) + if index is not None: + self.middle_matrix[index] = d[1:] + elif frame_type == 0xb4: + d = list(response_data) + if len(d) == 2: + self.xb4 = d + elif len(d) == 7: + index = self.matrix_map.get(d[0]) + if index is not None: + self.ring_matrix[index] = d[1:] + elif frame_type == 0xb5: + d = list(response_data) + if len(d) == 2: + self.xb5 = d + elif len(d) == 7: + index = self.matrix_map.get(d[0]) + if index is not None: + self.little_matrix[index] = d[1:] + + + def joint_map(self, pose): + l25_pose = [0.0] * 30 + + # 映射表，通过字典简化映射关系 + mapping = { + 0: 10, 1: 5, 2: 0, 3: 15, 4: None, 5: 20, + 6: None, 7: 6, 8: 1, 9: 16, 10: None, 11: 21, + 12: None, 13: 7, 14: 2, 15: 17, 16: None, 17: 22, + 18: None, 19: 8, 20: 3, 21: 18, 22: None, 23: 23, + 24: None, 25: 9, 26: 4, 27: 19, 28: None, 29: 24 + } + + # 遍历映射字典，进行值的映射 + for l25_idx, pose_idx in mapping.items(): + if pose_idx is not None: + l25_pose[l25_idx] = pose[pose_idx] + + return l25_pose + + + def state_to_cmd(self, l25_state): + + pose = [0.0] * 25 + mapping = { + 0: 10, 1: 5, 2: 0, 3: 15, 5: 20, 7: 6, + 8: 1, 9: 16, 11: 21, 13:7, 14: 2, 15: 17, 17: 22, + 19: 8, 20: 3, 21: 18, 23: 23, 25: 9, 26: 4, + 27: 19, 29: 24 + } + # 遍历映射字典，更新pose的值 + for l25_idx, pose_idx in mapping.items(): + pose[pose_idx] = l25_state[l25_idx] + return pose + def action_play(self): + self.send_command(0xA0,[]) + + def get_current_status(self, j=''): + self.send_command(FrameProperty.THUMB_POS, j) + #time.sleep(0.001) + self.send_command(FrameProperty.INDEX_POS,j) + #time.sleep(0.001) + self.send_command(FrameProperty.MIDDLE_POS,j) + #time.sleep(0.001) + self.send_command(FrameProperty.RING_POS,j) + #time.sleep(0.001) + self.send_command(FrameProperty.LITTLE_POS, j) + #time.sleep(0.001) + state= self.x41+ self.x42+ self.x43+ self.x44+ self.x45 + if len(state) == 30: + l25_state = self.state_to_cmd(l25_state=state) + return l25_state + + def get_current_pub_status(self): + state= self.x41+ self.x42+ self.x43+ self.x44+ self.x45 + if len(state) == 30: + l25_state = self.state_to_cmd(l25_state=state) + return l25_state + + def get_current_state_topic(self): + self.send_command(0x01,[]) + #time.sleep(0.001) + self.send_command(0x02,[]) + # time.sleep(0.001) + self.send_command(0x03,[]) + #time.sleep(0.001) + self.send_command(0x04,[]) + #time.sleep(0.001) + self.send_command(0x06,[]) + #time.sleep(0.001) + state = self.x03+self.x02+self.x01+self.x04+self.x06 + return state + def get_speed(self,j=''): + self.send_command(FrameProperty.THUMB_SPEED, j) + #time.sleep(0.01) + self.send_command(FrameProperty.INDEX_SPEED, j) + #time.sleep(0.01) + self.send_command(FrameProperty.MIDDLE_SPEED, j) + #time.sleep(0.01) + self.send_command(FrameProperty.RING_SPEED, j) + #time.sleep(0.01) + self.send_command(FrameProperty.LITTLE_SPEED, j) + #time.sleep(0.01) + speed = self.x49+ self.x4a+ self.x4b+ self.x4c+ self.x4d + if len(speed) == 30: + l25_speed = self.state_to_cmd(l25_state=speed) + return l25_speed + + def get_finger_torque(self): + self.send_command(FrameProperty.THUMB_TORQUE,[]) + self.send_command(FrameProperty.INDEX_TORQUE,[]) + self.send_command(FrameProperty.MIDDLE_TORQUE,[]) + self.send_command(FrameProperty.RING_TORQUE,[]) + self.send_command(FrameProperty.LITTLE_TORQUE,[]) + return self.x51+self.x52+self.x53+self.x54+self.x55 + + def get_torque(self): + return self.get_finger_torque() + def get_fault(self): + self.get_thumbn_fault() + #time.sleep(0.001) + self.get_index_fault() + #time.sleep(0.001) + self.get_middle_fault() + #time.sleep(0.001) + self.get_ring_fault() + #time.sleep(0.001) + self.get_little_fault() + #time.sleep(0.001) + return [self.x59]+[self.x5a]+[self.x5b]+[self.x5c]+[self.x5d] + def get_threshold(self): + self.get_thumb_threshold() + self.get_index_threshold() + self.get_middle_threshold() + self.get_ring_threshold() + self.get_little_threshold() + return [self.x61]+[self.x62]+[self.x63]+[self.x64]+[self.x65] + def get_version(self): + if self.xc1 == []: + self.send_command(FrameProperty.HAND_HARDWARE_VERSION,[]) + return self.xc1 + def get_normal_force(self): + self.send_command(FrameProperty.HAND_NORMAL_FORCE,[]) + return self.x90 + def get_tangential_force(self): + self.send_command(FrameProperty.HAND_TANGENTIAL_FORCE,[]) + return self.x91 + def get_tangential_force_dir(self): + self.send_command(FrameProperty.HAND_TANGENTIAL_FORCE_DIR,[]) + return self.x92 + def get_approach_inc(self): + self.send_command(FrameProperty.HAND_APPROACH_INC,[]) + return self.x93 + def get_force(self): + '''获取压感数据''' + return [self.x90,self.x91 , self.x92 , self.x93] + + def get_matrix_touch(self): + self.send_command(0xb1,[0xc6]) + time.sleep(0.03) + self.send_command(0xb2,[0xc6]) + time.sleep(0.03) + self.send_command(0xb3,[0xc6]) + time.sleep(0.03) + self.send_command(0xb4,[0xc6]) + time.sleep(0.03) + self.send_command(0xb5,[0xc6]) + time.sleep(0.03) + return self.thumb_matrix , self.index_matrix , self.middle_matrix , self.ring_matrix , self.little_matrix + + def get_touch_type(self): + '''Get touch type''' + self.send_command(0xb1,[]) + time.sleep(0.03) + if len(self.xb1) == 2: + return 2 + else: + return -1 + + def get_touch(self): + '''Get touch data (not supported yet)''' + return [-1] * 6 + + + def get_current(self): + return [0] * 21 + def get_temperature(self): + self.get_thumb_threshold() + self.get_index_threshold() + self.get_middle_threshold() + self.get_ring_threshold() + self.get_little_threshold() + return [self.x61]+[self.x62]+[self.x63]+[self.x64]+[self.x65] + + def get_finger_order(self): + return ["Thumb root", "Index root", "Middle root", "Ring root", "Little root", + "Thumb abduction", "Index abduction", "Middle abduction", "Ring abduction", "Little abduction", + "Thumb roll", "Reserved", "Reserved", "Reserved", "Reserved", + "Thumb middle", "Index middle", "Middle middle", "Ring middle", "Little middle", + "Thumb tip", "Index tip", "Middle tip", "Ring tip", "Little tip"] + + def close_can_interface(self): + if self.bus: + self.bus.shutdown() + def clear_faults(self, finger_mask=[1, 1, 1, 1, 1]): + """L25 暂不支持清除故障码""" + pass + ''' + 这个方法只用于展示数据关系映射，使用的话最好使用上面的方法 + ''' + def joint_map_2(self, pose): + l25_pose = [0.0]*30 #L25 CAN默认接收30个数据 pose控制L25发送的指令数据默认25个，这里进行映射 + ''' + 需要进行映射 + # L25 CAN数据格式 + #["拇指横摆0-10", "拇指侧摆1-5", "拇指根部2-0", "拇指中部3-15", "预留4-", "拇指指尖5-20", "预留6-", "食指侧摆7-6", "食指根部8-1", "食指中部9-16", "预留10-", "食指指尖11-21", "预留12-", "预留13-", "中指根部14-2", "中指中部15-17", "预留16-", "中指指尖17-22", "预留18-", "无名指侧摆19-8", "无名指根部20-3", "无名指中部21-18", "预留22-", "无名指指尖23-23", "预留24-", "小指侧摆25-9", "小指根部26-4", "小指中部27-19", "预留28-", "小指指尖29-24"] + # CMD 接收到的数据格式 + #["拇指根部0", "食指根部1", "中指根部2", "无名指根部3","小指根部4","拇指侧摆5","食指侧摆6","中指侧摆","无名指侧摆8","小指侧摆9","拇指横摆10","预留","预留","预留","预留","拇指中部15","食指中部16","中指中部17","无名指中部18","小指中部19","拇指指尖20","食指指尖21","中指指尖22","无名指指尖23","小指指尖24"] + ''' + l25_pose[0] = pose[10] + l25_pose[1] = pose[5] + l25_pose[2] = pose[0] + l25_pose[3] = pose[15] + l25_pose[4] = 0.0 + l25_pose[5] = pose[20] + l25_pose[6] = 0.0 + l25_pose[7] = pose[6] + l25_pose[8] = pose[1] + l25_pose[9] = pose[16] + l25_pose[10] = 0.0 + l25_pose[11] = pose[21] + l25_pose[12] = 0.0 + l25_pose[13] = 0.0 + l25_pose[14] = pose[2] + l25_pose[15] = pose[17] + l25_pose[16] = 0.0 + l25_pose[17] = pose[22] + l25_pose[18] = 0.0 + l25_pose[19] = pose[8] + l25_pose[20] = pose[3] + l25_pose[21] = pose[18] + l25_pose[22] = 0.0 + l25_pose[23] = pose[23] + l25_pose[24] = 0.0 + l25_pose[25] = pose[9] + l25_pose[26] = pose[4] + l25_pose[27] = pose[19] + l25_pose[28] = 0.0 + l25_pose[29] = pose[24] + return l25_pose + + def get_serial_number(self): + return [0] * 6 + def show_fun_table(self): + pass \ No newline at end of file diff --git a/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/core/can/linker_hand_l6_can.py b/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/core/can/linker_hand_l6_can.py new file mode 100644 index 0000000..d5cca5d --- /dev/null +++ b/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/core/can/linker_hand_l6_can.py @@ -0,0 +1,426 @@ +import can +import time, sys +import threading +import numpy as np +from utils.open_can import OpenCan +from utils.color_msg import ColorMsg +from can.exceptions import CanError + + +class LinkerHandL6Can: + def __init__(self, can_id, can_channel='can0', baudrate=1000000,yaml=""): + self.can_id = can_id + self.can_channel = can_channel + self.baudrate = baudrate + self.open_can = OpenCan(load_yaml=yaml) + + self.x01 = [0] * 6 # 关节位置 + self.x02 = [-1] * 6 # 转矩限制 + self.x05 = [0] * 6 # 速度 + self.x07 = [-1] * 6 # 加速度 + self.x33 = [0] * 6 # 温度 + self.x35 = [0] * 6 # 关节错误码 + self.x36 = [-1] * 6 # 电流 + self.xb0,self.xb1,self.xb2,self.xb3,self.xb4,self.xb5 = [-1] * 5,[-1] * 5,[-1] * 5,[-1] * 5,[-1] * 5,[-1] * 5 + + self.thumb_matrix = np.full((12, 6), -1) + self.index_matrix = np.full((12, 6), -1) + self.middle_matrix = np.full((12, 6), -1) + self.ring_matrix = np.full((12, 6), -1) + self.little_matrix = np.full((12, 6), -1) + self.matrix_map = { + 0: 0, + 16: 1, + 32: 2, + 48: 3, + 64: 4, + 80: 5, + 96: 6, + 112: 7, + 128: 8, + 144: 9, + 160: 10, + 176: 11, + } + self.serial_number = [] + self.serial_number_map = { + 0: 0, + 1: 1, + 2: 2, + 3: 3, + } + # Fault codes + + self.joint_angles = [0] * 6 + self.pressures = [200] * 6 # Default torque 200 + self.bus = self.init_can_bus(can_channel, baudrate) + self.normal_force, self.tangential_force, self.tangential_force_dir, self.approach_inc = [[-1] * 6 for _ in range(4)] + self.is_lock = False + self.version = None + # Start the receiving thread + self.running = True + self.receive_thread = threading.Thread(target=self.receive_response) + self.receive_thread.daemon = True + self.receive_thread.start() + + def init_can_bus(self, channel, baudrate): + """ + 尝试按优先级连接 CAN 总线，并实现回退机制。 + """ + # --- 统一异常处理块开始 --- + try: + if sys.platform == "linux": + # Linux 优先级：1. socketcan + try: + self.open_can.open_can(self.can_channel) + # 尝试 socketcan + bus = can.interface.Bus(channel=channel, interface="socketcan", bitrate=baudrate) + ColorMsg(msg=f"成功连接: interface='socketcan', channel='{channel}'", color="green") + return bus + except CanError as e: + # 如果 socketcan 失败，可以考虑在这里尝试其他 Linux 接口 (如 'pcan') + ColorMsg(msg=f"socketcan 接口连接失败: {e}", color="yellow") + raise # 重新抛出异常，让外层 try 捕获 + elif sys.platform == "win32": + # Windows 优先级：1. pcan + try: + bus = can.interface.Bus(channel=channel, interface='pcan', bitrate=baudrate) + ColorMsg(msg=f"成功连接: interface='pcan', channel='{channel}'", color="green") + return bus + except CanError as e: + ColorMsg(msg=f"pcan 接口连接失败，尝试回退到 'candle': {e}", color="yellow") + # Windows 优先级：2. candle (回退方法) + try: + bus = can.Bus(interface="candle", channel=channel, bitrate=baudrate) + ColorMsg(msg=f"成功连接: interface='candle', channel='{channel}'", color="green") + return bus + except CanError as e: + ColorMsg(msg=f"candle 接口连接失败: {e}", color="yellow") + raise # 两个接口都失败，抛出异常 + else: + raise EnvironmentError("Unsupported platform for CAN interface") + # --- 统一异常处理块结束 --- + except Exception as e: + # 如果任何一个接口尝试失败并抛出异常（包括 EnvironmentError） + ColorMsg(msg=f"致命错误：所有 CAN 接口连接尝试均失败或平台不受支持。请检查设备连接或驱动安装和配置文件中CAN参数的配置。\n错误详情: {e}", color="red") + # 保持 raise 动作，将错误信息传递给调用者，避免程序继续运行 + raise + + def send_frame(self, frame_property, data_list,sleep=0.003): + """Send a single CAN frame with specified properties and data.""" + frame_property_value = int(frame_property.value) if hasattr(frame_property, 'value') else frame_property + data = [frame_property_value] + [int(val) for val in data_list] + msg = can.Message(arbitration_id=self.can_id, data=data, is_extended_id=False) + try: + self.bus.send(msg) + except can.CanError as e: + print(f"Failed to send message: {e}") + self.open_can.open_can(self.can_channel) + time.sleep(1) + self.is_can = self.open_can.is_can_up_sysfs(interface=self.can_channel) + time.sleep(1) + if self.is_can: + self.bus = can.interface.Bus(channel=self.can_channel, interface="socketcan", bitrate=self.baudrate) + else: + print("Reconnecting CAN devices ....") + time.sleep(sleep) + + def set_joint_positions(self, joint_angles): + """Set the positions of 10 joints (joint_angles: list of 10 values).""" + if len(joint_angles) > 6: + self.joint_angles = joint_angles[:6] + else: + self.joint_angles = joint_angles + # Send angle control in frames + self.send_frame(0x01, self.joint_angles, sleep=0.003) + + def set_max_torque_limits(self, pressures, type="get"): + """Set maximum torque limits.""" + if type == "get": + self.pressures = [0.0] + else: + self.pressures = pressures[:6] + + def set_torque(self, torque=[180] * 6): + """Set L6 maximum torque limits.""" + if len(torque) != 6: + raise ValueError("Torque list must have 6 elements.") + return + self.send_frame(0x02, torque) + + def set_speed(self, speed=[180] * 6): + """Set L6 speed.""" + if len(speed) != 6: + raise ValueError("Speed list must have 6 elements.") + return + self.x05 = speed + for i in range(2): + time.sleep(0.001) + self.send_frame(0x05, speed) + + ''' -------------------Pressure Sensors---------------------- ''' + def get_normal_force(self): + self.send_frame(0x20, [],sleep=0.01) + + def get_tangential_force(self): + self.send_frame(0x21, [],sleep=0.01) + + def get_tangential_force_dir(self): + self.send_frame(0x22, [],sleep=0.01) + + def get_approach_inc(self): + self.send_frame(0x23, [],sleep=0.01) + + ''' -------------------Motor Temperature---------------------- ''' + def get_motor_temperature(self): + self.send_frame(0x33, []) + + # Motor fault codes + def get_motor_fault_code(self): + self.send_frame(0x35, []) + + def receive_response(self): + """Receive CAN responses and process them.""" + while self.running: + try: + msg = self.bus.recv(timeout=1.0) + if msg: + self.process_response(msg) + except can.CanError as e: + print(f"Error receiving CAN message: {e}") + + def process_response(self, msg): + """Process received CAN messages.""" + #if msg.arbitration_id == self.can_id: + if msg.arbitration_id in (self.can_id, self.can_id + 8): + try: + frame_type = msg.data[0] + response_data = msg.data[1:] + if len(list(response_data)) == 0: + return + except: + return + if frame_type == 0x01: # 0x01 + self.x01 = list(response_data) + elif frame_type == 0x02: # 0x02 + self.x02 = list(response_data) + elif frame_type == 0x05: # Set speed + self.x05 = list(response_data) + elif frame_type == 0x20: + d = list(response_data) + self.normal_force = [float(i) for i in d] + elif frame_type == 0x21: + d = list(response_data) + self.tangential_force = [float(i) for i in d] + elif frame_type == 0x22: + d = list(response_data) + self.tangential_force_dir = [float(i) for i in d] + elif frame_type == 0x23: + d = list(response_data) + self.approach_inc = [float(i) for i in d] + elif frame_type == 0x33: # L6 temperature + self.x33 = list(response_data) + elif frame_type == 0x35: # L6 fault codes + self.x35 = list(response_data) + elif frame_type == 0x36: # L6 电流 + self.x36 = list(response_data) + elif frame_type == 0xb0: + self.xb0 = list(response_data) + elif frame_type == 0xb1: + d = list(response_data) + if len(d) == 2: + self.xb1 = d + elif len(d) == 7: + index = self.matrix_map.get(d[0]) + if index is not None: + self.thumb_matrix[index] = d[1:] # Remove the first flag bit + elif frame_type == 0xb2: + d = list(response_data) + if len(d) == 2: + self.xb2 = d + elif len(d) == 7: + index = self.matrix_map.get(d[0]) + if index is not None: + self.index_matrix[index] = d[1:] # Remove the first flag bit + elif frame_type == 0xb3: + d = list(response_data) + if len(d) == 2: + self.xb3 = d + elif len(d) == 7: + index = self.matrix_map.get(d[0]) + if index is not None: + self.middle_matrix[index] = d[1:] # Remove the first flag bit + elif frame_type == 0xb4: + d = list(response_data) + if len(d) == 2: + self.xb4 = d + elif len(d) == 7: + index = self.matrix_map.get(d[0]) + if index is not None: + self.ring_matrix[index] = d[1:] # Remove the first flag bit + elif frame_type == 0xb5: + d = list(response_data) + if len(d) == 2: + self.xb5 = d + elif len(d) == 7: + index = self.matrix_map.get(d[0]) + if index is not None: + self.little_matrix[index] = d[1:] # Remove the first flag bit + elif frame_type == 0x64: # L6 version number + self.version = list(response_data) + elif frame_type == 0xC2: # L6 version number + self.version = list(response_data) + elif frame_type == 0xC0: + d = list(response_data) + index = self.serial_number_map.get(d[0]) + if index is not None: + self.serial_number=self.serial_number + d[1:] + else: + self.serial_number=self.serial_number + [-1] * 6 + + + + def get_version(self): + self.send_frame(0x64, [],sleep=0.1) + time.sleep(0.1) + if self.version is None: + self.send_frame(0xC2, [],sleep=0.1) + time.sleep(0.1) + return self.version + + def get_current_status(self): + self.send_frame(0x01, [],sleep=0.005) + return self.x01 + + def get_current_pub_status(self): + return self.x01 + + def get_speed(self): + #self.send_frame(0x05, [],sleep=0.003) + #print("L6暂不支持读取实时速度") + return [0] * 6 + + def get_current(self): + '''Not supported yet.''' + self.send_frame(0x36, [],sleep=0.005) + return self.x36 + + + + def get_torque(self): + '''Not supported yet.''' + self.send_frame(0x2, [],sleep=0.01) + return self.x02 + + def get_touch_type(self): + '''Get touch type''' + self.send_frame(0xb1,[]) + t = [] + for i in range(3): + t = self.xb1 + time.sleep(0.01) + if len(t) == 2: + return 2 + else: + self.send_frame(0x20,[],sleep=0.03) + time.sleep(0.01) + if self.normal_force[0] == -1: + return -1 + else: + return 1 + + + def get_touch(self): + '''Get touch data''' + self.send_frame(0xb1,[],sleep=0.03) + self.send_frame(0xb2,[],sleep=0.03) + self.send_frame(0xb3,[],sleep=0.03) + self.send_frame(0xb4,[],sleep=0.03) + self.send_frame(0xb5,[],sleep=0.03) + return [self.xb1[1],self.xb2[1],self.xb3[1],self.xb4[1],self.xb5[1],0] # The last digit is palm, currently not available + + def get_matrix_touch(self): + self.send_frame(0xb1,[0xc6],sleep=0.01) + self.send_frame(0xb2,[0xc6],sleep=0.01) + self.send_frame(0xb3,[0xc6],sleep=0.01) + self.send_frame(0xb4,[0xc6],sleep=0.01) + self.send_frame(0xb5,[0xc6],sleep=0.01) + + return self.thumb_matrix , self.index_matrix , self.middle_matrix , self.ring_matrix , self.little_matrix + + def get_matrix_touch_v2(self): + self.send_frame(0xb1,[0xc6],sleep=0.009) + self.send_frame(0xb2,[0xc6],sleep=0.009) + self.send_frame(0xb3,[0xc6],sleep=0.009) + self.send_frame(0xb4,[0xc6],sleep=0.009) + self.send_frame(0xb5,[0xc6],sleep=0.009) + return self.thumb_matrix , self.index_matrix , self.middle_matrix , self.ring_matrix , self.little_matrix + + def get_thumb_matrix_touch(self,sleep_time=0.005): + self.send_frame(0xb1,[0xc6],sleep=sleep_time) + return self.thumb_matrix + + def get_index_matrix_touch(self,sleep_time=0.005): + self.send_frame(0xb2,[0xc6],sleep=sleep_time) + return self.index_matrix + + def get_middle_matrix_touch(self,sleep_time=0.005): + self.send_frame(0xb3,[0xc6],sleep=sleep_time) + return self.middle_matrix + + def get_ring_matrix_touch(self,sleep_time=0.005): + self.send_frame(0xb4,[0xc6],sleep=sleep_time) + return self.ring_matrix + + def get_little_matrix_touch(self,sleep_time=0.005): + self.send_frame(0xb5,[0xc6],sleep=sleep_time) + return self.little_matrix + + def get_force(self): + '''Get pressure.''' + return [self.normal_force, self.tangential_force, self.tangential_force_dir, self.approach_inc] + + def get_temperature(self): + '''Get temperature.''' + self.get_motor_temperature() + return self.x33 + + def get_fault(self): + '''Get faults.''' + self.get_motor_fault_code() + return self.x35 + + 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 show_fun_table(self): + pass + + def clear_faults(self, finger_mask=[1, 1, 1, 1, 1]): + """O6 暂不支持清除故障码""" + pass + + def get_serial_number(self): + try: + self.send_frame(0xC0,[],sleep=0.005) + # 1. 使用 bytes() 函数将整数列表转换为字节对象 + # bytes() 接收一个由 0-255 之间的整数组成的列表。 + byte_data = bytes(self.serial_number) + # 2. 使用 .decode() 方法将字节对象解码为 ASCII 字符串 + result_string = byte_data.decode('ascii') + if result_string == "": + return "-1" + else: + # print(f"原始 ASCII 码列表: {self.serial_number}") + # print(f"解码后的字符串: {result_string}") + return result_string + except: + return "-1" + + def close_can_interface(self): + """Stop the CAN communication.""" + self.running = False + if self.receive_thread.is_alive(): + self.receive_thread.join() + if self.bus: + self.bus.shutdown() diff --git a/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/core/can/linker_hand_l7_can.py b/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/core/can/linker_hand_l7_can.py new file mode 100644 index 0000000..446d24a --- /dev/null +++ b/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/core/can/linker_hand_l7_can.py @@ -0,0 +1,419 @@ +import can +import time, sys +import threading +import numpy as np +from utils.open_can import OpenCan +from utils.color_msg import ColorMsg +from can.exceptions import CanError + + +class LinkerHandL7Can: + def __init__(self, can_id, can_channel='can0', baudrate=1000000,yaml=""): + self.can_id = can_id + self.can_channel = can_channel + self.baudrate = baudrate + self.open_can = OpenCan(load_yaml=yaml) + + self.x01 = [0] * 7 + self.x02 = [-1] * 7 + self.x05 = [0] * 7 + self.x33 = [0] * 7 + self.xb0,self.xb1,self.xb2,self.xb3,self.xb4,self.xb5 = [-1] * 5,[-1] * 5,[-1] * 5,[-1] * 5,[-1] * 5,[-1] * 5 + self.thumb_matrix = np.full((12, 6), -1) + self.index_matrix = np.full((12, 6), -1) + self.middle_matrix = np.full((12, 6), -1) + self.ring_matrix = np.full((12, 6), -1) + self.little_matrix = np.full((12, 6), -1) + self.matrix_map = { + 0: 0, + 16: 1, + 32: 2, + 48: 3, + 64: 4, + 80: 5, + 96: 6, + 112: 7, + 128: 8, + 144: 9, + 160: 10, + 176: 11, + } + self.serial_number = [] + self.serial_number_map = { + 0: 0, + 1: 1, + 2: 2, + 3: 3, + } + # Fault codes + self.x35 = [0] * 7, [0] * 7 + self.joint_angles = [0] * 10 + self.pressures = [200] * 7 # Default torque 200 + self.bus = self.init_can_bus(can_channel, baudrate) + self.normal_force, self.tangential_force, self.tangential_force_dir, self.approach_inc = [[-1] * 7 for _ in range(4)] + self.is_lock = False + self.version = None + # Start the receiving thread + self.running = True + self.receive_thread = threading.Thread(target=self.receive_response) + self.receive_thread.daemon = True + self.receive_thread.start() + + def init_can_bus(self, channel, baudrate): + """ + 尝试按优先级连接 CAN 总线，并实现回退机制。 + """ + # --- 统一异常处理块开始 --- + try: + if sys.platform == "linux": + # Linux 优先级：1. socketcan + try: + self.open_can.open_can(self.can_channel) + # 尝试 socketcan + bus = can.interface.Bus(channel=channel, interface="socketcan", bitrate=baudrate) + ColorMsg(msg=f"成功连接: interface='socketcan', channel='{channel}'", color="green") + return bus + except CanError as e: + # 如果 socketcan 失败，可以考虑在这里尝试其他 Linux 接口 (如 'pcan') + ColorMsg(msg=f"socketcan 接口连接失败: {e}", color="yellow") + raise # 重新抛出异常，让外层 try 捕获 + elif sys.platform == "win32": + # Windows 优先级：1. pcan + try: + bus = can.interface.Bus(channel=channel, interface='pcan', bitrate=baudrate) + ColorMsg(msg=f"成功连接: interface='pcan', channel='{channel}'", color="green") + return bus + except CanError as e: + ColorMsg(msg=f"pcan 接口连接失败，尝试回退到 'candle': {e}", color="yellow") + # Windows 优先级：2. candle (回退方法) + try: + bus = can.Bus(interface="candle", channel=channel, bitrate=baudrate) + ColorMsg(msg=f"成功连接: interface='candle', channel='{channel}'", color="green") + return bus + except CanError as e: + ColorMsg(msg=f"candle 接口连接失败: {e}", color="yellow") + raise # 两个接口都失败，抛出异常 + else: + raise EnvironmentError("Unsupported platform for CAN interface") + # --- 统一异常处理块结束 --- + except Exception as e: + # 如果任何一个接口尝试失败并抛出异常（包括 EnvironmentError） + ColorMsg(msg=f"致命错误：所有 CAN 接口连接尝试均失败或平台不受支持。请检查设备连接或驱动安装和配置文件中CAN参数的配置。\n错误详情: {e}", color="red") + # 保持 raise 动作，将错误信息传递给调用者，避免程序继续运行 + raise + + def send_frame(self, frame_property, data_list,sleep=0.005): + """Send a single CAN frame with specified properties and data.""" + frame_property_value = int(frame_property.value) if hasattr(frame_property, 'value') else frame_property + data = [frame_property_value] + [int(val) for val in data_list] + msg = can.Message(arbitration_id=self.can_id, data=data, is_extended_id=False) + try: + self.bus.send(msg) + except can.CanError as e: + print(f"Failed to send message: {e}") + self.open_can.open_can(self.can_channel) + time.sleep(1) + self.is_can = self.open_can.is_can_up_sysfs(interface=self.can_channel) + time.sleep(1) + if self.is_can: + self.bus = can.interface.Bus(channel=self.can_channel, interface="socketcan", bitrate=self.baudrate) + else: + print("Reconnecting CAN devices ....") + time.sleep(sleep) + + def set_joint_positions(self, joint_angles): + """Set the positions of 10 joints (joint_angles: list of 10 values).""" + self.is_lock = True + if len(joint_angles) > 7: + self.joint_angles = joint_angles[:7] + else: + self.joint_angles = joint_angles + # Send angle control in frames + self.send_frame(0x01, self.joint_angles, sleep=0.003) + self.is_lock = False + + def set_max_torque_limits(self, pressures, type="get"): + """Set maximum torque limits.""" + if type == "get": + self.pressures = [0.0] + else: + self.pressures = pressures[:7] + + def set_torque(self, torque=[180] * 7): + """Set L7 maximum torque limits.""" + if len(torque) != 7: + raise ValueError("Torque list must have 7 elements.") + return + self.send_frame(0x02, torque) + + def set_speed(self, speed=[180] * 7): + """Set L7 speed.""" + if len(speed) != 7: + raise ValueError("Speed list must have 7 elements.") + return + self.x05 = speed + for i in range(2): + time.sleep(0.001) + self.send_frame(0x05, speed) + + ''' -------------------Pressure Sensors---------------------- ''' + def get_normal_force(self): + self.send_frame(0x20, [],sleep=0.004) + + def get_tangential_force(self): + self.send_frame(0x21, [],sleep=0.004) + + def get_tangential_force_dir(self): + self.send_frame(0x22, [],sleep=0.004) + + def get_approach_inc(self): + self.send_frame(0x23, [],sleep=0.004) + + ''' -------------------Motor Temperature---------------------- ''' + def get_motor_temperature(self): + self.send_frame(0x33, []) + + # Motor fault codes + def get_motor_fault_code(self): + self.send_frame(0x35, []) + + def receive_response(self): + """Receive CAN responses and process them.""" + while self.running: + try: + msg = self.bus.recv(timeout=1.0) + if msg: + self.process_response(msg) + except can.CanError as e: + print(f"Error receiving CAN message: {e}") + + def process_response(self, msg): + """Process received CAN messages.""" + if msg.arbitration_id == self.can_id: + frame_type = msg.data[0] + response_data = msg.data[1:] + if len(list(response_data)) == 0: + return + if frame_type == 0x01: # 0x01 + self.x01 = list(response_data) + elif frame_type == 0x02: # 0x02 + self.x02 = list(response_data) + elif frame_type == 0x05: # Set speed + self.x05 = list(response_data) + elif frame_type == 0x20: + d = list(response_data) + self.normal_force = [float(i) for i in d] + elif frame_type == 0x21: + d = list(response_data) + self.tangential_force = [float(i) for i in d] + elif frame_type == 0x22: + d = list(response_data) + self.tangential_force_dir = [float(i) for i in d] + elif frame_type == 0x23: + d = list(response_data) + self.approach_inc = [float(i) for i in d] + elif frame_type == 0x33: # L7 temperature + self.x33 = list(response_data) + elif frame_type == 0x35: # L7 fault codes + self.x35 = list(response_data) + elif frame_type == 0xb0: + self.xb0 = list(response_data) + elif frame_type == 0xb1: + d = list(response_data) + if len(d) == 2: + self.xb1 = d + elif len(d) == 7: + index = self.matrix_map.get(d[0]) + if index is not None: + self.thumb_matrix[index] = d[1:] # Remove the first flag bit + elif frame_type == 0xb2: + d = list(response_data) + if len(d) == 2: + self.xb2 = d + elif len(d) == 7: + index = self.matrix_map.get(d[0]) + if index is not None: + self.index_matrix[index] = d[1:] # Remove the first flag bit + elif frame_type == 0xb3: + d = list(response_data) + if len(d) == 2: + self.xb3 = d + elif len(d) == 7: + index = self.matrix_map.get(d[0]) + if index is not None: + self.middle_matrix[index] = d[1:] # Remove the first flag bit + elif frame_type == 0xb4: + d = list(response_data) + if len(d) == 2: + self.xb4 = d + elif len(d) == 7: + index = self.matrix_map.get(d[0]) + if index is not None: + self.ring_matrix[index] = d[1:] # Remove the first flag bit + elif frame_type == 0xb5: + d = list(response_data) + if len(d) == 2: + self.xb5 = d + elif len(d) == 7: + index = self.matrix_map.get(d[0]) + if index is not None: + self.little_matrix[index] = d[1:] # Remove the first flag bit + elif frame_type == 0x64: # L7 version number + self.version = list(response_data) + elif frame_type == 0xC2: # O6 version number + self.version = list(response_data) + elif frame_type == 0xC0: + d = list(response_data) + index = self.serial_number_map.get(d[0]) + if index is not None: + self.serial_number=self.serial_number + d[1:] + else: + self.serial_number=self.serial_number + [-1] * 6 + + def get_version(self): + self.send_frame(0x64, [], sleep=0.1) + time.sleep(0.1) + if self.version is None: + self.send_frame(0xC2, [], sleep=0.1) + time.sleep(0.1) + return self.version + + def get_current_status(self): + if self.is_lock: + return self.x01 + elif self.is_lock == False: + self.send_frame(0x01, [],sleep=0.003) + return self.x01 + + def get_current_pub_status(self): + return self.x01 + + def get_speed(self): + self.send_frame(0x05, [],sleep=0.003) + return self.x05 + + def get_current(self): + '''Not supported yet.''' + self.send_frame(0x2, [],sleep=0.1) + return self.x02 + + + + def get_torque(self): + '''Not supported yet.''' + self.send_frame(0x2, [],sleep=0.01) + return self.x02 + + def get_touch_type(self): + '''Get touch type''' + self.send_frame(0xb1,[]) + t = [] + for i in range(3): + t = self.xb1 + time.sleep(0.01) + if len(t) == 2: + return 2 + else: + self.send_frame(0x20,[],sleep=0.03) + time.sleep(0.01) + if self.normal_force[0] == -1: + return -1 + else: + return 1 + + + def get_touch(self): + '''Get touch data''' + self.send_frame(0xb1,[],sleep=0.03) + self.send_frame(0xb2,[],sleep=0.03) + self.send_frame(0xb3,[],sleep=0.03) + self.send_frame(0xb4,[],sleep=0.03) + self.send_frame(0xb5,[],sleep=0.03) + return [self.xb1[1],self.xb2[1],self.xb3[1],self.xb4[1],self.xb5[1],0] # The last digit is palm, currently not available + + def get_matrix_touch(self): + self.send_frame(0xb1,[0xc6],sleep=0.01) + self.send_frame(0xb2,[0xc6],sleep=0.01) + self.send_frame(0xb3,[0xc6],sleep=0.01) + self.send_frame(0xb4,[0xc6],sleep=0.01) + self.send_frame(0xb5,[0xc6],sleep=0.01) + + return self.thumb_matrix , self.index_matrix , self.middle_matrix , self.ring_matrix , self.little_matrix + + def get_matrix_touch_v2(self): + self.send_frame(0xb1,[0xc6],sleep=0.005) + self.send_frame(0xb2,[0xc6],sleep=0.005) + self.send_frame(0xb3,[0xc6],sleep=0.005) + self.send_frame(0xb4,[0xc6],sleep=0.005) + self.send_frame(0xb5,[0xc6],sleep=0.005) + return self.thumb_matrix , self.index_matrix , self.middle_matrix , self.ring_matrix , self.little_matrix + + + def get_thumb_matrix_touch(self,sleep_time=0.005): + self.send_frame(0xb1,[0xc6],sleep=sleep_time) + return self.thumb_matrix + + def get_index_matrix_touch(self,sleep_time=0.005): + self.send_frame(0xb2,[0xc6],sleep=sleep_time) + return self.index_matrix + + def get_middle_matrix_touch(self,sleep_time=0.005): + self.send_frame(0xb3,[0xc6],sleep=sleep_time) + return self.middle_matrix + + def get_ring_matrix_touch(self,sleep_time=0.005): + self.send_frame(0xb4,[0xc6],sleep=sleep_time) + return self.ring_matrix + + def get_little_matrix_touch(self,sleep_time=0.005): + self.send_frame(0xb5,[0xc6],sleep=sleep_time) + return self.little_matrix + + def get_force(self): + '''Get pressure.''' + return [self.normal_force, self.tangential_force, self.tangential_force_dir, self.approach_inc] + + def get_temperature(self): + '''Get temperature.''' + self.get_motor_temperature() + return self.x33 + + def get_fault(self): + '''Get faults.''' + self.get_motor_fault_code() + return self.x35 + + def get_serial_number(self): + try: + self.send_frame(0xC0,[],sleep=0.005) + # 1. 使用 bytes() 函数将整数列表转换为字节对象 + # bytes() 接收一个由 0-255 之间的整数组成的列表。 + byte_data = bytes(self.serial_number) + # 2. 使用 .decode() 方法将字节对象解码为 ASCII 字符串 + result_string = byte_data.decode('ascii') + if result_string == "": + return "-1" + else: + # print(f"原始 ASCII 码列表: {self.serial_number}") + # print(f"解码后的字符串: {result_string}") + return result_string + except: + return "-1" + + def get_finger_order(self): + return ["thumb_cmc_pitch", "thumb_cmc_yaw", "index_mcp_pitch", "middle_mcp_pitch", "ring_mcp_pitch", "pinky_mcp_pitch", "thumb_cmc_roll"] + + def show_fun_table(self): + pass + + def clear_faults(self, finger_mask=[1, 1, 1, 1, 1]): + """L7 暂不支持清除故障码""" + pass + + def close_can_interface(self): + """Stop the CAN communication.""" + self.running = False + if self.receive_thread.is_alive(): + self.receive_thread.join() + if self.bus: + self.bus.shutdown() diff --git a/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/core/can/linker_hand_o6_can.py b/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/core/can/linker_hand_o6_can.py new file mode 100644 index 0000000..5c95ab8 --- /dev/null +++ b/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/core/can/linker_hand_o6_can.py @@ -0,0 +1,447 @@ +import can +import time, sys +import threading +import numpy as np +from utils.open_can import OpenCan +from utils.color_msg import ColorMsg +from can.exceptions import CanError + + +class LinkerHandO6Can: + def __init__(self, can_id, can_channel='can0', baudrate=1000000,yaml=""): + self.can_id = can_id + self.can_channel = can_channel + self.baudrate = baudrate + self.open_can = OpenCan(load_yaml=yaml) + + self.x01 = [0] * 6 # 关节位置 + self.x02 = [-1] * 6 # 转矩限制 + self.x05 = [0] * 6 # 速度 + self.x07 = [-1] * 6 # 加速度 + self.x33 = [0] * 6 # 温度 + self.x35 = [0] * 6 # 关节错误码 + self.x36 = [-1] * 6 # 电流 + self.xb0,self.xb1,self.xb2,self.xb3,self.xb4,self.xb5 = [-1] * 5,[-1] * 5,[-1] * 5,[-1] * 5,[-1] * 5,[-1] * 5 + + self.thumb_matrix = np.full((10, 4), -1) + self.index_matrix = np.full((10, 4), -1) + self.middle_matrix = np.full((10, 4), -1) + self.ring_matrix = np.full((10, 4), -1) + self.little_matrix = np.full((10, 4), -1) + self.matrix_map = { + 0: 0, + 16: 1, + 32: 2, + 48: 3, + 64: 4, + 80: 5, + 96: 6, + 112: 7, + 128: 8, + 144: 9, + 160: 10, + 176: 11, + } + self.serial_number = [] + self.serial_number_map = { + 0: 0, + 1: 1, + 2: 2, + 3: 3, + } + # Fault codes + + self.joint_angles = [0] * 6 + self.pressures = [200] * 6 # Default torque 200 + self.bus = self.init_can_bus(can_channel, baudrate) + self.normal_force, self.tangential_force, self.tangential_force_dir, self.approach_inc = [[-1] * 6 for _ in range(4)] + self.is_lock = False + self.version = None + # Start the receiving thread + self.running = True + + self.receive_thread = threading.Thread(target=self.receive_response) + self.receive_thread.daemon = True + self.receive_thread.start() + time.sleep(0.1) + self._check_touch_type() + + def _check_touch_type(self): + '''根据SN编码判断压感类型''' + self.sn = self.get_serial_number() + time.sleep(0.1) + if self.sn != "-1": + parts = self.sn.split("-") + if parts[4] == "A": + self.touch_type = 1 + elif parts[4] == "B": + self.touch_type = 2 + self.touch_code = 0xA4 # 6*12 O6 一律0XA4 + elif parts[4] == "J": + self.touch_type = 3 + elif parts[4] == "F": + self.touch_type = 4 + self.touch_code = 0xA4 # 4*10 + elif parts[4] == "Z": + self.touch_type = -1 + else: + # 如果没有SN编码则根据返回数据进行判断 + self.touch_type = self.get_touch_type() + + + def init_can_bus(self, channel, baudrate): + """ + 尝试按优先级连接 CAN 总线，并实现回退机制。 + """ + # --- 统一异常处理块开始 --- + try: + if sys.platform == "linux": + # Linux 优先级：1. socketcan + try: + self.open_can.open_can(self.can_channel) + # 尝试 socketcan + bus = can.interface.Bus(channel=channel, interface="socketcan", bitrate=baudrate) + ColorMsg(msg=f"成功连接: interface='socketcan', channel='{channel}'", color="green") + return bus + except CanError as e: + # 如果 socketcan 失败，可以考虑在这里尝试其他 Linux 接口 (如 'pcan') + ColorMsg(msg=f"socketcan 接口连接失败: {e}", color="yellow") + raise # 重新抛出异常，让外层 try 捕获 + elif sys.platform == "win32": + # Windows 优先级：1. pcan + try: + bus = can.interface.Bus(channel=channel, interface='pcan', bitrate=baudrate) + ColorMsg(msg=f"成功连接: interface='pcan', channel='{channel}'", color="green") + return bus + except CanError as e: + ColorMsg(msg=f"pcan 接口连接失败，尝试回退到 'candle': {e}", color="yellow") + # Windows 优先级：2. candle (回退方法) + try: + bus = can.Bus(interface="candle", channel=channel, bitrate=baudrate) + ColorMsg(msg=f"成功连接: interface='candle', channel='{channel}'", color="green") + return bus + except CanError as e: + ColorMsg(msg=f"candle 接口连接失败: {e}", color="yellow") + raise # 两个接口都失败，抛出异常 + else: + raise EnvironmentError("Unsupported platform for CAN interface") + # --- 统一异常处理块结束 --- + except Exception as e: + # 如果任何一个接口尝试失败并抛出异常（包括 EnvironmentError） + ColorMsg(msg=f"致命错误：所有 CAN 接口连接尝试均失败或平台不受支持。请检查设备连接或驱动安装和配置文件中CAN参数的配置。\n错误详情: {e}", color="red") + # 保持 raise 动作，将错误信息传递给调用者，避免程序继续运行 + raise + + def send_frame(self, frame_property, data_list,sleep=0.005): + """Send a single CAN frame with specified properties and data.""" + frame_property_value = int(frame_property.value) if hasattr(frame_property, 'value') else frame_property + data = [frame_property_value] + [int(val) for val in data_list] + msg = can.Message(arbitration_id=self.can_id, data=data, is_extended_id=False) + try: + self.bus.send(msg) + except can.CanError as e: + print(f"Failed to send message: {e}") + self.open_can.open_can(self.can_channel) + time.sleep(1) + self.is_can = self.open_can.is_can_up_sysfs(interface=self.can_channel) + time.sleep(1) + if self.is_can: + self.bus = can.interface.Bus(channel=self.can_channel, interface="socketcan", bitrate=self.baudrate) + else: + print("Reconnecting CAN devices ....") + time.sleep(sleep) + + def set_joint_positions(self, joint_angles): + """Set the positions of 10 joints (joint_angles: list of 10 values).""" + if len(joint_angles) > 6: + self.joint_angles = joint_angles[:6] + else: + self.joint_angles = joint_angles + # Send angle control in frames + self.send_frame(0x01, self.joint_angles, sleep=0.003) + + def set_max_torque_limits(self, pressures, type="get"): + """Set maximum torque limits.""" + if type == "get": + self.pressures = [0.0] + else: + self.pressures = pressures[:6] + + def set_torque(self, torque=[180] * 6): + """Set L6 maximum torque limits.""" + if len(torque) != 6: + raise ValueError("Torque list must have 6 elements.") + return + self.send_frame(0x02, torque) + + def set_speed(self, speed=[180] * 6): + """Set L6 speed.""" + if len(speed) != 6: + raise ValueError("Speed list must have 6 elements.") + return + self.x05 = speed + for i in range(2): + time.sleep(0.001) + self.send_frame(0x05, speed) + + ''' -------------------Pressure Sensors---------------------- ''' + def get_normal_force(self): + self.send_frame(0x20, [],sleep=0.01) + + def get_tangential_force(self): + self.send_frame(0x21, [],sleep=0.01) + + def get_tangential_force_dir(self): + self.send_frame(0x22, [],sleep=0.01) + + def get_approach_inc(self): + self.send_frame(0x23, [],sleep=0.01) + + ''' -------------------Motor Temperature---------------------- ''' + def get_motor_temperature(self): + self.send_frame(0x33, []) + + # Motor fault codes + def get_motor_fault_code(self): + self.send_frame(0x35, []) + + def receive_response(self): + """Receive CAN responses and process them.""" + while self.running: + try: + msg = self.bus.recv(timeout=1.0) + if msg: + self.process_response(msg) + except can.CanError as e: + print(f"Error receiving CAN message: {e}") + + def process_response(self, msg): + """Process received CAN messages.""" + if msg.arbitration_id == self.can_id: + frame_type = msg.data[0] + response_data = msg.data[1:] + if len(list(response_data)) == 0: + return + if frame_type == 0x01: # 0x01 + self.x01 = list(response_data) + elif frame_type == 0x02: # 0x02 + self.x02 = list(response_data) + elif frame_type == 0x05: # Set speed + self.x05 = list(response_data) + elif frame_type == 0x20: + d = list(response_data) + self.normal_force = [float(i) for i in d] + elif frame_type == 0x21: + d = list(response_data) + self.tangential_force = [float(i) for i in d] + elif frame_type == 0x22: + d = list(response_data) + self.tangential_force_dir = [float(i) for i in d] + elif frame_type == 0x23: + d = list(response_data) + self.approach_inc = [float(i) for i in d] + elif frame_type == 0x33: # O6 temperature + self.x33 = list(response_data) + elif frame_type == 0x35: # O6 fault codes + self.x35 = list(response_data) + elif frame_type == 0x36: # O6 电流 + self.x36 = list(response_data) + elif frame_type == 0xb0: + self.xb0 = list(response_data) + elif frame_type == 0xb1: + d = list(response_data) + if len(d) == 2: + self.xb1 = d + elif len(d) == 5: + index = self.matrix_map.get(d[0]) + if index is not None: + self.thumb_matrix[index] = d[1:] # Remove the first flag bit + elif frame_type == 0xb2: + d = list(response_data) + if len(d) == 2: + self.xb2 = d + elif len(d) == 5: + index = self.matrix_map.get(d[0]) + if index is not None: + self.index_matrix[index] = d[1:] # Remove the first flag bit + elif frame_type == 0xb3: + d = list(response_data) + if len(d) == 2: + self.xb3 = d + elif len(d) == 5: + index = self.matrix_map.get(d[0]) + if index is not None: + self.middle_matrix[index] = d[1:] # Remove the first flag bit + elif frame_type == 0xb4: + d = list(response_data) + if len(d) == 2: + self.xb4 = d + elif len(d) == 5: + index = self.matrix_map.get(d[0]) + if index is not None: + self.ring_matrix[index] = d[1:] # Remove the first flag bit + elif frame_type == 0xb5: + d = list(response_data) + if len(d) == 2: + self.xb5 = d + elif len(d) == 5: + index = self.matrix_map.get(d[0]) + if index is not None: + self.little_matrix[index] = d[1:] # Remove the first flag bit + elif frame_type == 0x64: # O6 version number + self.version = list(response_data) + elif frame_type == 0xC2: # O6 version number + self.version = list(response_data) + elif frame_type == 0xC0: + d = list(response_data) + index = self.serial_number_map.get(d[0]) + if index is not None: + self.serial_number += d[1:] + else: + self.serial_number=self.serial_number + [-1] * 6 + + + + def get_version(self): + self.send_frame(0x64, [],sleep=0.1) + time.sleep(0.1) + if self.version is None: + self.send_frame(0xC2, [],sleep=0.1) + time.sleep(0.1) + return self.version + + def get_current_status(self): + self.send_frame(0x01, [],sleep=0.005) + return self.x01 + + def get_current_pub_status(self): + return self.x01 + + def get_speed(self): + self.send_frame(0x05, [],sleep=0.002) + #print("L6暂不支持读取实时速度") + return self.x05 + + def get_current(self): + '''Not supported yet.''' + self.send_frame(0x36, [],sleep=0.005) + return self.x36 + + + + def get_torque(self): + '''Not supported yet.''' + self.send_frame(0x2, [],sleep=0.01) + return self.x02 + + def get_touch_type(self): + '''Get touch type''' + self.send_frame(0xb1,[]) + t = [] + for i in range(3): + t = self.xb1 + time.sleep(0.01) + if len(t) == 2: + return 2 + else: + self.send_frame(0x20,[],sleep=0.03) + time.sleep(0.01) + if self.normal_force[0] == -1: + return -1 + else: + return 1 + + + def get_touch(self): + '''Get touch data''' + self.send_frame(0xb1,[],sleep=0.03) + self.send_frame(0xb2,[],sleep=0.03) + self.send_frame(0xb3,[],sleep=0.03) + self.send_frame(0xb4,[],sleep=0.03) + self.send_frame(0xb5,[],sleep=0.03) + return [self.xb1[1],self.xb2[1],self.xb3[1],self.xb4[1],self.xb5[1],0] # The last digit is palm, currently not available + + def get_matrix_touch(self): + self.send_frame(0xb1,[self.touch_code],sleep=0.01) + self.send_frame(0xb2,[self.touch_code],sleep=0.01) + self.send_frame(0xb3,[self.touch_code],sleep=0.01) + self.send_frame(0xb4,[self.touch_code],sleep=0.01) + self.send_frame(0xb5,[self.touch_code],sleep=0.01) + + return self.thumb_matrix , self.index_matrix , self.middle_matrix , self.ring_matrix , self.little_matrix + + def get_matrix_touch_v2(self): + self.send_frame(0xb1,[self.touch_code],sleep=0.009) + self.send_frame(0xb2,[self.touch_code],sleep=0.009) + self.send_frame(0xb3,[self.touch_code],sleep=0.009) + self.send_frame(0xb4,[self.touch_code],sleep=0.009) + self.send_frame(0xb5,[self.touch_code],sleep=0.009) + return self.thumb_matrix , self.index_matrix , self.middle_matrix , self.ring_matrix , self.little_matrix + + def get_thumb_matrix_touch(self,sleep_time=0.002): + self.send_frame(0xb1,[self.touch_code],sleep=sleep_time) + return self.thumb_matrix + + def get_index_matrix_touch(self,sleep_time=0.002): + self.send_frame(0xb2,[self.touch_code],sleep=sleep_time) + return self.index_matrix + + def get_middle_matrix_touch(self,sleep_time=0.002): + self.send_frame(0xb3,[self.touch_code],sleep=sleep_time) + return self.middle_matrix + + def get_ring_matrix_touch(self,sleep_time=0.002): + self.send_frame(0xb4,[self.touch_code],sleep=sleep_time) + return self.ring_matrix + + def get_little_matrix_touch(self,sleep_time=0.002): + self.send_frame(0xb5,[self.touch_code],sleep=sleep_time) + return self.little_matrix + + def get_force(self): + '''Get pressure.''' + return [self.normal_force, self.tangential_force, self.tangential_force_dir, self.approach_inc] + + def get_temperature(self): + '''Get temperature.''' + self.get_motor_temperature() + return self.x33 + + def get_fault(self): + '''Get faults.''' + self.get_motor_fault_code() + return self.x35 + + 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, finger_mask=[1, 1, 1, 1, 1]): + """O6 暂不支持清除故障码""" + pass + def show_fun_table(self): + pass + + def get_serial_number(self): + try: + self.send_frame(0xC0,[],sleep=0.005) + # 1. 使用 bytes() 函数将整数列表转换为字节对象 + # bytes() 接收一个由 0-255 之间的整数组成的列表。 + byte_data = bytes(self.serial_number) + # 2. 使用 .decode() 方法将字节对象解码为 ASCII 字符串 + result_string = byte_data.decode('ascii') + if result_string == "": + return "-1" + else: + # print(f"原始 ASCII 码列表: {self.serial_number}") + # print(f"解码后的字符串: {result_string}") + return result_string + except: + return "-1" + + def close_can_interface(self): + """Stop the CAN communication.""" + self.running = False + if self.receive_thread.is_alive(): + self.receive_thread.join() + if self.bus: + self.bus.shutdown() diff --git a/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/core/rs485/linker_hand_l10_rs485.py b/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/core/rs485/linker_hand_l10_rs485.py new file mode 100644 index 0000000..8db78ab --- /dev/null +++ b/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/core/rs485/linker_hand_l10_rs485.py @@ -0,0 +1,345 @@ +#!/usr/bin/env python3 +import os +import time +import struct +from typing import Dict, List +import numpy as np +from pymodbus.client import ModbusSerialClient +_INTERVAL = 0.005 # 8 ms +class LinkerHandL10RS485: + KEYS = ["thumb_cmc_pitch", "thumb_cmc_roll", "index_mcp_pitch", "middle_mcp_pitch", + "ring_mcp_pitch", "pinky_mcp_pitch", "index_mcp_roll", "ring_mcp_roll", + "pinky_mcp_roll", "thumb_cmc_yaw"] + + def __init__(self, hand_id=0x27, modbus_port="/dev/ttyUSB0", baudrate=115200): + self.slave = hand_id + self.cli = ModbusSerialClient( + port=modbus_port, + baudrate=baudrate, + bytesize=8, + parity="N", + stopbits=1, + timeout=0.05, # 50 ms 超时 + retries=3, # 重试次数 + retry_on_empty=True, + handle_local_echo=False + ) + # 在 pymodbus 3.5.1 中，连接需要显式调用 connect() + self.connected = self.cli.connect() + if not self.connected: + raise ConnectionError(f"RS485 connect fail to {modbus_port}") + + # -------------------------------------------------- + # 批量读取接口 + # -------------------------------------------------- + def read_angles(self) -> List[int]: + time.sleep(_INTERVAL) + rsp = self.cli.read_input_registers(address=0, count=10, slave=self.slave) + if rsp.isError(): + raise RuntimeError(f"read_angles failed: {rsp}") + return rsp.registers + + def read_torques(self) -> List[int]: + time.sleep(_INTERVAL) + rsp = self.cli.read_input_registers(address=10, count=10, slave=self.slave) + if rsp.isError(): + raise RuntimeError(f"read_torques failed: {rsp}") + return rsp.registers + + def read_speeds(self) -> List[int]: + time.sleep(_INTERVAL) + rsp = self.cli.read_input_registers(address=20, count=10, slave=self.slave) + if rsp.isError(): + raise RuntimeError(f"read_speeds failed: {rsp}") + return rsp.registers + + def read_temperatures(self) -> List[int]: + time.sleep(_INTERVAL) + rsp = self.cli.read_input_registers(address=40, count=10, slave=self.slave) + if rsp.isError(): + raise RuntimeError(f"read_temperatures failed: {rsp}") + return rsp.registers + + def read_error_codes(self) -> List[int]: + time.sleep(_INTERVAL) + rsp = self.cli.read_input_registers(address=50, count=10, slave=self.slave) + if rsp.isError(): + raise RuntimeError(f"read_error_codes failed: {rsp}") + return rsp.registers + + def read_versions(self) -> dict: + time.sleep(_INTERVAL) + rsp = self.cli.read_input_registers(address=158, count=6, slave=self.slave) + if rsp.isError(): + raise RuntimeError(f"read_versions failed: {rsp}") + keys = ["hand_freedom", "hand_version", "hand_number", + "hand_direction", "software_version", "hardware_version"] + #return dict(zip(keys, rsp.registers)) + return rsp.registers + + # -------------------------------------------------- + # 5 个压力传感器 + # -------------------------------------------------- + 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_pinky(self) -> np.ndarray: + return np.array(self._pressure(5), dtype=np.uint8) + + # def _pressure(self, finger: int) -> List[int]: + # time.sleep(_INTERVAL) + # # 先选择手指 + # wrsp = self.cli.write_register(address=60, value=finger, slave=self.slave) + # if wrsp.isError(): + # raise RuntimeError(f"write finger select {finger} failed: {wrsp}") + + # time.sleep(_INTERVAL) + # # 读取压力传感器数据 (96个寄存器) + # rrsp = self.cli.read_input_registers(address=62, count=96, slave=self.slave) + # if rrsp.isError(): + # raise RuntimeError(f"read pressure finger={finger} failed: {rrsp}") + # return np.array(rrsp.registers, dtype=np.uint8) + 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 = 70 # 写入手指选择 + read_address = 72 # 读取压力数据 + 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 write_angles(self, vals: List[int]): + vals = [int(x) for x in vals] + if not self.is_valid_10xuint8(vals): + raise ValueError("需要 10 个 0-255 整数") + + time.sleep(_INTERVAL) + rsp = self.cli.write_registers(address=0, values=vals, slave=self.slave) + if rsp.isError(): + raise RuntimeError(f"write_angles failed: {rsp}") + + def write_speeds(self, vals: List[int]): + vals = [int(x) for x in vals] + if not self.is_valid_10xuint8(vals): + raise ValueError("需要 10 个 0-255 整数") + + time.sleep(_INTERVAL) + rsp = self.cli.write_registers(address=20, values=vals, slave=self.slave) + if rsp.isError(): + raise RuntimeError(f"write_speeds failed: {rsp}") + + def write_torques(self, vals: List[int]): + vals = [int(x) for x in vals] + if not self.is_valid_10xuint8(vals): + raise ValueError("需要 10 个 0-255 整数") + + time.sleep(_INTERVAL) + rsp = self.cli.write_registers(address=10, values=vals, slave=self.slave) + if rsp.isError(): + raise RuntimeError(f"write_torques failed: {rsp}") + + # -------------------------------------------------- + # 上下文管理 + # -------------------------------------------------- + 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() + + # -------------------------------------------------- + # 工具函数 + # -------------------------------------------------- + def is_valid_10xuint8(self, lst) -> bool: + if len(lst) != 10: + return False + return all(isinstance(x, int) and 0 <= x <= 255 for x in lst) + + # -------------------------------------------------- + # 固定 API 接口 + # -------------------------------------------------- + def set_joint_positions(self, joint_angles=None): + joint_angles = joint_angles or [0] * 10 + self.write_angles(joint_angles) + + def set_speed(self, speed=None): + speed = speed or [200] * 10 + self.write_speeds(speed) + + def set_torque(self, torque=None): + torque = torque or [200] * 10 + self.write_torques(torque) + + def set_current(self, current=None): + print("当前L10不支持设置电流", flush=True) + + def get_version(self) -> dict: + return self.read_versions() + + def get_current(self): + print("当前L10不支持获取电流", flush=True) + + def get_state(self) -> List[int]: + return self.read_angles() + + def get_state_for_pub(self) -> List[int]: + return self.get_state() + + def get_current_status(self) -> List[int]: + return self.get_state() + + def get_speed(self) -> List[int]: + return self.read_speeds() + + def get_joint_speed(self) -> List[int]: + return self.get_speed() + + def get_touch_type(self) -> int: + return 2 + + def get_normal_force(self) -> List[int]: + return [-1] * 5 + + def get_tangential_force(self) -> List[int]: + return [-1] * 5 + + def get_approach_inc(self) -> List[int]: + return [-1] * 5 + + def get_touch(self) -> List[int]: + 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[List[int]]: + return self.get_thumb_matrix_touch(),self.get_index_matrix_touch(), self.get_middle_matrix_touch(), self.get_ring_matrix_touch(), self.get_little_matrix_touch() + + def get_matrix_touch_v2(self) -> List[List[int]]: + return self.get_matrix_touch() + + def get_torque(self) -> List[int]: + return self.read_torques() + + def get_temperature(self) -> List[int]: + return self.read_temperatures() + + def get_fault(self) -> List[int]: + return self.read_error_codes() + + def get_serial_number(self): + return [0] * 6 + + def clear_faults(self): + pass + +# ------------------- demo ------------------- +if __name__ == "__main__": + try: + with LinkerHandL10RS485(hand_id=0x27, modbus_port="/dev/ttyUSB0", baudrate=115200) as hand: + print("连接成功!") + + # 测试读取角度 + angles = hand.read_angles() + print("角度:", dict(zip(LinkerHandL10RS485.KEYS, angles))) + + # 测试读取版本信息 + ver = hand.get_version() + print("版本信息:", ver) + + # 测试压力传感器 + print("拇指压力传感器数据长度:", len(hand.read_pressure_thumb())) + + # 测试其他读取功能 + print("电流:", hand.read_torques()) + print("速度:", hand.read_speeds()) + print("温度:", hand.read_temperatures()) + print("错误码:", hand.read_error_codes()) + + except Exception as e: + print(f"错误: {e}") diff --git a/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/core/rs485/linker_hand_l6_rs485.py b/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/core/rs485/linker_hand_l6_rs485.py new file mode 100644 index 0000000..81db0e2 --- /dev/null +++ b/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/core/rs485/linker_hand_l6_rs485.py @@ -0,0 +1,460 @@ +#!/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 = 36 # 写入手指选择 (保持寄存器) + read_address = 52 # 读取压力数据 (输入寄存器) + read_count = 71 # 读取 71 个寄存器 + skip_count = 0 # 不跳过数据点 + + # 0. 参数校验和手指写入值确定 + if finger < 1 or finger > 5: + raise ValueError(f"无效的手指编号: {finger}。手指编号应在 1 到 5 之间。") + + finger_write_value = finger + + # 1. 写入手指选择寄存器 (地址 36) + time.sleep(0.08) + 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.08) + + # 2. 读取地址 52 的数据 + 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 位数据 (得到 71 个 8 位数据点) + final_data_71 = [reg_value & 255 for reg_value in registers_16bit] + + # b. 不跳过数据点 (按协议) + effective_data = np.array(final_data_71, dtype=np.uint8) + + # c. 截取当前手指的矩阵数据 (71 个有效点中截取 72 个点，可能需要多读) + # 注: 协议返回 71 个点，手指数 1-5，每个手指需要 72 点 + # 这里取全部数据 + start_idx = 0 + end_idx = min(len(effective_data), finger_size) # 取较小值 + + finger_data_flat = effective_data[start_idx:end_idx] + + # d. 验证数据长度 + if finger_data_flat.size < finger_size: + # 如果数据不足，尝试多读一些 + rrsp2 = self.cli.read_input_registers(address=read_address + read_count, count=10, slave=self.slave) + if not rrsp2.isError(): + extra_data = [reg_value & 255 for reg_value in rrsp2.registers] + finger_data_flat = np.concatenate([finger_data_flat, np.array(extra_data, dtype=np.uint8)]) + + # 最终确保有足够数据 + if finger_data_flat.size >= finger_size: + finger_data_flat = finger_data_flat[:finger_size] + else: + raise ValueError( + f"数据提取失败。期望 {finger_size} 点 ({rows}x{cols})，" + f"但仅获取到 {finger_data_flat.size} 点。请检查协议。" + ) + + # 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 get_finger_order(self): + return ["thumb_cmc_pitch", "thumb_cmc_yaw", "index_mcp_pitch", "middle_mcp_pitch", "ring_mcp_pitch", "pinky_mcp_pitch"] + + # -------------------------------------------------- + # 便捷方法 + # -------------------------------------------------- + + 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}") 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 new file mode 100644 index 0000000..d68b621 --- /dev/null +++ b/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/core/rs485/linker_hand_l6_rs485.py.bak @@ -0,0 +1,444 @@ +#!/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_l7_rs485.py b/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/core/rs485/linker_hand_l7_rs485.py new file mode 100644 index 0000000..274a1ae --- /dev/null +++ b/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/core/rs485/linker_hand_l7_rs485.py @@ -0,0 +1,423 @@ +#!/usr/bin/env python3 +import time +from typing import List, Dict, Union +import numpy as np +from pymodbus.client import ModbusSerialClient +from pymodbus.exceptions import ModbusException + +# --- 协议常量和寄存器地址定义 (根据 O7 协议文件) --- + +# RS485 通信设置 +DEFAULT_BAUDRATE = 115200 + +# O7机械手七个可控关节的键名 (根据保持寄存器和输入寄存器地址 0-6) +O7_JOINT_KEYS = [ + "Thumb_Pitch", "Thumb_Yaw", "Index_Pitch", "Middle_Pitch", + "Ring_Pitch", "Little_Pitch", "Thumb_Roll" +] + +# 保持寄存器地址 (写操作 FC 16) +HR_ADDR = { + "Position_Start": 0, # 关节目标位置 (7 个寄存器: 0-6) + "Torque_Start": 7, # 关节目标转矩 (7 个寄存器: 7-13) + "Speed_Start": 14, # 关节目标速度 (7 个寄存器: 14-20) + "Pressure_Select": 42 # 压力传感器数据选择 (1 个寄存器) + # 21-41 为堵转保护阈值、时间和扭矩，暂未实现 +} + +# 输入寄存器地址 (读操作 FC 04) +IR_ADDR = { + "Current_Position_Start": 0, # 当前关节位置 (7 个寄存器: 0-6) + "Current_Torque_Start": 7, # 当前关节转矩 (7 个寄存器: 7-13) + "Current_Speed_Start": 14, # 当前关节速度 (7 个寄存器: 14-20) + "Current_Temperature_Start": 21, # 当前关节温度 (7 个寄存器: 21-27) + "Error_Code_Start": 28, # 当前关节错误码 (7 个寄存器: 28-34) + "Tip_Force_Start": 35, # 指尖力数据 (20 个寄存器: 35-54) + "Pressure_Data_Start": 57, # 压力传感器数据起始 (96 个寄存器: 57-152) + "Version_Start": 153 # 版本信息 (6 个寄存器: 153-158) +} + +# 辅助常量 +_JOINT_COUNT = 7 +_VERSION_COUNT = 6 +_TIP_FORCE_COUNT = 20 +_PRESSURE_REG_COUNT = 96 +_PRESSURE_ROWS = 12 # 从 IR 56 (0xC6) 推断 +_PRESSURE_COLS = 6 # 从 IR 56 (0xC6) 推断 +_PRESSURE_DATA_SIZE = _PRESSURE_ROWS * _PRESSURE_COLS # 72 +_PRESSURE_HEADER_SKIP = 10 # 假设跳过 10 个头部/校验字节 + +# 通信间隔时间 (使用 L10 参考中的 5ms) +_INTERVAL = 0.005 + + +class LinkerHandL7RS485: + """ + O7机械手 Modbus RTU (RS485) 控制类。 + 使用 pymodbus 3.5.1 版本和 O7 机械手协议。 + """ + def __init__(self, + hand_id: int = 0x27, + modbus_port: str = "/dev/ttyUSB0", + baudrate: int = DEFAULT_BAUDRATE, + timeout: float = 0.05): + """ + 初始化 Modbus 客户端。 + + :param hand_id: Modbus 从站地址 (0x27: 右手, 0x28: 左手) + :param modbus_port: 串口名称 + :param baudrate: 波特率 (默认为 115200) + :param timeout: 通信超时时间 (秒) + """ + self.slave = hand_id + self.cli = ModbusSerialClient( + port=modbus_port, + baudrate=baudrate, + bytesize=8, + parity="N", + stopbits=1, # 确保与 pymodbus 3.x 兼容的写法 + timeout=timeout, + retries=3, # 重试次数 + retry_on_empty=True, + handle_local_echo=False, + method='rtu' + ) + + # 尝试连接 + self.connected = self.cli.connect() + if not self.connected: + raise ConnectionError(f"RS485 connect fail to {modbus_port} with ID {hex(hand_id)}.") + + print(f"O7机械手 Modbus ID {hex(hand_id)} 连接成功到 {modbus_port}。") + + + # -------------------------------------------------- + # 核心读写函数 (基于 pymodbus 3.5.1) + # -------------------------------------------------- + + def _read_input_registers(self, address: int, count: int) -> List[int]: + """封装 Modbus 读取输入寄存器 (FC 04) 操作。""" + time.sleep(_INTERVAL) + try: + rsp = self.cli.read_input_registers( + address=address, + count=count, + slave=self.slave + ) + + # 使用 L10 参考中验证过的 3.x 兼容错误检查 + if rsp.isError(): + raise RuntimeError(f"Modbus FC04 读取失败。地址: {address}, 错误: {rsp}") + + return rsp.registers + + except ModbusException as e: + # 捕获通信超时、CRC 错误等 Modbus 异常 + raise RuntimeError(f"Modbus 通信异常。地址: {address}, 错误: {e}") + except Exception as e: + raise RuntimeError(f"未知读取异常。地址: {address}, 错误: {e}") + + def _write_holding_registers(self, address: int, values: List[int]): + """封装 Modbus 写入保持寄存器 (FC 16) 操作。""" + time.sleep(_INTERVAL) + + # 批量写入 (FC 16) + if len(values) > 1: + write_func = self.cli.write_registers + # 单个写入 (FC 06) + elif len(values) == 1: + write_func = lambda address, values, slave: self.cli.write_register(address, values[0], slave) + else: + raise ValueError("写入值列表不能为空。") + + try: + rsp = write_func( + address=address, + values=values, + slave=self.slave + ) + + if rsp.isError(): + raise RuntimeError(f"Modbus FC16 写入失败。地址: {address}, 错误: {rsp}") + + except ModbusException as e: + raise RuntimeError(f"Modbus 通信异常。地址: {address}, 错误: {e}") + except Exception as e: + raise RuntimeError(f"未知写入异常。地址: {address}, 错误: {e}") + + # -------------------------------------------------- + # 读操作 (Read API) + # -------------------------------------------------- + + def get_joint_positions(self) -> Dict[str, int]: + """读取当前关节位置 (地址 0-6)。""" + registers = self._read_input_registers(IR_ADDR["Current_Position_Start"], _JOINT_COUNT) + #return dict(zip(O7_JOINT_KEYS, registers)) + return registers + + def get_current_torques(self) -> Dict[str, int]: + """读取当前关节转矩 (地址 7-13)。""" + registers = self._read_input_registers(IR_ADDR["Current_Torque_Start"], _JOINT_COUNT) + #return dict(zip(O7_JOINT_KEYS, registers)) + return registers + + def get_current_speeds(self) -> Dict[str, int]: + """读取当前关节速度 (地址 14-20)。""" + registers = self._read_input_registers(IR_ADDR["Current_Speed_Start"], _JOINT_COUNT) + #return dict(zip(O7_JOINT_KEYS, registers)) + return registers + + def get_temperatures(self) -> Dict[str, int]: + """读取当前关节温度 (地址 21-27)。""" + registers = self._read_input_registers(IR_ADDR["Current_Temperature_Start"], _JOINT_COUNT) + #return dict(zip(O7_JOINT_KEYS, registers)) + return registers + + def get_error_codes(self) -> Dict[str, int]: + """读取当前关节错误码 (地址 28-34)。""" + registers = self._read_input_registers(IR_ADDR["Error_Code_Start"], _JOINT_COUNT) + #return dict(zip(O7_JOINT_KEYS, registers)) + return registers + + def get_tip_forces(self) -> List[int]: + """读取指尖法向力、切向力等数据 (地址 35-54)。""" + return self._read_input_registers(IR_ADDR["Tip_Force_Start"], _TIP_FORCE_COUNT) + + def get_version(self) -> List[int]: + """读取版本信息 (地址 153-158)。""" + return self._read_input_registers(IR_ADDR["Version_Start"], _VERSION_COUNT) + + def get_pressure_matrix(self, finger_id: int) -> np.ndarray: + """ + 读取特定手指的压力传感器数据矩阵。 + + :param finger_id: 手指编号 (1: 大拇指, 2: 食指, 3: 中指, 4: 无名指, 5: 小拇指) + :return: 12x6 的压力数据矩阵 (np.ndarray) + """ + if not (1 <= finger_id <= 5): + raise ValueError(f"无效的手指编号: {finger_id}。应在 1 到 5 之间。") + + # 1. 写入手指选择寄存器 (HR 42) + # 使用单个写入 (FC 06) + self._write_holding_registers(HR_ADDR["Pressure_Select"], [finger_id]) + + # 2. 读取压力传感器数据 (IR 57, 96 个寄存器) + time.sleep(_INTERVAL) # 等待数据更新 + registers_16bit: List[int] = self._read_input_registers( + IR_ADDR["Pressure_Data_Start"], + _PRESSURE_REG_COUNT + ) + + # 3. 数据解析 (假设与 L10 类似的数据格式: 低 8 位有效，有头部数据) + + # a. 提取低 8 位数据 (得到 96 个 8 位数据点) + final_data_96 = [reg_value & 255 for reg_value in registers_16bit] + + # b. 跳过头部数据点 + effective_data = np.array(final_data_96[_PRESSURE_HEADER_SKIP:], dtype=np.uint8) + + # c. 截取当前手指的矩阵数据 (72 个点) + finger_data_flat = effective_data[:_PRESSURE_DATA_SIZE] + + if finger_data_flat.size != _PRESSURE_DATA_SIZE: + raise ValueError( + f"压力数据提取失败。期望 {_PRESSURE_DATA_SIZE} 点，" + f"但仅截取到 {finger_data_flat.size} 点。请检查协议解析逻辑。" + ) + + # d. 重塑为二维矩阵 (12 行 6 列) + finger_matrix = finger_data_flat.reshape((_PRESSURE_ROWS, _PRESSURE_COLS)) + return finger_matrix + + + # -------------------------------------------------- + # 写操作 (Write API) + # -------------------------------------------------- + + def set_joint_positions(self, joint_angles: List[int]): + """ + 设置所有 7 个关节的目标位置 (地址 0-6)。 + :param joint_angles: 7 个 0-255 的整数值列表 + """ + if len(joint_angles) != _JOINT_COUNT: + raise ValueError(f"需要 {_JOINT_COUNT} 个关节位置值，提供了 {len(joint_angles)} 个。") + self._write_holding_registers(HR_ADDR["Position_Start"], joint_angles) + + def set_torques(self, torques: List[int]): + """ + 设置所有 7 个关节的目标转矩 (地址 7-13)。 + :param torques: 7 个 0-255 的整数值列表 + """ + if len(torques) != _JOINT_COUNT: + raise ValueError(f"需要 {_JOINT_COUNT} 个关节转矩值，提供了 {len(torques)} 个。") + self._write_holding_registers(HR_ADDR["Torque_Start"], torques) + + + def set_speeds(self, speeds: List[int]): + """ + 设置所有 7 个关节的目标速度 (地址 14-20)。 + :param speeds: 7 个 0-255 的整数值列表 + """ + if len(speeds) != _JOINT_COUNT: + raise ValueError(f"需要 {_JOINT_COUNT} 个关节速度值，提供了 {len(speeds)} 个。") + self._write_holding_registers(HR_ADDR["Speed_Start"], speeds) + + def set_speed(self, speed:List[int] = [200] * 7): + self.set_speeds(speed) + + def set_torque(self, torque: List[int] = [250] * 7): + self.set_torques(torque) + + def set_current(self, current=None): + print("当前L7不支持设置电流", flush=True) + + def get_current(self): + #print("当前L7不支持获取电流", flush=True) + return [-1] * 7 + + def get_state(self) -> List[int]: + return self.get_joint_positions() + + + def get_state_for_pub(self) -> List[int]: + return self.get_joint_positions() + + def get_current_status(self) -> List[int]: + return self.get_joint_positions() + + def get_speed(self) -> List[int]: + return self.get_current_speeds() + + def get_joint_speed(self) -> List[int]: + return self.get_speed() + + def get_touch_type(self) -> int: + return 2 + + def get_normal_force(self) -> List[int]: + return [-1] * 5 + + def get_tangential_force(self) -> List[int]: + return [-1] * 5 + + def get_approach_inc(self) -> List[int]: + return [-1] * 5 + + def get_touch(self) -> List[int]: + return [-1] * 5 + + def get_thumb_matrix_touch(self,sleep_time=0): + return self.get_pressure_matrix(finger_id=1) + + def get_index_matrix_touch(self,sleep_time=0): + return self.get_pressure_matrix(finger_id=2) + + def get_middle_matrix_touch(self,sleep_time=0): + return self.get_pressure_matrix(finger_id=3) + + def get_ring_matrix_touch(self,sleep_time=0): + return self.get_pressure_matrix(finger_id=4) + + def get_little_matrix_touch(self,sleep_time=0): + return self.get_pressure_matrix(finger_id=5) + + def get_matrix_touch(self) -> List[List[int]]: + return self.get_thumb_matrix_touch(),self.get_index_matrix_touch(), self.get_middle_matrix_touch(), self.get_ring_matrix_touch(), self.get_little_matrix_touch() + + def get_matrix_touch_v2(self) -> List[List[int]]: + return self.get_matrix_touch() + + def get_torque(self) -> List[int]: + return self.get_current_torques() + + def get_temperature(self) -> List[int]: + return self.get_temperatures() + + def get_fault(self) -> List[int]: + return self.get_error_codes() + + def get_serial_number(self): + return [0] * 6 + + def get_finger_order(self): + return ["thumb_cmc_pitch", "thumb_cmc_yaw", "index_mcp_pitch", "middle_mcp_pitch", "ring_mcp_pitch", "pinky_mcp_pitch", "thumb_cmc_roll"] + + def show_fun_table(self): + pass + + def clear_faults(self): + pass + # -------------------------------------------------- + # 上下文管理 + # -------------------------------------------------- + + def close(self): + """断开 Modbus 连接。""" + if self.connected: + self.cli.close() + self.connected = False + print("Modbus 连接已断开。") + + def __enter__(self): + return self + + def __exit__(self, exc_type, exc_val, exc_tb): + self.close() + + +# ------------------- Demo/使用示例 ------------------- +if __name__ == "__main__": + # --- 配置区域 --- + # 右手 Modbus ID: 0x27 (39) + # 左手 Modbus ID: 0x28 (40) + TARGET_HAND_ID = 0x28 # <--- 请根据需要修改为 0x27 或 0x28 + PORT = "/dev/ttyUSB0" # <--- 请修改为您的实际串口，例如 'COM3' + + try: + # 使用上下文管理器，确保连接自动关闭 + with LinkerHandL7RS485(hand_id=TARGET_HAND_ID, modbus_port=PORT) as hand: + print("\n--- 1. 读取当前状态 ---") + + # 读取当前关节位置、速度、转矩 + angles = hand.get_joint_positions() + print(f"当前关节位置 (7DOF): {angles}") + + speeds = hand.get_current_speeds() + print(f"当前关节速度: {speeds}") + + # 读取传感器和错误信息 + temps = hand.get_temperatures() + print(f"关节温度: {temps}") + + errors = hand.get_error_codes() + print(f"关节错误码: {errors}") + + # 读取版本 + version_info = hand.get_version() + print(f"版本信息 (Hand_freedom, ..., hardware_version): {version_info}") + + # --- 2. 写入指令示例 --- + print("\n--- 2. 写入指令示例 (设置所有关节到 128) ---") + + # 假设要将所有关节位置设置到中间值 128 + target_angles = [128] * _JOINT_COUNT + hand.set_joint_positions(target_angles) + print(f"写入目标角度: {target_angles}") + + # 假设要设置所有关节的速度到 100 + target_speeds = [100] * _JOINT_COUNT + hand.set_speeds(target_speeds) + print(f"写入目标速度: {target_speeds}") + + # --- 3. 压力传感器读取示例 --- + print("\n--- 3. 压力传感器读取 (大拇指 1) ---") + thumb_matrix = hand.get_pressure_matrix(finger_id=1) + print(f"大拇指压力矩阵 (12x6):") + print(thumb_matrix) + + except ConnectionError as e: + print(f"致命错误: 连接失败。{e}") + except RuntimeError as e: + print(f"致命错误: Modbus 操作失败。{e}") + 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 b/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/core/rs485/linker_hand_o6_rs485.py new file mode 100644 index 0000000..98240fd --- /dev/null +++ b/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/core/rs485/linker_hand_o6_rs485.py @@ -0,0 +1,671 @@ +#!/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 _pressure(self, finger: int) -> np.ndarray: + """ + 读取压力传感器数据 (10x4矩阵) + """ + rows = 10 # 10行 + cols = 4 # 4列 + finger_size = rows * cols # 40个数据点 + + # modbus 地址 (按O6协议文档) + write_address = 18 # 写入手指选择 (保持寄存器) + read_address = 47 # 读取压力数据 (输入寄存器) + read_count = 40 # 读取40个寄存器 + + # 0. 参数校验 + if finger < 1 or finger > 5: + raise ValueError(f"无效的手指编号: {finger}。手指编号应在 1 到 5 之间。") + + # 1. 写入手指选择寄存器 (地址18) + time.sleep(0.01) + self._write_reg(write_address, finger) + + # 2. 读取压力数据 + data = self._execute_read(read_address, read_count) + + # 3. 转换为numpy数组并重塑为10x4矩阵 + finger_matrix = np.array(data, dtype=np.uint8).reshape((rows, cols)) + + return finger_matrix + + def get_thumb_matrix_touch(self,sleep_time=0): + return np.array(self._pressure(1), dtype=np.uint8) + + + def get_index_matrix_touch(self,sleep_time=0): + return np.array(self._pressure(2), dtype=np.uint8) + + def get_middle_matrix_touch(self,sleep_time=0): + return np.array(self._pressure(3), dtype=np.uint8) + + def get_ring_matrix_touch(self,sleep_time=0): + return np.array(self._pressure(4), dtype=np.uint8) + + def get_little_matrix_touch(self,sleep_time=0): + return np.array(self._pressure(5), dtype=np.uint8) + + 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}") \ 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 new file mode 100644 index 0000000..cb08dd9 --- /dev/null +++ b/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/core/rs485/linker_hand_o6_rs485.py.bak @@ -0,0 +1,1157 @@ +#!/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/LinkerHand/linker_hand_api.py b/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/linker_hand_api.py new file mode 100644 index 0000000..631a7c9 --- /dev/null +++ b/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/linker_hand_api.py @@ -0,0 +1,355 @@ +#!/usr/bin/env python3 +# -*- coding: utf-8 -*- +import sys, os, time,threading +sys.path.append(os.path.dirname(os.path.abspath(__file__))) +from utils.mapping import * +from utils.color_msg import ColorMsg +from utils.load_write_yaml import LoadWriteYaml +from utils.open_can import OpenCan + +class LinkerHandApi: + def __init__(self, hand_type="left", hand_joint="L10", modbus = "None",can="can0"): # Ubuntu:can0 win:PCAN_USBBUS1 + self.last_position = [] + self.yaml = LoadWriteYaml() + self.config = self.yaml.load_setting_yaml() + self.version = self.config["VERSION"] + self.can = can + ColorMsg(msg=f"Current SDK version: {self.version}", color="green") + self.hand_joint = hand_joint + self.hand_type = hand_type + self.is_palm_touch = -1 # 是否为全掌压力传感器 + if self.hand_type == "left": + self.hand_id = 0x28 # Left hand + if self.hand_type == "right": + self.hand_id = 0x27 # Right hand + if self.hand_joint.upper() == "O6": + if modbus != "None": + from core.rs485.linker_hand_o6_rs485 import LinkerHandO6RS485 + self.hand = LinkerHandO6RS485(hand_id=self.hand_id,modbus_port=modbus,baudrate=115200) + else: + from core.can.linker_hand_o6_can import LinkerHandO6Can + self.hand = LinkerHandO6Can(can_id=self.hand_id,can_channel=self.can, yaml=self.yaml) + if self.hand_joint == "L6": + if modbus != "None": + from core.rs485.linker_hand_l6_rs485 import LinkerHandL6RS485 + self.hand = LinkerHandL6RS485(hand_id=self.hand_id,modbus_port=modbus,baudrate=115200) + else: + from core.can.linker_hand_l6_can import LinkerHandL6Can + self.hand = LinkerHandL6Can(can_id=self.hand_id,can_channel=self.can, yaml=self.yaml) + if self.hand_joint == "L7": + if modbus != "None": + from core.rs485.linker_hand_l7_rs485 import LinkerHandL7RS485 + self.hand = LinkerHandL7RS485(hand_id=self.hand_id,modbus_port=modbus,baudrate=115200) + else: + from core.can.linker_hand_l7_can import LinkerHandL7Can + self.hand = LinkerHandL7Can(can_id=self.hand_id,can_channel=self.can, yaml=self.yaml) + if self.hand_joint == "L10": + if modbus != "None": + from core.rs485.linker_hand_l10_rs485 import LinkerHandL10RS485 + self.hand = LinkerHandL10RS485(hand_id=self.hand_id,modbus_port=modbus,baudrate=115200) + else: + from core.can.linker_hand_l10_can import LinkerHandL10Can + self.hand = LinkerHandL10Can(can_id=self.hand_id,can_channel=self.can, yaml=self.yaml) + if self.hand_joint == "L20": + from core.can.linker_hand_l20_can import LinkerHandL20Can + self.hand = LinkerHandL20Can(can_id=self.hand_id,can_channel=self.can, yaml=self.yaml) + if self.hand_joint == "G20": + from core.can.linker_hand_g20_can import LinkerHandG20Can + self.hand = LinkerHandG20Can(can_id=self.hand_id,can_channel=self.can, yaml=self.yaml) + time.sleep(0.01) + self.is_palm_touch = self.hand.get_touch_sensor_type() + ColorMsg(msg=f"传感器类型:{self.is_palm_touch}") + if self.hand_joint == "L21": + from core.can.linker_hand_l21_can import LinkerHandL21Can + self.hand = LinkerHandL21Can(can_id=self.hand_id,can_channel=self.can, yaml=self.yaml) + if self.hand_joint == "L25": + from core.can.linker_hand_l25_can import LinkerHandL25Can + self.hand = LinkerHandL25Can(can_id=self.hand_id,can_channel=self.can, yaml=self.yaml) + # Open can0 + if sys.platform == "linux" and modbus=="None": + self.open_can = OpenCan(load_yaml=self.yaml) + self.open_can.open_can(self.can) + self.is_can = self.open_can.is_can_up_sysfs(interface=self.can) + if not self.is_can: + ColorMsg(msg=f"{self.can} interface is not open", color="red") + sys.exit(1) + version = self.get_embedded_version() + self.serial_number = self.get_serial_number() + if version == None or len(version) == 0: + ColorMsg(msg="Warning: Hardware version number not recognized, it is recommended to terminate the program and re insert USB to CAN conversion", color="yellow") + else: + ColorMsg(msg=f"Embedded:{version}", color="green") + ColorMsg(msg=f"Linker Hand Serial Number: {self.serial_number}", color="green") + + + # Five-finger movement + def finger_move(self, pose=[]): + ''' + Five-finger movement + @params: pose list L7 len(7) | L10 len(10) | L20 len(20) | L25 len(25) 0~255 + ''' + + if len(pose) == 0: + return + pose = [int(v) for v in pose] + if any(not isinstance(x, (int, float)) or x < 0 or x > 255 for x in pose): + ColorMsg(msg=f"The numerical range cannot be less than 0 or greater than 255",color="red") + return + if (self.hand_joint.upper() == "O6" or self.hand_joint.upper() == "L6") and len(pose) == 6: + self.hand.set_joint_positions(pose) + elif self.hand_joint == "L7" and len(pose) == 7: + self.hand.set_joint_positions(pose) + elif self.hand_joint == "L10" and len(pose) == 10: + self.hand.set_joint_positions(pose) + elif self.hand_joint == "L20" and len(pose) == 20: + self.hand.set_joint_positions(pose) + elif self.hand_joint == "G20" and len(pose) == 20: + self.hand.set_joint_positions(pose) + elif self.hand_joint == "L21" and len(pose) == 25: + self.hand.set_joint_positions(pose) + elif self.hand_joint == "L25" and len(pose) == 25: + self.hand.set_joint_positions(pose) + else: + ColorMsg(msg=f"Current LinkerHand is {self.hand_type}{self.hand_joint}, action sequence is {pose}, does not match", color="red") + self.last_position = pose + + def _get_normal_force(self): + '''# Get normal force''' + self.hand.get_normal_force() + + def _get_tangential_force(self): + '''# Get tangential force''' + self.hand.get_tangential_force() + + def _get_tangential_force_dir(self): + '''# Get tangential force direction''' + self.hand.get_tangential_force_dir() + + def _get_approach_inc(self): + '''# Get approach increment''' + self.hand.get_approach_inc() + + + def set_speed(self, speed=[100]*5): + '''# Set speed''' + has_non_int = any(not isinstance(x, (int, float)) or x < 0 or x > 255 for x in speed) + if has_non_int: + print("Set Speed The numerical range can only be positive integers or floating-point numbers between 0 and 255", flush=True) + return + if len(speed) < 5: + print("数据长度不够,至少5个元素", flush=True) + return + if self.hand_joint == "L7" and len(speed) < 7: + print("数据长度不够,至少7个元素", flush=True) + return + ColorMsg(msg=f"{self.hand_type} {self.hand_joint} set speed to {speed}", color="green") + self.hand.set_speed(speed=speed) + + def set_joint_speed(self, speed=[100]*5): + '''Set speed by topic''' + if len(speed) == 0: + return + if any(not isinstance(x, (int, float)) or x < 10 or x > 255 for x in speed): + ColorMsg(msg=f"The numerical range cannot be less than 10 or greater than 255",color="red") + return + self.hand.set_speed(speed=speed) + + def set_torque(self, torque=[180] * 5): + '''Set maximum torque''' + has_non_int = any(not isinstance(x, (int, float)) or x < 0 or x > 255 for x in torque) + if has_non_int: + print("Set Torque The numerical range can only be positive integers or floating-point numbers between 0 and 255", flush=True) + return + if len(torque) < 5: + print("数据长度不够,至少5个元素", flush=True) + return + if self.hand_joint == "L7" and len(torque) < 7: + print("数据长度不够,至少7个元素", flush=True) + return + if (self.hand_joint == "L6" or self.hand_joint == "O6") and len(torque) != 6: + print("L6 or O6数据长度错误,至少6个元素", flush=True) + return + ColorMsg(msg=f"{self.hand_type} {self.hand_joint} set maximum torque to {torque}", color="green") + return self.hand.set_torque(torque=torque) + + + def set_current(self, current=[250] * 5): + '''Set current L7/L10/L25 not supported''' + if any(not isinstance(x, (int, float)) or x < 0 or x > 255 for x in current): + print("Set Current The numerical range can only be positive integers or floating-point numbers between 0 and 255", flush=True) + return + if self.hand_joint == "L20": + return self.hand.set_current(current=current) + else: + pass + + def get_embedded_version(self): + '''Get embedded version''' + return self.hand.get_version() + + def get_serial_number(self): + '''Get serial number''' + try: + return self.hand.sn + except: + return self.hand.get_serial_number() + + def get_current(self): + '''Get current''' + return self.hand.get_current() + + def get_state(self): + '''Get current joint state''' + return self.hand.get_current_status() + + + def get_state_for_pub(self): + return self.hand.get_current_pub_status() + + def get_speed(self): + '''Get speed''' + return self.hand.get_speed() + + + def get_joint_speed(self): + speed = [] + if self.hand_joint.upper() == "O6" or self.hand_joint.upper() == "L6": + return self.hand.get_speed() + elif self.hand_joint == "L7": + return self.hand.get_speed() + elif self.hand_joint == "L10": + speed = self.hand.get_speed() + return speed + elif self.hand_joint == "G20": + return self.hand.get_speed() + elif self.hand_joint == "L20": + speed = self.hand.get_speed() + return [255, speed[1], speed[2], speed[3], speed[4], 255, 255, 255, 255, 255, speed[0], 255, 255, 255, 255, 255, 255, 255, 255, 255] + elif self.hand_joint == "L21": + return self.hand.get_speed() + elif self.hand_joint == "L25": + return self.hand.get_speed() + + def get_touch_type(self): + '''Get touch type''' + try: + return self.hand.touch_type + except: + return self.hand.get_touch_type() + + def get_force(self): + '''Get normal force, tangential force, tangential force direction, approach sensing data''' + self._get_normal_force() + self._get_tangential_force() + self._get_tangential_force_dir() + self._get_approach_inc() + return self.hand.get_force() + + def get_touch(self): + '''Get touch data''' + return self.hand.get_touch() + + def get_matrix_touch(self): + return self.hand.get_matrix_touch() + + def get_matrix_touch_v2(self): + return self.hand.get_matrix_touch_v2() + + + def get_thumb_matrix_touch(self,sleep_time=0): + if sleep_time > 0: + return self.hand.get_thumb_matrix_touch(sleep_time=sleep_time) + else: + return self.hand.get_thumb_matrix_touch() + + def get_index_matrix_touch(self,sleep_time=0): + if sleep_time > 0: + return self.hand.get_index_matrix_touch(sleep_time=sleep_time) + else: + return self.hand.get_index_matrix_touch() + + def get_middle_matrix_touch(self,sleep_time=0): + if sleep_time > 0: + return self.hand.get_middle_matrix_touch(sleep_time=sleep_time) + else: + return self.hand.get_middle_matrix_touch() + + def get_ring_matrix_touch(self,sleep_time=0): + if sleep_time > 0: + return self.hand.get_ring_matrix_touch(sleep_time=sleep_time) + else: + return self.hand.get_ring_matrix_touch() + + def get_little_matrix_touch(self,sleep_time=0): + if sleep_time > 0: + return self.hand.get_little_matrix_touch(sleep_time=sleep_time) + else: + return self.hand.get_little_matrix_touch() + + def get_palm_matrix_touch(self,sleep_time=0): + if self.is_palm_touch == 5: + if sleep_time > 0: + return self.hand.get_palm_matrix_touch(sleep_time=sleep_time) + else: + return self.hand.get_palm_matrix_touch() + + def get_torque(self): + '''Get current maximum torque''' + return self.hand.get_torque() + + def get_temperature(self): + '''Get current motor temperature''' + return self.hand.get_temperature() + + def get_fault(self): + '''Get motor fault code''' + return self.hand.get_fault() + + def clear_faults(self): + '''Clear motor fault codes Not supported yet, currently only supports L20''' + self.hand.clear_faults() + return [0] * 5 + + def set_enable(self): + '''Set motor enable Only supports L25''' + if self.hand_joint == "L25": + self.hand.set_enable_mode() + else: + pass + + def set_disable(self): + '''Set motor disable Only supports L25''' + if self.hand_joint == "L25": + self.hand.set_disability_mode() + else: + pass + + def get_finger_order(self): + '''Get finger motor order''' + # if self.hand_joint == "L21" or self.hand_joint == "L25" or self.hand_joint == "G20": + # return self.hand.get_finger_order() + # else: + # return [] + return self.hand.get_finger_order() + + def range_to_arc_left(self, state, hand_joint): + return range_to_arc_left(left_range=state, hand_joint=hand_joint) + + def range_to_arc_right(self, state, hand_joint): + return range_to_arc_right(right_range=state, hand_joint=hand_joint) + + def arc_to_range_left(self,state,hand_joint): + return arc_to_range_left(hand_arc_l=state,hand_joint=hand_joint) + + def arc_to_range_right(self,state,hand_joint): + return arc_to_range_right(right_arc=state,hand_joint=hand_joint) + + def show_fun_table(self): + self.hand.show_fun_table() + + def close_can(self): + if sys.platform == "linux" and modbus=="None": + self.open_can.close_can(can=self.can) + +if __name__ == "__main__": + hand = LinkerHandApi(hand_type="right", hand_joint="L10") diff --git a/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/utils/__init__.py b/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/utils/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/utils/color_msg.py b/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/utils/color_msg.py new file mode 100644 index 0000000..9ba06ac --- /dev/null +++ b/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/utils/color_msg.py @@ -0,0 +1,27 @@ +#! /usr/bin/env python3 + +import time + +class ColorMsg(): + def __init__(self,msg: str,color: str = '', timestamp: bool = True) -> None: + self.msg = msg + self.color = color + self.timestamp = timestamp + self.colorMsg(msg=self.msg, color=self.color, timestamp=self.timestamp) + + def colorMsg(self,msg: str, color: str = '', timestamp: bool = True): + str = "" + if timestamp: + str += time.strftime('%Y-%m-%d %H:%M:%S', + time.localtime(time.time())) + " " + if color == "red": + str += "\033[1;31;40m" + elif color == "green": + str += "\033[1;32;40m" + elif color == "yellow": + str += "\033[1;33;40m" + else: + print(str + msg, flush=True) + return + str += msg + "\033[0m" + print(str, flush=True) \ No newline at end of file diff --git a/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/utils/init_linker_hand.py b/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/utils/init_linker_hand.py new file mode 100644 index 0000000..6bfcae7 --- /dev/null +++ b/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/utils/init_linker_hand.py @@ -0,0 +1,81 @@ +''' +Author: HJX +Date: 2025-04-01 14:09:21 +LastEditors: Please set LastEditors +LastEditTime: 2025-04-08 11:18:23 +FilePath: /Linker_Hand_SDK_ROS/src/linker_hand_sdk_ros/scripts/LinkerHand/utils/init_linker_hand.py +Description: +symbol_custom_string_obkorol_copyright: +''' +import yaml, os, sys +sys.path.append(os.path.dirname(os.path.abspath(__file__))) +from load_write_yaml import LoadWriteYaml + +class InitLinkerHand(): + def __init__(self): + self.yaml = LoadWriteYaml() + self.setting = self.yaml.load_setting_yaml() + + def current_hand(self): + ''' + 初始化灵巧手 + return: hand_joint str L7/L10/L20/L21/L25, hand_type str left or right + ''' + # 左手是否配置 + self.left_hand = None + self.left_hand_joint = None + self.left_hand_type = None + self.left_hand_force = None + self.left_hand_pose = None + self.left_hand_torque = [200, 200, 200, 200, 200] + self.left_hand_speed = [80, 200, 200, 200, 200] + # 右手是否配置 + self.right_hand = None + self.right_hand_joint = None + self.right_hand_type = None + self.right_hand_force = None + self.right_hand_pose = None + self.right_hand_torque = [200, 200, 200, 200, 200] + self.right_hand_speed = [80, 200, 200, 200, 200] + if self.setting['LINKER_HAND']['LEFT_HAND']['EXISTS'] == True: + self.left_hand = True + self.left_hand_joint = self.setting['LINKER_HAND']['LEFT_HAND']['JOINT'] + self.left_hand_type = "left" + self.left_hand_force = self.setting['LINKER_HAND']['LEFT_HAND']['TOUCH'] + if self.left_hand_joint == "L7": + # The data length of L7 is 7, reinitialize here + self.left_hand_pose = [255, 200, 255, 255, 255, 255, 180] + self.left_hand_torque = [250, 250, 250, 250, 250, 250, 250] + self.left_hand_speed = [120, 180, 180, 180, 180, 180, 180] + elif self.left_hand_joint == "L10": + self.left_hand_pose = [255, 200, 255, 255, 255, 255, 180, 180, 180, 41] + elif self.left_hand_joint == "L20": + self.left_hand_pose = [255,255,255,255,255,255,10,100,180,240,245,255,255,255,255,255,255,255,255,255] + elif self.left_hand_joint == "L21": + self.left_hand_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.left_hand_joint == "L25": + self.left_hand_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 self.setting['LINKER_HAND']['RIGHT_HAND']['EXISTS'] == True: + self.right_hand = True + self.right_hand_joint = self.setting['LINKER_HAND']['RIGHT_HAND']['JOINT'] + self.right_hand_type = "right" + self.right_hand_force = self.setting['LINKER_HAND']['RIGHT_HAND']['TOUCH'] + if self.right_hand_joint == "L7": + # The data length of L7 is 7, reinitialize here + self.right_hand_pose = [255, 200, 255, 255, 255, 255, 180] + self.right_hand_torque = [250, 250, 250, 250, 250, 250, 250] + self.right_hand_speed = [120, 250, 250, 250, 250, 250, 250] + elif self.right_hand_joint == "L10": + self.right_hand_pose = [255, 200, 255, 255, 255, 255, 180, 180, 180, 41] + elif self.right_hand_joint == "L20": + self.right_hand_pose = [255,255,255,255,255,255,10,100,180,240,245,255,255,255,255,255,255,255,255,255] + elif self.right_hand_joint == "L21": + self.right_hand_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.right_hand_joint == "L25": + self.right_hand_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] + + + return self.left_hand ,self.left_hand_joint ,self.left_hand_type ,self.left_hand_force,self.left_hand_pose, self.left_hand_torque, self.left_hand_speed ,self.right_hand ,self.right_hand_joint ,self.right_hand_type ,self.right_hand_force,self.right_hand_pose, self.right_hand_torque, self.right_hand_speed,self.setting + + \ No newline at end of file diff --git a/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/utils/load_write_yaml.py b/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/utils/load_write_yaml.py new file mode 100644 index 0000000..160674c --- /dev/null +++ b/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/utils/load_write_yaml.py @@ -0,0 +1,101 @@ +#!/usr/bin/env python3 +# -*- coding: utf-8 -*- +''' +Author: HJX +Date: 2025-04-01 14:09:21 +LastEditors: Please set LastEditors +LastEditTime: 2025-04-11 10:19:01 +FilePath: /LinkerHand_Python_SDK/LinkerHand/utils/load_write_yaml.py +Description: +symbol_custom_string_obkorol_copyright: +''' +import yaml, os, sys +class LoadWriteYaml(): + def __init__(self): + # 由于是API形式，这里要给配置文件目录绝对路径 + #yaml_path = "/home/linkerhand/ROS2/linker_hand_ros2_sdk/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand" + yaml_path = os.path.dirname(os.path.abspath(__file__)) + "/../../LinkerHand" + self.setting_path = yaml_path+"/config/setting.yaml" + self.l7_positions = yaml_path+"/config/L7_positions.yaml" + self.l10_positions = yaml_path+"/config/L10_positions.yaml" + self.l20_positions = yaml_path+"/config/L20_positions.yaml" + self.l21_positions = yaml_path+"/config/L21_positions.yaml" + self.l25_positions = yaml_path+"/config/L25_positions.yaml" + + + def load_setting_yaml(self): + try: + with open(self.setting_path, 'r', encoding='utf-8') as file: + setting = yaml.safe_load(file) + self.sdk_version = setting["VERSION"] + self.left_hand_exists = setting['LINKER_HAND']['LEFT_HAND']['EXISTS'] + self.left_hand_names = setting['LINKER_HAND']['LEFT_HAND']['NAME'] + self.left_hand_joint = setting['LINKER_HAND']['LEFT_HAND']['JOINT'] + self.left_hand_force = setting['LINKER_HAND']['LEFT_HAND']['TOUCH'] + self.right_hand_exists = setting['LINKER_HAND']['RIGHT_HAND']['EXISTS'] + self.right_hand_names = setting['LINKER_HAND']['RIGHT_HAND']['NAME'] + self.right_hand_joint = setting['LINKER_HAND']['RIGHT_HAND']['JOINT'] + self.right_hand_force = setting['LINKER_HAND']['RIGHT_HAND']['TOUCH'] + self.password = setting['PASSWORD'] + except Exception as e: + setting = None + print(f"Error reading setting.yaml: {e}") + self.setting = setting + return self.setting + + def load_action_yaml(self,hand_joint="",hand_type=""): + if hand_joint == "L20": + action_path = self.l20_positions + elif hand_joint == "L10": + action_path = self.l10_positions + elif hand_joint == "L25": + action_path = self.l25_positions + elif hand_joint == "L21": + action_path = self.l21_positions + elif hand_joint == "L7": + action_path = self.l7_positions + print(action_path) + try: + with open(action_path, 'r', encoding='utf-8') as file: + yaml_data = yaml.safe_load(file) + if hand_type == "left": + self.action_yaml = yaml_data["LEFT_HAND"] + else: + self.action_yaml = yaml_data["RIGHT_HAND"] + except Exception as e: + self.action_yaml = None + print(f"yaml配置文件不存在: {e}") + return self.action_yaml + + def write_to_yaml(self, action_name, action_pos,hand_joint="",hand_type=""): + a = False + if hand_joint == "L20": + action_path = self.l20_positions + elif hand_joint == "L10": + action_path = self.l10_positions + elif hand_joint == "L7": + action_path = self.l7_positions + elif hand_joint == "L21": + action_path = self.l21_positions + elif hand_joint == "L25": + action_path = self.l25_positions + try: + with open(action_path, 'r', encoding='utf-8') as file: + yaml_data = yaml.safe_load(file) + print(yaml_data) + if hand_type == "left": + if yaml_data["LEFT_HAND"] == None: + yaml_data["LEFT_HAND"] = [] + yaml_data["LEFT_HAND"].append({"ACTION_NAME": action_name, "POSITION": action_pos}) + elif hand_type == "right": + if yaml_data["RIGHT_HAND"] == None: + yaml_data["RIGHT_HAND"] = [] + yaml_data["RIGHT_HAND"].append({"ACTION_NAME": action_name, "POSITION": action_pos}) + with open(action_path, 'w', encoding='utf-8') as file: + yaml.safe_dump(yaml_data, file, allow_unicode=True) + a = True + except Exception as e: + a = False + print(f"Error writing to yaml file: {e}") + return a + \ No newline at end of file diff --git a/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/utils/mapping.py b/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/utils/mapping.py new file mode 100644 index 0000000..deb6a19 --- /dev/null +++ b/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/utils/mapping.py @@ -0,0 +1,383 @@ +#--------------------------------------------------------------------------------------------------- +# L6 L +l6_l_min = [0, 0, 0, 0, 0, 0] +l6_l_max = [0.99, 1.39, 1.26, 1.26, 1.26, 1.26] +l6_l_derict = [-1, -1, -1, -1, -1, -1] +# L6 R +l6_r_min = [0, 0, 0, 0, 0, 0] +l6_r_max = [0.99, 1.39, 1.26, 1.26, 1.26, 1.26] +l6_r_derict = [-1, -1, -1, -1, -1, -1] +#--------------------------------------------------------------------------------------------------- +# O6 L +o6_l_min = [0, 0, 0, 0, 0, 0] +o6_l_max = [0.58, 1.36, 1.6, 1.6, 1.6, 1.6] +o6_l_derict = [-1, -1, -1, -1, -1, -1] +# O6 R +o6_r_min = [0, 0, 0, 0, 0, 0] +o6_r_max = [0.58, 1.36, 1.6, 1.6, 1.6, 1.6] +o6_r_derict = [-1, -1, -1, -1, -1, -1] +#--------------------------------------------------------------------------------------------------- +# L7 L OK +l7_l_min = [0, 0, 0, 0, 0, 0, 0] +l7_l_max = [0.44, 1.43, 1.62, 1.62, 1.62, 1.62, 1.01] +l7_l_derict = [-1, -1, -1, -1, -1, -1, -1] +# L7 R OK (urdf后续会更改！！！) +l7_r_min = [0, -1.43, 0, 0, 0, 0, 0] +l7_r_max = [0.75, 0, 1.62, 1.62, 1.62, 1.62, 1.54] +l7_r_derict = [-1, 0, -1, -1, -1, -1, -1] +#--------------------------------------------------------------------------------------------------- +# L10 L OK +l10_l_min = [0, 0, 0, 0, 0, 0, 0, -0.26, -0.26, -0.52] +l10_l_max = [1.45, 1.43, 1.62, 1.62, 1.62, 1.62, 0.26, 0, 0, 1.01] +l10_l_derict = [-1, -1, -1, -1, -1, -1, 0, -1, -1, -1] +# L10 R OK +l10_r_min = [0, 0, 0, 0, 0, 0, -0.26, 0, 0, -0.52] +l10_r_max = [0.75, 1.43, 1.62, 1.62, 1.62, 1.62, 0.21, 0.21, 0.34, 1.01] +l10_r_derict = [-1, -1, -1, -1, -1, -1, 0, 0, 0, -1] +#--------------------------------------------------------------------------------------------------- +# L20 L OK +l20_l_min = [0, 0, 0, 0, 0, -0.297, -0.26, -0.26, -0.26, -0.26, 0.122, 0, 0, 0, 0, 0, 0, 0, 0, 0] +l20_l_max = [0.87, 1.4, 1.4, 1.4, 1.4, 0.683, 0.26, 0.26, 0.26, 0.26, 1.78, 0, 0, 0, 0, 1.29, 1.08, 1.08, 1.08, 1.08] +l20_l_derict = [-1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, 0, 0, 0, 0, -1, -1, -1, -1, -1] +# L20 R OK +l20_r_min = [0, 0, 0, 0, 0, -0.297, -0.26, -0.26, -0.26, -0.26, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0] +l20_r_max = [0.87, 1.4, 1.4, 1.4, 1.4, 0.683, 0.26, 0.26, 0.26, 0.26, 1.78, 0, 0, 0, 0, 1.29, 1.08, 1.08, 1.08, 1.08] +l20_r_derict = [-1, -1, -1, -1, -1, -1, 0, 0, 0, 0, -1, 0, 0, 0, 0, -1, -1, -1, -1, -1] +#--------------------------------------------------------------------------------------------------- +# L21 L OK +l21_l_min = [0, 0, 0, 0, 0, 0, 0, -0.18, -0.18, 0, -0.6, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0] +l21_l_max = [1, 1.57, 1.57, 1.57, 1.57, 1.6, 0.18, 0.18, 0.18, 0.18, 0.6, 0, 0, 0, 0, 1.57, 0, 0, 0, 0, 1.57, 1.57, 1.57, 1.57, 1.57] +l21_l_derict = [-1, -1, -1, -1, -1, -1, -1, -1, -1, 0, -1, 0, 0, 0, 0, -1, 0, 0, 0, 0, -1, -1, -1, -1, -1] +# L21 R OK +l21_r_min = [0, 0, 0, 0, 0, 0, -0.18, -0.18, -0.18, -0.18, -0.6, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0] +l21_r_max = [1, 1.57, 1.57, 1.57, 1.57, 1.6, 0.18, 0.18, 0.18, 0.18, 0.6, 0, 0, 0, 0, 1.57, 0, 0, 0, 0, 1.57, 1.57, 1.57, 1.57, 1.57] +l21_r_derict = [-1, -1, -1, -1, -1, -1, 0, 0, 0, 0, -1, 0, 0, 0, 0, -1, 0, 0, 0, 0, -1, -1, -1, -1, -1] +#--------------------------------------------------------------------------------------------------- +#--------------------------------------------------------------------------------------------------- +# L25 L OK +l25_l_min = [0, 0, 0, 0, 0, 0, -0.26, -0.26, -0.26, -0.26, -0.26, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0] +l25_l_max = [0.9, 1.57, 1.57, 1.57, 1.57, 1.3, 0.26, 0.26, 0.26, 0.26, 0.61, 0, 0, 0, 0, 1.57, 1.57, 1.57, 1.57, 1.57, 1.57, 1.57, 1.57, 1.57, 1.57] +l25_l_derict = [-1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1] +# L25 R OK +l25_r_min = [0, 0, 0, 0, 0, 0, -0.26, -0.26, -0.26, -0.26, -0.26, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0] +l25_r_max = [0.9, 1.57, 1.57, 1.57, 1.57, 1.3, 0.26, 0.26, 0.26, 0.26, 0.61, 0, 0, 0, 0, 1.57, 1.57, 1.57, 1.57, 1.57, 1.57, 1.57, 1.57, 1.57, 1.57] +l25_r_derict = [-1, -1, -1, -1, -1, -1, 0, 0, 0, 0, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1] +#--------------------------------------------------------------------------------------------------- + +def range_to_arc_left(left_range,hand_joint): + num=0 + if hand_joint == "L6": + num = 6 + l_min = l6_l_min + l_max = l6_l_max + l_derict = l6_l_derict + elif hand_joint == "O6": + num = 6 + l_min = o6_l_min + l_max = o6_l_max + l_derict = o6_l_derict + elif hand_joint == "L7": + num = 7 + l_min = l7_l_min + l_max = l7_l_max + l_derict = l7_l_derict + elif hand_joint == "L10": + num = 10 + l_min = l10_l_min + l_max = l10_l_max + l_derict = l10_l_derict + elif hand_joint == "L20": + num = 20 + l_min = l20_l_min + l_max = l20_l_max + l_derict = l20_l_derict + elif hand_joint == "L21": + num = 25 + l_min = l21_l_min + l_max = l21_l_max + l_derict = l21_l_derict + hand_arc = [0] * num + for i in range(num): + if hand_joint == "L20": + if 11 <= i <= 14: continue + if hand_joint == "L21": + if 11 <= i <= 14: continue + if 16 <= i <= 19: continue + val_l = is_within_range(left_range[i], 0, 255) + if l_derict[i] == -1: + hand_arc[i] = scale_value(val_l, 0, 255, l_max[i], l_min[i]) + else: + hand_arc[i] = scale_value(val_l, 0, 255, l_min[i], l_max[i]) + return hand_arc + +def range_to_arc_right(right_range,hand_joint): + num=0 + if hand_joint == "L6": + num = 6 + r_min = l6_r_min + r_max = l6_r_max + r_derict = l6_r_derict + elif hand_joint == "O6": + num = 6 + r_min = o6_r_min + r_max = o6_r_max + r_derict = o6_r_derict + elif hand_joint == "L7": + num = 7 + r_min = l7_r_min + r_max = l7_r_max + r_derict = l7_r_derict + elif hand_joint == "L10": + num = 10 + r_min = l10_r_min + r_max = l10_r_max + r_derict = l10_r_derict + elif hand_joint == "L20": + num = 20 + r_min = l20_r_min + r_max = l20_r_max + r_derict = l20_r_derict + elif hand_joint == "L21": + num = 25 + r_min = l21_r_min + r_max = l21_r_max + r_derict = l21_r_derict + hand_arc = [0] * num + for i in range(num): + if hand_joint == "L20": + if 11 <= i <= 14: continue + if hand_joint == "L21": + if 11 <= i <= 14: continue + if 16 <= i <= 19: continue + val_r = is_within_range(right_range[i], 0, 255) + if r_derict[i] == -1: + hand_arc[i] = scale_value(val_r, 0, 255, r_max[i], r_min[i]) + else: + hand_arc[i] = scale_value(val_r, 0, 255, r_min[i], r_max[i]) + return hand_arc + +''' +def arc_to_range_left(left_arc,hand_joint): + num=0 + if hand_joint == "L7": + num = 7 + l_min = l7_l_min + l_max = l7_l_max + l_derict = l7_l_derict + elif hand_joint == "L10": + num = 10 + l_min = l10_l_min + l_max = l10_l_max + l_derict = l10_l_derict + elif hand_joint == "L20": + num = 20 + l_min = l20_l_min + l_max = l20_l_max + l_derict = l20_l_derict + elif hand_joint == "L21": + num = 25 + l_min = l21_l_min + l_max = l21_l_max + l_derict = l21_l_derict + hand_range = [0] * num + for i in range(num): + if hand_joint == "L20": + if 11 <= i <= 14: continue + if hand_joint == "L21": + if 11 <= i <= 14: continue + if 16 <= i <= 19: continue + val_l = is_within_range(left_arc[i], 0, 255) + if l_derict[i] == -1: + hand_range[i] = scale_value(val_l, 0, 255, l_max[i], l_min[i]) + else: + hand_range[i] = scale_value(val_l, 0, 255, l_min[i], l_max[i]) + return hand_range + ''' +def arc_to_range_left(hand_arc_l,hand_joint): + num=0 + if hand_joint == "O6": + num = 6 + l_min = o6_l_min + l_max = o6_l_max + l_derict = o6_l_derict + elif hand_joint == "L7": + num = 7 + l_min = l7_l_min + l_max = l7_l_max + l_derict = l7_l_derict + elif hand_joint == "L10": + num = 10 + l_min = l10_l_min + l_max = l10_l_max + l_derict = l10_l_derict + elif hand_joint == "L20": + num = 20 + l_min = l20_l_min + l_max = l20_l_max + l_derict = l20_l_derict + elif hand_joint == "L21": + num = 25 + l_min = l21_l_min + l_max = l21_l_max + l_derict = l21_l_derict + hand_range = [0] * num + #hand_range_l = [0] * 7 + for i in range(num): + if hand_joint == "L20": + if 11 <= i <= 14: continue + if hand_joint == "L21": + if 11 <= i <= 14: continue + if 16 <= i <= 19: continue + val_l = is_within_range(hand_arc_l[i], l_min[i], l_max[i]) + if l_derict[i] == -1: + hand_range[i] = scale_value(val_l, l_min[i], l_max[i], 255, 0) + else: + hand_range[i] = scale_value(val_l, l_min[i], l_max[i], 0, 255) + + return hand_range + +def arc_to_range_right(right_arc,hand_joint): + num=0 + if hand_joint == "O6": + num = 6 + r_min = o6_r_min + r_max = o6_r_max + r_derict = o6_r_derict + elif hand_joint == "L7": + num = 7 + r_min = l7_r_min + r_max = l7_r_max + r_derict = l7_r_derict + elif hand_joint == "L10": + num = 10 + r_min = l10_r_min + r_max = l10_r_max + r_derict = l10_r_derict + elif hand_joint == "L20": + num = 20 + r_min = l20_r_min + r_max = l20_r_max + r_derict = l20_r_derict + elif hand_joint == "L21": + num = 25 + r_min = l21_r_min + r_max = l21_r_max + r_derict = l21_r_derict + hand_range = [0] * num + for i in range(num): + if hand_joint == "L20": + if 11 <= i <= 14: continue + if hand_joint == "L21": + if 11 <= i <= 14: continue + if 16 <= i <= 19: continue + val_r = is_within_range(right_arc[i], r_min[i], r_max[i]) + if r_derict[i] == -1: + hand_range[i] = scale_value(val_r, r_min[i], r_max[i], 255, 0) + else: + hand_range[i] = scale_value(val_r, r_min[i], r_max[i], 0, 255) + return hand_range + + + + +def range_to_arc_right_l20(hand_range_r): + hand_arc_r = [0] * 20 + for i in range(20): + if 11 <= i <= 14: continue + val_r = is_within_range(hand_range_r[i], 0, 255) + if l20_r_derict[i] == -1: + hand_arc_r[i] = scale_value(val_r, 0, 255, l20_r_max[i], l20_r_min[i]) + else: + hand_arc_r[i] = scale_value(val_r, 0, 255, l20_r_min[i], l20_r_max[i]) + return hand_arc_r + + +def range_to_arc_left_l20(hand_range_l): + hand_arc_l = [0] * 20 + for i in range(20): + if 11 <= i <= 14: continue + val_l = is_within_range(hand_range_l[i], 0, 255) + if l20_l_derict[i] == -1: + hand_arc_l[i] = scale_value(val_l, 0, 255, l20_l_max[i], l20_l_min[i]) + else: + hand_arc_l[i] = scale_value(val_l, 0, 255, l20_l_min[i], l20_l_max[i]) + return hand_arc_l + + +def arc_to_range_right_l20(hand_arc_r): + hand_range_r = [0] * 20 + for i in range(20): + if 11 <= i <= 14: continue + val_r = is_within_range(hand_arc_r[i], l20_r_min[i], l20_r_max[i]) + if l20_r_derict[i] == -1: + hand_range_r[i] = scale_value(val_r, l20_r_min[i], l20_r_max[i], 255, 0) + else: + hand_range_r[i] = scale_value(val_r, l20_r_min[i], l20_r_max[i], 0, 255) + return hand_range_r + + +def arc_to_range_left_l20(hand_arc_l): + hand_range_l = [0] * 20 + for i in range(20): + if 11 <= i <= 14: continue + val_l = is_within_range(hand_arc_l[i], l20_l_min[i], l20_l_max[i]) + if l20_l_derict[i] == -1: + hand_range_l[i] = scale_value(val_l, l20_l_min[i], l20_l_max[i], 255, 0) + else: + hand_range_l[i] = scale_value(val_l, l20_l_min[i], l20_l_max[i], 0, 255) + + return hand_range_l + + +def range_to_arc_right_10(hand_range_r): + hand_arc_r = [0] * 10 + for i in range(10): + val_r = is_within_range(hand_range_r[i], 0, 255) + if l10_r_derict[i] == -1: + hand_arc_r[i] = scale_value(val_r, 0, 255, l10_r_max[i], l10_r_min[i]) + else: + hand_arc_r[i] = scale_value(val_r, 0, 255, l10_r_min[i], l10_r_max[i]) + + return hand_arc_r + + +def range_to_arc_left_10(hand_range_l): + hand_arc_l = [0] * 10 + for i in range(10): + val_l = is_within_range(hand_range_l[i], 0, 255) + if l10_l_derict[i] == -1: + hand_arc_l[i] = scale_value(val_l, 0, 255, l10_l_max[i], l10_l_min[i]) + else: + hand_arc_l[i] = scale_value(val_l, 0, 255, l10_l_min[i], l10_l_max[i]) + return hand_arc_l + + +def arc_to_range_right_10(hand_arc_r): + hand_range_r = [0] * 10 + for i in range(10): + val_r = is_within_range(hand_arc_r[i], l10_r_min[i], l10_r_max[i]) + if l10_r_derict[i] == -1: + hand_range_r[i] = scale_value(val_r, l10_r_min[i], l10_r_max[i], 255, 0) + else: + hand_range_r[i] = scale_value(val_r, l10_r_min[i], l10_r_max[i], 0, 255) + return hand_range_r + + +def arc_to_range_left_10(hand_arc_l): + hand_range_l = [0] * 10 + for i in range(10): + val_l = is_within_range(hand_arc_l[i], l10_l_min[i], l10_l_max[i]) + if l10_l_derict[i] == -1: + hand_range_l[i] = scale_value(val_l, l10_l_min[i], l10_l_max[i], 255, 0) + else: + hand_range_l[i] = scale_value(val_l, l10_l_min[i], l10_l_max[i], 0, 255) + + return hand_range_l + + +def scale_value(original_value, a_min, a_max, b_min, b_max): + return (original_value - a_min) * (b_max - b_min) / (a_max - a_min) + b_min + + +def is_within_range(value, min_value, max_value): + return min(max_value, max(min_value, value)) diff --git a/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/utils/open_can.py b/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/utils/open_can.py new file mode 100644 index 0000000..96aa932 --- /dev/null +++ b/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/utils/open_can.py @@ -0,0 +1,145 @@ +#!/usr/bin/env python3 +# -*- coding: utf-8 -*- +''' +Author: HJX +Date: 2025-04-01 14:09:21 +LastEditors: Please set LastEditors +LastEditTime: 2025-04-11 09:15:31 +FilePath: /Linker_Hand_SDK_ROS/src/linker_hand_sdk_ros/scripts/LinkerHand/utils/open_can.py +Description: +symbol_custom_string_obkorol_copyright: +''' +import sys,os,time,subprocess +sys.path.append(os.path.dirname(os.path.abspath(__file__))) +from color_msg import ColorMsg +from load_write_yaml import LoadWriteYaml +# from ament_index_python.packages import get_package_share_directory +import os + + +class OpenCan: + def __init__(self,load_yaml=None): + self.yaml = LoadWriteYaml() + self.password = self.yaml.load_setting_yaml()["PASSWORD"] + + def open_can0(self): + try: + # 检查 can0 接口是否已存在并处于 up 状态 + result = subprocess.run( + ["ip", "link", "show", "can0"], + check=True, + text=True, + capture_output=True + ) + if "state UP" in result.stdout: + return + # 如果没有处于 UP 状态，则配置接口 + subprocess.run( + ["sudo", "-S", "ip", "link", "set", "can0", "up", "type", "can", "bitrate", "1000000"], + input=f"{self.password}\n", + check=True, + text=True, + capture_output=True + ) + + except subprocess.CalledProcessError as e: + pass + except Exception as e: + pass + def open_can(self,can="can0"): + try: + # 检查 can0 接口是否已存在并处于 up 状态 + result = subprocess.run( + ["ip", "link", "show", can], + check=True, + text=True, + capture_output=True + ) + if "state UP" in result.stdout: + return + # 如果没有处于 UP 状态，则配置接口 + subprocess.run( + ["sudo", "-S", "ip", "link", "set", can, "up", "type", "can", "bitrate", "1000000"], + input=f"{self.password}\n", + check=True, + text=True, + capture_output=True + ) + except subprocess.CalledProcessError as e: + pass + except Exception as e: + pass + + + def is_can_up_sysfs(self, interface="can0"): + # 检查接口目录是否存在 + if not os.path.exists(f"/sys/class/net/{interface}"): + return False + # 读取接口状态 + try: + with open(f"/sys/class/net/{interface}/operstate", "r") as f: + state = f.read().strip() + if state == "up": + return True + except Exception as e: + print(f"Error reading CAN interface state: {e}") + return False + + def close_can0(self): + try: + # 检查 can0 接口是否存在 + result = subprocess.run( + ["ip", "link", "show", "can0"], + check=True, + text=True, + capture_output=True + ) + + # 如果接口存在且处于 UP 状态，则关闭它 + if "state UP" in result.stdout: + subprocess.run( + ["sudo", "-S", "ip", "link", "set", "can0", "down"], + input=f"{self.password}\n", + check=True, + text=True, + capture_output=True + ) + return True + return False + + except subprocess.CalledProcessError as e: + print(f"Error closing CAN interface: {e}") + return False + except Exception as e: + print(f"Unexpected error: {e}") + return False + + def close_can(self,can="can0"): + try: + # 检查 can0 接口是否存在 + result = subprocess.run( + ["ip", "link", "show", can], + check=True, + text=True, + capture_output=True + ) + + # 如果接口存在且处于 UP 状态，则关闭它 + if "state UP" in result.stdout: + subprocess.run( + ["sudo", "-S", "ip", "link", "set", can, "down"], + input=f"{self.password}\n", + check=True, + text=True, + capture_output=True + ) + return True + return False + + except subprocess.CalledProcessError as e: + print(f"Error closing CAN interface: {e}") + return False + except Exception as e: + print(f"Unexpected error: {e}") + return False + \ No newline at end of file diff --git a/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/__init__.py b/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/linker_hand.py b/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/linker_hand.py new file mode 100644 index 0000000..45d729e --- /dev/null +++ b/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/linker_hand.py @@ -0,0 +1,421 @@ +#!/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.modbus != "None": + 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 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 and self.modbus == "None": + 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.modbus == "None" and self.touch_pub.get_subscription_count() > 0: + """单点式压力传感器""" + self.force = self.api.get_force() + if self.is_touch == True and (self.touch_type > 1 or self.modbus != "None") 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.api.clear_faults() # 自动清除错误编码 + 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.modbus == "None" 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 or self.modbus != "None") 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("程序已退出。") 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 new file mode 100644 index 0000000..81c31a9 --- /dev/null +++ b/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/linker_hand.py.bak @@ -0,0 +1,414 @@ +#!/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("程序已退出。") diff --git a/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/linker_hand_advanced_g20.py b/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/linker_hand_advanced_g20.py new file mode 100644 index 0000000..2bf1f83 --- /dev/null +++ b/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/linker_hand_advanced_g20.py @@ -0,0 +1,258 @@ +#!/usr/bin/env python3 +# -*- coding: utf-8 -*- + +from re import A +import rclpy,sys # ROS2 Python接口库 +import time +import argparse +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 + +# Linker Hand 型号 +HAND_JOINT = "G20" +# 默认手指关节位置 +DEFAULT_POSITION = [255, 255, 255, 255, 255, 255, 193, 148, 105, 42, 245, 255, 255, 255, 255, 255, 255, 255, 255, 255] +# 默认手指关节速度 +DEFAULT_SPEED=[255, 255, 255, 255, 255] +# 默认手指关节力矩 +DEFAULT_TORQUE = [255, 255, 255, 255, 255] +# 压感传感器延迟时间 +TOUCH_SLEEP_TIME = 0.003 + + +class LinkerHandAdvancedG20(Node): + def __init__(self, name, hand_type, can, is_touch): + super().__init__(name) + self.hand_type = hand_type + self.hand_joint = HAND_JOINT + if is_touch == "true": + self.is_touch = True + else: + self.is_touch = False + self.can = can + self.modbus = "None" + time.sleep(0.1) + self._check_linker_hand_type() + self.last_hand_post_cmd = None # 最新手指位置命令 + self.last_hand_vel_cmd = None # 最新手指速度命令 + self.last_hand_eff_cmd = None # 最新手指力矩命令 + 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.hz = 1.0/60.0 + # ros时间获取 + self.stamp_clock = Clock() + self._init_hand() + time.sleep(2) + self.count = 0 + self.timer = self.create_timer(self.hz, self.run) # 100 Hz + + + def _check_linker_hand_type(self): + if self.modbus != "None": + ColorMsg(msg=f"Modbus暂不支持", color="red") + sys.exit(0) + if self.hand_joint.upper() != "G20": + ColorMsg(msg=f"Linker Hand hand_joint参数错误", color="red") + sys.exit(0) + + 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) + 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() + self.api.set_speed(speed=DEFAULT_SPEED) + time.sleep(0.1) + self.api.set_torque(torque=DEFAULT_TORQUE) + time.sleep(0.1) + self.api.finger_move(pose=DEFAULT_POSITION) + time.sleep(0.1) + + 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 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 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 run(self): + # 执行手控制指令 + if self.last_hand_post_cmd != None: + self.api.finger_move(pose=self.last_hand_post_cmd) + self.last_hand_post_cmd = None + # 优先获取手指状态并且发布 + self.last_hand_state = self.api.get_state() + self.last_hand_vel = [0.0] * len(self.last_hand_state) + # 发布手状态 + msg_state = self.joint_state_msg(self.last_hand_state, self.last_hand_vel) + self.hand_state_pub.publish(msg_state) + + if self.is_touch == True: + # 获取压感数据 + if self.count == 2: + self.matrix_dic["thumb_matrix"] = self.api.get_thumb_matrix_touch(sleep_time=TOUCH_SLEEP_TIME).tolist() + if self.count == 4: + self.matrix_dic["index_matrix"] = self.api.get_index_matrix_touch(sleep_time=TOUCH_SLEEP_TIME).tolist() + if self.count == 6: + self.matrix_dic["middle_matrix"] = self.api.get_middle_matrix_touch(sleep_time=TOUCH_SLEEP_TIME).tolist() + if self.count == 8: + self.matrix_dic["ring_matrix"] = self.api.get_ring_matrix_touch(sleep_time=TOUCH_SLEEP_TIME).tolist() + if self.count == 10: + self.matrix_dic["little_matrix"] = self.api.get_little_matrix_touch(sleep_time=TOUCH_SLEEP_TIME).tolist() + # 发布矩阵压感数据JSON格式 + self.pub_matrix_dic() + # 发布矩阵压感和值JSON格式 + self.pub_matrix_mass(dic=self.matrix_dic) + # 发布矩阵压感点云格式 + self.pub_matrix_point_cloud() + self.count += 1 + if self.count == 11: + self.count = 0 + + 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 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 个数 or 5 帧，每帧 4×10=40 个数 列x行 + # 摊平到一维 + flat_list = [v for frame in all_matrices for v in frame] + 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.get_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 close_can(self): + self.api.open_can.close_can(can=self.can) + sys.exit(0) + + +def main(args=None): + ''' + 本节点用于收集手指状态和压感数据。 + '/cb_{self.hand_type}_hand_control_cmd' 话题类型为 sensor_msgs/msg/JointState 控制话题，限制 30Hz + /cb_{self.hand_type}_hand_state 话题类型为 sensor_msgs/msg/JointState 30Hz + '/cb_{self.hand_type}_hand_matrix_touch' 话题类型为 std_msgs/msg/String 30Hz + 启动命令: + ros2 run linker_hand_ros2_sdk linker_hand_advanced_g20 --hand_type left --can can0 --is_touch true + ''' + try: + rclpy.init(args=args) + parser = argparse.ArgumentParser() + parser.add_argument('--hand_type', required=True) + parser.add_argument('--can', required=True) + parser.add_argument('--is_touch', choices=['true','false'], required=True) + + args = parser.parse_args() + node = LinkerHandAdvancedG20(name="linker_hand_advanced_g20",hand_type=args.hand_type,can=args.can,is_touch=args.is_touch) + embedded_version = node.embedded_version + rclpy.spin(node) # 主循环，监听 ROS 回调 + except KeyboardInterrupt: + print("收到 Ctrl+C，准备退出...") + finally: + # node.close_can() # 关闭 CAN 或其他硬件资源 + # node.destroy_node() # 销毁 ROS 节点 + # rclpy.shutdown() # 关闭 ROS + print("程序已退出。") \ No newline at end of file diff --git a/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/linker_hand_advanced_l10.py b/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/linker_hand_advanced_l10.py new file mode 100644 index 0000000..37a7be3 --- /dev/null +++ b/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/linker_hand_advanced_l10.py @@ -0,0 +1,246 @@ +#!/usr/bin/env python3 +# -*- coding: utf-8 -*- + +from re import A +import rclpy,sys # ROS2 Python接口库 +import time +import argparse +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 LinkerHandAdvancedL10(Node): + def __init__(self, name, hand_type, can, is_touch): + super().__init__(name) + self.hand_type = hand_type + self.hand_joint = "L10" + if is_touch == "true": + self.is_touch = True + else: + self.is_touch = False + self.can = can + self.modbus = "None" + time.sleep(0.1) + self._check_linker_hand_type() + self.last_hand_post_cmd = None # 最新手指位置命令 + self.last_hand_vel_cmd = None # 最新手指速度命令 + self.last_hand_eff_cmd = None # 最新手指力矩命令 + self.count = 0 # 循环计数器 + 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.hz = 1.0/60.0 + # ros时间获取 + self.stamp_clock = Clock() + self._init_hand() + time.sleep(2) + self.timer = self.create_timer(self.hz, self.run) # 100 Hz + + + def _check_linker_hand_type(self): + if self.modbus != "None": + ColorMsg(msg=f"Modbus暂不支持", color="red") + sys.exit(0) + if self.hand_joint.upper() != "L10": + ColorMsg(msg=f"L10以外其他Linker Hand暂不支持", color="red") + sys.exit(0) + + + 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) + 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() + if self.hand_joint.upper() == "L10": + pose = [255, 200, 255, 255, 255, 255, 180, 180, 180, 41] + torque = [255, 255, 255, 255, 255, 255, 255, 255, 255, 255] + speed = [255, 255, 255, 255, 255, 255, 255, 255, 255, 255] + 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) + self.serial_number = self.api.get_serial_number() + + 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 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 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 run(self): + # 执行手控制指令 + if self.last_hand_post_cmd != None: + self.api.finger_move(pose=self.last_hand_post_cmd) + self.last_hand_post_cmd = None + # 优先获取手指状态并且发布 + self.last_hand_state = self.api.get_state() + self.last_hand_vel = [0.0] * len(self.last_hand_state) + # 发布手状态 + msg_state = self.joint_state_msg(self.last_hand_state) + self.hand_state_pub.publish(msg_state) + # 获取压感数据 + if self.is_touch == True: + self.matrix_dic["thumb_matrix"] = self.api.get_thumb_matrix_touch(sleep_time=0.003).tolist() + self.matrix_dic["index_matrix"] = self.api.get_index_matrix_touch(sleep_time=0.004).tolist() + self.matrix_dic["middle_matrix"] = self.api.get_middle_matrix_touch(sleep_time=0.003).tolist() + self.matrix_dic["ring_matrix"] = self.api.get_ring_matrix_touch(sleep_time=0.004).tolist() + self.matrix_dic["little_matrix"] = self.api.get_little_matrix_touch(sleep_time=0.004).tolist() + # 发布矩阵压感数据JSON格式 + self.pub_matrix_dic() + # 发布矩阵压感和值JSON格式 + self.pub_matrix_mass(dic=self.matrix_dic) + # 发布矩阵压感点云格式 + self.pub_matrix_point_cloud() + + + 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 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 个数 or 5 帧，每帧 4×10=40 个数 列x行 + # 摊平到一维 + flat_list = [v for frame in all_matrices for v in frame] + 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.get_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 close_can(self): + self.api.open_can.close_can(can=self.can) + sys.exit(0) + + +def main(args=None): + ''' + 本节点用于收集手指状态和压感数据。 + '/cb_{self.hand_type}_hand_control_cmd' 话题类型为 sensor_msgs/msg/JointState 控制话题，限制 30Hz + /cb_{self.hand_type}_hand_state 话题类型为 sensor_msgs/msg/JointState 30Hz + '/cb_{self.hand_type}_hand_matrix_touch' 话题类型为 std_msgs/msg/String 30Hz + '/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) + 启动命令: + ros2 run linker_hand_ros2_sdk linker_hand_advanced_l10 --hand_type left --can can0 --is_touch true + ''' + try: + rclpy.init(args=args) + parser = argparse.ArgumentParser() + parser.add_argument('--hand_type', required=True) + parser.add_argument('--can', required=True) + parser.add_argument('--is_touch', choices=['true','false'], required=True) + + args = parser.parse_args() + node = LinkerHandAdvancedL10(name="linker_hand_advanced_l10",hand_type=args.hand_type,can=args.can,is_touch=args.is_touch) + embedded_version = node.embedded_version + rclpy.spin(node) # 主循环，监听 ROS 回调 + except KeyboardInterrupt: + print("收到 Ctrl+C，准备退出...") + finally: + # node.close_can() # 关闭 CAN 或其他硬件资源 + # node.destroy_node() # 销毁 ROS 节点 + # rclpy.shutdown() # 关闭 ROS + print("程序已退出。") \ No newline at end of file diff --git a/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/linker_hand_advanced_l6.py b/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/linker_hand_advanced_l6.py new file mode 100644 index 0000000..2672283 --- /dev/null +++ b/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/linker_hand_advanced_l6.py @@ -0,0 +1,251 @@ +#!/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 +import argparse +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 LinkerHandAdvancedL6(Node): + def __init__(self, name, hand_type, can, is_touch): + super().__init__(name) + self.hand_type = hand_type + self.hand_joint = "L6" + if is_touch == "true": + self.is_touch = True + else: + self.is_touch = False + self.can = can + self.modbus = "None" + time.sleep(0.1) + self._check_linker_hand_type() + self.last_hand_post_cmd = None # 最新手指位置命令 + self.last_hand_vel_cmd = None # 最新手指速度命令 + self.last_hand_eff_cmd = None # 最新手指力矩命令 + self.count = 0 # 循环计数器 + 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.hz = 1.0/60.0 + # ros时间获取 + self.stamp_clock = Clock() + self._init_hand() + time.sleep(2) + self.timer = self.create_timer(self.hz, self.run) # 60 Hz + + + def _check_linker_hand_type(self): + if self.modbus != "None": + ColorMsg(msg=f"Modbus暂不支持", color="red") + sys.exit(0) + if self.hand_joint.upper() != "L6": + ColorMsg(msg=f"L6以外其他Linker Hand暂不支持", color="red") + sys.exit(0) + + 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) + 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() + if self.hand_joint.upper() == "O6" or self.hand_joint.upper() == "L6": + pose = [200, 255, 255, 255, 255, 180] + torque = [255, 255, 255, 255, 255, 255] + # O6 最大速度阈值 + speed = [255, 255, 255, 255, 255, 255] + 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 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 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 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 run(self): + # 执行手控制指令 + if self.last_hand_post_cmd != None: + self.api.finger_move(pose=self.last_hand_post_cmd) + self.last_hand_post_cmd = None + #time.sleep(0.002) + # 优先获取手指状态并且发布 + self.last_hand_state = self.api.get_state() + self.last_hand_vel = self.api.get_joint_speed() + # 发布手状态 + msg_state = self.joint_state_msg(self.last_hand_state, self.last_hand_vel) + self.hand_state_pub.publish(msg_state) + time.sleep(0.002) + # 获取压感数据 + if self.is_touch == True: + self.matrix_dic["thumb_matrix"] = self.api.get_thumb_matrix_touch(sleep_time=0.003).tolist() + self.matrix_dic["index_matrix"] = self.api.get_index_matrix_touch(sleep_time=0.003).tolist() + self.matrix_dic["middle_matrix"] = self.api.get_middle_matrix_touch(sleep_time=0.003).tolist() + self.matrix_dic["ring_matrix"] = self.api.get_ring_matrix_touch(sleep_time=0.003).tolist() + self.matrix_dic["little_matrix"] = self.api.get_little_matrix_touch(sleep_time=0.003).tolist() + # 发布矩阵压感数据JSON格式 + self.pub_matrix_dic() + # 发布矩阵压感和值JSON格式 + self.pub_matrix_mass(dic=self.matrix_dic) + # 发布矩阵压感点云格式 + self.pub_matrix_point_cloud() + + 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 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 个数 or 5 帧，每帧 4×10=40 个数 列x行 + # 摊平到一维 + flat_list = [v for frame in all_matrices for v in frame] + 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.get_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 close_can(self): + self.api.open_can.close_can(can=self.can) + sys.exit(0) + + +def main(args=None): + ''' + 本节点用于收集手指状态和压感数据。 + '/cb_{self.hand_type}_hand_control_cmd' 话题类型为 sensor_msgs/msg/JointState 控制话题，限制 30Hz + /cb_{self.hand_type}_hand_state 话题类型为 sensor_msgs/msg/JointState 40Hz + '/cb_{self.hand_type}_hand_matrix_touch' 话题类型为 std_msgs/msg/String 40Hz + 启动命令: + ros2 run linker_hand_ros2_sdk linker_hand_advanced_l6 --hand_type left --can can0 --is_touch true + ''' + try: + rclpy.init(args=args) + parser = argparse.ArgumentParser() + parser.add_argument('--hand_type', required=True) + parser.add_argument('--can', required=True) + parser.add_argument('--is_touch', choices=['true','false'], required=True) + + args = parser.parse_args() + node = LinkerHandAdvancedL6(name="linker_hand_advanced_l6",hand_type=args.hand_type,can=args.can,is_touch=args.is_touch) + embedded_version = node.embedded_version + if embedded_version[2] < 8 and len(embedded_version) != 3: + ColorMsg(msg=f"固件版本过低，请升级固件到V{embedded_version[0]}.{embedded_version[1]}.8及以上版本", color="red") + sys.exit(0) + rclpy.spin(node) # 主循环，监听 ROS 回调 + except KeyboardInterrupt: + print("收到 Ctrl+C，准备退出...") + finally: + # node.close_can() # 关闭 CAN 或其他硬件资源 + # node.destroy_node() # 销毁 ROS 节点 + # rclpy.shutdown() # 关闭 ROS + print("程序已退出。") diff --git a/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/linker_hand_advanced_l7.py b/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/linker_hand_advanced_l7.py new file mode 100644 index 0000000..7ed58bd --- /dev/null +++ b/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/linker_hand_advanced_l7.py @@ -0,0 +1,281 @@ +#!/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 argparse +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 LinkerHandAdvancedL7(Node): + def __init__(self, name, hand_type, can, is_touch): + super().__init__(name) + self.hand_type = hand_type + self.hand_joint = "L7" + if is_touch == "true": + self.is_touch = True + else: + self.is_touch = False + self.can = can + self.modbus = "None" + time.sleep(0.1) + self._check_linker_hand_type() + self.last_hand_post_cmd = None # 最新手指位置命令 + self.last_hand_vel_cmd = None # 最新手指速度命令 + self.last_hand_eff_cmd = None # 最新手指力矩命令 + self.count = 0 # 循环计数器 + 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.hz = 1.0/60.0 + # ros时间获取 + self.stamp_clock = Clock() + self._init_hand() + time.sleep(2) + self.timer = self.create_timer(self.hz, self.run) # 100 Hz + + + def _check_linker_hand_type(self): + if self.modbus != "None": + ColorMsg(msg=f"Modbus暂不支持", color="red") + sys.exit(0) + if self.hand_joint.upper() != "L7": + ColorMsg(msg=f"L6以外其他Linker Hand暂不支持", color="red") + sys.exit(0) + + + 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) + 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() + if self.hand_joint.upper() == "L7": + pose = [255, 200, 255, 255, 255, 255, 180] + torque = [255] * 7 + speed = [255] * 7 + 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) + self.serial_number = self.api.get_serial_number() + + 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 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 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 run(self): + # 优先获取手指状态并且发布 + self.last_hand_state = self.api.get_state() + self.last_hand_vel = self.api.get_joint_speed() + # 发布手状态 + msg_state = self.joint_state_msg(self.last_hand_state, self.last_hand_vel) + self.hand_state_pub.publish(msg_state) + # 执行手控制指令 + if self.last_hand_post_cmd != None: + self.api.finger_move(pose=self.last_hand_post_cmd) + time.sleep(0.003) + 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.005) + # 获取压感数据 + if self.is_touch == True: + if self.count == 3: + self.matrix_dic["thumb_matrix"] = self.api.get_thumb_matrix_touch(sleep_time=0.006).tolist() + if self.count == 4: + self.matrix_dic["index_matrix"] = self.api.get_index_matrix_touch(sleep_time=0.006).tolist() + if self.count == 5: + self.matrix_dic["middle_matrix"] = self.api.get_middle_matrix_touch(sleep_time=0.006).tolist() + if self.count == 6: + self.matrix_dic["ring_matrix"] = self.api.get_ring_matrix_touch(sleep_time=0.006).tolist() + if self.count == 7: + self.matrix_dic["little_matrix"] = self.api.get_little_matrix_touch(sleep_time=0.006).tolist() + # 发布矩阵压感数据JSON格式 + self.pub_matrix_dic() + # 发布矩阵压感和值JSON格式 + self.pub_matrix_mass(dic=self.matrix_dic) + # 发布矩阵压感点云格式 + self.pub_matrix_point_cloud() + self.count += 1 + if self.count == 8: + self.count = 0 + time.sleep(0.006) + + 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 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 个数 or 5 帧，每帧 4×10=40 个数 列x行 + # 摊平到一维 + flat_list = [v for frame in all_matrices for v in frame] + 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.get_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 close_can(self): + self.api.open_can.close_can(can=self.can) + sys.exit(0) + + +def main(args=None): + ''' + 本节点用于收集手指状态和压感数据。 + '/cb_{self.hand_type}_hand_control_cmd' 话题类型为 sensor_msgs/msg/JointState 控制话题，限制 30Hz + /cb_{self.hand_type}_hand_state 话题类型为 sensor_msgs/msg/JointState 40Hz + '/cb_{self.hand_type}_hand_matrix_touch' 话题类型为 std_msgs/msg/String 40Hz + 启动命令: + ros2 run linker_hand_ros2_sdk linker_hand_advanced_l7 --hand_type left --can can0 --is_touch true + ''' + try: + rclpy.init(args=args) + parser = argparse.ArgumentParser() + parser.add_argument('--hand_type', required=True) + parser.add_argument('--can', required=True) + parser.add_argument('--is_touch', choices=['true','false'], required=True) + + args = parser.parse_args() + node = LinkerHandAdvancedL7(name="linker_hand_collect_l7",hand_type=args.hand_type,can=args.can,is_touch=args.is_touch) + embedded_version = node.embedded_version + rclpy.spin(node) # 主循环，监听 ROS 回调 + except KeyboardInterrupt: + print("收到 Ctrl+C，准备退出...") + finally: + # node.close_can() # 关闭 CAN 或其他硬件资源 + # node.destroy_node() # 销毁 ROS 节点 + # rclpy.shutdown() # 关闭 ROS + print("程序已退出。") diff --git a/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/linker_hand_advanced_o6.py b/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/linker_hand_advanced_o6.py new file mode 100644 index 0000000..42638c8 --- /dev/null +++ b/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/linker_hand_advanced_o6.py @@ -0,0 +1,252 @@ +#!/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 argparse +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 LinkerHandAdvancedO6(Node): + def __init__(self, name, hand_type, can, is_touch): + super().__init__(name) + self.hand_type = hand_type + self.hand_joint = "O6" + if is_touch == "true": + self.is_touch = True + else: + self.is_touch = False + self.can = can + self.modbus = "None" + time.sleep(0.1) + self._check_linker_hand_type() + self.last_hand_post_cmd = None # 最新手指位置命令 + self.last_hand_vel_cmd = None # 最新手指速度命令 + self.last_hand_eff_cmd = None # 最新手指力矩命令 + 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.hz = 1.0/60.0 + # ros时间获取 + self.stamp_clock = Clock() + self._init_hand() + time.sleep(2) + self.timer = self.create_timer(self.hz, self.run) # 100 Hz + + + def _check_linker_hand_type(self): + if self.modbus != "None": + ColorMsg(msg=f"Modbus暂不支持", color="red") + sys.exit(0) + if self.hand_joint.upper() != "O6": + ColorMsg(msg=f"O6以外其他Linker Hand暂不支持", color="red") + sys.exit(0) + + 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) + 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() + if self.hand_joint.upper() == "O6" or self.hand_joint.upper() == "L6": + pose = [200, 255, 255, 255, 255, 180] + torque = [255, 255, 255, 255, 255, 255] + # O6 最大速度阈值 + speed = [255, 255, 255, 255, 255, 255] + 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 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 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 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 run(self): + # 执行手控制指令 + 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: + speed = vel + self.api.set_joint_speed(speed=speed) + self.last_hand_vel_cmd = None + # 优先获取手指状态并且发布 + self.last_hand_state = self.api.get_state() + self.last_hand_vel = self.api.get_joint_speed() + # 发布手状态 + msg_state = self.joint_state_msg(self.last_hand_state, self.last_hand_vel) + self.hand_state_pub.publish(msg_state) + if self.is_touch == True: + # 获取压感数据 + self.matrix_dic["thumb_matrix"] = self.api.get_thumb_matrix_touch(sleep_time=0.002).tolist() + self.matrix_dic["index_matrix"] = self.api.get_index_matrix_touch(sleep_time=0.002).tolist() + self.matrix_dic["middle_matrix"] = self.api.get_middle_matrix_touch(sleep_time=0.002).tolist() + self.matrix_dic["ring_matrix"] = self.api.get_ring_matrix_touch(sleep_time=0.002).tolist() + self.matrix_dic["little_matrix"] = self.api.get_little_matrix_touch(sleep_time=0.002).tolist() + # 发布矩阵压感数据JSON格式 + self.pub_matrix_dic() + # 发布矩阵压感和值JSON格式 + self.pub_matrix_mass(dic=self.matrix_dic) + # 发布矩阵压感点云格式 + self.pub_matrix_point_cloud() + + 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 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 个数 or 5 帧，每帧 4×10=40 个数 列x行 + # 摊平到一维 + flat_list = [v for frame in all_matrices for v in frame] + 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.get_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 close_can(self): + self.api.open_can.close_can(can=self.can) + sys.exit(0) + + +def main(args=None): + ''' + 本节点用于收集手指状态和压感数据。 + '/cb_{self.hand_type}_hand_control_cmd' 话题类型为 sensor_msgs/msg/JointState 控制话题，限制 30Hz + /cb_{self.hand_type}_hand_state 话题类型为 sensor_msgs/msg/JointState 50Hz + '/cb_{self.hand_type}_hand_matrix_touch' 话题类型为 std_msgs/msg/String 50Hz + 启动命令: + ros2 run linker_hand_ros2_sdk linker_hand_advanced_o6 --hand_type right --can can0 --is_touch true + ''' + try: + rclpy.init(args=args) + parser = argparse.ArgumentParser() + parser.add_argument('--hand_type', required=True) + parser.add_argument('--can', required=True) + parser.add_argument('--is_touch', choices=['true','false'], required=True) + + args = parser.parse_args() + node = LinkerHandAdvancedO6(name="linker_hand_advanced_o6",hand_type=args.hand_type,can=args.can,is_touch=args.is_touch) + embedded_version = node.embedded_version + rclpy.spin(node) # 主循环，监听 ROS 回调 + except KeyboardInterrupt: + print("收到 Ctrl+C，准备退出...") + finally: + # node.close_can() # 关闭 CAN 或其他硬件资源 + # node.destroy_node() # 销毁 ROS 节点 + # rclpy.shutdown() # 关闭 ROS + print("程序已退出。") \ No newline at end of file diff --git a/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/linker_hand_g20_palm_touch.py b/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/linker_hand_g20_palm_touch.py new file mode 100644 index 0000000..eda447c --- /dev/null +++ b/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/linker_hand_g20_palm_touch.py @@ -0,0 +1,268 @@ +#!/usr/bin/env python3 +# -*- coding: utf-8 -*- + +from re import A +import rclpy,sys # ROS2 Python接口库 +import time +import argparse +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 + +# Linker Hand 型号 +HAND_JOINT = "G20" +# 默认手指关节位置 +DEFAULT_POSITION = [255, 255, 255, 255, 255, 255, 193, 148, 105, 42, 245, 255, 255, 255, 255, 255, 255, 255, 255, 255] +# 默认手指关节速度 +DEFAULT_SPEED=[255, 255, 255, 255, 255] +# 默认手指关节力矩 +DEFAULT_TORQUE = [255, 255, 255, 255, 255] +# 压感传感器延迟时间 +TOUCH_SLEEP_TIME = 0.003 + + + +class LinkerHandAdvancedG20(Node): + def __init__(self, name, hand_type, can, is_touch): + super().__init__(name) + self.hand_type = hand_type + self.hand_joint = HAND_JOINT + if is_touch == "true": + self.is_touch = True + else: + self.is_touch = False + self.can = can + self.modbus = "None" + time.sleep(0.1) + self._check_linker_hand_type() + self.last_hand_post_cmd = None # 最新手指位置命令 + self.last_hand_vel_cmd = None # 最新手指速度命令 + self.last_hand_eff_cmd = None # 最新手指力矩命令 + 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.hz = 1.0/60.0 + # ros时间获取 + self.stamp_clock = Clock() + self._init_hand() + time.sleep(2) + self.count = 0 + self.timer = self.create_timer(self.hz, self.run) + + + def _check_linker_hand_type(self): + if self.modbus != "None": + ColorMsg(msg=f"Modbus暂不支持", color="red") + sys.exit(0) + if self.hand_joint.upper() != "G20": + ColorMsg(msg=f"Linker Hand hand_joint参数错误", color="red") + sys.exit(0) + + 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) + 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() + self.api.set_speed(speed=DEFAULT_SPEED) + time.sleep(0.1) + self.api.set_torque(torque=DEFAULT_TORQUE) + time.sleep(0.1) + self.api.finger_move(pose=DEFAULT_POSITION) + time.sleep(0.1) + self.palm_touch = self.api.is_palm_touch + if self.palm_touch == 5: + self.touch_sleep_time = 0.03 + ColorMsg(msg=f"{self.hand_type} {self.hand_joint} 全掌压感版", color="green") + else: + self.touch_sleep_time = 0.003 + + 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 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 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 run(self): + # 执行手控制指令 + if self.last_hand_post_cmd != None: + self.api.finger_move(pose=self.last_hand_post_cmd) + self.last_hand_post_cmd = None + # 优先获取手指状态并且发布 + self.last_hand_state = self.api.get_state() + self.last_hand_vel = [0.0] * len(self.last_hand_state) + # 发布手状态 + msg_state = self.joint_state_msg(self.last_hand_state, self.last_hand_vel) + self.hand_state_pub.publish(msg_state) + + if self.is_touch == True: + # 获取压感数据 + if self.count == 2: + self.matrix_dic["thumb_matrix"] = self.api.get_thumb_matrix_touch(sleep_time=self.touch_sleep_time).tolist() + if self.count == 4: + self.matrix_dic["index_matrix"] = self.api.get_index_matrix_touch(sleep_time=self.touch_sleep_time).tolist() + if self.count == 6: + self.matrix_dic["middle_matrix"] = self.api.get_middle_matrix_touch(sleep_time=self.touch_sleep_time).tolist() + if self.count == 8: + self.matrix_dic["ring_matrix"] = self.api.get_ring_matrix_touch(sleep_time=self.touch_sleep_time).tolist() + if self.count == 10: + self.matrix_dic["little_matrix"] = self.api.get_little_matrix_touch(sleep_time=self.touch_sleep_time).tolist() + if self.count == 14 and self.palm_touch == 5: + self.matrix_dic["palm_matrix"] = self.api.get_palm_matrix_touch(sleep_time=self.touch_sleep_time).tolist() + # 发布矩阵压感数据JSON格式 + self.pub_matrix_dic() + # 发布矩阵压感和值JSON格式 + self.pub_matrix_mass(dic=self.matrix_dic) + # 发布矩阵压感点云格式 + #self.pub_matrix_point_cloud() + self.count += 1 + if self.count == 15: + self.count = 0 + + 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 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"] = self.api.hand.thumb_matrix_palm_mass + self.matrix_mass_dic["index_mass"] = self.api.hand.index_matrix_palm_mass + self.matrix_mass_dic["middle_mass"] = self.api.hand.middle_matrix_palm_mass + self.matrix_mass_dic["ring_mass"] = self.api.hand.ring_matrix_palm_mass + self.matrix_mass_dic["little_mass"] = self.api.hand.little_matrix_palm_mass + self.matrix_mass_dic["palm_mass"] = self.api.hand.palm_matrix_palm_mass + 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 个数 or 5 帧，每帧 4×10=40 个数 列x行 + # # 摊平到一维 + # flat_list = [v for frame in all_matrices for v in frame] + # 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.get_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 close_can(self): + self.api.open_can.close_can(can=self.can) + sys.exit(0) + + +def main(args=None): + ''' + 本节点用于收集手指状态和压感数据。 + '/cb_{self.hand_type}_hand_control_cmd' 话题类型为 sensor_msgs/msg/JointState 控制话题，限制 30Hz + /cb_{self.hand_type}_hand_state 话题类型为 sensor_msgs/msg/JointState 30Hz + '/cb_{self.hand_type}_hand_matrix_touch' 话题类型为 std_msgs/msg/String 30Hz + 启动命令: + ros2 run linker_hand_ros2_sdk linker_hand_g20_palm_touch --hand_type left --can can0 --is_touch true + ''' + try: + rclpy.init(args=args) + parser = argparse.ArgumentParser() + parser.add_argument('--hand_type', required=True) + parser.add_argument('--can', required=True) + parser.add_argument('--is_touch', choices=['true','false'], required=True) + + args = parser.parse_args() + node = LinkerHandAdvancedG20(name="linker_hand_g20_palm_touch",hand_type=args.hand_type,can=args.can,is_touch=args.is_touch) + embedded_version = node.embedded_version + rclpy.spin(node) # 主循环，监听 ROS 回调 + except KeyboardInterrupt: + print("收到 Ctrl+C，准备退出...") + finally: + node.close_can() # 关闭 CAN 或其他硬件资源 + # node.destroy_node() # 销毁 ROS 节点 + # rclpy.shutdown() # 关闭 ROS + print("程序已退出。") \ No newline at end of file diff --git a/src/linker_hand_ros2_sdk/package.xml b/src/linker_hand_ros2_sdk/package.xml new file mode 100644 index 0000000..665b626 --- /dev/null +++ b/src/linker_hand_ros2_sdk/package.xml @@ -0,0 +1,21 @@ + + + + linker_hand_ros2_sdk + 0.0.0 + TODO: Package description + linker-robot + TODO: License declaration + + ament_copyright + ament_flake8 + ament_pep257 + python3-pytest + + rclpy + launch + + + ament_python + + diff --git a/src/linker_hand_ros2_sdk/pyproject.toml b/src/linker_hand_ros2_sdk/pyproject.toml new file mode 100644 index 0000000..638dd9c --- /dev/null +++ b/src/linker_hand_ros2_sdk/pyproject.toml @@ -0,0 +1,3 @@ +[build-system] +requires = ["setuptools>=61.0"] +build-backend = "setuptools.build_meta" diff --git a/src/linker_hand_ros2_sdk/resource/linker_hand_ros2_sdk b/src/linker_hand_ros2_sdk/resource/linker_hand_ros2_sdk new file mode 100644 index 0000000..e69de29 diff --git a/src/linker_hand_ros2_sdk/setup.cfg b/src/linker_hand_ros2_sdk/setup.cfg new file mode 100644 index 0000000..eb74b8f --- /dev/null +++ b/src/linker_hand_ros2_sdk/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/linker_hand_ros2_sdk +[install] +install_scripts=$base/lib/linker_hand_ros2_sdk diff --git a/src/linker_hand_ros2_sdk/setup.py b/src/linker_hand_ros2_sdk/setup.py new file mode 100644 index 0000000..2e4ed2e --- /dev/null +++ b/src/linker_hand_ros2_sdk/setup.py @@ -0,0 +1,50 @@ +#!/usr/bin/env python3 +# -*- coding: utf-8 -*- +import os +from glob import glob +from setuptools import find_packages, setup + +package_name = 'linker_hand_ros2_sdk' + +this_dir = os.path.abspath(os.path.dirname(__file__)) +custom_dir = os.path.join(this_dir, package_name, "LinkerHand") + +data_files = [ + ('share/ament_index/resource_index/packages', + ['resource/' + package_name]), + ('share/' + package_name, ['package.xml']), + (os.path.join('share', package_name, 'launch'), glob('launch/*.launch.py')), +] + +# for root, dirs, files in os.walk(custom_dir): +# if files: +# relative_path = os.path.relpath(root, os.path.join(this_dir, package_name)) +# target_path = os.path.join('share', package_name, relative_path) +# # 修复这里：路径必须是相对路径 +# files_full_path = [os.path.relpath(os.path.join(root, f), start=os.getcwd()) for f in files] +# data_files.append((target_path, files_full_path)) + + +setup( + name=package_name, + version='0.0.0', + packages=find_packages(include=[package_name, f"{package_name}.*"]), + data_files=data_files, + install_requires=['setuptools'], + zip_safe=True, + maintainer='linker-robot', + maintainer_email='linker-robot@todo.todo', + description='ROS2 SDK for Linker Hand', + license='TODO: License declaration', + entry_points={ + 'console_scripts': [ + 'linker_hand_sdk = linker_hand_ros2_sdk.linker_hand:main', + 'linker_hand_advanced_o6 = linker_hand_ros2_sdk.linker_hand_advanced_o6:main', + 'linker_hand_advanced_l6 = linker_hand_ros2_sdk.linker_hand_advanced_l6:main', + 'linker_hand_advanced_l7 = linker_hand_ros2_sdk.linker_hand_advanced_l7:main', + 'linker_hand_advanced_l10 = linker_hand_ros2_sdk.linker_hand_advanced_l10:main', + 'linker_hand_advanced_g20 = linker_hand_ros2_sdk.linker_hand_advanced_g20:main', + 'linker_hand_g20_palm_touch = linker_hand_ros2_sdk.linker_hand_g20_palm_touch:main', + ], + }, +) diff --git a/src/linker_hand_ros2_sdk/test/test_copyright.py b/src/linker_hand_ros2_sdk/test/test_copyright.py new file mode 100644 index 0000000..97a3919 --- /dev/null +++ b/src/linker_hand_ros2_sdk/test/test_copyright.py @@ -0,0 +1,25 @@ +# Copyright 2015 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from ament_copyright.main import main +import pytest + + +# Remove the `skip` decorator once the source file(s) have a copyright header +@pytest.mark.skip(reason='No copyright header has been placed in the generated source file.') +@pytest.mark.copyright +@pytest.mark.linter +def test_copyright(): + rc = main(argv=['.', 'test']) + assert rc == 0, 'Found errors' diff --git a/src/linker_hand_ros2_sdk/test/test_flake8.py b/src/linker_hand_ros2_sdk/test/test_flake8.py new file mode 100644 index 0000000..27ee107 --- /dev/null +++ b/src/linker_hand_ros2_sdk/test/test_flake8.py @@ -0,0 +1,25 @@ +# Copyright 2017 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from ament_flake8.main import main_with_errors +import pytest + + +@pytest.mark.flake8 +@pytest.mark.linter +def test_flake8(): + rc, errors = main_with_errors(argv=[]) + assert rc == 0, \ + 'Found %d code style errors / warnings:\n' % len(errors) + \ + '\n'.join(errors) diff --git a/src/linker_hand_ros2_sdk/test/test_pep257.py b/src/linker_hand_ros2_sdk/test/test_pep257.py new file mode 100644 index 0000000..b234a38 --- /dev/null +++ b/src/linker_hand_ros2_sdk/test/test_pep257.py @@ -0,0 +1,23 @@ +# Copyright 2015 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from ament_pep257.main import main +import pytest + + +@pytest.mark.linter +@pytest.mark.pep257 +def test_pep257(): + rc = main(argv=['.', 'test']) + assert rc == 0, 'Found code style errors / warnings' diff --git a/src/linkerhand_retarget/linkerhand_retarget/config/base_config.yml b/src/linkerhand_retarget/linkerhand_retarget/config/base_config.yml index 0f864e6..ed90974 100644 --- a/src/linkerhand_retarget/linkerhand_retarget/config/base_config.yml +++ b/src/linkerhand_retarget/linkerhand_retarget/config/base_config.yml @@ -27,7 +27,7 @@ serial: baudrate: 460800 port: /dev/ttyUSB1 right: - baudrate: 460800 + baudrate: 2000000 port: /dev/ttyUSB0 serial_debug: false system: diff --git a/src/linkerhand_retarget/linkerhand_retarget/motion/linkerforce/retarget.py b/src/linkerhand_retarget/linkerhand_retarget/motion/linkerforce/retarget.py index 8dac21c..4ae9a10 100644 --- a/src/linkerhand_retarget/linkerhand_retarget/motion/linkerforce/retarget.py +++ b/src/linkerhand_retarget/linkerhand_retarget/motion/linkerforce/retarget.py @@ -741,6 +741,20 @@ class Retarget(): is_stable = var_left < threshold and var_right < threshold return is_stable, max(var_left, var_right) + + def _is_left_connected(self): + return self.force_reader_left is not None and self.force_reader_left.handtype == 'Left' + + def _is_right_connected(self): + return self.force_reader_right is not None and self.force_reader_right.handtype == 'Right' + + def _connected_hand_labels(self): + labels = [] + if self._is_left_connected(): + labels.append("左手") + if self._is_right_connected(): + labels.append("右手") + return labels def _calibration_with_progress(self, stability_window=30, stability_threshold=0.03): """ @@ -750,6 +764,11 @@ class Retarget(): stability_window: 稳定性检测窗口（帧数） stability_threshold: 稳定性阈值 """ + left_valid = self._is_left_connected() + right_valid = self._is_right_connected() + if not left_valid and not right_valid: + return False + self.calibration_data_left = [] self.calibration_data_right = [] @@ -765,30 +784,46 @@ class Retarget(): bar_format="{l_bar}{bar}| {n_fmt}/{total_fmt} [{postfix}]", postfix="") as pbar: while collected_duration < target_stable_duration: - left_pos = copy.deepcopy(self.force_reader_left.poslist) - right_pos = copy.deepcopy(self.force_reader_right.poslist) + left_pos = copy.deepcopy(self.force_reader_left.poslist) if left_valid else None + right_pos = copy.deepcopy(self.force_reader_right.poslist) if right_valid else None - temp_buffer_left.append(left_pos) - temp_buffer_right.append(right_pos) + if left_valid: + temp_buffer_left.append(left_pos) + if right_valid: + temp_buffer_right.append(right_pos) - if len(temp_buffer_left) > stability_window: + if left_valid and len(temp_buffer_left) > stability_window: temp_buffer_left.pop(0) + if right_valid and len(temp_buffer_right) > stability_window: temp_buffer_right.pop(0) - if len(temp_buffer_left) < stability_window: - pbar.set_postfix_str(f"{Fore.YELLOW}等待数据 {len(temp_buffer_left)}/{stability_window}{Fore.RESET}") + buffer_lengths = [] + if left_valid: + buffer_lengths.append(len(temp_buffer_left)) + if right_valid: + buffer_lengths.append(len(temp_buffer_right)) + min_buffer_len = min(buffer_lengths) + if min_buffer_len < stability_window: + pbar.set_postfix_str(f"{Fore.YELLOW}等待数据 {min_buffer_len}/{stability_window}{Fore.RESET}") pbar.refresh() time.sleep(1.0 / 30) continue - var_left = np.var(temp_buffer_left, axis=0).mean() - var_right = np.var(temp_buffer_right, axis=0).mean() + variances = [] + drifts = [] + if left_valid: + var_left = np.var(temp_buffer_left, axis=0).mean() + drift_left = np.abs(np.array(temp_buffer_left[-1]) - np.array(temp_buffer_left[0])).mean() + variances.append(var_left) + drifts.append(drift_left) + if right_valid: + var_right = np.var(temp_buffer_right, axis=0).mean() + drift_right = np.abs(np.array(temp_buffer_right[-1]) - np.array(temp_buffer_right[0])).mean() + variances.append(var_right) + drifts.append(drift_right) - drift_left = np.abs(np.array(temp_buffer_left[-1]) - np.array(temp_buffer_left[0])).mean() - drift_right = np.abs(np.array(temp_buffer_right[-1]) - np.array(temp_buffer_right[0])).mean() - - variance = max(var_left, var_right) - drift = max(drift_left, drift_right) + variance = max(variances) + drift = max(drifts) is_stable = (variance < stability_threshold and drift < stability_threshold) @@ -815,22 +850,29 @@ class Retarget(): time.sleep(1.0 / 30) if len(stability_samples) > 0: - self.calibration_data_left = [s[0] for s in stability_samples] - self.calibration_data_right = [s[1] for s in stability_samples] + self.calibration_data_left = [s[0] for s in stability_samples if s[0] is not None] + self.calibration_data_right = [s[1] for s in stability_samples if s[1] is not None] print(f"{Fore.GREEN}采集完成，有效样本: {len(stability_samples)} 帧{Fore.RESET}") + return True + return False def run_calibration(self): """ 执行自动标定流程 """ # 检查手套连接状态 - left_valid = self.force_reader_left and self.force_reader_left.handtype == 'Left' - right_valid = self.force_reader_right and self.force_reader_right.handtype == 'Right' + left_valid = self._is_left_connected() + right_valid = self._is_right_connected() if not left_valid and not right_valid: print(f"\n{Fore.RED}【标定失败】左右手套均未连接，请检查设备连接后重试{Fore.RESET}\n") self.calibration_in_progress = False return False + print(f"\n{Fore.CYAN}本次标定设备: {', '.join(self._connected_hand_labels())}{Fore.RESET}") + if not left_valid: + print(f"{Fore.YELLOW}左手未连接，本次跳过左手采样，并保留已有/样例标定数据{Fore.RESET}") + if not right_valid: + print(f"{Fore.YELLOW}右手未连接，本次跳过右手采样，并保留已有/样例标定数据{Fore.RESET}") self.calibration_in_progress = True @@ -844,11 +886,15 @@ class Retarget(): self._calibration_with_progress(10) - if len(self.calibration_data_left) > 0: - avg_left_open = self._calculate_weighted_average(self.calibration_data_left) - avg_right_open = self._calculate_weighted_average(self.calibration_data_right) - self.lefthand.calibrationoriginal = avg_left_open - self.righthand.calibrationoriginal = avg_right_open + open_ok = ((not left_valid or len(self.calibration_data_left) > 0) and + (not right_valid or len(self.calibration_data_right) > 0)) + if open_ok: + if left_valid: + avg_left_open = self._calculate_weighted_average(self.calibration_data_left) + self.lefthand.calibrationoriginal = avg_left_open + if right_valid: + avg_right_open = self._calculate_weighted_average(self.calibration_data_right) + self.righthand.calibrationoriginal = avg_right_open else: print(f"\n{Fore.RED}【标定失败】五指张开数据采集失败{Fore.RESET}\n") self.calibration_in_progress = False @@ -863,11 +909,15 @@ class Retarget(): self._calibration_with_progress(10) - if len(self.calibration_data_left) > 0: - avg_left_opose = self._calculate_weighted_average(self.calibration_data_left) - avg_right_opose = self._calculate_weighted_average(self.calibration_data_right) - self.lefthand.calibrationopose = avg_left_opose - self.righthand.calibrationopose = avg_right_opose + opose_ok = ((not left_valid or len(self.calibration_data_left) > 0) and + (not right_valid or len(self.calibration_data_right) > 0)) + if opose_ok: + if left_valid: + avg_left_opose = self._calculate_weighted_average(self.calibration_data_left) + self.lefthand.calibrationopose = avg_left_opose + if right_valid: + avg_right_opose = self._calculate_weighted_average(self.calibration_data_right) + self.righthand.calibrationopose = avg_right_opose else: print(f"\n{Fore.RED}【标定失败】O型手势数据采集失败{Fore.RESET}\n") self.calibration_in_progress = False @@ -880,14 +930,18 @@ class Retarget(): print(f"\n{Fore.YELLOW}{'='*50}{Fore.RESET}") print(f"{Fore.YELLOW}【标定 3/{total_steps}】请握紧拳头 (对应电机值0){Fore.RESET}") print(f"{Fore.YELLOW}{'='*50}{Fore.RESET}\n") - + self._calibration_with_progress(10) - - if len(self.calibration_data_left) > 0: - avg_left_fist = self._calculate_weighted_average(self.calibration_data_left) - avg_right_fist = self._calculate_weighted_average(self.calibration_data_right) - self.lefthand.calibrationfistpose = avg_left_fist - self.righthand.calibrationfistpose = avg_right_fist + + fist_ok = ((not left_valid or len(self.calibration_data_left) > 0) and + (not right_valid or len(self.calibration_data_right) > 0)) + if fist_ok: + if left_valid: + avg_left_fist = self._calculate_weighted_average(self.calibration_data_left) + self.lefthand.calibrationfistpose = avg_left_fist + if right_valid: + avg_right_fist = self._calculate_weighted_average(self.calibration_data_right) + self.righthand.calibrationfistpose = avg_right_fist else: print(f"\n{Fore.RED}【标定失败】握拳数据采集失败{Fore.RESET}\n") self.calibration_in_progress = False @@ -896,14 +950,18 @@ class Retarget(): self._calculate_fist_from_extension() # ===== 保存标定数据 ===== - if self._save_to_tmp(): - print(f"\n{Fore.GREEN}{'='*50}{Fore.RESET}") - print(f"{Fore.GREEN}【标定完成】{'三个' if self.show_fist_calibration else '两个'}姿势数据已保存{Fore.RESET}") - print(f"{Fore.GREEN}{'='*50}{Fore.RESET}\n") + if not self._save_to_tmp(): + self.calibration_in_progress = False + return False + print(f"\n{Fore.GREEN}{'='*50}{Fore.RESET}") + print(f"{Fore.GREEN}【标定完成】{'三个' if self.show_fist_calibration else '两个'}姿势数据已保存{Fore.RESET}") + print(f"{Fore.GREEN}{'='*50}{Fore.RESET}\n") self.calibration_in_progress = False - self.righthand.initialize_mapper() - self.lefthand.initialize_mapper() + if right_valid: + self.righthand.initialize_mapper() + if left_valid: + self.lefthand.initialize_mapper() return True def _calculate_fist_from_extension(self): @@ -911,29 +969,52 @@ class Retarget(): 从 original 和 opose 延伸计算 fist 值 fist = opose + (opose - original) * extend_ratio """ - import numpy as np - original_l = self.lefthand.calibrationoriginal original_r = self.righthand.calibrationoriginal opose_l = self.lefthand.calibrationopose opose_r = self.righthand.calibrationopose - - if original_l is None or opose_l is None: - return - + ratio = self.fist_extend_ratio + generated = [] - fist_l = [] - fist_r = [] - for i in range(len(original_l)): - fist_l.append(opose_l[i] + (opose_l[i] - original_l[i]) * ratio) - for i in range(len(original_r)): - fist_r.append(opose_r[i] + (opose_r[i] - original_r[i]) * ratio) + if original_l is not None and opose_l is not None: + fist_l = [] + for i in range(len(original_l)): + fist_l.append(opose_l[i] + (opose_l[i] - original_l[i]) * ratio) + self.lefthand.calibrationfistpose = fist_l + generated.append("左手") + + if original_r is not None and opose_r is not None: + fist_r = [] + for i in range(len(original_r)): + fist_r.append(opose_r[i] + (opose_r[i] - original_r[i]) * ratio) + self.righthand.calibrationfistpose = fist_r + generated.append("右手") - self.lefthand.calibrationfistpose = fist_l - self.righthand.calibrationfistpose = fist_r - - self.node.get_logger().info(f"[自动计算] 握拳值已从 O型延伸 {ratio*100:.0f}% 生成") + if generated: + self.node.get_logger().info(f"[自动计算] {', '.join(generated)}握拳值已从 O型延伸 {ratio*100:.0f}% 生成") + + def _load_calibration_fallback_data(self): + """读取历史标定数据；没有历史数据时使用样例标定数据。""" + fallback_data = {} + + if TMP_FILE_PATH.exists(): + try: + fallback_data.update(json.loads(TMP_FILE_PATH.read_text()) or {}) + except Exception as e: + self.node.get_logger().warn(f"读取历史标定数据失败，将尝试样例数据: {e}") + + if SAMPLE_FILE_PATH.exists(): + try: + with open(SAMPLE_FILE_PATH, 'r') as f: + sample_data = yaml.safe_load(f) or {} + for key, value in sample_data.items(): + if fallback_data.get(key) is None: + fallback_data[key] = value + except Exception as e: + self.node.get_logger().warn(f"读取样例标定数据失败: {e}") + + return fallback_data def _save_to_tmp(self): """ @@ -943,60 +1024,57 @@ class Retarget(): """ # v2.8.6 版本添加标定差异检测 def calculate_vector_difference(vec1, vec2): - """计算两个向量之间的差异""" - if not vec1 or not vec2: - return 0 - - # 确保向量长度一致 - min_len = min(len(vec1), len(vec2)) - if min_len == 0: - return 0 - - # 计算欧几里得距离作为差异度量 - squared_diff = 0 - for i in range(min_len): - squared_diff += (vec1[i] - vec2[i]) ** 2 - return math.sqrt(squared_diff) - - # 检查右手标定数据的差异 - right_diff_original_fist = calculate_vector_difference( - self.righthand.calibrationoriginal, - self.righthand.calibrationfistpose - ) - - # 检查左手标定数据的差异 - left_diff_original_fist = calculate_vector_difference( - self.lefthand.calibrationoriginal, - self.lefthand.calibrationfistpose - ) + """计算两个向量之间的差异""" + if vec1 is None or vec2 is None: + return 0 + + min_len = min(len(vec1), len(vec2)) + if min_len == 0: + return 0 + + squared_diff = 0 + for i in range(min_len): + squared_diff += (vec1[i] - vec2[i]) ** 2 + return math.sqrt(squared_diff) # 设置阈值，根据实际情况调整 # 这个阈值表示两个向量之间的最小可接受差异 MIN_DIFFERENCE_THRESHOLD = 3.0 - + + right_connected = self._is_right_connected() + left_connected = self._is_left_connected() + # 检查右手数据是否有效 right_valid = False - if hasattr(self, 'force_reader_right') and self.force_reader_right.handtype == 'Right': - # 检查original和fistpose的差异 + if right_connected: + right_diff_original_fist = calculate_vector_difference( + self.righthand.calibrationoriginal, + self.righthand.calibrationfistpose + ) if right_diff_original_fist > MIN_DIFFERENCE_THRESHOLD: right_valid = True else: self.node.get_logger().error("右手张手和握拳标定数据差异过小，可能未正确标定") - + # 检查左手数据是否有效 left_valid = False - if hasattr(self, 'force_reader_left') and self.force_reader_left.handtype == 'Left': - # 检查original和fistpose的差异 + if left_connected: + left_diff_original_fist = calculate_vector_difference( + self.lefthand.calibrationoriginal, + self.lefthand.calibrationfistpose + ) if left_diff_original_fist > MIN_DIFFERENCE_THRESHOLD: left_valid = True else: self.node.get_logger().error("左手张手和握拳标定数据差异过小，可能未正确标定") - + # 如果没有有效的新数据，直接返回不保存 if not right_valid and not left_valid: self.node.get_logger().error("没有有效的标定数据差异，取消保存") return False + fallback_data = self._load_calibration_fallback_data() + data = { "timestamp": datetime.now().isoformat(), "jointangleoriginal_r": self.righthand.calibrationoriginal, @@ -1007,30 +1085,49 @@ class Retarget(): "jointangleopose_l": self.lefthand.calibrationopose } + def apply_fallback(hand_label, hand_obj, suffix): + keys_and_attrs = [ + (f"jointangleoriginal_{suffix}", "calibrationoriginal"), + (f"jointanglefist_{suffix}", "calibrationfistpose"), + (f"jointangleopose_{suffix}", "calibrationopose"), + ] + missing_keys = [] + for key, attr in keys_and_attrs: + value = fallback_data.get(key) + if value is None: + missing_keys.append(key) + continue + data[key] = value + setattr(hand_obj, attr, value) + + if missing_keys: + self.node.get_logger().warn( + f"{hand_label}未参与本次有效标定，且缺少历史/样例数据: {missing_keys}" + ) + return False + + self.node.get_logger().warning(f"{hand_label}未参与本次有效标定，采用历史/样例标定内容。") + return True + + if not right_valid: + right_ready = apply_fallback("右手", self.righthand, "r") + else: + right_ready = True + + if not left_valid: + left_ready = apply_fallback("左手", self.lefthand, "l") + else: + left_ready = True + + if right_connected and not right_ready: + self.node.get_logger().error("右手缺少可用标定数据，取消保存") + return False + if left_connected and not left_ready: + self.node.get_logger().error("左手缺少可用标定数据，取消保存") + return False + try: TMP_FILE_PATH.parent.mkdir(parents=True, exist_ok=True) - if TMP_FILE_PATH.exists(): - content = TMP_FILE_PATH.read_text() - historydata = json.loads(content) - # 根据有效性更新数据 - if not right_valid: - data["jointangleoriginal_r"] = historydata["jointangleoriginal_r"] - data["jointanglefist_r"] = historydata["jointanglefist_r"] - data["jointangleopose_r"] = historydata["jointangleopose_r"] - # 同时更新 righthand 的标定属性 - self.righthand.calibrationoriginal = historydata["jointangleoriginal_r"] - self.righthand.calibrationfistpose = historydata["jointanglefist_r"] - self.righthand.calibrationopose = historydata["jointangleopose_r"] - self.node.get_logger().warning("右手未能正确标定,采用上一次的标定内容。") - if not left_valid: - data["jointangleoriginal_l"] = historydata["jointangleoriginal_l"] - data["jointanglefist_l"] = historydata["jointanglefist_l"] - data["jointangleopose_l"] = historydata["jointangleopose_l"] - # 同时更新 lefthand 的标定属性 - self.lefthand.calibrationoriginal = historydata["jointangleoriginal_l"] - self.lefthand.calibrationfistpose = historydata["jointanglefist_l"] - self.lefthand.calibrationopose = historydata["jointangleopose_l"] - self.node.get_logger().warning("左手未能正确标定,采用上一次的标定内容。") json_str = json.dumps(data, indent=2) TMP_FILE_PATH.write_text(json_str) self.node.get_logger().info("标定数据保存成功") @@ -1286,4 +1383,4 @@ class Retarget(): self.node.get_logger().info("模式切换: 固定握拳姿态") elif mode is not None: - self.node.get_logger().warn(f"未知模式: {mode}") \ No newline at end of file + self.node.get_logger().warn(f"未知模式: {mode}")