-
-
Save salmagro/2e698ad4fbf9dae40244769c5ab74434 to your computer and use it in GitHub Desktop.
| 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 |
| 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
Why to use math.sin for quaternion_from_euler and then np.arcsin for euler_from_quaternion? Wouldn't it be better to use numpy in both cases?
Why to use
math.sinforquaternion_from_eulerand thennp.arcsinforeuler_from_quaternion? Wouldn't it be better to usenumpyin both cases?
True, I think I got some legacy examples from http://docs.ros.org/en/rolling/Tutorials/Intermediate/Tf2/Writing-A-Tf2-Broadcaster-Py.html#write-the-broadcaster-node
quaternion_from_euler results are inconsistent with docstring
q[0] is w
q[1] is x
q[2] is y
q[3] is z