Merge branch 'develop' of 192.168.1.19:vinman/xArm-Python-SDK into develop

Author: vimior

doc/api/xarm_api.md

@@ -1196,6 +1196,16 @@ xArm-Python-SDK API Documentation (V1.15.0): class XArmAPI in module xarm.wrappe
 >     code: See the [API Code Documentation](./xarm_api_code.md#api-code) for details.
 
 
+#### def __get_tool_digital_input__(self, ionum=None):
+
+> Get the digital value of the specified Tool GPIO,Compared with the "get_tgpio_digital" interface,  
+>     the value of TI2 is obtained when the ionum is not transmitted.  
+>   
+> :param ionum: 0 or 1 or or 2 or 3 or 4 (both 0 and 4), default is None  
+> :return: tuple((code, value or value list)), only when code is 0, the returned result is correct.  
+>     code: See the [API Code Documentation](./xarm_api_code.md#api-code) for details.
+
+
 #### def __get_traj_speeding__(self, rate):
 
 > Obtain the joint and velocity values of joint overspeed during trajectory recording  
@@ -2049,11 +2059,12 @@ xArm-Python-SDK API Documentation (V1.15.0): class XArmAPI in module xarm.wrappe
 
 #### def __set_bio_gripper_control_mode__(self, mode):
 
-> Set the mode of the bio gripper  
+> Set the bio gripper control mode  
 > Note:  
 >     1. Only available in the new version of BIO Gripper  
 >   
-> :param mode: mode  
+> :param mode: 0: bio gripper opening and closing mode  
+>              1: position loop mode  
 >   
 > :return: code  
 >     code: See the [API Code Documentation](./xarm_api_code.md#api-code) for details.
@@ -2073,30 +2084,29 @@ xArm-Python-SDK API Documentation (V1.15.0): class XArmAPI in module xarm.wrappe
 
 #### def __set_bio_gripper_force__(self, force):
 
-> Set the force of the bio gripper  
+> Set the bio gripper force  
 > Note:  
 >     1. Only available in the new version of BIO Gripper  
 >   
-> :param force: force (1-100)  
+> :param force: gripper force between 10 and 100  
 >   
 > :return: code  
 >     code: See the [API Code Documentation](./xarm_api_code.md#api-code) for details.
 
 
 #### def __set_bio_gripper_position__(self, pos, speed=0, force=100, wait=True, timeout=5, **kwargs):
 
-> Set the position of the bio gripper  
-> Note:  
->     1. Only available in the new version of BIO Gripper  
+> Set the bio gripper position  
 >   
-> :param pos: position(70-150)  
-> :param speed: speed value, default is 0 (not set the speed)  
-> :param force: force value, default is 100  
-> :param wait: whether to wait for the bio gripper motion complete, default is True  
+> :param pos: gripper pos between 71 and 150  
+> :param speed: gripper speed between 0 and 4500  
+> :param force: gripper force between 10 and 100  
+> :param wait: whether to wait for the robotiq motion to complete, default is True  
 > :param timeout: maximum waiting time(unit: second), default is 5, only available if wait=True  
 >   
-> :return: code  
->     code: See the [API Code Documentation](./xarm_api_code.md#api-code) for details.
+> :return: tuple((code, robotiq_response))  
+>     code: See the [API Code Documentation](./xarm_api_code.md#api-code) for details.  
+>     robotiq_response: See the robotiq documentation
 
 
 #### def __set_bio_gripper_speed__(self, speed):

xarm/tools/blockly/_blockly_base.py

@@ -224,6 +224,24 @@ def _get_condition_expression(self, value_block, arg_map=None):
             addr = int(fields[0].text.replace(' ', '').replace('0x', '').replace(',', '').replace('\xa0', ''), 16)
             return "list(map(lambda x: hex(x).split('0x')[1].upper().zfill(4)[:2] + ' ' + hex(x).split('0x')[1]." \
                    "upper().zfill(4)[2:], self._arm.read_holding_registers({}, 1)[1]))[0]".format(str(addr))
+        elif block.attrib['type'] == 'get_position':
+            direction_li = ['X', 'Y', 'Z', 'R', 'P', 'Y']
+            fields = self._get_nodes('field', root=block)
+            is_axis = True if fields[0].text == 'axis' else False
+            direction = int(fields[1].text)
+            if is_axis:
+                return 'round(self._arm.get_position_aa()[1][{}], 2)'.format(direction-1)
+            else:
+                return 'round(self._arm.get_position()[1][{}], 2)'.format(direction-1)
+        elif block.attrib['type'] == 'get_joint_angle':
+            angle_li = ['J1', 'J2', 'J3', 'J4', 'J5', 'J6']
+            fields = self._get_nodes('field', root=block)
+            servo_angle = fields[0].text
+            return 'round(self._arm.get_servo_angle(servo_id={})[1], 2)'.format(servo_angle)
+        elif block.attrib['type'] == 'check_bio_g2_gripper_is_catch':
+            fields = self._get_nodes('field', root=block)
+            timeout = float(fields[0].text)
+            return 'self._arm.arm.check_bio_gripper_is_catch(timeout={})'.format(timeout)
 
 
     def __get_logic_compare(self, block, arg_map=None):

xarm/tools/blockly/_blockly_handler.py

@@ -8,6 +8,7 @@
 
 import json
 import re
+import math
 from ._blockly_base import _BlocklyBase, OPS_MAP
 from ._blockly_highlight import HIGHLIGHT_BLOCKS
 
@@ -417,8 +418,9 @@ def _handle_gpio_set_digital(self, block, indent=0, arg_map=None):
         fields = self._get_nodes('field', root=block)
         io = fields[0].text
         value = 0 if fields[1].text == 'LOW' else 1
-        delay_sec = fields[2].text if len(fields) > 2 else 0
-        self._append_main_code('code = self._arm.set_tgpio_digital({}, {}, delay_sec={})'.format(io, value, delay_sec), indent + 2)
+        is_sync = fields[2].text if len(fields) > 2 else 'True'
+        delay_sec = fields[3].text if len(fields) > 3 else 0
+        self._append_main_code('code = self._arm.set_tgpio_digital({}, {}, delay_sec={}, sync={})'.format(io, value, delay_sec, is_sync), indent + 2)
         self._append_main_code('if not self._check_code(code, \'set_tgpio_digital\'):', indent + 2)
         self._append_main_code('    return', indent + 2)
 
@@ -435,14 +437,36 @@ def _handle_gpio_set_digital_with_xyz(self, block, indent=0, arg_map=None):
         self._append_main_code('if not self._check_code(code, \'set_tgpio_digital_with_xyz\'):', indent + 2)
         self._append_main_code('    return', indent + 2)
 
+    def _handle_gpio_set_digital_with_xyz_var(self, block, indent=0, arg_map=None):
+        value_nodes = self._get_nodes('value', root=block)
+        x = self._get_block_val(value_nodes[0], arg_map)
+        y = self._get_block_val(value_nodes[1], arg_map)
+        z = self._get_block_val(value_nodes[2], arg_map)
+        xyz_li = [x, y, z]
+        xyz = []
+        for i in range(3):
+            if re.match('\d+(\.\d+)*$', xyz_li[i]) is not None:
+                xyz.append(float(xyz_li[i]))
+            else:
+                xyz.append(math.log2(3) if i == 0 else math.log2(5) if i == 1 else math.log2(7))
+        xyz = str(xyz).replace(str(math.log2(3)), xyz_li[0]).replace(str(math.log2(5)), xyz_li[1]).replace(
+            str(math.log2(7)), xyz_li[2])
+        tol_r = self._get_block_val(value_nodes[3], arg_map)
+        io = self._get_block_val(value_nodes[4], arg_map)
+        value = 0 if self._get_block_val(value_nodes[5], arg_map) == 'LOW' else 1
+        self._append_main_code(
+            'code = self._arm.set_tgpio_digital_with_xyz({}, {}, {}, {})'.format(io, value, xyz, tol_r), indent + 2)
+        self._append_main_code('if not self._check_code(code, \'set_tgpio_digital_with_xyz\'):', indent + 2)
+        self._append_main_code('    return', indent + 2)
+
     def _handle_get_suction_cup(self, block, indent=0, arg_map=None):
         self._append_main_code('{}self._arm.get_suction_cup(hardware_version={})'.format('{}', self._vacuum_version), indent + 2)
 
     def _handle_check_air_pump_state(self, block, indent=0, arg_map=None):
         fields = self._get_nodes('field', root=block)
         state = 1 if fields[0].text == 'ON' else 0
         timeout = float(fields[1].text)
-        self._append_main_code('self._arm.arm.check_air_pump_state({}, timeout={})'.format(state, timeout), indent + 2)
+        self._append_main_code('self._arm.arm.check_air_pump_state({}, timeout={}, hardware_version={})'.format(state, timeout, self._vacuum_version), indent + 2)
 
     def _handle_check_bio_gripper_is_catch(self, block, indent=0, arg_map=None):
         fields = self._get_nodes('field', root=block)
@@ -464,7 +488,7 @@ def _handle_set_suction_cup(self, block, indent=0, arg_map=None):
             wait = False
         fields = self._get_nodes('field', root=block, name='delay')
         delay_sec = fields[0].text if len(fields) > 0 else 0
-        self._append_main_code('code = self._arm.set_suction_cup({}, wait={}, delay_sec={}, hardware_version={})'.format( on, wait, delay_sec, self._vacuum_version), indent + 2)
+        self._append_main_code('code = self._arm.set_suction_cup({}, wait={}, delay_sec={}, hardware_version={})'.format(on, wait, delay_sec, self._vacuum_version), indent + 2)
         self._append_main_code('if not self._check_code(code, \'set_suction_cup\'):', indent + 2)
         self._append_main_code('    return', indent + 2)
 
@@ -511,8 +535,9 @@ def _handle_gpio_set_controller_digital(self, block, indent=0, arg_map=None):
         fields = self._get_nodes('field', root=block)
         io = fields[0].text
         value = 0 if fields[1].text == 'LOW' else 1
-        delay_sec = fields[2].text if len(fields) > 2 else 0
-        self._append_main_code('code = self._arm.set_cgpio_digital({}, {}, delay_sec={})'.format(io, value, delay_sec), indent + 2)
+        is_sync = fields[2].text if len(fields) > 2 else 'True'
+        delay_sec = fields[3].text if len(fields) > 3 else 0
+        self._append_main_code('code = self._arm.set_cgpio_digital({}, {}, delay_sec={}, sync={})'.format(io, value, delay_sec, is_sync), indent + 2)
         self._append_main_code('if not self._check_code(code, \'set_cgpio_digital\'):', indent + 2)
         self._append_main_code('    return', indent + 2)
 
@@ -525,6 +550,27 @@ def _handle_gpio_set_controller_digital_with_xyz(self, block, indent=0, arg_map=
         tol_r = fields[3].text
         io = fields[4].text
         value = 0 if fields[5].text == 'LOW' else 1
+        self._append_main_code(
+            'code = self._arm.set_cgpio_digital_with_xyz({}, {}, {}, {})'.format(io, value, xyz, tol_r), indent + 2)
+        self._append_main_code('if not self._check_code(code, \'set_cgpio_digital_with_xyz\'):', indent + 2)
+        self._append_main_code('    return', indent + 2)
+
+    def _handle_gpio_set_controller_digital_with_xyz_var(self, block, indent=0, arg_map=None):
+        value_nodes = self._get_nodes('value', root=block)
+        x = self._get_block_val(value_nodes[0], arg_map)
+        y = self._get_block_val(value_nodes[1], arg_map)
+        z = self._get_block_val(value_nodes[2], arg_map)
+        xyz_li = [x, y, z]
+        xyz = []
+        for i in range(3):
+            if re.match('\d+(\.\d+)*$', xyz_li[i]) is not None:
+                xyz.append(float(xyz_li[i]))
+            else:
+                xyz.append(math.log2(3) if i == 0 else math.log2(5) if i == 1 else math.log2(7))
+        xyz = str(xyz).replace(str(math.log2(3)), xyz_li[0]).replace(str(math.log2(5)), xyz_li[1]).replace(str(math.log2(7)), xyz_li[2])
+        tol_r = self._get_block_val(value_nodes[3], arg_map)
+        io = self._get_block_val(value_nodes[4], arg_map)
+        value = 0 if self._get_block_val(value_nodes[5], arg_map) == 'LOW' else 1
         self._append_main_code('code = self._arm.set_cgpio_digital_with_xyz({}, {}, {}, {})'.format(io, value, xyz, tol_r), indent + 2)
         self._append_main_code('if not self._check_code(code, \'set_cgpio_digital_with_xyz\'):', indent + 2)
         self._append_main_code('    return', indent + 2)
@@ -533,20 +579,43 @@ def _handle_gpio_set_controller_digital_do(self, block, indent=0, arg_map=None):
         fields = self._get_nodes('field', root=block)
         io = fields[0].text
         value = 0 if fields[1].text == 'LOW' else 1
-        delay_sec = fields[2].text if len(fields) > 2 else 0
-        self._append_main_code('code = self._arm.set_cgpio_digital({}, {}, delay_sec={})'.format(io, value, delay_sec), indent + 2)
-        self._append_main_code('if not self._check_code(code, \'set_suction_cup\'):', indent + 2)
+        is_sync = fields[2].text if len(fields) > 2 else 'True'
+        delay_sec = fields[3].text if len(fields) > 3 else 0
+        self._append_main_code('code = self._arm.set_cgpio_digital({}, {}, delay_sec={}, sync={})'.format(io, value, delay_sec, is_sync), indent + 2)
+        self._append_main_code('if not self._check_code(code, \'set_cgpio_digital\'):', indent + 2)
         self._append_main_code('    return', indent + 2)
 
     def _handle_gpio_set_controller_digital_with_xyz_do(self, block, indent=0, arg_map=None):
-        fields = self._get_nodes('field', root=block)
-        x = fields[0].text
-        y = fields[1].text
-        z = fields[2].text
+        value_nodes = self._get_nodes('value', root=block)
+        x = self._get_block_val(value_nodes[0], arg_map)
+        y = self._get_block_val(value_nodes[1], arg_map)
+        z = self._get_block_val(value_nodes[2], arg_map)
         xyz = list(map(float, [x, y, z]))
-        tol_r = fields[3].text
-        io = fields[4].text
-        value = 0 if fields[5].text == 'LOW' else 1
+        tol_r = self._get_block_val(value_nodes[3], arg_map)
+        io = self._get_block_val(value_nodes[4], arg_map)
+        value = 0 if self._get_block_val(value_nodes[5], arg_map) == 'LOW' else 1
+        self._append_main_code(
+            'code = self._arm.set_cgpio_digital_with_xyz({}, {}, {}, {})'.format(io, value, xyz, tol_r), indent + 2)
+        self._append_main_code('if not self._check_code(code, \'set_cgpio_digital_with_xyz\'):', indent + 2)
+        self._append_main_code('    return', indent + 2)
+        
+    def _handle_gpio_set_controller_digital_with_xyz_do_var(self, block, indent=0, arg_map=None):
+        value_nodes = self._get_nodes('value', root=block)
+        x = self._get_block_val(value_nodes[0], arg_map)
+        y = self._get_block_val(value_nodes[1], arg_map)
+        z = self._get_block_val(value_nodes[2], arg_map)
+        tol_r = self._get_block_val(value_nodes[3], arg_map)
+        io = self._get_block_val(value_nodes[4], arg_map)
+        value = 0 if self._get_block_val(value_nodes[5], arg_map) == 'LOW' else 1
+        xyz_li = [x, y, z]
+        xyz = []
+        for i in range(3):
+            if re.match('\d+(\.\d+)*$', xyz_li[i]) is not None:
+                xyz.append(float(xyz_li[i]))
+            else:
+                xyz.append(math.log2(3) if i == 0 else math.log2(5) if i == 1 else math.log2(7))
+        xyz = str(xyz).replace(str(math.log2(3)), xyz_li[0]).replace(str(math.log2(5)), xyz_li[1]).replace(
+            str(math.log2(7)), xyz_li[2])
         self._append_main_code('code = self._arm.set_cgpio_digital_with_xyz({}, {}, {}, {})'.format(io, value, xyz, tol_r), indent + 2)
         self._append_main_code('if not self._check_code(code, \'set_cgpio_digital_with_xyz\'):', indent + 2)
         self._append_main_code('    return', indent + 2)
@@ -564,11 +633,35 @@ def _handle_gpio_set_controller_analog_with_xyz(self, block, indent=0, arg_map=N
         self._append_main_code('if not self._check_code(code, \'set_cgpio_analog_with_xyz\'):', indent + 2)
         self._append_main_code('    return', indent + 2)
 
+    def _handle_gpio_set_controller_analog_with_xyz_var(self, block, indent=0, arg_map=None):
+        value_nodes = self._get_nodes('value', root=block)
+        x = self._get_block_val(value_nodes[0], arg_map)
+        y = self._get_block_val(value_nodes[1], arg_map)
+        z = self._get_block_val(value_nodes[2], arg_map)
+        # xyz = list(map(lambda x: float(x) if re.match('\d+(\.\d+)*$', x) is not None else [x, y, z].index(x), [x, y, z]))
+        xyz_li = [x, y, z]
+        xyz = []
+        for i in range(3):
+            if re.match('\d+(\.\d+)*$', xyz_li[i]) is not None:
+                xyz.append(float(xyz_li[i]))
+            else:
+                xyz.append(math.log2(3) if i == 0 else math.log2(5) if i == 1 else math.log2(7))
+        xyz = str(xyz).replace(str(math.log2(3)), xyz_li[0]).replace(str(math.log2(5)), xyz_li[1]).replace(
+            str(math.log2(7)), xyz_li[2])
+        tol_r = self._get_block_val(value_nodes[3], arg_map)
+        io = self._get_block_val(value_nodes[4], arg_map)
+        value = self._get_block_val(value_nodes[5], arg_map)
+        self._append_main_code('code = self._arm.set_cgpio_analog_with_xyz({}, {}, {}, {})'.format(io, value, xyz, tol_r), indent + 2)
+        self._append_main_code('if not self._check_code(code, \'set_cgpio_analog_with_xyz\'):', indent + 2)
+        self._append_main_code('    return', indent + 2)
+
     def _handle_gpio_set_controller_analog(self, block, indent=0, arg_map=None):
-        io = self._get_node('field', block).text
+        fields = self._get_nodes('field', root=block)
+        io = fields[0].text
+        is_sync = fields[1].text
         value = self._get_node('value', root=block)
         value = self._get_block_val(value, arg_map=arg_map)
-        self._append_main_code('code = self._arm.set_cgpio_analog({}, {})'.format(io, value), indent + 2)
+        self._append_main_code('code = self._arm.set_cgpio_analog({}, {}, sync={})'.format(io, value, is_sync), indent + 2)
         self._append_main_code('if not self._check_code(code, \'set_cgpio_analog\'):', indent + 2)
         self._append_main_code('    return', indent + 2)
 
@@ -686,6 +779,11 @@ def _handle_gripper_set_status(self, block, indent=0, arg_map=None):
         self._append_main_code('if not self._check_code(code, \'set_gripper_status\'):', indent + 2)
         self._append_main_code('    return', indent + 2)
 
+    def _handle_set_bio_g2_gripper_init(self, block, indent=0, arg_map=None):
+        self._append_main_code('code = self._arm.set_bio_gripper_enable(True)', indent + 2)
+        self._append_main_code('if not self._check_code(code, \'set_bio_gripper_enable\'):', indent + 2)
+        self._append_main_code('    return', indent + 2)
+
     def _handle_set_bio_gripper_init(self, block, indent=0, arg_map=None):
         self._append_main_code('code = self._arm.set_bio_gripper_enable(True)', indent + 2)
         self._append_main_code('if not self._check_code(code, \'set_bio_gripper_enable\'):', indent + 2)
@@ -707,6 +805,16 @@ def _handle_set_bio_gripper(self, block, indent=0, arg_map=None):
             self._append_main_code('if not self._check_code(code, \'close_bio_gripper\'):', indent + 2)
             self._append_main_code('    return', indent + 2)
 
+    def _handle_set_bio_gripper_pos_force(self, block, indent=0, arg_map=None):
+        fields = self._get_nodes('field', root=block)
+        pos = fields[0].text
+        speed = fields[1].text
+        force = fields[2].text
+        wait = fields[3].text == 'TRUE'
+        self._append_main_code('code = self._arm.set_bio_gripper_position(pos={}, speed={}, force={}, wait={})'.format(pos, speed, force, wait), indent + 2)
+        self._append_main_code('if not self._check_code(code, \'open_bio_gripper\'):', indent + 2)
+        self._append_main_code('    return', indent + 2)
+
     def _handle_set_robotiq_init(self, block, indent=0, arg_map=None):
         self._append_main_code('code, ret = self._arm.robotiq_reset()', indent + 2)
         self._append_main_code('if not self._check_code(code, \'robotiq_reset\'):', indent + 2)
@@ -1124,6 +1232,7 @@ def _handle_python_code(self, block, indent=0, arg_map=None, **kwargs):
             if (self._is_exec or (not self._is_exec and not self._is_ide)) and code.strip() and code not in \
                     ['finally:', 'else:'] and all([i not in code for i in ['elif', 'except', 'def', 'class']]) \
                     and not code.startswith('@'):
+                # 只有在studio执行blockly时或者SDK直接调用run_blockly_app时才会执行
                 code_indent = re.match('(\s*).*', code).group(1)
                 self._append_main_code(code_indent + 'if not self.is_alive:', indent + 2)
                 self._append_main_code(code_indent + 'return', indent + 3)