SCARA机器人手眼标定之目标抓取

SCARA机器人手眼标定之目标抓取的例子:

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

 RectifyImages := true


image.png

*使用实例图像
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)
    *当没有找到时,可以通过SCARA机械手有眼标定来获取位姿和摄像机内参
    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

if (RectifyImages)
    *获取摄像机坐标系下测量平面位姿校正后的映射关系
    prepare_rectification_map (Map, CameraParam, MPInCamPose, MappingScale, MPInCamPoseMapping)
    *将图像坐标系下圆点[0,0]坐标转换为世界坐标系下MapUpperLeftX, MapUpperLeftY坐标
    image_points_to_world_plane (CameraParam, MPInCamPoseMapping, \
    0, 0, 'm', MapUpperLeftX, MapUpperLeftY)
endif
* 
dev_update_off ()
*该参数确定使用find_shape_model查找的形状模型可以部分在图像外面,允许
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, 0, 0, 600, 600, WindowHandle)

*设置字体信息:字体大小14,字体类型:mono,粗体
set_display_font (WindowHandle, 16, 'mono', 'true', 'false')

*清除窗口内容
dev_clear_window ()

*显示变换后的图像
dev_display (ModelImage)

*设置字宽度
dev_set_line_width (2)
********创建形状模板*********************************
*产生一个标准矩形
gen_rectangle1 (ModelROI, 400, 500, 1100, 1300)

*对图像进行高斯滤波
gauss_filter (ModelImage, ImageGauss, 5)

*剪切矩形里图像
reduce_domain (ImageGauss, ModelROI, ImageReduced)

*用剪切图像创建形状模板
create_shape_model (ImageReduced, 'auto', rad(0), rad(360), 'auto',\
 'auto', 'use_polarity', [10,50], 'auto', ModelID)

*获取模板矩形区域的面积,中心坐标;
area_center (ModelROI, ModelROIArea, ModelROIRow, ModelROIColumn)

*显示模板轮廓
dev_display_shape_matching_results (ModelID, 'green', ModelROIRow, ModelROIColumn, 0, 1, 1, 0)

*通过记录相应的机器人姿势来定义物体上的抓取点
DefineGraspingPointByRobot := true

//指定对象的抓取点
//它要在图像中(只有当对象可以由工具以任何方向拾取时)
//或者通过用机器人抓取它并记录相应的机器人姿态


if (DefineGraspingPointByRobot)
    dev_set_colored (12)
    *机器人坐标系下模板抓取点位姿
    GraspingPointModelInBasePose := [0.2592,0.1997,0.1224,0,0,1.2572,0]
    
    *位姿反转,获取摄像机坐标系下机器人位姿
    pose_invert (CamInBasePose, BaseInCamPose)
    
    *将摄像机坐标系下机器人位姿转换为其次变换矩阵
    pose_to_hom_mat3d (BaseInCamPose, BaseInCamHomMat3D)
    
    *将仿射三维变换应用于点【GraspingPointModelInBasePose[0], \
    *GraspingPointModelInBasePose[1], GraspingPointModelInBasePose[2]】
    affine_trans_point_3d (BaseInCamHomMat3D, GraspingPointModelInBasePose[0], \
       GraspingPointModelInBasePose[1], GraspingPointModelInBasePose[2], Qx, Qy, Qz)
    
    *把点【Qx, Qy, Qz】投影到图像坐标系
    project_3d_point (Qx, Qy, Qz, CameraParam, GraspingPointModelRow, GraspingPointModelColumn)
    
    
    *抓取模板角度,位姿格式:[TransX; TransY ; TransZ; RotX; RotY; RotZ]
    GraspingPointModelAngle := GraspingPointModelInBasePose[5]
    
    *将图像坐标系下抓取位置点转换为世界坐标下,参数Scale可以缩放得到的三维坐标
    image_points_to_world_plane (CameraParam, MPInCamPoseMapping, GraspingPointModelRow,\
      GraspingPointModelColumn, MappingScale, GraspingPointModelXMP, GraspingPointModelYMP)
    *当前抓取坐标减去原点坐标
    GraspingPointModelRow := GraspingPointModelYMP - MapUpperLeftY / MappingScale
    GraspingPointModelColumn := GraspingPointModelXMP - MapUpperLeftX / MappingScale
        

