Hexapot IK Rotation funktioniert nur in zwei Dimensionen

Hallo,

ich habe folgendes Problem:

ich habe die Ortskoordinaten meiner Fußpunkte im globalen Koordinatensystem vorliegen, und möchte diese durch multiplikation mit den drei Rotationsmatrizen um die kanonischen Basisvektoren rotieren.
wenn ich das mache, dann erhalte ich bei einer Drehung um die x Achse eine sehr schöne neigung nach R/L, bei einer Drehung um die Z Achse eine drehung nach R/L, aber wenn ich um die Y-Achse drehen will, dann nigt er sich nicht vor und zurück, sondern die beine laufen auf einem Radius nach vorne und dann wieder nach hinten.

hier der Code: sx und cx stehenfür Sinus und Cosinus des Drehwinkel um die x-Achse usw.

Code:

---

        for(uint8_t j=1;j<=6;j++)                                                //globale Koordinaten drehen: R_x(phix)*R_y(phiy)*R_z(phiz)*glposition
        {
                glposition[j][0]=(cy*cz)*glposition[j][0]+                (-cy*sz)*glposition[j][1]+                (sy)*glposition[j][2];
                glposition[j][1]=(cx*sz+cz*sx*sy)*glposition[j][0]+(cx*cz-sx*sy*sz)*glposition[j][1]+(-cy*sx)*glposition[j][2];
                glposition[j][2]=(sx*sz-cx*cz*sy)*glposition[j][0]+(cx*sy*sz+cz*sx)*glposition[j][1]+(cx*cy)*glposition[j][2];
        }
        return ready;

---

vlt. kenn ja einer von euch das problem und kann mir weiterhelfen.
ich habe einen Fehler der globalen Koordinaten ausgeschlossen, da die anderen rotationen ja funktionieren...

vielen Dank mfg WarChild

noch als kleine ergänzung:
die Translation funktioniert in allen drei dimensionen.

So hab das Problem gefunden:
die Transformation von den Globalen zu den lokalen Koordinaten arbeitete mit einem Falschem verschiebungsvektor. Hab die 0 bei 210mm veriebung in x Richtung vergessen. Daher dachte der Roboter eine beine stünden sehr dicht beieinander und deshalb bekamen alle BEine nahezu die gleichen transformierten Koordinaten.

mfg WarChild