サンプルプログラム20：MM_S20_Viz_PlanAllVision

現在ご覧いただいているのは2.1.0 バージョンの内容です。異なるバージョンを参照する場合は、画面右上のボタンから切り替えが可能です。

■ 最新版をご利用される場合は、弊社ホームページよりダウンロードが可能です。ダウンロードにはパスワードが必要となりますので、サポート窓口までお問い合わせください。

■ ご利用中のバージョンが分からない場合や、サポートが必要な場合はお気軽にサポート窓口までご連絡ください。

プログラム概要

機能

ロボットはMech-Vizプロジェクトをトリガーして実行し、その後ループですべての計画された経路を取得し、把持および配置を実行します。このサンプルでは、カメラで1回撮影することで、Mech-Vizがすべてのビジョン結果に基づいた把持経路を計画します。通常は「1回の撮影で複数回の把持を行う」シーンで使用されます。

ファイル場所

Mech-VisionとMech-Vizソフトウェアのインストールディレクトリにある Communication Component/Robot_Interface/ABB/sample/MM_S20_Viz_PlanAllVision。

RobotWare6システムを使用する場合、ファイルの拡張子は .mod です。RobotWare7システムを使用する場合、ファイルの拡張子を .mod から .modx に変更する必要があります。

必要なプロジェクト

Mech-VisionとMech-Vizプロジェクト

ビジョン処理による移動 ステップの ビジョン処理の結果を再使用 パラメータにチェックが入っている必要があります。
sample20 1

使用前提

  1. 標準インターフェース通信設定が完了したこと。

  2. 自動キャリブレーションが完了したこと。
このサンプルプログラムは参考用です。ユーザーは実際の状況に応じて、このプログラムを基に変更を加える必要があります。このプログラムをそのまま使用しないでください。

プログラム説明

以下はMM_S20_Viz_PlanAllVisionサンプルプログラムのコードと関連する説明です。

