class UArm_SDK(object): def __init__(self): ''' connect to UArm ''' self.swift = SwiftAPI() self.swift.connect() self.swift.get_power_status() print(self.swift.get_device_info()) self.swift.reset(wait=True) # back to home position print('init complete') self.gripper_temp = 0 # keep track of gripper state def __del__(self): ''' disconnect UArm ''' self.swift.disconnect() print('uarm disconnected') def set_servo_angle(self, joint_angles, dt): ''' set servo angle via SDK input: joint_angles, 5-vector: [theta1, theta2, theta3, theta4, pump state] in degrees dt, time step ''' wait = True self.swift.set_servo_angle(servo_id=0, angle=joint_angles[0] + 90, speed=5000, wait=wait) time.sleep(dt / 4) self.swift.set_servo_angle(servo_id=1, angle=joint_angles[1], speed=5000, wait=wait) time.sleep(dt / 4) self.swift.set_servo_angle(servo_id=2, angle=joint_angles[2] - joint_angles[1], speed=5000, wait=wait) time.sleep(dt / 4) self.swift.set_servo_angle(servo_id=3, angle=180 - joint_angles[3], speed=5000, wait=wait) time.sleep(dt / 4) if joint_angles[4] > 0: self.swift.set_pump(on=True) elif joint_angles[4] == 0: self.swift.set_pump(on=False) else: print("ERROR") def control_uarm_via_traj(self, position, wrist_angle, pump_state, dt): ''' set end effector position, wrist angle and pump state via SDK input: position, 3-vector: [px, py, pz] wrist_angle: wrist angle in rad pump_state: bool, 0 - off, 1 - on ''' px, py, pz = position[0], position[1], position[2] # conver m to mm px *= 1000 py *= 1000 pz *= 1000 # change end effector position e = self.swift.set_position(x=px, y=py, z=pz, speed=100000, wait=True) print(e) # change wrist angle self.swift.set_wrist(90 - wrist_angle * 180 / PI) if self.gripper_temp == 0 and pump_state == 1: # enable suction cup self.swift.set_pump(on=True, wait=True) print('pump on') self.gripper_temp = 1 if self.gripper_temp == 1 and pump_state == 0: # disable suction cup self.swift.set_pump(on=False, wait=True) print('pump off') self.gripper_temp = 0 time.sleep(dt)
class uarmRobotClass(object): def __init__(self): self.x_pos = 0 self.y_pos = 0 self.tcp_rot = 0 self.z_pos = 0 self.pick = 0 self.connect = 0 self.swift = SwiftAPI(filters={'hwid': 'USB VID:PID=2341:0042'}) self.swift.waiting_ready(timeout=3) self.swift.set_speed_factor(0.5) self.swift.waiting_ready(timeout=3) self.swift.disconnect() def openPort(self): self.swift.connect() #swift = SwiftAPI(filters={'hwid': 'USB VID:PID=2341:0042'}) self.swift.waiting_ready(timeout=3) self.swift.set_speed_factor(0.5) self.swift.waiting_ready(timeout=3) self.swift.set_position(150, 0, 50, wait=True) def closePort(self): self.swift.disconnect() self.swift.waiting_ready(timeout=3) def move(self): Index = 0 pick = 0 connect = 0 while True: if (dummy == False) and (connect == 0) and (self.connect == 1): self.openPort() ret = self.swift.get_position(wait=True) print("ret values %d, %d, %d" % (ret[0], ret[1], ret[2])) x_pos = ret[0] y_pos = ret[1] tcp_rot = ret[2] print("connected") connect = 1 if (dummy == False) and (connect == 1) and (self.connect == 0): self.closePort() connect = 0 print("disconnnected") if (self.x_pos != 0) or (self.y_pos != 0) or (self.tcp_rot != 0) or (self.z_pos != 0) or (self.pick != 0) or (self.connect != 0): if logLevel > 1: print("index = %d: x_pos = %d; y_pos = %d; z_pos = %d; tcp_rot = %d; pick = %d; connect = %d" % (Index, self.x_pos, self.y_pos, self.z_pos, self.tcp_rot, self.pick, self.connect)) Index = Index + 1 if (dummy == False) and (connect == 1) and ((self.x_pos != 0) or (self.y_pos != 0) or (self.tcp_rot != 0) or (self.z_pos != 0)): self.swift.set_position(x=self.x_pos, y=self.y_pos, z=self.z_pos, wait=True, relative=True); ret = self.swift.get_position(wait=True) if logLevel > 0: print("ret values %d, %d, %d" % (ret[0], ret[1], ret[2])) if (dummy == False) and (pick == 0) and (self.pick != 0): self.swift.set_pump(on=True) pick = 1 if (dummy == False) and (pick == 1) and (self.pick == 0): self.swift.set_pump(on=False) pick = 0 time.sleep(0.001);
def key_callback(ret, key=None): print('report {}: {}, time: {}'.format(key, ret, time.time())) def limit_switch_callback(ret): print('report limit switch: {}, time: {}'.format(ret, time.time())) swift = SwiftAPI(filters={'hwid': 'USB VID:PID=2341:0042'}, do_not_open=True) swift.register_power_callback(callback=power_callback) swift.register_report_position_callback(callback=pos_callback) swift.register_key0_callback( callback=functools.partial(key_callback, key='key0')) swift.register_key1_callback( callback=functools.partial(key_callback, key='key1')) swift.register_limit_switch_callback(callback=limit_switch_callback) swift.connect() swift.waiting_ready() swift.set_report_position(interval=0.1) swift.set_report_keys(on=True) swift.set_servo_detach() time.sleep(60) swift.set_report_position(interval=0) swift.set_report_keys(on=False) swift.set_servo_attach() swift.disconnect()
class UArmHandler(object): def __init__(self, ui): super(UArmHandler, self).__init__() self.ui = ui self.arm = None self.port = None self.cmd_que = queue.Queue(100) self.cmd_thread = XArmThread(self.cmd_que, check=False) self.cmd_thread.signal.connect(self.run_cmd) self.cmd_thread.start() self.report_que = queue.Queue() self.report_thread = XArmThread(self.report_que, check=True) self.report_thread.signal.connect(self.update_ui) self.report_thread.start() def run_cmd(self, item): try: if self.arm and self.arm.connected: func = getattr(self.arm, item['cmd']) # log(item['cmd']+':', func(*item.get('args', []), **item.get('kwargs', {}))) ret = func(*item.get('args', []), **item.get('kwargs', {})) if item['cmd'] == 'get_position' and isinstance( ret, list) and len(ret) >= 3: self.report_que.put({ 'type': 'location', 'item': { 'position': ret, 'angles': None } }) elif item['cmd'] == 'get_polar' and isinstance( ret, list) and len(ret) >= 3: self.report_que.put({ 'type': 'location', 'item': { 'position': [None, None, None, *ret], 'angles': None } }) elif item['cmd'] == 'get_servo_angle' and isinstance( ret, list) and len(ret) >= 3: self.report_que.put({ 'type': 'location', 'item': { 'position': None, 'angles': ret } }) elif item['cmd'] == 'get_device_info': self.report_que.put({'type': 'info', 'item': ret}) elif item['cmd'] == 'get_mode' or item['cmd'] == 'set_mode': self.report_que.put({ 'type': 'mode', 'item': { 'mode': ret } }) # 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() self.report_connected_callback({'connected': False}) logger.debug('cmd: {}, ret: xArm is not connected'.format( item['cmd'])) except: pass def connect(self, port): try: logger.debug('try connect to {}'.format(port)) if self.arm and self.arm.connected: logger.info('disconnect from {}'.format(self.port)) self.arm.disconnect() except Exception as e: print(e) threading.Thread(target=self.connnect_thread, args=(port, ), daemon=True).start() def connnect_thread(self, port): try: self.port = port self.arm = SwiftAPI(port=port, do_not_open=True) self.arm.connect() if not self.arm.connected: time.sleep(0.5) self.arm.waiting_ready() self.report_connected_callback({'connected': True}) device_info = self.arm.get_device_info() self.report_que.put({'type': 'info', 'item': device_info}) mode = self.arm.get_mode() self.report_que.put({'type': 'mode', 'item': {'mode': mode}}) position = self.arm.get_position() if isinstance(position, list) and len(position) >= 3: self.report_que.put({ 'type': 'location', 'item': { 'position': position, 'angles': None } }) polar = self.arm.get_polar() if isinstance(polar, list) and len(polar) >= 3: self.report_que.put({ 'type': 'location', 'item': { 'position': [None, None, None, *polar], 'angles': None } }) angles = self.arm.get_servo_angle() if isinstance(angles, list) and len(angles) >= 3: self.report_que.put({ 'type': 'location', 'item': { 'position': None, 'angles': angles } }) return True except Exception as e: # print(e) self.report_connected_callback({'connected': False}) def disconnect(self): try: if self.arm and self.arm.connected: self.arm.disconnect() self.report_connected_callback({'connected': False}) # logger.info('diconnect from {}'.format(self.addr)) except Exception as e: print(e) def update_ui(self, data): item = data['item'] if data['type'] == 'timeout': if not self.arm or not self.arm.connected: self.ui.update_connect_status(False) elif data['type'] == 'connect': self.ui.update_connect_status(item['connected']) elif data['type'] == 'location': pos = item['position'] angles = item['angles'] if angles: self.ui.axis_ui.update_joints(angles) if pos: self.ui.cartesian_ui.update_cartesians(pos) elif data['type'] == 'info': self.ui.update_device_info(item) elif data['type'] == 'mode': self.ui.update_mode(item['mode']) def report_connected_callback(self, item): self.report_que.put({'type': 'connect', 'item': item}) def put_cmd_que(self, item): self.cmd_que.put(item)