endif

*显示模板矩形的面积和中心坐标
area_center (ModelROI, ModelROIArea, ModelROIRow, ModelROIColumn)

*设置模板原点坐标,该原点相对于形状模板重心的位置[GraspingPointModelRow - ModelROIRow,
    * GraspingPointModelColumn - 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 := 2 to 6 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(0), rad(360), 0.5, 0, 0.5,\
              'least_squares', [0,3], 0.9, Row, Column, Angle, Score)
    if (|Row| < 1)
        disp_message (WindowHandle,'No objects found', 'window', 12, 12, 'black', 'true')
        continue
    endif
    * 
    * 获取最左边工件的抓取坐标和角度
    LeftmostIdx := sort_index(Column)[0]
    GraspingPointRow := Row[LeftmostIdx]
    GraspingPointColumn := Column[LeftmostIdx]
    GraspingPointAngle := Angle[LeftmostIdx]
    * 
    * 显示匹配结果和待抓取的零件
    dev_display_shape_matching_results (ModelID, 'blue', Row, Column, Angle, 1, 1, 0)
    dev_display_shape_matching_results (ModelID, 'green', GraspingPointRow,\
         GraspingPointColumn, GraspingPointAngle, 1, 1, 0)
    disp_message (WindowHandle, |Row| + ' objects found (Green: Object to be grasped)',\
          'window', 12, 12, 'black', 'true')
    disp_continue_message (WindowHandle, 'black', 'true')
    
    * 
    * 计算出机器人坐标系下校正对象的位姿
    calculate_point_to_approach_scara_stationary (GraspingPointRow, GraspingPointColumn, \
          GraspingPointAngle + rad(GraspingPointModelAngle), RectifyImages, \
          MapUpperLeftX, MapUpperLeftY,\
         MappingScale, MPInCamHomMat3DMapping, CameraParam, MPInCamPose, \
         CamInBasePose, ObjInBasePose)
    * 

    *将摄像机坐标系下校正对象位姿中X,Y,Z偏移量转换为mm
    ToolInBasePoseMM := [ObjInBasePose[0:2] * 1000,ObjInBasePose[3:6]]

    * Pick and place the object
    *在这里,机器人应该移动到上面确定的对象的位置(ToolInBasePoseMM),在那里对象应该被拾取,
    *然后放置在某个预定位置(像PlacePositionInBasePoseMM)。
    *最后,机器人应该再次移动到备用姿势,以便拍摄测量平面的未被遮挡的图像。
    stop ()
endfor

//这儿机器人连接被关闭
*清除形状模板
clear_shape_model (ModelID)

**该参数确定使用find_shape_model查找的形状模型可以部分在图像外面,禁止
set_system ('border_shape_models', 'false')
dev_clear_window ()


本文出自勇哥的网站《少有人走的路》wwww.skcircle.com,转载请注明出处!讨论可扫码加群:

发表评论:

◎欢迎参与讨论,请在这里发表您的看法、交流您的观点。

会员中心
搜索
«    2024年3月    »
123
45678910
11121314151617
18192021222324
25262728293031
网站分类
标签列表
最新留言
    热门文章 | 热评文章 | 随机文章
文章归档
友情链接
  • 订阅本站的 RSS 2.0 新闻聚合
  • 扫描加本站机器视觉QQ群,验证答案为:halcon勇哥的机器视觉
  • 点击查阅微信群二维码
  • 扫描加勇哥的非标自动化群,验证答案:C#/C++/VB勇哥的非标自动化群
  • 扫描加站长微信:站长微信:abc496103864
  • 扫描加站长QQ:
  • 扫描赞赏本站:
  • 留言板:

Powered By Z-BlogPHP 1.7.2

Copyright Your skcircle.com Rights Reserved.

鄂ICP备18008319号


站长QQ:496103864 微信:abc496103864