Coppelia Sim (v-REP)仿真 机器人3D相机手眼标定与实时视觉追踪 (三)

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

坐标转换的步骤为:

1.图像坐标点转到相机坐标系下的点

2.相机坐标系下的点转为夹爪坐标系下的点

3.夹爪坐标系下的点转为机械手极坐标系下的点

跟踪的方式

1.采用标定板的第一个坐标点作为跟踪点

3.机器人每次移动到该点位,测试姿态是不是正确

python 复制代码
# 测试标定的结果:

# 初始拍照位置
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)

测试的结果的

相关推荐
天行健王春城老师11 小时前
基于TRIZ的教育机器人功能创新
经验分享·机器人
xwz小王子11 小时前
Science Robotics 综述揭示演化研究新范式,从机器人复活远古生物!
机器人·古生物启发
mirrornan11 小时前
产品如何3D建模?如何根据使用场景选购3D扫描仪?
科技·3d·3d建模·3d模型·三维扫描
兔老大的胡萝卜11 小时前
关于 3D Engine Design for Virtual Globes(三维数字地球引擎设计)
人工智能·3d
奔跑的花短裤11 小时前
少儿编程启蒙学习
学习·青少年编程·机器人·ai编程
深蓝学院11 小时前
无需姿态,即刻重建!NoPoSplat,重新定义3DGS的重建Pipeline
3d
智方科技11 小时前
cesium 3DTiles之pnts格式详解
3d
mirrornan12 小时前
3D看车如何实现?有哪些功能特点和优势?
3d·3d模型·3d交互展示·3d看车
一介青烟小生16 小时前
cesium渲染3DTiles模型和glb模型
3d·cesium
Matlab程序猿小助手1 天前
【MATLAB源码-第213期】基于matlab的16QAM调制解调系统软硬判决对比仿真,输出误码率曲线对比图。
开发语言·嵌入式硬件·算法·matlab·机器人