[Quadropod] Krok

[Mechano]

Siemka

Zachęcony podobnym tematem kolegi Wojcik98 postanowiłem również pochwalić się nad czym aktualnie pracuję.

Od dłuższego czasu chodziła mi po głowie myśl zbudowania czegoś innego niż linefollower czy minisumo, najlepiej czegoś kroczącego. Jako, że nie przepadam (to dosyć lekkie słowo) za pająkowatymi a na humanoida jestem, na razie, za cienki w uszach postawiłem na quadropoda. Dodatkowo dużo dała mi seria Jamesa Brutona o budowie OpenDoga (ale wiadomo, nie ten budżet i możliwości).

Założenia projektu były dosyć jasne:

• użycie tanich mikroserw,
• układ nóg psa,
• użycie platformy kompatybilnej z arduino,
• możliwości rozwoju,

Mechanika

Jako napędy wybór padł na serwa TowerPro sg 90. Trochę się obawiałem, czy będą wystarczające ale jak oszacowałem wagę całości na ~500g stwierdziłem nie no, dadzą radę! Nogi są zbudowane w oparciu o pocięty profil aluminiowy 25x25x2 (taki miałem a nie chciałem robić nowych kosztów) podstawa bo blacha stalowa, chyba z obudowy zasilacza atx. Wszystko skręcone śrubami M2 i wkrętami do orczyków, które przyszły razem z serwami. Płytka z elektroniką przykręcona na szczycie za pomocą 4 śrub i drewnianych dystansów.

Taka budowa sprawuje się świetnie poza jednym miejscem. Początkowo całość zamiast śrub w nogach była sklejona na gorąca ale to się często łamało więc szybko to zastąpiłem śrubami. Feler jednak występuje w miejscu połączenia serw w "udzie". W pierwszej wersji orczyk był bezpośrednio przyklejony do obudowy serwa ale to też nie dawało rady więc wymyśliłem coś takiego:

Orczyk jest przykręcony do kawałka drewna dwoma śrubami a on do serwa jest przymocowany za pomocą wkręta. Takie rozwiązanie działa ale te drewienka też czasami nie dają rady i pękają. Na razie działa ale nie za bardzo mam pomysł jak można by to jeszcze usprawnić.

Elektronika

Jako mózgu postanowiłem użyć czegoś innego niż dotychczas i wybór padł na płytkę blue pill z mikrokotntrolerem STM32f103. Jestem przekonany, że mniejsze arduino też dało by radę ale po prostu chciałem tego spróbować i zostawić sobie dużą rezerwę zasobów. Do komunikacji bezprzewodowej używam modułu hc05. Drugi jest w przerobionym symulatorze RC (jako aparatura, razem z resztą z nrf24l01). Jako bajer dorzuciłem jeszcze miejsce na moduł IMU MPU6050 ale na razie nie udało mi się go jeszcze uruchomić na tej płytce. W planach na przyszłość jest dodanie czujnika odległości i czujników podłoża na stopy. Blue pill jest zasilana z 3,3V więc użyłem LF33, serwa zasilam prosto z 5V z zasilacza atx przez złącze xt90.

Program

I tu się zaczynają dla mnie prawdziwe schody, na ten moment działają wszystkie serwa, i moduł bluetooth. Zacząłem prace nad kinematyką odwrotną i tutaj dużo pomagają filmy z OpenDogiem (chociaż mój przypadek jest trochę inny, chyba prostszy). Mam kilka niewiadomych, które muszę pozbierać i w jednym wpisie Wam je przedstawię, mam nadzieję, że pomożecie wyjaśnić wątpliwości.

Plany co do programu są takie, żeby uruchomić kinematykę odwrotną, dodać interpolację (znowu z filmu Brutona) i spróbować utrzymywać dynamiczną równowagę. Na całość patrzę po kawałku, nie zbyt wiele na raz bo się w tym pogubię.

Jak na razie jestem bardzo zadowolony z tego co wyszło, cały czas do przodu!

Jeżeli macie już jakieś przemyślenia albo widzicie jakieś błędy nowicjusza to chętnie posłucham, z góry dzięki!

 

Edytowano Lipiec 29, 2019 przez Mechano

[Mechano]

Zebrałem się wreszcie i postaram się Wam opisać mój problem dotyczący początków z kinematyką odwrotną.

I tutaj małe ostrzeżenie: jeżeli chodzi o matematykę to powiedzieć, że jestem kiepski to jak nic nie powiedzieć. Jestem kompletnie nie matematycznym umysłem, więc przychodzi mi to bardzo ciężko i często nie widzę rzeczy "oczywistych". Jeżeli nie starcza Ci cierpliwości  to wybacz ale ja na prawdę nie trolluję, po prostu nie wiem.

