fix:1. The blockly object has been clamped and the control has been changed to continuously detect the grasped state; 2. blockly enables judgment to obtain the torque sensor value; 3. Python IDE is compatible with running the blockly module; 4. blockly's xarm5 end leveling repair; 5. blockly trajectory Record to determine whether the file name is empty; 6. blockly sends the modbus command module to return a character; 7. detects that Blockly contains a counter control before printing the counter value

Author: SchoolMater

xarm/tools/blockly/_blockly_base.py

@@ -202,17 +202,19 @@ def _get_condition_expression(self, value_block, arg_map=None):
             cmd_li = re.sub('\s+', ' ', cmd_li)
             cmd_li = cmd_li.strip().split(' ')
             int_li = [int(da, 16) for da in cmd_li]
-            return '[data if isinstance(data, int) else [hex(da).split("0x")[1].upper().zfill(2) for da in data] for ' \
-                   'data in self._arm.getset_tgpio_modbus_data({}, host_id={})]'.format(int_li, host_id)
+            return '[" ".join([hex(da).split("0x")[1].upper().zfill(2) for da in data]) if isinstance(data, list) else data for ' \
+                   'data in self._arm.getset_tgpio_modbus_data({}, host_id={})][-1]'.format(int_li, host_id)
         elif block.attrib['type'] == 'get_gripper_status':
             fields = self._get_nodes('field', root=block)
             timeout = fields[0].text
-            return '0 == self._arm.arm.check_gripper_status()'
+            return '0 == self._arm.arm.check_catch_gripper_status({})'.format(timeout)
         elif block.attrib['type'] == 'get_ft_sensor':
             direction_li = ['Fx', 'Fy', 'Fz', 'Tx', 'Ty', 'Tz']
             fields = self._get_nodes('field', root=block)
             direction = fields[0].text
-            return 'self._arm.get_joints_torque()[1][{}]'.format(direction_li.index(direction))
+            return '[self._arm.ft_sensor_enable(1), self._arm.set_state(0), self._arm.get_ft_sensor_data()[1][{}]][2] if' \
+                   ' self._arm.get_ft_sensor_config()[1][1] == 0 else self._arm.get_ft_sensor_data()[1][{}]'.\
+                format(direction_li.index(direction), direction_li.index(direction))
         elif block.attrib['type'] == 'get_counter':
             return 'self._arm.count'
 

xarm/tools/blockly/_blockly_handler.py

@@ -36,6 +36,8 @@ def __init__(self, xml_path):
         self._count_callbacks= []
 
         self.is_run_linear_track = False
+        self._is_run_blockly = False
+        self._listen_counter = False
 
     def _append_init_code(self, code):
         self._init_code_list.append(code)
@@ -100,11 +102,13 @@ def _handle_set_angle_acceleration(self, block, indent=0, arg_map=None):
         self._append_main_code('self._angle_acc = {}'.format(self._get_field_value(block)), indent + 2)
 
     def _handle_set_counter_increase(self, block, indent=0, arg_map=None):
+        self._listen_counter = True
         self._append_main_code('code = self._arm.set_counter_increase()', indent + 2)
         self._append_main_code('if not self._check_code(code, \'set_counter_increase\'):', indent + 2)
         self._append_main_code('    return', indent + 2)
 
     def _handle_set_counter_reset(self, block, indent=0, arg_map=None):
+        self._listen_counter = True
         self._append_main_code('code = self._arm.set_counter_reset()', indent + 2)
         self._append_main_code('if not self._check_code(code, \'set_counter_reset\'):', indent + 2)
         self._append_main_code('    return', indent + 2)
@@ -337,9 +341,12 @@ def _handle_studio_run_traj(self, block, indent=0, arg_map=None):
         filename = fields[0].text
         speed = fields[1].text
         times = fields[2].text
-        self._append_main_code('code = self._arm.playback_trajectory(times={}, filename=\'{}\', wait=True, double_speed={})'.format(times, filename, speed), indent + 2)
-        self._append_main_code('if not self._check_code(code, \'playback_trajectory\'):', indent + 2)
-        self._append_main_code('    return', indent + 2)
+        if filename:
+            self._append_main_code('code = self._arm.playback_trajectory(times={}, filename=\'{}\', wait=True, double_speed={})'.format(times, filename, speed), indent + 2)
+            self._append_main_code('if not self._check_code(code, \'playback_trajectory\'):', indent + 2)
+            self._append_main_code('    return', indent + 2)
+        else:
+            self._append_main_code('pass', indent + 2)
 
     def _handle_app_studio_traj(self, block, indent=0, arg_map=None):
         fields = self._get_nodes('field', root=block)
@@ -1089,11 +1096,12 @@ def _handle_python_code(self, block, indent=0, arg_map=None, **kwargs):
 
     def _handle_set_end_level(self, block, indent=0, arg_map=None, **kwargs):
         if not self.axis_type:
