Merge pull request #137 from elephantrobotics/mycobot280_vision

Mycobot280 vision

mycobot_280/mycobot_280/CMakeLists.txt

@@ -30,6 +30,7 @@ catkin_install_python(PROGRAMS
   scripts/follow_display_gripper.py
   scripts/slider_control_gripper.py
   scripts/listen_real_gripper.py
+  scripts/detect_stag.py
   DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
 )
 

mycobot_280/mycobot_280/camera_calibration/EyesInHand_matrix.json

@@ -0,0 +1 @@
+[[-0.996091560743614, 0.02359232376557422, -0.0851175943897141, -22.980358398950056], [-0.04045681823508953, -0.9785014715102557, 0.20223282649104501, -7.126480805661541], [-0.07851654904314409, 0.20488599881789024, 0.9756315283009006, 30.736245192702807], [0.0, 0.0, 0.0, 1.0]]

mycobot_280/mycobot_280/camera_calibration/camera_calibration.py

@@ -0,0 +1,197 @@
+import cv2
+from uvc_camera import UVCCamera
+import stag
+import numpy as np
+import json
+import time
+import os
+from scipy.linalg import svd
+from pymycobot import *
+from marker_utils import *
+import shutil
+import glob
+
+
+ports = glob.glob('/dev/ttyUSB*') + glob.glob('/dev/ttyACM*')
+print(ports)
+if ports:
+    arm_port = ports[0]
+else:
+    raise Exception("No MyCobot device found")
+
+
+mc = MyCobot(port=arm_port)  # 设置端口
+
+np.set_printoptions(suppress=True, formatter={'float_kind': '{:.2f}'.format})
+
+
+
+class camera_detect:
+    #Camera parameter initialize
+    def __init__(self, camera_id, marker_size, mtx, dist):
+        self.camera_id = camera_id
+        self.mtx = mtx
+        self.dist = dist
+        self.marker_size = marker_size
+        self.camera = UVCCamera(self.camera_id, self.mtx, self.dist)
+        self.camera_open()
+
+        self.origin_mycbot_horizontal = [0,60,-60,0,0,0]
+        self.origin_mycbot_level = [0, 5, -104, 14, 0, 90]
+
+        # Initialize EyesInHand_matrix to None or load from a document if available
+        self.EyesInHand_matrix = None
+        file_dir = os.path.dirname(os.path.dirname(os.path.abspath(__file__)))
+        self.matrix_file_path = os.path.join(file_dir, "config","EyesInHand_matrix.json")
+
+        self.load_matrix()
+
+    def save_matrix(self, filename="EyesInHand_matrix.json"):
+        # Save the EyesInHand_matrix to a JSON file
+        if self.EyesInHand_matrix is not None:
+            with open(filename, 'w') as f:
+                json.dump(self.EyesInHand_matrix.tolist(), f)
+
+            try:
+                # 复制文件到目标路径
+                shutil.copy(filename, self.matrix_file_path)
+                print(f"File copied to {self.matrix_file_path}")
+            except IOError as e:
+                print(f"Failed to copy file: {e}")
+
+    def load_matrix(self, filename="EyesInHand_matrix.json"):
+        # Load the EyesInHand_matrix from a JSON file, if it exists
+        try:
+            with open(filename, 'r') as f:
+                self.EyesInHand_matrix = np.array(json.load(f))
+        except FileNotFoundError:
+            print("Matrix file not found. EyesInHand_matrix will be initialized later.")
+
+    def wait(self):
+        time.sleep(0.5)
+        while(mc.is_moving() == 1):
+            time.sleep(0.2)
+
+    def camera_open(self):
+        self.camera.capture()  # 打开摄像头
+
+    # 获取物体坐标(相机系)
+    def calc_markers_base_position(self, corners, ids):
+        if len(corners) == 0:
+            return []
+        rvecs, tvecs = solve_marker_pnp(corners, self.marker_size, self.mtx, self.dist)  # 通过二维码角点获取物体旋转向量和平移向量
+        for i, tvec, rvec in zip(ids, tvecs, rvecs):
+            tvec = tvec.squeeze().tolist()
+            rvec = rvec.squeeze().tolist()
+            rotvector = np.array([[rvec[0], rvec[1], rvec[2]]])
+            Rotation = cv2.Rodrigues(rotvector)[0]  # 将旋转向量转为旋转矩阵
+            Euler = self.CvtRotationMatrixToEulerAngle(Rotation)  # 将旋转矩阵转为欧拉角
+            target_coords = np.array([tvec[0], tvec[1], tvec[2], Euler[0], Euler[1], Euler[2]])  # 物体坐标(相机系)
+        return target_coords
+
+
+    def eyes_in_hand_calculate(self, pose, tbe1, Mc1, tbe2, Mc2, tbe3, Mc3, Mr):
+
+        tbe1, Mc1, tbe2, Mc2, tbe3, Mc3, Mr = map(np.array, [tbe1, Mc1, tbe2, Mc2, tbe3, Mc3, Mr])
+        # Convert pose from degrees to radians
+        euler = np.array(pose) * np.pi / 180
+        Rbe = self.CvtEulerAngleToRotationMatrix(euler)
+        print("Rbe", Rbe)
+        Reb = Rbe.T
+
+        A = np.hstack([(Mc2 - Mc1).reshape(-1, 1), 
+                    (Mc3 - Mc1).reshape(-1, 1), 
+                    (Mc3 - Mc2).reshape(-1, 1)])
+
+        b = Reb @ np.hstack([(tbe1 - tbe2).reshape(-1, 1), 
+                            (tbe1 - tbe3).reshape(-1, 1), 
+                            (tbe2 - tbe3).reshape(-1, 1)])
+
+        print("A = ", A)
+        print("B = ", b)
+        U, S, Vt = svd(A @ b.T)
+        Rce = Vt.T @ U.T
+
+        tce = Reb @ (Mr - (1/3)*(tbe1 + tbe2 + tbe3) - (1/3)*(Rbe @ Rce @ (Mc1 + Mc2 + Mc3)))
+
+        eyes_in_hand_matrix = np.vstack([np.hstack([Rce, tce.reshape(-1, 1)]), np.array([0, 0, 0, 1])])
+
+        return eyes_in_hand_matrix
+
+    # 读取Camera坐标（单次）
+    def stag_identify(self):
+        self.camera.update_frame()  # 刷新相机界面
+        frame = self.camera.color_frame()  # 获取当前帧
+        (corners, ids, rejected_corners) = stag.detectMarkers(frame, 11)  # 获取画面中二维码的角度和id
+        # 绘制检测到的标记及其ID
+        stag.drawDetectedMarkers(frame, corners, ids)
+        # 绘制被拒绝的候选区域，颜色设为红色
+        stag.drawDetectedMarkers(frame, rejected_corners, border_color=(255, 0, 0))
+        marker_pos_pack = self.calc_markers_base_position(corners, ids)  # 获取物的坐标(相机系)
+        if(len(marker_pos_pack) == 0):
+            marker_pos_pack = self.stag_identify()
+        # print("Camera coords = ", marker_pos_pack)
+        # cv2.imshow("rrrr", frame)
+        # cv2.waitKey(1)
+        return marker_pos_pack
+
+    def Eyes_in_hand_calibration(self, ml):
+        ml.send_angles(self.origin_mycbot_level, 50)  # 移动到观测点
+        self.wait()
+        input("make sure camera can observe the stag, enter any key quit")
+        coords = ml.get_coords()
+        pose = coords[3:6]
+        print(pose)
+        # self.camera_open_loop()
+        Mc1,tbe1 = self.reg_get(ml)
+        ml.send_coord(1, coords[0] + 30, 30)
+        self.wait()
+        Mc2,tbe2 = self.reg_get(ml)
+        ml.send_coord(1, coords[0] - 10, 30)
+        self.wait()
+        ml.send_coord(3, coords[2] + 20, 30)
+        self.wait()
+        Mc3,tbe3 = self.reg_get(ml)
+
+        input("Move the end of the robot arm to the calibration point, press any key to release servo")
+        ml.release_all_servos()
+        input("focus servo and get current coords")
+        ml.power_on()
+        time.sleep(1)
+        coords = ml.get_coords()
+        while len(coords) == 0:
+            coords = ml.get_coords()
+        Mr = coords[0:3]
+        print(Mr)
+
+        self.EyesInHand_matrix = self.eyes_in_hand_calculate(pose, tbe1, Mc1, tbe2, Mc2, tbe3, Mc3, Mr)
+        print("EyesInHand_matrix = ", self.EyesInHand_matrix)
+        self.save_matrix()  # Save the matrix to a file after calculating it
+        print("save successe")
+
+    def reg_get(self, ml):
+        for i in range(50):
+            Mc_all = self.stag_identify()
+        tbe_all = ml.get_coords() # 获取机械臂当前坐标
+        while (tbe_all is None):
+            tbe_all = ml.get_coords()
+
+        tbe = tbe_all[0:3]
+        Mc = Mc_all[0:3]
+        print("tbe = ", tbe)
+        print("Mc = ", Mc)
+        return Mc,tbe
+
+
+if __name__ == "__main__":
+    if mc.is_power_on()==0:
+        mc.power_on()
+    camera_params = np.load("camera_params.npz")  # 相机配置文件
+    mtx, dist = camera_params["mtx"], camera_params["dist"]
+    m = camera_detect(0, 32, mtx, dist)
+    tool_len = 20
+    mc.set_tool_reference([0, 0, tool_len, 0, 0, 0])
+    mc.set_end_type(1)
+
+    m.Eyes_in_hand_calibration(mc)  #手眼标定
+