Moim punktem wyjścia jest odcinek OpenDog #9 o kinematyce odwrotnej w osiach XYZ. Robię to razem z nim od tyłu i mam wyznaczoną "długość nogi". Oba segmenty każdej nogi są równej długości (na tej samej zasadzie jak w linku). Liczę to w ten sposób i działa:

Moja wątpliwość dotyczy następnego etapu. Według filmu mam potem obliczać kąt w "udzie" (w tej samej płaszczyźnie to w kolanie). Tylko nie za bardzo wiem, od czego ma on zależeć? Inaczej mówiąc: do czego ma się on odnosić? Do płaszczyzny poziomej (czyli podstawy robota)? Chyba, żeby zrobić inną kolejność i najpierw obliczyć długość nogi a potem na tej podstawie policzyć kąt w udzie?

Zrobiłem kilka rysunków i obliczeń, zaznaczyłem tam co wiem a gdzie jest mój problem, mam nadzieję, że wyjaśnią ewentualne wątpliwości.

Tutaj dwoma znakami zapytania zaznaczyłem dwie możliwości, które widzę ale nie wiem czy którakolwiek jest poprawna.

A tutaj wklejam jeszcze trzeci etap, o którego na razie nie zaimplementowałem ale zostawiam do oceny czy tutaj też dobrze myślę.

Jeżeli napisałem coś jeszcze nie jasno to spróbuję jeszcze raz wytłumaczyć.

Z góry dzięki!

[wn2001]

Witaj, gratuluję ciekawego projektu! Opisałem bardzo podobnego do Twojego robota, wkrótce opis powinien zostać zaakceptowany (Garfield) 🙂 Nie oglądałem tego konkretnego filmu, ale pan James Bruton, jak wspomniał kiedyś Kolega @deshipu nie jest technikiem i mimo, że tworzy wspaniałe projekty to również pogubił się kiedyś w którymś z filmów o kinematyce odwrotnej 😉

Ale do rzeczy:

Zakładam, że chodzi Ci o IK dla każdej z nogi osobna (bo może być też dla całego korpusu, ale sam nie wiem jak to zrobić 😉). Zapomnij o trzech wymiarach - masz płaszczyznę XY, czyli dwuwymiarową (noga nie wychyli się przecież "w bok"). Teraz pozycję każdej z nóg odnosić można do punktu obrotu pierwszego serwa (uda, nie kolana), powiedzmy, że w X to będzie oś biegnąca w prawo, Y - w górę, a punkt 0,0 to właśnie punkt obrotu

Teraz zadanie dla Ciebie - narysuj to, co napisałem i jak to rozumiesz - dla jednej nogi. Ładnie, kolorami zaznacz punkt obrotu uda, udo, kolano, ramię odchodzące od kolana, spróbuj zaznaczyć kąty, które Cię interesują. Narysuj to w jakiejś losowej pozycji, ważne, aby nie była ona charakterystyczna np. ramiona wyprostowane, zgięte w kącie prostym itd.

Pozdrawiam 🙂

[deshipu]

Może to ci pomoże: https://tote.readthedocs.io/en/latest/ik.html

Generalnie, kąt w kolanie daje ci długość nogi (i liczysz go tak jak napisałeś, z wzoru cosinusów), a kąt w udzie pozycje jej końca i liczysz go z atan2.

Edytowano Lipiec 31, 2019 przez deshipu

