KUKA （库卡）抓取样例程序

本章将介绍用于完成简单识别并移动的机器人样例程序。

前提

  1. 已参照 KUKA （库卡）标准接口通信配置 完成标准接口通信配置。

  2. 已参照 KUKA （库卡）标定操作流程 完成外参标定。

  3. Mech-Vision 和 Mech-Viz 工程已搭建，并已勾选 自动加载工程 。

  4. 机器人工具中心点已定义好。

  5. 调低机器人运行速度，以避免调试过程中发生碰撞。

样例程序介绍

机器人简单抓取和放置的样例程序位于梅卡曼德系统软件安装目录下 Mech-Center/Robot_Interface/KUKA/sample 文件夹内。

按照使用方式分为三类：

使用 Mech-Vision 进行视觉识别并获取结果

&ACCESS RVO
&REL 5
DEF MM_SAMPLE01 ( )
  ;FUNCTION:Eye to Hand simple pick and place with Mech-Vision
  ;mechmind, 2022-5-31
  INT Job
  INT Pos_Num
  INT Last_Data
  INT MM_Status
  E6POS MM_pick
  E6POS MM_waypoint
  E6POS MM_camera_capture
  E6POS MM_drop
  INT MM_Label
  INT MM_Speed
  ;FOLD PTP HOME Vel=100 % DEFAULT;%{PE}%R 8.3.44,%MKUKATPBASIS,%CMOVE,%VPTP,%P 1:PTP, 2:HOME, 3:, 5:100, 7:DEFAULT
