From fc9f6b23fc60f648b0dc49d518156d29ff21cd2a Mon Sep 17 00:00:00 2001 From: gac-bernina Date: Fri, 15 Mar 2024 13:27:51 +0100 Subject: [PATCH] Changed robot.frame, tool, remote_allowed_recorded to be persistent FS adjustables --- config/variables.properties | 4 + devices/robot_settings.properties | 3 + devices/test.properties | 6 + script/devices/RobotBernina.py | 241 ++++++------------------------ script/devices/RobotTCP.py | 102 ++++++------- script/elements/adjustables.py | 34 +++++ script/elements/array.py | 147 ++++++++++++++++++ script/local.py | 2 + script/test/test.py | 31 ++++ 9 files changed, 324 insertions(+), 246 deletions(-) create mode 100644 config/variables.properties create mode 100644 devices/robot_settings.properties create mode 100644 devices/test.properties create mode 100644 script/elements/adjustables.py create mode 100644 script/elements/array.py create mode 100644 script/test/test.py diff --git a/config/variables.properties b/config/variables.properties new file mode 100644 index 0000000..f6f5d2e --- /dev/null +++ b/config/variables.properties @@ -0,0 +1,4 @@ +#Thu Mar 14 07:47:28 CET 2024 +LastRunDate=240314 +DaySequentialNumber=1 +FileSequentialNumber=4 diff --git a/devices/robot_settings.properties b/devices/robot_settings.properties new file mode 100644 index 0000000..106b0dd --- /dev/null +++ b/devices/robot_settings.properties @@ -0,0 +1,3 @@ +#Wed Mar 13 18:55:09 CET 2024 +tool=t_JF01T0 +frame=f_4mRad diff --git a/devices/test.properties b/devices/test.properties new file mode 100644 index 0000000..004922e --- /dev/null +++ b/devices/test.properties @@ -0,0 +1,6 @@ +#Wed Mar 13 21:14:11 CET 2024 +num=null +huhu=null +hu=null +speed=null +frame=f_4mRad diff --git a/script/devices/RobotBernina.py b/script/devices/RobotBernina.py index 4329324..55087c8 100644 --- a/script/devices/RobotBernina.py +++ b/script/devices/RobotBernina.py @@ -1,14 +1,5 @@ -""" ---> eco has to take care of using the correctkeywords (i.e. r instead of t_det) ---> recording can be done with robot.record() similar to robot.simulate() taking the usual kwargs, issuing either a linear, joint or cartesian motion with record = True flag. -the flag then saves coordinates, tool, frame temporarily, starts motion command in thread, adds callback function to send an event, do an interpolation and save the interpolated values to save coordinates, ---> any motion command will automatically rset the motion ---> resent_motion should cancel the recording task / kill it -rename save_coordinates to checked / tested or so. -""" - - 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" @@ -18,14 +9,6 @@ DESC_DEFAULT = DESCS[0] FLANGE = "flange" -TOOLS = ["t_JF01T03", "t_JF07T32", ] -#TOOLS = ["t_Detector",] -TOOL_DEFAULT = TOOL_DET = TOOLS[0] - -FRAMES = ["f_actualFrame", "f_direct", "f_2mRad", "f_4mRad", "f_6mRad", "f_8mRad"] -#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 @@ -39,158 +22,10 @@ run("devices/RobotTCP") override_remote_safety = False simulation = False -def dimensionality_decorator(func): - def decorated(self, *args): - if hasattr(self[0], "__len__"): - return Array([func(s_, *args) for s_ in self]) - else: - return Array(func(self, *args)) - return decorated - -def indexing_decorator(func): - def decorated(self, v, *args): - nn = len(shape(v)) - if (nn ==1): - if (type(v[0]) is bool): - return Array([s_ for s_, v_ in zip(self,v) if v_]) - else: - return Array([self[v_] for v_ in v]) - elif nn ==2: - if type(v[0]) is bool: - return Array([[s_ for s_, v_ in zip(self[n],v[n]) if v_ ] for n in range(nn)]) - else: - ret = func(self, v, *args) - if hasattr(ret, "__len__"): - return Array(ret) - else: - return ret - return decorated - -class Array(list): - def __init__(self, data): - super(Array, self).__init__(data) - self.getint = super(Array, self).__getitem__ - @dimensionality_decorator - def __add__(self, v): - return add(self, v) - @dimensionality_decorator - def __sub__(self, v): - return sub(self, v) - @dimensionality_decorator - def __mul__(self, v): - return mul(self, v) - @dimensionality_decorator - def __div__(self, v): - return div(self, v) - @dimensionality_decorator - def interpolate(self, num): - return interpolate(self, num) - @dimensionality_decorator - def __lt__(self, v): - return lower_than(self, v) - @dimensionality_decorator - def __le__(self, v): - return lower_equal(self, v) - @dimensionality_decorator - def __gt__(self, v): - return greater_than(self, v) - @dimensionality_decorator - def __ge__(self, v): - return greater_equal(self, v) - @indexing_decorator - def __getitem__(self, v): - return super(Array, self).__getitem__(v) - @property - def shape(self): - return shape(self) - @property - def abs(self): - return arr_abs(self) - @property - def T(self): - return Array(transpose(self)) -### math helper functions -def shape(self): - o = self - s = [] - while hasattr(o, "__len__"): - s = s+[len(o)] - o = o[0] - return s - -def transpose(a): - mm = len(a[0]) - a_t = [[v[m] for v in a] for m in range(mm)] - return a_t - -def arr_abs(a): - n = a.shape - if len(n)==1: - a_abs = [abs(e) for e in a] - elif len(n)==2: - a_abs = [[abs(e) for e in v] for v in a] - return Array(a_abs) - -def interpolate(v,points_per_interval, round_to=2): - v_i=[[round(v[m]+1.*n/points_per_interval*(v[m+1]-v[m]), round_to) for n in range(points_per_interval)] for m in range(len(v)-1)] - ret = [] - for i in v_i: - ret=ret+i - ret=ret+[v[-1]] - return ret - -def add(list1, v): - if hasattr(v, "__len__"): - return [l+v_ for l, v_ in zip(list1, v)] - else: - return [l+v for l in list1] - -def sub(list1, v): - if hasattr(v, "__len__"): - return [l-v_ for l, v_ in zip(list1, v)] - else: - return [l-v for l in list1] - -def mul(list1, v): - if hasattr(v, "__len__"): - return [l*v_ for l, v_ in zip(list1, v)] - else: - return [l*v for l in list1] - -def div(list1, v): - if hasattr(v, "__len__"): - return [1.*l/v_ for l, v_ in zip(list1, v)] - else: - return [1.*l/v for l in list1] - -def lower_than(self, v): - if hasattr(v, "__len__"): - return [v_>s_ for v_, s_ in zip(v, self)] - else: - return [v>s_ for s_ in self] - -def lower_equal(self, v): - if hasattr(v, "__len__"): - return [v_>=s_ for v_, s_ in zip(v, self)] - else: - return [v>=s_ for s_ in self] - -def greater_than(self, v): - if hasattr(v, "__len__"): - return [s_>v_ for v_, s_ in zip(v, self)] - else: - return [s_>v for s_ in self] - -def greater_equal(self, v): - if hasattr(v, "__len__"): - return [s_>=v_ for v_, s_ in zip(v, self)] - else: - return [s_>=v for s_ in self] - class RobotBernina(RobotTCP): def __init__(self, name, server, timeout = 1000, retries = 1): - RobotTCP.__init__(self, name, server, timeout, retries) - self.invert_detector_rz = False + RobotTCP.__init__(self, name, server, timeout, retries) + self.invert_detector_rz = AdjustableFS(name = "invert_detector_rz", default_value=False) self.set_tasks(TASKS) self.set_known_points(KNOWN_POINTS) self.setPolling(DEFAULT_ROBOT_POLLING) @@ -198,18 +33,32 @@ class RobotBernina(RobotTCP): self.last_command_position = None self.override_remote_safety = override_remote_safety temp = {"frame": Array([]), "tool": Array([]), "pos": Array([])} - self.remote_allowed_recorded = {"spherical": temp, "cartesian": temp, "joint": temp} - self.remote_allowed_deltas = { + 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 set_override_remote_safety(self, value): - self.override_remote_safety = bool(value) + self.override_remote_safety(bool(value)) def set_invert_detector_rz(self, value): - self.invert_detector_rz = bool(value) + self.invert_detector_rz = (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: @@ -218,30 +67,31 @@ class RobotBernina(RobotTCP): # 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 = [self.remote_allowed_deltas[motion][k] for k in ks] - cds = [self.remote_allowed_deltas[motion][k] for k in ks] - rc = self.remote_allowed_recorded[motion] + 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 = rc["frame"] + fr = Array(rc["frame"]) idxs = (fr-frame_trsf).abs < cds idxs_b =[all(idx) for idx in idxs] - rc = {"pos": rc["pos"][idxs_b], "tool": rc["tool"][idxs_b]} + 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": rc["pos"][idxs_b]} + rc = {"pos": Array(rc["pos"])[idxs_b]} if len(rc["pos"]) > 0: for tr in rc["pos"]: - idxs1 = (tr-a1).abs < ds - idxs2 = (tr-a2).abs < ds + 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): @@ -265,7 +115,6 @@ class RobotBernina(RobotTCP): 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)) - return True 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 @@ -286,9 +135,11 @@ class RobotBernina(RobotTCP): move(sync = True, **kwargs) frame = Array(self.frame_trsf) tool = Array(self.tool_trsf) - self.remote_allowed_recorded[k]["frame"].append(frame) - self.remote_allowed_recorded[k]["tool"].append(tool) - self.remote_allowed_recorded[k]["pos"].append(interp) + 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 @@ -355,7 +206,7 @@ class RobotBernina(RobotTCP): z = r*math.cos(delta)*math.cos(gamma) rz = -math.asin(-math.sin(-math.sin(gamma)*math.sin(-delta)*math.sqrt(1/(math.cos(-delta)**2+(math.sin(gamma)*math.sin(-delta))**2)))) rx, ry, rz = self.rad2deg([-delta,+gamma, rz]) - if self.invert_detector_rz: + if self.invert_detector_rz(): rz = rz+180 if abs(rz/180) > 0: rz = rz - (rz+180)/360*360 @@ -393,7 +244,7 @@ class RobotBernina(RobotTCP): def point_reachable(self, point,tool=None, verbose=True): if tool == None: - tool = self.tool + tool = self.tool() self.herej() reachable = self.eval_bool("pointToJoint(" + tool + ",tcp_j," + point + ",j)") if verbose: @@ -435,9 +286,9 @@ class RobotBernina(RobotTCP): sim = [] if frame == None: - frame = self.frame + frame = self.frame() if tool == None: - tool = self.tool + tool = self.tool() if desc == None: desc = self.default_desc self.set_frame(frame, name="tcp_f_spherical", change_default=False) @@ -487,9 +338,9 @@ class RobotBernina(RobotTCP): """ sim = [] if frame == None: - frame = self.frame + frame = self.frame() if tool == None: - tool = self.tool + tool = self.tool() if desc == None: desc = self.default_desc self.set_frame(frame, name="tcp_f_spherical", change_default=False) @@ -578,7 +429,7 @@ class RobotBernina(RobotTCP): if desc == None: desc = self.default_desc if tool == None: - tool = self.tool + 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(): @@ -739,9 +590,9 @@ def make_archivable(Motor): #robot.latency = 0.005 robot.set_default_desc(DESC_DEFAULT) robot.default_speed = DEFAULT_SPEED -robot.set_frame(FRAME_DEFAULT) -robot.set_frame(FRAME_DEFAULT, name="tcp_f_spherical", change_default=False) -robot.set_tool(TOOL_DEFAULT) +robot.set_frame(robot.frame()) +robot.set_frame(robot.frame(), name="tcp_f_spherical", change_default=False) +robot.set_tool(robot.tool()) robot.setPolling(DEFAULT_ROBOT_POLLING) robot.get_current_point() #TODO: REMOVE WHEN CURRENT POINT REPORTED BY POLLING MESSAGE robot.set_motors_enabled(True) diff --git a/script/devices/RobotTCP.py b/script/devices/RobotTCP.py index 5d5d2c0..eeb5080 100644 --- a/script/devices/RobotTCP.py +++ b/script/devices/RobotTCP.py @@ -3,10 +3,10 @@ import threading MAX_NUMBER_PARAMETERS = 20 run("devices/RobotMotors") - class RobotTCP(TcpDevice, Stoppable): def __init__(self, name, server, timeout = 1000, retries = 1): - TcpDevice.__init__(self, name, server) + TcpDevice.__init__(self, name, server) + #DeviceBase.__init__(self, name, DeviceConfig({"frame": "f_4mRad"})) self.on_poll_status = {} self.on_poll_status_set = {} self.timeout = timeout @@ -47,11 +47,11 @@ class RobotTCP(TcpDevice, Stoppable): self.linear_axis_motors = {} self.joint_motors_enabled = False self.joint_motors = {} - self.tool = None + self.tool = AdjustableFS(name="tool", default_value="t_JF01T03") self.default_desc = None self.tool_open = None self.tool_trsf = [0.0] * 6 - self.frame = FRAME_DEFAULT + self.frame = AdjustableFS(name="frame", default_value="f_4mRad") self.frame_trsf = [0.0] * 6 self.polling_interval = 0.01 self.reset = True @@ -70,7 +70,7 @@ class RobotTCP(TcpDevice, Stoppable): self.reset = True def set_tool(self, tool): - self.tool=tool + self.tool(tool) self.set_str(tool, name="s_actualTool") self.evaluate("t_actualTool=" + tool) #self.tool_trsf = self.get_tool_trsf() @@ -80,7 +80,7 @@ class RobotTCP(TcpDevice, Stoppable): self.is_tool_open() def get_tool(self): - return self.tool + return self.tool() def set_frame(self, frame, name=None, change_default=True): if name is not None: @@ -88,24 +88,24 @@ class RobotTCP(TcpDevice, Stoppable): if change_default: self.set_str(frame, name="s_actualFrame") self.evaluate("f_actualFrame" + "=" + frame) - self.frame=frame + 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 + return self.frame() def set_default_frame(self): - self.set_frame(FRAME_DEFAULT) + self.set_frame(self.frame()) def assert_tool(self, tool=None): if tool is None: - if self.tool is None: + if self.tool() is None: raise Exception("Tool is undefined") - elif self.tool != tool: - raise Exception("Invalid tool: " + self.tool) + elif self.tool() != tool(): + raise Exception("Invalid tool: " + self.tool()) def set_default_desc(self,default_desc): self.default_desc=default_desc @@ -274,13 +274,13 @@ class RobotTCP(TcpDevice, Stoppable): def get_tool_trsf(self, name=None): if name is None: self.assert_tool() - name = self.tool + name = self.tool() return self.get_trsf(name+".trsf") def set_tool_trsf(self, trsf, name=None): if name is None: self.assert_tool() - name = self.tool + name = self.tool() self.set_trsf(trsf, name+".trsf") def eval_int(self, cmd): @@ -488,7 +488,7 @@ class RobotTCP(TcpDevice, Stoppable): if self.isSimulated(): self.simulated_point = "" if desc is None: desc = self.default_desc - if tool is None: tool = self.tool + if tool is None: tool = self.tool() #If joint_or_point is a list assumes ir is a point if not is_string(joint_or_point): robot.set_pnt(joint_or_point , "tcp_p") @@ -508,7 +508,7 @@ class RobotTCP(TcpDevice, Stoppable): if self.isSimulated(): self.simulated_point = "" if desc is None: desc = self.default_desc - if tool is None: tool = self.tool + if tool is None: tool = self.tool() if not is_string(point): robot.set_pnt(point , "tcp_p") point = "tcp_p" @@ -526,7 +526,7 @@ class RobotTCP(TcpDevice, Stoppable): if self.isSimulated(): self.simulated_point = "" if desc is None: desc = self.default_desc - if tool is None: tool = self.tool + if tool is None: tool = self.tool() #TODO: in new robot movel and movej is freezing controller ret = self.eval_int("movec(" + point_interm + ", " + point_target + ", " + tool + ", " + desc +")") @@ -563,17 +563,17 @@ class RobotTCP(TcpDevice, Stoppable): """ #Tool - Not synchronized calls: atention to open/close only when state is Ready def open_tool(self, tool=None): - if tool is None: tool = self.tool + if tool is None: tool = self.tool() self.evaluate(tool + ".gripper=true") self.tool_open = True def close_tool(self, tool=None): - if tool is None: tool = self.tool + if tool is None: tool = self.tool() self.evaluate(tool + ".gripper=false") self.tool_open = False def is_tool_open(self, tool=None): - if tool is None: tool = self.tool + if tool is None: tool = self.tool() self.tool_open = robot.eval_bool(tool + ".gripper") return self.tool_open @@ -589,26 +589,26 @@ class RobotTCP(TcpDevice, Stoppable): return self.eval_float("distance(" + pnt1 + ", " + pnt2 + ")") def compose(self, pnt, frame = None, trsf = "tcp_t"): - if frame is None: frame = self.frame + if frame is None: frame = self.frame() return self.eval_pnt("compose(" + pnt + ", " + frame + ", " + trsf + ")") def here(self, tool=None, frame=None): - if tool is None: tool = self.tool - if frame is None: frame = self.frame + if tool is None: tool = self.tool() + if frame is None: frame = self.frame() return self.eval_pnt("here(" + tool + ", " + frame + ")") def joint_to_point(self, tool=None, frame=None, joint="tcp_j"): - if tool is None: tool = self.tool - if frame is None: frame = self.frame + if tool is None: tool = self.tool() + if frame is None: frame = self.frame() return self.eval_pnt("jointToPoint(" + tool + ", " + frame + ", " + joint +")") def point_to_joint(self, tool=None, initial_joint="tcp_j", point="tcp_p"): - if tool is None: tool = self.tool + if tool is None: tool = self.tool() if self.eval_bool("pointToJoint(" + tool + ", " + initial_joint + ", " + point +", j)"): return self.get_jnt() def position(self, point, frame=None): - if frame is None: frame = self.frame + if frame is None: frame = self.frame() return self.eval_trf("position(" + point + ", " + frame + ")") #Profile @@ -690,9 +690,9 @@ class RobotTCP(TcpDevice, Stoppable): print "Communication resumed" robot.set_default_desc(DESC_DEFAULT) robot.default_speed = DEFAULT_SPEED - robot.set_frame(FRAME_DEFAULT) - robot.set_frame(FRAME_DEFAULT, name="tcp_f_spherical", change_default=False) - robot.set_tool(TOOL_DEFAULT) + robot.set_frame(self.frame()) + robot.set_frame(self.frame(), name="tcp_f_spherical", change_default=False) + robot.set_tool(self.tool()) robot.setPolling(DEFAULT_ROBOT_POLLING) if self.reset or (self.state==State.Offline): self.check_task() @@ -736,8 +736,8 @@ class RobotTCP(TcpDevice, Stoppable): self.on_task_finished(cur_task, ret) self.current_task, self.current_task_ret = None, ret - self.frame = sts[8] - self.tool = sts[9] + self.frame(sts[8]) + self.tool(sts[9]) for i in range(6): self.frame_trsf[i] = float(sts[10 + i]) for i in range(6): @@ -765,16 +765,16 @@ class RobotTCP(TcpDevice, Stoppable): self.on_poll_status.update({ "connected": self.connected, "powered": self.powered, - "invert_detector_rz": self.invert_detector_rz, + "invert_detector_rz": self.invert_detector_rz(), "override_remote_safety": self.override_remote_safety, "speed": self.speed, "settled": self.settled, "task": self.current_task, "mode": self.working_mode, "status": self.status, - "frame": self.frame, + "frame": self.frame(), "frame_coordinates": self.frame_trsf, - "tool": self.tool, + "tool": self.tool(), "tool_coordinates": self.tool_trsf, "cartesian_motors_enabled": self.cartesian_motors_enabled, "spherical_motors_enabled": self.spherical_motors_enabled, @@ -922,7 +922,7 @@ class RobotTCP(TcpDevice, Stoppable): pos.update(self.joint_pos) pos.update(self.linear_axis_pos) self.on_poll_status = { - "invert_detector_rz": self.invert_detector_rz, + "invert_detector_rz": self.invert_detector_rz(), "connected": self.connected, "powered": self.powered, "speed": self.speed, @@ -931,8 +931,8 @@ class RobotTCP(TcpDevice, Stoppable): "mode": self.working_mode, "override_remote_safety": self.override_remote_safety, "status": self.status, - "frame": self.frame, - "tool": self.tool, + "frame": self.frame(), + "tool": self.tool(), "pos": pos, "cartesian_motors_enabled": self.cartesian_motors_enabled, "spherical_motors_enabled": self.spherical_motors_enabled, @@ -977,25 +977,25 @@ class RobotTCP(TcpDevice, Stoppable): def set_tool_coordinates(self, v, name=None): if name is None: - name = self.tool + name = self.tool() self.set_trsf(v, name=name + ".trsf") - if name == self.tool: + if name == self.tool(): self.set_trsf(v, name="t_actualTool.trsf") def set_frame_coordinates(self, v, name=None): if name is None: - name = self.frame + name = self.frame() self.set_trsf(v, name=name + ".trsf") - if name == self.frame: + if name == self.frame(): self.set_trsf(v, name="f_actualFrame.trsf") #Cartesian space def get_cartesian_pos(self, tool=None, frame=None, return_dict=True): if tool is None: self.assert_tool() - tool = self.tool + tool = self.tool() if frame is None: - frame = self.frame + frame = self.frame() #Do not work #self.evaluate("tcp_j=herej(); tcp_p=jointToPoint(" + tool + ", " + frame + ", tcp_j)") self.evaluate("tcp_j=herej()") @@ -1028,18 +1028,18 @@ class RobotTCP(TcpDevice, Stoppable): def get_cartesian_destination(self, tool=None, frame=None,return_dict=True): if tool is None: self.assert_tool() - tool = self.tool + tool = self.tool() if frame is None: - frame = self.frame + frame = self.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 + tool = self.tool() if frame is None: - frame = self.frame + frame = self.frame() #return self.here(tool, frame) return self.get_spherical_pos() @@ -1181,7 +1181,7 @@ class RobotTCP(TcpDevice, Stoppable): m.initialize() #Alignment def align(self, point = None, frame = None, desc = None): - if frame is None: frame = self.frame + if frame is None: frame = self.frame() self.assert_tool() if desc is None: desc = self.default_desc if point is None: @@ -1190,9 +1190,9 @@ class RobotTCP(TcpDevice, Stoppable): self.set_pnt(point) np=self.eval_trf('align(tcp_p.trsf, ' + frame + '.trsf)') self.set_pnt(np) - self.movej("tcp_p", self.tool, desc) + self.movej("tcp_p", self.tool(), desc) #TODO: The first command is not executed (but receive a move id) - self.movej("tcp_p", self.tool, desc, sync = True) + self.movej("tcp_p", self.tool(), desc, sync = True) return np #High-level, exclusive motion task. diff --git a/script/elements/adjustables.py b/script/elements/adjustables.py new file mode 100644 index 0000000..157ed12 --- /dev/null +++ b/script/elements/adjustables.py @@ -0,0 +1,34 @@ +from json import load, dump +from os import path +import os + +BASEPATH = "C:/dev/pshell/config/bernina_robot/adjustables_fs/" + +class AdjustableFS: + def __init__(self, name=None, default_value=None, file_path=None): + if file_path is None: + file_path = BASEPATH + name + self.file_path = file_path + if not path.exists(self.file_path): + if not path.exists(path.dirname(self.file_path)): + os.mkdir(path.dirname(self.file_path)) + self.write_value(default_value) + self.name = name + + def get_current_value(self): + with open(self.file_path, "r") as f: + res = load(f) + return res["value"] + + def write_value(self, value): + with open(self.file_path, "w") as f: + dump({"value": value}, f, indent=4) + + def __call__(self, value=None): + if not value is None: + self.write_value(value) + else: + return self.get_current_value() + + def __repr__(self): + return "Current value of {} at: ".format(self.name) + str(self.get_current_value()) \ No newline at end of file diff --git a/script/elements/array.py b/script/elements/array.py new file mode 100644 index 0000000..5d2e9a7 --- /dev/null +++ b/script/elements/array.py @@ -0,0 +1,147 @@ +def dimensionality_decorator(func): + def decorated(self, *args): + if hasattr(self[0], "__len__"): + return Array([func(s_, *args) for s_ in self]) + else: + return Array(func(self, *args)) + return decorated + +def indexing_decorator(func): + def decorated(self, v, *args): + nn = len(shape(v)) + if (nn ==1): + if (type(v[0]) is bool): + return Array([s_ for s_, v_ in zip(self,v) if v_]) + else: + return Array([self[v_] for v_ in v]) + elif nn ==2: + if type(v[0]) is bool: + return Array([[s_ for s_, v_ in zip(self[n],v[n]) if v_ ] for n in range(nn)]) + else: + ret = func(self, v, *args) + if hasattr(ret, "__len__"): + return Array(ret) + else: + return ret + return decorated + +class Array(list): + def __init__(self, data): + super(Array, self).__init__(data) + self.getint = super(Array, self).__getitem__ + @dimensionality_decorator + def __add__(self, v): + return add(self, v) + @dimensionality_decorator + def __sub__(self, v): + return sub(self, v) + @dimensionality_decorator + def __mul__(self, v): + return mul(self, v) + @dimensionality_decorator + def __div__(self, v): + return div(self, v) + @dimensionality_decorator + def interpolate(self, num): + return interpolate(self, num) + @dimensionality_decorator + def __lt__(self, v): + return lower_than(self, v) + @dimensionality_decorator + def __le__(self, v): + return lower_equal(self, v) + @dimensionality_decorator + def __gt__(self, v): + return greater_than(self, v) + @dimensionality_decorator + def __ge__(self, v): + return greater_equal(self, v) + @indexing_decorator + def __getitem__(self, v): + return super(Array, self).__getitem__(v) + @property + def shape(self): + return shape(self) + @property + def abs(self): + return arr_abs(self) + @property + def T(self): + return Array(transpose(self)) +### math helper functions +def shape(self): + o = self + s = [] + while hasattr(o, "__len__"): + s = s+[len(o)] + o = o[0] + return s + +def transpose(a): + mm = len(a[0]) + a_t = [[v[m] for v in a] for m in range(mm)] + return a_t + +def arr_abs(a): + n = a.shape + if len(n)==1: + a_abs = [abs(e) for e in a] + elif len(n)==2: + a_abs = [[abs(e) for e in v] for v in a] + return Array(a_abs) + +def interpolate(v,points_per_interval, round_to=2): + v_i=[[round(v[m]+1.*n/points_per_interval*(v[m+1]-v[m]), round_to) for n in range(points_per_interval)] for m in range(len(v)-1)] + ret = [] + for i in v_i: + ret=ret+i + ret=ret+[v[-1]] + return ret + +def add(list1, v): + if hasattr(v, "__len__"): + return [l+v_ for l, v_ in zip(list1, v)] + else: + return [l+v for l in list1] + +def sub(list1, v): + if hasattr(v, "__len__"): + return [l-v_ for l, v_ in zip(list1, v)] + else: + return [l-v for l in list1] + +def mul(list1, v): + if hasattr(v, "__len__"): + return [l*v_ for l, v_ in zip(list1, v)] + else: + return [l*v for l in list1] + +def div(list1, v): + if hasattr(v, "__len__"): + return [1.*l/v_ for l, v_ in zip(list1, v)] + else: + return [1.*l/v for l in list1] + +def lower_than(self, v): + if hasattr(v, "__len__"): + return [v_>s_ for v_, s_ in zip(v, self)] + else: + return [v>s_ for s_ in self] + +def lower_equal(self, v): + if hasattr(v, "__len__"): + return [v_>=s_ for v_, s_ in zip(v, self)] + else: + return [v>=s_ for s_ in self] + +def greater_than(self, v): + if hasattr(v, "__len__"): + return [s_>v_ for v_, s_ in zip(v, self)] + else: + return [s_>v for s_ in self] + +def greater_equal(self, v): + if hasattr(v, "__len__"): + return [s_>=v_ for v_, s_ in zip(v, self)] + else: + return [s_>=v for s_ in self] diff --git a/script/local.py b/script/local.py index 31ba22a..7ee2cfb 100644 --- a/script/local.py +++ b/script/local.py @@ -2,6 +2,8 @@ # Deployment specific global definitions - executed after startup.py ################################################################################################### +run("elements/array") +run("elements/adjustables") run("devices/RobotBernina") run("devices/Axis") run ("motion/tools") diff --git a/script/test/test.py b/script/test/test.py new file mode 100644 index 0000000..4a2eb32 --- /dev/null +++ b/script/test/test.py @@ -0,0 +1,31 @@ +from json import load, dump +from os import path +import os + + +class AdjustableFS: + def __init__(self, file_path, name=None, default_value=None): + self.file_path = file_path + if not path.exists(self.file_path): + if not path.exists(path.dirname(self.file_path)): + os.mkdir(path.dirname(self.file_path)) + self._write_value(default_value) + self.name = name + + def get_current_value(self): + with open(self.file_path, "r") as f: + res = load(f) + return res["value"] + + def write_value(self, value): + with open(self.file_path, "w") as f: + dump({"value": value}, f, indent=4) + + def __call__(self, value=None): + if not value is None: + self.write_value(value) + else: + return self.get_current_value() + + def __repr__(self): + return "Current value of {} at: ".format(self.name) + str(self.get_current_value()) \ No newline at end of file