import threading 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) #DeviceBase.__init__(self, name, DeviceConfig({"frame": "f_4mRad"})) self.on_poll_status = {} self.on_poll_status_set = {} 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.last_remote_motion = "None" self.cartesian_destination = None self.spherical_destination = None self.linear_axis_destination = None #self.flange_pos = [None] * 6 self.cartesian_pos = {} self.spherical_pos = {} self.joint_pos = {} self.linear_axis_pos = {} self.cartesian_motors_enabled = False self.cartesian_motors = {} self.spherical_motors_enabled = False self.spherical_motors = {} self.linear_axis_motors_enabled = None self.linear_axis_motors = {} self.joint_motors_enabled = False self.joint_motors = {} self.tool = AdjustableFS(name="tool", default_value="t_JF01T03") self.tool_persistent = AdjustableFS(name="tool_persistent", default_value="t_JF01T03") self.default_desc = None self.tool_open = None self.tool_trsf = [0.0] * 6 self.frame = AdjustableFS(name="frame", default_value="f_4mRad") self.frame_persistent = AdjustableFS(name="frame_persistent", default_value="f_4mRad") self.frame_trsf = [0.0] * 6 self.polling_interval = 0.01 self.reset = True self.default_tolerance = 5 self.default_speed = 100 self.latency = 0 self.last_msg_timestamp = 0 self.task_start_retries = 3 self.exception_on_task_start_failure = True #Tasks may start and be finished when checked self.simulated_point = "" self.update_start_time = 0 self.polling_env = 1 def doInitialize(self): super(RobotTCP, self).doInitialize() self.reset = True def set_tool(self, tool): self.tool(tool) self.tool_persistent(tool) self.set_str(tool, name="s_actualTool") self.evaluate("t_actualTool=" + tool) #self.tool_trsf = self.get_tool_trsf() 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, name=None, change_default=True): if name is not None: self.evaluate(name + "=" + frame) if change_default: self.set_str(frame, name="s_actualFrame") self.evaluate("f_actualFrame" + "=" + frame) self.frame(frame) self.frame_persistent(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(self.frame()) 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): if self.latency >0: timespan = time.time() - self.last_msg_timestamp if timespan150): 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) self.last_msg_timestamp = time.time() 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 - Still true? def get_str(self, name='tcp_s'): return self.execute('get_str', name) def set_str(self, l, name = "tcp_s"): self.evaluate(name + '="' + str(l) + '"') 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"): l = [round(ll, 3) for ll in l] 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("tcp_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(5.0) if not self.is_powered(): raise Exception("Enabling power timed out after 5s.") #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): modestr = { 1: { "mode": "manual", "status": { 0: "programmed", 1: "connection", 2: "joint", 3: "cartesian_frame", 4: "cartesian_tool", 5: "point", 6: "hold", }, }, 2: { "mode": "test", "status": { 0: "programmed", 1: "connection", 2: "programmed_fast", 3: "hold", }, }, 3: { "mode": "local", "status": { 0: "programmed", 1: "connection", 2: "hold", }, }, 4: { "mode": "remote", "status": { 0: "programmed", 1: "connection", 2: "hold", }, }, } prev_status = self.status prev_mode = self.working_mode if mode in modestr.keys(): self.working_mode = modestr[mode]["mode"] self.status = modestr[mode]["status"][status] else: self.working_mode = "invalid" self.status = "invalid" if self.working_mode != prev_mode: try: self.on_change_working_mode(self.working_mode, prev_mode) except: pass if self.status != prev_status: try: self.on_change_status(self.status, prev_status) 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, msg=""): self.evaluate("stopMove()") #get_context().sendEvent("stop", "Stop cmd received: stopping robot motions! \n" + msg) def resume(self): self.evaluate("restartMove()") def reset_motion(self, joint=None, timeout=None, msg=""): self.evaluate("resetMotion()" if (joint is None) else ("resetMotion(" + joint + ")")) ### reinitialize all motors (target values) if self.cartesian_motors_enabled: for m in self.cartesian_motors.values(): m.doInitialize() if self.spherical_motors_enabled: for m in self.spherical_motors.values(): m.doInitialize() if self.linear_axis_motors_enabled: for m in self.linear_axis_motors.values(): m.doInitialize() if self.joint_motors_enabled: for m in self.joint_motors.values(): m.doInitialize() #get_context().sendEvent("reset_motion", "Reset motion cmd received: Cancelling all stored movement commands on controller! \n" + msg) 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 self.isSimulated(): self.simulated_point = "" 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" #TODO: in new robot movel and movej is freezing controller ret = self.eval_int("movej(" + joint_or_point + ", " + tool + ", " + desc +")") #ret = int(self.execute('movej',joint_or_point, tool, desc)) if sync: self.wait_end_of_move() if self.isSimulated(): self.simulated_point = joint_or_point return ret def movel(self, point, tool=None, desc=None, sync=False): if self.isSimulated(): self.simulated_point = "" 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" #TODO: in new robot movel and movej is freezing controller ret = self.eval_int("movel(" + point + ", " + tool + ", " + desc +")") #ret = int(self.execute('movel',point, tool, desc)) if sync: self.wait_end_of_move() if self.isSimulated(): self.simulated_point = point return ret def movec(self, point_interm, point_target, tool=None, desc=None, sync=False): if self.isSimulated(): self.simulated_point = "" if desc is None: desc = self.default_desc if tool is None: tool = self.tool() #TODO: in new robot movel and movej is freezing controller ret = self.eval_int("movec(" + point_interm + ", " + point_target + ", " + tool + ", " + desc +")") #ret = int(self.execute('movec', point_interm, point_target, tool, desc)) if sync: self.wait_end_of_move() if self.isSimulated(): self.simulated_point = point_target 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) if priority<1 or priority > 100: raise Exception("Invalid priority: " + str(priority)) cmd = 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+=')' #TODO: in new robot exec taskCreate is freezing controller #REMOVE if bug is fixed #self.execute('task_create',name, str(priority), program, *args) print 'taskCreate "' + name + '", ' + str(priority) + ', ' + cmd self.evaluate('taskCreate "' + name + '", ' + str(priority) + ', ' + cmd) if self.isSimulated(): self.simulated_point = "" 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" sleep(5) robot.set_default_desc(DESC_DEFAULT) robot.default_speed = DEFAULT_SPEED robot.set_frame(self.frame_persistent()) robot.set_frame(self.frame_persistent(), name="tcp_f_spherical", change_default=False) robot.set_tool(self.tool_persistent()) robot.setPolling(DEFAULT_ROBOT_POLLING) 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_env(self): 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_env", 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 self.isSimulated(): if time.time() - self.current_task_timestamp > 3.0: log("Task "+ str(cur_task) + " finished with code: -1", False) ret = -1 if self.current_task_timestamp>0 else None self.on_task_finished(self.current_task, ret) self.current_task, self.current_task_ret = None, ret else: if int(sts[6]) < 0: try: ret = int(sts[7]) except: ret = None log("Task "+ str(cur_task) + " finished with code: " + str(ret), False) self.on_task_finished(cur_task, ret) self.current_task, self.current_task_ret = None, ret self.frame(sts[8]) self.tool(sts[9]) for i in range(6): self.frame_trsf[i] = float(sts[10 + i]) for i in range(6): self.tool_trsf[i] = float(sts[16 + i]) self.update_spherical_pos() ev_index = 22 #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, }, None) self.on_poll_status.update({ "connected": self.connected, "powered": self.powered, "override_remote_safety": self.override_remote_safety, "speed": self.speed, "settled": self.settled, "task": self.current_task, "mode": self.working_mode, "status": self.status, "frame": self.get_frame_tree(self.frame()), "frame_coordinates": self.frame_trsf, "tool": self.get_frame_tree(self.tool()), "tool_coordinates": self.tool_trsf, "cartesian_motors_enabled": self.cartesian_motors_enabled, "spherical_motors_enabled": self.spherical_motors_enabled, }) def doUpdate_fast(self): 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_fast", cur_task) for i, name in enumerate(ROBOT_JOINT_MOTORS): self.joint_pos[name] = float(sts[0 + i]) for i, name in enumerate(ROBOT_CARTESIAN_MOTORS): self.cartesian_pos[name] = float(sts[6 + i]) self.linear_axis_pos = {"z_lin": float(sts[12])} self.update_spherical_pos() ev_index = 8 #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.setCache({ # "pos": self.get_current_point_cached() if self.state==State.Ready else None #TODO: make it calculated in robot by polling funtion # }, None) pos = self.cartesian_pos.copy() pos.update(self.spherical_pos) pos.update(self.joint_pos) pos.update(self.linear_axis_pos) self.on_poll_status.update({ "pos": pos, }) if self.cartesian_motors_enabled: for m in self.cartesian_motors.values(): m.readback.update() if self.spherical_motors_enabled: for m in self.spherical_motors.values(): m.readback.update() if self.linear_axis_motors_enabled: for m in self.linear_axis_motors.values(): m.readback.update() if self.joint_motors_enabled: for m in self.joint_motors.values(): m.readback.update() def doUpdate(self): try: self.doUpdate_fast() except: if self.state != State.Offline: print >> sys.stderr, "Update error: " + str(sys.exc_info()[1]) self.setState(State.Offline) if time.time() - self.update_start_time >= self.polling_env: self.update_start_time = time.time() try: self.doUpdate_env() except: if self.state != State.Offline: print >> sys.stderr, "Update error: " + str(sys.exc_info()[1]) self.setState(State.Offline) self.on_polling() def doUpdate_backup(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 self.isSimulated(): if time.time() - self.current_task_timestamp > 3.0: log("Task "+ str(cur_task) + " finished with code: -1", False) ret = -1 if self.current_task_timestamp>0 else None self.on_task_finished(self.current_task, ret) self.current_task, self.current_task_ret = None, ret else: if int(sts[6]) < 0: try: ret = int(sts[7]) except: ret = None log("Task "+ str(cur_task) + " finished with code: " + str(ret), False) self.on_task_finished(cur_task, ret) self.current_task, self.current_task_ret = None, ret for i in range(6): self.joint_pos[ROBOT_JOINT_MOTORS[i]] = float(sts[8 + i]) for i in range(6): self.cartesian_pos[ROBOT_MOTORS[i]] = float(sts[14 + i]) self.linear_axis_pos = {"z_lin": float(sts[20])} self.update_spherical_pos() ev_index = 21 #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) pos = self.cartesian_pos.copy() pos.update(self.spherical_pos) pos.update(self.joint_pos) pos.update(self.linear_axis_pos) self.on_poll_status = { "connected": self.connected, "powered": self.powered, "speed": self.speed, "settled": self.settled, "task": self.current_task, "mode": self.working_mode, "override_remote_safety": self.override_remote_safety, "status": self.status, "frame": self.frame(), "tool": self.tool(), "pos": pos, "cartesian_motors_enabled": self.cartesian_motors_enabled, "spherical_motors_enabled": self.spherical_motors_enabled, } get_context().sendEvent("polling", self.on_poll_status) if self.cartesian_motors_enabled: for m in self.cartesian_motors.values(): m.readback.update() if self.spherical_motors_enabled: for m in self.spherical_motors.values(): m.readback.update() if self.linear_axis_motors_enabled: for m in self.linear_axis_motors.values(): m.readback.update() if self.joint_motors_enabled: for m in self.joint_motors.values(): m.readback.update() except: if self.state != State.Offline: print >> sys.stderr, "Update error: " + str(sys.exc_info()[1]) self.setState(State.Offline) def on_poll_info(self): on_poll_config_methods ={ "powered": {"cmd":"robot.set_powered", "def_kwargs": ""}, "override_remote_safety": {"cmd":"robot.set_override_remote_safety", "def_kwargs": ""}, "speed": {"cmd":"robot.set_monitor_speed", "def_kwargs": ""}, "frame": {"cmd": "robot.set_frame", "def_kwargs": "change_default=True",}, "frame_coordinates": {"cmd": "robot.set_frame_coordinates", "def_kwargs": "",}, "tool": {"cmd":"robot.set_tool", "def_kwargs": ""}, "tool_coordinates": {"cmd": "robot.set_tool_coordinates", "def_kwargs": "",}, "cartesian_motors_enabled": {"cmd":"robot.set_motors_enabled", "def_kwargs": "pseudos=['cartesian']"}, "spherical_motors_enabled": {"cmd":"robot.set_motors_enabled", "def_kwargs": "pseudos=['spherical']"}, } on_poll_info = [k for k in self.on_poll_status if not k in list(on_poll_config_methods.keys())+["pos"]] return {"info": on_poll_info, "config": on_poll_config_methods} def set_tool_coordinates(self, v, name=None): if name is None: name = self.tool() self.set_trsf(v, name=name + ".trsf") if name == self.tool(): self.set_trsf(v, name="t_actualTool.trsf") def set_frame_coordinates(self, v, name=None): if name is None: name = self.frame() self.set_trsf(v, name=name + ".trsf") if name == self.frame(): self.set_trsf(v, name="f_actualFrame.trsf") #Cartesian space def get_cartesian_pos(self, tool=None, frame=None, return_dict=True): if tool is None: self.assert_tool() tool = self.tool() if frame is None: frame = self.frame() #Do not work #self.evaluate("tcp_j=herej(); tcp_p=jointToPoint(" + tool + ", " + frame + ", tcp_j)") self.evaluate("tcp_j=herej()") self.evaluate("tcp_p=jointToPoint(" + tool + ", " + frame + ", tcp_j)") x,y,z,rx,ry,rz = self.get_pnt() if return_dict: ret = {"x": x, "y": y, "z": z, "rx": rx, "ry": ry, "rz": rz} else: ret = [x, y, z, rx, ry, rz] return ret def get_joint_pos(self, return_dict=True): js = self.herej() if return_dict: ret = {k: v for k, v in zip(["j1", "j2", "j3", "j4", "j5", "j6"], js)} else: ret = js return ret def get_flange_pos(self, frame=None, return_dict=True): return self.get_cartesian_pos(FLANGE, frame, return_dict=return_dict) def get_linear_axis_pos(self, return_dict = True): val = self.get_float("n_actPosLinAx") if return_dict: return {"z_lin": val} else: return val def get_cartesian_destination(self, tool=None, frame=None,return_dict=True): if tool is None: self.assert_tool() tool = self.tool() if frame is None: frame = self.frame() #return self.here(tool, frame) self.get_cartesian_pos(return_dict=return_dict) def get_spherical_destination(self, tool=None, frame=None): if tool is None: self.assert_tool() tool = self.tool() if frame is None: frame = self.frame() #return self.here(tool, frame) return self.get_spherical_pos() def get_distance_to_pnt(self, name): if self.isSimulated(): if name and (name==self.simulated_point): return 0 else: return 10000 #self.here(self.tool, self.frame) #??? self.set_pnt(self.get_cartesian_pos(return_dict=False) ) 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=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 self.isSimulated(): 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, self.current_task_timestamp = program, None, time.time() 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) if self.isSimulated(): self.current_task_timestamp=-1 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_polling(self): get_context().sendEvent("polling", self.on_poll_status) def on_event(self,ev): pass def on_change_working_mode(self, working_mode, prev_working_mode): if working_mode == "remote": self.set_profile("remote") get_context().sendEvent("motion", "Working mode changed from {} to {}, changing user. \n".format(prev_working_mode, working_mode)) else: self.set_profile("default") def on_change_status(self, status, prev_status): if status in ["joint", "cartesian_frame", "cartesian_tool", "point"]: msg = "Manual mode change to " + status + " mode detected" print(msg) self.reset_motion(msg=msg) get_context().sendEvent("reset_motion", "Cancelling all stored movement commands on controller! \n" + msg) pass def on_task_finished(self, task, ret): pass