xarm-python-sdk 1.15.2__py3-none-any.whl

xarm/x3/base_board.py

@@ -0,0 +1,198 @@
+# -*- coding: utf-8 -*-
+# __author: rock
+# @time: 2021-04-02
+
+import math
+import time
+from ..core.utils.log import logger
+from .base import Base
+from .decorator import xarm_is_connected
+
+
+class BaseBoard(Base):
+
+    def __init__(self):
+        super(BaseBoard, self).__init__()
+
+    @xarm_is_connected(_type='get')
+    def get_base_board_version(self, board_id=10):
+        versions = ['*', '*', '*']
+
+        ret1 = self.arm_cmd.tgpio_addr_r16(0x0801, bid=board_id)
+        ret2 = self.arm_cmd.tgpio_addr_r16(0x0802, bid=board_id)
+        ret3 = self.arm_cmd.tgpio_addr_r16(0x0803, bid=board_id)
+
+        code = 0
+        if ret1[0] == 0 and len(ret1) == 2:
+            versions[0] = ret1[1]
+        else:
+            code = ret1[0]
+        if ret2[0] == 0 and len(ret2) == 2:
+            versions[1] = ret2[1]
+        else:
+            code = ret2[0]
+        if ret3[0] == 0 and len(ret3) == 2:
+            versions[2] = ret3[1]
+        else:
+            code = ret3[0]
+
+        return code, '.'.join(map(str, versions))
+
+    @xarm_is_connected(_type='get')
+    def get_current_angle(self, board_id=10):
+        code, acc_xyz = self.get_imu_data(board_id)
+        self.arm_cmd.tgpio_addr_w16(addr=0x0606, value=1, bid=board_id)
+        if code == 0 and acc_xyz[0] != 0 and acc_xyz[1] != 0 and acc_xyz[2] != 0:
+            angle = self.__get_z_axios_offset_angle(acc_xyz[0], acc_xyz[1], acc_xyz[2])
+            return code, angle
+        else:
+            return code, 0
+
+    @staticmethod
+    def __get_z_axios_offset_angle(x=1, y=1, z=1):
+        angle = math.degrees(math.atan(z / (math.sqrt(abs((x ** 2) + (y ** 2))))))
+        angle = 90 - angle
+        return angle
+
+    @xarm_is_connected(_type='set')
+    def write_sn(self, sn='', servo_id=10):
+        code = 0
+        if len(sn) == 14:
+            for i in range(0, 14, 2):
+                ret = self.arm_cmd.tgpio_addr_w16(addr=0x1900 + (int(i / 2)), value=ord(sn[i]) | ord(sn[i + 1]) << 8, bid=servo_id)
+                code = self._check_code(ret[0])
+                time.sleep(0.1)
+                if code != 0:
+                    self.log_api_info('API -> write_sn -> code={}, sn={}'.format(code, sn), code=code)
+                    return code
+        self.log_api_info('API -> write_sn -> code={}, sn={}'.format(code, sn), code=code)
+        return code
+
+    @xarm_is_connected(_type='get')
+    def get_sn(self, servo_id=10):
+        rd_sn = ''
+        ret = [0, '']
+        for i in range(0, 14, 2):
+            ret = self.arm_cmd.tgpio_addr_r16(addr=0x0900 + (int(i / 2)), bid=servo_id)
+            time.sleep(0.1)
+            rd_sn = ''.join([rd_sn, chr(ret[1] & 0x00FF)])
+            rd_sn = ''.join([rd_sn, chr((ret[1] >> 8) & 0x00FF)])
+            ret[0] = self._check_code(ret[0])
+            if ret[0] != 0:
+                self.log_api_info('API -> get_sn -> code={}, sn={}'.format(ret[0], rd_sn), code=ret[0])
+                return ret[0], ''
+        self.log_api_info('API -> get_sn -> code={}, sn={}'.format(ret[0], rd_sn), code=ret[0])
+        return ret[0], rd_sn
+
+    @xarm_is_connected(_type='set')
+    def write_iden_to_base(self, idens, servo_id=10):
+        cmds = [0x0D00, 0x0D0C, 0x0D18, 0x0D24, 0x0D30, 0x0D3C, 0x0D48]
+        code = 0
+        if idens:
+            for i, data in enumerate(idens):
+                # print(i, data)
+                for j, d in enumerate(data):
+                    ret = self.arm_cmd.tgpio_addr_w32(addr=(cmds[i] + (2 * j)) | 0x1000, value=d, bid=servo_id)
+                    # print("%x, %f, ret:%d" % (cmds[i] + (2 * j), d, ret[0]))
+
+                    time.sleep(0.1)
+                    code = ret[0]
+                    if code != 0:
+                        return code
+            return code
+        else:
+            return 1
+
+    @xarm_is_connected(_type='get')
+    def get_imu_data(self, board_id=10):
+        code = 0
+        if board_id == 9:
+            self.arm_cmd.tgpio_addr_w16(addr=0x0606, value=1, bid=board_id)
+
+        ret1 = self.arm_cmd.tgpio_addr_r32(addr=0x0C00, bid=board_id, fmt='>f')
+        ret2 = self.arm_cmd.tgpio_addr_r32(addr=0x0C02, bid=board_id, fmt='>f')
+        ret3 = self.arm_cmd.tgpio_addr_r32(addr=0x0C04, bid=board_id, fmt='>f')
+        code = 0 if ret1[0] == 0 else ret1[0]
+        code = code if ret2[0] == 0 else ret2[0]
+        code = code if ret3[0] == 0 else ret3[0]
+
+        if board_id == 9:
+            self.arm_cmd.tgpio_addr_w16(addr=0x0606, value=0, bid=board_id)
+        if code != 0:
+            return code, [1, 1, 1]
+        else:
+            return code, [ret1[1], ret2[1], ret3[1]]
+
+    @xarm_is_connected(_type='get')
+    def read_iden_from_base(self, servo_id=10):
+        cmds = [0x0D00, 0x0D0C, 0x0D18, 0x0D24, 0x0D30, 0x0D3C, 0x0D48]
+        code = 0
+        conf = []
+        for i in range(7):
+            vl = []
+            for j in range(6):
+                ret = self.arm_cmd.tgpio_addr_r32((cmds[i] + (2 * j)), servo_id, fmt='>f')
+                time.sleep(0.01)
+                # print("%x, %f, ret:%d" % (cmds[i] + (2 * j), ret[1], ret[0]))
+                vl.append(ret[1])
+                code = ret[0]
+                if code != 0:
+                    return code, None
+            conf.append(vl)
+        return code, conf
+        
+    @xarm_is_connected(_type='set')
+    def write_poe_to_end(self, datas, servo_id=9):
+        cmds1 = [0x0E00, 0x0E0C, 0x0E18, 0x0E24, 0x0E30, 0x0E3C, 0x0E48]
+        cmds2 = 0x0F00
+        code = 0
+        if datas:
+            joints, homes = datas
+            for i, data in enumerate(joints):
+                for j, d in enumerate(data):
+                    ret = self.arm_cmd.tgpio_addr_w32(addr=(cmds1[i] + (2 * j)) | 0x1000, value=d, bid=servo_id)
+                    time.sleep(0.1)
+                    code = ret[0]
+                    if code != 0:
+                        return code
+            for i, data in enumerate(homes):
+                for j, d in enumerate(data):
+                    ret = self.arm_cmd.tgpio_addr_w32(addr=(cmds2 + (2 * (i * len(data) + j))) | 0x1000, value=d, bid=servo_id)
+                    time.sleep(0.1)
+                    code = ret[0]
+                    if code != 0:
+                        return code
+            return code
+        else:
+            return 1
+
+    @xarm_is_connected(_type='get')
+    def read_poe_from_end(self, servo_id=9):
+        cmds = [0x0E00, 0x0E0C, 0x0E18, 0x0E24, 0x0E30, 0x0E3C, 0x0E48]
+        cmds2 = 0x0F00
+        code = 0
+        conf = []
+        joints_conf = []
+        homes_conf = []
+        for i in range(7):
+            vl = []
+            for j in range(6):
+                ret = self.arm_cmd.tgpio_addr_r32(cmds[i] + (2 * j), servo_id, fmt='>f')
+                time.sleep(0.01)
+                vl.append(ret[1])
+                code = ret[0]
+                if code != 0:
+                    return code, None
+            joints_conf.append(vl)
+        for i in range(4):
+            vl = []
+            for j in range(4):
+                ret = self.arm_cmd.tgpio_addr_r32(cmds2 + 2 * (i * 4 + j), servo_id, fmt='>f')
+                time.sleep(0.01)
+                vl.append(ret[1])
+                code = ret[0]
+                if code != 0:
+                    return code, None
+            homes_conf.append(vl)
+        conf = [joints_conf, homes_conf]
+        return code, conf

