From 90552327048ccb66a93ab99ed4de1966fa9f53b6 Mon Sep 17 00:00:00 2001 From: gac-bernina Date: Mon, 18 Mar 2024 18:31:58 +0100 Subject: [PATCH] Fixed issues with spherical motion --- script/devices/RobotBernina.py | 24 ++++++++++++++++++++---- 1 file changed, 20 insertions(+), 4 deletions(-) diff --git a/script/devices/RobotBernina.py b/script/devices/RobotBernina.py index 55087c8..bf21b19 100644 --- a/script/devices/RobotBernina.py +++ b/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}