class RobotTCP(TcpDevice, Stoppable): def __init__(self, name, server, timeout = 500, 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 def _sendReceive(self, msg_id, msg = ""): tx = self.header if (self.header != None) else "" tx = tx + msg_id + " " + msg if (len(tx)>127): raise Exception("Exceeded maximum message size") self.getLogger().finer("TX = '" + str(tx)+ "'") if (self.trailer != None): tx = tx + self.trailer rx = self.sendReceive(tx, None, self.trailer , 0, self.timeout, self.retries) rx=rx[:-1] #Remove 0A self.getLogger().finer("RX = '" + str(rx) + "'") if rx[:3] != msg_id: print rx 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): id = "%03d" % self.msg_id self.msg_id = (self.msg_id+1)%1000 return self._sendReceive(id, msg) def execute(self, command, *argv): msg = str(command) if len(argv)>10: raise Exception("Exceeded maximum number of parameters") for i in range(len(argv)): msg += (self.cmd_separator if (i==0) else self.array_separator) + str(argv[i]) rx = self.call(msg) if rx.count(self.array_separator)>0: return rx.split(self.array_separator) return rx def read_event(self): ev = self._sendReceive("EVT") if ev.strip() == "": return None return ev def evaluate(self, cmd): ret = self.execute('eval', cmd) if ret.strip() != "": raise Exception(ret) def get_var(self, name): return self.execute('get_var', name) def get_str(self, name): return self.execute('get_str', name) def get_arr(self, name, size): return self.execute('get_arr', name, size) def get_bool(self, name = "b"): return True if (self.execute('get_bool', name).strip() == '1') else False def get_int(self, name ="n"): return int(self.get_var(name)) def get_float(self, name ="n"): return float(self.get_var(name)) def get_int_arr(self, size, name="arr"): # not working. A Jython bug in PyUnicaode? #return [int(x) for x in self.get_arr("arr", 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="arr"): #return [float(x) for x in self.get_arr("arr", size)] a=self.get_arr(name, size) ret = []; for i in range(size): ret.append(float(a[i])); return ret def get_trf(self, name="t"): a = self.execute('get_trf', name) ret = [] for i in range(6): ret.append(float(a[i])) return ret def set_tr(self, l, name="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="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="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="p"): a = self.execute('get_pnt', name) ret = [] for i in range(6): ret.append(float(a[i])) return ret def eval_int(self, cmd): self.evaluate("n=" + cmd) return self.get_int() def eval_float(self, cmd): self.evaluate("n=" + cmd) return self.get_float() def eval_bool(self, cmd): self.evaluate("b=" + cmd) return self.get_bool() def eval_jnt(self, cmd): self.evaluate("j=" + cmd) return self.get_jnt() def eval_trf(self, cmd): self.evaluate("t=" + cmd) return self.get_trf() def eval_pnt(self, cmd): self.evaluate("p=" + cmd) return self.get_pnt() #Robot control def is_powered(self): self.powered = self.eval_bool("isPowered()") return self.powered def get_monitor_speed(self): return self.eval_int("getMonitorSpeed()") 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 enable(self): return self.evaluate("enablePower()") def disable(self): return self.evaluate("disablePower()") def is_calibrated(self): return self.eval_bool("isCalibrated()") def read_working_mode(self): try: mode = self.eval_int("workingMode(arr)") status = int(self.get_var("arr[0]")) 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" except: self.working_mode = "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("") #Motion control def stop(self): self.evaluate("stopMove()") def resume(self): self.evaluate("restartMove()") def resetMotion(self): self.evaluate("resetMotion()") def is_empty(self): self.empty = self.eval_bool("isEmpty()") return self.empty def is_settled(self): self.settled = self.eval_bool("isSettled()") return self.powered 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): robot_tcp.evaluate("getJointForce(arr)") return robot_tcp.get_float_arr(6) #Tool #This function can timeout since it synchronizes move. Better check state before def open(self, tool): return self.evaluate("open(" + tool + " )") #This function can timeout since it synchronizes move. Better check state before def close(self, tool): return self.evaluate("close(" + tool + " )") #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, trsf): return self.eval_pnt("compose(" + pnt + ", " + frame + ", " + trsf + ")") def here(self, tool, frame): return self.eval_pnt("here(" + tool + ", " + frame + ")") def joint_to_point(self, tool, frame, joint): return self.eval_pnt("jointToPoint(" + tool + ", " + frame + ", " + joint +")") def point_to_joint(self, tool, initial_joint, point): if self.eval_bool("pointToJoint(" + tool + ", " + initial_joint + ", " + point +", j)"): return self.get_jnt() def position(self, point, frame): return self.eval_trf("position(" + point + ", " + frame + ")") #Updating def update_status(self): #TODO: This can optimized with a single call self.read_working_mode() self.is_powered() self.get_monitor_speed() self.is_empty() self.is_settled() def poll_events(self): ev = self.read_event() if ev is not None: self.getLogger().info(ev) self.on_event(ev) def doUpdate(self): try: start = time.time() self.update_status() self.poll_events() self.setState(State.Busy if self.status=="move" else State.Ready) self.setState(State.Ready if self.empty and self.settled else State.Busy) #print time.time() - start except Exception, e: print >> sys.stderr, "Update error: " + str(e) self.setState(State.Offline) def on_event(self,ev): pass def mount(self, puck, sample): return self.execute('mount', puck, sample) add_device(RobotTCP("robot_tcp", "129.129.126.100:1000"), force = True) robot_tcp.setPolling(500) #robot_tcp.set_monitor_speed(20)