xarm/x3/code.py

@@ -0,0 +1,62 @@
+#!/usr/bin/env python3
+# Software License Agreement (BSD License)
+#
+# Copyright (c) 2018, UFACTORY, Inc.
+# All rights reserved.
+#
+# Author: Vinman <vinman.wen@ufactory.cc> <vinman.cub@gmail.com>
+
+
+from ..core.config.x_config import XCONF
+
+
+class APIState(object):
+    NOT_CONNECTED = -1  # 已断开或未连接
+    NOT_READY = -2  # 未使能或者设置状态
+    API_EXCEPTION = -3  # 接口异常，可能是参数错误
+    CMD_NOT_EXIST = -4  # 命令不存在
+    TCP_LIMIT = -6  # 笛卡尔限位
+    JOINT_LIMIT = -7  # 关节角度限位
+    OUT_OF_RANGE = -8  # 超出范围
+    EMERGENCY_STOP = -9  # 紧急停止
+    SERVO_NOT_EXIST = -10  # 不存在此ID的关节
+    CONVERT_FAILED = -11  # 转换Blockly失败
+    RUN_BLOCKLY_EXCEPTION = -12  # 运行blockly app异常
+    NORMAL = 0  # 正常
+    HAS_ERROR = XCONF.UxbusState.ERR_CODE  # 有尚未清除的错误
+    HAS_WARN = XCONF.UxbusState.WAR_CODE  # 有尚未清除的警告
+    RES_TIMEOUT = XCONF.UxbusState.ERR_TOUT  # 命令回复超时
+    RES_LENGTH_ERROR = XCONF.UxbusState.ERR_LENG  # TCP长度错误
+    CMD_NUM_ERROR = XCONF.UxbusState.ERR_NUM  # TCP序号错误
+    CMD_PROT_ERROR = XCONF.UxbusState.ERR_PROT  # TCP协议标志错误
+    FUN_ERROR = XCONF.UxbusState.ERR_FUN  # TCP回复指令和发送指令不匹配
+    NO_TCP = XCONF.UxbusState.ERR_NOTTCP  # 写数据异常
+    STATE_NOT_READY = XCONF.UxbusState.STATE_NOT_READY  # 参数错误
+    RET_IS_INVALID = XCONF.UxbusState.INVALID  # 结果无效
+    OTHER = XCONF.UxbusState.ERR_OTHER  # 其它错误
+    PARAM_ERROR = XCONF.UxbusState.ERR_PARAM  # 参数错误
+
+    HOST_ID_ERR = 20  # 主机ID错误, 看使用的接口，可能是末端IO也可能是滑轨
+    MODBUS_BAUD_NOT_SUPPORT = 21  # modbus不支持此波特率
+    MODBUS_BAUD_NOT_CORRECT = 22  # 末端modbus波特率不正确
+    MODBUS_ERR_LENG = 23  # modbus回复数据长度错误
+
+    TRAJ_RW_FAILED = 31  # 读写轨迹失败(加载轨迹或保存轨迹)
+    TRAJ_RW_TOUT = 32  # 读写轨迹等待超时(加载轨迹或保存轨迹)
+    TRAJ_PLAYBACK_TOUT = 33  # 回放轨迹超时(多种情况)
+    TRAJ_PLAYBACK_FAILED = 34  # 回放轨迹失败(多种情况)
+    SUCTION_CUP_TOUT = 41  # 等待吸泵设置超时
+
+    MODE_IS_NOT_CORRECT = 51  # 模式不正确
+
+    LINEAR_TRACK_HAS_FAULT = 80  # 滑轨有错误
+    LINEAR_TRACK_SCI_IS_LOW = 81  # 滑轨的SCI被置低了
+    LINEAR_TRACK_NOT_INIT = 82  # 直线滑轨未初始化
+
+    WAIT_FINISH_TIMEOUT = 100  # 等待操作完成超时
+    CHECK_FAILED = 101  # 等待操作完成过程检测状态连续失败次数过多
+    END_EFFECTOR_HAS_FAULT = 102  # 末端配件有错误
+    END_EFFECTOR_NOT_ENABLED = 103  # 末端配件未使能
+
+    # 129 ~ 144: 标准modbus tcp的异常码，实际异常码(api_code - 0x80)
+

