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 |
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
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/
Usage http://docs.ros.org/en/rolling/Tutorials/Tf2/Writing-A-Tf2-Broadcaster-Py.html#writingatf2broadcasterpy