diff --git a/script/devices/RobotBernina.py b/script/devices/RobotBernina.py index 8525ac4..4329324 100644 --- a/script/devices/RobotBernina.py +++ b/script/devices/RobotBernina.py @@ -37,7 +37,7 @@ ROBOT_SPHERICAL_MOTORS = ["r" , "gamma", "delta"] run("devices/RobotTCP") override_remote_safety = False -simulation = True +simulation = False def dimensionality_decorator(func): def decorated(self, *args): @@ -190,6 +190,7 @@ def greater_equal(self, v): class RobotBernina(RobotTCP): def __init__(self, name, server, timeout = 1000, retries = 1): RobotTCP.__init__(self, name, server, timeout, retries) + self.invert_detector_rz = False self.set_tasks(TASKS) self.set_known_points(KNOWN_POINTS) self.setPolling(DEFAULT_ROBOT_POLLING) @@ -206,7 +207,10 @@ class RobotBernina(RobotTCP): def set_override_remote_safety(self, value): self.override_remote_safety = bool(value) - + + def set_invert_detector_rz(self, value): + self.invert_detector_rz = bool(value) + def remote_allowed(self, from_coordinates, to_coordinates, frame_trsf, tool_trsf, motion): if self.override_remote_safety: return True @@ -351,6 +355,10 @@ 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: + rz = rz+180 + if abs(rz/180) > 0: + rz = rz - (rz+180)/360*360 if return_dict: ret = {"x": x, "y": y, "z": z, "rx": rx, "ry": ry, "rz":rz} else: @@ -359,7 +367,11 @@ class RobotBernina(RobotTCP): 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)) + if z**2+x**2>0: + gamma = math.copysign(1, x)*math.acos(z/math.sqrt(z**2+x**2)) + else: + ry = kwargs["ry"] if "ry" in kwargs.keys() else self.cartesian_pos["ry"] + gamma, = self.deg2rad([ry]) delta = math.asin(y/r) gamma, delta = self.rad2deg([gamma, delta]) if return_dict: @@ -523,8 +535,19 @@ class RobotBernina(RobotTCP): }) 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: + distance = self.distance_p("tcp_p_spherical[1]", "tcp_p_spherical[3]") + + ## check if current point is on spherical trajectory (rx, ry, rz) + calculated_cartesian_starting_pos = self.sph2cart(**self.cart2sph(**target_cart_linear)) + is_spherical_starting_point = [abs(target_cart_linear[k] - calculated_cartesian_starting_pos[k]) < 1. for k in ["x", "y", "z"]] + is_spherical_starting_point = is_spherical_starting_point + [abs(abs(abs(target_cart_linear[k] - calculated_cartesian_starting_pos[k])-180)-180) < 1. for k in ["rx", "ry", "rz"]] + print(target_cart_linear) + print(calculated_cartesian_starting_pos) + print(is_spherical_starting_point) + is_spherical_starting_point=all(is_spherical_starting_point) + + if (self.point_reachable("tcp_p_spherical[3]"))&(distance>.1): + if (self.point_reachable("tcp_p_spherical[2]"))& (distance>1) & is_spherical_starting_point: if simulate: if len(sim)==0: for i in range(11): @@ -536,8 +559,9 @@ class RobotBernina(RobotTCP): ret = self.movec("tcp_p_spherical[2]", "tcp_p_spherical[3]", tool=None, desc=None, sync=sync) print("moving angle circle") else: + print("moving angle linear") if simulate: - if len(sim==0): + if len(sim)==0: for i in range(11): sim.append(self.get_movel_interpolation(i/10., coordinates=coordinates, idx_pnt1=1, idx_pnt2=3)) else: @@ -546,7 +570,6 @@ class RobotBernina(RobotTCP): else: self.set_motion_queue_empty(False) ret = self.movel("tcp_p_spherical[3]", tool=None, desc=None, sync=sync) - print("moving angle linear") if simulate: return sim diff --git a/script/devices/RobotTCP.py b/script/devices/RobotTCP.py index 8981597..5d5d2c0 100644 --- a/script/devices/RobotTCP.py +++ b/script/devices/RobotTCP.py @@ -765,6 +765,7 @@ class RobotTCP(TcpDevice, Stoppable): self.on_poll_status.update({ "connected": self.connected, "powered": self.powered, + "invert_detector_rz": self.invert_detector_rz, "override_remote_safety": self.override_remote_safety, "speed": self.speed, "settled": self.settled, @@ -921,6 +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, "connected": self.connected, "powered": self.powered, "speed": self.speed, @@ -960,6 +962,7 @@ class RobotTCP(TcpDevice, Stoppable): def on_poll_info(self): on_poll_config_methods ={ "powered": {"cmd":"robot.set_powered", "def_kwargs": ""}, + "invert_detector_rz": {"cmd":"robot.set_invert_detector_rz", "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",},