Programme d’exemple 20 : MM_S20_Viz_PlanAllVision

Vous consultez actuellement la documentation pour la dernière version (2.1.2). Pour accéder à une autre version, cliquez sur le bouton "Changer de version" situé dans le coin supérieur droit de la page.

■ Si vous n’êtes pas sûr de la version du produit que vous utilisez, veuillez contacter le support technique Mech-Mind pour obtenir de l’aide.

Présentation du programme

Description

Le robot déclenche l’exécution du projet Mech-Viz. Ensuite, le robot utilise des boucles for pour obtenir toutes les trajectoires planifiées et effectuer la prise et la dépose. Dans cet exemple, une fois que la caméra a capturé une image, Mech-Viz planifie des trajectoires de prise pour tous les résultats de vision. Ce programme est applicable aux scénarios où une seule image est utilisée pour effectuer des opérations de prise à plusieurs reprises.

Chemin du fichier

Vous pouvez accéder au répertoire d’installation de Mech-Vision et Mech-Viz et trouver le fichier en suivant le chemin Communication Component/Robot_Interface/ABB/sample/MM_S20_Viz_PlanAllVision.

Pour RobotWare6, l’extension de fichier est .mod. Pour RobotWare7, veuillez modifier l’extension de fichier de .mod à .modx.

Projet

projets Mech-Vision et Mech-Viz

Dans le projet Mech-Viz, le paramètre Réutiliser le résultat de vision de l’étape Vision Move doit être activé.
sample20 1

Prérequis

  1. Vous avez configuré la communication via l’interface standard.

  2. L’étalonnage automatique est terminé.
Ce programme d’exemple est fourni uniquement à titre de référence. Avant d’utiliser le programme, veuillez le modifier selon le scénario réel.

Description du programme

Cette partie décrit le programme d’exemple MM_S20_Viz_PlanAllVision.

La seule différence entre le programme d’exemple MM_S20_Viz_PlanAllVision et le programme d’exemple MM_S2_Viz_Basic est que MM_S20_Viz_PlanAllVision peut utiliser des boucles for pour obtenir toutes les trajectoires planifiées et effectuer la prise et la dépose (le code de cette fonctionnalité est en gras). Par conséquent, seule la fonctionnalité d’utilisation de boucles for pour obtenir toutes les trajectoires planifiées et effectuer la prise et la dépose est décrite dans la section suivante. Pour plus d’informations sur les parties de MM_S20_Viz_PlanAllVision qui sont cohérentes avec celles de MM_S2_Viz_Basic, voir Programme d’exemple : 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

Le flux de travail correspondant au code du programme d’exemple ci-dessus est illustré dans la figure ci-dessous.

sample20

Le tableau ci-dessous décrit la fonctionnalité consistant à utiliser des boucles for pour obtenir toutes les trajectoires planifiées et effectuer la prise et la dépose. Vous pouvez cliquer sur l’hyperlien du nom de commande pour afficher sa description détaillée.

Fonctionnalité Code et description

Obtenir la trajectoire planifiée

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

  • MM_Get_PlanData: La commande permettant d’obtenir la trajectoire planifiée. Les points de passage Vision Move obtenus par cette commande contiennent, en plus des poses, les données Vision Move et les données personnalisées (le cas échéant), tandis que les points de passage Vision Move obtenus par la commande MM_Get_VizData ne contiennent ni données Vision Move ni données personnalisées.

  • 0 : Obtenir la trajectoire planifiée depuis Mech-Viz.

  • 3 : Le format des données attendu en retour, à savoir pose (en positions articulaires), type de mouvement, ID d’outil, vitesse, données Vision Move de Mech-Viz, élément 1 des données de sortie personnalisées, ..., élément N des données de sortie personnalisées.

  • pose_num : La variable qui stocke le nombre de points de passage renvoyés par le système de vision.

  • vis_pose_num : La variable qui stocke le numéro de séquence du dernier point de passage Vision Move (point de passage de prise) dans la trajectoire planifiée totale.

  • status : La variable qui stocke le code d’état d’exécution de la commande.

Stocker la trajectoire planifiée par boucle

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

  • Ligne 1 : FOR indique une boucle for. i est utilisé pour contrôler le nombre d’itérations de la boucle (c’est-à-dire que i commence à 1 et s’incrémente de 1 après chaque itération jusqu’à dépasser la valeur de pose_num, moment où la boucle se termine). pose_num est le troisième paramètre de la commande MM_Get_PlanData, qui représente le nombre de points de passage renvoyés par le système de vision.

  • Ligne 2 : Attribuer la valeur de i à count, où count représente l’ID du point de passage actuel dans la trajectoire planifiée totale.

  • Ligne 3 : La commande MM_Get_PlanJps stocke les positions articulaires, le type de mouvement, l’ID d’outil et la vitesse d’un point de passage spécifique dans des variables spécifiques. La commande complète stocke respectivement les positions articulaires, le type de mouvement, l’ID d’outil et la vitesse du point de passage dont l’ID est count dans les variables JPS{count}, movetype{count}, toolnum{count} et speed{count}.

Calculer pick_cnt et residual

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

Ce programme d’exemple suppose que chaque trajectoire de prise planifiée contient 3 points de passage. pose_num DIV 3 représente le quotient de la valeur pose_num divisée par 3, et pose_num MOD 3 représente le reste de la valeur pose_num divisée par 3. pick_cnt est le nombre total d’opérations de prise planifiées. Si residual n’est pas égal à 0, le nombre planifié de points de passage de prise est inférieur à 3 (c’est-à-dire qu’une erreur s’est produite pendant la planification de trajectoire et une opération de replanification est nécessaire).

Déterminer s’il y a eu une erreur pendant la planification de trajectoire

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

Si le nombre d’opérations de prise (pick_cnt) est inférieur à 1 ou si la valeur de residual n’est pas 0, une erreur s’est produite pendant la planification de trajectoire. Vous devez ajouter ici du code de traitement, par exemple le code pour redémarrer le projet Mech-Viz puis obtenir la trajectoire planifiée.

Effectuer la prise et la dépose par boucle

 
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

Le code ci-dessus indique que, dans la boucle for, le robot se déplace vers les 3 points de passage planifiés à chaque fois pour effectuer l’opération de prise, puis réalise l’opération de dépose. i est utilisé pour contrôler le nombre d’itérations de la boucle (c’est-à-dire que i commence à 1 et s’incrémente de 1 après chaque itération jusqu’à dépasser la valeur de pick_cnt, moment où la boucle se termine). Lorsque i s’incrémente de 1, count s’incrémente de 3. {1+count} à {3+count} indiquent l’ID des 3 points de passage planifiés à chaque fois dans la trajectoire planifiée totale.

Cette page est-elle utile ?

Veuillez nous indiquer comment améliorer :

Nous accordons de l’importance à votre vie privée

Nous utilisons des cookies pour vous offrir la meilleure expérience possible sur notre site web. En continuant à utiliser le site, vous reconnaissez accepter l’utilisation des cookies. Si vous refusez, un cookie unique sera utilisé pour garantir que vous ne soyez pas suivi ou reconnu lors de votre visite sur ce site.

  • Préférence
  • J’accepte
  • Je n’accepte pas