diff --git a/devices/z_lin.properties b/devices/z_lin.properties new file mode 100644 index 0000000..bc3ca81 --- /dev/null +++ b/devices/z_lin.properties @@ -0,0 +1,11 @@ +#Tue Nov 14 14:31:51 CET 2023 +description=null +maxValue=NaN +minValue=NaN +offset=0.0 +precision=-1 +resolution=NaN +rotation=false +scale=1.0 +sign_bit=0 +unit=null diff --git a/script/devices/RobotBernina.py b/script/devices/RobotBernina.py index 3cfa30b..51eab39 100644 --- a/script/devices/RobotBernina.py +++ b/script/devices/RobotBernina.py @@ -46,34 +46,33 @@ class RobotBernina(RobotTCP): def update_spherical_pos(self): self.spherical_pos=self.cart2sph(return_dict=True, **self.cartesian_pos) - def get_movec_interpolated(self, r=None, gamma=None, delta=None, tool=None, frame=None): - if frame == None: - frame = self.frame - if tool == None: - tool = self.tool - 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} - for k, v in target_sph.items(): - if v == None: - target_sph[k] = current_sph[k] - self.set_pnt([current_cart[k] for k in ["x", "y", "z", "rx", "ry", "rz"]], name="tcp_p_spherical[0]") - target_cart = current_cart.copy() - target_cart.update(self.sph2cart(return_dict=True, **target_sph)) - 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]") - a = self.execute("get_movec_interpolation") - return a + 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]) @@ -119,7 +118,23 @@ class RobotBernina(RobotTCP): 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): + 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: @@ -136,11 +151,34 @@ class RobotBernina(RobotTCP): 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): - ret = self.movel("tcp_p_spherical[1]", tool=tool, desc=None, sync=sync) - print(ret) + 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): + 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: @@ -155,8 +193,6 @@ class RobotBernina(RobotTCP): for k, v in target_sph.items(): if v == None: target_sph[k] = current_sph[k] - #def movel(self, point, tool=None, desc=None, sync=False) - #def movec(self, point_interm, point_target, tool=None, desc=None, sync=False) # First only move in the radial direction target_sph_linear = current_sph.copy() @@ -167,9 +203,12 @@ class RobotBernina(RobotTCP): 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): - ret = self.movel("tcp_p_spherical[1]", tool=tool, desc=None, sync=sync) - print(ret) - print("moving radius") + 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() @@ -187,12 +226,21 @@ class RobotBernina(RobotTCP): 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: - ret = self.movec("tcp_p_spherical[2]", "tcp_p_spherical[3]", tool=None, desc=None, sync=sync) - print("moving angle circle") + 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: - ret = self.movel("tcp_p_spherical[3]", tool=None, desc=None, sync=sync) - print("moving angle linear") - print(ret) + 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): @@ -278,13 +326,15 @@ class RobotBernina(RobotTCP): def wait_ready(self): self.waitState(State.Ready, 1000) #robot.state.assertReady() + def motor_to_epics_pvs(self, motors=[]): - self.epics_pvs = [CAS(str("SARES20-ROB:" + motor.name + ".VAL").upper(), motor, "double") for motor in 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.105:1234"), force = True) + add_device(RobotBernina("robot", "129.129.243.106:1234"), force = True) time.sleep(0.1) diff --git a/script/devices/RobotMotors.py b/script/devices/RobotMotors.py index bbadc65..03c8b11 100644 --- a/script/devices/RobotMotors.py +++ b/script/devices/RobotMotors.py @@ -1,12 +1,36 @@ - +import time import ch.psi.pshell.device.PositionerConfig as PositionerConfig +class LinearAxisMotor(PositionerBase): + def __init__(self, robot, name = "z_lin"): + PositionerBase.__init__(self, name, PositionerConfig()) + self.robot = robot + + #ATTENTION: Always initialize linear axis motors before scanning (or call robot.set_lin_axis_motor_enabled(True)) + def doInitialize(self): + self.setCache(self.doRead(), None) + + def doRead(self): + return float("nan") if self.robot.linear_axis_destination is None else float(self.robot.linear_axis_destination[self.name]) + + def doWrite(self, value): + if self.robot.linear_axis_destination is not None: + self.robot.linear_axis_destination.update({self.name: value}) + self.setCache(float(value), None) + robot.evaluate("n_moveAbsPos = " + str(value)) + robot.evaluate("setEcAo(ecao_posSet,n_moveAbsPos)") + + def doReadReadback(self): + return float("nan") if self.robot.linear_axis_pos is None else float(self.robot.linear_axis_pos[self.name]) + + def stop(self): + robot.evaluate("setEcDo(ecdo_linMotStop,true)") + time.sleep(0.1) + robot.evaluate("setEcDo(ecdo_linMotStop,false)") ROBOT_MOTORS = ["x" , "y", "z", "rx", "ry", "rz"] - - -class RobotCartesianMotor (PositionerBase): +class RobotCartesianMotor(PositionerBase): def __init__(self, robot, index): PositionerBase.__init__(self, ROBOT_MOTORS[index], PositionerConfig()) self.index = index @@ -49,7 +73,7 @@ class RobotSphericalMotor (PositionerBase): def doRead(self): return float("nan") if self.robot.spherical_pos is None else robot.spherical_pos[self.name] - def doWrite(self, value): + def doWrite(self, value): if self.robot.spherical_destination is not None: self.setCache(float(value), None) robot.move_spherical(**{self.name:value}) diff --git a/script/devices/RobotTCP.py b/script/devices/RobotTCP.py index 469025c..8c768be 100644 --- a/script/devices/RobotTCP.py +++ b/script/devices/RobotTCP.py @@ -32,14 +32,18 @@ class RobotTCP(TcpDevice, Stoppable): self.current_points = [] 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 = None @@ -636,7 +640,7 @@ class RobotTCP(TcpDevice, Stoppable): 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() @@ -679,9 +683,10 @@ class RobotTCP(TcpDevice, Stoppable): 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 = 20 #7 + ev_index = 21 #7 ev = sts[ev_index] if len(sts)>ev_index else "" if len(ev.strip()) >0: self.getLogger().info(ev) @@ -702,6 +707,7 @@ class RobotTCP(TcpDevice, Stoppable): 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, @@ -724,6 +730,9 @@ class RobotTCP(TcpDevice, Stoppable): if self.spherical_motors_enabled: for m in self.spherical_motors: m.readback.update() + if self.linear_axis_motors_enabled: + for m in self.linear_axis_motors: + m.readback.update() if self.joint_motors_enabled: for m in self.joint_motors: m.readback.update() @@ -783,7 +792,13 @@ class RobotTCP(TcpDevice, Stoppable): 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() @@ -863,7 +878,7 @@ class RobotTCP(TcpDevice, Stoppable): return "localhost" in robot.client.getServerAddress() #Cartesian peudo-motors - def set_motors_enabled(self, value, pseudos=["cartesian", "spherical"]): + def set_motors_enabled(self, value, pseudos=["cartesian", "spherical", "linear"]): if "cartesian" in pseudos: if value !=self.cartesian_motors_enabled: self.cartesian_motors_enabled = value @@ -903,7 +918,25 @@ class RobotTCP(TcpDevice, Stoppable): self.spherical_destination = self.get_spherical_pos() for m in self.spherical_motors: m.initialize() - + + if "linear" in pseudos: + if value !=self.linear_axis_motors_enabled: + self.linear_axis_motors_enabled = value + if value: + self.linear_axis_motors.append(LinearAxisMotor(self, "z_lin")) + add_device(self.linear_axis_motors[0], True) + self.linear_axis_destination = self.get_linear_axis_pos() + self.linear_axis_pos = self.linear_axis_destination + else: + for m in self.linear_axis_motors: + remove_device(m) + self.linear_axis_motors = [] + self.linear_axis_destination = None + else: + if value: + self.linear_axis_destination = self.get_linear_axis_pos() + for m in self.linear_axis_motors: + m.initialize() #Joint motors def set_joint_motors_enabled(self, value): if value !=self.joint_motors_enabled: