Exemple de programme 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 capture une image, Mech-Viz planifie des trajectoires de prise pour tous les résultats de vision. Ce programme s’applique aux scénarios où une image est utilisée pour effectuer la prise plusieurs fois.

Chemin du fichier

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

Projet

Projets Mech-Vision et Mech-Viz

Dans le projet Mech-Viz, le paramètre Reuse Vision Result de l’étape Vision Move doit être activé.

Prérequis

Vous avez configuré la communication de l’Interface Standard.
Calibration automatique est terminée.

Ce programme d’exemple est fourni uniquement à titre de référence. Avant d’utiliser le programme, veuillez le modifier en fonction du 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 correspondant à cette fonctionnalité est en gras). Par conséquent, les fonctionnalités du programme MM_S20_Viz_PlanAllVision similaires à celles de MM_S2_Viz_Basic ne sont pas décrites dans cette partie. Pour plus d’informations sur ces fonctionnalités, voir Exemple de programme 2: MM_S2_Viz_Basic.

DEF  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
;---------------------------------------------------
   ;set current tool no. to 1
   BAS(#TOOL,1)
   ;set current base no. to 0
   BAS(#BASE,0)
   ;move to robot home position
PTP HOME Vel=100 % DEFAULT
   ;initialize communication parameters (initialization is required only once)
   MM_Init_Socket("XML_Kuka_MMIND",873,871,60)
RECAP:
   ;move to image-capturing position
LIN camera_capture Vel=1 m/s CPDAT1 Tool[1] Base[0]
   ;trigger Mech-Viz project
   MM_Start_Viz(2,init_jps)
   ;get planned path
   MM_Get_PlanData(0,3,pos_num,vis_pos_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
      halt
   ENDIF
   ;save waypoints of the planned path to local variables one by one
   FOR count=1 TO pos_num
      MM_Get_PlanJps(count,3,pick_point[count],move_type[count],tool_num[count],speed[count])
   ENDFOR
   ;parse pick cycle count, here suppose 3 points per planned path
   pick_cnt = pos_num / 3
   residual = pos_num - pick_cnt*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
      halt
      GOTO RECAP
   ENDIF
   ;repeatedly run pick-and-place cycle using for-loop
   FOR i=1 TO pick_cnt
      count=(i-1)*3
      Xpick_point1=pick_point[1+count]
      Xpick_point2=pick_point[2+count]
      Xpick_point3=pick_point[3+count]
      ;move to intermediate waypoint of picking
PTP pick_waypoint CONT Vel=50 % PDAT7 Tool[1] Base[0]
      ;follow the planned path to pick
PTP pick_point1 Vel=50 % PDAT1 Tool[1] Base[0]
PTP pick_point2 Vel=10 % PDAT2 Tool[1] Base[0]
      ;add object grasping logic here, such as "$OUT[1]=TRUE"
      halt
PTP pick_point3 Vel=50 % PDAT3 Tool[1] Base[0]
      ;move to intermediate waypoint of placing
PTP drop_waypoint CONT Vel=100 % PDAT2 Tool[1] Base[0]
      ;move to approach waypoint of placing
LIN drop_app Vel=1 m/s CPDAT3 Tool[1] Base[0]
      ;move to placing waypoint
LIN drop Vel=0.3 m/s CPDAT4 Tool[1] Base[0]
      ;add object releasing logic here, such as "$OUT[1]=FALSE"
      halt
      ;move to departure waypoint of placing
LIN drop_app Vel=1 m/s CPDAT3 Tool[1] Base[0]
      ;move to intermediate waypoint of placing
PTP drop_waypoint CONT Vel=100 % PDAT2 Tool[1] Base[0]
   ENDFOR
   ;finish pick and-place cycle, and jump back to camera capturing
   GOTO RECAP
END

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

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 consulter sa description détaillée.

Fonctionnalité Code et description

Fonctionnalité	Code et description
Obtenir la trajectoire planifiée	`;get planned path MM_Get_PlanData(0,3,pos_num,vis_pos_num,status)` MM_Get_PlanData: La commande pour obtenir la trajectoire planifiée. Les points de passage Vision Move obtenus par cette commande contiennent, en plus des poses, des données Vision Move et des 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 la pose (en positions articulaires), le type de mouvement, l’ID d’outil, la vitesse, les données Vision Move de Mech-Viz, l’élément 1 des données de sortie personnalisées, ..., l’é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 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 bouclage	`FOR count=1 TO pos_num MM_Get_PlanJps(count,3,pick_point[count],move_type[count],tool_num[count],speed[count]) ENDFOR` Ligne 1: FOR indique une boucle for. count est utilisé pour contrôler le nombre d’itérations de la boucle (c’est-à-dire que count démarre à 1 et s’incrémente de 1 après chaque itération jusqu’à dépasser la valeur de pose_num, moment auquel 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: La commande MM_Get_PlanJps enregistre 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. L’ensemble de la commande 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 pick_point[count], move_type[count], tool_num[count] et speed[count]. Ligne 3: La boucle for se termine.
Calculer pick_cnt et residual	`;parse pick cycle count, here suppose 3 points per planned path pick_cnt = pos_num / 3 residual = pos_num - pick_cnt3` Ce programme d’exemple suppose que chaque trajectoire de prise planifiée contient 3 points de passage. "pick_cnt = pos_num / 3" représente le quotient de la valeur pose_num divisée par 3, et "pos_num - pick_cnt3" représente le reste de la valeur pose_num divisée par 3. pick_cnt est le nombre total de prises planifiées. Si residual n’est pas égal à 0, le nombre de points de passage de prise planifié est inférieur à 3 (c’est-à-dire qu’une erreur s’est produite lors de la planification de trajectoire et qu’une replanification est nécessaire).
Déterminer si une erreur s’est produite lors de la planification de trajectoire	`IF (pick_cnt<1) OR (residual<>0) THEN halt GOTO RECAP ENDIF` Si le nombre de prises (pick_cnt) est inférieur à 1 ou si la valeur de residual n’est pas 0, une erreur s’est produite lors de la planification de trajectoire. Vous devez ajouter ici du code de traitement, comme le code pour redémarrer le projet Mech-Viz puis obtenir la trajectoire planifiée.
Effectuer la prise et la dépose par bouclage	FOR i=1 TO pick_cnt count=(i-1)*3 Xpick_point1=pick_point[1+count] Xpick_point2=pick_point[2+count] Xpick_point3=pick_point[3+count] ;move to intermediate waypoint of picking PTP pick_waypoint CONT Vel=50 % PDAT7 Tool[1] Base[0] ;follow the planned path to pick PTP pick_point1 Vel=50 % PDAT1 Tool[1] Base[0] PTP pick_point2 Vel=10 % PDAT2 Tool[1] Base[0] ;add object grasping logic here, such as "$OUT[1]=TRUE" halt PTP pick_point3 Vel=50 % PDAT3 Tool[1] Base[0] ;move to intermediate waypoint of placing PTP drop_waypoint CONT Vel=100 % PDAT2 Tool[1] Base[0] ;move to approach waypoint of placing LIN drop_app Vel=1 m/s CPDAT3 Tool[1] Base[0] ;move to placing waypoint LIN drop Vel=0.3 m/s CPDAT4 Tool[1] Base[0] ;add object releasing logic here, such as "$OUT[1]=FALSE" halt ;move to departure waypoint of placing LIN drop_app Vel=1 m/s CPDAT3 Tool[1] Base[0] ;move to intermediate waypoint of placing PTP drop_waypoint CONT Vel=100 % PDAT2 Tool[1] Base[0] 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 démarre à 1 et s’incrémente de 1 après chaque itération jusqu’à dépasser la valeur de pick_cnt, moment auquel la boucle se termine. Lorsque i s’incrémente de 1, count s’incrémente de 3. [1+count] à [3+count] désignent l’ID des 3 points de passage planifiés à chaque fois dans la trajectoire planifiée totale.

Obtenir la trajectoire planifiée

;get planned path
MM_Get_PlanData(0,3,pos_num,vis_pos_num,status)

MM_Get_PlanData: La commande pour obtenir la trajectoire planifiée. Les points de passage Vision Move obtenus par cette commande contiennent, en plus des poses, des données Vision Move et des 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 la pose (en positions articulaires), le type de mouvement, l’ID d’outil, la vitesse, les données Vision Move de Mech-Viz, l’élément 1 des données de sortie personnalisées, ..., l’é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 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 bouclage

FOR count=1 TO pos_num
MM_Get_PlanJps(count,3,pick_point[count],move_type[count],tool_num[count],speed[count])
ENDFOR

Ligne 1: FOR indique une boucle for. count est utilisé pour contrôler le nombre d’itérations de la boucle (c’est-à-dire que count démarre à 1 et s’incrémente de 1 après chaque itération jusqu’à dépasser la valeur de pose_num, moment auquel 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: La commande MM_Get_PlanJps enregistre 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. L’ensemble de la commande 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 pick_point[count], move_type[count], tool_num[count] et speed[count].
Ligne 3: La boucle for se termine.

Calculer pick_cnt et residual

;parse pick cycle count, here suppose 3 points per planned path
pick_cnt = pos_num / 3
residual = pos_num - pick_cnt*3

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

Déterminer si une erreur s’est produite lors de la planification de trajectoire

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

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

Effectuer la prise et la dépose par bouclage

   FOR i=1 TO pick_cnt
      count=(i-1)*3
      Xpick_point1=pick_point[1+count]
      Xpick_point2=pick_point[2+count]
      Xpick_point3=pick_point[3+count]
      ;move to intermediate waypoint of picking
PTP pick_waypoint CONT Vel=50 % PDAT7 Tool[1] Base[0]
      ;follow the planned path to pick
PTP pick_point1 Vel=50 % PDAT1 Tool[1] Base[0]
PTP pick_point2 Vel=10 % PDAT2 Tool[1] Base[0]
      ;add object grasping logic here, such as "$OUT[1]=TRUE"
      halt
PTP pick_point3 Vel=50 % PDAT3 Tool[1] Base[0]
      ;move to intermediate waypoint of placing
PTP drop_waypoint CONT Vel=100 % PDAT2 Tool[1] Base[0]
      ;move to approach waypoint of placing
LIN drop_app Vel=1 m/s CPDAT3 Tool[1] Base[0]
      ;move to placing waypoint
LIN drop Vel=0.3 m/s CPDAT4 Tool[1] Base[0]
      ;add object releasing logic here, such as "$OUT[1]=FALSE"
      halt
      ;move to departure waypoint of placing
LIN drop_app Vel=1 m/s CPDAT3 Tool[1] Base[0]
      ;move to intermediate waypoint of placing
PTP drop_waypoint CONT Vel=100 % PDAT2 Tool[1] Base[0]
   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 démarre à 1 et s’incrémente de 1 après chaque itération jusqu’à dépasser la valeur de pick_cnt, moment auquel la boucle se termine. Lorsque i s’incrémente de 1, count s’incrémente de 3. [1+count] à [3+count] désignent l’ID des 3 points de passage planifiés à chaque fois dans la trajectoire planifiée totale.

Cette page est-elle utile ?

Laisser un commentaire

Exemple de programme 20: MM_S20_Viz_PlanAllVision

Présentation du programme

Description du programme

Cette page est-elle utile ?

Merci pour votre soutien !

Veuillez nous indiquer comment améliorer :

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