mycobot_280/mycobot_280/camera_calibration/marker_utils.py

@@ -0,0 +1,59 @@
+import cv2
+import numpy as np
+import typing as T
+from numpy.typing import NDArray, ArrayLike
+
+
+class MarkerInfo(T.TypedDict):
+    corners: np.ndarray
+    tvec: np.ndarray
+    rvec: np.ndarray
+    num_id: int
+
+
+def solve_marker_pnp(corners: NDArray, marker_size: int, mtx: NDArray, dist: NDArray):
+    """
+    This will estimate the rvec and tvec for each of the marker corners detected by:
+       corners, ids, rejectedImgPoints = detector.detectMarkers(image)
+    corners - is an array of detected corners for each detected marker in the image
+    marker_size - is the size of the detected markers
+    mtx - is the camera matrix
+    distortion - is the camera distortion matrix
+    RETURN list of rvecs, tvecs, and trash (so that it corresponds to the old estimatePoseSingleMarkers())
+    """
+    marker_points = np.array(
+        [
+            [-marker_size / 2, marker_size / 2, 0],
+            [marker_size / 2, marker_size / 2, 0],
+            [marker_size / 2, -marker_size / 2, 0],
+            [-marker_size / 2, -marker_size / 2, 0],
+        ],
+        dtype=np.float32,
+    )
+    rvecs = []
+    tvecs = []
+    for corner in corners:
+        retval, rvec, tvec = cv2.solvePnP(
+            marker_points,
+            corner,
+            mtx,
+            dist,
+            flags=cv2.SOLVEPNP_IPPE_SQUARE,
+        )
+        if retval:
+            rvecs.append(rvec)
+            tvecs.append(tvec)
+
+    rvecs = np.array(rvecs)  # type: ignore
+    tvecs = np.array(tvecs)  # type: ignore
+    (rvecs - tvecs).any()  # type: ignore
+    return rvecs, tvecs
+
+
+def draw_marker(frame: np.ndarray, corners, tvecs, rvecs, ids, mtx, dist) -> None:
+    # cv2.aruco.drawDetectedMarkers(frame, corners, None, borderColor=(0, 255, 0))
+    cv2.aruco.drawDetectedMarkers(frame, corners, ids, borderColor=(0, 200, 200))
+    for i in range(len(ids)):
+        corner, tvec, rvec, marker_id = corners[i], tvecs[i], rvecs[i], ids[i]
+        cv2.drawFrameAxes(frame, mtx, dist, rvec, tvec, 60, 2)
+

