Hey,
also ich denke das Du das nicht unbedingt benötigst bzw nur bedingt und Dir die ganze Sache nur unnötig erschwert.

Wenn man sich bewußt macht das das RoboterKOS den tatsächlichen Körper des Roboters darstellen soll.
Dein RoboterKOS endet bei den geometrischen Gegebenheiten Deines Bots, die Fußpunkte stellen gleichzeitig (oder auch nur) Koordinaten im RaumKOS dar. Damit hast Du eine Verbindung der beiden Systeme. Da sie dadurch verbunden sind braucht man eigentlich doch nichst mehr transformieren. Man sieht das es nur wichtig ist in welche Richtung Du rechnen möchtest - also vom RaumKOS zum RoboterKOS, oder von RoboterKOS zu RaumKOS - das Ergebnis sollte in beiden Fällen das gleiche sein, und Dir eine Koordinate im RaumKOS liefern nicht im RoboterKOS, denn dort kennst Du bereits die die Du benötigst. Aber nicht vergessen es ist ein Koordinate innerhalb deines beschriebenen Systems, nicht mehr und nicht weniger.
Leider kann ich Dir zu Vektoren und Rotationmatrixen auch noch nicht sonderlich viel sagen.


nfg