(self, roll, pitch, yaw)
| 67 | ]) |
| 68 | |
| 69 | def transform_to_camera_frame(self, roll, pitch, yaw): |
| 70 | R_roll = self.rotation_matrix_roll(roll) |
| 71 | R_pitch = self.rotation_matrix_pitch(pitch) |
| 72 | R_yaw = self.rotation_matrix_yaw(yaw) |
| 73 | R_combined = np.dot(R_pitch, np.dot(R_yaw, R_roll)) |
| 74 | QW, QX, QY, QZ = rotation_matrix_to_quaternion(R_combined) |
| 75 | print(f"QW: {QW}, QX: {QX}, QY: {QY}, QZ: {QZ}") |
| 76 | transformation_matrix = np.array([ |
| 77 | [0, -1, 0], |
| 78 | [0, 0, -1], |
| 79 | [1, 0, 0] |
| 80 | ]) |
| 81 | new_R = np.dot(transformation_matrix, R_combined) |
| 82 | QW_new, QX_new, QY_new, QZ_new = rotation_matrix_to_quaternion(new_R) |
| 83 | return QW_new, QX_new, QY_new, QZ_new |
| 84 | |
| 85 | def set_camera_pose(self, x, y, z, pitch, yaw, roll, path_params): |
| 86 | yaw = -yaw |
no test coverage detected