-            return 
+            return
+        self._append_main_code('self._arm.arm.wait_move()', indent + 2)
         self._append_main_code('current_angle = self._arm.angles', indent + 2)
         if self.axis_type[0] == 5:
             self._append_main_code('angle = -(current_angle[1] + current_angle[2])', indent + 2)
-            self._append_main_code('code = self._arm.set_servo_angle(servo_id=4, angle=angle)', indent + 2,)
+            self._append_main_code('code = self._arm.set_servo_angle(angle=[*current_angle[:3], angle, current_angle[4]])', indent + 2,)
         elif self.axis_type[0] == 6:
             self._append_main_code('angle_5 = {}'.format('-(current_angle[1] - current_angle[2])' if self.axis_type[1]==9 or self.axis_type[1]==12 else
                                                        '-(current_angle[1] + current_angle[2])'), indent + 2)
@@ -1138,7 +1146,7 @@ def _handle_set_ft_sensor(self, block, indent=0, arg_map=None):
         for index, axis in enumerate(force_axis_list):
             if axis == force_axis:
                 force_axis_value[index] = 1
-                force_ref_value[index] = int(force_ref)
+                force_ref_value[index] = float(force_ref)
         self._append_main_code('code = self._arm.config_force_control({}, {}, {}, [0] * 6)'.format(ref_frame, force_axis_value,
                                force_ref_value), indent + 2)
         self._append_main_code('if not self._check_code(code, \'set_tgpio_modbus\'):', indent + 2)
@@ -1150,7 +1158,7 @@ def _handle_set_ft_sensor(self, block, indent=0, arg_map=None):
         self._append_main_code('self._arm.ft_sensor_app_set(2)', indent + 2)
         self._append_main_code('self._arm.set_state(0)', indent + 2)
         self._append_main_code('start_time = time.time()', indent + 2)
-        self._append_main_code('while time.time() - start_time > {}:'.format(wait_time), indent + 2)
+        self._append_main_code('while time.time() - start_time < {}:'.format(wait_time), indent + 2)
         self._append_main_code('if self._arm.error_code != 0:', indent + 3)
         self._append_main_code('    return', indent + 4)
         self._append_main_code('self._arm.ft_sensor_app_set(0)', indent + 2)
@@ -1160,7 +1168,11 @@ def _handle_studio_run_blockly(self, block, indent=0, arg_map=None):
         projectName = fields[0].text
         fileName = fields[1].text
         times = fields[2].text
-        self._append_main_code('start_run_blockly(fileName="{}", times={})'.format(fileName, times), indent + 2)
+        if not self._is_exec:
+            self._append_main_code('self._start_run_blockly(fileName="{}", times={})'.format(fileName.lstrip('/'), times), indent + 2)
+        else:
+            self._append_main_code('start_run_blockly(fileName="{}", times={})'.format(fileName, times), indent + 2)
+        self._is_run_blockly = True
 
     def _handle_studio_run_gcode(self, block, indent=0, arg_map=None):
         fields = self._get_nodes('field', root=block)
@@ -1169,5 +1181,3 @@ def _handle_studio_run_gcode(self, block, indent=0, arg_map=None):
         times = fields[2].text
         self._append_main_code('start_run_gcode(projectName="{}", fileName="{}", times={})'.format(projectName, fileName, times), indent + 2)
 
-
-

xarm/tools/blockly/_blockly_tool.py

@@ -105,8 +105,9 @@ def _finish_robot_main_run_codes(self, error_exit=True, stop_exit=True):
                 self._append_main_code('            self._arm.release_error_warn_changed_callback(self._error_warn_changed_callback)', indent=-1)
             if stop_exit:
                 self._append_main_code('            self._arm.release_state_changed_callback(self._state_changed_callback)', indent=-1)
-        self._append_main_code('            if hasattr(self._arm, \'release_count_changed_callback\'):', indent=-1)
-        self._append_main_code('                self._arm.release_count_changed_callback(self._count_changed_callback)', indent=-1)
+        if self._listen_counter:
+            self._append_main_code('            if hasattr(self._arm, \'release_count_changed_callback\'):', indent=-1)
+            self._append_main_code('                self._arm.release_count_changed_callback(self._count_changed_callback)', indent=-1)
 
         # if self._listen_tgpio_digital or self._listen_tgpio_analog or self._listen_cgpio_state \
         #     or len(self._tgpio_digital_callbacks) or len(self._tgpio_analog_callbacks) or len(self._cgpio_digital_callbacks) or len(self._cgpio_analog_callbacks):
