320新增三指夹爪部分控制接口

Author: wangWking

pymycobot/common.py

@@ -22,6 +22,7 @@ class ProGripper(object):
     SET_GRIPPER_PAUSE = 37
     SET_GRIPPER_RESUME = 38
     SET_GRIPPER_STOP = 39
+    SET_GRIPPER_ANGLES = 45
 
 
 class ProtocolCode(object):

pymycobot/error.py

@@ -787,41 +787,63 @@ def calibration_parameters(**kwargs):
                         "The range of 'torque_value' in {} is 100 ~ 300, but the received value is {}".format(parameter,
                                                                                                               torque_value))
 
-                elif parameter == "gripper_speed":
-                    gripper_id, speed = value
-                    if not isinstance(gripper_id, int) or not isinstance(speed, int):
-                        raise MyCobot320DataException(
-                            "Both 'gripper_id' and 'speed' in {} must be integers".format(parameter))
-                    if gripper_id < 1 or gripper_id > 254:
-                        raise MyCobot320DataException(
-                            "The range of 'gripper_id' in {} is 1 ~ 254, but the received value is {}".format(parameter,
-                                                                                                              gripper_id))
-                    if speed < 1 or torque_value > 100:
-                        raise MyCobot320DataException(
-                            "The range of 'speed' in {} is 1 ~ 100, but the received value is {}".format(parameter,
-                                                                                                         speed))
-                elif parameter == "set_gripper_args":
-                    if len(value) != 3:
-                        raise ValueError(f"Expected 3 arguments, but got {len(value)}")
-                    gripper_id, address, data = value
-                    if not isinstance(gripper_id, int) or not isinstance(address, int) or not isinstance(data, int):
-                        raise MyCobot320DataException(
-                            "All arguments in {} must be integers".format(parameter))
-                    if gripper_id < 1 or gripper_id > 254:
-                        raise MyCobot320DataException(
-                            "The range of 'gripper_id' in {} is 1 ~ 254, but the received value is {}".format(parameter,
-                                                                                                              gripper_id))
-                    invalid_addresses = [1, 2, 4, 5, 6, 7, 8, 9, 11, 12, 13, 14, 15, 16, 17, 18, 19, 20, 22, 24, 26, 27, 28, 32, 33,
-                                         36,
-                                         37, 38, 39, 40, 42, 44]
-                    if address < 1 or address > 44:
+            elif parameter == "gripper_speed":
+                gripper_id, speed = value
+                if not isinstance(gripper_id, int) or not isinstance(speed, int):
+                    raise MyCobot320DataException(
+                        "Both 'gripper_id' and 'speed' in {} must be integers".format(parameter))
+                if gripper_id < 1 or gripper_id > 254:
+                    raise MyCobot320DataException(
+                        "The range of 'gripper_id' in {} is 1 ~ 254, but the received value is {}".format(parameter,
+                                                                                                          gripper_id))
+                if speed < 1 or speed > 100:
+                    raise MyCobot320DataException(
+                        "The range of 'speed' in {} is 1 ~ 100, but the received value is {}".format(parameter,
+                                                                                                     speed))
+            elif parameter == "joint_id":
+                check_value_type(parameter, value_type, MyCobot320DataException, int)
+                if not 0 <= value <= 6:
+                    raise MyCobot320DataException(
+                        "speed value not right, should be 1 ~ 100, the received speed is %s"
+                        % value
+                    )
+            elif parameter == "set_gripper_args":
+                gripper_id, address, data = value
+                if not isinstance(gripper_id, int) or not isinstance(address, int):
+                    raise MyCobot320DataException(
+                        "All arguments in {} must be integers".format(parameter))
+                if gripper_id < 1 or gripper_id > 254:
+                    raise MyCobot320DataException(
+                        "The range of 'gripper_id' in {} is 1 ~ 254, but the received value is {}".format(parameter,
+                                                                                                          gripper_id))
+                invalid_addresses = [1, 2, 4, 5, 6, 7, 8, 9, 11, 12, 13, 14, 15, 16, 17, 18, 19, 20, 22, 24, 26, 27, 28, 32, 33,
+                                     36,
+                                     37, 38, 39, 40, 42, 44]
+                if address < 1 or address > 48:
+                    raise MyCobot320DataException(
+                        "The range of 'address' in {} is 1 ~ 48, but the received value is {}".format(parameter,
+                                                                                                      address))
+                if address in invalid_addresses:
+                    raise MyCobot320DataException(
+                        "'address' in {} cannot be one of the following values: {}, but the received value is {}".format(
+                            parameter, invalid_addresses, address))
+                print('data:', data)
+                if isinstance(data, list):
+                    # 处理 data 为列表的情况
+                    if len(data) != 6:
                         raise MyCobot320DataException(
-                            "The range of 'address' in {} is 1 ~ 44, but the received value is {}".format(parameter,
-                                                                                                          address))
-                    if address in invalid_addresses:
+                            "data must be a list of length 6。"
+                        )
+                    for index, value in enumerate(data):
+                        if not (0 <= value <= 100):
+                            raise MyCobot320DataException(
+                                "Each value in the data list must be between 0 and 100. Error index {}: The received value is {}".format(index, value)
+                            )
+                else:
+                    if not isinstance(data, int):
                         raise MyCobot320DataException(
-                            "'address' in {} cannot be one of the following values: {}, but the received value is {}".format(
-                                parameter, invalid_addresses, address))
+                            "The parameter {} must be an integer".format(data)
+                        )
                     # 根据 address 来处理 value
                     if address in [3, 43]:
                         if data < 1 or data > 254:
