MM_S21_Vis_As_Uframe
プログラム概要
機能 |
ロボットがMech-Visionプロジェクトを実行してビジョン結果を取得し、その結果を座標系として用いて、把持および配置動作のフローを実行します。 |
||
ファイルパス |
Mech-VisionとMech-Vizソフトウェアのインストールディレクトリにある
|
||
必要なプロジェクト |
Mech-Visionプロジェクト |
||
使用前提 |
|
| このサンプルプログラムは参考用です。ユーザーは実際の状況に応じて、このプログラムを基に変更を加える必要があります。このプログラムをそのまま使用しないでください。 |
プログラム説明
以下はMM_S21_Vis_As_Uframeサンプルプログラムのコードと関連する説明です。
| このサンプルは、MM_S1_Vis_Basicサンプルに比べて、すべての計画された経路をループで取得し、把持および配置を行う機能（太字部分のコード）のみが追加されています。そのため、MM_S1_Vis_Basicサンプルと同じ部分のコードについては、以下で再度説明することはありません（詳細は MM_S1_Vis_Basicサンプルの説明 をご参照ください）。 |
MODULE MM_S21_Vis_As_Uframe
!----------------------------------------------------------
! FUNCTION: trigger Mech-Vision project and get vision result
! Mech-Mind, 2026-1-28
!----------------------------------------------------------
!define local num variables
LOCAL VAR num pose_num:=0;
LOCAL VAR num status:=0;
LOCAL VAR num label:=0;
LOCAL VAR num toolid:=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:=[[1462.50,0.00,1905.00],[2.93133E-9,0,1,0],[0,0,0,0],[9E+9,9E+9,9E+9,9E+9,9E+9,9E+9]];
LOCAL PERS robtarget pick_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 pickpoint:=[[500,100,300],[0.00226227,-0.99991,-0.00439596,0.0124994],[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]];
!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]];
!define LOCAL wobjdata variables
TASK PERS wobjdata MM_Wobj:=[FALSE,TRUE,"",[[0,0,0],[1,0,0,0]],[[0,0,0],[1,0,0,0]]];
PROC Sample_21()
!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\WObj:=MM_Wobj;
!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 status;
IF status=3099 THEN
TPWrite "MM: Communication Error";
STOP;
ENDIF
!trigger NO.1 Mech-Vision project
MM_Start_Vis 1,0,2,snap_jps,status;
IF status<>1102 THEN
!add error handling logic here according to different error codes
TPWrite "MM: Status Error";
STOP;
ENDIF
!get vision result from NO.1 Mech-Vision project
stop;
MM_Get_VisData 1,pose_num,status;
!check whether vision result has been got from Mech-Vision successfully
IF status<>1100 THEN
!add error handling logic here according to different error codes
!e.g.: status=1003 means no point cloud in ROI
!e.g.: status=1002 means no vision result
Stop;
ENDIF
!close socket connection
MM_Close_Socket;
!save first vision point data to local variables
MM_Get_Pose 1,pickpoint,label,toolid;
!Convert the visual points to the workpiece coordinates
MM_Wobj.oframe.trans:=pickpoint.trans;
MM_Wobj.oframe.rot:=pickpoint.rot;
!move to intermediate waypoint of picking
MoveJ pick_waypoint,v1000,z50,gripper1;
!move to approach waypoint of picking
MoveL RelTool(pickpoint,0,0,-100), v1000, fine, gripper1\WObj:=MM_Wobj;
!move to picking waypoint
MoveL pickpoint, v300, fine, gripper1\WObj:=MM_Wobj;
!add object grasping logic here, such as "setdo DO_1, 1;"
Stop;
!move to departure waypoint of picking
MoveL RelTool(pickpoint,0,0,-100), v1000, fine, gripper1\WObj:=MM_Wobj;
!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;
!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;
ENDPROC
ENDMODULE上記のサンプルプログラムの処理流れは、下図の通りです。
下表は追加されたコードとその説明です。コマンド名のリンクをクリックすることで、その詳細を確認できます。
| 処理流れ | コートと説明 |
|---|---|
TCP位置姿勢を座標系の原点として保存 |
MM_Wobj は、取得したTCP位置姿勢を座標として解析し、指定した変数に保存できます。trans は並進量、rot は回転量を表します。 |
把持と配置を実行 |
上記のコードでは、ロボットはまず把持中間点へ移動し、続いて把持アプローチ点（把持点から Z 軸負方向に 100 mm オフセットした位置）へ移動します。その後、低速かつ高精度で把持点へ移動します。把持点では、必要に応じてグリッパーを閉じるロジックを追加できます。停止後、ロボットは把持離れる点（把持アプローチ点）へ戻ります。次に、ロボットは配置中間点へ移動し、配置アプローチ点（配置点から Z 軸負方向に 100 mm オフセットした位置）へ移動します。その後、低速かつ高精度で配置点へ移動します。配置点では、必要に応じてグリッパーを開くロジックを追加できます。停止後、ロボットは配置離れる点（配置アプローチ点）へ戻ります。最後に、関節絶対移動で初期位置へ戻り、1 回の把持・配置サイクルが完了します。 |