Halcon之根据SCARA手眼标定来执行拾取和放置

Halcon之根据SCARA手眼标定来执行拾取和放置

  这个示例展示了如何使用SCARA机器人根据SCARA手眼校准确定的校准信息来执行拾取和放置应用程序。其主要分为两个部分：
  一是利用模型图像来定义目标的形状；
  二是根据形状模型，在每幅图像中寻找目标，对于选中的目标，计算机器人坐标，然后进行抓取。

（为了使示例适用于实际应用程序，必须通过摄像机获取图像，而不能从文件中读取）

* 为了使用提供的示例图像运行例程，必须将校正图像设置为true。
*
RectifyImages := true
* 
* 读取HDevelop示例程序中的calibrate_hand_eye_scara_stationary_cam和align_hand_eye_scara_stationary_cam.hdev之一提供的校准信息。

try
    * 读出手眼标定的结果
    * 从文本文件中读取相机外部的3D位姿
    read_pose ('cam_in_base_pose.dat', CamInBasePose)
    * 读取被抓取物体的位姿估算所需的数据
    * 从文本文件中读取相机内部参数
    read_cam_par ('camera_parameters.dat', CameraParam)
    * 读取在相机3D位姿上测量平面的的3D位姿，MPInCamPose为测量平面的3D位姿
    read_pose ('measurement_plane_in_cam_pose.dat', MPInCamPose)
catch (Exception)
    * 当文件的校准信息不能获取，则使用标注的校准信息代替
    * 要提供文件上的校准信息，运行halcon例程中的 calibrate_hand_eye_scara_stationary_cam.hdev
    * 或者 align_hand_eye_scara_stationary_cam.hdev
    CamInBasePose := [0.05592166548,0.19497621789,0.48025117245,180.09816119,29.85593363,179.94389014,0]
    CameraParam := [0.0165251,-642.277,4.65521e-006,4.65e-006,595.817,521.75,1280,1024]
    MPInCamPose := [0.0045679683065,-0.0028695297318,0.4088853425,359.78658429,29.732027579,0.22946472765,0]
endtry
* 
* 准备校准map以消除图像失真
if (RectifyImages)
    * 将测量平面的3D位姿的点坐标转换为摄像机坐标系的齐次3D变换矩阵
    prepare_rectification_map (Map, CameraParam, MPInCamPose, MappingScale, MPInCamPoseMapping)
    * 将图像点转换为z=0的世界坐标系平面
    image_points_to_world_plane (CameraParam, MPInCamPoseMapping, 0, 0, 'm', MapUpperLeftX, MapUpperLeftY)
endif
* 
dev_update_off ()
* 设置HALCON系统参数，border_shape_models利用多个算子寻找形状模型
set_system ('border_shape_models', 'true')
* 
* 此处, 应建立与机器人的连接，并将机器人移动到一个确定的备用位姿
* 以允许获取测量平面的无遮挡图像
* 
* 确定被抓去对象的形状模型

* - 获取用于模型生成的图像
read_image (Image, '3d_machine_vision/handeye/scara_stationary_cam_setup_01_metal_parts_04')
* 图像校正
if (RectifyImages)
    * 对图像进行一般变换
    map_image (Image, Map, ModelImage)
else 
    * 如果不需要校正，则复制图像并为它分配一个新内存
    copy_image (Image, ModelImage)
endif
* 
dev_close_window ()
dev_open_window_fit_image (ModelImage, 0, 0, 600, 600, WindowHandle)
set_display_font (WindowHandle, 16, 'mono', 'true', 'false')
dev_clear_window ()
dev_display (ModelImage)
dev_set_line_width (2)
*
* - 创建形状模型
*
* 创建一个平行于坐标轴的矩形(也可以用算子画任意方向的矩形)
draw_