Symulator manipulatora i kinematyka odwrotna

[matelekt]

Witam,

Stworzyłem sobie prostego robota z wykorzystaniem bibliotek OpenGl:

tu jego kod:

void robot2()
{
glColor3d(1, 0, 1);
glPushMatrix();
//
glRotated(teta, 0, 0, 1);
glTranslated(0, 0, -10);
os();
podstawa2(10, 20);
glTranslated(0, 0, 10);
//
glRotated(-90, 1, 0, 0);
podstawa2(10,100);
glTranslated(0,0,100);
glRotated(90, 1, 0, 0);
//
glRotated(psi, 0,0,1);
glTranslated(0, 0, -10);
os();
podstawa2(10, 20);
glTranslated(0, 0, 10);
//
glRotated(-90, 1, 0, 0);
podstawa2(10, 100);


glPopMatrix();
}

Sprawa wygląda tak, że chciałem do niego dodać możliwość poruszania się ale z wykorzystaniem kinematyki odwrotnej. Wyznaczyłem równania, przekształciłem i wyszło takie coś:

cq = (double)((px *px) + (py *py) - (100*100) -(100 *100)) / (double)(2 * 100 * 100);
s2 =  -sqrt(1-cq*cq);
psi = atan2(s2, cq)* (180 / 3.142);

k1 = 100 + 100*cq;
k2 = 100*s2;

teta = atan2(py*k1 - k2*px, px*k1 - k2*py)*(180/ 3.142);

no i teraz tak, ramiona poruszają się ale nie tak jak bym chciał. To znaczy, gdy zwiększam wartość współrzędnej na x robot przesuwa sie po y natomiast gdy zwiększam y końcówką robota porusza się po x i po y.

Nie wiem czy problem jest w osobliwości robota czy może coś z złymi osiami współrzędnych. Tak czy inaczej jeśli ktoś mógłby mnie nakierować na dobra drogę, dać jakiś trop to byłbym bardzo wdzięczny.

Robota i równania zaciągnąłem ze strony: http://www.robotyka.com/teoria.php/teoria.75