commit 07300696c4d58fa071e9a626444dd80e52e1c823
Author: hofee <lexhofee@gmail.com>
Date:   Sun Oct 6 21:30:20 2024 +0800

    init control_util

diff --git a/README.md b/README.md
new file mode 100644
index 0000000..c94204e
--- /dev/null
+++ b/README.md
@@ -0,0 +1 @@
+nbv reconstruction controll franka
\ No newline at end of file
diff --git a/utils/control_util.py b/utils/control_util.py
new file mode 100644
index 0000000..65d9129
--- /dev/null
+++ b/utils/control_util.py
@@ -0,0 +1,39 @@
+import numpy as np
+from frankapy import FrankaArm
+
+class ControlUtil:
+    
+    __fa = FrankaArm(robot_num=2)
+    
+    DISPLAYTABLE_TO_BASE:np.ndarray = np.asarray([
+        [1, 0, 0, 0],
+        [0, 1, 0, 0],
+        [0, 0, 1, 0],
+        [0, 0, 0, 1]
+        ])
+    
+    GRIPPER_TO_CAMERA:np.ndarray = np.asarray([
+        [1, 0, 0, 0],
+        [0, 1, 0, 0],
+        [0, 0, 1, 0],
+        [0, 0, 0, 1]
+        ])
+    
+    @staticmethod
+    def get_franka_arm() -> FrankaArm:
+        return ControlUtil.__fa
+    
+    @staticmethod
+    def get_pose() -> np.ndarray:
+        return ControlUtil.__fa.get_pose().matrix
+    
+    @staticmethod
+    def reset() -> None:
+        ControlUtil.__fa.reset_joints()
+        
+# ----------- Debug Test -------------
+
+if __name__ == "__main__":
+    print(ControlUtil.get_pose())
+    ControlUtil.reset()
+    print(ControlUtil.get_pose())
\ No newline at end of file
diff --git a/utils/inference_util.py b/utils/inference_util.py
new file mode 100644
index 0000000..e69de29