サンプルプログラム
本節では標準インターフェースを用いた把持と配置のサンプルプログラムとその操作手順について紹介します。
サンプルプログラムの紹介
簡単な把持・配置のサンプルプログラムは、Mech-Mindソフトウェアシステムのインストールディレクトリ Mech-Center/Robot_Interface/KAWASAKI/sample
フォルダに格納されています。
以下のサンプルプログラムを紹介します。
Mech-Visionからビジョン結果を取得
.PROGRAM vision_sample_1()
;**********************************************************
;* FUNCTION:simple pick and place with vis
;* 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を設定します。TCPは位置姿勢変量gripperによって定義されます。ティーチングペンダントでTCPを設定する方法については、 ロボットのTCPを設定 をご参照ください。
-
第9行目:ロボットを初期位置に移動します。ティーチングペンダントで初期位置を設定する方法については、 初期位置を設定 をご参照ください。
-
第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-In-Handモードでは、撮像が完了するまでロボットが完全に停止している必要があるため待機時間の設定が必要です。Eye To Handモードでは、mm_start_visとmm_get_visdataの間に移動(MoveLまたはMoveJ)がある場合、待機時間の設定は必要ありません。
-
第21行目:mm_get_visdataを呼び出し、Mech-Visionの認識結果を取得します。pos_numは受信した位置姿勢の数を保存するために使用され、ret2は本コマンドを実行するステータスコードを保存するために使用されます。詳細については、 ビジョン目標点の取得 をご参照ください。
-
第22~24行目:返されたステータスコードを確認します(1100は正常実行を表すステータスコード)。エラーコードが返された場合、プログラムは一時停止します。
-
第26行目:ロボットが直線運動で把持位置上部100㎜のところまで移動した後、接近する際に衝突がないように、オフセットを修正する必要があります。
-
第27行目:ロボットが直線運動で把持位置まで移動させます。
-
第29行目:ワークを把持するために、ここでロボットハンドの制御ロジックを追加する必要があります。
-
第30行目:把持位置からツールZ軸方向の上に沿って100㎜移動し、離れる際に衝突がないように、オフセットを修正する必要があります。
-
第32行目:ロボットが直線運動で把持位置上部100㎜のところまで移動した後、接近する際に衝突が起こさないよう、オフセットを修正する必要があります。
-
第33行目:ロボットが直線運動で固定された配置位置まで移動させます。
-
第34行目:ワークを配置するために、ここでハンドの制御ロジックを追加する必要があります。
-
第35行目:配置位置からツールZ軸方向の上に沿って100㎜移動し、離れる際に衝突がないよう、オフセットを修正する必要があります。
-
第36行目:ロボットを初期位置に移動させます。
Mech-Vizで動作経路を計画
.PROGRAM vision_sample_2()
;**********************************************************
;* FUNCTION:simple pick and place with viz
;* 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を設定します。TCPは位置姿勢変量gripperによって定義されます。ティーチングペンダントでTCPを設定する方法については、 ロボットのTCPを設定 をご参照ください。
-
第9行目:ロボットを初期位置に移動します。ティーチングペンダントで初期位置を設定する方法については、 初期位置を設定 をご参照ください。
-
第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軸方向の上に沿って100㎜移動します。離れる際に衝突が起こさないよう、オフセットを修正する必要があります。
-
第42行目:ロボットを配置位置上部100㎜まで移動させ、接近する際に衝突が起こさないように、オフセットを修正する必要があります。
-
第43行目:ロボットを直線運動で固定された配置位置に移動させます。
-
第44行目:ワークを配置するために、ここでロボットハンドの制御ロジックを追加する必要があります。
-
第45行目:配置位置からZ軸方向の上に沿って100mm移動し、離れる際に衝突が起こさないように、オフセットを修正する必要があります。
-
第46行目:ロボットを初期位置に移動させます。
Mech-Visionの「経路計画」ステップを使用して経路を取得
.PROGRAM vision_sample_3()
;**********************************************************
;* FUNCTION:simple pick and place with vis path planning
;* 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を設定します。TCPは位置姿勢変量gripperによって定義されます。ティーチングペンダントでTCPを設定する方法については、 ロボットのTCPを設定 をご参照ください。
-
第9行目:ロボットを初期位置に移動します。ティーチングペンダントで初期位置を設定する方法については、 初期位置を設定 をご参照ください。
-
第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-In-Handモードでは、撮像が完了するまでロボットが完全に停止している必要があるため待機時間の設定が必要です。Eye To Handモードでは、mm_start_visとmm_get_vispathの間に移動(MoveLまたはMoveJ)がある場合、待機時間の設定は必要ありません。
-
第22行目:mm_get_vispathを呼び出し、Mech-Visionの「経路計画」ステップの結果を取得します。詳細については、 Mech-Visionの「経路計画」ステップの結果を取得 をご参照ください。
-
第23~25行目:計画した経路を取得するためのステータスコードを確認します(1103は正常実行を表すステータスコード)。エラーコードが返された場合、プログラムは一時停止します。
-
第26~28行目:ループで取得した経路点を指定した変数に保存します。詳細については、 ビジョン結果を取得 をご参照ください。
-
第30~36行目:ループでロボットを計画された経路を把持位置まで移動させます。ワークを把持するために、ロボットハンドの制御ロジックを追加する必要があります。
-
第38行目:把持位置からツールZ軸方向の上に沿って100㎜移動し、離れる際に衝突がないように、オフセットを修正する必要があります。
-
第40行目:ロボットを配置位置上部100㎜まで移動させ、接近する際に衝突が起こさないように、オフセットを修正する必要があります。
-
第41行目:ロボットを直線運動で固定された配置位置に移動させます。
-
第42行目:ワークを配置するために、ここでロボットハンドの制御ロジックを追加する必要があります。
-
第43行目:配置位置からZ軸方向の上に沿って100mm移動し、離れる際に衝突が起こさないように、オフセットを修正する必要があります。
-
第44行目:ロボットを初期位置に移動させます。
サンプルプログラムを実行
サンプルプログラムを実行する前に、次の前提条件を満たす必要があります。
-
標準インターフェースの通信設定 を参照して通信設定を行います。
-
キャリブレーションの操作手順 を参照し、コントローラにインストールされたキャリブレーションプログラムを実行するか、手動でロボットを移動させてキャリブレーションポイントを追加します。この手順によって外部パラメータのキャリブレーションが行われます。
-
Mech-Visionのソリューションとプロジェクトを構築し、ソリューションまたはプロジェクトの自動読み込みが設定されます。
-
Mech-Vizプロジェクトを構築し、Mech-Vizプロジェクトリソースパネルでプロジェクト名を右クリックし、 自動的に読み込む にチェックを入れます。
-
ロボットを初期位置、写真撮影位置、ロボットTCP、経路点に移動させ、ティーチペンダントでそれらの位置でのロボットの位置姿勢が対応する変数に記録されます。
上記の前提条件を満たしたら、 こちら を参照してプログラムを実行します。