已经可以正常驱动灵巧手并获取反馈
This commit is contained in:
@@ -14,7 +14,7 @@ def generate_launch_description():
|
||||
'hand_type': 'left', # 配置Linker Hand灵巧手类型 left | right 字母为小写
|
||||
'hand_joint': "L20", # O6\L6P\L6\L7\L10\L20\G20(工业版)\L21 字母为大写
|
||||
'is_touch': True, # 配置Linker Hand灵巧手是否有压力传感器 True | False
|
||||
'can': 'can0', # 这里需要修改为实际的CAN总线名称 如果是win系统则类似于 PCAN_USBBUS1
|
||||
'can': 'can1', # 这里需要修改为实际的CAN总线名称 如果是win系统则类似于 PCAN_USBBUS1
|
||||
"modbus": "None" # "None" | "/dev/ttyUSB0" 这里需要修改为实际的Modbus总线名称 如果是win系统则 COM* Ubuntu则为/dev/ttyUSB*
|
||||
}],
|
||||
),
|
||||
@@ -28,7 +28,7 @@ def generate_launch_description():
|
||||
'hand_type': 'right', # 配置Linker Hand灵巧手类型 left | right 字母为小写
|
||||
'hand_joint': "L20", # O6\L6P\L6\L7\L10\L20\G20(工业版)\L21 字母为大写
|
||||
'is_touch': True, # 配置Linker Hand灵巧手是否有压力传感器 True | False
|
||||
'can': 'can1', # 这里需要修改为实际的CAN总线名称 如果是win系统则类似于 PCAN_USBBUS1
|
||||
'can': 'can0', # 这里需要修改为实际的CAN总线名称 如果是win系统则类似于 PCAN_USBBUS1
|
||||
"modbus": "None" # "None" | "/dev/ttyUSB0" 这里需要修改为实际的Modbus总线名称 如果是win系统则 COM* Ubuntu则为/dev/ttyUSB*
|
||||
}],
|
||||
),
|
||||
|
||||
@@ -3,9 +3,9 @@ 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
|
||||
CAN: "can1" # 配置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: L10 # 左手型号 O6/L6/L7/L10/L20/G20/L21/L25/
|
||||
JOINT: L20 # 左手型号 O6/L6/L7/L10/L20/G20/L21/L25/
|
||||
NAME: # 默认值，不用修改
|
||||
- joint41
|
||||
- joint42
|
||||
@@ -29,11 +29,11 @@ LINKER_HAND: # 手部配置信息
|
||||
- joint60
|
||||
|
||||
RIGHT_HAND:
|
||||
EXISTS: False # 是否存在右手
|
||||
TOUCH: False # 是否有压力传感器
|
||||
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: L10 # 右手型号 O6/L6/L7/L10/L20/G20/L21/L25/
|
||||
JOINT: L20 # 右手型号 O6/L6/L7/L10/L20/G20/L21/L25/
|
||||
NAME: # 默认值，不用修改
|
||||
- joint71
|
||||
- joint72
|
||||
|
||||
BIN
Binary file not shown.
+137
-69
@@ -10,21 +10,40 @@ 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
|
||||
ROLL_POS = 0x01 # All fingers roll joint position
|
||||
YAW_POS = 0x02 # All fingers yaw joint position
|
||||
ROOT1_POS = 0x03 # All fingers root1 joint position
|
||||
ROOT2_POS = 0x04 # All fingers root2 joint position
|
||||
ROOT3_POS = 0x05 # All fingers root3 joint position
|
||||
TIP_POS = 0x06 # All fingers tip joint position
|
||||
ROLL_SPEED = 0x09 # All fingers roll joint speed
|
||||
YAW_SPEED = 0x0A # All fingers yaw joint speed
|
||||
ROOT1_SPEED = 0x0B # All fingers root1 joint speed
|
||||
ROOT2_SPEED = 0x0C # All fingers root2 joint speed
|
||||
ROOT3_SPEED = 0x0D # All fingers root3 joint speed
|
||||
TIP_SPEED = 0x0E # All fingers tip joint speed
|
||||
ROLL_TORQUE = 0x11 # All fingers roll joint load/current threshold
|
||||
YAW_TORQUE = 0x12 # All fingers yaw joint load/current threshold
|
||||
ROOT1_TORQUE = 0x13 # All fingers root1 joint load/current threshold
|
||||
ROOT2_TORQUE = 0x14 # All fingers root2 joint load/current threshold
|
||||
ROOT3_TORQUE = 0x15 # All fingers root3 joint load/current threshold
|
||||
TIP_TORQUE = 0x16 # All fingers tip joint load/current threshold
|
||||
ROLL_FAULT = 0x19 # All fingers roll joint fault
|
||||
YAW_FAULT = 0x1A # All fingers yaw joint fault
|
||||
ROOT1_FAULT = 0x1B # All fingers root1 joint fault
|
||||
ROOT2_FAULT = 0x1C # All fingers root2 joint fault
|
||||
ROOT3_FAULT = 0x1D # All fingers root3 joint fault
|
||||
TIP_FAULT = 0x1E # All fingers tip joint fault
|
||||
ROLL_TEMPERATURE = 0x21 # All fingers roll joint temperature
|
||||
YAW_TEMPERATURE = 0x22 # All fingers yaw joint temperature
|
||||
ROOT1_TEMPERATURE = 0x23 # All fingers root1 joint temperature
|
||||
ROOT2_TEMPERATURE = 0x24 # All fingers root2 joint temperature
|
||||
ROOT3_TEMPERATURE = 0x25 # All fingers root3 joint temperature
|
||||
TIP_TEMPERATURE = 0x26 # All fingers tip joint temperature
|
||||
FINGER_SPEED = 0x81 # Per-finger speed
|
||||
FINGER_TORQUE = 0x82 # Per-finger torque/current threshold
|
||||
FINGER_FAULT = 0x83 # Per-finger fault
|
||||
FINGER_TEMPERATURE = 0x84 # Per-finger temperature
|
||||
HAND_UID = 0xC0 # Device unique identifier Read only --------
|
||||
HAND_HARDWARE_VERSION = 0xC1 # Hardware version Read only --------
|
||||
HAND_SOFTWARE_VERSION = 0xC2 # Software version Read only --------
|
||||
@@ -42,9 +61,13 @@ class LinkerHandL20Can:
|
||||
self.running = True
|
||||
self.x05 = [255] * 5
|
||||
self.x06, self.x07 = [],[]
|
||||
self.x09, self.x0a, self.x0b, self.x0c, self.x0d, self.x0e = [[-1] * 5 for _ in range(6)]
|
||||
self.x11, self.x12, self.x13, self.x14, self.x15, self.x16 = [[-1] * 5 for _ in range(6)]
|
||||
self.x19, self.x1a, self.x1b, self.x1c, self.x1d, self.x1e = [[-1] * 5 for _ in range(6)]
|
||||
self.x21, self.x22, self.x23, self.x24, self.x25, self.x26 = [[-1] * 5 for _ in range(6)]
|
||||
self.x81, self.x82, self.x83, self.x84 = [[-1] * 5 for _ in range(4)]
|
||||
# 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)
|
||||
@@ -89,12 +112,12 @@ class LinkerHandL20Can:
|
||||
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)
|
||||
# Start receive thread before querying touch type so replies can be processed.
|
||||
self.receive_thread = threading.Thread(target=self.receive_response)
|
||||
self.receive_thread.daemon = True
|
||||
self.receive_thread.start()
|
||||
self.get_touch_type()
|
||||
time.sleep(0.1)
|
||||
|
||||
def init_can_bus(self, channel, baudrate):
|
||||
"""
|
||||
@@ -168,16 +191,16 @@ class LinkerHandL20Can:
|
||||
|
||||
|
||||
def set_finger_base(self, angles):
|
||||
self.send_command(FrameProperty.JOINT_PITCH_NR, angles)
|
||||
self.send_command(FrameProperty.ROOT1_POS, angles)
|
||||
|
||||
def set_finger_tip(self, angles):
|
||||
self.send_command(FrameProperty.JOINT_TIP_NR, angles)
|
||||
self.send_command(FrameProperty.TIP_POS, angles)
|
||||
|
||||
def set_finger_middle(self, angles):
|
||||
self.send_command(FrameProperty.JOINT_YAW_NR, angles)
|
||||
self.send_command(FrameProperty.YAW_POS, angles)
|
||||
|
||||
def set_thumb_roll(self, angle):
|
||||
self.send_command(FrameProperty.JOINT_ROLL_NR, angle)
|
||||
self.send_command(FrameProperty.ROLL_POS, 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
|
||||
@@ -202,35 +225,36 @@ class LinkerHandL20Can:
|
||||
self.send_command(frame, angles)
|
||||
|
||||
def set_joint_yaw(self, angles):
|
||||
self.send_command(0x02, angles)
|
||||
self.send_command(FrameProperty.YAW_POS, angles)
|
||||
|
||||
def set_joint_roll(self, thumb_roll):
|
||||
self.send_command(0x03, [thumb_roll, 0, 0, 0, 0])
|
||||
self.send_command(FrameProperty.ROLL_POS, [thumb_roll, 0, 0, 0, 0])
|
||||
|
||||
def set_joint_speed(self, speed):
|
||||
self.x05 = speed
|
||||
self.send_command(0x05, speed)
|
||||
self.x81 = speed
|
||||
self.send_command(FrameProperty.FINGER_SPEED, speed)
|
||||
def set_electric_current(self, e_c=[]):
|
||||
self.send_command(0x06, e_c)
|
||||
self.x82 = e_c
|
||||
self.send_command(FrameProperty.FINGER_TORQUE, e_c)
|
||||
|
||||
def get_normal_force(self):
|
||||
self.send_command(0x20,[])
|
||||
self.send_command(0x90, [])
|
||||
|
||||
def get_tangential_force(self):
|
||||
self.send_command(0x21,[])
|
||||
self.send_command(0x91, [])
|
||||
|
||||
|
||||
def get_tangential_force_dir(self):
|
||||
self.send_command(0x22,[])
|
||||
self.send_command(0x92, [])
|
||||
|
||||
def get_approach_inc(self):
|
||||
self.send_command(0x23,[])
|
||||
self.send_command(0x93, [])
|
||||
|
||||
|
||||
|
||||
|
||||
def get_electric_current(self, e_c=[]):
|
||||
self.send_command(0x06, e_c)
|
||||
self.send_command(FrameProperty.FINGER_TORQUE, e_c)
|
||||
|
||||
def request_device_info(self):
|
||||
self.send_command(0xC0, [0])
|
||||
@@ -263,28 +287,73 @@ class LinkerHandL20Can:
|
||||
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 == 0x0A:
|
||||
self.x0a = list(response_data)
|
||||
elif frame_type == 0x0B:
|
||||
self.x0b = list(response_data)
|
||||
elif frame_type == 0x0C:
|
||||
self.x0c = list(response_data)
|
||||
elif frame_type == 0x0D:
|
||||
self.x0d = list(response_data)
|
||||
elif frame_type == 0x0E:
|
||||
self.x0e = list(response_data)
|
||||
elif frame_type == 0x11:
|
||||
self.x11 = list(response_data)
|
||||
elif frame_type == 0x12:
|
||||
self.x12 = list(response_data)
|
||||
elif frame_type == 0x13:
|
||||
self.x13 = list(response_data)
|
||||
elif frame_type == 0x14:
|
||||
self.x14 = list(response_data)
|
||||
elif frame_type == 0x15:
|
||||
self.x15 = list(response_data)
|
||||
elif frame_type == 0x16:
|
||||
self.x16 = list(response_data)
|
||||
elif frame_type == 0x19:
|
||||
self.x19 = list(response_data)
|
||||
elif frame_type == 0x1A:
|
||||
self.x1a = list(response_data)
|
||||
elif frame_type == 0x1B:
|
||||
self.x1b = list(response_data)
|
||||
elif frame_type == 0x1C:
|
||||
self.x1c = list(response_data)
|
||||
elif frame_type == 0x1D:
|
||||
self.x1d = list(response_data)
|
||||
elif frame_type == 0x1E:
|
||||
self.x1e = list(response_data)
|
||||
elif frame_type == 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]
|
||||
self.x21 = list(response_data)
|
||||
elif frame_type == 0x22:
|
||||
d = list(response_data)
|
||||
self.tangential_force_dir = [float(i) for i in d]
|
||||
self.x22 = list(response_data)
|
||||
elif frame_type == 0x23:
|
||||
d = list(response_data)
|
||||
self.approach_inc = [float(i) for i in d]
|
||||
self.x23 = list(response_data)
|
||||
elif frame_type == 0x24:
|
||||
self.x24 = list(response_data)
|
||||
elif frame_type == 0x25:
|
||||
self.x25 = list(response_data)
|
||||
elif frame_type == 0x26:
|
||||
self.x26 = list(response_data)
|
||||
elif frame_type == 0x81:
|
||||
self.x81 = list(response_data)
|
||||
elif frame_type == 0x82:
|
||||
self.x82 = list(response_data)
|
||||
elif frame_type == 0x83:
|
||||
self.x83 = list(response_data)
|
||||
elif frame_type == 0x84:
|
||||
self.x84 = list(response_data)
|
||||
elif frame_type == 0x90:
|
||||
self.normal_force = [float(i) for i in list(response_data)]
|
||||
elif frame_type == 0x91:
|
||||
self.tangential_force = [float(i) for i in list(response_data)]
|
||||
elif frame_type == 0x92:
|
||||
self.tangential_force_dir = [float(i) for i in list(response_data)]
|
||||
elif frame_type == 0x93:
|
||||
self.approach_inc = [float(i) for i in list(response_data)]
|
||||
elif frame_type == 0xb0:
|
||||
self.xb0 = list(response_data)
|
||||
elif frame_type == 0xb1:
|
||||
@@ -350,57 +419,56 @@ class LinkerHandL20Can:
|
||||
if len(speed) != 5:
|
||||
raise ValueError("Speed list must have 5 elements.")
|
||||
return
|
||||
self.send_command(0x05,speed)
|
||||
self.x81 = speed
|
||||
self.send_command(FrameProperty.FINGER_SPEED, 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)
|
||||
'''Set per-finger torque/current threshold.'''
|
||||
self.x82 = current
|
||||
self.send_command(FrameProperty.FINGER_TORQUE, 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
|
||||
self.send_command(FrameProperty.ROOT1_POS, [], sleep=0.01)
|
||||
self.send_command(FrameProperty.YAW_POS, [], sleep=0.01)
|
||||
self.send_command(FrameProperty.ROLL_POS, [], sleep=0.01)
|
||||
self.send_command(FrameProperty.TIP_POS, [], sleep=0.01)
|
||||
return self.x03 + self.x02 + self.x01 + self.x06
|
||||
|
||||
def get_current_pub_status(self):
|
||||
time.sleep(0.01)
|
||||
return self.x01 + self.x02 + self.x03 + self.x04
|
||||
return self.x03 + self.x02 + self.x01 + self.x06
|
||||
|
||||
def get_speed(self):
|
||||
'''Get current motor speed'''
|
||||
self.send_command(0x05, [0])
|
||||
self.send_command(FrameProperty.FINGER_SPEED, [])
|
||||
time.sleep(0.001)
|
||||
return self.x05
|
||||
return self.x81
|
||||
def get_current(self):
|
||||
'''Get current threshold'''
|
||||
self.send_command(0x06, [0])
|
||||
return self.x06
|
||||
'''Get per-finger torque/current threshold.'''
|
||||
self.send_command(FrameProperty.FINGER_TORQUE, [])
|
||||
return self.x82
|
||||
def get_torque(self):
|
||||
'''Get current motor torque, not supported for L20'''
|
||||
return [0] * 5
|
||||
'''Get current motor torque/current threshold.'''
|
||||
self.send_command(FrameProperty.FINGER_TORQUE, [])
|
||||
return self.x82
|
||||
def get_fault(self):
|
||||
self.send_command(0x07,[])
|
||||
self.send_command(FrameProperty.FINGER_FAULT, [])
|
||||
time.sleep(0.01)
|
||||
return self.x07
|
||||
return self.x83
|
||||
|
||||
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
|
||||
self.send_command(FrameProperty.FINGER_TEMPERATURE, [])
|
||||
return self.x84
|
||||
|
||||
def clear_faults(self):
|
||||
'''Clear motor faults'''
|
||||
self.send_command(0x07, [1, 1, 1, 1, 1])
|
||||
self.send_command(FrameProperty.FINGER_FAULT, [1, 1, 1, 1, 1])
|
||||
|
||||
def get_touch_type(self):
|
||||
'''Get touch type'''
|
||||
@@ -412,7 +480,7 @@ class LinkerHandL20Can:
|
||||
elif self.xb0 == [1]:
|
||||
return 1
|
||||
else:
|
||||
self.send_command(0x20,[],sleep=0.03)
|
||||
self.send_command(0x90, [], sleep=0.03)
|
||||
time.sleep(0.01)
|
||||
if self.normal_force[0] == -1:
|
||||
return -1
|
||||
@@ -458,8 +526,8 @@ class LinkerHandL20Can:
|
||||
|
||||
def get_faults(self):
|
||||
'''Get motor fault codes'''
|
||||
self.send_command(0x07, [])
|
||||
return self.x07
|
||||
self.send_command(FrameProperty.FINGER_FAULT, [])
|
||||
return self.x83
|
||||
def get_force(self):
|
||||
'''Get pressure sensor data'''
|
||||
return [self.normal_force,self.tangential_force,self.tangential_force_dir,self.approach_inc]
|
||||
|
||||
Binary file not shown.
@@ -192,6 +192,12 @@ class LinkerHand(Node):
|
||||
self.matrix_touch_pub = self.create_publisher(String, self.topic(f'/cb_{self.hand_type}_hand_matrix_touch'), 10)
|
||||
self.matrix_touch_pub_pc = self.create_publisher(PointCloud2, self.topic(f'/cb_{self.hand_type}_hand_matrix_touch_pc'), 10)
|
||||
self.matrix_touch_mass_pub = self.create_publisher(String, self.topic(f'/cb_{self.hand_type}_hand_matrix_touch_mass'), 10)
|
||||
elif self.hand_joint == "L20":
|
||||
ColorMsg(msg=f"{self.hand_type} {self.hand_joint} matrix pressure sensing enabled", color='green')
|
||||
self.touch_type = 2
|
||||
self.matrix_touch_pub = self.create_publisher(String, self.topic(f'/cb_{self.hand_type}_hand_matrix_touch'), 10)
|
||||
self.matrix_touch_pub_pc = self.create_publisher(PointCloud2, self.topic(f'/cb_{self.hand_type}_hand_matrix_touch_pc'), 10)
|
||||
self.matrix_touch_mass_pub = self.create_publisher(String, self.topic(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, self.topic(f'/cb_{self.hand_type}_hand_matrix_touch'), 10)
|
||||
@@ -223,7 +229,7 @@ class LinkerHand(Node):
|
||||
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]
|
||||
pose = None
|
||||
elif self.hand_joint == "G20":
|
||||
# G20 uses a different CAN protocol from L20 even though both
|
||||
# expose a 20-value ROS command. This is the calibrated startup
|
||||
@@ -236,8 +242,19 @@ class LinkerHand(Node):
|
||||
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 self.startup_pose:
|
||||
if pose is None or len(self.startup_pose) != len(pose):
|
||||
expected = 0 if pose is None else len(pose)
|
||||
expected_lengths = {
|
||||
"O6": 6,
|
||||
"L6": 6,
|
||||
"L6P": 6,
|
||||
"L7": 7,
|
||||
"L10": 10,
|
||||
"L20": 20,
|
||||
"G20": 20,
|
||||
"L21": 25,
|
||||
"L25": 25,
|
||||
}
|
||||
expected = expected_lengths.get(str(self.hand_joint).upper(), 0)
|
||||
if len(self.startup_pose) != expected:
|
||||
raise ValueError(
|
||||
f'startup_pose for {self.hand_joint} must contain {expected} values'
|
||||
)
|
||||
@@ -376,7 +393,6 @@ class LinkerHand(Node):
|
||||
}
|
||||
|
||||
if self.run_count == 9:
|
||||
self.api.clear_faults() # 自动清除错误编码
|
||||
self.run_count = 0
|
||||
self.run_count += 1
|
||||
time.sleep(0.003)
|
||||
|
||||
Reference in New Issue
Block a user