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)

测试的结果的

相关推荐
Robot25113 小时前
Figure 02迎重大升级!!人形机器人独角兽[Figure AI]商业化加速
人工智能·机器人·微信公众平台
FreeIPCC17 小时前
谈一下开源生态对 AI人工智能大模型的促进作用
大数据·人工智能·机器人·开源
前端Hardy18 小时前
HTML&CSS:数据卡片可以这样设计
前端·javascript·css·3d·html
施努卡机器视觉19 小时前
电解车间铜业机器人剥片技术是现代铜冶炼过程中自动化和智能化的重要体现
运维·机器人·自动化
小彭努力中1 天前
138. CSS3DRenderer渲染HTML标签
前端·深度学习·3d·webgl·three.js
zhd15306915625ff1 天前
库卡机器人日常维护
网络·机器人·自动化·机器人备件
古月居GYH1 天前
ROS一键安装脚本
人工智能·机器人·ros
AI生成未来1 天前
斯坦福&UC伯克利开源突破性视觉场景生成与编辑技术,精准描绘3D/4D世界!
3d·3d场景·4d
清流君1 天前
【运动规划】移动机器人运动规划与轨迹优化全解析 | 经典算法总结
人工智能·笔记·算法·机器人·自动驾驶·运动规划
Matlab程序猿小助手1 天前
【MATLAB源码-第218期】基于matlab的北方苍鹰优化算法(NGO)无人机三维路径规划,输出做短路径图和适应度曲线.
开发语言·嵌入式硬件·算法·matlab·机器人·无人机