class uArmSwift:
def __init__(self):
self.swift = SwiftAPI(filters={'hwid': 'USB VID:PID=2341:0042'},
cmd_pend_size=2,
callback_thread_pool_size=1)
if not self.swift.connected:
print('lose connect')
self.swift.waiting_ready()
device_info = self.swift.get_device_info()
print(device_info)
firmware_version = device_info['firmware_version']
if firmware_version and not firmware_version.startswith(
('0.', '1.', '2.', '3.')):
self.swift.set_speed_factor(0.00005)
self.swift.set_mode(0)
self.speed = 500000
self.swift.set_wrist(angle=90)
self.wristAngle = self.swift.get_servo_angle(0, timeout=10)
def set_position(self, x=100, y=0, z=100, wait=False):
self.swift.set_position(x, y, z, speed=self.speed, wait=wait)
def set_polar(self, stretch, rotation, height, wait=False):
self.swift.set_polar(stretch,
rotation,
height,
speed=self.speed,
wait=wait)
def set_servo_angle(self, num, angle, wait=False):
if num < 0 and num > 3:
print("num is wrong")
self.swift.set_servo_angle(num,
angle,
wait,
speed=self.speed,
wait=wait)
def set_wrist(self, angle=90, wait=False): # 第四电机
self.swift.set_wrist(angle, wait)
def set_pump(self, on=False):
self.swift.set_pump(on)
def set_buzzer(self, freq=1000, duration=1, wait=False):
self.swift.set_buzzer(freq, duration, wait)
def get_position(self):
return self.swift.get_position()
def get_servo_angle(self, id=0):
return self.swift.get_servo_angle(id, timeout=10)
def is_moving(self):
return self.swift.get_is_moving()
def disconnect(self):
self.swift.disconnect()
from uarm.wrapper import SwiftAPI
swift = SwiftAPI()
swift.waiting_ready(timeout=10)
swift.set_position(140, 0, 10, wait=True)
swift.set_servo_detach()
test1 = swift.get_position()
print(test1)
test2 = swift.get_servo_angle()
print(test2)
test = input(
"now the uArm is detached, that mean you can freely move the arm without the servo-motors blocking it. for the next step, deplace the arm in another position than before and type enter to continue"
)
test1 = swift.get_position()
print(test1)
test2 = swift.get_servo_angle()
print(test2)
test = input(
"has you can see, the arm has moved but the position is the same : after detached the arm, if you move it you can only have the servo-motors angles. type enter to continue BUT be careful : the arm will be attach so don't block it in any way."
)
swift.set_servo_attach()
swift.set_position(140, 0, 50, wait=True)
y = data[0]
x_out = -0.3498 * int(float(x)) / 1932 * 2016 + 524.2 #+0.01*int(y)
y_out = -0.355 * int(
float(y)) / 1449 * 1504 + 341 - 0.01 * (1400 - int(float(x)))
swift.set_position(x=x_out, y=y_out, z=15)
swift.set_wrist(90)
time.sleep(0.2)
#down = input("Down? ")
swift.set_pump(True)
swift.set_position(x=x_out, y=y_out, z=0)
#up = input("Up? ")
time.sleep(0.2)
swift.set_position(x=x_out, y=y_out, z=30)
ang1 = swift.get_servo_angle()[0]
x = data[3]
y = data[2]
r = data[4]
x_out = -0.3498 * int(float(x)) + 524.2 #+0.01*int(y)
y_out = -0.355 * int(float(y)) + 341 - 0.01 * (1400 - int(float(x)))
swift.set_position(x=x_out, y=y_out, z=15)
time.sleep(6)
ang2 = swift.get_servo_angle()[0]
swift.set_wrist(90 + (ang2 - ang1))
#down = input("Down? ")
time.sleep(0.5)
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)
ret = swift.get_power_status()
print('power status: {}'.format(ret))
ret = swift.get_device_info()
print('device info: {}'.format(ret))
ret = swift.get_limit_switch()
print('limit switch: {}'.format(ret))
ret = swift.get_gripper_catch()
print('gripper catch: {}'.format(ret))
ret = swift.get_mode()
print('mode: {}'.format(ret))
print('set mode:', swift.set_mode(1))
ret = swift.get_mode()
print('mode: {}'.format(ret))
ret = swift.get_servo_attach(servo_id=2)
print('servo attach: {}'.format(ret))
ret = swift.get_servo_angle()
print('servo angle: {}'.format(ret))
def print_callback(ret, key=None):
print('{}: {}'.format(key, ret))
swift.get_polar(wait=False, callback=functools.partial(print_callback, key='polar'))
swift.get_position(wait=False, callback=functools.partial(print_callback, key='position'))
swift.flush_cmd()
swift.disconnect()