xarm/x3/decorator.py

@@ -0,0 +1,104 @@
+# !/usr/bin/env python3
+# Software License Agreement (BSD License)
+#
+# Copyright (c) 2021, UFACTORY, Inc.
+# All rights reserved.
+#
+# Author: Vinman <vinman.wen@ufactory.cc> <vinman.cub@gmail.com>
+
+
+import time
+import math
+import functools
+from ..core.utils.log import logger
+from .code import APIState
+from ..core.config.x_config import XCONF
+
+
+def check_modbus_baud(baud=2000000, _type='set', default=None, host_id=XCONF.TGPIO_HOST_ID):
+    def _check_modbus_baud(func):
+        @functools.wraps(func)
+        def decorator(self, *args, **kwargs):
+            code = self.checkset_modbus_baud(baud, host_id=host_id)
+            if code != 0:
+                logger.error('check modbus baud is failed, code={}'.format(code))
+                return code if _type == 'set' else (code, default if default != -99 else [])
+            else:
+                return func(self, *args, **kwargs)
+        return decorator
+    return _check_modbus_baud
+
+
+def xarm_is_connected(_type='set'):
+    def _xarm_is_connected(func):
+        @functools.wraps(func)
+        def decorator(self, *args, **kwargs):
+            if self.connected:
+                return func(self, *args, **kwargs)
+            else:
+                logger.error('xArm is not connected')
+                return APIState.NOT_CONNECTED if _type == 'set' else (APIState.NOT_CONNECTED, 'xArm is not connect')
+        return decorator
+    return _xarm_is_connected
+
+
+def xarm_is_ready(_type='set'):
+    def _xarm_is_ready(func):
+        @functools.wraps(func)
+        def decorator(self, *args, **kwargs):
+            if self.connected and kwargs.get('auto_enable', False):
+                if not self.ready:
+                    self.motion_enable(enable=True)
+                    self.set_mode(0)
+                    self.set_state(0)
+            if self.connected:
+                if self.check_xarm_is_ready:
+                    return func(self, *args, **kwargs)
+                else:
+                    logger.error('xArm is not ready')
+                    logger.info('Please check the arm for errors. If so, please clear the error first. '
+                                'Then enable the motor, set the mode and set the state')
+                    return APIState.NOT_READY if _type == 'set' else (APIState.NOT_READY, 'xArm is not ready')
+            else:
+                logger.error('xArm is not connected')
+                return APIState.NOT_CONNECTED if _type == 'set' else (APIState.NOT_CONNECTED, 'xArm is not connect')
+        return decorator
+    return _xarm_is_ready
+
+
+def xarm_wait_until_not_pause(func):
+    @functools.wraps(func)
+    def decorator(self, *args, **kwargs):
+        self.wait_until_not_pause()
+        return func(self, *args, **kwargs)
+    return decorator
+
+
+def xarm_wait_until_cmdnum_lt_max(func):
+    @functools.wraps(func)
+    def decorator(self, *args, **kwargs):
+        self.wait_until_cmdnum_lt_max()
+        return func(self, *args, **kwargs)
+    return decorator
+
+
+def xarm_is_not_simulation_mode(ret=0):
+    def _xarm_is_not_simulation_mode(func):
+        @functools.wraps(func)
+        def decorator(self, *args, **kwargs):
+            if not self.check_is_simulation_robot():
+                return func(self, *args, **kwargs)
+            else:
+                return ret
+        return decorator
+    return _xarm_is_not_simulation_mode
+
+
+def api_log(func):
+    @functools.wraps(func)
+    def decorator(self, *args, **kwargs):
+        ret = func(self, *args, **kwargs)
+        logger.info('{}, ret={}, args={}, kwargs={}'.format(func.__name__, ret, args[1:], kwargs))
+        return ret
+    return decorator
+

