Gentle : Robotyka i edukacja - Igor Zubrycki: Moje zdanie na temat robotyki i edukacji robotyki. Informacje na temat ROS, Linuxa, miękkiej robotyki (soft robotics)

W tym poście pokażę, jak możemy wykorzystać Jakobian, do wyliczenia kinematyki odwrotnej i kinematyki prędkości manipulatora o dwóch stopniach swobody.

Wyobraźmy sobie robocika o 2 stopniach swobody, o 2 złączach obrotowych. Robot jest opisany równaniami kinematyki prostej, gdzie położenie końcówki (x ) jest funkcją zmiennych t1, t2

$x_k= l_1 * cos( t_1)+l_2 * cos (t_1+t_2) \\ y_k = l_1 * sin (t_1) + l_2 * sin (t_1+t_2)$

Opiszemy go w p5js funkcją robocik. W funkcji opisujemy położenie 2 złącza (x1,y1) i końcówki (x_k,y_k) i wizualizujemy za pomocą linii. Żeby było lepiej widać, robocik ma za końcówkę czerwony kwadrat.

Niestety p5js korzysta z lewoskrętnego układu współrzędnych i ma standardowo oś y skierowaną w dół, stąd przy rysowaniu linii musiałem dać znak - przy wspólrzędnych y.

W funkcji draw, dla każdej klatki animacji zadajemy ruch robocikowi. Żeby robot nie stał w miejscu wykorzystuję funkcję noise, która zmienia wartość w płynny ale losowy sposób.

Teraz zastanówmy się, jak można sprawić, aby robot jechał do pozycji myszki. To znaczy, aby położenie końcówki zbiegało do pozycji myszki. To zagadnienie ma dużo wspólnego z kinematyką odwrotną, jednak zamiast rozwiązywać je wprost, wykorzystamy Jakobian manipulatora.

Okazuje się bowiem, że kinematykę odwrotną możemy rozwiązać iteracyjnie, w kolejnych krokach iteracji licząc takie równanie.

$\vec{q_{next}}=\vec{q_{current}}+ J^{-1} (\vec{x_{goal}}-\vec{x(q_{current})})$

Równanie oznacza, że każdej klatce animacji będziemy obliczać nową wartość wektora q (wartości złaczowych) poprzez poprawienie starej wartości złączowej o poprawkę wynikającą z pomnożenia odwrotności Jakobianu przez błąd pozycji.

Wystarczy “tylko” policzyć odwrotność Jakobianu. Myśle, że same obliczenia przedstawię na filmie, tutaj napiszę Jakobian

$J=\frac{ \partial \vec{x}}{\partial \vec{q}}=\begin{bmatrix} -l_1 sin(t_1) & -l_2 sin (t_1+ t_2) \\ l_1 cos(t_1) & l_2 cos( t_1+t_2) \end{bmatrix}$

Jego odwrotność to:

$J^{-1}= \frac{1}{l_1 l_2 sin (t_2)} \begin{bmatrix} l_2 cos( t_1+t_2) & l_2 sin (t_1+t_2) \\ -l_1 cos (t_1)& -l_1 sin(t_1) \end{bmatrix}$

Zauważmy, że jeśli sin(t2)=0 to nie będziemy mogli policzyć odwrotności Jakobianu. Poza dla której to się dzieje, nazywa się konfiguracją osobliwą i za pomocą przedstawionego wyżej algorytmu nie policzymy wtedy kinematyki odwrotnej.

Ok, przedstawmy nasz kod jako animację w p5js.

Do tego potrzebować będziemy parę funkcji pomocniczych:

funkcji błędu, liczącej o ile aktualna pozycja końcówki różni się od pozycji pożądanej
odwrotności jakobianu. Jako, że w p5js nie ma obliczeń macierzowych, zamiast tego policzymy dwa wektory wierszy, które będziemy mnożyć razy błąd
wreszcie funkcji uaktualniającej wartości złączowe (update_q)

Pomiędzy funkcjami dane będziemy przesyłali jako wektory czyli obiekt klasy p5.vector, może trochę mylić, że pierwszy element takiego wektora to .x drugi to .y, nawet jeśli to będzie wektor zmiennych złączowych q

Podsumowująć, policzyliśmy odwrotność Jakobianu, a to pomogło nam na bieżąco animować pozycją robota w zależności od pozycji myszki.