サンプルプログラム10：MM_S10_Viz_Subtask
プログラム概要
機能 |
本サンプルは2つのプログラムで構成されています。サブプログラム（バックグラウンドプログラム）はMech-Vizプロジェクトの実行をトリガーし、計画された経路を取得します。一方、メインプログラム（フォアグラウンドプログラム）は計画された経路に基づいてロボットを移動させ、ロボットが把持領域から離れる際にサブプログラムをトリガーして次の経路を事前に計画し、サイクルタイムを短縮します。
|
||
ファイル場所 |
サブプログラム：Mech-VisionとMech-Vizソフトウェアのインストールディレクトリにある メインプログラム：Mech-VisionとMech-Vizソフトウェアのインストールディレクトリにある |
||
必要なプロジェクト |
Mech-VisionとMech-Vizプロジェクト |
||
使用前提 |
|
|
プログラム説明
以下は、サブプログラムのコードとその説明です。
| サブプログラム内でMech-Vizプロジェクトの実行をトリガーし、計画された経路を取得するコードは「MM_S2_Viz_Basic」サンプルと類似しています。そのため、MM_S2_Viz_Basicサンプルと同じ部分のコードについては、以下で再度説明することはありません（詳細は MM_S2_Viz_Basicサンプルの説明 をご参照ください）。 |
DEF MM_S10_Sub ( )
;---------------------------------------------------
; FUNCTION: run Mech-Viz project and get planned
; path in subtask (run together with
; MM_S10_Viz_Subtask)
; Mech-Mind, 2023-12-25
;---------------------------------------------------
$OUT[2001]=FALSE
;initialize communication parameters (initialization is required only once)
MM_Init_Socket("XML_Kuka_MMIND",873,871,60)
LOOP
IF $OUT[2001]==TRUE THEN
;trigger Mech-Viz project
MM_Start_Viz(2,init_jps_b)
;get planned path, 1st argument (1) means getting pose in JPs
MM_Get_VizData(1,pos_num_b,vis_pos_num_b,status_b)
;check whether planned path has been got from Mech-Viz successfully
IF status_b==2100 THEN
;save waypoints of the planned path to local variables one by one
MM_Get_Jps(1,pick_point_b[1],label_b[1],toolid_b[1])
MM_Get_Jps(2,pick_point_b[2],label_b[2],toolid_b[2])
MM_Get_Jps(3,pick_point_b[3],label_b[3],toolid_b[3])
ENDIF
$OUT[2001]=FALSE
ENDIF
ENDLOOP
END上記のコードは、サブプログラムが実行される際に、まず$OUT[2001]をFALSEに設定し、通信パラメータを初期化した後、LOOPループを使用して$OUT[2001]の値を監視し続けることを示しています。
-
$OUT[2001]がTRUEの場合、サブプログラムがMech-Vizプロジェクトをトリガーし、計画された経路を取得し、$OUT[2001]をFALSEに設定します。
-
$OUT[2001]がFALSEの場合、サブプログラムが$OUT[2001]の値を継続的に判断します。
以下は、メインプログラムのコードとその説明です。
| メインプログラム内で計画された経路に基づいて把持または配置を実行するコードは「MM_S2_Viz_Basic」サンプルと類似しています。そのため、MM_S2_Viz_Basicサンプルと同じ部分のコードについては、以下で再度説明することはありません（詳細は MM_S2_Viz_Basicサンプルの説明 をご参照ください）。 |
DEF MM_S10_Viz_Subtask ( )
;---------------------------------------------------
; FUNCTION: run Mech-Viz project and get planned
; path in subtask (run together with MM_S10_Sub)
; 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
;trigger Mech-Viz project and get planned path
$OUT[2001]=TRUE
LOOP
;move to wait position for picking
LIN pick_wait_point Vel=1 m/s CPDAT6 Tool[1] Base[0]
;wait until subtask program finished
WAIT FOR ($OUT[2001]==FALSE)
;check whether planned path has been got from Mech-Viz successfully
IF status_b<> 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 jps variables one by one
Xpick_point1=pick_point_b[1]
Xpick_point2=pick_point_b[2]
Xpick_point3=pick_point_b[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]
;trigger Mech-Viz project and get planned path in advance
TRIGGER WHEN DISTANCE = 1 DELAY = 0 DO $OUT[2001]=TRUE
;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
ENDLOOP
END上記のサンプルプログラムの処理流れは、下図の通りです。
下表はメインプログラムの主要なコードとその説明です。
| 処理流れ | コートと説明 | ||
|---|---|---|---|
Mech-Vizプロジェクトをトリガーし、計画された経路を取得 |
上記のコードは、メインプログラムで$OUT[2001]をTRUEに設定することを意味します。この時、サブプログラムは$OUT[2001]がTRUEであることを検出し、Mech-Vizプロジェクトを実行して、計画された経路を取得します。 |
||
ループで次の経路を事前に計画（把持→次の把持経路を計画→配置） |
上記のコードは、メインプログラムが「LOOP」と「ENDLOOP」の間のコードを繰り返し実行することを意味します。 上記のコードは、ロボットが把持する前に待機点に移動し、サブプログラムが計画された経路を取得して保存するのを待機することを示しています。つまり、$OUT[2001]がTRUEからFALSEに変わるのを待つことで、計画された経路が取得され、保存されたことを確認します。 上記のコードは、サブプログラムで取得した計画経路（pick_point_b[1]、pick_point_b[2]、pick_point_b[3]）をそれぞれXpick_point1、Xpick_point2、Xpick_point3に保存することを意味します。 上記のコードは、ロボットは計画された経路に従って、まず把持のアプローチ点（pick_point1）に移動し、把持点（pick_point2）に移動して把持を行い（例えば、$OUT[1] = TRUE）、その後、把持の離れる点（pick_point3）に移動することを意味します。
上記のコードは、ロボットが目標点（drop_waypoint、つまり配置プロセス中の中間点）に接近する際に$OUT[2001]=TRUEをトリガーすることを意味します。つまり、サブプログラムがMech-Vizプロジェクトを再実行し、計画経路を取得します。この時点でロボットはすで把持領域外にいるため、ロボットは配置が完了するのを待つことなく、次の把持経路を事前に計画することができます。
上記のコードは、ロボットが配置の中間点（drop_waypoint）、配置のアプローチ点（drop_app）、配置点（drop）の順に移動して配置動作（例えば$OUT[1]=FALSE）を行い、その後、配置の離れ点（drop_app）、Home位置の順に移動することを意味します。 |