From 84d1120d6b7e29929ad6ba7672decfde4b2c5762 Mon Sep 17 00:00:00 2001 From: Pierre Tilak Date: Mon, 3 Jul 2023 15:05:27 +0200 Subject: [PATCH] Fix RPY --- api/parsers/ulgparser.py | 20 +++++++++----------- 1 file changed, 9 insertions(+), 11 deletions(-) diff --git a/api/parsers/ulgparser.py b/api/parsers/ulgparser.py index cb5551a..40de1bf 100644 --- a/api/parsers/ulgparser.py +++ b/api/parsers/ulgparser.py @@ -16,21 +16,19 @@ def euler_from_quaternion(self, w, x, y, z): angles = {} #roll(x-axis rotation) - sinr_cosp = 2 * (w * x + y * z); - cosr_cosp = 1 - 2 * (x * x + y * y); - angles['roll'] = math.atan2(sinr_cosp, cosr_cosp); + sinr_cosp = 2 * (w * x + y * z) + cosr_cosp = 1 - 2 * (x * x + y * y) + angles['roll'] = math.atan2(sinr_cosp, cosr_cosp) #pitch (y-axis rotation) - sinp = 2 * (w * y - z * x); - if (abs(sinp) >= 1): - angles['pitch'] = math.copysign(math.pi / 2, sinp); # use 90 degrees if out of range - else: - angles['pitch'] = math.asin(sinp); + sinp = math.sqrt(1+2*(w*y - x*z)) + cosp = math.sqrt(1-2*(w*y - x*z)) + angles['pitch'] = 2*math.atan2(sinp,cosp) - math.pi/2 # yaw (z-axis rotation) - siny_cosp = 2 * (w * z + x * y); - cosy_cosp = 1 - 2 * (y * y + z * z); - angles['yaw'] = math.atan2(siny_cosp, cosy_cosp); + siny_cosp = 2 * (w * z + x * y) + cosy_cosp = 1 - 2 * (y * y + z * z) + angles['yaw'] = math.atan2(siny_cosp, cosy_cosp) return angles