[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

[Treker (Damian Szymański)]

@Mechano dzięki za publikację ciekawego workloga 😉

6 godzin temu, wn2001 napisał:

Opisałem bardzo podobnego do Twojego robota, wkrótce opis powinien zostać zaakceptowany (Garfield) 🙂

Właśnie zaakceptowałem opis tego robota: 

 

[Mechano]

Na początku chcę bardzo podziękować za zainteresowanie tematem i odpowiedzi. Jednocześnie przepraszam, za tak późną odpowiedź ale raz, że zajmuje mi to sporo czasu i trudno przychodzi to, a dwa, że najpierw chcę jak najwięcej opcji sam sprawdzić zanim zajmę komuś czas.

@wn2001

Dzięki za pomysł, zrobię to w wolnej chwili, chociaż chyba już to ogarnąłem.

@deshipu

Świetna strona! Bardzo dobrze tłumaczy amatorowi co chcemy osiągnąć i jak to zrobić. Chociaż zauważyłem, że przykład dotyczy nogi o nieco innej budowie niż moja ale to, czego nie rozumiałem (czyli kątów kolana) jest wspólne. Dzięki!

@Treker

Nie ma sprawy, dzięki za prowadzenie super portalu 😉

 

Ok ale, żeby nie było tak pięknie to nie wszystko działa tak jak bym tego chciał. Całość obliczeń kinematyki wygląda tak:

// hip;
//kat alfa
alfa = atan2(szerokosc, wysokosc); //kat w radianach
alfa2 = (180 * alfa) / PI; //zamiana radianow na stopnie
alfa2 = alfa2 + 90;
servo3.write(180-alfa2);
servo6.write(180-alfa2);
servo9.write(alfa2);
servo12.write(alfa2);
  
// knee:
//kat beta
dlugoscNogi = sqrt(pow(wysokosc, 2) + pow(dlugosc, 2));
dlugoscNogi = constrain(dlugoscNogi, 15, 100);
beta = atan2(dlugosc, wysokosc); //radiany
if(beta <= -1) beta = -0.99;
beta2 = (180 * beta) / PI; //zamiana radianow na stopnie
beta2 = constrain(beta2, 10, 170);
beta3 = acos((pow(udo, 2) + pow(dlugoscNogi, 2) - pow(piszczel, 2) ) / (2 * udo * dlugoscNogi)); //radiany
beta3 = (180 * beta3) / PI; //zamiana radianow na stopnie
beta3 = constrain(beta3, 10, 160);
beta4 = beta2 + beta3;
servo2.write(beta4);
servo5.write(180-beta4);
servo8.write(beta4);
servo11.write(180-beta4);
Serial.print(" dlugosc nogi: ");
Serial.print(dlugoscNogi);
 
//ankle:
//kat gamma
kat = (pow(piszczel, 2) + pow(udo, 2) - pow(dlugoscNogi, 2))/(2 * piszczel * udo);
  if(kat <= -1) kat = -0.99;
kat2 = acos(kat);
kat3 = (180 * kat2) / PI; //zamiana radianow na stopnie
servo1.write(180-kat3);
servo4.write(kat3);
servo7.write(180-kat3);
servo10.write(kat3);

Problemy:

• przede wszystkim z jakiegoś powodu nie mogę ograniczyć zakresu kąta beta3 (sprawdziłem, że chodzi dokładnie o ten kąt, a beta4=beta2+beta3); dwa serwa potrzebują "odwróconego" sygnału, który uzyskuję w linijce:

servo5.write(180-beta4);

Ale gdy wartość beta4 jest równa 0 i 180-beta4 powinno być równe 180 to z jakiegoś powodu pojawia się tu wartość 0 i dwa serwa wariują. Niestety rozwiązania w stylu:

beta4 = constrain(beta4, 10, 170);
lub
if(beta4 <=10) beta4 = 10;

działają dopóki wartość kąta jest większa niż 0, potem wszystko się zeruje i warunki przestają działać. Nie mam pojęcia na czym polega problem, czy to wina obsługi trygonometrii (strzelam)?

• drugi problem polega na tym, że robot w dowolnym momencie się wiesza, wiem to bo przestaje wysyłać cokolwiek po uarcie i nie reaguje na bluetooth (jeden uart wysyła dane do komputera, a drugi obsługuje hc05), dodatkowo bardzo powtarzalnie skręca się serwo nr 12 podłączone do pinu PB1 (skręca się o 90 stopnie do środka robota i utrzymuje pozycje). No i tutaj już kompletnie nie mam pomysłu dlaczego tak się dzieje. Jak można to diagnozować bez debuggera? W czasie pisania tego posta robot leżący na stole obok mnie zrobił tak ~3 razy, pomaga tylko reset procesora (i wyjęcie, i włożenie kabla usb).
• ostatni problem jest związany z matmą -> posuw korpusu robota "do przodu" czyli zwiększanie zmiennej "długość" dobrze działa tylko w jedną stronę (gdy długość jest dodatnia, długość nogi rośnie), w drugą stronę (dla ujemnej długości) zwiększa się długość nogi ale robot nie przesuwa się do tyłu.

Za każdą pomoc będę bardzo wdzięczny, już i tak bardzo pomogliście.

 

[Wojcik98]

Cześć, tak bez możliwości przetestowania samemu kodu trudno coś powiedzieć, ale zauważyłem dwie rzeczy:

1) Dlaczego "if(beta <= -1) beta = -0.99;"? beta jest w radianach, dlaczego jest ograniczasz ją z dołu do akurat -1?

