Fixed issues with spherical motion

script/devices/RobotBernina.py

@@ -41,11 +41,12 @@ class RobotBernina(RobotTCP):
         }
         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))
 
     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]):
@@ -204,11 +205,26 @@ class RobotBernina(RobotTCP):
         x = r*math.cos(delta)*math.sin(gamma)
         y = r*math.sin(delta)
         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])
+
+        # New try first delta
+        ry = -math.asin(-math.cos(delta)*math.sin(gamma))
+        rx = -math.acos(math.cos(delta)*math.cos(gamma)/math.cos(ry))
+        rz = math.asin(-math.sin(ry)*math.sin(rx)*math.sqrt(1/(math.cos(rx)**2+(math.sin(ry)*math.sin(rx))**2)))
+        rx, ry, rz = self.rad2deg([rx, ry, rz])
+        #end new try
+
+        # New try: first gamme
+        #ry = gamma
+        #rx = -math.acos(math.cos(delta)*math.cos(gamma)/math.cos(ry))
+        #rz = -math.asin(-math.sin(-math.sin(ry)*math.sin(rx)*math.sqrt(1/(math.cos(rx)**2+(math.sin(ry)*math.sin(rx))**2))))
+        #rx, ry, rz = self.rad2deg([rx, ry, rz])
+        # End new try
+        
+        #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:
+        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}