def xarm_init(): arm = XArmAPI("192.168.31.100") time.sleep(0.5) if arm.warn_code != 0: arm.clean_warn() if arm.error_code != 0: arm.clean_error() arm.motion_enable(enable=True) arm.set_mode(0) arm.set_state(state=0) return arm
sys.path.append(os.path.join(os.path.dirname(__file__), '../../..')) from xarm.wrapper import XArmAPI from configparser import ConfigParser parser = ConfigParser() parser.read('../robot.conf') try: ip = parser.get('xArm', 'ip') except: ip = input('Please input the xArm ip address[192.168.1.194]:') if not ip: ip = '192.168.1.194' arm = XArmAPI(ip) time.sleep(0.5) if arm.warn_code != 0: arm.clean_warn() if arm.error_code != 0: arm.clean_error() print('set IO0 high level') arm.set_tgpio_digital(0, 1) time.sleep(2) print('set IO1 high level') arm.set_tgpio_digital(1, 1) time.sleep(2) print('set IO0 low level') arm.set_tgpio_digital(0, 0) print('set IO1 low level') arm.set_tgpio_digital(1, 0)
class XArmHandler(object): def __init__(self, ui): super(XArmHandler, self).__init__() self.ui = ui self.xarm = None self.addr = None self.report_type = 'normal' self.cmd_que = queue.Queue(100) self.cmd_thread = XArmThread(self.cmd_que) self.cmd_thread.signal.connect(self.run_cmd) self.cmd_thread.start() # threading.Thread(target=self._handle_cmd_thread, daemon=True).start() self.report_que = queue.Queue() self.report_thread = XArmThread(self.report_que) self.report_thread.signal.connect(self.update_ui) self.report_thread.start() def run_cmd(self, item): try: if self.xarm and self.xarm.connected: func = getattr(self.xarm, item['cmd']) # log(item['cmd']+':', func(*item.get('args', []), **item.get('kwargs', {}))) ret = func(*item.get('args', []), **item.get('kwargs', {})) if isinstance(ret, int) and ret == TCP_OR_JOINT_LIMIT: self.ui.reset_flag() logger.debug('cmd: {}, ret:{}, args: {}, kwargs: {}'.format(item['cmd'], ret, item.get('args', []), item.get('kwargs', {}))) else: self.cmd_que.queue.clear() logger.debug('cmd: {}, ret: xArm is not connected'.format(item['cmd'])) except: pass def connect(self, addr, report_type='normal'): try: logger.debug('try connect to {}, report: {}'.format(addr, report_type)) if self.xarm and self.xarm.connected: logger.info('disconnect from {}'.format(self.addr)) self.xarm.disconnect() except Exception as e: print(e) threading.Thread(target=self.connnect_thread, args=(addr, report_type), daemon=True).start() def connnect_thread(self, addr, report_type): try: self.addr = addr self.xarm = XArmAPI(port=addr, enable_heartbeat=True, enable_report=True, report_type=report_type, do_not_open=True) self.xarm.register_maable_mtbrake_changed_callback(self.report_brake_callback) self.xarm.register_state_changed_callback(self.report_state_callback) self.xarm.register_connect_changed_callback(self.report_connected_callback) self.xarm.register_report_location_callback(self.report_location_callback, report_cartesian=True, report_joints=True) self.xarm.register_error_warn_changed_callback(self.report_warn_error_callback) self.xarm.register_cmdnum_changed_callback(self.report_cmdnum_callback) self.xarm.connect() if not self.xarm.connected: time.sleep(0.5) # threading.Timer(1, self.init_robot).start() self.report_type = report_type return True except Exception as e: self.report_que.put({ 'type': 'connect', 'item': { 'mainConnected': False, 'reportConnected': False } }) def disconnect(self): try: if self.xarm and self.xarm.connected: self.xarm.disconnect() # logger.info('diconnect from {}'.format(self.addr)) except Exception as e: print(e) def update_ui(self, data): item = data['item'] if data['type'] == 'brake': maable, mtbrake = item['maable'], item['mtbrake'] self.ui.axis_ui.brake_changed_callback(list(map(lambda x: bool(x[0] & x[1]), zip(mtbrake, maable)))) maable_value = 0 for i, v in enumerate(maable): if v is True: maable_value += pow(2, i) mtbrake_value = 0 for i, v in enumerate(mtbrake): if v is True: mtbrake_value += pow(2, i) self.ui.update_maable_mtbrake(maable_value, mtbrake_value) # self.ui.update_maable_mtbrake(maable, mtbrake) # mtbrake = [mtbrake & 0x01, mtbrake >> 1 & 0x01, mtbrake >> 2 & 0x01, mtbrake >> 3 & 0x01, # mtbrake >> 4 & 0x01, mtbrake >> 5 & 0x01, mtbrake >> 6 & 0x01, mtbrake >> 7 & 0x01] # maable = [maable & 0x01, maable >> 1 & 0x01, maable >> 2 & 0x01, maable >> 3 & 0x01, # maable >> 4 & 0x01, maable >> 5 & 0x01, maable >> 6 & 0x01, maable >> 7 & 0x01] # self.ui.axis_ui.brake_changed_callback(list(map(lambda x: bool(x[0] & x[1]), zip(mtbrake, maable)))) elif data['type'] == 'error': self.ui.update_warn_error([item['warnCode'], item['errorCode']]) if item['errorCode'] != 0 or item['warnCode'] != 0: if item['warnCode'] != 0: logger.warn('warn: code: {}, desc: {}'.format(item['warnCode'], ControlWarn(item['warnCode']).description)) if item['errorCode'] != 0: logger.error('error: code: {}, desc: {}'.format(item['errorCode'], ControlError(item['errorCode']).description)) else: logger.info('warn: {}, error: {}'.format(item['warnCode'], item['errorCode'])) if item['errorCode'] != 0: try: self.get_servo_debug_msg() except Exception as e: logger.error(str(e)) elif data['type'] == 'state': state = item['state'] self.ui.update_state(state) elif data['type'] == 'connect': self.ui.update_connect_status([item['mainConnected'], item['reportConnected']]) elif data['type'] == 'location': pos = item['position'] angles = item['angles'] self.ui.axis_ui.update_joints(angles) self.ui.cartesian_ui.update_cartesians(pos) elif data['type'] == 'cmdnum': cmdnum = item['cmdnum'] self.ui.update_cmd_count(cmdnum) def report_state_callback(self, item): self.report_que.put({ 'type': 'state', 'item': item }) def report_cmdnum_callback(self, item): self.report_que.put({ 'type': 'cmdnum', 'item': item }) def report_connected_callback(self, item): self.report_que.put({ 'type': 'connect', 'item': item }) def report_location_callback(self, item): try: pos = item['cartesian'] angles = item['joints'] location = { 'position': [float('{:.1f}'.format(p * RAD_DEGREE)) if 2 < i < 6 else float('{:.1f}'.format(p)) for i, p in enumerate(pos)], 'angles': [float('{:.1f}'.format(angle * RAD_DEGREE)) for angle in angles], } self.report_que.put({ 'type': 'location', 'item': location }) except Exception as e: print(e) pass def report_brake_callback(self, item): self.report_que.put({ 'type': 'brake', 'item': item }) def report_warn_error_callback(self, item): self.report_que.put({ 'type': 'error', 'item': item }) def get_servo_debug_msg(self, only_log_error_servo=True): if self.ui.main_ui.window.log_window.isHidden(): self.ui.main_ui.window.log_window.show() self.ui.main_ui.logAction.setText('Close-Log') try: if self.xarm and self.xarm.connected: ret = self.xarm.get_servo_debug_msg() if ret[0] in [0, x2_config.UX2_ERR_CODE, x2_config.UX2_WAR_CODE]: logger.log('=' * 50) for i in range(1, 8): servo = '机械臂第{}轴'.format(i) log_servo_error(servo, ret[i * 2 - 1], ret[i * 2], only_log_error_servo=only_log_error_servo) log_servo_error('机械爪', ret[15], ret[16], only_log_error_servo=only_log_error_servo) logger.log('=' * 50) elif ret[0] == 3: logger.debug('cmd: get_servo_debug_msg, ret: 通信错误') else: logger.debug('cmd: get_servo_debug_msg, ret: xArm is not connected') except Exception as e: print(e) def init_robot(self): global on_init with lock: on_init = True if self.xarm and self.xarm.connected: if self.xarm.warn_code != 0: logger.debug('clean warn:', self.xarm.clean_warn()) if self.xarm.error_code != 0: logger.debug('clean error:', self.xarm.clean_error()) time.sleep(0.3) logger.debug('motion enable:', self.xarm.motion_enable(enable=True)) time.sleep(0.5) logger.debug('set state:', self.xarm.set_state(0)) time.sleep(0.2) self.xarm.get_version() logger.info('xarm_version:', self.xarm.version) with lock: on_init = False def put_cmd_que(self, item): self.cmd_que.put(item)
import functools sys.path.append(os.path.join(os.path.dirname(__file__), '../..')) from xarm.wrapper import XArmAPI from xarm.core.utils.log import logger # logger.setLevel(logger.DEBUG) xarm = XArmAPI(port='192.168.1.113', enable_heartbeat=True, enable_report=True, report_type='normal', is_radian=True) xarm.motion_enable(True) xarm.set_state(0) xarm.clean_error() xarm.set_state(0) time.sleep(5) # print(xarm.get_servo_angle()) # print(xarm.get_position(is_radian=False)) # print(time.time()) # print(xarm.get_position()) # print(xarm.get_servo_angle()) # print(xarm.is_tcp_limit([300, 0, 100, 180, 0, 0])) # print(xarm.is_joint_limit([90, -45, 0, 0, 0, 0, 0])) # print(time.time()) # def test(): # print('default_is_radian:', xarm.default_is_radian)
class XArmWrapperSingleton: __instance = None @staticmethod def get_instance(): """ Static access method. """ if XArmWrapperSingleton.__instance is None: XArmWrapperSingleton() return XArmWrapperSingleton.__instance def __init__(self): """ Virtually private constructor. """ if XArmWrapperSingleton.__instance is not None: raise SingletonException("This class is a singleton!") else: self.params = { 'speed': 100, 'acc': 2000, 'angle_speed': 20, 'angle_acc': 500, 'events': {}, 'variables': {}, 'callback_in_thread': True, 'quit': False } XArmWrapperSingleton.__instance = self self.arm = XArmAPI('192.168.1.229', do_not_open=True) self.end_effector = ArduinoUdp('192.168.1.230') self.arm_is_busy = False self.arm.connect() self.arm.clean_warn() self.arm.clean_error() self.blower(0) self.vacuum_granular(0) self.vacuum_solid(0) print(self.move_granular(0)) print(self.move_syringe(0)) print(self.move_cylinder(0)) time.sleep(1) self.arm.motion_enable(True) self.arm.set_mode(0) self.arm.set_state(0) self.initial_position() time.sleep(1) self.arm.register_error_warn_changed_callback( self.error_warn_change_callback) self.arm.register_state_changed_callback( self.state_changed_callback) self.arm.register_report_location_callback( self.location_change_callback) if hasattr(self.arm, 'register_count_changed_callback'): def count_changed_callback(data): pprint('counter val: {}'.format(data['count'])) self.arm.register_count_changed_callback( count_changed_callback) self.arm.register_connect_changed_callback( self.connect_changed_callback) def update_parameters(self, speed=100, acc=2000, angle_speed=20, angle_acc=500): self.params['speed'] = speed self.params['angle_speed'] = angle_speed self.params['acc'] = acc self.params['angle_acc'] = angle_acc def move_join(self, servo_id=None, angle=None, speed=None, mvacc=None, mvtime=None, relative=False, is_radian=None, wait=False, timeout=None, radius=None, **kwargs): code = self.arm.set_servo_angle(angle=angle, speed=self.params['angle_speed'], mvacc=self.params['angle_acc'], wait=wait, radius=radius) if code != 0: self.params['quit'] = True pprint('set_servo_angle, code={}'.format(code)) def set_gripper_position(self, pos, wait=True, speed=None, auto_enable=False, timeout=None, is_modbus=True, **kwargs): if self.arm.error_code == 0 and not self.params['quit']: code = self.arm.set_gripper_position(pos, wait=wait, speed=speed, auto_enable=True) if code != 0: self.params['quit'] = True pprint('set_gripper_position, code={}'.format(code)) def move_arc_line(self, x=None, y=None, z=None, roll=None, pitch=None, yaw=None, radius=50, wait=False): code = self.arm.set_position(*[x, y, z, roll, pitch, yaw], speed=self.params['speed'], mvacc=self.params['acc'], radius=radius, wait=wait) if code != 0: self.params['quit'] = True pprint('set_position, code={}'.format(code)) def initial_position(self): self.arm.set_servo_angle(angle=[0.0, -60.0, 0.0, 0.0, 60.0, 0.0], speed=self.params['angle_speed'], mvacc=self.params['angle_acc'], wait=True, radius=-1.0) def is_arm_busy(self): return self.arm_is_busy def set_is_arm_busy(self, status=False): self.arm_is_busy = status def set_pause_time(self, pause_time): self.arm.set_pause_time(pause_time) def is_working_envelope(self, x, y, z): radius_xy = sqrt(x**2 + y**2) if 191 < radius_xy > 600: return True else: return False # Register error/warn changed callback def location_change_callback(self, data): c = data['cartesian'] # print(f'{c[0]} X {c[1]} = {self.is_working_envelope(c[0], c[1], c[2])}') if self.is_working_envelope(c[0], c[1], c[2]): pass # self.arm.emergency_stop() # if data and data['error_code'] != 0: # self.arm.set_state(4) # self.params['quit'] = True # pprint('err={}, quit'.format(data['error_code'])) # self.arm.release_error_warn_changed_callback(self.error_warn_change_callback) # Register error/warn changed callback def error_warn_change_callback(self, data): if data and data['error_code'] != 0: self.arm.set_state(4) self.params['quit'] = True pprint('err={}, quit'.format(data['error_code'])) self.arm.release_error_warn_changed_callback( self.error_warn_change_callback) # Register state changed callback def state_changed_callback(self, data): if data and data['state'] == 4: if self.arm.version_number[0] >= 1 and self.arm.version_number[ 1] >= 1 and self.arm.version_number[2] > 0: self.params['quit'] = True pprint('state=4, quit') self.arm.release_state_changed_callback( self.state_changed_callback) def connect_changed_callback(self, data): if data and not data['connected']: self.params['quit'] = True pprint('disconnect, connected={}, reported={}, quit'.format( data['connected'], data['reported'])) self.arm.release_connect_changed_callback( self.error_warn_change_callback) def is_connected(self): return self.arm.connected def move_granular(self, position): """ :param position: maximum value is 9000 :return: """ if position > EIG.MAX_GRANULAR: print( f'invalid position : {position} is more than maximum position') position = EIG.MAX_GRANULAR ret = self.end_effector.send( convert(DATA(EIG.MOVE_GRANULAR_MOTOR, position))) print(ret) return ret[0] def move_syringe(self, position): """ :param position: maximum value is 13000 :return: """ if position > EIG.MAX_SYRINGE: print( f'invalid position : {position} is more than maximum position') position = EIG.MAX_SYRINGE ret = self.end_effector.send( convert(DATA(EIG.MOVE_PULLER_MOTOR, position))) print(ret) return ret[0] def move_cylinder(self, position): """ :param position: maximum value is 13000 :return: """ if position > EIG.MAX_CYLINDER: print( f'invalid position : {position} is more than maximum position') position = EIG.MAX_CYLINDER self.end_effector.send( convert(DATA(EIG.MOVE_CYLINDER_MOTOR, position))) ret = self.end_effector.send( convert(DATA(EIG.MOVE_CYLINDER_MOTOR, position))) print(ret) return ret[0] def vacuum_solid(self, position): if position == 0: ret = self.end_effector.send( convert(DATA(EIG.OFF_CYLINDER_VACUUM, position))) print(ret) return ret[0] else: ret = self.end_effector.send( convert(DATA(EIG.ON_CYLINDER_VACUUM, position))) print(ret) return ret[0] def vacuum_granular(self, position): if position == 0: ret = self.end_effector.send( convert(DATA(EIG.OFF_GRANULAR_VACUUM, position))) print(ret) return ret[0] else: ret = self.end_effector.send( convert(DATA(EIG.ON_GRANULAR_VACUUM, position))) print(ret) return ret[0] def blower(self, position): if position == 0: ret = self.end_effector.send( convert(DATA(EIG.OFF_BLOWER, position))) print(ret) return ret[0] else: ret = self.end_effector.send(convert(DATA(EIG.ON_BLOWER, position))) print(ret) return ret[0] def reset(self, speed=None, mvacc=None, mvtime=None, is_radian=None, wait=False, timeout=None): self.arm.reset(speed=speed, mvacc=mvacc, mvtime=mvtime, is_radian=is_radian, wait=wait, timeout=timeout)