From 1f879d7c3f20c50e3fc72941493e247c821aab5e Mon Sep 17 00:00:00 2001 From: gac-bernina Date: Tue, 27 Feb 2024 14:52:09 +0100 Subject: [PATCH] Fixed some issues with recording trajectories for remote motion --- script/devices/RobotBernina.py | 96 ++++++++++++++++++++-------------- script/devices/RobotTCP.py | 23 ++++++-- 2 files changed, 76 insertions(+), 43 deletions(-) diff --git a/script/devices/RobotBernina.py b/script/devices/RobotBernina.py index 11c63a4..8525ac4 100644 --- a/script/devices/RobotBernina.py +++ b/script/devices/RobotBernina.py @@ -36,8 +36,8 @@ ROBOT_SPHERICAL_MOTORS = ["r" , "gamma", "delta"] run("devices/RobotTCP") - -simulation = False +override_remote_safety = False +simulation = True def dimensionality_decorator(func): def decorated(self, *args): @@ -104,6 +104,9 @@ class Array(list): 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 @@ -120,6 +123,14 @@ def transpose(a): 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 = [] @@ -184,41 +195,49 @@ class RobotBernina(RobotTCP): 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([])} - self.remote_allowed = {"spherical": temp, "cartesian": temp, "joint": temp} + self.remote_allowed_recorded = {"spherical": temp, "cartesian": temp, "joint": temp} self.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.}, } + def set_override_remote_safety(self, value): + self.override_remote_safety = bool(value) + 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 = [self.remote_allowed_deltas[motion][k] for k in ks] - cds = [self.remote_allowed_deltas["cartesian"][k] for k in ROBOT_CARTESIAN_MOTORS] - rc = self.remote_allowed[motion] - #preselection by frame with concatenated arrays - if "frame" in rc.keys(): - fr = rc["frame"] - idxs = (fr-frame_trsf) < cds - idxs_b =[all(idx) for idx in idxs] - rc = {"pos": rc["pos"][idxs_b], "tool": rc["tool"][idxs_b]} - - if "tool" in rc.keys(): - tr = rc["tool"] - idxs = (tr-tool_trsf) < cds - idxs_b =[all(idx) for idx in idxs] - rc = {"pos": rc["pos"][idxs_b]} - for tr in rc["pos"]: - idxs1 = (tr-a1) < ds - idxs2 = (tr-a2) < ds - idx1 = [all(idx) for idx in idxs1] - idx2 = [all(idx) for idx in idxs2] - if (any(idx1)) & (any(idx2)): - return True + cds = [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"] + 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]} + 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]} + if len(rc["pos"]) > 0: + for tr in rc["pos"]: + idxs1 = (tr-a1).abs < ds + idxs2 = (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 return False def general_motion(self, **kwargs): @@ -242,7 +261,7 @@ 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 + 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 @@ -255,14 +274,17 @@ class RobotBernina(RobotTCP): 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 - interp = Array(move(simulate=True, coordinates = k, **kwargs)).T.interpolate(10).T + 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) - self.remote_allowed[k]["frame"].append(frame) - self.remote_allowed[k]["tool"].append(tool) - self.remote_allowed[k]["pos"].append(interp) + self.remote_allowed_recorded[k]["frame"].append(frame) + self.remote_allowed_recorded[k]["tool"].append(tool) + self.remote_allowed_recorded[k]["pos"].append(interp) self.on_recording_finished() return True @@ -327,11 +349,12 @@ class RobotBernina(RobotTCP): 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,+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 return_dict: - ret = {"x": x, "y": y, "z": z, "rx": rx, "ry": ry} + ret = {"x": x, "y": y, "z": z, "rx": rx, "ry": ry, "rz":rz} else: - ret = [x, y, z, rx, ry] + ret = [x, y, z, rx, ry, rz] return ret def cart2sph(self, x=None, y=None, z=None, return_dict=True, **kwargs): @@ -412,7 +435,7 @@ class RobotBernina(RobotTCP): if v == None: target_cart[k] = current_cart[k] if (self.working_mode == "remote") & (~simulate): - if not remote_allowed(current_cart, target_cart, frame_trsf=self.get_trsf(frame+".trsf"), tool_trsf=self.get_trsf(tool+".trsf"), motion="cartesian"): + 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 @@ -466,7 +489,7 @@ class RobotBernina(RobotTCP): if v == None: target_sph[k] = current_sph[k] if (self.working_mode == "remote") & (~simulate): - if not remote_allowed(current_sph, target_sph, frame_trsf=self.get_trsf(frame+".trsf"), tool_trsf=self.get_trsf(tool+".trsf"), motion="spherical"): + 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 # First only move in the radial direction @@ -539,7 +562,7 @@ class RobotBernina(RobotTCP): if v == None: target_jnt[k] = current_jnt[k] if (self.working_mode == "remote") & (~simulate): - if not remote_allowed(current_jnt, target_jnt, frame_trsf=None, tool_trsf=None, motion="joint"): + 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: @@ -585,9 +608,6 @@ class RobotBernina(RobotTCP): #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: diff --git a/script/devices/RobotTCP.py b/script/devices/RobotTCP.py index d10cb23..8981597 100644 --- a/script/devices/RobotTCP.py +++ b/script/devices/RobotTCP.py @@ -326,10 +326,9 @@ class RobotTCP(TcpDevice, Stoppable): def enable(self): if not self.is_powered(): self.evaluate("enablePower()") - time.sleep(1.0) + time.sleep(5.0) if not self.is_powered(): - raise Exception("Cannot enable power") - + raise Exception("Enabling power timed out after 5s.") #waits for power to be actually cut off def disable(self): self.evaluate("disablePower()", timeout=5000) @@ -688,7 +687,13 @@ class RobotTCP(TcpDevice, Stoppable): def _update_state(self): #self.setState(State.Busy if self.status=="move" else State.Ready) if self.state==State.Offline: - print "Communication resumed" + 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.setPolling(DEFAULT_ROBOT_POLLING) if self.reset or (self.state==State.Offline): self.check_task() if self.current_task is not None: @@ -760,6 +765,7 @@ class RobotTCP(TcpDevice, Stoppable): self.on_poll_status.update({ "connected": self.connected, "powered": self.powered, + "override_remote_safety": self.override_remote_safety, "speed": self.speed, "settled": self.settled, "task": self.current_task, @@ -921,6 +927,7 @@ class RobotTCP(TcpDevice, Stoppable): "settled": self.settled, "task": self.current_task, "mode": self.working_mode, + "override_remote_safety": self.override_remote_safety, "status": self.status, "frame": self.frame, "tool": self.tool, @@ -953,6 +960,7 @@ class RobotTCP(TcpDevice, Stoppable): def on_poll_info(self): on_poll_config_methods ={ "powered": {"cmd":"robot.set_powered", "def_kwargs": ""}, + "override_remote_safety": {"cmd":"robot.set_override_remote_safety", "def_kwargs": ""}, "speed": {"cmd":"robot.set_monitor_speed", "def_kwargs": ""}, "frame": {"cmd": "robot.set_frame", "def_kwargs": "change_default=True",}, "frame_coordinates": {"cmd": "robot.set_frame_coordinates", "def_kwargs": "",}, @@ -1287,7 +1295,12 @@ class RobotTCP(TcpDevice, Stoppable): pass def on_change_working_mode(self, working_mode, prev_working_mode): - pass + if working_mode == "remote": + self.set_profile("remote") + get_context().sendEvent("motion", "Working mode changed from {} to {}, changing user. \n".format(prev_working_mode, working_mode)) + else: + self.set_profile("default") + def on_change_status(self, status, prev_status): if status in ["joint", "cartesian_frame", "cartesian_tool", "point"]: