refactory: task generate

2025-09-04 16:28:11 +08:00
parent 87b3198e26
commit da022d4f83
19 changed files with 2926 additions and 22 deletions
--- a/utils/pose.py
+++ b/utils/pose.py
@@ -0,0 +1,15 @@
+
+from scipy.spatial.transform import Rotation as R
+
+class PoseUtil:
+    @staticmethod
+    def get_quaternion_wxyz_from_rotation_matrix(rotation_matrix):
+        rot = R.from_matrix(rotation_matrix)
+        quat = rot.as_quat()
+
+        if quat.shape[0] == 4:
+            quaternions_wxyz = quat[[3, 0, 1, 2]]
+        else:
+            quaternions_wxyz = quat[:, [3, 0, 1, 2]]
+        return quaternions_wxyz
+