サンプルプログラム19:MM_S19_Vis_PlanAllVision
プログラム概要
機能 |
ロボットはMech-Visionプロジェクトをトリガーして実行し、その後ループですべての計画された経路を取得して、把持および配置を実行します。このサンプルでは、カメラで1回撮影することで、Mech-Visionがすべてのビジョン結果に基づいた把持経路を計画します。通常は「1回の撮影で複数回の把持を行う」シーンで使用されます。 |
ファイル場所 |
Mech-VisionとMech-Vizソフトウェアのインストールディレクトリにある |
必要なプロジェクト |
|
使用前提 |
|
このサンプルプログラムは参考用です。ユーザーは実際の状況に応じて、このプログラムを基に変更を加える必要があります。このプログラムをそのまま使用しないでください。 |
プログラム説明
以下はMM_S19_Vis_PlanAllVisionサンプルプログラムのコードと関連する説明です。
このサンプルは、MM_S3_Vis_Pathサンプルに比べて、すべての計画された経路をループで取得し、把持および配置を行う機能(太字部分のコード)のみが追加されています。そのため、MM_S3_Vis_Pathサンプルと同じ部分のコードについては、以下で再度説明することはありません(詳細は MM_S3_Vis_Pathサンプルの説明 をご参照ください)。 |
DEF MM_S19_Vis_PlanAllVision ( )
;---------------------------------------------------
; FUNCTION: trigger Mech-Vision project, plan all
; vision results and get all planned paths
; 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)
RECAP:
;move to image-capturing position
LIN camera_capture Vel=1 m/s CPDAT1 Tool[1] Base[0]
;trigger Mech-Viz project
MM_Start_Vis(1,0,2,init_jps)
;get planned path from NO.1 Mech-Vision project; 2nd argument (1) means getting pose in JPs
MM_Get_Vispath(1,1,pos_num,vis_pos_num,status)
;check whether planned path has been got from Mech-Vision successfully
IF status<> 1103 THEN
;add error handling logic here according to different error codes
;e.g.: status=1003 means no point cloud in ROI
;e.g.: status=1002 means no vision results
halt
ENDIF
;save waypoints of the planned path to local variables one by one
FOR count=1 TO pos_num
MM_Get_Jps(count,pick_point[count],label[count],toolid[count])
ENDFOR
;parse pick cycle count, here suppose 5 points per planned path
pick_cnt = pos_num / 5
residual = pos_num - pick_cnt*5
;check if parsed data is valid; if not, retry to get planned path or add some error handling logic
IF (pick_cnt<1) OR (residual<>0) THEN
halt
GOTO RECAP
ENDIF
;repeatedly run pick-and-place cycle using for-loop
FOR i=1 TO pick_cnt
count=(i-1)*5
Xpick_point1=pick_point[1+count]
Xpick_point2=pick_point[2+count]
Xpick_point3=pick_point[3+count]
Xpick_point4=pick_point[4+count]
Xpick_point5=pick_point[5+count]
;follow the planned path to pick
;move to approach waypoint of picking
PTP pick_point1 CONT Vel=50 % PDAT1 Tool[1] Base[0]
PTP pick_point2 Vel=50 % PDAT2 Tool[1] Base[0]
PTP pick_point3 Vel=10 % PDAT3 Tool[1] Base[0]
;add object grasping logic here, such as "$OUT[1]=TRUE"
halt
PTP pick_point4 Vel=50 % PDAT5 Tool[1] Base[0]
PTP pick_point5 CONT Vel=50 % PDAT6 Tool[1] Base[0]
;move to intermediate waypoint of placing
PTP drop_waypoint CONT Vel=100 % PDAT4 Tool[1] Base[0]
;move to approach waypoint of placing
LIN drop_app Vel=1 m/s CPDAT2 Tool[1] Base[0]
;move to placing waypoint
LIN drop Vel=0.3 m/s CPDAT3 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 CPDAT2 Tool[1] Base[0]
;move to intermediate waypoint of placing
PTP drop_waypoint CONT Vel=100 % PDAT4 Tool[1] Base[0]
ENDFOR
;finish pick and-place cycle, and jump back to camera capturing
GOTO RECAP
END
上記のサンプルプログラムの処理流れは、下図の通りです。

下表は追加されたコードとその説明です。コマンド名のリンクをクリックすることで、その詳細を確認できます。
処理流れ | コートと説明 |
---|---|
ループで計画された経路を保存 |
|
pick_cntとresidualを計算 |
本サンプルでは、計画された経路には5つの経路点が含まれると仮定しています。「pos_num / 5」はpose_numを5で割った商、「pos_num - pick_cnt*5」はpose_numを5で割った余りを表します。pick_cntは計画された把持回数を表します。residualが0でない場合、ある経路計画において把持経路点の数が5未満であることを意味し、経路計画に異常が発生したため再計画が必要です。 |
経路計画が異常かどうかを判断 |
把持回数pick_cntが1未満である場合、またはresidualが0でない場合、経路計画に異常が発生していることを示します。この場合、処理ロジックを追加する必要があります。例えば、Mech-Visionプロジェクトを再度トリガーして実行し、計画された経路を再取得するなどの対応が必要です。 |
ループで把持および配置を実行 |
上記のコードは、FORループ内でロボットが各計画された経路における5つの経路点に移動し、把持を完了した後に配置を続けて実行することを意味します。iはループ回数を制御するために使用されます。iは1から始まり、各ループ後に1ずつ増加します。iが把持回数pick_cntを超えるとループは終了します。iが1増加すると、countが5増加します。そうすることで、[1+count]~[5+count]が計画された5つの経路点が経路全体における番号を表すようになります。 |