[feat]1.Added the interface for obtaining trajectory validity;2.Compatible with 1305 joint range;3.Added the interface for running gcode files;4.Adjust the interface information notes of "set_cgpio_digital" and "set_cgpio_digital";5.Modify the code translated by running Blockly controls in Blockly and PythonIDE;6.Added Get Holding Register Control

Author: SchoolMater

xarm/core/config/x_code.py

@@ -248,6 +248,16 @@
             'desc': '请通过控制器上的紧急停止按钮重启机械臂。如多次重启无效，请联系技术支持。',
         }
     },
+    58: {
+        'en': {
+            'title': 'Torque Command Timeout',
+            'desc': 'Please check the connection between the Control Box and the robot.',
+        },
+        'cn': {
+            'title': '转矩指令超时',
+            'desc': '请检查控制器与机械臂的连接。',
+        }
+    },
     'other': {
         'en': {
             'title': 'Joint Error',

xarm/core/config/x_config.py

@@ -41,6 +41,9 @@ class Type:
             XARM6_X11 = 11
             XARM6_X12 = 12
             XARM7_X13 = 13
+            XARM5_X4_1305 = 51305
+            XARM6_X4_1305 = 61305
+            XARM7_X4_1305 = 71305
 
         JOINT_LIMITS = {
             Axis.XARM5: {
@@ -51,6 +54,13 @@ class Type:
                     (-1.692969, math.pi),  # (-1.75, math.pi),
                     (-2 * math.pi, 2 * math.pi)
                 ],
+                Type.XARM5_X4_1305: [
+                    (-2 * math.pi, 2 * math.pi),
+                    (-2.042035, 2.024581),
+                    (-3.822271, 0.174532),
+                    (-1.692969, math.pi),  # (-1.75, math.pi),
+                    (-2 * math.pi, 2 * math.pi)
+                ],
             },
             Axis.XARM6: {
                 Type.XARM6_X4: [
@@ -61,6 +71,14 @@ class Type:
                     (-1.692969, math.pi),  # (-1.75, math.pi),
                     (-2 * math.pi, 2 * math.pi)
                 ],
+                Type.XARM6_X4_1305: [
+                    (-2 * math.pi, 2 * math.pi),
+                    (-2.042035, 2.024581),
+                    (-3.822271, 0.174532),
+                    (-2 * math.pi, 2 * math.pi),
+                    (-1.692969, math.pi),  # (-1.75, math.pi),
+                    (-2 * math.pi, 2 * math.pi)
+                ],
                 Type.XARM6_X8: [
                     (-2 * math.pi, 2 * math.pi),
                     (-2.059488, 2.094395),  # (-2.18, 2.18),
@@ -114,6 +132,15 @@ class Type:
                     (-1.692969, math.pi),  # (-1.75, math.pi),
                     (-2 * math.pi, 2 * math.pi)
                 ],
+                Type.XARM7_X4_1305: [
+                    (-2 * math.pi, 2 * math.pi),
+                    (-2.042035, 2.024581),
+                    (-2 * math.pi, 2 * math.pi),
+                    (-0.10472, 3.92699),
+                    (-2 * math.pi, 2 * math.pi),
+                    (-1.692969, math.pi),  # (-1.75, math.pi),
+                    (-2 * math.pi, 2 * math.pi)
+                ],
                 Type.XARM7_X13: [
                     (-2 * math.pi, 2 * math.pi),
                     (-2.094395, 2.059488),  # (-2.18, 2.18),
@@ -388,7 +415,8 @@ class UxbusReg:
         IMPEDANCE_CTRL_MBK = 210
         IMPEDANCE_CTRL_CONFIG = 211
         FTSENSOR_GET_CONFIG = 212
-
+        
+        GET_TRAJ_SPEEDING = 230
         GET_MAX_JOINT_VELOCITY = 231
         SET_COMMON_PARAM = 232
         GET_COMMON_PARAM = 233

xarm/core/wrapper/uxbus_cmd.py

@@ -1432,3 +1432,14 @@ def get_common_info(self, param_type):
             else:
                 data[0] = XCONF.UxbusState.ERR_PARAM
         return data
+    
+    def get_traj_speeding(self, rate):
+        txdata = bytes([rate])
+        ret = self.getset_nu8(XCONF.UxbusReg.GET_TRAJ_SPEEDING, txdata, 1, -1)
+        data = [0] * 4
+        data[0] = ret[0]
+        if ret[0] != XCONF.UxbusState.ERR_NOTTCP:
+            data[1] = convert.bytes_to_int32(ret[1:5])
+            data[2] = convert.bytes_to_int32(ret[5:9])
+            data[3] = round(math.degrees(convert.bytes_to_fp32(ret[9:])),3)
+        return data

xarm/tools/blockly/_blockly_base.py

@@ -217,6 +217,11 @@ def _get_condition_expression(self, value_block, arg_map=None):
                 format(direction_li.index(direction), direction_li.index(direction))
         elif block.attrib['type'] == 'get_counter':
             return 'self._arm.count'
+        elif block.attrib['type'] == 'get_single_holding_register':
+            fields = self._get_nodes('field', root=block)
+            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))
 
 
     def __get_logic_compare(self, block, arg_map=None):

xarm/tools/blockly/_blockly_handler.py

@@ -398,8 +398,11 @@ def _handle_tool_console_with_variable(self, block, indent=0, arg_map=None):
 
     def _handle_wait(self, block, indent=0, arg_map=None):
         value = self._get_node('value', root=block)
-        value = self._get_nodes('field', root=value, descendant=True)[0].text
-        self._append_main_code('time.sleep({})'.format(value), indent + 2)
+        value = float(self._get_nodes('field', root=value, descendant=True)[0].text)
+        self._append_main_code('for i in range({}):'.format(int(value / 0.1)), indent + 2)
+        self._append_main_code('    time.sleep(0.1)', indent + 2)
+        self._append_main_code('    if not self.is_alive:', indent=indent + 2)
+        self._append_main_code('        return', indent=indent + 2)
 
     def _handle_gpio_get_digital(self, block, indent=0, arg_map=None):
         io = self._get_node('field', block).text
@@ -1211,5 +1214,15 @@ def _handle_studio_run_gcode(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_gcode(projectName="{}", fileName="{}", times={})'.format(projectName, fileName, times), indent + 2)
+        if self._is_exec:
+            self._append_main_code('start_run_gcode(projectName="{}", fileName="{}", times={})'.format(projectName, fileName, times), indent + 2)
+        else:
+            self._append_main_code("for i in range({}):".format(times), indent + 2)
+            self._append_main_code('    self._arm.run_gcode_app(path="{}")'.format(projectName+fileName), indent + 2)
+
+    def _handle_write_single_holding_register(self, block, indent=0, arg_map=None):
+        fields = self._get_nodes('field', root=block)
+        addr = int(fields[0].text.replace(' ', '').replace('0x','').replace(',','').replace('\xa0', ''), 16)
+        value = int(fields[1].text.replace(' ', '').replace('0x','').replace(',','').replace('\xa0', ''), 16)
+        self._append_main_code('self._arm.write_single_holding_register({}, {})'.format(addr, value), indent + 2)
 

xarm/tools/blockly/_blockly_tool.py

@@ -267,8 +267,10 @@ def __define_listen_gpio_thread_func(self):
 
             self._append_main_init_code('    def _listen_gpio_thread(self):')
             if self._listen_tgpio_digital or len(self._tgpio_digital_callbacks):
+                self._append_main_init_code('        _, values2 = self._arm.get_tgpio_digital(2)')
                 self._append_main_init_code('        _, values = self._arm.get_tgpio_digital()')
-                self._append_main_init_code('        tgpio_digitals = values if _ == 0 else [0] * 2')
+                self._append_main_init_code('        values.insert(2, values2)')
+                self._append_main_init_code('        tgpio_digitals = values if _ == 0 else [0] * 5')
             if self._listen_tgpio_analog or len(self._tgpio_analog_callbacks):
                 self._append_main_init_code('        _, values = self._arm.get_tgpio_analog()')
                 self._append_main_init_code('        tgpio_analogs = values if _ == 0 else [0] * 2')
@@ -281,10 +283,12 @@ def __define_listen_gpio_thread_func(self):
 
             self._append_main_init_code('        while self.alive:')
             if self._listen_tgpio_digital or len(self._tgpio_digital_callbacks):
+                self._append_main_init_code('            _, values2 = self._arm.get_tgpio_digital(2)')
                 self._append_main_init_code('            _, values = self._arm.get_tgpio_digital()')
+                self._append_main_init_code('            values.insert(2, values2)')
                 self._append_main_init_code('            if _ == 0 and tgpio_digitals is not None:')
                 self._append_main_init_code('                for item in self._tgpio_digital_callbacks:')
-                self._append_main_init_code('                    for io in range(2):')
+                self._append_main_init_code('                    for io in range(5):')
                 self._append_main_init_code('                        if item[\'io\'] == io and eval(\'{} {} {}\'.format(values[io], item[\'op\'], item[\'trigger\'])) and not eval(\'{} {} {}\'.format(tgpio_digitals[io], item[\'op\'], item[\'trigger\'])):')
                 self._append_main_init_code('                            self._callback_que.put(item[\'callback\'])')
                 self._append_main_init_code('            tgpio_digitals = values if _ == 0 else tgpio_digitals')
@@ -361,7 +365,7 @@ 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')
+            self._append_main_init_code('           code = self._arm.run_blockly_app(fileName, init=False, is_exec=True, 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:
@@ -462,9 +466,11 @@ def _init_main_codes(self, arm=None):
         self._append_main_code('RobotMain.pprint(\'xArm-Python-SDK Version:{}\'.format(version.__version__))', indent=indent)
         if arm is None:
             self._append_main_code('arm = XArmAPI(sys.argv[1], baud_checkset=False)', indent=indent)
+            self._append_main_code('time.sleep(0.5)', indent=indent)
         elif isinstance(arm, str):
             self._append_main_code('arm = XArmAPI(\'{}\', baud_checkset=False)'.format(arm), indent=indent)
-
+            self._append_main_code('time.sleep(0.5)', indent=indent)
+            
         self._append_main_code('robot_main = RobotMain(arm)', indent=indent)
         self._append_main_code('robot_main.run()', indent=indent)
         self._append_main_code('', indent=-1)

xarm/version.py

@@ -1 +1 @@
-__version__ = '1.14.4'
+__version__ = '1.14.5'

xarm/wrapper/xarm_api.py

@@ -1947,7 +1947,7 @@ def get_cgpio_digital(self, ionum=None):
         """
         Get the digital value of the specified Controller GPIO
 
-        :param ionum: 0~15 or None(both 0~15), default is None
+        :param ionum: 0~7(CI0~CI7), 8~15(DI0~DI7) or None(both 0~15), 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.
         """
@@ -1966,7 +1966,7 @@ def set_cgpio_digital(self, ionum, value, delay_sec=None, sync=True):
         """
         Set the digital value of the specified Controller GPIO
 
-        :param ionum: 0~15
+        :param ionum: 0~7(CO0~CO7), 8~15(DO0~DO7)
         :param value: value
         :param delay_sec: delay effective time from the current start, in seconds, default is None(effective immediately)
         :param sync: whether to execute in the motion queue, set to False to execute immediately(default is True)
@@ -1993,7 +1993,7 @@ def set_cgpio_analog(self, ionum, value, sync=True):
     def set_cgpio_digital_input_function(self, ionum, fun):
         """
         Set the digital input functional mode of the Controller GPIO
-        :param ionum: 0~15
+        :param ionum: 0~7(CI0~CI7), 8~15(DI0~DI7)
         :param fun: functional mode
             0: general input
             1: external emergency stop
@@ -2010,7 +2010,7 @@ def set_cgpio_digital_input_function(self, ionum, fun):
     def set_cgpio_digital_output_function(self, ionum, fun):
         """
         Set the digital output functional mode of the specified Controller GPIO
-        :param ionum: 0~15
+        :param ionum: 0~7(CO0~CO7), 8~15(DO0~DO7)
         :param fun: functionnal mode
             0: general output
             1: emergency stop
@@ -4198,3 +4198,26 @@ def get_c38_error_info(self, is_radian=None):
             return 0, err_info
             # ret[1][1] = ret[1][1] if is_rad else math.degrees(ret[1][1])
         return ret
+    
+    def run_gcode_app(self, path, **kwargs):
+        """
+        Run gcode project file by xArmStudio software
+        :param path: gcode file path
+        
+        :return: code, only when code is 0, the returned result is correct.
+        """
+        return self._arm.run_gcode_app(path, **kwargs)
+
+    def get_traj_speeding(self, rate):
+        """
+        Obtain the joint and velocity values of joint overspeed during trajectory recording
+        :param rate: speed rate, It can only be 1/2/4
+
+        :return: tuple((code, speed_info)), only when code is 0, the returned result is correct.
+            code: See the [API Code Documentation](./xarm_api_code.md#api-code) for details.
+            speed_info: [result_code, servo_id, servo_speed]
+                        result_code: 0：Pass, -1: Fail, >0: abnormal(1:Trajectory not loaded or incorrect status;2:The input magnification is incorrect)
+                        servo_id: Effective only when result_code is -1
+                        servo_speed: Effective only when result_code is -1
+        """
+        return self._arm.get_traj_speeding(rate)

xarm/x3/base.py

@@ -2328,7 +2328,7 @@ def _wait_feedback(self, timeout=None, trans_id=-1, ignore_log=False):
             time.sleep(0.05)
         return APIState.WAIT_FINISH_TIMEOUT, -1
 
-    def wait_move(self, timeout=None, trans_id=-1):
+    def wait_move(self, timeout=None, trans_id=-1, set_cnt=2):
         if self._support_feedback and trans_id > 0:
             return self._wait_feedback(timeout, trans_id)[0]
         if timeout is not None:
@@ -2338,7 +2338,7 @@ def wait_move(self, timeout=None, trans_id=-1):
         _, state = self.get_state()
         cnt = 0
         state5_cnt = 0
-        max_cnt = 2 if _ == 0 and state == 1 else 10
+        max_cnt = set_cnt if _ == 0 and state == 1 else 10
         while timeout is None or time.monotonic() < expired:
             if not self.connected:
                 self.log_api_info('wait_move, xarm is disconnect', code=APIState.NOT_CONNECTED)
@@ -2367,7 +2367,7 @@ def wait_move(self, timeout=None, trans_id=-1):
                 continue
             if state == 0 or state == 1:
                 cnt = 0
-                max_cnt = 2
+                max_cnt = set_cnt
                 time.sleep(0.05)
                 continue
             else:
@@ -2596,4 +2596,9 @@ def get_common_param(self, param_type, return_val=True):
     def get_common_info(self, param_type, return_val=True):
         ret = self.arm_cmd.get_common_info(param_type) 
         ret[0] = self._check_code(ret[0])
-        return ret[0], ret[1] if return_val else ret[1:]
+        return ret[0], ret[1] if return_val else ret[1:]
+        
+    def get_traj_speeding(self, rate):
+        ret = self.arm_cmd.get_traj_speeding(rate)
+        ret[0] = self._check_code(ret[0])
+        return ret[0], ret[1:]

xarm/x3/xarm.py

@@ -66,7 +66,8 @@ def _is_out_of_tcp_range(self, value, i):
     def _is_out_of_joint_range(self, angle, i):
         if not self._check_joint_limit or self._stream_type != 'socket' or not self._enable_report or angle == math.inf:
             return False
-        joint_limit = XCONF.Robot.JOINT_LIMITS.get(self.axis).get(self.device_type, [])
+        device_type = int('{}1305'.format(self.axis)) if self.sn and int(self.sn[2:6]) >= 1305 else self.device_type
+        joint_limit = XCONF.Robot.JOINT_LIMITS.get(self.axis).get(device_type, [])
         if i < len(joint_limit):
             angle_range = joint_limit[i]
             if angle < angle_range[0] - math.radians(0.1) or angle > angle_range[1] + math.radians(0.1):
@@ -766,7 +767,8 @@ def set_reduced_joint_range(self, joint_range, is_radian=None):
             # limits = list(map(lambda x: round(math.radians(x), 3), limits))
 
         for i in range(self.axis):
-            joint_limit = XCONF.Robot.JOINT_LIMITS.get(self.axis).get(self.device_type, [])
+            device_type = int('{}1305'.format(self.axis)) if self.sn and int(self.sn[2:6]) >= 1305 else self.device_type
+            joint_limit = XCONF.Robot.JOINT_LIMITS.get(self.axis).get(device_type, [])
             if i < len(joint_limit):
                 angle_range = joint_limit[i]
                 # angle_range = list(map(lambda x: round(x, 3), joint_limit[i]))
@@ -1860,4 +1862,57 @@ def send_hex_cmd(self, datas, timeout=10):
 
     @xarm_is_connected(_type='get')
     def get_trans_id(self):
-        return self.arm_cmd.get_trans_id()
+        return self.arm_cmd.get_trans_id()
+        
+    @xarm_is_connected(_type='set')
+    def run_gcode_app(self, path=None, **kwargs):
+        sock = None
+        try:
+            if not os.path.exists(path):
+                dir_name = 'lite6' if self.axis == 6 and self.device_type == 9 else '850' if self.axis == 6 and self.device_type == 12 else 'xarm7T' if self.axis == 7 and self.device_type == 13 else 'xarm{}'.format(
+                    self.axis)
+                path = os.path.join('/home/uf' if sys.platform.startswith('linux') else os.path.expanduser('~'), '.UFACTORY', 'projects', 'test', dir_name, 'gcode', path)
+            if not os.path.exists(path):
+                raise FileNotFoundError('{} is not found'.format(path))
+            sock = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
+            sock.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1)
+            sock.setblocking(True)
+            sock.settimeout(10)
+            sock.connect((self._port, 504))
+            with open(path, 'r') as f:
+                datas = f.read()
+                lines = datas.split('\n')
+            is_continue = False
+            code = APIState.NORMAL
+            for line in lines:
+                line = line.strip()
+                if line.startswith(';('):
+                    is_continue = True
+                if not line or is_continue:
+                    if line.endswith(')'):
+                        is_continue = False
+                    continue
+                if line.startswith(';'):
+                    continue
+                GCODE_PATTERN = r'([A-Z])([-+]?[0-9.]+)'
+                CLEAN_PATTERN = r'\s+|\(.*?\)|;.*'
+                data = re.sub(CLEAN_PATTERN, '', line.strip().upper())
+                if not data:
+                    return -14
+                if data[0] == '%':
+                    return -15
+                if not re.findall(GCODE_PATTERN, data):
+                    return -16
+
+                sock.send(line.strip().encode('utf-8', 'replace') + b'\n')
+                err, status, code, buffer1, buffer2 = sock.recv(5)
+                if err != 0 or code != 0:
+                    return err if err != 0 else code
+            self.wait_move(set_cnt=5)
+            return code
+        except Exception as e:
+            code = -13
+            return code
+        finally:
+            if sock:
+                sock.close()