获取标定位姿

Massage/MassageControl/hardware/dobot_nova5.py

@@ -718,7 +718,7 @@ class dobot_nova5:
         self.last_pose_command = np.zeros(6)
         self.last_input_command = None
         self.tcp_offset = None
-        self.init_J = [0,30,-120,0,90,0]
+        self.init_J = [-45,60,-120,0,90,0]
         self.filter_matirx = np.zeros((1,7)) # 位姿伺服用
         self.filter_matrix = np.zeros((1,6)) # 角度伺服用
         sleep(1)

Massage/MassageControl/tools/capture.py

@@ -0,0 +1,60 @@
+from hardware.dobot_nova5 import dobot_nova5
+import cv2
+from hardware.remote_cam import ToolCamera
+import threading
+
+import numpy as np
+import os
+class Getpose:
+    def __init__(self):
+        self.arm = dobot_nova5("192.168.5.1")
+        self.arm.start()
+        self.arm.init()
+        self.cam = ToolCamera(host='127.0.0.1')
+        self.cam.start()
+        
+        self.thread1=threading.Thread(target=self.show)
+        self.thread1.start()
+        self.pose_data=[]
+        self.image_data=[]
+        self.arm.start_drag()
+    
+    def show(self):
+        while True:
+            self.rgb, self.depth, intrinsics = self.cam.get_latest_frame()
+            cv2.imshow(self.rgb)
+
+    def add_data(self):
+        angle=self.arm.getAngel()
+        self.pose_data=self.pose_data.append(angle)
+        print(angle)
+
+    def save_data(self):
+        data=np.array(self.pose_data)
+        np.savetxt('pose.txt', data ,fmt='%.5f')
+
+if __name__ == "__main__":
+    save_directory = '/home/jsfb/jsfb_ws/global_config/captured_images'
+    if not os.path.exists(save_directory):
+        os.makedirs(save_directory)
+    sele=Getpose()
+    
+    key = cv2.waitKey(1) & 0xFF
+    while True:
+        i=1
+        if key == ord('s'):
+            img_path = f'photo{i}.png'
+            filename = os.path.join(save_directory, img_path)
+            cv2.imwrite(filename, sele.rgb) # 保存照片
+            sele.add_data()
+            
+            filename2 = os.path.join(save_directory, f'pose.txt')
+            np.savetxt(filename2, sele.pose_data)
+            # pose = increase_dof(poselist[index])
+            
+            i += 1
+            # 按下'q'键退出循环
+        elif key == ord('q'):
+            sele.arm.disableRobot()
+            break
+    

Massage/MassageControl/tools/get_visual_valibration_pose.py

@@ -1,36 +0,0 @@
-from hardware.dobot_nova5 import dobot_nova5
-import cv2
-from hardware.remote_cam import ToolCamera
-import threading
-import keyboard
-import numpy as np
-class Getpose:
-    def __init__(self):
-        self.arm = dobot_nova5("192.168.5.1")
-        self.arm.start()
-        self.cam = ToolCamera(host='127.0.0.1')
-        self.cam.start()
-        self.thread1=threading.Thread(target=self.show)
-        self.thread1.start()
-        self.data=[]
-        self.arm.start_drag()
-    def show(self):
-        while True:
-            rgb, depth, intrinsics = self.cam.get_latest_frame()
-            cv2.imshow(rgb)
-    def add_data(self):
-        angle=self.arm.getAngel()
-        self.data=self.data.append(angle)
-        print(angle)
-    def save_data(self):
-        data=np.array(self.data)
-        np.savetxt('pose.txt', data ,fmt='%.5f')
-if __name__ == "__main__":
-    
-    sele=Getpose()
-
-    keyboard.add_hotkey('ctrl+shift+a', lambda: sele.add_data())
-    keyboard.add_hotkey('ctrl+shift+s', lambda: sele.save_data())
-    keyboard.add_hotkey('ctrl+shift+c', lambda: sele.arm.disableRobot())
-    # 阻塞监听（保持程序运行）
-    keyboard.wait('esc')  # 按ESC退出