サンプルプログラム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サンプルの説明 をご参照ください）。 |
MODULE 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
!----------------------------------------------------------
!define variables
PERS num pose_num_b:=5;
PERS num vis_pose_num_b:=3;
PERS num toolid_b{5}:=[0,0,0,0,0];
PERS num label_b{5}:=[0,0,0,0,0];
PERS num status_b:=2100;
PERS bool flag_vis_b:=FALSE;
CONST jointtarget snap_jps_b:=[[0,0,0,0,90,0],[9E+9,9E+9,9E+9,9E+9,9E+9,9E+9]];
PERS jointtarget jps_b{5}:=
[
[[0,0,0,0,90,0],[9E+9,9E+9,9E+9,9E+9,9E+9,9E+9]],
[[0,0,0,0,90,0],[9E+9,9E+9,9E+9,9E+9,9E+9,9E+9]],
[[0,0,0,0,90,0],[9E+9,9E+9,9E+9,9E+9,9E+9,9E+9]],
[[11.1329,49.0771,-36.9666,0.5343,79.2476,-169.477],[9E+9,9E+9,9E+9,9E+9,9E+9,9E+9]],
[[0,0,0,0,90,0],[9E+9,9E+9,9E+9,9E+9,9E+9,9E+9]]
];
PROC main()
flag_vis_b:=FALSE;
!initialize communication parameters (initialization is required only once)
MM_Init_Socket "127.0.0.1",50000,300;
WHILE TRUE DO
IF flag_vis_b=TRUE THEN
MM_Open_Socket;
!trigger Mech-Viz project
MM_Start_Viz 2,snap_jps_b;
!get planned path, 1st argument (1) means getting pose in JPs
MM_Get_VizData 1, pose_num_b, vis_pose_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,jps_b{1},label_b{1},toolid_b{1};
MM_Get_JPS 2,jps_b{2},label_b{2},toolid_b{2};
MM_Get_JPS 3,jps_b{3},label_b{3},toolid_b{3};
ENDIF
MM_Close_Socket;
flag_vis_b:=FALSE;
ENDIF
ENDWHILE
ENDPROC
ENDMODULE上記のコードは、サブプログラムが実行される際に、まずflag_vis_bをFALSEに設定し、通信パラメータを初期化した後、WHILEループを使用してflag_vis_bの値を監視し続けることを示しています。
-
flag_vis_bがTRUEの場合、サブプログラムがMech-Vizプロジェクトをトリガーし、計画された経路を取得し、flag_vis_bをFALSEに設定します。
-
flag_vis_bがFALSEの場合、サブプログラムがflag_vis_bの値を監視し続けます。
以下は、メインプログラムのコードとその説明です。
| メインプログラム内で計画された経路に基づいて把持または配置を実行するコードは「MM_S2_Viz_Basic」サンプルと類似しています。そのため、MM_S2_Viz_Basicサンプルと同じ部分のコードについては、以下で再度説明することはありません（詳細は MM_S2_Viz_Basicサンプルの説明 をご参照ください）。 |
MODULE 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
!----------------------------------------------------------
!define local num variables
PERS bool flag_vis_b:=FALSE;
PERS num status_b:=2100;
PERS num pose_num_b:=5;
PERS num toolid_b{5}:=[0,0,0,0,0];
PERS num label_b{5}:=[0,0,0,0,0];
PERS num vis_pose_num_b:=3;
LOCAL VAR num count:=0;
!define local joint&pose variables
LOCAL CONST jointtarget home:=[[0,0,0,0,90,0],[9E+9,9E+9,9E+9,9E+9,9E+9,9E+9]];
LOCAL PERS robtarget pick_wait_point:=[[302.00,0.00,558.00],[0,0,-1,0],[0,0,0,0],[9E+9,9E+9,9E+9,9E+9,9E+9,9E+9]];
LOCAL PERS robtarget drop_waypoint:=[[302.00,0.00,558.00],[0,0,-1,0],[0,0,0,0],[9E+9,9E+9,9E+9,9E+9,9E+9,9E+9]];
LOCAL PERS robtarget drop:=[[302.00,0.00,558.00],[0,0,-1,0],[0,0,0,0],[9E+9,9E+9,9E+9,9E+9,9E+9,9E+9]];
PERS jointtarget jps_b{5}:=
[
[[0,0,0,0,0,0],[9E+9,9E+9,9E+9,9E+9,9E+9,9E+9]],
[[0,0,0,0,0,0],[9E+9,9E+9,9E+9,9E+9,9E+9,9E+9]],
[[0,0,0,0,0,0],[9E+9,9E+9,9E+9,9E+9,9E+9,9E+9]],
[[0,0,0,0,0,0],[9E+9,9E+9,9E+9,9E+9,9E+9,9E+9]],
[[0,0,0,0,0,0],[9E+9,9E+9,9E+9,9E+9,9E+9,9E+9]]
];
!define local tooldata variables
LOCAL PERS tooldata gripper1:=[TRUE,[[0,0,0],[1,0,0,0]],[0.001,[0,0,0.001],[1,0,0,0],0,0,0]];
PROC Sample_10()
!set the acceleration parameters
AccSet 50, 50;
!set the velocity parameters
VelSet 50, 1000;
!move to robot home position
MoveAbsJ home\NoEOffs,v3000,fine,gripper1;
!trigger Mech-Viz project and get planned path
trigger_vis_b;
LOOP:
!move to wait position for picking
MoveL pick_wait_point,v1000,fine,gripper1;
!wait until subtask program finished
WaitUntil(flag_vis_b=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
Stop;
ENDIF
!follow the planned path to pick
!move to approach waypoint of picking
MoveAbsJ jps_b{1},v1000,fine,gripper1;
!move to picking waypoint
MoveAbsJ jps_b{2},v300,fine,gripper1;
!add object grasping logic here, such as "setdo DO_1, 1;"
Stop;
!move to departure waypoint of picking
MoveAbsJ jps_b{3},v1000,fine,gripper1;
!move to intermediate waypoint of placing, and trigger Mech-Viz project and get planned path in advance
MoveJSync drop_waypoint,v1000,z50,gripper1,"trigger_vis_b";
!move to approach waypoint of placing
MoveL RelTool(drop,0,0,-100),v1000,fine,gripper1;
!move to placing waypoint
MoveL drop,v300,fine,gripper1;
!add object releasing logic here, such as "setdo DO_1, 0;"
Stop;
!move to departure waypoint of placing
MoveL RelTool(drop,0,0,-100),v1000,fine,gripper1;
!move back to robot home position
MoveAbsJ home\NoEOffs,v3000,fine,gripper1;
GOTO LOOP;
ENDPROC
PROC trigger_vis_b()
flag_vis_b:=TRUE;
ENDPROC
ENDMODULE上記のサンプルプログラムの処理流れは、下図の通りです。
下表はメインプログラムの主要なコードとその説明です。
| 処理流れ | コートと説明 | ||
|---|---|---|---|
Mech-Vizプロジェクトをトリガーし、計画された経路を取得 |
上記のコードは、メインプログラムがtrigger_vis_b()関数を呼び出していることを意味します。trigger_vis_b()関数のコードは以下の通りです。 上記のコードは、trigger_vis_b()関数で、flag_vis_bをTRUEに設定することを意味します。その場合、サブプログラムはflag_vis_bがTRUEであると検出したため、Mech-Vizプロジェクトをトリガーし、計画された経路を取得します。 |
||
ループで次の経路を事前に計画（把持→次の把持経路を計画→配置） |
上記のコードは、メインプログラムが「LOOP」と「GOTO LOOP」の間のコードを繰り返し実行することを意味します。 上記のコードは、ロボットが把持する前に待機点に移動し、サブプログラムが計画された経路を取得して保存するのを待機することを示しています。つまり、flag_vis_bがTRUEからFALSEに変わるのを待つことで、計画された経路が取得され、保存されたことを確認します。 上記のコードは、ロボットが計画された経路に従って把持のアプローチ点（jps_b{1}）に移動し、次に把持点（jps_b{2}）に移動して把持を行い（例：setdo DO_1, 1;）、その後、把持の離れる点（jps_b{3}）に移動することを意味します。 上記のコードは、ロボットが目標点（drop_waypoint、すなわち配置プロセスにおける中間点）まで最速で移動し、目標点でtrigger_vis_b()関数を呼び出すことを意味します。メインプログラムは再度、trigger_vis_b()関数を呼び出します。つまり、再度Mech-Vizプロジェクトをトリガーし、計画された経路を取得します。この時点でロボットはすで把持領域外にいるため、ロボットは配置が完了するのを待つことなく、次の把持経路を事前に計画することができます。
上記のコードは、ロボットが配置のアプローチ点（RelTool(drop,0,0,-100)）、配置点（drop）に移動して配置（例：setdo DO_1, 0;）を行い、その後、配置の離れ点（RelTool(drop,0,0,-100)）、初期位置（home）の順に移動することを意味します。 |