From 71a88fcef212501e9e38b34decaf3ac19dfc3c83 Mon Sep 17 00:00:00 2001 From: TMM Date: Sun, 30 Mar 2025 17:25:42 -0400 Subject: [PATCH 1/2] Fix of to_eulerian function to handle singularities (pitch 90 or -90) --- PythonClient/airsim/utils.py | 35 ++++++++++++++++++----------------- 1 file changed, 18 insertions(+), 17 deletions(-) diff --git a/PythonClient/airsim/utils.py b/PythonClient/airsim/utils.py index 7f866d7d0a..acd64a9fd4 100644 --- a/PythonClient/airsim/utils.py +++ b/PythonClient/airsim/utils.py @@ -51,7 +51,7 @@ def write_file(filename, bstr): # helper method for converting getOrientation to roll/pitch/yaw # https:#en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles - + def to_eularian_angles(q): z = q.z_val y = q.y_val @@ -59,23 +59,24 @@ def to_eularian_angles(q): w = q.w_val ysqr = y * y - # roll (x-axis rotation) - t0 = +2.0 * (w*x + y*z) - t1 = +1.0 - 2.0*(x*x + ysqr) - roll = math.atan2(t0, t1) - # pitch (y-axis rotation) - t2 = +2.0 * (w*y - z*x) - if (t2 > 1.0): - t2 = 1 - if (t2 < -1.0): - t2 = -1.0 - pitch = math.asin(t2) - - # yaw (z-axis rotation) - t3 = +2.0 * (w*z + x*y) - t4 = +1.0 - 2.0 * (ysqr + z*z) - yaw = math.atan2(t3, t4) + test = +2.0 * (w*y - z*x) + #near -90 or 90 need to be handled different + if (abs(test) > 0.99999): + pitch = math.pi/2 * test + yaw = 2 * math.atan2(z,w) + roll = 0 + else: + #not sigularity + pitch = math.asin(test) + # roll (x-axis rotation) + t0 = +2.0 * (w*x + y*z) + t1 = +1.0 - 2.0*(x*x + ysqr) + roll = math.atan2(t0, t1) + # yaw (z-axis rotation) + t3 = +2.0 * (w*z + x*y) + t4 = +1.0 - 2.0 * (ysqr + z*z) + yaw = math.atan2(t3, t4) return (pitch, roll, yaw) From 1d2074b06a9e60ed8ca33f2901ad4210056027f6 Mon Sep 17 00:00:00 2001 From: TMM Date: Sun, 30 Mar 2025 20:03:06 -0400 Subject: [PATCH 2/2] Updated threshold for sigularity based on results from testing --- PythonClient/airsim/utils.py | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/PythonClient/airsim/utils.py b/PythonClient/airsim/utils.py index acd64a9fd4..ee30580f9b 100644 --- a/PythonClient/airsim/utils.py +++ b/PythonClient/airsim/utils.py @@ -61,8 +61,8 @@ def to_eularian_angles(q): # pitch (y-axis rotation) test = +2.0 * (w*y - z*x) - #near -90 or 90 need to be handled different - if (abs(test) > 0.99999): + #pitch near -90 or 90 need to be handled differently + if (abs(test) > 0.999999): pitch = math.pi/2 * test yaw = 2 * math.atan2(z,w) roll = 0 @@ -74,9 +74,9 @@ def to_eularian_angles(q): t1 = +1.0 - 2.0*(x*x + ysqr) roll = math.atan2(t0, t1) # yaw (z-axis rotation) - t3 = +2.0 * (w*z + x*y) - t4 = +1.0 - 2.0 * (ysqr + z*z) - yaw = math.atan2(t3, t4) + t2 = +2.0 * (w*z + x*y) + t3 = +1.0 - 2.0 * (ysqr + z*z) + yaw = math.atan2(t2, t3) return (pitch, roll, yaw)