Aus (1) ergibt sich zudem
(3)
r²-h² = a²(cos²(alpha)-sin²(alpha)) + b²(cos²(beta)-sin²(beta))
= a²*cos(2*alpha) + b²*cos(2*beta)
= a²*cos(2*beta+2*delta) + b²*cos(2*beta)
= (b²+a²*cos(2*delta))*cos(2*beta) - a²*sin(2*delta)*sin(2*beta)

Wobei delta=alpha-beta sich bis auf das Vorzeichen und mod 2*pi ergibt aus
(2)
cos(delta) = (r²+h²-a²-b²)/(2ab)

(3) ist eine quadratische Gleichung in cos(2*beta) bzw sin(2*beta). Damit haben wir die Transformation, die wir brauchen; bis auf etwas Gefummel mit den Vorzeichen.

Wenn ich's richtig verstanden hab, willst du eine Transformation von Raumkoordinaten in deinen Koordinatenraum und umgekehrt.

Was dir als Raumkoordinaten genehn ist, weiß ich jetzt nicht. Anbieten tun sich kartesische, Kugel- oder Zylinderkoordinaten.
Was der Stummel am Ende das Armes macht/kann ist mir auch nicht ganz klar. Ist wohl ein Greifer/Magnet, mit dem aus ner bestimmten Richtung an ein Objekt rangefahren werden muss.

Der Koordinatenraum der ersten drei Freiheitsgrade ist ein
K = S1 x Ia x Id
wobei S1 die 1-Spähre (also Kreis) und Ia, Ib Intervalle sind und x das kartesische Produkt, also
phi in S1
alpha in Ia
delta in Id

Die Raumkoordinaten transformierst du auf Zylinderkoordinaten und dann in deinen Koordinatenraum und erhälst nen Vektor (phi, alpha, delta)
Wenn dein Arm bei (phi0, alpha0, delta0) steht und er nach (phi1, alpha1, delta1) bewegt weden soll, dann bewegst du ihn einfach auf der kürzesten Verbingung im Koordinatenraum, wobei der Koordinatenraum mit der Standard-Metrik versehen sei. Dadurch bewegt sich der Arm von Punkt 0 nach 1. Die kürzeste Verbindung im Koordinatenraum (Strecke) wird aber *nicht* zu einer Strecke im R³.

Jenachdem wie dein Arm aufgebaut ist, hat der Koordinatenraum ne andere Topologie. Wenn phi nicht komplett schwenken kann muss man S1 irgendwo aufschneiden und bekommt
K = Iphi x Ia x Id
Und wenn der Arm auf die andere Seite überschwenken kann ohne sich zu drehen, dürfte es
K = S2 x Id
sein, wobei S2 = 2-Späre (Kugeloberfläche). In dem Fall ist phi nur noch eindeutig mod pi, nicht mehr mod 2pi.