このサンプルは、MM_S2_Viz_Basicサンプルに比べて、すべての計画された経路をループで取得し、把持および配置を行う機能（太字部分のコード）のみが追加されています。そのため、MM_S2_Viz_Basicサンプルと同じ部分のコードについては、以下で再度説明することはありません（詳細は MM_S2_Viz_Basicサンプルの説明 をご参照ください）。 
MODULE MM_S20_Viz_PlanAllVision
!----------------------------------------------------------
! FUNCTION: trigger Mech-Viz project, plan all vision
! results and get all planned results 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 pick_cnt:=0;
LOCAL VAR num residual:=0;
LOCAL VAR num movetype{20}:=[0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0];
LOCAL VAR num toolnum{20}:=[0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0];
LOCAL VAR num speed{20}:=[0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,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 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 VAR 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 VAR 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{20}:=
[
    [[-4.5438,26.6029,0.467,-0.1719,62.9165,-5.0819],[9E+9,9E+9,9E+9,9E+9,9E+9,9E+9]],
    [[-4.5664,32.0439,7.6047,-0.1988,50.3379,-5.0558],[9E+9,9E+9,9E+9,9E+9,9E+9,9E+9]],
    [[-4.5363,25.4009,-2.7016,-0.1659,67.2872,-5.0885],[9E+9,9E+9,9E+9,9E+9,9E+9,9E+9]],
    [[-20.7719,15.0701,16.7232,0.1053,57.9831,-111.82],[9E+9,9E+9,9E+9,9E+9,9E+9,9E+9]],
    [[-20.7567,21.7711,23.8763,0.1281,44.1289,-111.841],[9E+9,9E+9,9E+9,9E+9,9E+9,9E+9]],
    [[-20.777,13.5249,13.6202,0.1005,62.6312,-111.816],[9E+9,9E+9,9E+9,9E+9,9E+9,9E+9]],
    [[0.3363,10.0698,23.0021,0.0017,56.8211,89.2201],[9E+9,9E+9,9E+9,9E+9,9E+9,9E+9]],
    [[0.3365,17.2854,30.3891,0.0022,42.2185,89.2197],[9E+9,9E+9,9E+9,9E+9,9E+9,9E+9]],
    [[0.3362,8.4182,19.8363,0.0017,61.6386,89.2202],[9E+9,9E+9,9E+9,9E+9,9E+9,9E+9]],
    [[-2.6715,8.5325,24.7355,0.0089,56.6276,265.9],[9E+9,9E+9,9E+9,9E+9,9E+9,9E+9]],
    [[-2.6701,15.8879,32.2202,0.0112,41.7875,265.897],[9E+9,9E+9,9E+9,9E+9,9E+9,9E+9]],
    [[-2.672,6.8572,21.5409,0.0085,61.4976,265.9],[9E+9,9E+9,9E+9,9E+9,9E+9,9E+9]],
    [[21.387,12.8326,19.5238,-0.0095,57.6221,110.349],[9E+9,9E+9,9E+9,9E+9,9E+9,9E+9]],
    [[21.3856,19.7174,26.8272,-0.0117,43.434,110.351],[9E+9,9E+9,9E+9,9E+9,9E+9,9E+9]],
    [[21.3874,11.2544,16.3788,-0.0091,62.3455,110.349],[9E+9,9E+9,9E+9,9E+9,9E+9,9E+9]],
    [[10.5881,41.5724,-23.6476,-0.3127,72.1244,189.431],[9E+9,9E+9,9E+9,9E+9,9E+9,9E+9]],
    [[10.5502,45.3582,-15.4893,-0.3431,60.1803,189.468],[9E+9,9E+9,9E+9,9E+9,9E+9,9E+9]],
    [[10.6008,40.9927,-27.4632,-0.306,76.5197,189.419],[9E+9,9E+9,9E+9,9E+9,9E+9,9E+9]],
    [[28.7848,90.399,-13.628,14.5039,10.6719,-165.371],[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_20()
    !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;
RECAP:
    !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
    !close socket connection
    MM_Close_Socket;
    !save all waypoint data to local variables using for-loop, a maximum of 50 points are supported
    FOR i FROM 1 TO pose_num DO
        count:=i;
        MM_Get_PlanJps count,3,JPS{count},movetype{count},toolnum{count},speed{count};
    ENDFOR
    !parse pick cycle count, here suppose 3 points per planned path
    pick_cnt:= pose_num DIV 3;
    residual:= pose_num MOD 3;
    !check if parsed data is valid; if not, retry to get planned path or add some error handling logic
    IF (pick_cnt<1) OR (residual<>0) THEN
        Stop;
        GOTO RECAP;
    ENDIF
    !repeatedly run pick-and-place cycle using for-loop
    FOR i FROM 1 TO pick_cnt DO
        count:=(i-1)*3;
        !move to intermediate waypoint of picking
        MoveJ pick_waypoint,v1000,z50,gripper1;
        !follow the planned path to pick
        MoveAbsJ jps{1+count},v1000,fine,gripper1;
        MoveAbsJ jps{2+count},v1000,fine,gripper1;
        !add object grasping logic here, such as "setdo DO_1, 1;";
        Stop;
        MoveAbsJ jps{3+count},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;
        !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 to intermediate waypoint of placing
        MoveJ drop_waypoint,v1000,z50,gripper1;
    ENDFOR
    !finish pick and-place cycle, and jump back to camera capturing
    GOTO RECAP;
ENDPROC
ENDMODULE

上記のサンプルプログラムの処理流れは、下図の通りです。

sample20

下表は追加されたコードとその説明です。コマンド名のリンクをクリックすることで、その詳細を確認できます。

処理流れ コートと説明

計画された経路を取得

 
!get planned path
MM_Get_PlanData 0, 3, pose_num, vis_pose_num, status;

  • MM_Get_PlanData：計画された経路を取得するためのコマンド。このコマンドを使用して取得した「ビジョン処理による移動」の経路点は、位置姿勢に加えて、「ビジョン処理による移動」の計画結果やカスタムデータ（ある場合）も含まれます。一方、 MM_Get_VizData コマンドを使用して取得した「ビジョン処理による移動」の経路点には、「ビジョン処理による移動」の計画結果やカスタムデータは含まれていません。

  • 0：Mech-Vizから計画された経路を取得します。

  • 3：返されるデータ形式。「位置姿勢（関節角度形式）、移動方式、ロボットハンド番号、速度、Mech-Vizの「ビジョン処理による移動」の計画結果、カスタムデータ項目1、…、カスタムデータ項目N」といった形式でデータを返します。

  • pose_num：この変数はビジョンシステムから返された経路点の数を保存します。

  • vis_pose_num：この変数は、「ビジョン処理による移動」の最後の経路点（把持点）が経路全体における位置番号を保存します。

  • status：この変数はコマンド実行のステータスコードを保存します。

ループで計画された経路を保存

 
FOR i FROM 1 TO pose_num DO
    count:=i;
    MM_Get_PlanJps count,3,JPS{count},movetype{count},toolnum{count},speed{count};
ENDFOR

  • 1行目：FORは以下がループプログラム部分を示します。iはループの回数を制御します（iは1から始まり、毎回ループ後に1増加し、pose_numより大きくなるとループが終了します）。pose_numは MM_Get_PlanData コマンドにおける3番目のパラメータで、このパラメータはビジョンシステムが返す経路点の数を示します。

  • 2行目：iの値をcountに代入します。countは、計画された経路における現在の経路点の番号を示します。

  • 3行目：MM_Get_PlanJps コマンドは、ある経路点の関節角度、移動方式、ロボットハンド番号、速度をそれぞれ指定された変数に保存することを意味します。つまり、このコマンドはcount番目の経路点の関節角度、移動方式、ロボットハンド番号、速度をそれぞれJPS{count}、movetype{count}、toolnum{count}、speed{count}に保存することを意味します。

pick_cntとresidualを計算

 
!parse pick cycle count, here suppose 3 points per planned path
pick_cnt:= pose_num DIV 3;
residual:= pose_num MOD 3;

本サンプルでは、計画された経路には3つの経路点が含まれると仮定しています。「pose_num DIV 3」はpose_numを3で割った商、「pose_num MOD 3」はpose_numを3で割った余りを表します。pick_cntは計画された把持回数を表します。residualが0でない場合、ある経路計画において把持経路点の数が3未満であることを意味し、経路計画に異常が発生したため再計画が必要です。

経路計画が異常かどうかを判断

 
IF (pick_cnt<1) OR (residual<>0) THEN
    Stop;
    GOTO RECAP;
ENDIF

把持回数pick_cntが1未満である場合、またはresidualが0でない場合、経路計画に異常が発生していることを示します。この場合、処理ロジックを追加する必要があります。例えば、Mech-Vizプロジェクトを再度トリガーして実行し、計画された経路を再取得するなどの対応が必要です。

ループで把持および配置を実行

 
FOR i FROM 1 TO pick_cnt DO
    count:=(i-1)*3;
    !move to intermediate waypoint of picking
    MoveJ pick_waypoint,v1000,z50,gripper1;
    !follow the planned path to pick
    MoveAbsJ jps{1+count},v1000,fine,gripper1;
    MoveAbsJ jps{2+count},v1000,fine,gripper1;
    !add object grasping logic here, such as "setdo DO_1, 1;";
    Stop;
    MoveAbsJ jps{3+count},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;
    !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 to intermediate waypoint of placing
    MoveJ drop_waypoint,v1000,z50,gripper1;
ENDFOR

上記のコードは、FORループ内でロボットが各計画された経路における3つの経路点に移動し、把持を完了した後に配置を続けて実行することを意味します。iはループ回数を制御するために使用されます。iは1から始まり、各ループ後に1ずつ増加します。iが把持回数pick_cntを超えるとループは終了します。iが1増加すると、countが3増加します。そうすることで、{1+count}~{3+count}が計画された3つの経路点が経路全体における番号を表すようになります。

この情報は役に立ちましたか？

ご意見・ご要望がございましたら、以下よりお寄せください：

Mech-Mindは、お客様のプライバシーを重視しています

このサイトでは最高の体験を提供するために Cookie を使用しています。サイトの閲覧を続ける場合、Cookie の使用に同意したことになります。「拒否する」を選択すると、このサイトを訪れた際に追跡や記憶が行われないように単独の Cookie が使用されます。

  • Preference
  • 同意する
  • 拒否する