Das ganze erinnert mich an einen Teil des Leistungsumfangs von COSIMIR
https://de.wikipedia.org/wiki/COSIMIR

Ggf. hilft es sich mal da die Diplom und Doktorarbeiten anzusehen und deren Litteratur Verzeichnisse.
Bsp.:
https://eldorado.tu-dortmund.de/bits...ssertation.pdf

Dort wurde die (Roboter)Zelle als Basis genommen, da sich ja der Roboter und jedes Objekt darin befinden.
Alle KS wurden also ins Welt-KS transformiert.
Wobei es damals (bei der Version mit der ich gearbeitet habe) noch keine Kollisionserkennung gab für Teile des Roboter die nicht zum Greifer gehörten.

Jedes Objekt wurde in COSIMIR als hiraschischer Baum aus 4x4 Matritzen beschrieben, so das man den Objektursprung in allen 6 Freiheitsgraden auf den Welt-KS Ursprung bezog und auf den Objektursprung dann Punkte und Flächen der Objekte.
Bei einem Roboter selbst wurde so dann auch die kinematische Kette von der Basis bis zum TCP aufgebaut.
Für inverse Kiniematik und Endstellungen waren dann Eigenschaften in den entsprechenden Objekten hinterlegt.