863 lines
32 KiB
Python
863 lines
32 KiB
Python
import threading
|
|
|
|
FRAME_DEFAULT = "world"
|
|
FLANGE = "flange"
|
|
|
|
MAX_NUMBER_PARAMETERS = 20
|
|
|
|
run("devices/RobotMotors")
|
|
|
|
class RobotTCP(TcpDevice, Stoppable):
|
|
def __init__(self, name, server, timeout = 1000, retries = 1):
|
|
TcpDevice.__init__(self, name, server)
|
|
self.timeout = timeout
|
|
self.retries = retries
|
|
self.header = None
|
|
self.trailer = "\n"
|
|
self.array_separator = '|'
|
|
self.cmd_separator = ' '
|
|
self.msg_id = 0
|
|
self.working_mode = "invalid"
|
|
self.status = "invalid"
|
|
self.powered = None
|
|
self.settled = None
|
|
self.empty = None
|
|
self.working_mode = None
|
|
self.status = None
|
|
self.lock = threading.Lock()
|
|
self.joint_forces = None
|
|
self.current_task = None
|
|
self.current_task_ret = None
|
|
self.high_level_tasks = []
|
|
self.known_points = []
|
|
self.current_points = []
|
|
self.cartesian_destination = None
|
|
#self.flange_pos = [None] * 6
|
|
self.cartesian_pos = [None] * 6
|
|
self.joint_pos = [None] * 6
|
|
self.cartesian_motors_enabled = False
|
|
self.cartesian_motors = []
|
|
self.joint_motors_enabled = False
|
|
self.joint_motors = []
|
|
self.tool = None
|
|
self.default_desc = None
|
|
self.tool_open = None
|
|
#self.tool_trsf = [0.0] * 6
|
|
self.frame = FRAME_DEFAULT
|
|
self.polling_interval = 0.01
|
|
self.reset = True
|
|
self.default_tolerance = 5
|
|
self.default_speed = 100
|
|
|
|
self.task_start_retries = 3
|
|
self.exception_on_task_start_failure = True #Tasks may start and be finished when checked
|
|
|
|
|
|
def doInitialize(self):
|
|
super(RobotTCP, self).doInitialize()
|
|
self.reset = True
|
|
|
|
def set_tool(self, tool):
|
|
self.tool = tool
|
|
#self.tool_trsf = self.get_tool_trsf()
|
|
self.evaluate("tcp_curtool=" + tool)
|
|
if self.cartesian_motors_enabled:
|
|
self.update()
|
|
self.set_motors_enabled(True)
|
|
self.is_tool_open()
|
|
|
|
def get_tool(self):
|
|
return self.tool
|
|
|
|
def set_frame(self, frame):
|
|
self.frame = frame
|
|
self.evaluate("tcp_curframe=" + frame)
|
|
if self.cartesian_motors_enabled:
|
|
self.update()
|
|
self.set_motors_enabled(True)
|
|
self.waitCacheChange(5000)
|
|
|
|
def get_frame(self):
|
|
return self.frame
|
|
|
|
def set_default_frame(self):
|
|
self.set_frame(FRAME_DEFAULT)
|
|
|
|
def assert_tool(self, tool=None):
|
|
if tool is None:
|
|
if self.tool is None:
|
|
raise Exception("Tool is undefined")
|
|
elif self.tool != tool:
|
|
raise Exception("Invalid tool: " + self.tool)
|
|
|
|
def set_default_desc(self,default_desc):
|
|
self.default_desc=default_desc
|
|
|
|
def get_default_desc(self):
|
|
return self.default_desc
|
|
|
|
def set_tasks(self,tasks):
|
|
self.high_level_tasks=tasks
|
|
|
|
def get_tasks(self):
|
|
return self.high_level_tasks
|
|
|
|
def set_known_points(self, points):
|
|
self.known_points=points
|
|
|
|
def get_known_points(self):
|
|
return self.known_points
|
|
|
|
def get_current_points(self, tolerance = None):
|
|
ret = self.is_in_points(*self.known_points, tolerance = tolerance)
|
|
current_points = []
|
|
for i in range(len(ret)):
|
|
if ret[i] == True:
|
|
current_points.append(self.known_points[i])
|
|
return current_points
|
|
|
|
def get_current_point(self, tolerance = None):
|
|
current_points = self.get_current_points(tolerance)
|
|
if (current_points is not None) and ( len(current_points) >0):
|
|
return current_points[0]
|
|
return None
|
|
|
|
def get_current_points_cached(self):
|
|
return self.current_points
|
|
|
|
def get_current_point_cached(self):
|
|
if (self.current_points is not None) and (len (self.current_points) >0):
|
|
return self.current_points[0]
|
|
return None
|
|
|
|
def assert_in_known_point(self, tolerance = None):
|
|
if self.get_current_point(tolerance) is None:
|
|
raise Exception ("Robot not in known point")
|
|
|
|
def _sendReceive(self, msg_id, msg = "", timeout = None):
|
|
tx = self.header if (self.header != None) else ""
|
|
tx = tx + msg_id + " " + msg
|
|
if (len(tx)>150):
|
|
raise Exception("Exceeded maximum message size")
|
|
self.getLogger().finer("TX = '" + str(tx)+ "'")
|
|
if (self.trailer != None): tx = tx + self.trailer
|
|
if self.isSimulated():
|
|
return ""
|
|
rx = self.sendReceive(tx, msg_id, self.trailer , 0, self.timeout if timeout is None else timeout, self.retries)
|
|
rx=rx[:-1] #Remove 0A
|
|
self.getLogger().finer("RX = '" + str(rx) + "'")
|
|
if rx[:3] != msg_id:
|
|
if (time.time()-start) >= timeout:
|
|
raise Exception("Received invalid message id: " + str(rx[:3]) + " - expecting:" + msg_id )
|
|
if len(rx)<4:
|
|
raise Exception("Invalid message size: " + str(len(rx)) )
|
|
if rx[3] == "*":
|
|
raise Exception(rx[4:])
|
|
return rx[4:]
|
|
|
|
def call(self, msg, timeout = None):
|
|
self.lock.acquire()
|
|
try:
|
|
id = "%03d" % self.msg_id
|
|
self.msg_id = (self.msg_id+1)%1000
|
|
return self._sendReceive(id, msg, timeout)
|
|
finally:
|
|
self.lock.release()
|
|
|
|
def execute(self, command, *args, **kwargs):
|
|
timeout = None if (kwargs is None) or (not kwargs.has_key("timeout")) else kwargs["timeout"]
|
|
msg = str(command)
|
|
if len(args)>MAX_NUMBER_PARAMETERS:
|
|
raise Exception("Exceeded maximum number of parameters")
|
|
for i in range(len(args)):
|
|
msg += (self.cmd_separator if (i==0) else self.array_separator) + str(args[i])
|
|
rx = self.call(msg, timeout)
|
|
if rx.count(self.array_separator)>0:
|
|
return rx.split(self.array_separator)
|
|
return rx
|
|
|
|
def evaluate(self, cmd, timeout=None):
|
|
ret = self.execute('eval', cmd, timeout=timeout)
|
|
if is_string(ret):
|
|
if ret.strip() != "": raise Exception(ret)
|
|
|
|
def get_var(self, name):
|
|
return self.execute('get_var', name)
|
|
|
|
#Makes app crash
|
|
#def get_str(self, name='s'):
|
|
# return self.execute('get_str', name)
|
|
|
|
def get_arr(self, name, size):
|
|
return self.execute('get_arr', name, size)
|
|
|
|
def get_bool(self, name = "tcp_b"):
|
|
return True if (self.execute('get_bool', name).strip() == '1') else False
|
|
|
|
def get_int(self, name ="tcp_n"):
|
|
return int(self.get_var(name))
|
|
|
|
def get_float(self, name ="tcp_n"):
|
|
return float(self.get_var(name))
|
|
|
|
def get_int_arr(self, size, name="tcp_a"):
|
|
# not working. A Jython bug in PyUnicaode?
|
|
#return [int(x) for x in self.get_arr("tcp_a", size)]
|
|
ret = []
|
|
a=self.get_arr(name, size)
|
|
for i in range(size):
|
|
ret.append(int(a[i]))
|
|
return ret
|
|
|
|
def get_float_arr(self, size, name="tcp_a"):
|
|
#return [float(x) for x in self.get_arr("tcp_a", size)]
|
|
a=self.get_arr(name, size)
|
|
ret = [];
|
|
for i in range(size): ret.append(float(a[i]));
|
|
return ret
|
|
|
|
def get_trsf(self, name="tcp_t"):
|
|
a = self.execute('get_trf', name)
|
|
ret = []
|
|
for i in range(6): ret.append(float(a[i]))
|
|
return ret
|
|
|
|
def set_trsf(self, l, name="tcp_t"):
|
|
self.evaluate(name + "={" + str(l[0]) + ","+ str(l[1]) + ","+ str(l[2]) + ","+ str(l[3]) + ","+ str(l[4]) + ","+ str(l[5]) + "}")
|
|
|
|
def get_jnt(self, name="tcp_j"):
|
|
a = self.execute('get_jnt', name)
|
|
ret = []
|
|
for i in range(6): ret.append(float(a[i]))
|
|
return ret
|
|
|
|
def set_jnt(self, l, name="tcp_j"):
|
|
self.evaluate(name + "={" + str(l[0]) + ","+ str(l[1]) + ","+ str(l[2]) + ","+ str(l[3]) + ","+ str(l[4]) + ","+ str(l[5]) + "}")
|
|
|
|
def get_pnt(self, name="tcp_p"):
|
|
#a = self.execute('get_pnt', name)
|
|
#ret = []
|
|
#for i in range(6): ret.append(float(a[i]))
|
|
#return ret
|
|
return self.get_trsf(name+".trsf")
|
|
|
|
#trsf = (x,y,z,rx,ry,rz)
|
|
#TODO: config = (shoulder, elbow, wrist)
|
|
def set_pnt(self, trsf, name="tcp_p", config=None):
|
|
self.set_trsf(trsf, name+".trsf")
|
|
|
|
def get_tool_trsf(self, name=None):
|
|
if name is None:
|
|
self.assert_tool()
|
|
name = self.tool
|
|
return self.get_trsf(name+".trsf")
|
|
|
|
def set_tool_trsf(self, trsf, name=None):
|
|
if name is None:
|
|
self.assert_tool()
|
|
name = self.tool
|
|
self.set_trsf(trsf, name+".trsf")
|
|
|
|
def eval_int(self, cmd):
|
|
if self.isSimulated():
|
|
return 0
|
|
self.evaluate("tcp_n=" + cmd)
|
|
return self.get_int()
|
|
|
|
def eval_float(self, cmd):
|
|
if self.isSimulated():
|
|
return 0.0
|
|
self.evaluate("tcp_n=" + cmd)
|
|
return self.get_float()
|
|
|
|
def eval_bool(self, cmd):
|
|
if self.isSimulated():
|
|
return False
|
|
self.evaluate("tcp_b=" + cmd)
|
|
return self.get_bool()
|
|
|
|
#def eval_str(self, cmd):
|
|
# self.evaluate("s=" + cmd)
|
|
# return self.get_str()
|
|
|
|
def eval_jnt(self, cmd):
|
|
self.evaluate("tcp_j=" + cmd)
|
|
return self.get_jnt()
|
|
|
|
def eval_trf(self, cmd):
|
|
self.evaluate("tcp_t=" + cmd)
|
|
return self.get_trsf()
|
|
|
|
def eval_pnt(self, cmd):
|
|
self.evaluate("tcp_p=" + cmd)
|
|
return self.get_pnt()
|
|
|
|
|
|
#Robot control
|
|
def is_powered(self):
|
|
self.powered = self.eval_bool("isPowered()")
|
|
return self.powered
|
|
|
|
def enable(self):
|
|
if not self.is_powered():
|
|
self.evaluate("enablePower()")
|
|
time.sleep(1.0)
|
|
if not self.is_powered():
|
|
raise Exception("Cannot enable power")
|
|
|
|
#waits for power to be actually cut off
|
|
def disable(self):
|
|
self.evaluate("disablePower()", timeout=5000)
|
|
|
|
def get_monitor_speed(self):
|
|
self.speed = self.eval_int("getMonitorSpeed()")
|
|
return self.speed
|
|
|
|
def set_monitor_speed(self, speed):
|
|
ret = self.eval_int("setMonitorSpeed(" + str(speed) + ")")
|
|
if (ret==-1): raise Exception("The robot is not in remote working mode")
|
|
if (ret==-2): raise Exception("The monitor speed is under the control of the operator")
|
|
if (ret==-3): raise Exception("The specified speed is not supported")
|
|
|
|
def set_default_speed(self):
|
|
set_monitor_speed(self.default_speed)
|
|
|
|
def is_calibrated(self):
|
|
return self.eval_bool("isCalibrated()")
|
|
|
|
def save_program(self):
|
|
ret = self.execute('save', timeout=5000)
|
|
if str(ret) != "0": raise Exception("Error saving program: " + str(ret))
|
|
|
|
def _update_working_mode(self, mode, status):
|
|
cur_mode = self.working_mode
|
|
if mode==1:
|
|
self.working_mode = "manual"
|
|
self.status = "hold" if status==6 else "move"
|
|
elif mode==2:
|
|
self.working_mode = "test"
|
|
self.status = "hold" if status==3 else "move"
|
|
elif mode==3:
|
|
self.working_mode = "local"
|
|
self.status = "hold" if status==2 else "move"
|
|
elif mode==4:
|
|
self.working_mode = "remote"
|
|
self.status = "hold" if status==2 else "move"
|
|
else:
|
|
self.working_mode = "invalid"
|
|
self.status = "invalid"
|
|
if self.working_mode != cur_mode:
|
|
try:
|
|
self.on_change_working_mode(self.working_mode)
|
|
except:
|
|
pass
|
|
|
|
def read_working_mode(self):
|
|
try:
|
|
mode = self.eval_int("workingMode(tcp_a)")
|
|
status = int(self.get_var("tcp_a[0]"))
|
|
self._update_working_mode(mode, status)
|
|
self._update_state()
|
|
except:
|
|
self.working_mode = "invalid"
|
|
self.status = "invalid"
|
|
return self.working_mode
|
|
|
|
def get_emergency_stop_sts(self):
|
|
st = self.eval_int("esStatus()")
|
|
if (st== 1): return "active"
|
|
if (st== 2): return "activated"
|
|
return "off"
|
|
|
|
def get_safety_fault_signal(self):
|
|
fault = self.eval_bool("safetyFault(s)")
|
|
#if (fault):
|
|
# return get_str()
|
|
return fault
|
|
|
|
#Motion control
|
|
def stop(self):
|
|
self.evaluate("stopMove()")
|
|
|
|
def resume(self):
|
|
self.evaluate("restartMove()")
|
|
|
|
def reset_motion(self, joint=None):
|
|
self.evaluate("resetMotion()" if (joint is None) else ("resetMotion(" + joint + ")"))
|
|
|
|
def is_empty(self):
|
|
self.empty = self.eval_bool("isEmpty()")
|
|
self._update_state()
|
|
return self.empty
|
|
|
|
def is_settled(self):
|
|
self.settled = self.eval_bool("isSettled()")
|
|
self._update_state()
|
|
return self.settled
|
|
|
|
def get_move_id(self):
|
|
return self.eval_int("getMoveId()")
|
|
|
|
def set_move_id(self, id):
|
|
return self.evaluate("setMoveId(" + str(id) + " )")
|
|
|
|
def get_joint_forces(self):
|
|
try:
|
|
self.evaluate("getJointForce(tcp_a)")
|
|
self.joint_forces = self.get_float_arr(6)
|
|
return self.joint_forces
|
|
except:
|
|
self.joint_forces = None
|
|
raise
|
|
|
|
def movej(self, joint_or_point, tool=None, desc=None, sync=False):
|
|
if desc is None: desc = self.default_desc
|
|
if tool is None: tool = self.tool
|
|
#If joint_or_point is a list assumes ir is a point
|
|
if not is_string(joint_or_point):
|
|
robot.set_pnt(joint_or_point , "tcp_p")
|
|
joint_or_point = "tcp_p"
|
|
ret = self.eval_int("movej(" + joint_or_point + ", " + tool + ", " + desc +")")
|
|
if sync:
|
|
self.wait_end_of_move()
|
|
return ret
|
|
|
|
def movel(self, point, tool=None, desc=None, sync=False):
|
|
if desc is None: desc = self.default_desc
|
|
if tool is None: tool = self.tool
|
|
if not is_string(point):
|
|
robot.set_pnt(point , "tcp_p")
|
|
point = "tcp_p"
|
|
ret = self.eval_int("movel(" + point + ", " + tool + ", " + desc +")")
|
|
if sync:
|
|
self.wait_end_of_move()
|
|
return ret
|
|
|
|
def movec(self, point_interm, point_target, tool=None, desc=None, sync=False):
|
|
if desc is None: desc = self.default_desc
|
|
if tool is None: tool = self.tool
|
|
ret = self.eval_int("movec(" + point_interm + ", " + point_target + ", " + tool + ", " + desc +")")
|
|
if sync:
|
|
self.wait_end_of_move()
|
|
return ret
|
|
|
|
def wait_end_of_move(self):
|
|
time.sleep(0.05)
|
|
self.update()
|
|
#time.sleep(0.01)
|
|
#self.waitCacheChange(-1)
|
|
self.waitReady(-1)
|
|
#self.waitState(State.Ready, -1)
|
|
|
|
|
|
#Tool - synchronized as can freeze communication
|
|
"""
|
|
def open_tool(self, tool=None, timeout=3000):
|
|
#This function can timeout since it synchronizes move. Checking state before otherwise it can freeze the communication.
|
|
self.waitState(State.Ready, -1)
|
|
if tool is None: tool = self.tool
|
|
return self.evaluate("open(" + tool + " )", timeout=timeout)
|
|
|
|
def close_tool(self, tool=None, timeout=3000):
|
|
#This function can timeout since it synchronizes move. Checking state before otherwise it can freeze the communication.
|
|
self.waitState(State.Ready, -1)
|
|
if tool is None: tool = self.tool
|
|
return self.evaluate("close(" + tool + " )", timeout=timeout)
|
|
"""
|
|
#Tool - Not synchronized calls: atention to open/close only when state is Ready
|
|
def open_tool(self, tool=None):
|
|
if tool is None: tool = self.tool
|
|
self.evaluate(tool + ".gripper=true")
|
|
self.tool_open = True
|
|
|
|
def close_tool(self, tool=None):
|
|
if tool is None: tool = self.tool
|
|
self.evaluate(tool + ".gripper=false")
|
|
self.tool_open = False
|
|
|
|
def is_tool_open(self, tool=None):
|
|
if tool is None: tool = self.tool
|
|
self.tool_open = robot.eval_bool(tool + ".gripper")
|
|
return self.tool_open
|
|
|
|
|
|
#Arm position
|
|
def herej(self):
|
|
return self.eval_jnt("herej()")
|
|
|
|
def distance_t(self, trsf1, trsf2):
|
|
return self.eval_float("distance(" + trsf1 + ", " + trsf2 + ")")
|
|
|
|
def distance_p(self, pnt1, pnt2):
|
|
return self.eval_float("distance(" + pnt1 + ", " + pnt2 + ")")
|
|
|
|
def compose(self, pnt, frame = None, trsf = "tcp_t"):
|
|
if frame is None: frame = self.frame
|
|
return self.eval_pnt("compose(" + pnt + ", " + frame + ", " + trsf + ")")
|
|
|
|
def here(self, tool=None, frame=None):
|
|
if tool is None: tool = self.tool
|
|
if frame is None: frame = self.frame
|
|
return self.eval_pnt("here(" + tool + ", " + frame + ")")
|
|
|
|
def joint_to_point(self, tool=None, frame=None, joint="tcp_j"):
|
|
if tool is None: tool = self.tool
|
|
if frame is None: frame = self.frame
|
|
return self.eval_pnt("jointToPoint(" + tool + ", " + frame + ", " + joint +")")
|
|
|
|
def point_to_joint(self, tool=None, initial_joint="tcp_j", point="tcp_p"):
|
|
if tool is None: tool = self.tool
|
|
if self.eval_bool("pointToJoint(" + tool + ", " + initial_joint + ", " + point +", j)"):
|
|
return self.get_jnt()
|
|
|
|
def position(self, point, frame=None):
|
|
if frame is None: frame = self.frame
|
|
return self.eval_trf("position(" + point + ", " + frame + ")")
|
|
|
|
#Profile
|
|
def get_profile(self):
|
|
return self.execute('get_profile', timeout=2000)
|
|
|
|
def set_profile(self, name, pwd = ""):
|
|
self.execute('set_profile', str(name), str(pwd), timeout=5000)
|
|
|
|
|
|
#Task control
|
|
def task_create(self, program, *args, **kwargs):
|
|
program = str(program)
|
|
priority = 10 if (kwargs is None) or (not kwargs.has_key("priority")) or (kwargs["priority"] is None) else kwargs["priority"]
|
|
name = str(program if (kwargs is None) or (not kwargs.has_key("name")) else kwargs["name"])
|
|
|
|
if self.get_task_status(name)[0] != -1:
|
|
raise Exception("Task already exists: " + name)
|
|
|
|
#taskCreate "t1", 10, read(sMessage)
|
|
cmd = 'taskCreate "' + name + '", ' + str(priority) + ', ' + program + '('
|
|
for i in range(len(args)):
|
|
val = args[i]
|
|
if type(val) == bool:
|
|
if val == True: val = "true"
|
|
elif val == False: val = "false"
|
|
cmd += str(val) + (',' if (i<(len(args)-1)) else '')
|
|
cmd+=')'
|
|
self.evaluate(cmd)
|
|
|
|
def task_suspend(self, name):
|
|
self.evaluate('taskSuspend("' + str(name)+ '")')
|
|
|
|
#waits until the task is ready for restart
|
|
def task_resume(self, name):
|
|
self.evaluate('taskResume("' + str(name)+ '",0)', timeout = 2000)
|
|
|
|
#waits for the task to be actually killed
|
|
def task_kill(self, name):
|
|
#self.evaluate('taskKill("' + str(name)+ '")', timeout = 5000)
|
|
self.execute('kill', str(name), timeout=5000)
|
|
|
|
def get_task_status(self, name):
|
|
if self.isSimulated():
|
|
return (-1,"Stopped")
|
|
code = self.eval_int('taskStatus("' + str(name)+ '")')
|
|
#TODO: String assignments in $exec causes application to freeze
|
|
#status = self
|
|
if code== -1: status = "Stopped"
|
|
elif code== 0: status = "Paused"
|
|
elif code== 1: status = "Running"
|
|
#else: status = self.execute('get_help', code)
|
|
else: status = "Error"
|
|
return (code,status)
|
|
|
|
def get_tasks_status(self, *pars):
|
|
ret = self.execute("task_sts", *pars)
|
|
ret = ret[0:len(pars)]
|
|
for i in range(len(ret)):
|
|
try:
|
|
ret[i] = int(ret[i])
|
|
except:
|
|
ret[i] = None
|
|
return ret
|
|
|
|
def _update_state(self):
|
|
#self.setState(State.Busy if self.status=="move" else State.Ready)
|
|
if self.state==State.Offline:
|
|
print "Communication resumed"
|
|
if self.reset or (self.state==State.Offline):
|
|
self.check_task()
|
|
if self.current_task is not None:
|
|
print "Ongoing task: " + self.current_task
|
|
if (not self.settled) or (self.current_task is not None): self.setState(State.Busy)
|
|
elif not self.empty: self.setState(State.Paused)
|
|
else: self.setState(State.Ready)
|
|
|
|
def doUpdate(self):
|
|
try:
|
|
start = time.time()
|
|
cur_task = self.current_task #Can change in background so cache it
|
|
if self.isSimulated():
|
|
sts = [1, 1,"1", 100, "1", "1", 0, 0, \
|
|
0, 0, 0, 0, 0, 0, \
|
|
0, 0, 0, 0, 0, 0, \
|
|
]
|
|
else:
|
|
sts = self.execute("get_status", cur_task)
|
|
self._update_working_mode(int(sts[0]), int(sts[1]))
|
|
self.powered = sts[2] == "1"
|
|
self.speed = int(sts[3])
|
|
self.empty = sts[4] == "1"
|
|
self.settled = sts[5] == "1"
|
|
|
|
#TODO: add tool open
|
|
if cur_task is not None:
|
|
if int(sts[6]) < 0:
|
|
log("Task "+ str(cur_task) + " finished with code: " + str(sts[7]), False)
|
|
try:
|
|
self.current_task, self.current_task_ret = None, int(sts[7])
|
|
except:
|
|
self.current_task, self.current_task_ret = None, None
|
|
for i in range(6):
|
|
self.joint_pos[i] = float(sts[8 + i])
|
|
for i in range(6):
|
|
self.cartesian_pos[i] = float(sts[14 + i])
|
|
|
|
ev_index = 20 #7
|
|
ev = sts[ev_index] if len(sts)>ev_index else ""
|
|
if len(ev.strip()) >0:
|
|
self.getLogger().info(ev)
|
|
self.on_event(ev)
|
|
|
|
self._update_state()
|
|
self.reset = False
|
|
self.setCache({"powered": self.powered,
|
|
"speed": self.speed,
|
|
"empty": self.empty,
|
|
"settled": self.settled,
|
|
"task": cur_task,
|
|
"mode": self.working_mode,
|
|
"status": self.status,
|
|
"open": self.tool_open,
|
|
"pos": self.get_current_point_cached() if self.state==State.Ready else None #TODO: make it calculated in robot by polling funtion
|
|
}, None)
|
|
|
|
if self.cartesian_motors_enabled:
|
|
for m in self.cartesian_motors:
|
|
m.readback.update()
|
|
|
|
if self.joint_motors_enabled:
|
|
for m in self.joint_motors:
|
|
m.readback.update()
|
|
|
|
except:
|
|
if self.state != State.Offline:
|
|
print >> sys.stderr, "Update error: " + str(sys.exc_info()[1])
|
|
self.setState(State.Offline)
|
|
|
|
#Cartesian space
|
|
def get_flange_pos(self):
|
|
return self.eval_pnt("jointToPoint(" +FLANGE + ", " + self.frame + ", herej())")
|
|
|
|
def get_cartesian_pos(self):
|
|
self.assert_tool()
|
|
return self.eval_pnt("jointToPoint(" + self.tool + ", " + self.frame + ", herej())")
|
|
#self.set_jnt(self.herej())
|
|
#return self.joint_to_point(tool, frame)
|
|
|
|
def get_cartesian_destination(self):
|
|
self.assert_tool()
|
|
return self.here(self.tool, self.frame)
|
|
|
|
def get_distance_to_pnt(self, name):
|
|
#self.here(self.tool, self.frame) #???
|
|
self.get_cartesian_pos()
|
|
return self.distance_p("tcp_p", name)
|
|
|
|
def is_in_point(self, p, tolerance = None): #Tolerance in mm
|
|
if (tolerance is None) and p in self.known_points:
|
|
#If checking a known point with default tolerance, updates the position cache
|
|
return p in self.get_current_points()
|
|
tolerance = self.default_tolerance if tolerance == None else tolerance
|
|
d = self.get_distance_to_pnt(p)
|
|
if d<0:
|
|
raise Exception ("Error calculating distance to " + p + " : " + str(d))
|
|
return d<tolerance
|
|
|
|
def get_distance_to_pnts(self, *pars):
|
|
ret = self.execute("dist_pnt", *pars)
|
|
ret = ret[0:len(pars)]
|
|
for i in range(len(ret)):
|
|
try:
|
|
ret[i] = float(ret[i])
|
|
except:
|
|
ret[i] = None
|
|
return ret
|
|
|
|
def is_in_points(self, *pars, **kwargs): #Tolerance in mm
|
|
tolerance = self.default_tolerance if (kwargs is None) or (not kwargs.has_key("tolerance")) or (kwargs["tolerance"] is None) else kwargs["tolerance"]
|
|
ret = self.get_distance_to_pnts(*pars)
|
|
for i in range(len(ret)):
|
|
if ret[i]<0:
|
|
ret[i] = None
|
|
else:
|
|
ret[i] = ret[i]<tolerance
|
|
if (tolerance == self.default_tolerance) and (set(self.known_points).issubset(set(pars))): #Only update cache if tolerance is default
|
|
current_points = []
|
|
for i in range(len(ret)):
|
|
if ret[i] == True:
|
|
current_points.append(self.known_points[i])
|
|
self.current_points = current_points
|
|
return ret
|
|
|
|
def assert_in_point(self, p, tolerance = None): #Tolerance in mm
|
|
if not self.is_in_point(p, tolerance):
|
|
raise Exception ("Not in position " + p)
|
|
|
|
|
|
#Cartesian peudo-motors
|
|
def set_motors_enabled(self, value):
|
|
if value !=self.cartesian_motors_enabled:
|
|
self.cartesian_motors_enabled = value
|
|
if value:
|
|
for i in range(6):
|
|
self.cartesian_motors.append(RobotCartesianMotor(self, i))
|
|
add_device(self.cartesian_motors[i], True)
|
|
self.cartesian_destination = self.get_cartesian_destination()
|
|
else:
|
|
for m in self.cartesian_motors:
|
|
remove_device(m)
|
|
self.cartesian_motors = []
|
|
self.cartesian_destination = None
|
|
else:
|
|
if value:
|
|
self.cartesian_destination = self.get_cartesian_destination()
|
|
for m in self.cartesian_motors:
|
|
m.initialize()
|
|
|
|
|
|
#Cartesian peudo-motors
|
|
def set_joint_motors_enabled(self, value):
|
|
if value !=self.joint_motors_enabled:
|
|
self.joint_motors_enabled = value
|
|
if value:
|
|
for i in range(6):
|
|
self.joint_motors.append(RobotJointMotor(self, i))
|
|
add_device(self.joint_motors[i], True)
|
|
else:
|
|
for m in self.joint_motors:
|
|
remove_device(m)
|
|
self.joint_motors = []
|
|
else:
|
|
if value:
|
|
for m in self.joint_motors:
|
|
m.initialize()
|
|
#Alignment
|
|
def align(self, point = None, frame = None, desc = None):
|
|
if frame is None: frame = self.frame
|
|
self.assert_tool()
|
|
if desc is None: desc = self.default_desc
|
|
if point is None:
|
|
self.get_cartesian_pos()
|
|
else:
|
|
self.set_pnt(point)
|
|
np=self.eval_trf('align(tcp_p.trsf, ' + frame + '.trsf)')
|
|
self.set_pnt(np)
|
|
self.movej("tcp_p", self.tool, desc)
|
|
#TODO: The first command is not executed (but receive a move id)
|
|
self.movej("tcp_p", self.tool, desc, sync = True)
|
|
return np
|
|
|
|
#High-level, exclusive motion task.
|
|
def start_task(self, program, *args, **kwargs):
|
|
tasks = [t for t in self.high_level_tasks]
|
|
if (self.current_task is not None) and (not self.current_task in tasks):
|
|
tasks.append(self.current_task)
|
|
if not program in tasks:
|
|
tasks.append(program)
|
|
#for task in tasks:
|
|
# if self.get_task_status(task)[0]>=0:
|
|
# raise Exception("Ongoing high-level task: " + task)
|
|
ts = self.get_tasks_status(*tasks)
|
|
for i in range(len(ts)):
|
|
if ts[i] > 0:
|
|
raise Exception("Ongoing high-level task: " + tasks[i])
|
|
|
|
self.clear_task_ret()
|
|
|
|
for i in range(self.task_start_retries):
|
|
self.task_create(program, *args, **kwargs)
|
|
(code, status) = self.get_task_status(program)
|
|
if code>=0: break
|
|
if i < self.task_start_retries-1:
|
|
ret = self.get_task_ret(False)
|
|
if ret == 0: #Did't start
|
|
log("Retrying starting : " + str(program) + str(args) + " - status: " + str(status) + " (" + str(code) + ")", False)
|
|
print "Retrying starting : " + str(program) + str(args) + " - status: " + str(status) + " (" + str(code) + ")"
|
|
else :
|
|
print "Retrying aborted : " + str(program) + str(args) + " - ret: " + str(ret) + " - status: " + str(status) + " (" + str(code) + ")"
|
|
break
|
|
else:
|
|
log("Failed starting : " + str(program) + str(args), False)
|
|
print "Failed starting : " + str(program) + str(args)
|
|
if self.exception_on_task_start_failure:
|
|
raise Exception("Cannot start task: " + program + str(args))
|
|
|
|
log("Task started: " + str(program) + str(args) + " - status: " + str(status) + " (" + str(code) + ")", False)
|
|
self.current_task, self.current_task_ret = program, None
|
|
self.update()
|
|
#self._update_state()
|
|
|
|
#TODO: remove
|
|
if self.current_task is None:
|
|
log("Task finished in first polling : " + str(program) , False)
|
|
print "Task finished in first polling : " + str(program)
|
|
|
|
return code
|
|
|
|
def stop_task(self):
|
|
tasks = [t for t in self.high_level_tasks]
|
|
if (self.current_task is not None) and (not self.current_task in tasks):
|
|
tasks.append(self.current_task)
|
|
for task in tasks:
|
|
#if self.get_task_status(task)[0]>=0:
|
|
self.task_kill(task)
|
|
self.reset_motion()
|
|
|
|
def get_task_ret(self, cached = True):
|
|
return self.current_task_ret if cached else self.eval_int("tcp_ret")
|
|
|
|
def clear_task_ret(self):
|
|
return self.evaluate("tcp_ret=0")
|
|
|
|
def get_task(self):
|
|
return self.current_task
|
|
|
|
def check_task(self):
|
|
if self.current_task is None:
|
|
for task in self.high_level_tasks:
|
|
if self.get_task_status(task)[0]>=0:
|
|
self.current_task, self.current_task_ret = task, None
|
|
log("Task detected: " + str(self.current_task), False)
|
|
return self.current_task
|
|
|
|
def wait_task_finished (self, polling = None):
|
|
cur_polling = self.polling
|
|
if polling is not None:
|
|
self.polling = polling
|
|
try:
|
|
while self.get_task() != None:
|
|
time.sleep(self.polling_interval)
|
|
finally:
|
|
if polling is not None:
|
|
self.polling = cur_polling
|
|
ret = self.get_task_ret()
|
|
return ret
|
|
|
|
def assert_no_task(self):
|
|
task = self.check_task()
|
|
if task != None:
|
|
raise Exception("Ongoing task: " + task)
|
|
|
|
def on_event(self,ev):
|
|
pass
|
|
|
|
def on_change_working_mode(self, working_mode):
|
|
pass
|
|
|