サンプルプログラム19：MM_S19_Vis_PlanAllVision

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

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

プログラム概要

機能

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

ファイル場所

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

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

必要なプロジェクト

Mech-Visionプロジェクト

経路計画設定ツール の ワークフロー 内にある 全体的設定 ステップで、全部のビジョン処理の結果を計画 パラメータが有効になっている必要があります。
sample19 1

使用前提

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

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

プログラム説明

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

このサンプルは、MM_S3_Vis_Pathサンプルに比べて、すべての計画された経路をループで取得し、把持および配置を行う機能（太字部分のコード）のみが追加されています。そのため、MM_S3_Vis_Pathサンプルと同じ部分のコードについては、以下で再度説明することはありません（詳細は MM_S3_Vis_Pathサンプルの説明 をご参照ください）。 
MODULE MM_S19_Vis_PlanAllVision
!----------------------------------------------------------
! FUNCTION: trigger Mech-Vision project, plan all vision
! results and get all planned paths
! Mech-Mind, 2023-12-25
!----------------------------------------------------------
!define local num variables
LOCAL VAR num pose_num:=0;
LOCAL VAR num status:=0;
LOCAL VAR num toolid{20}:=[0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,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 label{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 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{20}:=
[
    [[0,0,0,0,90,0],[9E+9,9E+9,9E+9,9E+9,9E+9,9E+9]],
    [[65.8654,94.6966,32.2878,-3.7036,-39.7668,-111.261],[9E+9,9E+9,9E+9,9E+9,9E+9,9E+9]],
    [[66.3351,103.661,20.3594,-3.9153,-36.8271,-110.505],[9E+9,9E+9,9E+9,9E+9,9E+9,9E+9]],
    [[65.8654,94.6966,32.2878,-3.7036,-39.7668,-111.261],[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]],
    [[-9.7932,85.483,6.0459,-20.5518,-3.0126,-169.245],[9E+9,9E+9,9E+9,9E+9,9E+9,9E+9]],
    [[-9.653,95.4782,-4.3661,-23.6568,-2.6275,-165.996],[9E+9,9E+9,9E+9,9E+9,9E+9,9E+9]],
    [[-9.7932,85.483,6.0459,-20.5518,-3.0126,-169.245],[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]],
    [[28.8422,90.4027,-13.4878,13.7086,10.2751,-164.54],[9E+9,9E+9,9E+9,9E+9,9E+9,9E+9]],
    [[29.122,100.975,-26.3271,11.2142,12.4744,-161.72],[9E+9,9E+9,9E+9,9E+9,9E+9,9E+9]],
    [[28.8422,90.4027,-13.4878,13.7086,10.2751,-164.54],[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]],
    [[28.7848,90.399,-13.628,14.5039,10.6719,-165.371],[9E+9,9E+9,9E+9,9E+9,9E+9,9E+9]],
    [[29.0915,100.961,-26.4037,12.003,12.8098,-162.514],[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_19()
    !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 NO.1 Mech-Vision project
    MM_Start_Vis 1,0,2,snap_jps;
    !get planned path from NO.1 Mech-Vision project, 2nd argument (1) means getting pose in JPs
    MM_Get_VisPath 1,1,pose_num,vis_pose_num,status;
    !check whether planned path has been got from Mech-Vision successfully
    IF status<>1103 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 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_Jps count,jps{count},label{count},toolid{count};
    ENDFOR
    !parse pick cycle count, here suppose 5 points per planned path
    pick_cnt:= pose_num DIV 5;
    residual:= pose_num MOD 5;
    !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)*5;
        !follow the planned path to pick
        MoveAbsJ jps{1+count},v1000,z50,gripper1;
        MoveAbsJ jps{2+count},v1000,fine,gripper1;
        MoveAbsJ jps{3+count},v1000,fine,gripper1;
        !add object grasping logic here, such as "setdo DO_1, 1;"
        Stop;
        MoveAbsJ jps{4+count},v1000,fine,gripper1;
        MoveAbsJ jps{5+count},v1000,z50,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

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

sample19

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

処理流れ コートと説明

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

 
FOR i FROM 1 TO pose_num DO
    count:=i;
    MM_Get_JPS count,jps{count},label{count},toolid{count};
ENDFOR

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

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

  • 3行目：MM_Get_Jps コマンドは、ある経路点の関節角度、ラベル、およびロボットハンド番号を指定された変数に保存することを意味します。つまり、このコマンドはcount番目の経路点の関節角度、ラベル、ロボットハンド番号をそれぞれjps{count}、label{count}、toolid{count}に保存することを意味します。

pick_cntとresidualを計算

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

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

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

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

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

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

 
FOR i FROM 1 TO pick_cnt DO
    count:=(i-1)*5;
    !follow the planned path to pick
    MoveAbsJ jps{1+count},v1000,z50,gripper1;
    MoveAbsJ jps{2+count},v1000,fine,gripper1;
    MoveAbsJ jps{3+count},v1000,fine,gripper1;
    !add object grasping logic here, such as "setdo DO_1, 1;"
    Stop;
    MoveAbsJ jps{4+count},v1000,fine,gripper1;
    MoveAbsJ jps{5+count},v1000,z50,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ループ内でロボットが各計画された経路における5つの経路点に移動し、把持を完了した後に配置を続けて実行することを意味します。iはループ回数を制御するために使用されます。iは1から始まり、各ループ後に1ずつ増加します。iが把持回数pick_cntを超えるとループは終了します。iが1増加すると、countが5増加します。そうすることで、{1+count}~{5+count}が計画された5つの経路点が経路全体における番号を表すようになります。

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

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

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

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

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