from ch.psi.pshell.serial import TcpDevice import threading FRAME_DEFAULT = "world" import ch.psi.pshell.device.PositionerConfig as PositionerConfig ROBOT_MOTORS = ["x" , "y", "z", "rx", "ry", "rz"] class RobotCartesianMotor (PositionerBase): def __init__(self, robot, index): PositionerBase.__init__(self, robot.name + "_" + ROBOT_MOTORS[index], PositionerConfig()) self.index = index self.robot = robot #ATTENTION: Always initialize cartesian motors before scanning (or call robot.set_motors_enabled(True)) def doInitialize(self): self.setCache(None, None) self.setCache(self.doRead(), None) def doRead(self): return float("nan") if self.robot.cartesian_destination is None else float(robot.cartesian_destination[self.index]) def doWrite(self, value): if self.robot.cartesian_destination is not None: print "Move " + ROBOT_MOTORS[self.index] + " to " + str(value) self.robot.updating = True try: robot.cartesian_destination[self.index] = float(value) self.robot.set_pnt(robot.cartesian_destination , "tcp_p") self.robot.movel("tcp_p", self.robot.tool , DESC_SCAN) except: self.robot.updating = False def doReadReadback(self): #return float(self.robot.get_cartesian_pos(self._get_tool(),self.frame)[self.index]) return float("nan") if self.robot.cartesian_pos is None else float(self.robot.cartesian_pos[self.index]) ROBOT_JOINT_MOTORS = ["j1" , "j2", "j3", "j4", "j5", "j6"] class RobotJointMotor (PositionerBase): def __init__(self, robot, index): PositionerBase.__init__(self, robot.name + "_" + ROBOT_JOINT_MOTORS[index], PositionerConfig()) self.index = index self.robot = robot def doInitialize(self): self.setpoint = self.doReadReadback() self.setCache(self.doRead(), None) def doRead(self): return self.setpoint def doWrite(self, value): print "Move " + ROBOT_JOINT_MOTORS[self.index] + " to " + str(value) self.setpoint = value joint = self.robot.herej() joint[self.index] = value self.robot.updating = True try: self.robot.set_jnt(joint, name="tcp_j") self.robot.movej("tcp_j", self.robot.tool , DESC_SCAN) finally: self.robot.updating = False def doReadReadback(self): return self.robot.herej()[self.index] if self.robot.joint_pos is None else float(self.robot.joint_pos[self.index]) 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.high_level_tasks = [] self.cartesian_destination = None self.cartesian_pos = None self.joint_pos = None self.cartesian_motors_enabled = False self.cartesian_motors = [] self.joint_motors_enabled = False self.joint_motors = [] self.tool = None self.frame = FRAME_DEFAULT self.updating = False def doInitialize(self): super(RobotTCP, self).doInitialize() self.reset = True def set_tool(self,tool): self.tool = tool if self.cartesian_motors_enabled: self.cartesian_pos = self.get_cartesian_pos() for m in self.cartesian_motors: m.initialize() m.update() def get_tool(self): return self.tool def _sendReceive(self, msg_id, msg = "", timeout = None): 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, 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)>10: 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 type(ret) is str: 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: name = self.tool return self.get_trsf(name+".trsf") def set_tool_trsf(self, trsf, name=None): if name is None: name = self.tool self.set_trsf(trsf, name+".trsf") def eval_int(self, cmd): self.evaluate("tcp_n=" + cmd) return self.get_int() def eval_float(self, cmd): self.evaluate("tcp_n=" + cmd) return self.get_float() def eval_bool(self, cmd): 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 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): 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" 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, desc): return self.eval_int("movej(" + joint_or_point + ", " + tool + ", " + desc +")") def movel(self, point, tool, desc): return self.eval_int("movel(" + point + ", " + tool + ", " + desc +")") def movec(self, point_interm, point_target, tool, desc): return self.eval_int("movec(" + point_interm + ", " + point_target + ", " + tool + ", " + desc +")") #Tool #This function can timeout since it synchronizes move. #Better check state before otherwise it can freeze the communication def open_tool(self, tool): return self.evaluate("open(" + tool + " )", timeout=3000) #This function can timeout since it synchronizes move. Better check state before #Better check state before otherwise it can freeze the communication def close_tool(self, tool): return self.evaluate("close(" + tool + " )", timeout=3000) #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="tcp_j"): return self.eval_pnt("jointToPoint(" + tool + ", " + frame + ", " + joint +")") def point_to_joint(self, tool, initial_joint="tcp_j", point="tcp_p"): 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 + ")") #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")) 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)): cmd += str(args[i]) + (',' 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): 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 _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.get_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() #sts = self._sendReceive("EVT").strip().split(self.array_separator) sts = self.execute("get_status", self.current_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" ev = sts[7] if len(sts)>7 else "" if len(ev.strip()) >0: self.getLogger().info(ev) self.on_event(ev) #if (self.current_task is not None) and (self.get_task_status(self.current_task)[0]<0): if int(sts[6]) < 0: self.current_task = None self._update_state() self.reset = False self.setCache({"powered": self.powered, "speed": self.speed, "empty": self.empty, "settled": self.settled, "task": self.current_task, "mode": self.working_mode, "status": self.status }, None) if not self.updating: if self.cartesian_motors_enabled: self.cartesian_pos = self.get_cartesian_pos() for m in self.cartesian_motors: #m.update() m.readback.update() else: self.cartesian_pos = None if self.joint_motors_enabled: self.joint_pos = self.herej() 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_cartesian_pos(self): 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): 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) #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 = [] self.joint_destination = None else: if value: self.joint_destination = self.get_joint_destination() for m in self.joint_motors: m.initialize() #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(pro) if not program in tasks: tasks.append(pro) for task in tasks: if self.get_task_status(task)[0]>=0: raise Exception("Ongoing high-level task: " + task) self.task_create(program, *args, **kwargs) start = time.time() while self.get_task_status(program)[0] < 0: if time.time() - start > 5000: raise Exception("Cannot start task " + task) time.sleep(0.1) self.update() self.current_task = program self._update_state() 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) def get_task(self): if self.current_task is not None: return self.current_task for task in self.high_level_tasks: if self.get_task_status(task)[0]>=0: self.current_task = task return task return None def on_event(self,ev): pass TOOL_CALIBRATION = "tCalib" TOOL_SUNA= "tSuna" TOOL_DEFAULT= TOOL_SUNA DESC_FAST = "mFast" DESC_SLOW = "mSlow" DESC_SCAN = "mScan" DESC_DEFAULT = DESC_FAST simulation = True joint_forces = False class RobotSC(RobotTCP): def __init__(self, name, server, timeout = 1000, retries = 1): RobotTCP.__init__(self, name, server, timeout, retries) self.tool = TOOL_CALIBRATION def mount(self, puck, sample): return self.execute('mount',segment, puck, sample) def firstmount(self, puck, sample): return self.execute('firstmount', segment, puck, sample) def unmount(self, puck, sample): return self.execute('unmount',segment, puck, sample) def on_event(self,ev): #print "EVT: " + ev pass def doUpdate(self): #start = time.time() RobotTCP.doUpdate(self) global simulation if not simulation: if joint_forces: if self.state != State.Offline: self.get_joint_forces() for dev in [jf1, jf2, jf3, jf4,jf5, jf6, jfc]: dev.update() #print time.time() -start def start_task(self, program, *args, **kwargs): #TODO: Check safe position RobotTCP.start_task(self, program, *args, **kwargs) def stop_task(self): RobotTCP.stop_task(self) #TODO: Restore safe position def set_remote_mode(self): robot.set_profile("remote") def set_local(self): robot.set_profile("default") def set_tool(self,tool): if tool != self.tool: self.tool = tool for dev in (robot_x, robot_y, robot_z, robot_rx, robot_ry, robot_rz): dev.initialize() dev.update() if simulation: #add_device(RobotSC("robot","129.129.126.92:1000"),force = True) add_device(RobotSC("robot","129.129.110.99:1000"),force = True) else: add_device(RobotSC("robot", "129.129.110.100:1000"), force = True) robot.high_level_tasks = ["mount", "firstmount"] robot.setPolling(500)