サンプルプログラム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サンプルの説明 をご参照ください）。 |
1: !-------------------------------- ;
2: !FUNCTION: run Mech-Viz project ;
3: !and get planned path in subtask ;
4: !(run together with ;
5: !MM_S10_Viz_Subtask) ;
6: !Mech-Mind, 2023-12-25 ;
7: !-------------------------------- ;
8: ;
9: F[11]=(ON) ;
10: !trigger Mech-Viz project ;
11: CALL MM_START_VIZ(2,10) ;
12: !get planned path, 1st argument ;
13: !(1) means getting pose in JPs ;
14: CALL MM_GET_VIZ(1,51,52,53) ;
15: !check whether planned path has ;
16: !been got from Mech-Viz ;
17: !successfully ;
18: IF (R[53]=2100) THEN ;
19: !save waypoints of the planned ;
20: !path to local variables one ;
21: !by one ;
22: CALL MM_GET_JPS(1,60,70,80) ;
23: CALL MM_GET_JPS(2,61,71,81) ;
24: CALL MM_GET_JPS(3,62,72,82) ;
25: ENDIF ;
26: F[11]=(OFF) ;上記のコードは、サブタスクが実行時に以下の処理を行うことを示します。
-
F[11]をONに設定します。
-
Mech-Vizプロジェクトをトリガーして実行します。
-
計画された経路を取得します。
-
計画された経路を保存します。
-
F[11]をOFFに設定します。
これにより、F[11]の値に基づいてサブタスクの実行が完了したかどうかを判断できます。
以下は、メインタスクのコードとその説明です。
| メインタスク内で計画された経路に基づいて把持または配置を実行するコードは「MM_S2_Viz_Basic」サンプルと類似しています。そのため、MM_S2_Viz_Basicサンプルと同じ部分のコードについては、以下で再度説明することはありません（詳細は MM_S2_Viz_Basicサンプルの説明 をご参照ください）。 |
1: !-------------------------------- ;
2: !FUNCTION: run Mech-Viz project ;
3: !and get planned path in subtask ;
4: !(run together with ;
5: !MM_S10_Sub) ;
6: !Mech-Mind, 2023-12-25 ;
7: !-------------------------------- ;
8: ;
9: !set current uframe NO. to 0 ;
10: UFRAME_NUM=0 ;
11: !set current tool NO. to 1 ;
12: UTOOL_NUM=1 ;
13: !initialize communication ;
14: !parameters(initialization is ;
15: !required only once) ;
16: CALL MM_INIT_SKT('8','127.0.0.1',50000,5) ;
17: !move to robot home position ;
18:J P[1] 100% FINE ;
19: RUN MM_S10_SUB ;
20: LBL[1:LOOP] ;
21: !move to wait position for ;
22: !picking ;
23:L P[2] 1000mm/sec FINE ;
24: !wait until subtask program ;
25: !finished ;
26: WAIT (F[11]=OFF) ;
27: !check whether planned path has ;
28: !been got from Mech-Viz ;
29: !successfully ;
30: IF R[53]<>2100,JMP LBL[99] ;
31: !follow the planned path to pick ;
32: !move to approach waypoint ;
33: !of picking ;
34:J PR[60] 50% FINE ;
35: !move to picking waypoint ;
36:J PR[61] 10% FINE ;
37: !add object grasping logic here, ;
38: !such as "DO[1]=ON" ;
39: PAUSE ;
40: !move to departure waypoint ;
41: !of picking ;
42:J PR[62] 50% FINE ;
43: !move to intermediate waypoint ;
44: !of placing, and trigger Mech-Viz ;
45: !project and get planned path in ;
46: !advance ;
47:J P[3] 50% CNT100 DB 10.0mm,CALL MM_S10_SUB ;
48: !move to approach waypoint ;
49: !of placing ;
50:L P[4] 1000mm/sec FINE Tool_Offset,PR[2] ;
51: !move to placing waypoint ;
52:L P[4] 300mm/sec FINE ;
53: !add object releasing logic here, ;
54: !such as "DO[1]=OFF" ;
55: PAUSE ;
56: !move to departure waypoint ;
57: !of placing ;
58:L P[4] 1000mm/sec FINE Tool_Offset,PR[2] ;
59: !move back to robot home position ;
60:J P[1] 100% FINE ;
61: JMP LBL[1] ;
62: END ;
63: ;
64: LBL[99:vision error] ;
65: !add error handling logic here ;
66: !according to different ;
67: !error codes ;
68: !e.g.: status=2038 means no ;
69: !point cloud in ROI ;
70: PAUSE ;上記のサンプルプログラムの処理流れは、下図の通りです。
下表は主要なコードとその説明です。
| 処理流れ | コートと説明 | ||
|---|---|---|---|
Mech-Vizプロジェクトをトリガーし、計画された経路を取得 |
上記のコートは、メインタスクがMM_S10_SUBサブタスクを呼び出し、これによってMech-Vizプロジェクトをトリガーし、計画された経路を取得することを意味します。 |
||
ループで次の経路を事前に計画（把持→次の把持経路を計画→配置） |
上記のコードは、メインタスクがLBL[1]で示すコードを繰り返し実行することを意味します。 上記のコードは、ロボットが把持する前に待機点に移動し、サブタスクの実行が終了するのを待つことを意味します。つまり、F[11]がONからOFFに変わるのを待つことで、計画された経路が取得され、保存されたことを確認します。 上記のコードは、ロボットが計画された経路に従って把持のアプローチ点（PR[60]）に移動し、次に把持点（PR[61]）に移動して把持を行い（例えばDO[1]=ON）、その後、把持の離れる点（PR[62]）に移動することを意味します。
上記のコードは、ロボットがP[3]を中心とした球形範囲に到達した際に、MM_S10_SUBサブプログラムを呼び出すことを意味します。これにより、Mech-Vizプロジェクトが再度実行され、次の計画経路が取得されます。この時点でロボットはすで把持領域外にいるため、ロボットは配置が完了するのを待つことなく、次の把持経路を事前に計画することができます。
上記のコードは、ロボットが配置のアプローチ点（Tool_Offset, PR[2]）、配置点（P[4]）に移動して配置（例えばDO[1] = OFF）を行い、その後、配置の離れ点（Tool_Offset, PR[2]）、Home位置の順に移動することを意味します。 |