1.Add an interface for setting a new bio gripper；2.Added "get_tool_digital_input" interface；3."get_max_joint_velocity" interface adds parameter whether to return radian value；4."set_tgpio_digital_with_xyz" interface adds access TO3/4；5."set_collision_tool_model interface" adds setting model offset value；6.Blcockly added module for obtaining Euler angles/axis angles and joint angles；7.Blockly added a module to detect whether the bio gripper is grasping and the position of the position triggering IO

Author: SchoolMater

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.position_aa[{}], 2)'.format(direction-1)
+            else:
+                return 'round(self._arm.position[{}], 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)
 
@@ -654,15 +747,15 @@ def _handle_set_lite6_gripper(self, block, indent=0, arg_map=None):
 
     def _handle_gripper_set(self, block, indent=0, arg_map=None):
         fields = self._get_nodes('field', root=block)
-        if fields is not None and len(fields) >= 3:
+        if fields is not None and len(fields) >= 4:
             pos = fields[0].text
             speed = fields[1].text
-            wait = fields[2].text == 'TRUE'
+            wait = fields[3].text == 'TRUE'
         else:
             values = self._get_nodes('value', root=block)
             pos = self._get_nodes('field', root=values[0], descendant=True)[0].text
             speed = self._get_nodes('field', root=values[1], descendant=True)[0].text
-            wait = self._get_nodes('field', root=values[2], descendant=True)[0].text == 'TRUE'
+            wait = self._get_nodes('field', root=values[3], descendant=True)[0].text == 'TRUE'
         self._append_main_code('code = self._arm.set_gripper_position({}, wait={}, speed={}, auto_enable=True)'.format(pos, wait, speed), indent + 2)
         self._append_main_code('if not self._check_code(code, \'set_gripper_position\'):', 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)

xarm/version.py

@@ -1 +1 @@
-__version__ = '1.14.8'
+__version__ = '1.14.9'

xarm/wrapper/xarm_api.py

@@ -2802,6 +2802,47 @@ def set_bio_gripper_speed(self, speed):
         """
         return self._arm.set_bio_gripper_speed(speed)
 
+    def set_bio_gripper_control_mode(self, mode):
+        """
+        Set the bio gripper control 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.
+        """
+        
+        return self._arm.set_bio_gripper_control_mode(mode)
+
+    def set_bio_gripper_force(self, force):
+        """
+        Set the bio gripper force
+
+        :param force: gripper force between 10 and 100
+
+        :return: code
+            code: See the [API Code Documentation](./xarm_api_code.md#api-code) for details.
+        """
+
+        return self._arm.set_bio_gripper_force(force)
+
+    def set_bio_gripper_position(self, pos, speed=0, force=100, wait=True, timeout=5, **kwargs):
+        """
+        Set the bio gripper position
+
+        :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: tuple((code, robotiq_response))
+            code: See the [API Code Documentation](./xarm_api_code.md#api-code) for details.
+            robotiq_response: See the robotiq documentation
+        """
+        return self._arm.set_bio_gripper_position(pos, speed=speed, force=force, wait=wait, timeout=timeout, **kwargs)
+
     def open_bio_gripper(self, speed=0, wait=True, timeout=5, **kwargs):
         """
         Open the bio gripper