init control_util

README.md

@@ -0,0 +1 @@
+nbv reconstruction controll franka

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())