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)
