im grunde brauchen wir hierfür nur noch Geber und einen PI Regler
naja, "nur noch" ... War ja nur ein gutgemeinter Gedanke.

Wie sollte ich denn mit dem Cosinussatz Winkel über 180° definieren ?
Es gibt immer noch eine Zweideutigkeit in eurem System. Wenn man die Basis gedacht mit dem vorletzten DOF verbindet, kann der DOF dazwischen (in eurer ersten Skizze R2) immernoch über oder unter dieser Verbindungslinie liegen. Das muss man über eine Fallunterscheidung abfangen. Man berechnet also die Winkel für dieses Dreieck und kann sie dann auf zwei Arten legen. Ich dachte aber du meinst eh etwas anderes, nämlich den ersten rotatorischen DOF. Um aus x/z-Koordinate den Drehwinkel auf 360° zu berechnen, verwendet man den Tangens in einer Vierquadrantenversion. Er heisst meistens Atan2. Damit kann man dann auf den Vollkreis rechnen, weil die Eingangswerte nichtmehr als Bruch in den Tangens gehen sondern als zwei getrennte Argumente.