Convert Euler angles (roll, pitch, yaw) to quaternion. The order of Euler angles is yaw-pitch-roll (Z-Y-X axis rotation order).
(roll, pitch, yaw)
| 3 | from math import atan2, asin, degrees, cos, sin, sqrt |
| 4 | |
| 5 | def euler_to_quaternion(roll, pitch, yaw): |
| 6 | """ |
| 7 | Convert Euler angles (roll, pitch, yaw) to quaternion. |
| 8 | The order of Euler angles is yaw-pitch-roll (Z-Y-X axis rotation order). |
| 9 | """ |
| 10 | cy = math.cos(yaw * 0.5) |
| 11 | sy = math.sin(yaw * 0.5) |
| 12 | cp = math.cos(pitch * 0.5) |
| 13 | sp = math.sin(pitch * 0.5) |
| 14 | cr = math.cos(roll * 0.5) |
| 15 | sr = math.sin(roll * 0.5) |
| 16 | |
| 17 | w = cy * cp * cr + sy * sp * sr |
| 18 | x = cy * cp * sr - sy * sp * cr |
| 19 | y = sy * cp * cr + cy * sp * sr |
| 20 | z = sy * cp * sr - cy * sp * cr |
| 21 | |
| 22 | return [x, y, z, w] |
| 23 | |
| 24 | def quaternion_to_rotation_matrix(QW, QX, QY, QZ): |
| 25 | """ |
no outgoing calls
no test coverage detected