$BWDSTART=FALSE
PDAT_ACT=PDEFAULT
FDAT_ACT=FHOME
BAS(#PTP_PARAMS,100)
$H_POS=XHOME
PTP XHOME
  ;ENDFOLD

  BAS(#TOOL,1)
  LIN MM_camera_capture
  ;Set ip address of IPC
  MM_Init_Socket("XML_Kuka_MMIND",873,871,60)
  wait sec 0.1
  ;Set vision recipe
  MM_Switch_Model(1,1)
  ;Run vision project
  MM_Start_Vis(1,1,2,snap_jps)
  wait sec 1
  MM_Get_VisData(1,Last_Data,Pos_Num,MM_Status)
  IF MM_Status<> 1100 THEN
    halt
  ENDIF
  MM_Get_Pose(1,MM_pick,MM_Label,MM_Speed )
MM_pick.z=MM_pick.z+100
LIN MM_pick
MM_pick.z=MM_pick.z-100
  LIN MM_pick
  ;Add object grasping logic here.
  LIN_REL {z -100}#TOOL
 LIN MM_waypoint
MM_drop.z=MM_drop.z+100
LIN MM_drop
MM_drop.z=MM_drop.z-100
  LIN MM_drop
  ;Add object releasing logic here.
  LIN_REL {z -100}#TOOL
    ;FOLD PTP HOME Vel=100 % DEFAULT;%{PE}%R 8.3.44,%MKUKATPBASIS,%CMOVE,%VPTP,%P 1:PTP, 2:HOME, 3:, 5:100, 7:DEFAULT
$BWDSTART=FALSE
PDAT_ACT=PDEFAULT
FDAT_ACT=FHOME
BAS(#PTP_PARAMS,100)
$H_POS=XHOME
PTP XHOME
  ;ENDFOLD
END

程序逻辑解读

  1. 移动机器人至 Home 点。

  2. 移动到相机拍照位置。

  3. 调用 MM_Init_Socket 对通信进行初始化设置。

  4. 如需设置 Mech-Vision工程配方，可以在这里添加 MM_Switch_Model 设置 Mech-Vision 工程配方。

  5. MM_Start_Vis 触发 Mech-Vision 工程运行。

  6. 等待 1 秒。Eye To Hand模式下，若在 MM_Start_Vis 和 MM_Get_VisData 之间有移动步骤，则不需要在此设置等待。Eye In Hand 模式下则必须等待 1 秒，在相机拍照完成之前保持相机静止。

  7. 获取 Mech-Vision 工程计算出的位姿。

  8. 判断获取位姿的状态码是否正常，若状态码返回异常，用户需要进行处理，例如暂停机器人。

  9. 移动到抓取点。

  10. 移动到抓取点和放置点之间的安全区域。

  11. 移动到固定的放置点。

用户自定义部分

  • 定义工具中心点 TCP

    BAS(#TOOL,1)，设置机器人TCP，使用工具坐标系1。用户需将调用坐标系改为存储了实际 TCP 值的坐标系。

  • 示教拍照点

    LIN MM_camera_capture，用户需将拍照点位置记录在 MM_camera_capture 位姿变量中。

  • 示教过渡点

    LIN MM_waypoint，用户可添加一个或几个中间点，使机器人从抓取点移动至放置点过程中，无碰撞。

  • 示教放置点

    LIN MM_drop，用户需将放置位置记录在 MM_drop 位姿变量中。

  • 定义抓取前后，放置前后的接近距离

    • 接近抓取点

      MM_pick.z=MM_pick.z+100
      LIN MM_pick
      移动到抓取点上方 100mm 处，用户需要修改偏移量，保证接近过程无碰撞。

    • 离开抓取点

      LIN_REL {z -100}#TOOL
      从抓取点沿工具坐标系 Z 轴负方向移动 100mm，用户需要修改偏移量，保证离开过程无碰撞。

    • 接近放置点

      MM_drop.z=MM_drop.z+100
      LIN MM_drop
      移动到放置点上方100mm处，用户需要修改偏移量，保证接近过程无碰撞。

    • 离开放置点

      LIN_REL {z -100}#TOOL
      从放置点沿工具坐标系 Z 轴负方向移动 100mm，用户需要修改偏移量，保证离开过程无碰撞

    • 定义夹具动作

      在移动到抓取点和放置点时，需添加夹具控制逻辑用来开关夹具。

  • 定义机器人 Home 点

    机器人 Home 点需要用户预先设定。

使用 Mech-Viz 规划路径

&ACCESS RVO
&REL 2
DEF MM_SAMPLE02 ( )
INT Job
  INT Pos_Num
  INT VisPos_Num
  INT Last_Data
  INT MM_Status
  DECL E6POS MM_movepoint[20]
  E6POS MM_waypoint
  E6POS MM_camera_capture
  E6POS MM_drop
  DECL INT MM_Label[20]
  INT MM_Speed[20]
  INT count
  ;FOLD PTP HOME Vel=100 % DEFAULT;%{PE}%R 8.3.44,%MKUKATPBASIS,%CMOVE,%VPTP,%P 1:PTP, 2:HOME, 3:, 5:100, 7:DEFAULT
$BWDSTART=FALSE
PDAT_ACT=PDEFAULT
FDAT_ACT=FHOME
BAS(#PTP_PARAMS,100)
$H_POS=XHOME
PTP XHOME
  ;ENDFOLD

  BAS(#TOOL,1)
  LIN MM_camera_capture
  ;Set ip address of IPC
  MM_Init_Socket("XML_Kuka_MMIND",873,871,60)
  wait sec 0.1
  ;Set vision recipe
  ;MM_Switch_Model(1,1)
  ;Run Viz project
  MM_Start_Viz(1,snap_jps)
  wait sec 0.1
  ;set branch exitport
  ;MM_Set_Branch(1,1)
  ;get planned path
  MM_Get_VizData(2,Last_Data,Pos_Num,VisPos_Num,MM_Status)
  IF MM_Status<> 2100 THEN
    halt
  ENDIF
  FOR count=1 TO Pos_Num
  MM_Get_Pose(count,MM_movepoint[count],MM_Label[count],MM_Speed[count])
  ENDFOR
  ;follow the planned path to pick
  FOR count=1 TO Pos_Num
  LIN MM_movepoint[count]
  IF count==VisPos_Num THEN
   ;add object grasping logic here
  ENDIF
  ENDFOR
  ;go to drop location
  LIN MM_waypoint
MM_drop.z=MM_drop.z+100
  LIN MM_drop
MM_drop.z=MM_drop.z-100
  LIN MM_drop
  ;Add object releasing logic here.
  LIN_REL  {z -100}#TOOL
    ;FOLD PTP HOME Vel=100 % DEFAULT;%{PE}%R 8.3.44,%MKUKATPBASIS,%CMOVE,%VPTP,%P 1:PTP, 2:HOME, 3:, 5:100, 7:DEFAULT
$BWDSTART=FALSE
PDAT_ACT=PDEFAULT
FDAT_ACT=FHOME
BAS(#PTP_PARAMS,100)
$H_POS=XHOME
PTP XHOME
  ;ENDFOLD
END

程序逻辑解读

此样例使用 Mech-Viz 进行视觉引导抓取，然后放置到固定的放置点。

  1. 移动机器人至 Home 点。

  2. 移动到视觉拍照位置。

  3. 调用 MM_Init_Socket 对通信进行初始化设置。

  4. 如需设置 Mech-Vision 工程配方，可以在这里调用 MM_Switch_Model 设置 Mech-Vision 工程使用配方。

  5. 触发 Mech-Viz 工程运行。

  6. 获取 Mech-Viz 工程规划的移动路径。

  7. 判断状态码是否正常，若状态码返回异常，用户需要进行处理，例如暂停机器人。

  8. 使用循环将获取的位姿保存到变量。

  9. 使用循环使机器人沿Mech-Viz规划路径移动，完成视觉抓取并离开视觉区域。

  10. 移动到抓取点和放置点之间的中间区域。

  11. 移动到固定放置点进行放置。

用户自定义部分

  • 定义工具中心点 TCP

    BAS(#TOOL,1)，设置机器人 TCP，使用工具坐标系1。用户需将调用坐标系改为存储了实际 TCP 值的坐标系。

  • 示教拍照点

    LIN MM_camera_capture，用户需将拍照点位置记录在 MM_camera_capture 位姿变量中。

  • 示教过渡点

    LIN MM_waypoint，用户可添加一个或几个中间点，使机器人从抓取点移动至放置点过程中，无碰撞。

  • 示教放置点

    LIN MM_drop，用户需将放置位置记录在 MM_drop 位姿变量中。

  • 定义夹具动作

    在移动到抓取点和放置点时，需添加夹具控制逻辑用来开关夹具。

  • 定义机器人 Home 点

    机器人 Home 点需要用户预先设置。

使用 Mech-Vision “路径规划”步骤获取规划路径

&A&ACCESS RVO
&REL 2
DEF MM_SAMPLE03 ( )
  ;FUNCTION:Eye to Hand simple pick and place with Mech-Vision Path Planning Step
  ;mechmind, 2023-1-10
  INT Pos_Num
  INT VisPos_Num
  INT Last_Data
  INT MM_Status
  DECL E6POS MM_movepoint[20]
  E6POS MM_waypoint
  E6POS MM_camera_capture
  E6POS MM_drop
  INT MM_Label[20]
  INT MM_Speed[20]
  INT count
  ;FOLD PTP HOME Vel=100 % DEFAULT;%{PE}%R 8.3.44,%MKUKATPBASIS,%CMOVE,%VPTP,%P 1:PTP, 2:HOME, 3:, 5:100, 7:DEFAULT
$BWDSTART=FALSE
PDAT_ACT=PDEFAULT
FDAT_ACT=FHOME
BAS(#PTP_PARAMS,100)
$H_POS=XHOME
PTP XHOME
  ;ENDFOLD

  BAS(#TOOL,1)
  LIN MM_camera_capture
  ;Set ip address of IPC
  MM_Init_Socket("XML_Kuka_MMIND",873,871,60)
  wait sec 0.1
  ;Set vision recipe
  MM_Switch_Model(1,1)
  ;Run vision project
  MM_Start_Vis(1,1,2,snap_jps)
  wait sec 1
  ;get planned path
  MM_Get_VisPath(1,2,Last_Data,Pos_Num,VisPos_Num,MM_Status)
  IF MM_Status<> 1103 THEN
    halt
  ENDIF
  FOR count=1 TO Pos_Num
  MM_Get_Pose(count,MM_movepoint[count],MM_Label[count],MM_Speed[count])
  ENDFOR
  ;follow the planned path to pick
  FOR count=1 TO Pos_Num
  LIN MM_movepoint[count]
  IF count==VisPos_Num THEN
   ;add object grasping logic here
  ENDIF
  ENDFOR
  ;go to drop location
  LIN MM_waypoint
MM_drop.z=MM_drop.z+100
  LIN MM_drop
MM_drop.z=MM_drop.z-100
  LIN MM_drop
  ;Add object releasing logic here.
  LIN_REL  {z -100}#TOOL
    ;FOLD PTP HOME Vel=100 % DEFAULT;%{PE}%R 8.3.44,%MKUKATPBASIS,%CMOVE,%VPTP,%P 1:PTP, 2:HOME, 3:, 5:100, 7:DEFAULT
$BWDSTART=FALSE
PDAT_ACT=PDEFAULT
FDAT_ACT=FHOME
BAS(#PTP_PARAMS,100)
$H_POS=XHOME
PTP XHOME
  ;ENDFOLD
END

程序逻辑解读

此样例使用 Mech-Vision “路径规划”步骤获取规划路径，从而进行抓取，然后放置到固定的放置点。

  1. 移动机器人至 HOME 点。

  2. 移动到视觉拍照位置。

  3. 调用 MM_Init_Socket 对通信进行初始化设置。

  4. 如需设置 Mech-Vision 工程配方，可以调用 MM_Switch_Model 设置 Mech-Vision 工程所使用的配方。

  5. MM_Start_Vis 触发 Mech-Vision 工程运行。

  6. MM_Get_VisPath 获取 Mech-Vision “路径规划”步骤输出的移动路径。

  7. 判断状态码是否正常，若状态码返回异常，机器人程序将暂停，用户需要进行处理。

  8. 根据规划的路径点数量，使用循环将获取的路径点存放至变量中。

  9. 使用循环使机器人沿规划路径移动，完成视觉抓取并离开视觉区域。

  10. 移动到抓取点和放置点之间的中间区域。

  11. 移动到固定放置点进行放置。

用户自定义部分

  • 定义工具中心点 TCP

    BAS(#TOOL,1)，设置机器人 TCP，使用工具坐标系1。用户需将调用坐标系改为存储了实际 TCP 值的坐标系。

  • 示教拍照点

    LIN MM_camera_capture，用户需将拍照点位置记录在 MM_camera_capture 位姿变量中。

  • 示教过渡点

    LIN MM_waypoint，用户可添加一个或几个中间点，使机器人从抓取点移动至放置点过程中，无碰撞。

  • 示教放置点

    LIN MM_drop，用户需将放置位置记录在 MM_drop 位姿变量中。

  • 定义夹具动作

    在移动到抓取点和放置点时，需添加夹具控制逻辑用来开关夹具。

  • 定义机器人 Home 点

    机器人 Home 点需要用户预先设置。