-
-
Save LeekaiDel/7c4476f3435c723e2b5983a16433a65d to your computer and use it in GitHub Desktop.
ROS2 euler to quaternion transformation.
This file contains hidden or 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 hidden or 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 |
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment