Für KUKA liegt auf jedem Installationsmedium das Programm kue_weg.src bei. Da ist inbesondere auch eine Implementierung von INV_POS() in KRL enthalten und man kann da abschreiben. Ansonsten ist das keine Raketenwisssenschaft. INV_POS ist nichts anderes als die Invertierung einer 4x4 homogenen Transformationsmatrix und das findet man auch in Google/Wikipedia. Einfach mal mit "homogene Transformationsmatrix" googlen. Grob geht es so: Rotationsanteil invertieren durch transponieren, transponierten Rotationsanteil auf den Positionsanteil anwenden.
Fubini
(1) Eine Transformationsmatrix (4x4) besteht aus mehreren Komponenten(Rotationsmatrix, Vektor und Füller (0,0,0,1):
Rotationsmatrix (mit (Vektor)):
a11, a12, a13, (X)
a21, a22, a23, (Y)
a31, a32, a33, (Z)
0,0,0,1
(2) entspricht der Rotationsmatrix und wird durch Transponieren (Zeilen mit Spalten vertauschen) erreicht Vektor und Füller ignorieren):
a11, a21, a31, a14
a12, a22, a32, a24
a13, a23, a33, a34
(erster Schritt)
Hintergrund:
eine Transformationsmatrix gibt die Verschiebung und Verdrehung zwischen Bezugs- und Zielkoordinatensystem an! Durch das Invertieren wird Ziel- und Bezugskoordinatensystem vertauscht
(Bezugskoordinatensystem: Zeilen; Zielkoordinatensystem: Spalten - jetzt verkehrt rum)
Berechnung der Verschiebung (vierte Spalte):
Hier handelt es sich eine Multiplikation einer 3x3 Matrix mit einem Vektor!
daraus ergibt sich (nach (2) und vierte Spalte aus (1)):
a14 = -(a11*X+a21*Y+a31*Z)
a24 = -(a12*X+a22*Y+a32*Z)
a34 = -(a13*X+a23*Y+a33*Z)
Es geht ja in die andere Richtung, daher "-"
Bei der 4.Zeile ändert sich nichts
Zum besseren Verständnis ist immer eine kleine Skizze hilfreich!
Ich hoffe, dass ich nicht weiter verwirrt habe
Du musst ein Benutzerkonto haben um einen Kommentar hinterlassen zu können
__ERROR_41__
. Vor allem der Tipp mit dem "kue_weg.src" war sehr hilfreich 
.