import math 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" TOOLS = ["t_Detector",] TOOL_DEFAULT = TOOL_DET = TOOLS[0] FRAMES = ["f_actualFrame", "f_Experiment1", "f_Experiment2", "f_Experiment3"] FRAME_DEFAULT = FRAMES[1] DEFAULT_ROBOT_POLLING = 200 TASK_WAIT_ROBOT_POLLING = 50 DEFAULT_SPEED=100 run("devices/RobotTCP") simulation = True 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.setSimulated() # TODO: Remove me def deg2rad(self, angles): return [a/180.*math.pi for a in angles] def rad2deg(self, angles): return [a*180./math.pi for a in angles] 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) rx, ry = self.rad2deg([-delta, -math.pi/2+gamma]) if return_dict: ret = {"x": x, "y": y, "z": z, "rx": rx, "ry": ry} else: ret = [x, y, z, rx, ry] return ret def cart2sph(self, x=None, y=None, z=None, return_dict=True, **kwargs): r = math.sqrt(x**2 + y**2 + z**2) gamma = math.copysign(1, x)*math.acos(z/math.sqrt(z**2+x**2)) 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: print("Point is not in range") return reachable 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] 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: ret = self.movel("tcp_p_spherical[1]", tool=tool, desc=None, sync=sync) if simulate: return sim 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] # 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: 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]") if (self.point_reachable("tcp_p_spherical[2]"))&(self.point_reachable("tcp_p_spherical[3]"))&(self.distance_p("tcp_p_spherical[1]", "tcp_p_spherical[3]")>.1): if self.distance_p("tcp_p_spherical[2]", "tcp_p_spherical[3]")>1: if simulate: 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: if simulate: for i in range(10): sim.append(self.get_movel_interpolation(i/10.+0.1, coordinates=coordinates, idx_pnt1=1, idx_pnt2=3)) else: ret = self.movel("tcp_p_spherical[3]", tool=None, desc=None, sync=sync) print("moving angle linear") 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_change_working_mode(self, working_mode): 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}) if simulation: add_device(RobotBernina("robot","localhost:1234"),force = True) else: add_device(RobotBernina("robot", "129.129.243.106:1234"), force = True) time.sleep(0.1) #robot.latency = 0.005 robot.set_default_desc(DESC_DEFAULT) robot.default_speed = DEFAULT_SPEED robot.set_frame(FRAME_DEFAULT) robot.set_tool(TOOL_DEFAULT) robot.setPolling(DEFAULT_ROBOT_POLLING) robot.get_current_point() #TODO: REMOVE WHEN CURRENT POINT REPORTED BY POLLING MESSAGE robot.set_motors_enabled(True) robot.set_joint_motors_enabled(True) robot.motor_to_epics_pvs(robot.cartesian_motors+robot.spherical_motors+robot.joint_motors) #show_panel(robot)