Alternatywy do acos, atan2 i sqrt z math.h

[deshipu]

Postanowiłem spróbować swoich sił w konkursie, w którym jest limit 1000 bajtów na wielkość firmware (po skompilowaniu, oczywiście). Udało mi się już pomniejszyć kilka z moim projektów (polecam spróbować, to jest dość fajna zabawa i dużo się przy tym można nauczyć), ale teraz chcę spróbować zrobić to z moim chodzącym robotem. Problemem są funkcje kinematyki odwrotnej, bo choć noga ma tylko trzy stopnie swobody i daje się policzyć wprost przy pomocy trygonometrii na poziomie liceum, to niestety wykorzystuję tam dość kosztowne na AVR typy float i funkcje acos, atan2 i sqrt. Wynik jest taki, że taka prosta funkcja:

float _solve_triangle(float a, float b, float c) {
   // Calculate the angle between a and b, opposite to c.
   a = abs(a);
   b = abs(b);
   c = abs(c);
   c = min(c, a + b);
   b = min(b, a + c);
   a = min(a, b + c);
   return acos((a * a + b * b - c * c) / (2 * a * b));
}

float _norm(float a, float b) {
   // Calculate the norm of a vector.
   return sqrt(a * a + b * b);
}

bool _inverse_kinematics(float x, float y, float z,
                        float *ankle, float *knee, float *hip) {
   // Calculate angles for hip, knee and ankle, and put them in those variables.
   // Return true on success, and false if x and y are out of range.
   double f = _norm(x, y) - COXA;
   double d = _norm(f, z);
   d = min(d, FEMUR + TIBIA);
   *hip = atan2(y, x);
   if (isnan(*hip)) { return false; }
   *knee = _solve_triangle(FEMUR, d, TIBIA) - atan2(-z, f);
   if (isnan(*knee)) { return false; }
   *ankle = _solve_triangle(FEMUR, TIBIA, d) - PI2;
   if (isnan(*ankle)) { return false; }
   return true;
}

Po skompilowaniu zajmuje ponad 3kB miejsca. Winne są oczywiście operacje na liczbach zmiennoprzecinkowych, a także użycie funkcji z math.h. Teraz zastanawiam się co mogę z tym zrobić.

