标定支持单手连接。只接右手时会跳过左手采样
This commit is contained in:
+21
@@ -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
|
||||
|
||||
@@ -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*权限
|
||||
}],
|
||||
),
|
||||
])
|
||||
@@ -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*
|
||||
}],
|
||||
),
|
||||
])
|
||||
@@ -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总线名称
|
||||
}],
|
||||
),
|
||||
])
|
||||
@@ -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
|
||||
@@ -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
|
||||
@@ -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
|
||||
@@ -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
|
||||
@@ -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
|
||||
@@ -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
|
||||
@@ -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权限
|
||||
+1269
File diff suppressed because it is too large
Load Diff
+531
@@ -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()
|
||||
+478
@@ -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
|
||||
+822
@@ -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
|
||||
+448
@@ -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
|
||||
+848
@@ -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
|
||||
+426
@@ -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()
|
||||
+419
@@ -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()
|
||||
+447
@@ -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()
|
||||
+345
@@ -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}")
|
||||
+460
@@ -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}")
|
||||
+444
@@ -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}")
|
||||
+423
@@ -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}")
|
||||
+671
@@ -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}")
|
||||
+1157
File diff suppressed because it is too large
Load Diff
@@ -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")
|
||||
@@ -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)
|
||||
@@ -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
|
||||
|
||||
|
||||
@@ -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
|
||||
|
||||
@@ -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))
|
||||
@@ -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
|
||||
|
||||
@@ -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("程序已退出。")
|
||||
@@ -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("程序已退出。")
|
||||
@@ -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("程序已退出。")
|
||||
@@ -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("程序已退出。")
|
||||
@@ -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("程序已退出。")
|
||||
@@ -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("程序已退出。")
|
||||
@@ -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("程序已退出。")
|
||||
@@ -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("程序已退出。")
|
||||
@@ -0,0 +1,21 @@
|
||||
<?xml version="1.0"?>
|
||||
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
|
||||
<package format="3">
|
||||
<name>linker_hand_ros2_sdk</name>
|
||||
<version>0.0.0</version>
|
||||
<description>TODO: Package description</description>
|
||||
<maintainer email="linker-robot@todo.todo">linker-robot</maintainer>
|
||||
<license>TODO: License declaration</license>
|
||||
|
||||
<test_depend>ament_copyright</test_depend>
|
||||
<test_depend>ament_flake8</test_depend>
|
||||
<test_depend>ament_pep257</test_depend>
|
||||
<test_depend>python3-pytest</test_depend>
|
||||
|
||||
<exec_depend>rclpy</exec_depend>
|
||||
<exec_depend>launch</exec_depend>
|
||||
|
||||
<export>
|
||||
<build_type>ament_python</build_type>
|
||||
</export>
|
||||
</package>
|
||||
@@ -0,0 +1,3 @@
|
||||
[build-system]
|
||||
requires = ["setuptools>=61.0"]
|
||||
build-backend = "setuptools.build_meta"
|
||||
@@ -0,0 +1,4 @@
|
||||
[develop]
|
||||
script_dir=$base/lib/linker_hand_ros2_sdk
|
||||
[install]
|
||||
install_scripts=$base/lib/linker_hand_ros2_sdk
|
||||
@@ -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',
|
||||
],
|
||||
},
|
||||
)
|
||||
@@ -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'
|
||||
@@ -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)
|
||||
@@ -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'
|
||||
@@ -27,7 +27,7 @@ serial:
|
||||
baudrate: 460800
|
||||
port: /dev/ttyUSB1
|
||||
right:
|
||||
baudrate: 460800
|
||||
baudrate: 2000000
|
||||
port: /dev/ttyUSB0
|
||||
serial_debug: false
|
||||
system:
|
||||
|
||||
@@ -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}")
|
||||
self.node.get_logger().warn(f"未知模式: {mode}")
|
||||
|
||||
Reference in New Issue
Block a user