@@ -183,6 +184,7 @@ def _init_robot_main_class_codes(self, init=True, wait_seconds=1, mode=0, state=
         self.__define_callback_thread_func()
         self.__define_listen_gpio_thread_func()
         self.__define_listen_count_thread_func()
+        self.__define_run_blockly_func()
         self.__define_robot_init_func(init=init, wait_seconds=wait_seconds, mode=mode, state=state, error_exit=error_exit, stop_exit=stop_exit)
         self.__define_error_warn_changed_callback_func(error_exit=error_exit)
         self.__define_state_changed_callback_func(stop_exit=stop_exit)
@@ -327,6 +329,12 @@ def __define_listen_count_thread_func(self):
             self._append_main_init_code('            self._counter_val = values')
             self._append_main_init_code('            time.sleep(0.01)\n')
 
+    def __define_run_blockly_func(self):
+        if self._is_run_blockly and not self._is_exec:
+            self._append_main_init_code('    def _start_run_blockly(self, fileName, times):')
+            self._append_main_init_code('       for i in range(times):')
+            self._append_main_init_code('           self._arm.run_blockly_app(fileName, init=False, axis_type=[self._arm.axis, self._arm.device_type])\n')
+
     def __define_robot_init_func(self, init=True, wait_seconds=1, mode=0, state=0, error_exit=True, stop_exit=True):
         # Define XArm Init Function:
         self._append_main_init_code('    # Robot init')
@@ -343,9 +351,9 @@ def __define_robot_init_func(self, init=True, wait_seconds=1, mode=0, state=0, e
             self._append_main_init_code('        self._arm.register_error_warn_changed_callback(self._error_warn_changed_callback)')
         if stop_exit:
             self._append_main_init_code('        self._arm.register_state_changed_callback(self._state_changed_callback)')
-        
-        self._append_main_init_code('        if hasattr(self._arm, \'register_count_changed_callback\'):')
-        self._append_main_init_code('            self._arm.register_count_changed_callback(self._count_changed_callback)')
+        if self._listen_counter:
+            self._append_main_init_code('        if hasattr(self._arm, \'register_count_changed_callback\'):')
+            self._append_main_init_code('            self._arm.register_count_changed_callback(self._count_changed_callback)')
         self._append_main_init_code('')
 
     def __define_error_warn_changed_callback_func(self, error_exit=True):
@@ -372,10 +380,11 @@ def __define_state_changed_callback_func(self, stop_exit=True):
 
     def __define_count_changed_callback_func(self):
         # Define count changed callback
-        self._append_main_init_code('    # Register count changed callback')
-        self._append_main_init_code('    def _count_changed_callback(self, data):')
-        self._append_main_init_code('        if self.is_alive:')
-        self._append_main_init_code('            self.pprint(\'counter val: {}\'.format(data[\'count\']))\n')
+        if self._listen_counter:
+            self._append_main_init_code('    # Register count changed callback')
+            self._append_main_init_code('    def _count_changed_callback(self, data):')
+            self._append_main_init_code('        if self.is_alive:')
+            self._append_main_init_code('            self.pprint(\'counter val: {}\'.format(data[\'count\']))\n')
 
     def __define_pprint_func(self):
         self._append_main_init_code('    @staticmethod')

xarm/version.py

@@ -1 +1 @@
-__version__ = '1.13.28'
+__version__ = '1.13.29'

xarm/x3/gripper.py

@@ -425,7 +425,7 @@ def __check_gripper_position(self, target_pos, timeout=None):
             time.sleep(0.2)
         return code
 
-    def check_gripper_status(self, timeout=None):
+    def __check_gripper_status(self, timeout=None):
         start_move = False
         not_start_move_cnt = 0
         failed_cnt = 0
@@ -453,6 +453,34 @@ def check_gripper_status(self, timeout=None):
             time.sleep(0.1)
         return code
 
+    def check_catch_gripper_status(self, timeout=None):
+        start_move = False
+        not_start_move_cnt = 0
+        failed_cnt = 0
+        if not timeout or not isinstance(timeout, (int, float)) or timeout <= 0:
+            timeout = 10
+        expired = time.monotonic() + timeout
+        code = APIState.WAIT_FINISH_TIMEOUT
+        while self.connected and time.monotonic() < expired:
+            _, status = self.get_gripper_status()
+            failed_cnt = 0 if _ == 0 else failed_cnt + 1
+            if _ == 0:
+                if status & 0x03 == 2:
+                    if start_move:
+                        return 0
+                    else:
+                        not_start_move_cnt += 1
+                        if not_start_move_cnt > 2:
+                            return 0
+                elif not start_move:
+                    not_start_move_cnt = 0
+                    start_move = True
+            else:
+                if failed_cnt > 10:
+                    return APIState.CHECK_FAILED
+            time.sleep(0.1)
+        return code
+
     @xarm_is_connected(_type='set')
     def _set_modbus_gripper_position(self, pos, wait=False, speed=None, auto_enable=False, timeout=None, **kwargs):
         if kwargs.get('wait_motion', True):
@@ -486,7 +514,7 @@ def _set_modbus_gripper_position(self, pos, wait=False, speed=None, auto_enable=
         ret[0] = self._check_modbus_code(ret, only_check_code=True)
         if wait and ret[0] == 0:
             if self.gripper_is_support_status:
-                return self.check_gripper_status(timeout=timeout)
+                return self.__check_gripper_status(timeout=timeout)
             else:
                 return self.__check_gripper_position(pos, timeout=timeout)
         return ret[0]