diff --git a/src/linker_hand_ros2_sdk/launch/linker_hand_double.launch.py b/src/linker_hand_ros2_sdk/launch/linker_hand_double.launch.py index f5c283a..4f50f53 100644 --- a/src/linker_hand_ros2_sdk/launch/linker_hand_double.launch.py +++ b/src/linker_hand_ros2_sdk/launch/linker_hand_double.launch.py @@ -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* }], ), diff --git a/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/config/setting.yaml b/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/config/setting.yaml index f3a7511..793e043 100644 --- a/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/config/setting.yaml +++ b/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/config/setting.yaml @@ -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 diff --git a/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/core/can/__pycache__/linker_hand_l20_can.cpython-312.pyc b/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/core/can/__pycache__/linker_hand_l20_can.cpython-312.pyc new file mode 100644 index 0000000..c92aaee Binary files /dev/null and b/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/core/can/__pycache__/linker_hand_l20_can.cpython-312.pyc differ diff --git a/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/core/can/linker_hand_l20_can.py b/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/core/can/linker_hand_l20_can.py index 5b5f36e..18410d9 100644 --- a/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/core/can/linker_hand_l20_can.py +++ b/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/core/can/linker_hand_l20_can.py @@ -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] diff --git a/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/__pycache__/linker_hand.cpython-312.pyc b/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/__pycache__/linker_hand.cpython-312.pyc new file mode 100644 index 0000000..408d749 Binary files /dev/null and b/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/__pycache__/linker_hand.cpython-312.pyc differ diff --git a/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/linker_hand.py b/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/linker_hand.py index 6e7d22f..34d4e1d 100644 --- a/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/linker_hand.py +++ b/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/linker_hand.py @@ -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)