サンプルプログラム9：MM_S9_Viz_RunInAdvance
プログラム概要
機能 |
ロボットは把持時にMech-Vizプロジェクトをトリガーし、その後、分岐設定により配置時にカメラ撮影をトリガーします。これにより、次の経路を事前に計画し、サイクルタイムを短縮します。 |
||
ファイル場所 |
Mech-VisionとMech-Vizソフトウェアのインストールディレクトリにある
|
||
必要なプロジェクト |
Mech-VisionとMech-Vizプロジェクト
|
||
使用前提 |
|
| このサンプルプログラムは参考用です。ユーザーは実際の状況に応じて、このプログラムを基に変更を加える必要があります。このプログラムをそのまま使用しないでください。 |
プログラム説明
以下はMM_S9_Viz_RunInAdvanceサンプルプログラムのコードと関連する説明です。
| このサンプルは、MM_S5_Viz_SetBranchサンプルに比べて、次の経路を事前に計画する機能（太字部分のコード）のみが追加されています。そのため、MM_S5_Viz_SetBranchサンプルと同じ部分のコードについては、以下で再度説明することはありません（詳細は MM_S5_Viz_SetBranchサンプルの説明 をご参照ください）。 |
MODULE MM_S9_Viz_RunInAdvance
!----------------------------------------------------------
! FUNCTION: trigger Mech-Viz project then set branch and get
! planned path, trigger Mech-Viz project and set branch in
! advance during pick&place process
! Mech-Mind, 2023-12-25
!----------------------------------------------------------
!define local num variables
LOCAL VAR num pose_num:=0;
LOCAL VAR num status:=0;
LOCAL VAR num toolid{5}:=[0,0,0,0,0];
LOCAL VAR num vis_pose_num:=0;
LOCAL VAR num count:=0;
LOCAL VAR num label{5}:=[0,0,0,0,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 CONST jointtarget snap_jps:=[[0,0,0,0,90,0],[9E+9,9E+9,9E+9,9E+9,9E+9,9E+9]];
LOCAL PERS robtarget camera_capture:=[[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]];
LOCAL PERS jointtarget jps{5}:=
[
[[11.1329,49.0771,-36.9666,0.5343,79.2476,-169.477],[9E+9,9E+9,9E+9,9E+9,9E+9,9E+9]],
[[11.2355,52.1281,-23.3996,0.5938,62.6295,-169.548],[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]],
[[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]]
];
!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_9()
!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;
!initialize communication parameters (initialization is required only once)
MM_Init_Socket "127.0.0.1",50000,300;
!move to image-capturing position
MoveL camera_capture,v1000,fine,gripper1;
!open socket connection
MM_Open_Socket;
!trigger Mech-Viz project
MM_Start_Viz 2,snap_jps;
!set branch exit port
MM_Set_Branch 1,1;
LOOP:
!get planned path, 1st argument (1) means getting pose in JPs
MM_Get_VizData 1, pose_num, vis_pose_num, status;
!check whether planned path has been got from Mech-Viz successfully
IF status <> 2100 THEN
!add error handling logic here according to different error codes
!e.g.: status=2038 means no point cloud in ROI
Stop;
ENDIF
!save waypoints of the planned path to local variables one by one
MM_Get_Jps 1,jps{1},label{1},toolid{1};
MM_Get_JPS 2,jps{2},label{2},toolid{2};
MM_Get_JPS 3,jps{3},label{3},toolid{3};
!follow the planned path to pick
!move to approach waypoint of picking
MoveAbsJ jps{1},v1000,fine,gripper1;
!move to picking waypoint
MoveAbsJ jps{2},v300,fine,gripper1;
!add object grasping logic here, such as "setdo DO_1, 1;"
Stop;
!trigger Mech-Viz project but not to trigger camera capturing
MM_Start_Viz 2,snap_jps;
!move to departure waypoint of picking
MoveAbsJ jps{3},v1000,fine,gripper1;
!move to intermediate waypoint of placing
MoveJ drop_waypoint,v1000,z50,gripper1;
!move to approach waypoint of placing
MoveL RelTool(drop,0,0,-100),v1000,fine,gripper1;
!set branch exit port and trigger camera capturing when robot moves out of camera’s field of view
MM_Set_Branch 1,1;
!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;
END_LOOP:
!close socket connection
MM_Close_Socket;
ENDPROC
ENDMODULE上記のサンプルプログラムの処理流れは、下図の通りです。
以下は、次の経路を事前に計画するコードとその説明です。
| 処理流れ | コートと説明 | ||
|---|---|---|---|
ループで次の経路を事前に計画（把持→次の把持経路を計画→配置） |
上記のコードは、プログラムが「LOOP」と「GOTO LOOP」の間のコードを繰り返し実行することを意味します。 上記のコードは、ロボットが MM_Get_VizData コマンドを使用してMech-Vizで計画された経路を取得することを意味します。 上記のコードは、ロボットが MM_Get_Jps コマンドを使用して計画された経路を指定された変数に保存することを意味します。このサンプルでは、jps{1}、jps{2}、jps{3}がそれぞれ把持のアプローチ点、把持点、把持の離れる点であると仮定しています。 上記のコードは、ロボットが把持のアプローチ点、把持点、把持の離れる点の順に移動し、DOコマンド（例：setdo DO_1, 1;）を設定することでロボットハンドを制御し、把持を実行することを意味します。
上記のコードは、ロボットが MM_Start_Viz コマンドを使用してMech-Vizプロジェクトを再度トリガーします。この時点で、ロボットは今回計画された把持経路を取得し、把持点に移動しているため、配置が完了するのを待つことなく、事前にMech-Vizプロジェクトをトリガーして次の経路を計画することができます。
上記のコードは、ロボットが把持の離れる点（jps{3}）、中間点（drop_waypoint）、配置のアプローチ点（RelTool(drop,0,0,-100)）の順に移動することを意味します。 上記のコードは、ロボットが MM_Set_Branch コマンドを使用して「メッセージによって異なる分岐を実行」ステップの出口を設定することを意味します。この分岐出口では、「ビジョン処理による認識」ステップが実行され、Mech-Vizはビジョン認識の結果に基づいて次の把持経路を事前に計画します。 上記のコードは、ロボットが配置点（drop）に移動して配置を行い（例：setdo DO_1, 0;）、その後、配置の離れる点（RelTool(drop,0,0,-100)）、初期位置（home）の順に移動することを意味します。 |