Hmm, immer noch nicht weiter gekommen?

Ich kenne mich mit Deiner Problemstellung nicht aus. Aber ein Tipp von mir:
Die Position auf einer Kreisbahn kannst Du über den Radius berechnen. Stichwort: Berechnungen im Kreis oder Kreisberechnungen.
Damit kannst Du die Positionen ausrechnen, die Du auf einer Kreisbahn, ausgehend von einem Mittelpunkt (die Achse eines Motors) unter Angabe des Radius (Armlänge/Armteillänge bis zum nächsten Gelenk), maximal erreichen kannst.
Ist eine Position nicht erreichbar, müsste man auf den nächsten Drehpunkt übergehen (zum Beispiel einen zurück) und von dort aus rechnen.

Gemacht habe ich das noch nicht, aber das wäre mein Ansatz.

MfG, Moppi