-
-
Save quantumxt/6d085560744afa6df781f11871aaebdd 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