import math PVBASE = "SARES20-ROB:" KNOWN_POINTS = P_PARK, P_HOME = "park", "home" TASKS = MOVE_PARK, MOVE_HOME, TWEAK_X , TWEAK_Y = "movePark", "moveHome", "tweakX", "tweakY" DESCS = DESC_FAST,DESC_SLOW, = "mNomSpeed", "mNomSpeed" DESC_DEFAULT = DESCS[0] FLANGE = "flange" #DEFAULT_ROBOT_POLLING = 200 DEFAULT_ROBOT_POLLING = 200 TASK_WAIT_ROBOT_POLLING = 50 DEFAULT_SPEED=100 ROBOT_JOINT_MOTORS = ["j1" , "j2", "j3", "j4", "j5", "j6"] ROBOT_CARTESIAN_MOTORS = ["x" , "y", "z", "rx", "ry", "rz"] ROBOT_SPHERICAL_MOTORS = ["r" , "gamma", "delta"] FRAME_HIRARCHY = ["world.f_SwissFELCoord.f_ESBWorld.f_linearAxis." + k for k in ["f_direct.", "f_2mRad.", "f_4mRad.", "f_6mRad.", "f_8mRad.",]] FRAME_HIRARCHY += ["t_flange.t_Adapter." + k for k in ["t_JF01T03det.t_JF01T03.", "t_JF07T32det.t_JF07T32."]] run("devices/RobotTCP") override_remote_safety = False simulation = False class RobotBernina(RobotTCP): def __init__(self, name, server, timeout = 1000, retries = 1): RobotTCP.__init__(self, name, server, timeout, retries) self.set_tasks(TASKS) self.set_known_points(KNOWN_POINTS) self.setPolling(DEFAULT_ROBOT_POLLING) self.last_command_timestamp = None self.last_command_position = None self.override_remote_safety = override_remote_safety temp = {"frame": Array([]), "tool": Array([]), "pos": Array([])} default_remote_allowed_recorded = {"spherical": temp, "cartesian": temp, "joint": temp} default_remote_allowed_deltas = { "spherical": {'delta': 1., 'r': 50., 'gamma': 1.}, "cartesian": {'rx': 1., 'ry': 1., 'rz': 1., 'x': 1., 'y': 1., 'z': 1.}, "joint": {'j1': 1., 'j2': 1., 'j3': 1., 'j4': 1., 'j5': 1., 'j6': 1.}, } self.remote_allowed_recorded = AdjustableFS(name="remote_allowed", default_value = default_remote_allowed_recorded) self.remote_allowed_deltas = AdjustableFS(name="remote_allowed_deltas", default_value = default_remote_allowed_deltas) def get_frame_tree(self, frame): """frame can be a string of a tool or a frame""" for f in FRAME_HIRARCHY: if frame in f: return f.split("." + frame + ".")[0]+"." + frame return frame def set_override_remote_safety(self, value): self.override_remote_safety = bool(value) def reset_recorded_motions(self, index=None, motion=None): if any([index is None, motion is None]): temp = {"frame": Array([]), "tool": Array([]), "pos": Array([])} default_remote_allowed_recorded = {"spherical": temp, "cartesian": temp, "joint": temp} self.remote_allowed_recorded(default_remote_allowed_recorded) return "removed all recorded motions" else: rem = self.remote_allowed_recorded() for k in ["pos", "tool", "frame"]: rem[motion][k].pop[index] self.remote_allowed_recorded(rem) return "removed recorded " + motion + " motion with index " + str(index) def remote_allowed(self, from_coordinates, to_coordinates, frame_trsf, tool_trsf, motion): if self.override_remote_safety: return True ks = ROBOT_CARTESIAN_MOTORS if motion == "cartesian" else ROBOT_SPHERICAL_MOTORS if motion == "spherical" else ROBOT_JOINT_MOTORS # spherical motors are missing rx ry rz, maybe even in simulation a1 = [from_coordinates[k] for k in ks] a2 = [to_coordinates[k] for k in ks] ds = Array([self.remote_allowed_deltas()[motion][k] for k in ks]) cds = Array([self.remote_allowed_deltas()[motion][k] for k in ks]) rc = self.remote_allowed_recorded()[motion] if len(rc["pos"]) > 0: #preselection by frame with concatenated arrays if "frame" in rc.keys(): fr = Array(rc["frame"]) idxs = (fr-frame_trsf).abs < cds idxs_b =[all(idx) for idx in idxs] rc = {"pos": Array(rc["pos"])[idxs_b], "tool": Array(rc["tool"])[idxs_b]} if len(rc["pos"]) > 0: if "tool" in rc.keys(): tr = rc["tool"] idxs = (tr-tool_trsf).abs < cds idxs_b =[all(idx) for idx in idxs] rc = {"pos": Array(rc["pos"])[idxs_b]} if len(rc["pos"]) > 0: for tr in rc["pos"]: idxs1 = (Array(tr)-a1).abs < ds idxs2 = (Array(tr)-a2).abs < ds idx1 = [all(idx) for idx in idxs1] idx2 = [all(idx) for idx in idxs2] if (any(idx1)) & (any(idx2)): return True print("finish") return False def general_motion(self, **kwargs): ## check motion type self.reset_motion() if (sum([m in kwargs.keys() for m in ROBOT_JOINT_MOTORS])>0) & (sum([m in kwargs.keys() for m in ROBOT_SPHERICAL_MOTORS])==0) & (sum([m in kwargs.keys() for m in ROBOT_CARTESIAN_MOTORS])==0): move = self.move_joint elif (sum([m in kwargs.keys() for m in ROBOT_CARTESIAN_MOTORS])>0) & (sum([m in kwargs.keys() for m in ROBOT_SPHERICAL_MOTORS])==0) & (sum([m in kwargs.keys() for m in ROBOT_JOINT_MOTORS])==0): move = self.move_cartesian elif (sum([m in kwargs.keys() for m in ROBOT_SPHERICAL_MOTORS])>0) & (sum([m in kwargs.keys() for m in ROBOT_CARTESIAN_MOTORS])==0) & (sum([m in kwargs.keys() for m in ROBOT_JOINT_MOTORS])==0): move = self.move_spherical else: get_context().sendEvent("motion", "\nNo unique coordinate system found for these motions use only unique keywords out of:\n {}\n {}\n {}.".format(ROBOT_JOINT_MOTORS, ROBOT_CARTESIAN_MOTORS, ROBOT_SPHERICAL_MOTORS)) return self.last_remote_motion = "recording" move(sync = True, **kwargs) return True def record_motion(self, **kwargs): ## check motion type self.reset_motion(msg = "RECORDING TRAJECTORY TO LIST OF ALLOWED REMOTE MOTIONS - USE HANDHELD TO FINISH MOTION") if self.working_mode != "manual": get_context().sendEvent("motion", "Current working mode is {}. Change it to manual using the keyswitch to record motions.".format(self.working_mode)) if (sum([m in kwargs.keys() for m in ROBOT_JOINT_MOTORS])>0) & (sum([m in kwargs.keys() for m in ROBOT_SPHERICAL_MOTORS])==0) & (sum([m in kwargs.keys() for m in ROBOT_CARTESIAN_MOTORS])==0): k = "joint" move = self.move_joint elif (sum([m in kwargs.keys() for m in ROBOT_CARTESIAN_MOTORS])>0) & (sum([m in kwargs.keys() for m in ROBOT_SPHERICAL_MOTORS])==0) & (sum([m in kwargs.keys() for m in ROBOT_JOINT_MOTORS])==0): k = "cartesian" move = self.move_cartesian elif (sum([m in kwargs.keys() for m in ROBOT_SPHERICAL_MOTORS])>0) & (sum([m in kwargs.keys() for m in ROBOT_CARTESIAN_MOTORS])==0) & (sum([m in kwargs.keys() for m in ROBOT_JOINT_MOTORS])==0): k = "spherical" move = self.move_spherical else: get_context().sendEvent("motion", "\nNo unique coordinate system found for these motions use only unique keywords out of:\n {}\n {}\n {}.".format(ROBOT_JOINT_MOTORS, ROBOT_CARTESIAN_MOTORS, ROBOT_SPHERICAL_MOTORS)) return if k == "spherical": interp = Array([robot.cart2sph(return_dict=False, **{k:e for k,e in zip(ROBOT_CARTESIAN_MOTORS,v)}) for v in move(simulate=True, coordinates = "cartesian", **kwargs)]).T.interpolate(10).T else: interp = Array(move(simulate=True, coordinates = k, **kwargs)).T.interpolate(10).T self.last_remote_motion = "recording" move(sync = True, **kwargs) frame = Array(self.frame_trsf) tool = Array(self.tool_trsf) recorded = self.remote_allowed_recorded() recorded[k]["frame"].append(frame) recorded[k]["tool"].append(tool) recorded[k]["pos"].append(interp) self.remote_allowed_recorded(recorded) self.on_recording_finished() return True def on_recording_finished(self): get_context().sendEvent("motion", "Finished, recording") def set_motion_queue_empty(self, state=False): ### update the status of the robot, the queue of motion commands ### the status is polled automatically from the controller but this setting command does not have a delay self.empty = state self._update_state() def check_software_limits(self, j1=0, j2=0, j3=0, j4=0, j5=0, j6=0): j = [j1,j2,j3,j4,j5,j6] self.set_jnt(j, name="tcp_j") self.evaluate("tcp_b=isInRange(tcp_j)") return self.get_bool(name="tcp_b") def deg2rad(self, angles): if hasattr(angles, "__len__"): return [a/180.*math.pi for a in angles] else: return angles/180.*math.pi def rad2deg(self, angles): if hasattr(angles, "__len__"): return [a*180./math.pi for a in angles] else: return angles*180./math.pi def get_spherical_pos(self, frame = None, return_dict=True): cur_cart = self.get_cartesian_pos(frame = frame, return_dict=True) cur_sph = self.cart2sph(return_dict=return_dict, **cur_cart) return cur_sph def update_spherical_pos(self): self.spherical_pos=self.cart2sph(return_dict=True, **self.cartesian_pos) def get_movel_interpolation(self, i, coordinates="joint", idx_pnt1=0, idx_pnt2=1): """ coordinates: "joint" or "cartesian" returns the interpolated position in joint or cartesian coordinates at fraction i of the linear motion from point tcp_p_spherical[idx_pnt1] to tcp_p_spherical[idx_pnt2]. Note: i=0 returns the start point, i=1 the end point of the motion """ a = self.execute("get_movel_interpolation", i, coordinates, idx_pnt1, idx_pnt2) ret = [] for i in range(6): ret.append(float(a[i])) return ret def get_movec_interpolation(self, i, coordinates="joint"): """ coordinates: "joint" or "cartesian" returns the interpolated position in joint or cartesian coordinates at fraction i of the circular motion along the circle given by the start point tcp_p_spherical[1], the intermediate point tcp_p_spherical[2] and the target tcp_p_spherical[3]. Note: i=0 returns the start point, i=1 the end point of the motion """ a = self.execute("get_movec_interpolation", i, coordinates) ret = [] for i in range(6): ret.append(float(a[i])) return ret def sph2cart(self, r=None, gamma=None, delta=None, return_dict=True, **kwargs): gamma, delta = self.deg2rad([gamma, delta]) x = r*math.cos(delta)*math.sin(gamma) y = r*math.sin(delta) z = r*math.cos(delta)*math.cos(gamma) # New try first delta ry = -math.asin(-math.cos(delta)*math.sin(gamma)) rx = (-math.acos(math.cos(delta)*math.cos(gamma)/math.cos(ry))) rz = math.asin(-math.sin(ry)*math.sin(rx)*math.sqrt(1/(math.cos(rx)**2+(math.sin(ry)*math.sin(rx))**2))) rx, ry, rz = self.rad2deg([rx, ry, rz]) #end new try #Necessary to keep detector orientation for gamma larger 90 or smaller -90 if self.rad2deg(gamma) > 90: rz = 180-rz if self.rad2deg(gamma) < -90: rz = -180-rz if self.rad2deg(delta) > 90: rz = -rz #For delta --> 90, the calculation of rz runs into numerical issues: if (self.rad2deg(delta) < 90.05)&(self.rad2deg(delta)>89.95): rz = self.rad2deg(gamma) if abs(rz//180) > 0: rz = rz - (rz+180)//360*360 #Necessary correction for delta < 0 rx = math.copysign(1, delta)*rx rz = math.copysign(1, delta)*rz 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 cart2sph(self, x=None, y=None, z=None, return_dict=True, **kwargs): r = math.sqrt(x**2 + y**2 + z**2) if z**2+x**2>0: gamma = math.copysign(1, x)*math.acos(z/math.sqrt(z**2+x**2)) else: ry = kwargs["ry"] if "ry" in kwargs.keys() else self.cartesian_pos["ry"] gamma, = self.deg2rad([ry]) delta = math.asin(y/r) gamma, delta = self.rad2deg([gamma, delta]) if return_dict: ret = {"r": r, "gamma": gamma, "delta": delta} else: ret = [r,gamma, delta] return ret def check_calculations(self): sph = self.get_spherical_pos() cart_calc = self.sph2cart(**sph) cart = self.get_cartesian_pos(return_dict=True) print("spherical", sph) print("cart_calc", cart_calc) print("cart", cart) err = {k: cart[k]-cart_calc[k] for k in cart_calc.keys()} return err def point_reachable(self, point,tool=None, verbose=True): if tool == None: tool = self.tool() self.herej() reachable = self.eval_bool("pointToJoint(" + tool + ",tcp_j," + point + ",j)") if verbose: if not reachable: get_context().sendEvent("motion", "Point is not in range") print("Point is not in range") return reachable def simulate_stored_commands(self): if self.last_remote_motion == "None": ret = None elif self.last_remote_motion == "cartesian": target = self.cartesian_motors["x"].target_pos ret = self.move_cartesian(simulate = True, **target) elif self.last_remote_motion == "spherical": target = self.spherical_motors["r"].target_pos ret = self.move_spherical(simulate = True, **target) elif self.last_remote_motion == "joint": target = self.joint_motors["j1"].target_pos ret = self.move_joint(simulate = True, **target) return ret def move_cartesian(self, x=None, y=None, z=None, rx=None, ry=None, rz=None, tool=None, frame=None, desc=None, sync=False, simulate=False, coordinates="joint"): """ Motion in the cartesian coordinate system using a linear motion movel command to move from point tcp_p_spherical[0] to tcp_p_spherical[1]. frame: string The frame defines the cartesian coordinate system and origin. Must exist on the controller. sync: True or False If true, no further commands are accepted until the motion is finished. simulate: True or False coordinates: "joint" or "cartesian" If simulate = True, an array of interpolated positions in either joint or cartesian coordinates is returned. Setting coordinates only has an effect, when the motion is simulated. """ sim = [] if frame == None: frame = self.frame() if tool == None: tool = self.tool() if desc == None: desc = self.default_desc self.set_frame(frame, name="tcp_f_spherical", change_default=False) target_cart = {"x":x, "y":y, "z":z, "rx":rx, "ry":ry, "rz":rz} current_cart = self.get_cartesian_pos(frame = frame, return_dict=True) for k, v in target_cart.items(): if v == None: target_cart[k] = current_cart[k] if (self.working_mode == "remote") & (~simulate): if not self.remote_allowed(current_cart, target_cart, frame_trsf=self.get_trsf(frame+".trsf"), tool_trsf=self.get_trsf(tool+".trsf"), motion="cartesian"): get_context().sendEvent("motion", "Requested remote motion is not in recorded trajectories. Use rob.record_motion(*args) to record the motion in manual working mode first.") return False self.cartesian_destination = target_cart self.set_pnt([current_cart[k] for k in ["x", "y", "z", "rx", "ry", "rz"]], name="tcp_p_spherical[0]") self.set_pnt([target_cart[k] for k in ["x", "y", "z", "rx", "ry", "rz"]], name="tcp_p_spherical[1]") if self.point_reachable("tcp_p_spherical[1]", verbose=True): if simulate: for i in range(11): sim.append(self.get_movel_interpolation(i/10., coordinates=coordinates)) else: self.set_motion_queue_empty(False) ret = self.movel("tcp_p_spherical[1]", tool=tool, desc=None, sync=sync) else: return False if simulate: return sim else: return ret def move_spherical(self, r=None, gamma=None, delta=None, tool=None, frame=None, desc=None, sync=False, simulate=False, coordinates="joint"): """ Motion in the spherical coordinate system using a linear moteion movel command to change the radius from point tcp_p_spherical[0] to tcp_p_spherical[1], followed by a circular motion along the circle given by the start point tcp_p_spherical[1], the intermediate point tcp_p_spherical[2] and the target tcp_p_spherical[3]. frame: string The frame defines the cartesian coordinate system and origin. Must exist on the controller. sync: True or False If true, no further commands are accepted until the motion is finished. simulate: True or False coordinates: "joint" or "cartesian" If simulate = True, an array of interpolated positions in either joint or cartesian coordinates is returned. Setting coordinates only has an effect, when the motion is simulated. """ sim = [] if frame == None: frame = self.frame() if tool == None: tool = self.tool() if desc == None: desc = self.default_desc self.set_frame(frame, name="tcp_f_spherical", change_default=False) current_cart = self.get_cartesian_pos(frame = frame, return_dict=True) current_sph = self.get_spherical_pos(frame = frame, return_dict=True) target_sph = {"r": r, "gamma": gamma, "delta": delta} self.spherical_destination = target_sph for k, v in target_sph.items(): if v == None: target_sph[k] = current_sph[k] if (self.working_mode == "remote") & (~simulate): if not self.remote_allowed(current_sph, target_sph, frame_trsf=self.get_trsf(frame+".trsf"), tool_trsf=self.get_trsf(tool+".trsf"), motion="spherical"): get_context().sendEvent("motion", "Requested remote motion is not in recorded trajectories. Use rob.record_motion(*args) to record the motion in manual working mode first.") return False ## check if current point is on spherical trajectory (rx, ry, rz) calculated_cartesian_starting_pos = self.sph2cart(**self.cart2sph(**current_cart)) is_spherical_starting_point = [abs(current_cart[k] - calculated_cartesian_starting_pos[k]) < 1. for k in ["x", "y", "z"]] is_spherical_starting_point = is_spherical_starting_point + [abs(abs(abs(current_cart[k] - calculated_cartesian_starting_pos[k])-180)-180) < 1. for k in ["rx", "ry", "rz"]] is_spherical_starting_point=all(is_spherical_starting_point) if not is_spherical_starting_point: print("Moving linear as starting point is not on spherical position") target_cart = current_cart.copy() target_cart.update(self.sph2cart(return_dict=True, **target_sph)) return self.move_cartesian(tool=tool, frame=frame, desc=desc, sync=sync, simulate=simulate, coordinates=coordinates, **target_cart) # First only move in the radial direction target_sph_linear = current_sph.copy() target_sph_linear["r"] = target_sph["r"] target_cart_linear = current_cart.copy() tmp = self.sph2cart(return_dict=True, **target_sph_linear) target_cart_linear.update({k:tmp[k] for k in ["x", "y", "z"]}) self.set_pnt([current_cart[k] for k in ["x", "y", "z", "rx", "ry", "rz"]], name="tcp_p_spherical[0]") self.set_pnt([target_cart_linear[k] for k in ["x", "y", "z", "rx", "ry", "rz"]], name="tcp_p_spherical[1]") if (self.point_reachable("tcp_p_spherical[1]"))&(self.distance_p("tcp_p_spherical[0]", "tcp_p_spherical[1]")>.1): if simulate: for i in range(11): sim.append(self.get_movel_interpolation(i/10., coordinates=coordinates)) else: self.set_motion_queue_empty(False) ret = self.movel("tcp_p_spherical[1]", tool=tool, desc=None, sync=sync) print("moving radius") # Next move angles target_cart = current_cart.copy() target_cart.update(self.sph2cart(return_dict=True, **target_sph)) self.cartesian_destination = [target_cart[k] for k in ["x", "y", "z", "rx", "ry", "rz"]] self.set_pnt([target_cart[k] for k in ["x", "y", "z", "rx", "ry", "rz"]], name="tcp_p_spherical[3]") intermediate_cart = current_cart.copy() intermediate_sph = target_sph.copy() intermediate_sph.update({ "gamma": (target_sph["gamma"]+current_sph["gamma"])/2, "delta": (target_sph["delta"]+current_sph["delta"])/2 }) intermediate_cart.update(self.sph2cart(return_dict=True, **intermediate_sph)) self.set_pnt([intermediate_cart[k] for k in ["x", "y", "z", "rx", "ry", "rz"]], name="tcp_p_spherical[2]") distance = self.distance_p("tcp_p_spherical[1]", "tcp_p_spherical[3]") if (self.point_reachable("tcp_p_spherical[3]"))&(distance>.1): if (self.point_reachable("tcp_p_spherical[2]"))& (distance>1): if simulate: if len(sim)==0: for i in range(11): sim.append(self.get_movec_interpolation(i/10., coordinates=coordinates)) else: for i in range(10): sim.append(self.get_movec_interpolation(i/10.+0.1, coordinates=coordinates)) else: ret = self.movec("tcp_p_spherical[2]", "tcp_p_spherical[3]", tool=None, desc=None, sync=sync) print("moving angle circle") else: print("moving angle linear") if simulate: if len(sim)==0: for i in range(11): sim.append(self.get_movel_interpolation(i/10., coordinates=coordinates, idx_pnt1=1, idx_pnt2=3)) else: for i in range(10): sim.append(self.get_movel_interpolation(i/10.+0.1, coordinates=coordinates, idx_pnt1=1, idx_pnt2=3)) else: self.set_motion_queue_empty(False) ret = self.movel("tcp_p_spherical[3]", tool=None, desc=None, sync=sync) if simulate: return sim def move_joint(self, j1=None, j2=None, j3=None, j4=None, j5=None, j6=None, tool=None, desc=None, sync=False, simulate=False, coordinates="joint"): # Simulation not implemented yet if desc == None: desc = self.default_desc if tool == None: tool = self.tool() target_jnt = {"j1": j1, "j2": j2, "j3": j3, "j4": j4, "j5": j5, "j6": j6} current_jnt = self.get_joint_pos(return_dict=True) for k, v in target_jnt.items(): if v == None: target_jnt[k] = current_jnt[k] if (self.working_mode == "remote") & (~simulate): if not self.remote_allowed(current_jnt, target_jnt, frame_trsf=None, tool_trsf=None, motion="joint"): get_context().sendEvent("motion", "Requested remote motion is not in recorded trajectories. Use rob.record_motion(*args) to record the motion in manual working mode first.") return False if simulate: sim = [] cur_joint = [current_jnt[k] for k in ["j1", "j2", "j3", "j4", "j5", "j6"]] delta_joint = [target_jnt[k]-current_jnt[k] for k in ["j1", "j2", "j3", "j4", "j5", "j6"]] for i in range(11): sim.append([j0+dj*i/10 for j0, dj in zip(cur_joint, delta_joint)]) else: self.set_jnt([target_jnt[k] for k in ["j1", "j2", "j3", "j4", "j5", "j6"]], name="tcp_j") self.set_motion_queue_empty(False) self.movej("tcp_j", tool=tool , desc=desc, sync=sync) if simulate: return sim def move_home(self): if not self.is_in_point(P_HOME): self.start_task(MOVE_HOME) self.wait_task_finished(TASK_WAIT_ROBOT_POLLING) self.assert_home() def move_park(self): if not self.is_in_point(P_PARK): self.start_task(MOVE_PARK) self.wait_task_finished(TASK_WAIT_ROBOT_POLLING) self.assert_park() def tweak_x(self, offset): self.start_task(TWEAK_X, offset) self.wait_task_finished(TASK_WAIT_ROBOT_POLLING) def set_powered(self, value): if value: self.enable() if not value: self.disable() def tweak_y(self, offset): self.start_task(TWEAK_Y, offset) self.wait_task_finished(TASK_WAIT_ROBOT_POLLING) def on_event(self,ev): #print "EVT: " + ev pass def on_task_finished(self, task, ret): if self.isSimulated(): if ret is not None and ret <0: if task==MOVE_HOME: self.simulated_point=P_HOME elif task==MOVE_PARK: self.simulated_point=P_PARK else: self.simulated_point=None def doUpdate(self): #start = time.time() RobotTCP.doUpdate(self) def start_task(self, program, *args, **kwargs): #TODO: Check safe position self.task_name = program self.task_args = args self.task_kwargs = kwargs ret = RobotTCP.start_task(self, program, *args, **kwargs) def stop_task(self): RobotTCP.stop_task(self) #TODO: Restore safe position def set_remote_mode(self): self.set_profile("remote") def set_local(self): self.set_profile("default") def is_park(self): return self.is_in_point(P_PARK) def is_home(self): return self.is_in_point(P_HOME) def is_cleared(self): return self.get_current_point() is not None def assert_park(self): self.assert_in_point(P_PARK) def assert_home(self): self.assert_in_point(P_HOME) def assert_cleared(self): if not self.is_cleared(): raise Exception("Robot not in cleared position") def wait_ready(self): self.waitState(State.Ready, 1000) #robot.state.assertReady() def motor_to_epics_pvs(self, motors=[]): self.epics_pvs = {str("SARES20-ROB:" + motor.name + ".VAL").upper(): CAS(str("SARES20-ROB:" + motor.name + ".VAL").upper(), motor, "double") for motor in motors} self.epics_pvs.update({str("SARES20-ROB:" + motor.name + ".RBV").upper(): CAS(str("SARES20-ROB:" + motor.name + ".RBV").upper(), motor.readback, "double") for motor in motors}) def on_reconnected(self): #self.setup() sleep(2) get_context().restart() #robot.latency = 0.005 def setup(self): self.set_default_desc(DESC_DEFAULT) self.default_speed = DEFAULT_SPEED self.set_frame(self.frame_persistent()) self.set_frame(self.frame(), name="tcp_f_spherical", change_default=False) self.set_tool(self.tool_persistent()) self.setPolling(DEFAULT_ROBOT_POLLING) self.get_current_point() #TODO: REMOVE WHEN CURRENT POINT REPORTED BY POLLING MESSAGE self.set_motors_enabled(True) self.set_joint_motors_enabled(True) if simulation: add_device(RobotBernina("robot","localhost:1234"),force = True) else: add_device(RobotBernina("robot", "129.129.243.106:1234"), force = True) time.sleep(1) class DummySTP(RegisterBase): def __init__(self, val=None): self.val = val self.initialize() def doRead(self): return self.val def doWrite(self, val): self.val=val class MotorConfigPV(ReadonlyRegisterBase): def __init__(self, Motor, fieldname): self.motor = Motor self.fieldname = fieldname self.initialize() def doRead(self): return self.motor.config.getFieldValue(self.fieldname) def getUnit(self): return self.motor.getUnit() def make_archivable(Motor): base = PVBASE + Motor.name pv_def = { "": [Motor.readback, "double"], ".VAL": [Motor, "double"], ".RBV": [Motor.readback, "double"], ".EGU": [MotorConfigPV(Motor, "unit"), "string"], ".ADEL": [DummySTP(-1), "double"], #".ADEL": [DummySTP(0.002), "double"], ".MDEL": [DummySTP(0), "double"], ".RTYP": [DummySTP("motor"), "string"], ".SCAN": [DummySTP("passive"), "string"], ".PINI": [DummySTP("No"), "string"], ".PHAS": [DummySTP(0), "short"], ".PREC": [DummySTP(3), "short"], ".EVNT": [DummySTP(""), "string"], ".PRIO": [DummySTP(0), "short"],#should be ENUM with 0,1,3 being LOW, MEDIUM, HIGH ".DISV": [DummySTP(1), "short"], ".DISA": [DummySTP(0), "short"], ".SDIS": [DummySTP(""), "string"], ".PROC": [DummySTP(0), "short"],#should be uchar ".DISS": [DummySTP(0), "short"],#should be ENUM with 0,1,2,3 being NO?ALARM, MINOR, MAJOR, INVALID ".LCNT": [DummySTP(0), "short"],#should be uchar ".PACT": [DummySTP(0), "short"],#should be uchar ".NAME": [DummySTP(base), "string"], ".DESC": [DummySTP(base), "string"], ".ASG": [DummySTP(""), "string"], ".TSE": [DummySTP(0), "short"], ".TSEL": [DummySTP(""), "string"], ".DTYP": [DummySTP("asynMotor"), "string"], ".DISP": [DummySTP(0), "double"], ".PUTF": [DummySTP(0), "double"], ".PPRO": [DummySTP(0), "double"], ".TPRO": [DummySTP(0), "double"], ".STAT": [DummySTP(0), "short"], ".SEVR": [DummySTP(0), "short"], ".NSTA": [DummySTP(0), "short"], ".NSEV": [DummySTP(0), "short"], ".ACKS": [DummySTP(2), "short"], ".ACKT": [DummySTP(0), "short"], } epics_pvs = {base + field: CAS((base+field).upper(), device, dtype) for field, [device, dtype] in pv_def.items()} epics_pvs.update({base + ".FLNK": CAS((base+".FLNK").upper(), DummySTP((base+".RBV").upper()), "string")}) Motor.epics_pvs = epics_pvs robot.setup() motors = list(robot.cartesian_motors.values())+list(robot.spherical_motors.values())+list(robot.joint_motors.values())+list(robot.linear_axis_motors.values()) for m in motors: make_archivable(m) #robot.motor_to_epics_pvs() #show_panel(robot)