使用标定好的结果进行跟踪标定板的位置

坐标转换的步骤为：
1.图像坐标点转到相机坐标系下的点
2.相机坐标系下的点转为夹爪坐标系下的点
3.夹爪坐标系下的点转为机械手极坐标系下的点
跟踪的方式
1.采用标定板的第一个坐标点作为跟踪点
3.机器人每次移动到该点位，测试姿态是不是正确

# 测试标定的结果：

# 初始拍照位置
moveToConfig(sim, jointHandles, jmaxVel, jmaxAccel, jmaxJerk, targetjoinPos1)

# 10个拍照位置的验证
# for i in range(10):
while True:
    goalTr = targetPos[i].copy()
    # goalTr[2] = goalTr[2] - 0.2
    params = {}
    params['ik'] = {'tip': tipHandle, 'target': targetHandle}
    # params['object'] = targetHandle
    params['targetPose'] = goalTr
    params['maxVel'] = maxVel
    params['maxAccel'] = maxAccel
    params['maxJerk'] = maxJerk
    sim.moveToPose(params)

    img, [resX, resY] = sim.getVisionSensorImg(visionSensorHandle)
    img = np.frombuffer(img, dtype=np.uint8).reshape(resY, resX, 3)

    # In CoppeliaSim images are left to right (x-axis), and bottom to top (y-axis)
    # (consistent with the axes of vision sensors, pointing Z outwards, Y up)
    # and color format is RGB triplets, whereas OpenCV uses BGR:
    img = cv2.flip(cv2.cvtColor(img, cv2.COLOR_BGR2RGB), 0)
    # img=cv2.cvtColor(img, cv2.COLOR_BGR2RGB)

    # 获取深度图像
    Deepdate = sim.getVisionSensorDepth(deepSensorHandle, 1)
    num_floats = Deepdate[1][0] * Deepdate[1][1]
    depth_data = struct.unpack(f'{num_floats}f', Deepdate[0])
    depth_array = np.array(depth_data)
    depth_image = depth_array.reshape((Deepdate[1][1],  Deepdate[1][0]))
    depth_image = np.flipud(depth_image)

    gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
    # 找到棋盘格的角点
    ret, corners = cv2.findChessboardCorners(gray, chessboard_size, None)
    # 如果找到角点，使用第一个角点来转换为机器人的坐标
    if ret == True:
        u = corners[0][0][0]
        v = corners[0][0][1]
        Z = depth_image[int(v), int(u)]

        img = cv2.drawChessboardCorners(img, chessboard_size, corners, ret)
        ret, rvec, tvec = cv2.solvePnP(objp, corners, mtx, dist)

        # 计算相机坐标系下的三维点
        P_cam = pixel_to_camera_coordinates(u, v, Z, mtx)

        print("相机坐标系下的三维点 P_cam:", P_cam)
        # t_cam2gripper=t_cam2gripper.reshape(-1)
        # 计算物体在手爪坐标系中的位置

        # 计算点在末端坐标系下的坐标 P_end
        P_end = np.dot(R_cam2gripper, P_cam) + t_cam2gripper.reshape(-1)
        # sim.setObjectPosition(targetHandle, P_end,tipHandle)

        # 计算点在基座坐标系下的坐标 P_base
        tip_matrix = sim.getObjectMatrix(tipHandle)
        # 提取旋转矩阵 R_end_to_base (3x3)
        R_end_to_base = np.array([
            [tip_matrix[0], tip_matrix[1], tip_matrix[2]],
            [tip_matrix[4], tip_matrix[5], tip_matrix[6]],
            [tip_matrix[8], tip_matrix[9], tip_matrix[10]]
        ])

        # 提取平移向量 t_end_to_base (3x1)
        t_end_to_base = np.array([
            [tip_matrix[3]],
            [tip_matrix[7]],
            [tip_matrix[11]]
        ])

        P_base = np.dot(R_end_to_base, P_end) + t_end_to_base.reshape(-1)
        # sim.setObjectPosition(targetHandle, P_base)
        Tip_pose = sim.getObjectPose(tipHandle)

        # 将旋转向量转换为旋转矩阵
        R_board_to_camera, _ = cv2.Rodrigues(rvec)

        # 计算标定板相对于末端的旋转矩阵和平移向量
        R_board_to_end = R_cam2gripper @ R_board_to_camera
        t_board_to_end = R_cam2gripper @ tvec.flatten() + t_cam2gripper.flatten()

        # 计算标定板相对于世界坐标系的旋转矩阵和平移向量
        R_board_to_world = R_end_to_base @ R_board_to_end
        t_board_to_world = R_end_to_base @ t_board_to_end + t_end_to_base.flatten()
        chessboard_matrix = sim.getObjectMatrix(targetHandle)

        cal_chessboard_matrix = np.array([R_board_to_world[0][0], R_board_to_world[0][1], R_board_to_world[0][2], t_board_to_world[0],
                                          R_board_to_world[1][0], R_board_to_world[1][1], R_board_to_world[1][2], t_board_to_world[1],
                                          R_board_to_world[2][0], R_board_to_world[2][1], R_board_to_world[2][2], t_board_to_world[2]])

        sim.setObjectMatrix(targetHandle,  cal_chessboard_matrix)

        goalTr = Tip_pose.copy()
        goalTr[0]=P_base[0]
        goalTr[1]=P_base[1]
        goalTr[2]=P_base[2]
        params = {}
        params['ik'] = {'tip': tipHandle, 'target': targetHandle}
        # params['object'] = targetHandle
        params['targetPose'] = goalTr
        params['maxVel'] = maxVel
        params['maxAccel'] = maxAccel
        params['maxJerk'] = maxJerk
        sim.moveToPose(params)

    display.displayUpdated(img, depth_image)

测试的结果的