diff --git a/config/config.properties b/config/config.properties index 2f89360..c833049 100644 --- a/config/config.properties +++ b/config/config.properties @@ -1,4 +1,4 @@ -#Mon May 15 16:26:58 CEST 2023 +#Tue Oct 31 16:26:56 CET 2023 autoSaveScanData=true dataLayout=default dataPath={data}/{year}_{month}/{date}/{date}_{time}_{name} @@ -40,7 +40,7 @@ serverEnabled=true serverPort=8080 sessionHandling=Off simulation=false -terminalEnabled=false +terminalEnabled=true terminalPort=3579 userAuthenticator= userManagement=false diff --git a/devices/delta.properties b/devices/delta.properties new file mode 100644 index 0000000..9aec384 --- /dev/null +++ b/devices/delta.properties @@ -0,0 +1,11 @@ +#Tue Oct 31 17:33:12 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/devices/gamma.properties b/devices/gamma.properties new file mode 100644 index 0000000..9aec384 --- /dev/null +++ b/devices/gamma.properties @@ -0,0 +1,11 @@ +#Tue Oct 31 17:33:12 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/devices/j1.properties b/devices/j1.properties new file mode 100644 index 0000000..0887491 --- /dev/null +++ b/devices/j1.properties @@ -0,0 +1,11 @@ +#Thu Nov 02 10:53:37 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/devices/j2.properties b/devices/j2.properties new file mode 100644 index 0000000..0887491 --- /dev/null +++ b/devices/j2.properties @@ -0,0 +1,11 @@ +#Thu Nov 02 10:53:37 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/devices/j3.properties b/devices/j3.properties new file mode 100644 index 0000000..0887491 --- /dev/null +++ b/devices/j3.properties @@ -0,0 +1,11 @@ +#Thu Nov 02 10:53:37 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/devices/j4.properties b/devices/j4.properties new file mode 100644 index 0000000..0887491 --- /dev/null +++ b/devices/j4.properties @@ -0,0 +1,11 @@ +#Thu Nov 02 10:53:37 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/devices/j5.properties b/devices/j5.properties new file mode 100644 index 0000000..0887491 --- /dev/null +++ b/devices/j5.properties @@ -0,0 +1,11 @@ +#Thu Nov 02 10:53:37 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/devices/j6.properties b/devices/j6.properties new file mode 100644 index 0000000..0887491 --- /dev/null +++ b/devices/j6.properties @@ -0,0 +1,11 @@ +#Thu Nov 02 10:53:37 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/devices/r.properties b/devices/r.properties new file mode 100644 index 0000000..9aec384 --- /dev/null +++ b/devices/r.properties @@ -0,0 +1,11 @@ +#Tue Oct 31 17:33:12 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/devices/robot_j1.properties b/devices/robot_j1.properties new file mode 100644 index 0000000..6ea65db --- /dev/null +++ b/devices/robot_j1.properties @@ -0,0 +1,11 @@ +#Thu Nov 02 10:49:38 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/devices/robot_j2.properties b/devices/robot_j2.properties new file mode 100644 index 0000000..6ea65db --- /dev/null +++ b/devices/robot_j2.properties @@ -0,0 +1,11 @@ +#Thu Nov 02 10:49:38 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/devices/robot_j3.properties b/devices/robot_j3.properties new file mode 100644 index 0000000..6ea65db --- /dev/null +++ b/devices/robot_j3.properties @@ -0,0 +1,11 @@ +#Thu Nov 02 10:49:38 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/devices/robot_j4.properties b/devices/robot_j4.properties new file mode 100644 index 0000000..6ea65db --- /dev/null +++ b/devices/robot_j4.properties @@ -0,0 +1,11 @@ +#Thu Nov 02 10:49:38 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/devices/robot_j5.properties b/devices/robot_j5.properties new file mode 100644 index 0000000..6ea65db --- /dev/null +++ b/devices/robot_j5.properties @@ -0,0 +1,11 @@ +#Thu Nov 02 10:49:38 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/devices/robot_j6.properties b/devices/robot_j6.properties new file mode 100644 index 0000000..6ea65db --- /dev/null +++ b/devices/robot_j6.properties @@ -0,0 +1,11 @@ +#Thu Nov 02 10:49:38 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/devices/rx.properties b/devices/rx.properties new file mode 100644 index 0000000..05bdca9 --- /dev/null +++ b/devices/rx.properties @@ -0,0 +1,11 @@ +#Mon Oct 30 14:28:29 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/devices/ry.properties b/devices/ry.properties new file mode 100644 index 0000000..05bdca9 --- /dev/null +++ b/devices/ry.properties @@ -0,0 +1,11 @@ +#Mon Oct 30 14:28:29 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/devices/rz.properties b/devices/rz.properties new file mode 100644 index 0000000..05bdca9 --- /dev/null +++ b/devices/rz.properties @@ -0,0 +1,11 @@ +#Mon Oct 30 14:28:29 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/devices/x.properties b/devices/x.properties new file mode 100644 index 0000000..425f564 --- /dev/null +++ b/devices/x.properties @@ -0,0 +1,11 @@ +#Thu Nov 02 13:47:11 CET 2023 +description=null +maxValue=1000.0 +minValue=-1000.0 +offset=0.0 +precision=-1 +resolution=NaN +rotation=false +scale=1.0 +sign_bit=0 +unit=null diff --git a/devices/y.properties b/devices/y.properties new file mode 100644 index 0000000..05bdca9 --- /dev/null +++ b/devices/y.properties @@ -0,0 +1,11 @@ +#Mon Oct 30 14:28:29 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/devices/z.properties b/devices/z.properties new file mode 100644 index 0000000..05bdca9 --- /dev/null +++ b/devices/z.properties @@ -0,0 +1,11 @@ +#Mon Oct 30 14:28:29 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 ab3603b..3cfa30b 100644 --- a/script/devices/RobotBernina.py +++ b/script/devices/RobotBernina.py @@ -1,3 +1,4 @@ +import math KNOWN_POINTS = P_PARK, P_HOME = "park", "home" TASKS = MOVE_PARK, MOVE_HOME, TWEAK_X , TWEAK_Y = "movePark", "moveHome", "tweakX", "tweakY" @@ -10,9 +11,9 @@ TOOLS = ["t_Detector",] TOOL_DEFAULT = TOOL_DET = TOOLS[0] FRAMES = ["f_actualFrame", "f_Experiment1", "f_Experiment2", "f_Experiment3"] -FRAME_DEFAULT = FRAMES[0] +FRAME_DEFAULT = FRAMES[1] -DEFAULT_ROBOT_POLLING = 500 +DEFAULT_ROBOT_POLLING = 200 TASK_WAIT_ROBOT_POLLING = 50 DEFAULT_SPEED=100 @@ -31,28 +32,167 @@ class RobotBernina(RobotTCP): 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 move_spherical(self, r=None, gamma=None, delta=None): + 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_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 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): + 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): + ret = self.movel("tcp_p_spherical[1]", tool=tool, desc=None, sync=sync) + print(ret) + + + def move_spherical(self, r=None, gamma=None, delta=None, tool=None, frame=None, desc=None, sync=False): + 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] #def movel(self, point, tool=None, desc=None, sync=False) #def movec(self, point_interm, point_target, tool=None, desc=None, sync=False) - - if r is not None: - #separate in changing angle (movec) and changing r (movel) - print("not implemented") - else: - x,y,z,rx,ry,rz = self.get_cartesian_pos() - r = np.sqrt(x**2+y**2+z**2) - z = r*np.sin(np.deg2rad(90-delta))*np.cos(np.deg2rad(gamma)) - x = r*np.sin(np.deg2rad(90-delta))*np.sin(np.deg2rad(gamma)) - y = r*np.cos(np.deg2rad(90-delta)) - ry = gamma - rx = -delta - # set_pnt A - # set_pnt B - #eval(movectl - movec - - + + # 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): + ret = self.movel("tcp_p_spherical[1]", tool=tool, desc=None, sync=sync) + print(ret) + 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: + 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) def move_home(self): if not self.is_in_point(P_HOME): @@ -70,6 +210,11 @@ class RobotBernina(RobotTCP): 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) @@ -108,7 +253,7 @@ class RobotBernina(RobotTCP): def set_remote_mode(self): self.set_profile("remote") - + def set_local(self): self.set_profile("default") @@ -133,7 +278,9 @@ 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] + if simulation: add_device(RobotBernina("robot","localhost:1234"),force = True) else: @@ -148,5 +295,7 @@ 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) diff --git a/script/devices/RobotMotors.py b/script/devices/RobotMotors.py index 07ba862..bbadc65 100644 --- a/script/devices/RobotMotors.py +++ b/script/devices/RobotMotors.py @@ -8,7 +8,7 @@ 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()) + PositionerBase.__init__(self, ROBOT_MOTORS[index], PositionerConfig()) self.index = index self.robot = robot @@ -17,27 +17,55 @@ class RobotCartesianMotor (PositionerBase): self.setCache(self.doRead(), None) def doRead(self): - return float("nan") if self.robot.cartesian_destination is None else float(self.robot.cartesian_destination[self.index]) + return float("nan") if self.robot.cartesian_destination is None else float(self.robot.cartesian_destination[self.name]) def doWrite(self, value): if self.robot.cartesian_destination is not None: - #print "Move " + ROBOT_MOTORS[self.index] + " to " + str(value) - self.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) + self.setCache(float(value), None) + robot.move_cartesian(**{self.name:value}) def doReadReadback(self): - return float("nan") if self.robot.cartesian_pos is None else float(self.robot.cartesian_pos[self.index]) + return float("nan") if self.robot.cartesian_pos is None else float(self.robot.cartesian_pos[self.name]) + def stop(self): + robot.stop() + robot.reset_motion() + robot.resume() + def move_done(self): + """once move done, do readback update and send event and readback""" +ROBOT_SPHERICAL_MOTORS = ["r" , "gamma", "delta"] + +class RobotSphericalMotor (PositionerBase): + def __init__(self, robot, index): + PositionerBase.__init__(self, ROBOT_SPHERICAL_MOTORS[index], PositionerConfig()) + self.index = index + self.robot = robot + + def doInitialize(self): + self.setCache(self.doRead(), None) + + def doRead(self): + return float("nan") if self.robot.spherical_pos is None else robot.spherical_pos[self.name] + + def doWrite(self, value): + if self.robot.spherical_destination is not None: + self.setCache(float(value), None) + robot.move_spherical(**{self.name:value}) + + def doReadReadback(self): + return float("nan") if self.robot.spherical_pos is None else robot.spherical_pos[self.name] + + def stop(self): + robot.stop() + robot.reset_motion() + robot.resume() 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()) + PositionerBase.__init__(self, ROBOT_JOINT_MOTORS[index], PositionerConfig()) self.index = index self.robot = robot @@ -54,9 +82,9 @@ class RobotJointMotor (PositionerBase): joint = self.robot.herej() joint[self.index] = value self.robot.set_jnt(joint, name="tcp_j") - self.robot.movej("tcp_j", self.robot.tool , DESC_SCAN) + self.robot.movej("tcp_j", self.robot.tool , DESC_DEFAULT) def doReadReadback(self): - return self.robot.herej()[self.index] if self.robot.joint_pos is None else float(self.robot.joint_pos[self.index]) + return self.robot.herej()[self.index] if self.robot.joint_pos is None else float(self.robot.joint_pos[self.name]) \ No newline at end of file diff --git a/script/devices/RobotTCP.py b/script/devices/RobotTCP.py index b413482..469025c 100644 --- a/script/devices/RobotTCP.py +++ b/script/devices/RobotTCP.py @@ -7,6 +7,8 @@ run("devices/RobotMotors") class RobotTCP(TcpDevice, Stoppable): def __init__(self, name, server, timeout = 1000, retries = 1): TcpDevice.__init__(self, name, server) + self.on_poll_status = {} + self.on_poll_status_set = {} self.timeout = timeout self.retries = retries self.header = None @@ -29,11 +31,15 @@ class RobotTCP(TcpDevice, Stoppable): self.known_points = [] self.current_points = [] self.cartesian_destination = None + self.spherical_destination = None #self.flange_pos = [None] * 6 - self.cartesian_pos = [None] * 6 - self.joint_pos = [None] * 6 + self.cartesian_pos = {} + self.spherical_pos = {} + self.joint_pos = {} self.cartesian_motors_enabled = False self.cartesian_motors = [] + self.spherical_motors_enabled = False + self.spherical_motors = [] self.joint_motors_enabled = False self.joint_motors = [] self.tool = None @@ -47,7 +53,6 @@ class RobotTCP(TcpDevice, Stoppable): self.default_speed = 100 self.latency = 0 self.last_msg_timestamp = 0 - self.task_start_retries = 3 self.exception_on_task_start_failure = True #Tasks may start and be finished when checked self.simulated_point = "" @@ -68,13 +73,14 @@ class RobotTCP(TcpDevice, Stoppable): def get_tool(self): return self.tool - def set_frame(self, frame): - self.frame = frame - self.evaluate("tcp_curframe=" + frame) - if self.cartesian_motors_enabled: - self.update() - self.set_motors_enabled(True) - self.waitCacheChange(5000) + def set_frame(self, frame, name="tcp_curframe", change_default=True): + self.evaluate(name + "=" + frame) + if change_default: + self.frame = frame + if self.cartesian_motors_enabled: + self.update() + self.set_motors_enabled(True) + self.waitCacheChange(5000) def get_frame(self): return self.frame @@ -234,7 +240,7 @@ class RobotTCP(TcpDevice, Stoppable): 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]) + "}") @@ -631,7 +637,7 @@ class RobotTCP(TcpDevice, Stoppable): elif not self.empty: self.setState(State.Paused) else: self.setState(State.Ready) - def doUpdate(self): + def doUpdate(self): try: start = time.time() cur_task = self.current_task #Can change in background so cache it @@ -670,10 +676,11 @@ class RobotTCP(TcpDevice, Stoppable): for i in range(6): - self.joint_pos[i] = float(sts[8 + i]) + self.joint_pos[ROBOT_JOINT_MOTORS[i]] = float(sts[8 + i]) for i in range(6): - self.cartesian_pos[i] = float(sts[14 + i]) - + self.cartesian_pos[ROBOT_MOTORS[i]] = float(sts[14 + i]) + self.update_spherical_pos() + ev_index = 20 #7 ev = sts[ev_index] if len(sts)>ev_index else "" if len(ev.strip()) >0: @@ -692,19 +699,52 @@ class RobotTCP(TcpDevice, Stoppable): "open": self.tool_open, "pos": self.get_current_point_cached() if self.state==State.Ready else None #TODO: make it calculated in robot by polling funtion }, None) - + pos = self.cartesian_pos.copy() + pos.update(self.spherical_pos) + pos.update(self.joint_pos) + self.on_poll_status = { + "connected": self.connected, + "powered": self.powered, + "speed": self.speed, + "settled": self.settled, + "task": self.current_task, + "mode": self.working_mode, + "status": self.status, + "frame": self.frame, + "tool": self.tool, + "pos": pos, + "cartesian_motors_enabled": self.cartesian_motors_enabled, + "spherical_motors_enabled": self.spherical_motors_enabled, + } + + get_context().sendEvent("polling", self.on_poll_status) if self.cartesian_motors_enabled: for m in self.cartesian_motors: m.readback.update() - + if self.spherical_motors_enabled: + for m in self.spherical_motors: + m.readback.update() if self.joint_motors_enabled: 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) + self.setState(State.Offline) + + def on_poll_info(self): + on_poll_config_methods ={ + "powered": {"cmd":"robot.set_powered", "def_kwargs": ""}, + "speed": {"cmd":"robot.set_monitor_speed", "def_kwargs": ""}, + "frame": {"cmd": "robot.set_frame", "def_kwargs": "change_default=True",}, + "tool": {"cmd":"robot.set_tool", "def_kwargs": ""}, + "cartesian_motors_enabled": {"cmd":"robot.set_motors_enabled", "def_kwargs": "pseudos=['cartesian']"}, + "spherical_motors_enabled": {"cmd":"robot.set_motors_enabled", "def_kwargs": "pseudos=['spherical']"}, + } + on_poll_info = [k for k in self.on_poll_status if not k in list(on_poll_config_methods.keys())+["pos"]] + return {"info": on_poll_info, "config": on_poll_config_methods} #Cartesian space """ @@ -718,7 +758,7 @@ class RobotTCP(TcpDevice, Stoppable): #TODO: in new robot robot.evaluate("tcp_p=jointToPoint(tSuna, world, herej())") is freezing controller (cannot call herej directly) #We can consider atomic because tcp_j is only accessed in comTcp - def get_cartesian_pos(self, tool=None, frame=None): + def get_cartesian_pos(self, tool=None, frame=None, return_dict=True): if tool is None: self.assert_tool() tool = self.tool @@ -728,24 +768,39 @@ class RobotTCP(TcpDevice, Stoppable): #self.evaluate("tcp_j=herej(); tcp_p=jointToPoint(" + tool + ", " + frame + ", tcp_j)") self.evaluate("tcp_j=herej()") self.evaluate("tcp_p=jointToPoint(" + tool + ", " + frame + ", tcp_j)") - return self.get_pnt() + x,y,z,rx,ry,rz = self.get_pnt() + 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 #a = self.execute('get_pos', tool, frame) #ret = [] #for i in range(6): ret.append(float(a[i])) #return ret - def get_flange_pos(self, frame=None): - return self.get_cartesian_pos(FLANGE, frame) + def get_flange_pos(self, frame=None, return_dict=True): + return self.get_cartesian_pos(FLANGE, frame, return_dict=return_dict) - def get_cartesian_destination(self, tool=None, frame=None): + def get_cartesian_destination(self, tool=None, frame=None,return_dict=True): if tool is None: self.assert_tool() tool = self.tool if frame is None: frame = self.frame - return self.here(tool, frame) + #return self.here(tool, frame) + self.get_cartesian_pos(return_dict=return_dict) + + def get_spherical_destination(self, tool=None, frame=None): + if tool is None: + self.assert_tool() + tool = self.tool + if frame is None: + frame = self.frame + #return self.here(tool, frame) + return self.get_spherical_pos() def get_distance_to_pnt(self, name): if self.isSimulated(): @@ -754,7 +809,7 @@ class RobotTCP(TcpDevice, Stoppable): else: return 10000 #self.here(self.tool, self.frame) #??? - self.set_pnt(self.get_cartesian_pos() ) + self.set_pnt(self.get_cartesian_pos(return_dict=False) ) return self.distance_p("tcp_p", name) def is_in_point(self, p, tolerance = None): #Tolerance in mm @@ -806,28 +861,50 @@ class RobotTCP(TcpDevice, Stoppable): def is_emulation(self): return "localhost" in robot.client.getServerAddress() - #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_motors_enabled(self, value, pseudos=["cartesian", "spherical"]): + if "cartesian" in pseudos: + 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_pos(return_dict=True) + self.cartesian_pos = self.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_pos(return_dict=True) + for m in self.cartesian_motors: + m.initialize() + + if "spherical" in pseudos: + if value !=self.spherical_motors_enabled: + self.spherical_motors_enabled = value + if value: + for i in range(3): + self.spherical_motors.append(RobotSphericalMotor(self, i)) + add_device(self.spherical_motors[i], True) + self.spherical_destination = self.get_spherical_pos() + self.spherical_pos = self.spherical_destination + else: + for m in self.spherical_motors: + remove_device(m) + self.spherical_motors = [] + self.spherical_destination = None + else: + if value: + self.spherical_destination = self.get_spherical_pos() + for m in self.spherical_motors: + m.initialize() + + #Joint motors def set_joint_motors_enabled(self, value): if value !=self.joint_motors_enabled: self.joint_motors_enabled = value @@ -849,7 +926,7 @@ class RobotTCP(TcpDevice, Stoppable): self.assert_tool() if desc is None: desc = self.default_desc if point is None: - self.set_pnt(self.get_cartesian_pos() ) + self.set_pnt(self.get_cartesian_pos(return_dict=False) ) else: self.set_pnt(point) np=self.eval_trf('align(tcp_p.trsf, ' + frame + '.trsf)')