Oczywistym rozwiązaniem byłoby użycie wszędzie przeskalowanych zmiennych całkowitych, oraz użycie własnych funkcji acos, atan2 i sqrt. Wydawałoby się, że powinny istnieć całe krocie bibliotek implementujących takie funkcje. Niestety, kilka godzin poszukiwań zaowocowało znalezieniem jedynie funkcji, które używają zmiennych zmiennoprzecinkowych, ale są podobno szybsze -- a to głównie dlatego, że korzystają z tablic wartości -- co powoduje, że zajmują dużo miejsca. Do tego większość z nich nie jest w postaci bibliotek (znalazłem tylko AVRfix i SpeedTrig), tylko w postaci kawałków kodu albo opisanych na jakimś blogu (na przykład https://oscarliang.com/enhanced-arduino-c-custom-math-library/) albo po prostu dorzuconych do projektów robotów lub quadcopterów. Wśród takich urywków kodu udało mi się znaleźć obiecujący kod na na atan2:

int fast_atan2(int y, int x) {
   int yabs, angle;
   const static int pi4 = (1 << 12), pi34 = 3 * (1 << 12); // note pi = 1<<14
   if (x == 0 && y == 0) {
return 0;
   }
   yabs = ABS(y);
   if (x >= 0) {
angle = pi4 - pi4 * (x - yabs) / (x + yabs);
   } else {
angle = pi34 - pi4 * (x + yabs) / (yabs - x);
   }
   if (y < 0) {
return -angle;
   }
   return angle;
}

I teraz pytanie -- czy ja po prostu nie umiem szukać? Czy istnieją takie biblioteki, a ja po prostu ich nie widzę? Oczywiście zawsze mogę usiąść, zakasać rękawy i napisać swoje własne przybliżenie acos bazujące na jakichś wielomianach i sqrt bazujące na sumie szeregu -- i w zasadzie chyba na tym się skończy. Ale jestem leniwy i wolałbym użyć gotowej biblioteki, którą ktoś już napisał, przetestował i którą utrzymuje, naprawiając błędy etc.

Alternatywnie, czy ktoś może ma pomysły jak tę samą kinematykę odwrotną policzyć w lepszy sposób, bez uciekania się do kosztownych funkcji, albo zna jeszcze jakieś rozwiązanie, o którym nie pomyślałem?

[ Dodano: 19-12-2016, 23:15 ]

Przy okazji, coś chyba jest nie tak z formatowaniem kodu na forum -- pousuwało mi wcięcia...

[marek1707]

Od razu mówię: nie znam takich bibliotek a jeśli trzeba coś na szybko policzyć, piszę to w razie potrzeby na int-ach w formatach 8.8, 16.16, 24.8 itp.

A wracając do Twoich obliczeń: nie zyskasz wiele zastępując bilioteczne liczenie atan2 czy cos czymś własnym używającym także zmiennego przecinka, bo samo rozwijanie w szereg jest raczej optymalne a prawdziwym ciężarem są 4 podstawowe działania (+ wiele funkcji dla nich pomocniczych, np. normalizacja lub też konieczne gdzieś w końcu konwersje float↔int itd). Wyjściem byłoby kompletne wyrzucenie jakichkolwiek operacji fp czyli nieużywanie typu float/double (w AVR to to samo). To na pewno odciąży kod a obliczenia na stałym przecinku 16 lub 32-bitowym i tak musisz mieć.

Ale żebyśmy mogli lepiej zrozumieć co da się ugrać, opisz nam do czego tej kinematyki odwrotnej naprawdę używasz. Jak rozumiem jest to tak: wyznaczasz żądane położenie końcowe stopy w jakimś ruchu, potem jakąś jej trajektorię żeby to położenie osiągnąć z obecnej pozycji, a następnie dla wielu punktów 3D leżących na tej drodze wyliczasz "zgięcia" stawów czyli wysterowanie serwomechanizmów.

Być może uproszczenie jest jeszcze większe i o żadnej trajektorii nie ma mowy. Jest za to zakładane położenie końcowe i wyliczone z kinematyki odwrotnej wysterowania serw konieczne aby tę pozycję osiągnąć. Jeżeli sterownik serw dostanie rozkaz przejechania wszystkim serwami w tym samym czasie to wbudowane w niego interpolatory (liniowe?) załatwiają sprawę trajektorii przez liniowe zmiany położenia każdego stawu. Czy tak?

Czy to znaczy, że cała sprawa sprowadza się do znalezienia pozycji kilku serw dla danej pozycji stopy w 3D? Czy tych pozycji jest dużo?

[deshipu]

A wracając do Twoich obliczeń: nie zyskasz wiele zastępując bilioteczne liczenie atan2 czy cos czymś własnym używającym także zmiennego przecinka, bo samo rozwijanie w szereg jest raczej optymalne a prawdziwym ciężarem są 4 podstawowe działania (+ wiele funkcji dla nich pomocniczych, np. normalizacja lub też konieczne gdzieś w końcu konwersje float↔int itd). Wyjściem byłoby kompletne wyrzucenie jakichkolwiek operacji fp czyli nieużywanie typu float/double (w AVR to to samo).

Właśnie dlatego raczej szukam funkcji działających na intach. Niestety jest z tym dosyć ciężko, chyba się skończy na własnym kodzie.

Swoją drogą, w blogu, do którego podlinkowałem, autor twierdzi, że użycie przeskalowanych intów nie daje dużych zysków, bo i tak potem po każdym mnożeniu trzeba je dzielić, na przykład przez 1000 i to zabija wszystkie zyski. Nadal nie rozumiem dlaczego zamiast tego nie dzieli przez 1024 za pomocą bit shifta...

Ale żebyśmy mogli lepiej zrozumieć co da się ugrać, opisz nam do czego tej kinematyki odwrotnej naprawdę używasz. Jak rozumiem jest to tak: wyznaczasz żądane położenie końcowe stopy w jakimś ruchu, potem jakąś jej trajektorię żeby to położenie osiągnąć z obecnej pozycji, a następnie dla wielu punktów 3D leżących na tej drodze wyliczasz "zgięcia" stawów czyli wysterowanie serwomechanizmów.

Być może uproszczenie jest jeszcze większe i o żadnej trajektorii nie ma mowy. Jest za to zakładane położenie końcowe i wyliczone z kinematyki odwrotnej wysterowania serw konieczne aby tę pozycję osiągnąć. Jeżeli sterownik serw dostanie rozkaz przejechania wszystkim serwami w tym samym czasie to wbudowane w niego interpolatory (liniowe?) załatwiają sprawę trajektorii przez liniowe zmiany położenia każdego stawu. Czy tak?

Czy to znaczy, że cała sprawa sprowadza się do znalezienia pozycji kilku serw dla danej pozycji stopy w 3D? Czy tych pozycji jest dużo?

Niestety tych punktów jest wiele i niekoniecznie się powtarzają. O ile ruchem nogi w powietrzu się nie przejmuję specjalnie i robię tak jak piszesz (wyznaczam punkt końcowy i pozawalam serwom dotrzeć do wyznaczonej końcowej pozycji jak im tam jest wygodnie) do tego punkt, w którym stopa jest stawiana na ziemi jest zawsze taki sam względem ciała robota -- o tyle kiedy noga jest na ziemi, to musi poruszać się wzdłuż liczonej na bieżąco trajektorii, zsynchronizowanej z pozostałymi nogami, z balansowaniem ciałem i z ewentualnymi zmianami kierunku i prędkości. Jedyne uproszczenie, które mi przychodzi do głowy, to to, że wszystkie punkty będą leżały wtedy na tej samej powierzchni -- zatem mam tylko dwa wymiary, nie trzy. Niestety taka dwuwymiarowa tablica nadal byłaby gigantyczna, chyba, że udałoby mi się zrobić jakąś sprytną interpolację.

[marek1707]

Oczywiście, skalowanie przez 2^N jest fajne i wiele moich współczynników jest tak właśnie robionych (np. 5.545 to 5678/1024), ale zyskujesz tu tylko na czasie, bo jeśli gdziekolwiek w programie użyjesz "prawdziwego" dzielenia, to ta funkcja trafi do kodu wynikowego. Jak rozumiem w konkursie 1000 bajtów (że też ludzie mają czas.. 🙂 ) liczy się głownie objętość, więc samo dzielenie jako takie nie jest złe chyba, że łatwo można go usunąć z całego programu a wzrost objętości spowodowany przesuwaniem inline będzie zyskiem. Zawsze można też napisać funkcję

uint16_t mnoz_dziel(x, m, n);

która zrobi operację x*m/(2^n) bez dzielenia.

Tak, tablicę 2D a już na pewno 3D odrzuciłem w przedbiegach. Raczej chodzi mi po głowie znalezienie funkcji (liczonych stałoprzecinkowo, np. 16.16 lub 8.8) bezpośrednio odwzorowujących tylko jedną współrzędną przestrzeni na pozycję jednego serwa. Gdyby udało się to przybliżyć prostym wielomianem np. 2 czy 3-stopnia, stworzyć zależności każdego serwa od każdej współrzędnej a potem współczynniki tych wielomianów zapisać w tablicy, to może było by to małe. Pozycja jednego serwa byłaby liczona jako suma(?) trzech wielomianów - każdy "wołany" dla innego wymiaru 3D. Jeśli liczysz wszystko względem korpusu, to wystarczy umiejętność "obsługi" tylko jednej nogi, prawda? Może nie będzie to szybsze niż bezpośrednie obliczenia na float, ale może czas poświęcić czas w imię 1000 bajtów? 🙂

Zacząłbym od zastanowienia się, czy rzeczywiście da się stworzyć trzy niezależne funkcje liczące położenie serwa danego stawu w zależności od tylko jednej współrzędnej x,y,z w 3D i czy proste złożenie (zsumowanie?) tych funkcji daje żądane położenie tego serwa. Być może w niektórych przypadkach (i zakresach ruchów) funkcje upraszczałyby się (z wystarczającym przybliżeniem) do stałych lub do liniowych?

Kurcze, a może jakaś sieć? Są wstrętne, ale gdyby jakaś niewielka i dobrze nauczona robiła wszystko co trzeba (tj. oddawała sensowne wysterowania wszystkich serw), to byłaby dobrym rozwiązaniem. Kodu niedużo, trochę RAMu..

[Polecacz 101]

Produkcja i montaż PCB - wybierz sprawdzone PCBWay!
   • Darmowe płytki dla studentów i projektów non-profit
   • Tylko 5$ za 10 prototypów PCB w 24 godziny
   • Usługa projektowania PCB na zlecenie
   • Montaż PCB od 30$ + bezpłatna dostawa i szablony
   • Darmowe narzędzie do podglądu plików Gerber
Zobacz również » Film z fabryki PCBWay

[deshipu]

Zacząłbym od zastanowienia się, czy rzeczywiście da się stworzyć trzy niezależne funkcje liczące położenie serwa danego stawu w zależności od tylko jednej współrzędnej x,y,z w 3D i czy proste złożenie (zsumowanie?) tych funkcji daje żądane położenie tego serwa. Być może w niektórych przypadkach (i zakresach ruchów) funkcje upraszczałyby się (z wystarczającym przybliżeniem) do stałych lub do liniowych?

To raczej nie da rady. Już pierwsze serwo (to przymocowane do ciała, z pionową osią obrotu) to w zasadzie czysty atan2(x, y). Koniec nogi musi wykreślać linię prostą, a wszystkie serwa dają łuki, więc niestety te wielomiany to będą właśnie przybliżenia atan2 i acos. Chyba jednak tędy będzie najszybciej.

Kurcze, a może jakaś sieć? Są wstrętne, ale gdyby jakaś niewielka i dobrze nauczona robiła wszystko co trzeba (tj. oddawała sensowne wysterowania wszystkich serw), to byłaby dobrym rozwiązaniem. Kodu niedużo, trochę RAMu...

To by niewątpliwie było bardzo ciekawe, wręcz rewolucyjne rozwiązanie, ale obawiam się, że niestety jest to dziedzina do której mi bardzo daleko. O ile jeszcze prosty perceptron rozpoznający cyfry zrobić potrafię, o tyle takie coś musiałoby mieć sporo wewnętrznych warstw (nieliniowe zależności), być może nawet jakąś niestandardową architekturę i generalnie boję się na samą myśl.

W każdym razie bardzo dziękuję za pomysły, poeksperymentuję jeszcze z tymi szeregami.

[ Dodano: 20-12-2016, 23:35 ]

Trochę poszukałem i okazuje się, że istnieje prześliczna rodzina algorytmów o nazwie CORDIC, które używają tylko dodawania, odejmowania, przesunięcia bitowego i ewentualnie małych tablic wartości. No i daje się nimi liczyć w zasadzie wszystkie funkcje trygonometryczne. Po prostu jakby ktoś to stworzył specjalnie dla mnie.

Tutaj jest przykład dla atan2: http://ch.mathworks.com/examples/matlab-fixed-point-designer/mw/fixedpoint_product-fixpt_atan2_demo-calculate-fixed-point-arctangent#1

[marek1707]

Czy możesz wypisać końcowe równania dla chociaż jednego serwa? Dla wszystkich? Może pomijając to (oczywiste?) z atan2. Do tego zapodaj jeszcze zakresy przestrzeni dla jakiej wyliczasz położenia względem węzła zamocowania do kadłuba - jeżeli to tylko podłoga (trochę wyżej lub trochę niżej) to może nie używasz całej dostępnej? Mniejsze zakresy to lepsze dopasowanie.. Z ciekawości chcę spróbować z 4 działaniami całkowitoliczbowymi.

Jeśli jeszcze gdzieś używasz fp to nie warto, ale jeśli tylko tutaj to może uda się nie dołączać math.h? 🙂

[deshipu]

Cały kod do IK jest w pierwszym poście. Zakresy to jakieś -150:150 dla x i y oraz -40:-20 dla z (dla ulatwienia liczę wszystko w milimetrach) ale w sumie to nie ma znaczenia, bo to są wszystko kąty, więc ważne są tylko proporcje. Postaram się napisać więcej wieczorem, teraz jestem na telefonie.

Tak, plan jest taki, żeby się całkowicie pozbyć math.h oraz wszelkich floatów.

[ Dodano: 21-12-2016, 19:30 ]

Dobra, to teraz więcej szczegółów. Robot wygląda tak: http://tote.rtfd.org

W sekcji "inverse kinematics" można zobaczyć dokładnie jak wygłada noga i jak jest liczona kinematyka odwrotna.

Każdy ze stawów porusza się w teorii w zakresie 180°, choć w praktyce tylko biodra mają taki zakres, pozostałe stawy mają bliżej 90°.

Możliwe pozycje końca nogi można przyjąć jako kwadrat z jednym narożnikiem w centrum robota, o boku jakichś 150mm.

Wymiary nóg to (w milimetrach):

#define COXA 12.0

#define FEMUR 42.0

#define TIBIA 38.5

połowa odległości pomiędzy biodrami to:

#define BASE 21.0

Nie wiem co jeszcze mogę napisać...

[ Dodano: 21-12-2016, 19:35 ]

Co do algorytmów CORDIC, zrobiłem pierwszą przymiarkę do atan2 (na razie w Pythonie, bo łatwiej debugować):

import math

def ANGLE(a):
   return int(a * 10000.0 + 0.5)

ATAN = [
  ANGLE(45.000), # atan(1/1)
  ANGLE(26.565), # atan(1/2)
  ANGLE(14.036), # atan(1/4)
  ANGLE( 7.125), # atan(1/8)
  ANGLE( 3.576), # atan(1/16)
  ANGLE( 1.790), # atan(1/32)
  ANGLE( 0.895), # atan(1/64)
  ANGLE( 0.448), # atan(1/128)
  ANGLE( 0.224), # atan(1/256)
  ANGLE( 0.112), # atan(1/512)
  ANGLE( 0.056), # atan(1/1024)
  ANGLE( 0.028), # atan(1/2048)
]

def atan2(y, x):
   result = 0
   if x == 0 and y == 0:
       return 0
   if y < 0:
       x, y = -x, -y
       result -= ANGLE(180)
   if x < 0:
       x, y = y, -x
       result += ANGLE(90)
   while x < 1<<14 and y < 1<<14:
       x <<= 1
       y <<= 1
   for i, atan in enumerate(ATAN):
       if y > x>>i:
           tx = x
           x += y>>i
           y -= tx>>i
           result += atan
   result += ATAN[-1] >> 1
   return result


for y in xrange(100):
   for x in xrange(100):
       iatan = float(atan2(y, x))/10000
       matan = math.degrees(math.atan2(y, x))
       print "x: %d, y: %d, iatan: %f, matan: %f, diff: %f" % (
           x,
           y,
           iatan,
           matan,
           iatan - matan,
       )

co daje całkiem ładne wyniki (wycinek gdzieś ze środka):

x: 14, y: 68, iatan: 78.372000, matan: 78.366366, diff: 0.005634
x: 15, y: 68, iatan: 77.561000, matan: 77.560438, diff: 0.000562
x: 16, y: 68, iatan: 76.750000, matan: 76.759480, diff: -0.009480
x: 17, y: 68, iatan: 75.967000, matan: 75.963757, diff: 0.003243
x: 18, y: 68, iatan: 75.183000, matan: 75.173520, diff: 0.009480
x: 19, y: 68, iatan: 74.404000, matan: 74.389011, diff: 0.014989
x: 20, y: 68, iatan: 73.621000, matan: 73.610460, diff: 0.010540
x: 21, y: 68, iatan: 72.838000, matan: 72.838081, diff: -0.000081
x: 22, y: 68, iatan: 72.083000, matan: 72.072080, diff: 0.010920
x: 23, y: 68, iatan: 71.318000, matan: 71.312648, diff: 0.005352
x: 24, y: 68, iatan: 70.563000, matan: 70.559965, diff: 0.003035
x: 25, y: 68, iatan: 69.807000, matan: 69.814197, diff: -0.007197
x: 26, y: 68, iatan: 69.084000, matan: 69.075498, diff: 0.008502
x: 27, y: 68, iatan: 68.357000, matan: 68.344011, diff: 0.012989
x: 28, y: 68, iatan: 67.630000, matan: 67.619865, diff: 0.010135
x: 29, y: 68, iatan: 66.903000, matan: 66.903178, diff: -0.000178
x: 30, y: 68, iatan: 66.203000, matan: 66.194056, diff: 0.008944
x: 31, y: 68, iatan: 65.507000, matan: 65.492595, diff: 0.014405
x: 32, y: 68, iatan: 64.808000, matan: 64.798876, diff: 0.009124
x: 33, y: 68, iatan: 64.109000, matan: 64.112974, diff: -0.003974

[ Dodano: 21-12-2016, 22:23 ]

Jakby ktoś miał kiedyś podobną potrzebę, to popełniłem małą biblioteczkę tutaj: https://bitbucket.org/thesheep/imath/src/tip/imath.c

Po skompilowaniu ma jakieś 514 bajtów. Pewnie by się dało jeszcze kilka bajtów ukroić.