Created
January 11, 2021 20:13
-
-
Save salmagro/2e698ad4fbf9dae40244769c5ab74434 to your computer and use it in GitHub Desktop.
ROS2 euler to quaternion transformation.
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
def euler_from_quaternion(quaternion): | |
""" | |
Converts quaternion (w in last place) to euler roll, pitch, yaw | |
quaternion = [x, y, z, w] | |
Bellow should be replaced when porting for ROS 2 Python tf_conversions is done. | |
""" | |
x = quaternion.x | |
y = quaternion.y | |
z = quaternion.z | |
w = quaternion.w | |
sinr_cosp = 2 * (w * x + y * z) | |
cosr_cosp = 1 - 2 * (x * x + y * y) | |
roll = np.arctan2(sinr_cosp, cosr_cosp) | |
sinp = 2 * (w * y - z * x) | |
pitch = np.arcsin(sinp) | |
siny_cosp = 2 * (w * z + x * y) | |
cosy_cosp = 1 - 2 * (y * y + z * z) | |
yaw = np.arctan2(siny_cosp, cosy_cosp) | |
return roll, pitch, yaw |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
def quaternion_from_euler(roll, pitch, yaw): | |
""" | |
Converts euler roll, pitch, yaw to quaternion (w in last place) | |
quat = [x, y, z, w] | |
Bellow should be replaced when porting for ROS 2 Python tf_conversions is done. | |
""" | |
cy = math.cos(yaw * 0.5) | |
sy = math.sin(yaw * 0.5) | |
cp = math.cos(pitch * 0.5) | |
sp = math.sin(pitch * 0.5) | |
cr = math.cos(roll * 0.5) | |
sr = math.sin(roll * 0.5) | |
q = [0] * 4 | |
q[0] = cy * cp * cr + sy * sp * sr | |
q[1] = cy * cp * sr - sy * sp * cr | |
q[2] = sy * cp * sr + cy * sp * cr | |
q[3] = sy * cp * cr - cy * sp * sr | |
return q |
yes,as my test,quaternion_from_euler result is 'w' in first place,so it is [w,x,y,z]
Yes, thanks for noticing.
The correct info should be:
"""
Converts quaternion (w in last place) to euler roll, pitch, yaw
quaternion = [w, x, y, z]
Bellow should be replaced when porting for ROS 2 Python tf_conversions is done.
"""
There is https://discourse.ros.org/t/tf-transformations-ros-2-python-package-for-easy-tf-math/21077
Github: https://github.com/DLu/tf_transformations/
sudo apt install ros-foxy-tf-transformations
sudo pip3 install transforms3d
import tf_transformations
q = tf_transformations.quaternion_from_euler(r, p, y)
r, p, y = tf_transformations.euler_from_quaternion(quaternion)
Thanks, @Darkproduct. However, your proposed solutions are not available yet for Debian
.
In the meantime, I have included a class to avoid the confusion with the indexes and the order of x, y, z, w:
class Quaternion:
w: float
x: float
y: float
z: float
def quaternion_from_euler(roll, pitch, yaw):
"""
Converts euler roll, pitch, yaw to quaternion
"""
cy = math.cos(yaw * 0.5)
sy = math.sin(yaw * 0.5)
cp = math.cos(pitch * 0.5)
sp = math.sin(pitch * 0.5)
cr = math.cos(roll * 0.5)
sr = math.sin(roll * 0.5)
q = Quaternion()
q.w = cy * cp * cr + sy * sp * sr
q.x = cy * cp * sr - sy * sp * cr
q.y = sy * cp * sr + cy * sp * cr
q.z = sy * cp * cr - cy * sp * sr
return q
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment
quaternion_from_euler results are inconsistent with docstring
q[0] is w
q[1] is x
q[2] is y
q[3] is z