サンプルプログラム2：MM_S2_Viz_Basic
プログラム概要
機能 |
ロボットがMech-Vizプロジェクトをトリガーして実行し、その後計画された経路を取得して、把持および配置を実行します。 |
ファイル場所 |
Mech-VisionとMech-Vizソフトウェアのインストールディレクトリにある |
必要なプロジェクト |
Mech-VisionとMech-Vizプロジェクト |
使用前提 |
|
| このサンプルプログラムは参考用です。ユーザーは実際の状況に応じて、このプログラムを基に変更を加える必要があります。このプログラムをそのまま使用しないでください。 |
プログラム説明
以下はMM_S2_Viz_Basicサンプルプログラムのコードと関連する説明です。
DEF MM_S2_Viz_Basic ( )
;---------------------------------------------------
; FUNCTION: trigger Mech-Viz project and get
; planned path
; Mech-Mind, 2023-12-25
;---------------------------------------------------
;set current tool no. to 1
BAS(#TOOL,1)
;set current base no. to 0
BAS(#BASE,0)
;move to robot home position
PTP HOME Vel=100 % DEFAULT
;initialize communication parameters (initialization is required only once)
MM_Init_Socket("XML_Kuka_MMIND",873,871,60)
;move to image-capturing position
LIN camera_capture Vel=1 m/s CPDAT1 Tool[1] Base[0]
;trigger Mech-Viz project
MM_Start_Viz(2,init_jps)
;get planned path, 1st argument (1) means getting pose in JPs
MM_Get_VizData(1,pos_num,vis_pos_num,status)
;check whether planned path has been got from Mech-Viz successfully
IF status<> 2100 THEN
;add error handling logic here according to different error codes
;e.g.: status=2038 means no point cloud in ROI
halt
ENDIF
;save waypoints of the planned path to local variables one by one
MM_Get_Jps(1,Xpick_point1,label[1],toolid[1])
MM_Get_Jps(2,Xpick_point2,label[2],toolid[2])
MM_Get_Jps(3,Xpick_point3,label[3],toolid[3])
;follow the planned path to pick
;move to approach waypoint of picking
PTP pick_point1 Vel=50 % PDAT1 Tool[1] Base[0]
;move to picking waypoint
PTP pick_point2 Vel=10 % PDAT2 Tool[1] Base[0]
;add object grasping logic here, such as "$OUT[1]=TRUE"
halt
;move to departure waypoint of picking
PTP pick_point3 Vel=50 % PDAT3 Tool[1] Base[0]
;move to intermediate waypoint of placing
PTP drop_waypoint CONT Vel=100 % PDAT2 Tool[1] Base[0]
;move to approach waypoint of placing
LIN drop_app Vel=1 m/s CPDAT3 Tool[1] Base[0]
;move to placing waypoint
LIN drop Vel=0.3 m/s CPDAT4 Tool[1] Base[0]
;add object releasing logic here, such as "$OUT[1]=FALSE"
halt
;move to departure waypoint of placing
LIN drop_app Vel=1 m/s CPDAT3 Tool[1] Base[0]
;move back to robot home position
PTP HOME Vel=100 % DEFAULT
END上記のサンプルプログラムの処理流れは、下図の通りです。
下表は上記のプログラムのコードとその説明です。コマンド名のリンクをクリックすることで、その詳細を確認できます。
| 処理流れ | コートと説明 | ||
|---|---|---|---|
座標系設定 |
上記の2つのコマンドは、現在選択されているツール座標系とベース座標系を設定することを意味します。 |
||
Home位置に移動 |
このコマンドは、ロボットがポイントツーポイント方式でティーチングされたHome位置へ移動することを意味します。 |
||
通信初期化 |
ロボットは MM_Init_Socket コマンドを使用して、XML_Kuka_MMIND.xmlファイルの設定情報に基づき、ビジョンシステムとTCPプロトコル通信接続を確立します。
|
||
撮影位置に移動 |
このコマンドは、ロボットが直線移動の方式でティーチングされた撮影位置に移動することを意味します。 |
||
Mech-Vizプロジェクトをトリガー |
このコマンドは、ロボットがビジョンシステムをトリガーしてMech-Vizプロジェクトを実行し、Mech-VizがMech-Visionから出力されたビジョン結果に基づいて把持経路を計画することを意味します。 |
||
計画された経路を取得 |
このコマンドは、ロボットがMech-Vizプロジェクトから返された計画経路を取得することを意味します。
上記のコードは、ステータスコードstatusが2100の場合、ロボットがすべての計画された経路を正常に取得することを意味し、それ以外の場合はビジョンシステムに異常が発生し、プログラムはIFとENDIFの間のコードを実行することを意味します。ユーザーは、特定のエラーコードに応じて適切な処理を行うことが可能です。本サンプルでは、すべてのエラーコードに対して同一の処理を行い、haltコマンドを使用してプログラムの実行を一時停止しています。 |
||
計画された経路を保存 |
「MM_Get_Jps(1,Xpick_point1,label[1],toolid[1]」コマンドは、最初の経路点の関節角度、ラベル、およびロボットハンド番号をそれぞれ指定された変数に保存することを意味します。
|
||
把持のアプローチ点に移動 |
ロボットは把持のアプローチ点に移動します。ここでのpick_point1は、前述のXpick_point1と同じ位置を指します。 |
||
把持点に移動 |
ロボットは把持点に移動します。ここでのpick_point2は、前述のXpick_point2と同じ位置を指します。 |
||
DO設定による把持 |
ロボットが把持点に到達した後、DOコマンド（例えば「$OUT[1]=TRUE」）を設定して、ロボットハンドを制御し、ワークを把持します。実際の状況に応じてDO設定を調整する必要があります。
|
||
把持の離れる点に移動 |
ロボットは把持の離れる点に移動します。ここでのpick_point3は、前述のXpick_point3と同じ位置を指します。 |
||
中間点に移動 |
ロボットは把持の離れる点と配置のアプローチ点の間の中間点（drop_waypoint）に移動します。
|
||
配置のアプローチ点に移動 |
ロボットが中間点から配置のアプローチ点（drop_app）に移動します。
|
||
配置点に移動 |
ロボットが配置のアプローチ点から配置点（drop）に移動します。
|
||
DO設定による配置 |
ロボットが配置点に到達した後、DOコマンド（例えば「$OUT[1]=FALSE」）を設定して、ロボットハンドを制御し、ワークを配置します。実際の状況に応じてDO設定を調整する必要があります。
|
||
配置の離れる点に移動 |
ロボットが配置点から配置の離れる点（drop_app）に移動します。
|
||
Home位置に移動 |
ロボットが配置の離れる点から初期位置に戻ります。
|