@@ -849,24 +871,24 @@ def calibration_parameters(**kwargs):
                                 "Error in parameter '{}': The range of 'value' for address={} is 0 ~ 100, but the received value is {}".format(
                                     parameter, address, data))
 
-                elif parameter == "get_gripper_args":
-                    gripper_id, address = value
-                    if not isinstance(gripper_id, int) or not isinstance(address, int):
-                        raise MyCobot320DataException(
-                            "All arguments in {} must be integers".format(parameter))
-                    if gripper_id < 1 or gripper_id > 254:
-                        raise MyCobot320DataException(
-                            "The range of 'gripper_id' in {} is 1 ~ 254, but the received value is {}".format(parameter,
-                                                                                                              gripper_id))
-                    invalid_addresses = [5, 6, 10, 15, 16, 17, 18, 19, 20, 21, 23, 25, 29, 30, 31, 34, 35, 41, 43]
-                    if address < 1 or address > 44:
-                        raise MyCobot320DataException(
-                            "The range of 'address' in {} is 1 ~ 44, but the received value is {}".format(parameter,
-                                                                                                          address))
-                    if address in invalid_addresses:
-                        raise MyCobot320DataException(
-                            "'address' in {} cannot be one of the following values: {}, but the received value is {}".format(
-                                parameter, invalid_addresses, address))
+            elif parameter == "get_gripper_args":
+                gripper_id, address = value
+                if not isinstance(gripper_id, int) or not isinstance(address, int):
+                    raise MyCobot320DataException(
+                        "All arguments in {} must be integers".format(parameter))
+                if gripper_id < 1 or gripper_id > 254:
+                    raise MyCobot320DataException(
+                        "The range of 'gripper_id' in {} is 1 ~ 254, but the received value is {}".format(parameter,
+                                                                                                          gripper_id))
+                invalid_addresses = [5, 6, 10, 15, 16, 17, 18, 19, 20, 21, 23, 25, 29, 30, 31, 34, 35, 41, 43]
+                if address < 1 or address > 44:
+                    raise MyCobot320DataException(
+                        "The range of 'address' in {} is 1 ~ 44, but the received value is {}".format(parameter,
+                                                                                                      address))
+                if address in invalid_addresses:
+                    raise MyCobot320DataException(
+                        "'address' in {} cannot be one of the following values: {}, but the received value is {}".format(
+                            parameter, invalid_addresses, address))
     elif class_name in ["MechArm", "MechArmSocket"]:
         public_check(parameter_list, kwargs, robot_limit, class_name, MechArmDataException)
     elif class_name in ["MechArm270", "MechArmSocket"]:
@@ -1154,7 +1176,7 @@ def calibration_parameters(**kwargs):
             elif parameter == 'gripper_type':
                 check_0_or_1(parameter, value, [1, 3, 4], value_type, MyPalletizer260DataException, int)
             elif parameter == '_type_1':
-                check_0_or_1(parameter, value, [1, 2, 3, 4], value_type, MyPalletizer260DataException, int)
+                check_0_or_1(parameter, value, [1, 2, 3, 4, 5], value_type, MyPalletizer260DataException, int)
                 # if value not in [0, 1, 10]:
                 #     raise exception_class("The data supported by parameter {} is 0 or 1 or 10, but the received value is {}".format(parameter, value))
             elif parameter == 'gripper_value':

pymycobot/mycobot320.py