xarm/x3/events.py

@@ -0,0 +1,166 @@
+#!/usr/bin/env python3
+# Software License Agreement (BSD License)
+#
+# Copyright (c) 2019, UFACTORY, Inc.
+# All rights reserved.
+#
+# Author: Vinman <vinman.wen@ufactory.cc> <vinman.cub@gmail.com>
+
+REPORT_ID = 'REPORT'
+REPORT_LOCATION_ID = 'LOCATION'
+REPORT_CONNECT_CHANGED_ID = 'REPORT_CONNECT_CHANGED'
+REPORT_STATE_CHANGED_ID = 'REPORT_STATE_CHANGED'
+REPORT_MODE_CHANGED_ID = 'REPORT_MODE_CHANGED'
+REPORT_MTABLE_MTBRAKE_CHANGED_ID = 'REPORT_MTABLE_MTBRAKE_CHANGED'
+REPORT_ERROR_WARN_CHANGED_ID = 'REPORT_ERROR_WARN_CHANGED'
+REPORT_CMDNUM_CHANGED_ID = 'REPORT_CMDNUM_CHANGED'
+REPORT_TEMPERATURE_CHANGED_ID = 'REPORT_TEMPERATURE_CHANGED'
+REPORT_COUNT_CHANGED_ID = 'REPORT_COUNT_CHANGED'
+REPORT_IDEN_PROGRESS_CHANGED_ID = 'REPORT_IDEN_PROGRESS_CHANGED_ID'
+FEEDBACK_ID = 'FEEDBACK_ID'
+
+
+class Events(object):
+    REPORT_ID = REPORT_ID
+    REPORT_LOCATION_ID = REPORT_LOCATION_ID
+    REPORT_CONNECT_CHANGED_ID = REPORT_CONNECT_CHANGED_ID
+    REPORT_STATE_CHANGED_ID = REPORT_STATE_CHANGED_ID
+    REPORT_MODE_CHANGED_ID = REPORT_MODE_CHANGED_ID
+    REPORT_MTABLE_MTBRAKE_CHANGED_ID = REPORT_MTABLE_MTBRAKE_CHANGED_ID
+    REPORT_ERROR_WARN_CHANGED_ID = REPORT_ERROR_WARN_CHANGED_ID
+    REPORT_CMDNUM_CHANGED_ID = REPORT_CMDNUM_CHANGED_ID
+    REPORT_TEMPERATURE_CHANGED_ID = REPORT_TEMPERATURE_CHANGED_ID
+    REPORT_COUNT_CHANGED_ID = REPORT_COUNT_CHANGED_ID
+    REPORT_IDEN_PROGRESS_CHANGED_ID = REPORT_IDEN_PROGRESS_CHANGED_ID
+    FEEDBACK_ID = FEEDBACK_ID
+
+    def __init__(self):
+        self._report_callbacks = {
+            REPORT_ID: [],
+            REPORT_LOCATION_ID: [],
+            REPORT_CONNECT_CHANGED_ID: [],
+            REPORT_ERROR_WARN_CHANGED_ID: [],
+            REPORT_STATE_CHANGED_ID: [],
+            REPORT_MODE_CHANGED_ID: [],
+            REPORT_MTABLE_MTBRAKE_CHANGED_ID: [],
+            REPORT_CMDNUM_CHANGED_ID: [],
+            REPORT_COUNT_CHANGED_ID: [],
+            REPORT_IDEN_PROGRESS_CHANGED_ID: [],
+            FEEDBACK_ID: []
+        }
+
+    def _register_report_callback(self, report_id, callback):
+        if report_id not in self._report_callbacks.keys():
+            self._report_callbacks[report_id] = []
+        if (callable(callback) or isinstance(callback, dict)) and callback not in self._report_callbacks[report_id]:
+            self._report_callbacks[report_id].append(callback)
+            return True
+        elif not (callable(callback) or isinstance(callback, dict)):
+            return False
+        else:
+            return True
+
+    def _release_report_callback(self, report_id, callback):
+        if report_id in self._report_callbacks.keys():
+            if callback is None:
+                self._report_callbacks[report_id].clear()
+                return True
+            elif callback:
+                for cb in self._report_callbacks[report_id]:
+                    if callback == cb:
+                        self._report_callbacks[report_id].remove(callback)
+                        return True
+                    elif isinstance(cb, dict):
+                        if cb['callback'] == callback:
+                            self._report_callbacks[report_id].remove(cb)
+                            return True
+        return False
+
+    def register_report_callback(self, callback=None, report_cartesian=True, report_joints=True,
+                                 report_state=True, report_error_code=True, report_warn_code=True,
+                                 report_mtable=True, report_mtbrake=True, report_cmd_num=True):
+        return self._register_report_callback(REPORT_ID, {
+            'callback': callback,
+            'cartesian': report_cartesian,
+            'joints': report_joints,
+            'error_code': report_error_code,
+            'warn_code': report_warn_code,
+            'state': report_state,
+            'mtable': report_mtable,
+            'mtbrake': report_mtbrake,
+            'cmdnum': report_cmd_num
+        })
+
+    def register_report_location_callback(self, callback=None, report_cartesian=True, report_joints=False):
+        ret = self._register_report_callback(REPORT_LOCATION_ID, {
+            'callback': callback,
+            'cartesian': report_cartesian,
+            'joints': report_joints,
+        })
+        return ret
+
+    def register_connect_changed_callback(self, callback=None):
+        return self._register_report_callback(REPORT_CONNECT_CHANGED_ID, callback)
+
+    def register_state_changed_callback(self, callback=None):
+        return self._register_report_callback(REPORT_STATE_CHANGED_ID, callback)
+
+    def register_mode_changed_callback(self, callback=None):
+        return self._register_report_callback(REPORT_MODE_CHANGED_ID, callback)
+
+    def register_mtable_mtbrake_changed_callback(self, callback=None):
+        return self._register_report_callback(REPORT_MTABLE_MTBRAKE_CHANGED_ID, callback)
+
+    def register_error_warn_changed_callback(self, callback=None):
+        return self._register_report_callback(REPORT_ERROR_WARN_CHANGED_ID, callback)
+
+    def register_cmdnum_changed_callback(self, callback=None):
+        return self._register_report_callback(REPORT_CMDNUM_CHANGED_ID, callback)
+
+    def register_temperature_changed_callback(self, callback=None):
+        return self._register_report_callback(REPORT_TEMPERATURE_CHANGED_ID, callback)
+
+    def register_count_changed_callback(self, callback=None):
+        return self._register_report_callback(REPORT_COUNT_CHANGED_ID, callback)
+
+    def register_iden_progress_changed_callback(self, callback=None):
+        return self._register_report_callback(REPORT_IDEN_PROGRESS_CHANGED_ID, callback)
+    
+    def register_feedback_callback(self, callback=None):
+        return self._register_report_callback(FEEDBACK_ID, callback)
+
+    def release_report_callback(self, callback=None):
+        return self._release_report_callback(REPORT_ID, callback)
+
+    def release_report_location_callback(self, callback=None):
+        return self._release_report_callback(REPORT_LOCATION_ID, callback)
+
+    def release_connect_changed_callback(self, callback=None):
+        return self._release_report_callback(REPORT_CONNECT_CHANGED_ID, callback)
+
+    def release_state_changed_callback(self, callback=None):
+        return self._release_report_callback(REPORT_STATE_CHANGED_ID, callback)
+
+    def release_mode_changed_callback(self, callback=None):
+        return self._release_report_callback(REPORT_MODE_CHANGED_ID, callback)
+
+    def release_mtable_mtbrake_changed_callback(self, callback=None):
+        return self._release_report_callback(REPORT_MTABLE_MTBRAKE_CHANGED_ID, callback)
+
+    def release_error_warn_changed_callback(self, callback=None):
+        return self._release_report_callback(REPORT_ERROR_WARN_CHANGED_ID, callback)
+
+    def release_cmdnum_changed_callback(self, callback=None):
+        return self._release_report_callback(REPORT_CMDNUM_CHANGED_ID, callback)
+
+    def release_temperature_changed_callback(self, callback=None):
+        return self._release_report_callback(REPORT_TEMPERATURE_CHANGED_ID, callback)
+
+    def release_count_changed_callback(self, callback=None):
+        return self._release_report_callback(REPORT_COUNT_CHANGED_ID, callback)
+
+    def release_iden_progress_changed_callback(self, callback=None):
+        return self._release_report_callback(REPORT_IDEN_PROGRESS_CHANGED_ID, callback)
+    
+    def release_feedback_callback(self, callback=None):
+        return self._release_report_callback(FEEDBACK_ID, callback)