サンプルプログラム16：MM_S16_Viz_GetDirection
プログラム概要
機能 |
ロボットが計画された経路を取得し、把持を実行した後、ワーク群の向きに基づいて異なる配置方法を使用します。 |
||
ファイル場所 |
Mech-VisionとMech-Vizソフトウェアのインストールディレクトリにある
|
||
必要なプロジェクト |
Mech-Visionプロジェクト、Mech-Vizプロジェクト（ロボットハンド種類がデパレタイズ用吸盤である必要があります） |
||
使用前提 |
|
| このサンプルプログラムは参考用です。ユーザーは実際の状況に応じて、このプログラムを基に変更を加える必要があります。このプログラムをそのまま使用しないでください。 |
プログラム説明
以下はMM_S16_Viz_GetDirectionサンプルプログラムのコードと関連する説明です。
| このサンプルは、MM_S15_Viz_GetDoListサンプルに比べて、太字部分のコードのみが追加されています。そのため、MM_S15_Viz_GetDoListサンプルと同じ部分のコードについては、以下で再度説明することはありません（詳細は MM_S15_Viz_GetDoListサンプルの説明 をご参照ください）。 |
MODULE MM_S16_Viz_GetDirection
!----------------------------------------------------------
! FUNCTION: trigger Mech-Viz project, then get planned path
! and get box direction using command 210
! Mech-Mind, 2023-12-25
!----------------------------------------------------------
!define local num variables
LOCAL VAR num pose_num:=0;
LOCAL VAR num status:=0;
LOCAL VAR num vis_pose_num:=0;
LOCAL VAR num count:=0;
LOCAL VAR num box_direction:=0;
LOCAL VAR num movetype{5}:=[0,0,0,0,0];
LOCAL VAR num toolnum{5}:=[0,0,0,0,0];
LOCAL VAR num speed{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 VAR robtarget pickpoint:=[[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 VAR robtarget drop_waypoint_1:=[[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 VAR robtarget drop_1:=[[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 VAR robtarget drop_waypoint_2:=[[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 VAR robtarget drop_2:=[[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}:=
[
[[1.1835,39.2938,-17.0883,0.1382,67.7901,176.701],[9E+9,9E+9,9E+9,9E+9,9E+9,9E+9]],
[[1.2008,45.8522,-13.6729,0.1512,57.8163,176.689],[9E+9,9E+9,9E+9,9E+9,9E+9,9E+9]],
[[1.1835,39.2938,-17.0883,0.1382,67.7901,176.701],[9E+9,9E+9,9E+9,9E+9,9E+9,9E+9]],
[[36.2634,-36.6956,48.5019,-1.0197,78.1304,356.473],[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_16()
!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;
!get planned path
MM_Get_PlanData 0, 3, 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
!get gripper control signal list
MM_Get_Dolist 0,0;
!close socket connection
MM_Close_Socket;
!save waypoints of the planned path to local variables one by one
FOR i FROM 1 TO pose_num DO
count:=i;
MM_Get_PlanJps count,3,JPS{count},movetype{count},toolnum{count},speed{count};
ENDFOR
!follow the planned path to pick
FOR j FROM 1 TO pose_num DO
count:=j;
MoveAbsJ jps{count},v1000,fine,gripper1;
!set gripper control signal when current waypoint is picking waypoint
IF count=vis_pose_num THEN
!add object grasping logic here
Stop;
!set gripper control signal
!MM_Set_DoList 0, 1, go16_1;
!MM_Set_DoList 0, 2, go16_2;
!MM_Set_DoList 0, 3, go16_3;
!MM_Set_DoList 0, 4, go16_4;
ENDIF
ENDFOR
!get box direction status from planned results
box_direction:=MM_Plan_Results{17};
!place the box according to its direction
IF box_direction=0 THEN
!move to intermediate waypoint of placing
MoveJ drop_waypoint_1,v1000,z50,gripper1;
!move to approach waypoint of placing
MoveL RelTool(drop_1,0,0,-100),v1000,fine,gripper1;
!move to placing waypoint
MoveL drop_1,v300,fine,gripper1;
!add object releasing logic here, such as "setdo DO_1, 0;"
Stop;
!move to departure waypoint of placing
MoveL RelTool(drop_2,0,0,-100),v1000,fine,gripper1;
ELSE
!move to intermediate waypoint of placing
MoveJ drop_waypoint_2,v1000,z50,gripper1;
!move to approach waypoint of placing
MoveL RelTool(drop_2,0,0,-100),v1000,fine,gripper1;
!move to placing waypoint
MoveL drop_2,v300,fine,gripper1;
!add object releasing logic here, such as "setdo DO_1, 0;"
Stop;
!move to departure waypoint of placing
MoveL RelTool(drop_2,0,0,-100),v1000,fine,gripper1;
ENDIF
!move back to robot home position
MoveAbsJ home\NoEOffs,v3000,fine,gripper1;
ENDPROC
ENDMODULE上記のサンプルプログラムの処理流れは、下図の通りです。
下表は太字部分のコードとその説明です。コマンド名のリンクをクリックすることで、その詳細を確認できます。
| 処理流れ | コートと説明 | ||
|---|---|---|---|
ワーク群の向きを取得 |
ロボットは MM_Get_PlanData コマンドを使用して経路点の「ビジョン処理による移動」の計画結果を取得し、その後、MM_Get_PlanJps コマンドを使用して経路点の「ビジョン処理による移動」の計画結果をロボットメモリからMM_Plan_Resultsデータグループに保存します。 MM_Plan_Resultsデータグループの中、MM_Plan_Results{17}は吸盤の長辺に対するワーク群の向きです。0は平行、1は垂直を意味します。 上記のコードは、MM_Plan_Results{17}をbox_direction変数に代入することを意味します。つまり、box_direction変数はワーク群の向きを示すようになります。 |
||
ワーク群の向きに基づいて異なる配置方法を使用 |
上記のコードは、ワーク群が吸盤の長辺と平行している場合（box_directionが0の場合）、そのワーク群をdrop_1に配置します。それ以外の場合は、ワーク群をdrop_2に配置することを意味します。 |