Halcon pick_and_place_scara_stationary_cam.hdev程序学习

此示例显示如何基于由SCARA手眼校准确定的校准信息，使用SCARA机器人执行拾取和放置应用程序。 在第一步骤中，根据模型图像定义形状模型。 然后，基于该形状模型，在每个图像中搜索对象。 对于一个选定的对象，计算可用于掌握该对象的机器人坐标。 为了使该示例适用于实际应用，必须从相机获取图像（而不是从文件读取），并且必须实施机器人的控制（而不是在本示例中被注释掉的相应行）。 通常，图像必须在匹配之前校正。 如果摄像机完全正交于测量平面，则此步骤可以仅被省略。 要使用提供的示例图像运行示例程序，RectifyImages必须设置为true。

RectifyImages := true

try
　　//读取手眼校正的结果
　　read_pose ('cam_in_base_pose.dat', CamInBasePose)
　　read_cam_par ('camera_parameters.dat', CameraParam)
　　//读取要抓取的对象的姿态估计所需要的参数
　　read_pose ('measurement_plane_in_cam_pose.dat', MPInCamPose)
catch (Exception)　
　　CamInBasePose:=[0.05592166548,0.19497621789,0.48025117245,180.09816119,29.85593363,179.94389014,]　
　　CameraParam := [0.0165251,-642.277,.65521e-,.65e-,595.817,521.75,,　　]
　　MPInCamPose := [0.0045679683065,-0.0028695297318,0.4088853425,359.78658429,29.7320275　　,0.22946472765,]
endtry
//准备校准map以消除图像的失真
if (RectifyImages)
　　prepare_rectification_map (Map, CameraParam, MPInCamPose, MappingScale,MPInCamPoseMapping)
　　image_points_to_world_plane (CameraParam, MPInCamPoseMapping, , , 'm',MapUpperLeftX, MapUpperLeftY)
endif
dev_update_off ()
set_system ('border_shape_models', 'true')
//这里，应该建立与机器人的连接，并且机器人应该移动到一个定义的待机姿态，允许获取测量平面的未被遮挡的图像
//定义要抓取的对象形状模型
//获取模型的图像
read_image (Image, '3d_machine_vision/handeye/scara_stationary_cam_setup_01_metal_parts_01')
if (RectifyImages)
　　map_image (Image, Map, ModelImage)
else
　　copy_image (Image, ModelImage)
endif
dev_close_window ()
dev_open_window_fit_image (ModelImage, , , , , WindowHandle)
set_display_font (WindowHandle, , 'mono', 'true', 'false')
dev_clear_window ()
dev_display (ModelImage)
dev_set_line_width ()
//创建形状模型
gen_rectangle1 (ModelROI, , , , )
gauss_filter (ModelImage, ImageGauss, )
reduce_domain (ImageGauss, ModelROI, ImageReduced)
create_shape_model (ImageReduced, 'auto', rad(), rad(), 'auto', 'auto', 'use_polarity', [,], 'auto', ModelID)
area_center (ModelROI, ModelROIArea, ModelROIRow, ModelROIColumn)
dev_display_shape_matching_results (ModelID, 'green', ModelROIRow, ModelROIColumn, , , , )
//指定对象的抓取点
//它要在图像中（只有当对象可以由工具以任何方向拾取时）
//或者通过用机器人抓取它并记录相应的机器人姿态
DefineGraspingPointByRobot := true
if (DefineGraspingPointByRobot)
　　dev_set_colored ()
　　GraspingPointModelInBasePose := [0.2592,0.1997,0.1224,,,1.2572,]　
　　pose_invert (CamInBasePose, BaseInCamPose)
　　pose_to_hom_mat3d (BaseInCamPose, BaseInCamHomMat3D)
　　affine_trans_point_3d (BaseInCamHomMat3D,GraspingPointModelInBasePose[], GraspingPointModelInBasePose[], GraspingPointModelInBasePose[], Qx, Qy, Qz)
　　project_3d_point (Qx, Qy, Qz, CameraParam, GraspingPointModelRow,GraspingPointModelColumn)
　　GraspingPointModelAngle := GraspingPointModelInBasePose[]
　　if (RectifyImages)
　　　　//计算校正图像坐标　　　
　　　　image_points_to_world_plane (CameraParam, MPInCamPoseMapping, GraspingPointModelRow, GraspingPointModelColumn, MappingScale, GraspingPointModelXMP, GraspingPointModelYMP)　　　　
　　　　GraspingPointModelRow := GraspingPointModelYMP - MapUpperLeftY / MappingScale　　　　
　　　　GraspingPointModelColumn := GraspingPointModelXMP - MapUpperLeftX / MappingScale　　　　
　　　　//在以校正模型图片中显示抓取点
　　　　get_image_size (ModelImage, WidthM, HeightM)
　　　　CamParamRect := [,,MappingScale,MappingScale,-MapUpperLeftX / MappingScale,-MapUpperLeftY / MappingScale,WidthM,HeightM]　　　
　　　　GraspingPointModelXMP := MapUpperLeftX + GraspingPointModelColumn * MappingScale
　　　　GraspingPointModelYMP := MapUpperLeftY + GraspingPointModelRow * MappingScale　　　　
　　　　PoseCoordSystemVis := 　　　　[GraspingPointModelXMP,GraspingPointModelYMP,,,,GraspingPointModelA　　　　ngle,]　　　　
　　　　dev_set_colored ()
　　　　disp_3d_coord_system (WindowHandle, CamParamRect,PoseCoordSystemVis, 0.02)　　
　　else
　　　　//在原始模型图像中显示抓取点
　　　　pose_invert (CamInBasePose, BaseInCamPose)
　　　　pose_compose (BaseInCamPose, GraspingPointModelInBasePose,PoseCoordSystemVis)
　　　　dev_set_colored ()
　　　　disp_3d_coord_system (WindowHandle, CameraParam,PoseCoordSystemVis, 0.02)
　　endif　　
　　disp_message (WindowHandle, 'Model contours and grasping pose', 'window', 　　, , 'black', 'true')
else
　　binary_threshold (ImageReduced, Region, 'max_separability', 'light', 　　UsedThreshold)
　　fill_up (Region, RegionFillUp)
　　erosion_rectangle1 (RegionFillUp, RegionErosion, , )
　　smallest_rectangle2 (RegionErosion, GraspingPointModelRow,GraspingPointModelColumn, Phi, Length1, Length2)
　　gen_cross_contour_xld (GraspingPointModel, GraspingPointModelRow,GraspingPointModelColumn, , 0.785398)
　　dev_set_color ('yellow')
　　dev_display (GraspingPointModel)
　　disp_message (WindowHandle, 'Model contours and grasping point', 'window',, , 'black', 'true')
endif
area_center (ModelROI, ModelROIArea, ModelROIRow, ModelROIColumn)
set_shape_model_origin (ModelID, GraspingPointModelRow - ModelROIRow, GraspingPointModelColumn - ModelROIColumn)
disp_continue_message (WindowHandle, 'black', 'true')
stop ()
pose_to_hom_mat3d (MPInCamPoseMapping, MPInCamHomMat3DMapping)
//在下列图像中找被抓取的物体
for ImageIdx :=  to  by 1
　　//下一幅图片
　　read_image (Image,'3d_machine_vision/handeye/scara_stationary_cam_setup_01_metal_parts_' +ImageIdx$'02d')　　
　　//校正图片来适应标准模型的匹配
   //寻找实例