2) Czy beta4 może być równa 0? Najpierw robisz beta2 = constrain(beta2, 10, 170), a potem beta3 = constrain(beta3, 10, 160), czyli minimalna wartość beta4 to 10+110=20? Czy funkcja constrain działa inaczej, niż się domyślam?

Spróbuj wypisać wartości zmiennych beta2, 3, 4 przed i po wprowadzeniu ograniczeń, żeby można było zawęzić obszar poszukiwań buga. Napisz tutaj, co jest wypisywane.

[Mechano]

Pamiętam, że beta jest w radianach i to ograniczenie wprowadziłem na etapie debugowania chyba, żeby ograniczyć fizycznie ruch stawu (ale teraz nie mogę sobie przypomnieć na sto procent co ja wtedy myślałem, "a trzeba było napisać jakiś komentarz" 😉).

 

A co co drugiego pytania, dla zadanej dużej wysokości (oś z) efekt występuje dosyć szybko dla zwiększanej długości (oś x).

W spoczynku:

beta: -0.01 beta2 nie ogr: -0.61 beta2 ogr: -0.61 beta3 nie ogr: 11.70 beta3 ogr: 11.70            dlugosc nogi: 94.01 wysokosc 94.00 szerokosc 0.00 dlugosc -1.00

Niewielkie wychylenie drążka od długości daje wspomnianego buga:

beta: 0.22 beta2 nie ogr: 12.59 beta2 ogr: 12.59 beta3 nie ogr: 0.00 beta3 ogr: 0.00            dlugosc nogi: 96.32 wysokosc 94.00 szerokosc -1.00 dlugosc 21.00

Dla wychylenia w drugą stronę, efekt jest ten sam ale tym razem o to chodziło:

beta: -0.29 beta2 nie ogr: -16.59 beta2 ogr: -16.59 beta3 nie ogr: 0.00 beta3 ogr: 0.00            dlugosc nogi: 98.08 wysokosc 94.00 szerokosc 0.00 dlugosc -28.00

Ja dzisiaj mam chyba jakąś pomroczność jasną i nic tutaj nie widzę. Chyba muszę sobie odświeżyć wszystkie obliczenia.

A swoją drogą to podczas testowania i pracy robota przez kilkadziesiąt minut ani razu się nie zwiesił. Ciekawe.

[ethanak]

Czekaj... w sumie te bety to są w stopniach czy radianach?

Jeśli jakaś radianowa beta wychodzi Ci z obliczeń  12.59 to znaczy, że robot powinien zrobić trzy piruety, a potem pobawić się w Chucka Norrisa i z kopa załatwić problem...

[Mechano]

W radianach jest tylko wartość beta, potem beta2, beta3 i beta4 są już wyświetlane w stopniach.

Może Krok próbuje jakoś mi wymachać napis "Fatality!".

[Wojcik98]

Czy funkcja constrain() działa w jakimkolwiek przypadku? Bo tutaj nie wygląda, powinno być 10 ≤ beta2 ≤ 170, a tutaj masz przed i po ograniczeniu równą -0.61 albo - 16.59 (oprócz 12.59, które już jest w prawidłowym zakresie przed ograniczaniem).

[ethanak]

Jakiego typu jest beta4?

Tak przy okazji - ja bym jednak wszystkie obliczenia robił na radianach, a dopiero na samym końcu przeliczał to na stopnie (lub mikrosekundy) i to wrzucał bezpośrednio do serwa.

BTW constrain to makro a nie funkcja.

Edytowano Sierpień 12, 2019 przez ethanak

[Mechano]

@Wojcik98

Cytat

Czy funkcja constrain() działa w jakimkolwiek przypadku? Bo tutaj nie wygląda, powinno być 10 ≤ beta2 ≤ 170, a tutaj masz przed i po ograniczeniu równą -0.61 albo - 16.59

Też tego zjawiska nie rozumiem, zauważyłem to już wcześniej ale nie widzę błędu.

@ethanak

Do edukowałem się właśnie czym się różni funkcja od marka. Działa to mniej więcej tak, że makro podstawia kawałek innego kodu w miejscu jego wywołania a funkcja wymaga skoku do jej kodu? No dobra ale kod się kompiluje i kompilator nie wyrzuca ostrzeżeń. Na co mam uważać, zwrócić uwagę, co sprawdzić?

[ethanak]

kawałek kodu

x=(a) < (b)  ? (b) : (a) > (c) ? (c) : (a);

Podstaw teraz za a jakieś wyrażenie mające skutek uboczny.