@@ -576,46 +576,67 @@ def get_end_type(self):
         """
         return self._mesg(ProtocolCode.GET_END_TYPE, has_reply=True)
 
-    # Force Control Gripper
-    def set_pro_gripper(self, gripper_id, address, value):
+    # Force Control Gripper and myHand  Gripper Control
+    def set_pro_gripper(self, gripper_id, address, joint_id=0, value=None, speed=None):
         """Setting the force-controlled gripper parameters
 
         Args:
             gripper_id (int): 1 ~ 254
             address (int): Corresponding to the command sequence number in the force-controlled gripper protocol
             value : Parameters in the force-controlled gripper protocol
         """
-        self.calibration_parameters(class_name=self.__class__.__name__, set_gripper_args=[gripper_id, address, value])
-        return self._mesg(ProtocolCode.SET_TOQUE_GRIPPER, gripper_id, [address], [value])
+        if value is None:
+            # self.calibration_parameters(class_name=self.__class__.__name__,
+            #                             set_gripper_args=[gripper_id, address], joint_id=joint_id)
+            return self._mesg(ProtocolCode.SET_TOQUE_GRIPPER, gripper_id, [address], [joint_id])
+        elif speed:
+            self.calibration_parameters(class_name=self.__class__.__name__,
+                                        set_gripper_args=[gripper_id, address, value], joint_id=joint_id, speed=speed)
+            return self._mesg(ProtocolCode.SET_TOQUE_GRIPPER, gripper_id, [address], [joint_id], [value], [speed])
+        else:
+            self.calibration_parameters(class_name=self.__class__.__name__,
+                                        set_gripper_args=[gripper_id, address, value], joint_id=joint_id)
+            return self._mesg(ProtocolCode.SET_TOQUE_GRIPPER, gripper_id, [address], [joint_id], [value])
 
-    def get_pro_gripper(self, gripper_id, address):
+    def get_pro_gripper(self, gripper_id, address, joint_id=0):
         """Get the force-controlled gripper parameters
 
         Args:
             gripper_id (int): 1 ~ 254
             address (int): Corresponding to the command sequence number in the force-controlled gripper protocol
         """
-        self.calibration_parameters(class_name=self.__class__.__name__, get_gripper_args=[gripper_id, address])
-        return self._mesg(ProtocolCode.GET_TOQUE_GRIPPER, gripper_id, [address])
+        self.calibration_parameters(class_name=self.__class__.__name__, get_gripper_args=[gripper_id, address], joint_id=joint_id)
+        return self._mesg(ProtocolCode.GET_TOQUE_GRIPPER, gripper_id, [address], [joint_id])
 
-    def set_pro_gripper_angle(self, gripper_id, gripper_angle):
+    def set_pro_gripper_angle(self, gripper_id, joint_id=0, gripper_angle=None):
         """ Setting the angle of the force-controlled gripper
 
         Args:
             gripper_id (int): 1 ~ 254
             gripper_angle (int): 0 ~ 100
         """
         self.calibration_parameters(class_name=self.__class__.__name__, gripper_angle=[gripper_id, gripper_angle])
-        return self.set_pro_gripper(gripper_id, ProGripper.SET_GRIPPER_ANGLE, gripper_angle)
+        return self.set_pro_gripper(gripper_id, ProGripper.SET_GRIPPER_ANGLE, joint_id=joint_id, value=gripper_angle)
+
+    def set_pro_gripper_angles(self, gripper_id, gripper_angles, speed):
+        """ Setting the angle of the force-controlled gripper
+
+        Args:
+            gripper_id (int): 1 ~ 254
+            gripper_angles (list): 0 ~ 100
+            speed (int): 0 ~ 100
+        """
+        # self.calibration_parameters(class_name=self.__class__.__name__, gripper_angle=[gripper_id, gripper_angles])
+        return self.set_pro_gripper(gripper_id, ProGripper.SET_GRIPPER_ANGLES, value=gripper_angles, speed=speed)
 
-    def get_pro_gripper_angle(self, gripper_id):
+    def get_pro_gripper_angle(self, gripper_id, joint_id=0):
         """ Setting the angle of the force-controlled gripper
 
         Return:
             gripper_id (int): 1 ~ 254
         """
         self.calibration_parameters(class_name=self.__class__.__name__, gripper_id=gripper_id)
-        return self.get_pro_gripper(gripper_id, ProGripper.GET_GRIPPER_ANGLE)
+        return self.get_pro_gripper(gripper_id, ProGripper.GET_GRIPPER_ANGLE, joint_id=joint_id)
 
     def set_pro_gripper_open(self, gripper_id):
         """ Open force-controlled gripper
@@ -637,15 +658,15 @@ def set_pro_gripper_close(self, gripper_id):
         self.calibration_parameters(class_name=self.__class__.__name__, gripper_id=gripper_id)
         return self.set_pro_gripper_angle(gripper_id, 0)
 
-    def set_pro_gripper_calibration(self, gripper_id):
+    def set_pro_gripper_calibration(self, gripper_id, joint_id=0):
         """ Setting the gripper jaw zero position
 
         Args:
             gripper_id (int): 1 ~ 254
 
         """
         self.calibration_parameters(class_name=self.__class__.__name__, gripper_id=gripper_id)
-        return self.set_pro_gripper(gripper_id, ProGripper.SET_GRIPPER_CALIBRATION, 0)
+        return self.set_pro_gripper(gripper_id, ProGripper.SET_GRIPPER_CALIBRATION, joint_id=joint_id)
 
     def get_pro_gripper_status(self, gripper_id):
         """ Get the clamping status of the gripper