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)