mycobot_280/mycobot_280/camera_calibration/uvc_camera.py

@@ -0,0 +1,55 @@
+import cv2
+import numpy as np
+import time
+import typing
+
+
+class UVCCamera:
+    def __init__(
+        self,
+        cam_index=0,
+        mtx=None,
+        dist=None,
+        capture_size: typing.Tuple[int, int] = (640, 480),
+    ):
+        super().__init__()
+        self.cam_index = cam_index
+        self.mtx = mtx
+        self.dist = dist
+        self.curr_color_frame: typing.Union[np.ndarray, None] = None
+        self.capture_size = capture_size
+
+    def capture(self):
+        self.cap = cv2.VideoCapture(self.cam_index) #windows
+        width, height = self.capture_size
+        self.cap.set(cv2.CAP_PROP_FRAME_WIDTH, width)
+        self.cap.set(cv2.CAP_PROP_FRAME_HEIGHT, height)
+
+    def update_frame(self) -> bool:
+        ret, self.curr_color_frame = self.cap.read()
+        return ret
+
+    def color_frame(self) -> typing.Union[np.ndarray, None]:
+        return self.curr_color_frame
+
+    def release(self):
+        self.cap.release()
+
+
+if __name__ == "__main__":
+    cam = UVCCamera(2)
+    cam.capture()
+    while True:
+        if not cam.update_frame():
+            continue
+
+        frame = cam.color_frame()
+        if frame is None:
+            time.sleep(0.01)
+            continue
+
+        print(frame.shape)
+        window_name = "preview"
+        cv2.imshow(window_name, frame)
+        if cv2.waitKey(1) == ord("q"):
+            break

mycobot_280/mycobot_280/config/EyesInHand_matrix.json

@@ -0,0 +1 @@
+[[-0.996091560743614, 0.02359232376557422, -0.0851175943897141, -22.980358398950056], [-0.04045681823508953, -0.9785014715102557, 0.20223282649104501, -7.126480805661541], [-0.07851654904314409, 0.20488599881789024, 0.9756315283009006, 30.736245192702807], [0.0, 0.0, 0.0, 1.0]]