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 (okrem všeobecného výpočtu, aj zjednodušený, kde x_r=x_o, z_r=z_o).

---

Najmenej výpočtovo náročný výpočet, ktorý sme boli schopný nájsť, bola premena na tohto problému na druhý prípad, a potom použiť ten.

---

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ý.

V kóde nižšie je jednoduchý výpočet v Pythone pomocou numpy, kde je vyriešený daný problém.

   import numpy as np
   
   
   def sub_trojbodovy_pripad(kamera, oci, objekt):
       """
       Výpočet vnútorného uhlu kamera-oci-objekt pomocou skalarneho sucinu v dvoch rozmeroch
       :param kamera: np pole suradnic kamery
       :param oci: np pole suradnic oci
       :param objekt: np pole suradnic sledovaneh oobjektu
       :return: uhol kamera-oci-objekt v radianoch
       """
       k = kamera - oci
       s = objekt - oci
       return np.arccos((np.dot(k, s)) / (np.linalg.norm(k) * np.linalg.norm(s)))
   
   
   def trojbodovy_pripad(kamera, oci, objekt):
       """
       Funkcia ktora vrati dva uhly, jeden pre vertikalne natocenie a jeden pre horizontalne natocenie
       :param kamera: pole suradnic kamery v 3D
       :param oci: pole suradnic oci v 3D
       :param objekt: pole suradnic sledovaneho objektu v 3D
       :return: Vertikalne a horizontalne natocenie, ako uhly v radianoch, ulozene v np poli
       """
       vertikalny_uhol = sub_trojbodovy_pripad(
           np.array([kamera[1], kamera[2]]),
           np.array([oci[1], oci[2]]),
           np.array([objekt[1], objekt[2]])
       )
       horizontalny_uhol = sub_trojbodovy_pripad(
           np.array([kamera[0], kamera[2]]),
           np.array([oci[0], oci[2]]),
           np.array([objekt[0], objekt[2]])
       )
       return np.array([vertikalny_uhol, horizontalny_uhol])
   
   
   ka = np.array([3, 7, 9])  # suradnice kamery
   ob = np.array([5, 2, 4])  # suradnice sledovaneho objektu
   oc = np.array([0, 0, 0])  # suradnice oci
   print('Druha moznost')
   print('radiany: ', trojbodovy_pripad(ka, oc, ob))
   print('stupne:', trojbodovy_pripad(ka, oc, ob) * 57.2957795)

alebo v rovnaký kód v C-éčku:

  #include <stdio.h>
  #include <math.h>
  
  double sub_trojbodovy_pripad(double* kamera, double* oci, double* objekt)
  {
      double k[2], s[2];
      k[0] = kamera[0] - oci[0];
      k[1] = kamera[1] - oci[1];
      s[0] = objekt[0] - oci[0];
      s[1] = objekt[1] - oci[1];
      double dot_product = k[0] * s[0] + k[1] * s[1];
      double norm_k = sqrt(k[0] * k[0] + k[1] * k[1]);
      double norm_s = sqrt(s[0] * s[0] + s[1] * s[1]);
      return acos(dot_product / (norm_k * norm_s));
  }
  
  void trojbodovy_pripad(double* kamera, double* oci, double* objekt, double* result) 
  {
      double vertikalny_uhol = sub_trojbodovy_pripad(&kamera[1], &oci[1], &objekt[1]);
      double h_ka[2] = {kamera[0], kamera[2]};
      double h_oc[2] = {oci[0], oci[2]};
      double h_ob[2] = {objekt[0], objekt[2]};
      double horizontalny_uhol = sub_trojbodovy_pripad(h_ka, h_oc, h_ob);
      result[0] = vertikalny_uhol;
      result[1] = horizontalny_uhol;
  }
  
  int main() 
  {
      double ka[3] = {3, 7, 9};  // suradnice kamery
      double ob[3] = {5, 2, 4};  // suradnice sledovaneho objektu
      double oc[3] = {0, 0, 0};  // suradnice oci
      double result[2];
      trojbodovy_pripad(ka, oc, ob, result);
      printf("radiany: %lf, %lf\n", result[0], result[1]);
      printf("stupne: %lf, %lf\n", result[0] * 57.2957795, result[1] * 57.2957795);
      return 0;
  }