Kawasaki （川崎）抓取样例程序

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

样例程序介绍

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

按照使用方式分为三类：

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

• 使用 Mech-Viz 规划路径

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

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

.PROGRAM vision_sample_1()
;**********************************************************
;* FUNCTION:simple pick and place with Mech-Vision
;* mechmind, 2022-5-1
;**********************************************************
  accuracy 1 always
  speed 30 always
  TOOL gripper ;set TCP
  Home ;move robot home position
  lmove camera_capture ;move to camera capture position
  break
  pos_num = 0
  ;Set ip address of IPC
  call mm_init_skt(127,0,0,1,50000)
  twait 0.1
  ;Set vision recipe
  ;call mm_switch_model(1,1)
  ;Run vision project
  call mm_start_vis(1,1,2,#start_vis) ;(x,x,3,#start_vis) used for ETH initial position
  twait 1
  call mm_get_visdata(1,pos_num,ret2)
  if ret2 <> 1100
      halt
  end
  call mm_get_pose(1,&pick[1],label[l],speed[1])
  LAPPRO pick[1],100
  LMOVE pick[1]
  break
  ;Add object grasping logic here.
  ldepart 100
  lmove waypoint[1]
  lappro drop[1],100
  lmove drop[1]
  ;Add object releasing logic here.
  ldepart 100
  HOME
.END

程序逻辑解读

• 第 8 行：定义工具中心点 TCP。工具中心点由位姿变量 gripper 定义。关于如何使用示教器定义工具中心点，请参见 设置机器人工具中心点 。

• 第 9 行：机器人移动至 Home 点，关于如何示教定义 Home 点，请参见 设置 Home 点 。

• 第 10 行：机器人直线运动到相机拍照点。拍照点位姿由位姿变量 camera_capture 定义。关于如何使用示教器定义位姿变量，请参见 设置示教位姿数据 。

• 第 14 行：调用 mm_init_skt 对通信进行初始化设置，具体参见 初始化通信指令 。

• 第 16~17 行：如需设置 Mech-Vision 工程配方，可以在此处调用 mm_switch_model 设置工程配方，具体参见 切换 Mech-Vision 配方 。

• 第 19 行：调用 mm_start_vis 触发 Mech-Vision 工程运行，具体参见 启动 Mech-Vision 工程 。

• 第 20 行：设置相机拍照后的等待时间，此处设置为 1 秒。Eye To Hand 模式下，若在 mm_start_vis 和 mm_get_visdata 之间有移动步骤（MoveL 或 MoveJ），则不需要在此设置等待；Eye In Hand 模式下则必须设置等待时间，因为相机在拍照完成之前需保持静止。

• 第 21 行：调用 mm_get_visdata 获取 Mech-Vision 识别结果，pos_num 用于保存接收到的位姿数量，ret2 用于保存该指令执行的状态码。具体参见 获取视觉目标点 。

• 第 22~24 行：判断获取识别结果的状态码是否正常（1100 为正常状态码），若状态码为异常，机器人程序将暂停。

• 第 25 行：调用 mm_get_pose 获取第 1 个视觉结果，&pick[1] 用于保存第 1个位姿数据，label[l] 用于保存第 1 个位姿数据相对应的标签数据，speed[1] 用于保存第 1 个位姿数据相对应的移动速度，具体参见 获取视觉结果 。

• 第 26 行：机器人直线移动到抓取点上方 100mm 处，用户需要修改偏移量，保证接近过程无碰撞。

• 第 27 行：机器人直线移动到抓取点。

• 第 29 行：用户需在此处添加夹具控制逻辑以抓取物体。

• 第 30 行：从抓取点沿工具 Z 轴方向向上移动 100mm ，用户需要修改偏移量，保证离开过程无碰撞。

• 第 31 行：用户可添加一个或几个中间路径点，使机器人从抓取点移动至放置点过程中无碰撞。中间路径点由位姿变量 waypoint[1] 定义。关于如何使用示教器定义位姿变量，请参见 设置示教位姿数据 。

• 第 32 行：机器人移动到放置点上方 100mm 处，用户需要修改偏移量，保证接近过程无碰撞。

• 第 33 行：机器人直线移动到固定的放置点。

• 第 34 行：用户需在此处添加夹具控制逻辑以放置物体。

• 第 35 行：从放置点沿工具 Z 轴方向向上移动 100mm ，用户需要修改偏移量，保证离开过程无碰撞。

• 第 36 行：机器人移动至 Home 点。

使用 Mech-Viz 规划路径

.PROGRAM vision_sample_2()
;**********************************************************
;* FUNCTION:simple pick and place with Mech-Viz
;* mechmind, 2022-5-1
;**********************************************************
  accuracy 1 always
  speed 30 always
  TOOL gripper ;set TCP
  Home ;move robot home position
  LMOVE camera_capture ;move to camera_capture position
  break
  pos_num = 0
  ;Set ip address of IPC
  call mm_init_skt(127,0,0,1,50000)
  twait 0.1
  ;Set vision recipe
  ;call mm_switch_model(1,1)
  ;Run Viz project
  call mm_start_viz(1,#start_viz) ;(2,#start_viz) used for ETH viz initial position
  twait 0.1
  ;set branch exitport
  ;call mm_set_branch(1,1)
  ;get planned path
  call mm_get_vizdata(2,pos_num,vispos_num,ret1)
  if ret1 <> 2100
      halt
  end
  for count=1 to pos_num
    call mm_get_pose(count,&movepoint[count],label[count],speed[count])
  end
  ;follow the planned path to pick
  for count =1 to pos_num
    speed speed[count]
    LMOVE movepoint[count]
    if count == vispos_num then
        ;add object grasping logic here
    end
  end
  ;go to drop location
  ldepart 100
  lmove waypoint[1]
  lappro drop[1],100
  lmove drop[1] ;drop point
  ;add object releasing logic here
  ldepart 100
  HOME
.END

程序逻辑解读

• 第 8 行：定义工具中心点 TCP。工具中心点由位姿变量 gripper 定义。关于如何使用示教器定义工具中心点，请参见 设置机器人工具中心点 。

• 第 9 行：机器人移动至 Home 点，关于如何示教定义 Home 点，请参见 设置 Home 点 。

• 第 10 行：机器人直线运动到相机拍照点。拍照点位姿由位姿变量 camera_capture 定义。关于如何使用示教器定义位姿变量，请参见 设置示教位姿数据 。

• 第 14 行：调用 mm_init_skt 对通信进行初始化设置，具体参见 初始化通信指令 。

• 第 16~17 行：如需设置 Mech-Vision 工程配方，可以在此处调用 mm_switch_model 设置工程配方，具体参见 切换 Mech-Vision 配方 。

• 第 19 行：调用 mm_start_viz 触发 Mech-Viz 工程运行，具体参见 启动 Mech-Viz 工程 。

• 第 21~22 行：如需设置 Mech-Viz 工程中的分支，可在此处调用 mm_set_branch 设置分支步骤的出口，具体参见 选择 Mech-Viz 分支 。

• 第 24 行：调用 mm_get_vizdata 获取 Mech-Viz 工程规划的抓取路径，pos_num 用于保存接收到的位姿数量，vispos_num 用于保存第一个视觉移动在路径中的位置编号，ret1 用于保存该指令执行的状态码。具体参见 获取 Mech-Viz 数据 。

• 第 25~27 行：判断获取路径的状态码是否正常（2100 为正常状态码），若状态码为异常，机器人程序将暂停。

• 第 28~30 行：使用循环将获取的位姿保存到变量。调用 mm_get_pose 获取视觉结果和移动路径上点的位姿数据，具体参见 获取视觉结果 。

• 第 32~38 行：使用循环使机器人沿 Mech-Viz 规划的路径移动到抓取点，用户需在 36 行添加夹具控制逻辑以抓取物体。

• 第 40 行：从抓取点沿工具 Z 轴方向向上移动 100mm ，用户需要修改偏移量，保证离开过程无碰撞。

• 第 41 行：用户可添加一个或几个中间路径点，使机器人从抓取点移动至放置点过程中无碰撞。中间路径点由位姿变量 waypoint[1] 定义。关于如何使用示教器定义位姿变量，请参见 设置示教位姿数据 。

• 第 42 行：机器人移动到放置点上方 100mm 处，用户需要修改偏移量，保证接近过程无碰撞。

• 第 43 行：机器人直线移动到固定的放置点。

• 第 44 行：用户需在此处添加夹具控制逻辑以放置物体。

• 第 45 行：从放置点沿工具 Z 轴方向向上移动 100mm ，用户需要修改偏移量，保证离开过程无碰撞。

• 第 46 行：机器人移动至 Home 点。

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

.PROGRAM vision_sample_3()
;**********************************************************
;* FUNCTION:simple pick and place with Mech-Vision path planning
;* mechmind, 2023-2-1
;**********************************************************
  accuracy 1 always
  speed 30 always
  TOOL gripper ;set TCP
  Home ;move robot home position
  JMOVE camera_capture ;move to camera_capture position
  break
  pos_num = 0
  ;Set ip address of IPC
  call mm_init_skt(127,0,0,1,50000)
  twait 0.1
  ;Set vision recipe
  ;call mm_switch_model(1,1)
  ;Run Viz project
  call mm_start_vis(1,0,1,#start_vis)
  twait 0.1
  ;get planned path
  call mm_get_vispath(1,2,pos_num,vispos_num,ret1)
  if ret1 <> 1103
      halt
  end
  for count=1 to pos_num
    call mm_get_pose(count,&movepoint[count],label[count],speed[count])
  end
  ;follow the planned path to pick
  for count =1 to pos_num
    speed speed[count]
    LMOVE movepoint[count]
    if count == vispos_num then
        ;add object grasping logic here
    end
  end
  ;go to drop location
  ldepart 100
  lmove waypoint[1]
  lappro drop[1],100
  lmove drop[1] ;drop point
  ;add object releasing logic here
  ldepart 100
  HOME
.END

程序逻辑解读

• 第 8 行：定义工具中心点 TCP。工具中心点由位姿变量 gripper 定义。关于如何使用示教器定义工具中心点，请参见 设置机器人工具中心点 。

• 第 9 行：机器人移动至 Home 点，关于如何示教定义 Home 点，请参见 设置 Home 点 。

• 第 10 行：机器人以关节运动到相机拍照点。拍照点位姿由位姿变量 camera_capture 定义。关于如何使用示教器定义位姿变量，请参见 设置示教位姿数据 。

• 第 14 行：调用 mm_init_skt 对通信进行初始化设置，具体参见 初始化通信指令 。

• 第 16~17 行：如需设置 Mech-Vision 工程配方，可以在此处调用 mm_switch_model 设置工程配方，具体参见 切换 Mech-Vision 配方 。

• 第 19 行：调用 mm_start_vis 触发 Mech-Vision 工程运行，具体参见 启动 Mech-Vision 工程 。

• 第 20 行：设置相机拍照后的等待时间，此处设置为 1 秒。Eye To Hand 模式下，若在 mm_start_vis 和 mm_get_vispath 之间有移动步骤（MoveL 或 MoveJ），则不需要在此设置等待；Eye In Hand 模式下则必须设置等待时间，因为相机在拍照完成之前需保持静止。

• 第 22 行：调用 mm_get_vispath 获取 Mech-Vision “路径规划”步骤的结果。具体参见 获取 Mech-Vision “路径规划”步骤的结果 。

• 第 23~25 行：判断获取规划路径结果的状态码是否正常（1103 为正常状态码），若状态码为异常，机器人程序将暂停。

• 第 26~28 行：使用循环将获取的路径点数据保存到指定变量中，具体参见 获取视觉结果 。

• 第 30~36 行：使用循环使机器人沿规划路径移动到抓取点。用户需在添加夹具控制逻辑以抓取物体。

• 第 38 行：从抓取点沿工具 Z 轴方向向上移动 100mm ，用户需要修改偏移量，保证离开过程无碰撞。

• 第 39 行：用户可添加一个或几个中间路径点，使机器人从抓取点移动至放置点过程中无碰撞。中间路径点由位姿变量 waypoint[1] 定义。关于如何使用示教器定义位姿变量，请参见 设置示教位姿数据 。

• 第 40 行：机器人移动到放置点上方 100mm 处，用户需要修改偏移量，保证接近过程无碰撞。

• 第 41 行：机器人直线移动到固定的放置点。

• 第 42 行：用户需在此处添加夹具控制逻辑以放置物体。

• 第 43 行：从放置点沿工具 Z 轴方向向上移动 100mm ，用户需要修改偏移量，保证离开过程无碰撞。

• 第 44 行：机器人移动至 Home 点。

运行样例程序

运行样例程序之前，需满足以下前提：

1. 已参照 Kawasaki （川崎）标准接口通信配置 完成通信配置。

2. 已参照 Kawasaki （川崎）标定操作流程 完成外参标定。

3. 已搭建 Mech-Vision 方案 及 工程 ，并 自动加载当前方案 或 自动加载当前工程 。

4. 已搭建 Mech-Viz 工程，在 Mech-Viz 工程资源面板中，右键单击工程名称，勾选 设为自动加载 。

5. Home 点、相机拍照点、机器人工具中心点、中间路径点已示教位姿数据。

满足上述前提后，请参见 此处 运行程序。