Hallo zusammen,
ich habe folgende Sache wo ich nich weiter komme.
Applikation:
Roboter greift die Teile von der Platte/Tisch:
Die Position wird von der Kamera ermittelt und in ein Frame in Roboter geschrieben.
Die Vorpositionen werden auch von der Kamera berechnet (nur offset in Z)
Ab und zu ist die Position außerhalb von der Roboterreichweite.
Die Idee war die Pos in Achwerte umzurechnen und dann gegen Softwareendschalter prüfen.
Es sind 5 Positionen innsgesamt
2 Vorpos Hinfahren
Greifposition
2 Pos zum rausfahren
Alle Variablen sind global declariert.
Bei der Berechnug mit dser Inverse funktioniert es nur bei der erste und letzte Position. (Physikalisch geleich)
Bei den anderen stehen einfach keine Werte da. Die INVERSe funktion gibt kein Fehler aus.
Es sind einfach keine Werte in Achspos drin.
Hier Quellcode
CORR_FRAME_100_GERADE-> Kameraframes
;-- Posen Bewegungpositionen umschreiben
XP101_VorHol_gerade = CORR_FRAME_100_GERADE ;entry pose
XP102_VorHol_gerade = CORR_FRAME_200_GERADE ;pre pose
XP105_Hol_BigPack_gerade = CORR_FRAME_300_GERADE ;grasp pose
XP106_VorHol_gerade = CORR_FRAME_400_GERADE
XP107_VorHol_gerade = CORR_FRAME_500_GERADE
;-- Umrechnung von Base Koordinaten in World
;$target_status=#best
CALC_Meldungsnummer = 0
POSE_100_WORLD_GER = INVERSE(xP101_VorHol_gerade,WORLD_CALC_GER,CALC_Meldungsnummer)
POSE_200_WORLD_GER = INVERSE(xP102_VorHol_gerade,WORLD_CALC_GER,CALC_Meldungsnummer)
POSE_300_WORLD_GER = INVERSE(xP105_Hol_BigPack_gerade,WORLD_CALC_GER,CALC_Meldungsnummer)
POSE_400_WORLD_GER = INVERSE(xP106_VorHol_gerade,WORLD_CALC_GER,CALC_Meldungsnummer)
POSE_500_WORLD_GER = INVERSE(xP107_VorHol_gerade,WORLD_CALC_GER,CALC_Meldungsnummer)
IF ((POSE_100_WORLD_GER.A1 > $SOFTP_END[1]) OR (POSE_100_WORLD_GER.A1 < $SOFTN_END[1])OR(POSE_100_WORLD_GER.A2 > $SOFTP_END[2])OR(POSE_100_WORLD_GER.A2 < $SOFTN_END[2])OR(POSE_100_WORLD_GER.A3 > $SOFTP_END[3])OR ( POSE_100_WORLD_GER.A3 < $SOFTN_END[3] ) OR ( POSE_100_WORLD_GER.A4 > $SOFTP_END[4] ) OR ( POSE_100_WORLD_GER.A4 < $SOFTN_END[4] )OR ( POSE_100_WORLD_GER.A5 > $SOFTP_END[5] )OR( POSE_100_WORLD_GER.A5 < $SOFTN_END[5] )OR( POSE_100_WORLD_GER.A6 > $SOFTP_END[6] )OR ( POSE_100_WORLD_GER.A6 < $SOFTN_END[6])) THEN
$OUT[103]=TRUE
ELSE
$OUT[103]=FALSE
ENDIF
Ausschnitt aus config.dat
DECL FRAME P101_VorHol_gerade ;entry pose
DECL FRAME P102_VorHol_gerade ;pre pose
DECL FRAME P105_Hol_BigPack_gerade ;grasp pose
DECL FRAME P106_VorHol_gerade
DECL FRAME P107_VorHol_gerade
DECL E6POS XP101_VorHol_gerade={X -104.000,Y -747.000,Z 718.000,A -6.00000,B 39.0000,C 129.000}
DECL E6POS XP102_VorHol_gerade={X -104.000,Y -747.000,Z 565.000,A -6.00000,B 39.0000,C 129.000}
DECL E6POS XP105_Hol_BigPack_gerade={X -128.000,Y -783.000,Z 541.000,A -6.00000,B 39.0000,C 129.000}
DECL E6POS XP106_VorHol_gerade={X -104.000,Y -747.000,Z 565.000,A -6.00000,B 39.0000,C 129.000}
DECL E6POS XP107_VorHol_gerade={X -104.000,Y -747.000,Z 807.000,A -6.00000,B 39.0000,C 129.000}
DECL E6AXIS POSE_100_WORLD_GER={A1 80.9880371,A2 -136.121872,A3 148.016113,A4 29.0525970,A5 36.4547234,A6 84.6455688,E1 0.0,E2 0.0,E3 0.0,E4 0.0,E5 0.0,E6 0.0}
DECL E6AXIS POSE_200_WORLD_GER={}
DECL E6AXIS POSE_300_WORLD_GER={}
DECL E6AXIS POSE_400_WORLD_GER={}
DECL E6AXIS POSE_500_WORLD_GER={A1 80.9880371,A2 -138.564133,A3 140.878235,A4 24.0672283,A5 45.0359192,A6 91.2044144,E1 0.0,E2 0.0,E3 0.0,E4 0.0,E5 0.0,E6 0.0}
könnt ihr vieleicht mein Fehler sehen