　　if (RectifyImages)
　　　　map_image (Image, Map, SearchImage)
　　else
　　　　copy_image (Image, SearchImage)
　　endif
　　dev_clear_window ()
　　dev_display (SearchImage)
　　//寻找物体实例　　
　　find_shape_model (SearchImage, ModelID, rad(), rad(), 0.5, , 0.5, 　　'least_squares', [,], 0.9, Row, Column, Angle, Score)　　
　　if (|Row| < )
　　　　disp_message (WindowHandle, 'No objects found', 'window', , , 'black', 　　　　'true')
　　　　continue
　　endif
　　//选择一个指定的实例，这儿是最左边的　　
　　LeftmostIdx := sort_index(Column)[]
　　GraspingPointRow := Row[LeftmostIdx]　　
　　GraspingPointColumn := Column[LeftmostIdx]
　　GraspingPointAngle := Angle[LeftmostIdx]
　　//显示匹配结果和指出要抓取的物体
　　dev_display_shape_matching_results (ModelID, 'blue', Row, Column, Angle, , , )
　　dev_display_shape_matching_results (ModelID, 'green', GraspingPointRow, GraspingPointColumn, GraspingPointAngle, , , )
　　disp_message (WindowHandle, |Row| + ' objects found (Green: Object to be grasped)', 'window', , , 'black', 'true')
　　disp_continue_message (WindowHandle, 'black', 'true')
　　stop()
　　//计算需要靠近的点
　　calculate_point_to_approach_scara_stationary (GraspingPointRow, GraspingPointColumn, GraspingPointAngle + rad(GraspingPointModelAngle), RectifyImages, MapUpperLeftX, MapUpperLeftY, MappingScale, MPInCamHomMat3DMapping, CameraParam, MPInCamPose, CamInBasePose, ObjInBasePose)
　　//显示要被抓取的物体和抓取点
　　dev_clear_window ()
　　dev_display (SearchImage)
　　dev_display_shape_matching_results (ModelID, 'green', GraspingPointRow, GraspingPointColumn, GraspingPointAngle, , , )
　　dev_set_colored ()
　　if (RectifyImages)　　
　　　　get_image_size (SearchImage, Width, Height)
　　　　CamParamRect := [,,MappingScale,MappingScale,-MapUpperLeftX / MappingScale,-MapUpperLeftY / MappingScale,Width,Height]
　　　　GraspingPointXMP := MapUpperLeftX + GraspingPointColumn * MappingScale
　　　　GraspingPointYMP := MapUpperLeftY + GraspingPointRow * MappingScale
　　　　PoseCoordSystemVis := [GraspingPointXMP,GraspingPointYMP,,,,-deg(GraspingPointAngle) + GraspingPointModelAngle,]
　　　　disp_3d_coord_system (WindowHandle, CamParamRect, PoseCoordSystemVis, 0.02)
　　else
　　　　pose_invert (CamInBasePose, BaseInCamPose)
　　　　pose_compose (BaseInCamPose, ObjInBasePose, PoseCoordSystemVis)
　　　　disp_3d_coord_system (WindowHandle, CameraParam, PoseCoordSystemVis, 0.02)
　　endif
　　disp_message (WindowHandle, 'Press F5 to pick and place indicated object', 'window', , , 'black', 'true')
　　disp_message (WindowHandle, ['ObjInBasePose:','Tx: ','Ty: ','Tz: ','Alpha: ','Beta: ','Gamma: '] + ['',ObjInBasePose[:]$'.3f' + [' m',' m',' m',' deg',' deg',' deg']], 'window', , , 'black', 'true')
　　disp_continue_message (WindowHandle, 'black', 'true')
　　stop ()
　　//转换目标位置为mm
　　ToolInBasePoseMM := [ObjInBasePose[:] * ,ObjInBasePose[:]]
　　//抓取和放置物体
endfor
//这儿机器人连接被关闭
clear_shape_model (ModelID)
set_system ('border_shape_models', 'false')
dev_clear_window ()