optiomized spherical motion

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
             

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",},