Robot Eyes - Martin Slimák, Adam Struharňanský, Matej Magát
Cieľ projektu
Tento projekt je o vytvorení prototypu robotických očí, ktoré bude možné pripojiť k hlave humanoidného robota (napr. Nico), kde potom hlavným účelom takéhoto robota bude skúmanie v oblasti kognitívnej vedy.
Realizácia projektu
Projekt je realizovaný podľa návrhu z YouTube ( https://www.youtube.com/watch?v=Ftt9e8xnKE4), pričom oproti tomuto návrhu neboli v tejto verzii vykonané na konštrukcii žiadne zmeny, avšak boli použité servomotory, ktoré sa jemne líšia veľkosťou oproti pôvodným (presnejšie sú o trochu väčšie).
Protokol pre komunikáciu s očami je pomocou správ na jednom riadku, kde prvé písmeno označuje typ správy – teda či chceme nastaviť rýchlosť pohybu jednotlivých servomotorov (v tom prípade je to s), alebo či chceme aby nastavil servomotory na určité natočenie (vtedy je to t). Číslovanie servomotorov je v poradí 0 - ľavá vrchná mihalnica, 1 - pravá vrchná mihalnica, 2 pravá spodná mihalnica, 3 - ľavá spodná mihalnica, 4 - vertikálne natočenie očí, 5 - horizontálne natočenie očí, teda dokopy 6 servomotorov.
V prípade nastavenia rýchlosti, je za označeným typu správy číslo servomotora, a ďalej rýchlosť ako počet stupňov za jednotku času, v akom sa aktualizujú servomotory (približne 10ms). Na počiatku sú všetky rýchlosti natavené na 1. Príklad takého príkazu môže byť (nastaví rýchlosť 0-tému servomotoru, ktorý ovláda ľavú vrchnú mihalnicu na 5):
s 0 5
V prípade nastavenia natočení servomotorov je za označením typu správy šesť čísel označujúcich postupne natočenie každého servomotoru (v poradí od servomotoru s najmenším číslom po servomotor s najväčším). Príklad takéhoto príkazu môže byť (nastaví všetky servomotory na 90 stupňov, čo je aj počiatočná pozícia):
t 90 90 90 90 90 90
Na rozdiel od tutoriálu na YT, kde ovládanie bolo pomocou JoyStick-u, v našom prípade je pripojená RealSense kamera, ktorá vie vrátiť hĺbkovú mapu – teda vieme zistiť, v akej vzdialenosti sa nachádza ľubovoľný objekt (vieme zistiť všetky tri priestorové súradnice). Pomocou tejto informácie chceme, aby oči vždy sledovali nejaký určený objekt (napr. farebné kocky). Keďže má však kamera iné umiestnenie ako oči (bolo by to technicky príliš komplikované na rozdiel od SW riešenia), je potrebné prepočítať umiestenie daného objektu vzhľadom na ne, resp. priamo vypočítať uhly, ktoré hovoria, ktorým smerom sa oči pozerajú (a teda priamo sa to bude dať použiť pomocou spomínaného protokolu pre komunikáciu).
Nech je teda stred očí na súradniciach x_o, y_o, z_o, a stred RealSense senzora na súradniciach x_r, y_r, z_r, kde x hovorí a horizontálnom smere, y o smere vertikálnom, a z je smer „do scény“. Tiež predpokladáme, že robotické oči aj kamera sú natočené rovnakým smerom. Rozoberieme dve možnosti, podľa toho v akom tvare prichádza informácia o objekte z RealSense, buď dostaneme v 2D obrázku hĺbkovej mapy súradnice x_s, y_s a zároveň vzdialenosť d_s, alebo v pointcloud-e dostaneme priamo tri súradnice x_k, y_k, z_k daného bodu (v oboch prípadoch máme aj všeobecný výpočet, aj zjednodušený, kde x_r=x_o, z_r=z_o).
TODO prvá možnosť
V druhom prípade chceme si môžeme všimnúť, že zvislý uhol natočenia je nezávislý od x-ových súradníc všetkých objektov (kamera, očí aj objekt na ktorý sa chceme zamerať). Podobne horizontálny uhol nezávisí od y-lonových súradníc. To znamená, že to vieme presunúť do plochy, a počítať v 2D. Dostaneme teda trojuholník zadaný bodmi, kde chceme vypočítať jeden z vnútorných uhlov. Potom vieme použiť skalárny súčin na vypočítanie uhla. Ak teda K je vektor získaný odpočítaním súradníc objektu od súradníc očí (oranžovou), a S je vektor získaní odpočítaním súradníc kamery od súradníc očí (oranžovou), potom vnútorný uhol je α=acos((K⋅S)\/(|K|*|S|) (na vzorovom obrázku je tento uhol 45°). Pre výpočet natočenia očí je nutné pripočítať ešte uhol medzi osou z , očami a kamerou, (na vzorovom obrázku je rovný 23°), ktorý sa však počas prevádzky nemení a v zjednodušenom prípade, keď je kamera nad očami, tak je tento uhol nulový.
