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

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

坐标转换的步骤为：
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