Die Bewegung sollte doch theoretisch kein Problem sein, Du hast ja 2 Koordinatensysteme:

1. Das des Raumes in dem du dich Bewegst
2. Das des Roboters
(3-9). Die der einzelnen Beine, sind aber Ausschnitte von 2.

Das erste System gibt die Bewegung deines Torsos in X,Y,Z Richtung und die Ausrichtung des Roboters angegeben. Es wird ein Vektor zwischen der Aktuellen Position des Roboters und dem Zielpunkt gespannt (theoretisch sind hier auch Kurven möglich). Also wie wenn sich der Roboter auf einer Karte von Punkt A nach Punkt B bewegen würde und in Richtung Süden schaut.

Jetzt wird ein kleines Teilstück der Bewegung ins zweite System umgewandelt. Dieses liegt um alle Achsen gedreht im ersten System und dem entsprechend wird das Teilstück von System 1 in 2 umgewandelt. Jetzt können auf dieses Teilstück in System 2 die Korrekturmaßnahmen (vom vorherigen Teilstück) aufaddiert werden. Und diese Ergebnisse werden dann als delta X, delta Y,..... an die IK übergeben.

Die IK von jedem Bein wird zum Mittelpunkt des Torsos berechnet.

Die Randbedingungen von den Teilstücken werden durch die Maximalgeschwindigkeit des Roboters (evtl auch Minimalgeschwindigkeit wegen Rundungsfehlern als Drehung um 0,00001° pro step) bzw einer Vorgegebenen Geschwindigkeit festgelegt.


Damit sollten alle deine Bedingungen erfüllt sein und auch das laufen einer Kurve müsste machbar sein, solange die Teilstücke nicht zu groß sind, sonst wirds ein Dreieck .
Vorteil von dem Ganzen, du hast eine einfach Schnittstelle mit dem ersten Koordinatensystem auf dem Sogar noch Mapping integriert werden kann etc.

Frage bleibt die Aufteilung zwischen den Controllern. Die Beste Möglichkeit wird wohl sein, IK und Servoansteuerung auf einen Controller und alles was zur Berechnung der Lage und das addieren des Korrekturfaktors auf das "Gehirn" zu verteilen.
Dadurch bleibt dein IK+Servocontroller System fertig und muss "nie mehr" verändert werden (wers glaubt ). Die ganzen Änderungen im Code werden dann nur im "Gehirn" notwendig, bedeutet, hast du z.B. einen anderen Beschleunigungssensor zur Korrektur Berechnung verwendest muss an der IK nichts mehr geändert werden