From 07300696c4d58fa071e9a626444dd80e52e1c823 Mon Sep 17 00:00:00 2001
From: hofee <lexhofee@gmail.com>
Date: Sun, 6 Oct 2024 21:30:20 +0800
Subject: [PATCH] init control_util

---
 README.md               |  1 +
 utils/control_util.py   | 39 +++++++++++++++++++++++++++++++++++++++
 utils/inference_util.py |  0
 3 files changed, 40 insertions(+)
 create mode 100644 README.md
 create mode 100644 utils/control_util.py
 create mode 100644 utils/inference_util.py

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