Motoman SDA10F vs. orientácia efektora

plane_titlePoloha objektu v priestore je vo všeobecnosti  definovaná 6-timi parametrami, resp. hovoríme, že objekt má 6 stupňov voľnosti. Prvé tri [x,y,z] určujú  polohu objektu vzhľadom na počiatok súradnicového systému a zvyšné tri [roll, pitch, yaw]  natočenie  vzhľadom na jednotlivé osi.  V článku o pick&place sme si síce ukázali, čo všetko je potrebné nato, aby robot dokázal nájsť a uchopiť ľubovoľne umiestnený predmet vo svojom pracovnom priestore ale úlohu sme si zjednodušili tým, že všetky objekty boli umiestnené vodorovne. Eliminovali sme tým 2 stupne voľnosti – roll, pitch vďaka čomu mohol efektor pri uchopovaní vždy smerovať kolmo dole. V tomto článku si vysvetlíme ako rozšíriť existujúci „pipeline“ aj o detekciu náklonu daného objektu vzhľadom na osi x a y.

Prvým krokom, ktorý nevyhnutné spraviť, je doplnenie modelu ramena robota o efektor, v našom prípade o prísavku. Doteraz bola totiž inverzná kinematika riešená vzhľadom na stred koncovej príruby ramena, čo nám síce stačilo pri kolmom uchopovaní objektov, kde sa jednoducho v Z-ovej osi pri definovaní trajektórie pridal určitý offset ale akonáhle do hry vstupuje aj orientácia efektora, tento prístup už nepostačuje. Doplnením pôvodného xacro modelu ramena o model nástroja (obrázok mižšie) sa zabezpečí riešenie kinematiky vzhľadom na koniec samotnej prísavky.

motoman_tool

Model ľavého ramena pred a po doplnení nástroja (prísavky)

Ďalšou zmenou oproti základnej pick&place aplikácií je transformácia celého pointcloudu do  súradnicového systému robota hneď na začiatku spracovania. V predchádzajúcom prípade sa vyhľadávanie objektov odohrávalo v súradnicovom systéme Kinectu a až nakonci sa získané ťaziská clustrov prepočítavali do base_linku robota. Kým išlo o jednoltivé body nebol s tým žiadny problém ale pri práci s orientáciami a vektormi je názornejšie a aj z hľadiska debugu jednoduchšie ak sa celý pointcloud pretransformuje do base_linku a spracovanie sa odohráva v „človeku prirodzenom súradnicovom systéme“. V „tf“ API pre tento účel existuje funkcia „transformPointCloud„, samozrejme je potrebné mať definovanú polohu Kinectu v modeli pracoviska.

Pre prvé pokusy s orientáciou efektora použijeme originálnu ROS-Industrial kalibračnú mriežku, ktorú na jednom konci podoprieme za účelom vytvorenia naklonenej roviny. Aj z pointcloudu nižšie je zrejmé, o čo nám v tomto prípade pôjde – rozšíriť existujúci pipeline tak, aby motoman dokázal prísť k ťažisku kalibračnej mriežky v smere normálového vektora, čiže kolmo na rovinu kalibračnej mriežky.

cloud_plane

Pointcloud z Kinectu umiestneného nad pracoviskom

Z pohľadu riadenia robota sa toho veľa nezmení, orientáciu efektora sme zadávali aj v predchádzajúcej pick&place aplikácii, akurát bola počas celej doby pohybu konštantná. Hlavnou otázkou teda zostáva ako z pointcloudu vytiahnuť normálový vektor roviny kalibračnej mriežky. Táto úloha už hraničí s rozpoznávaním objektov v pointcloude a vyžaduje aj určité skúsenosti s PCL knižnicou. Pri jej riešení sme experimentovali s dvoma prístupmi:

Prvý spočíval vo využití ICP algoritmu, ktorým sme sa snažili zregistrovať umelo vytvorený model kalibračnej mriežky so skutočným pointcloudom. Výstupom bola matica rotácie a poloha ťažiska, inak povedané údaje o tom ako treba posunúť a otočiť model mriežky, aby čo najlepšie „zapadol“ do reálneho pointcloudu.

V druhom prípade sme zvolili odlišný postup a po vyfiltrovaní kalibračnej mriežky z pointcloudu sme pomocou vybraných bodov z „obvodu“ daného clustra (Convex Hullu)  vytvorili polygón prekrývajúci značnú časť mriežky. Následne sme vypočítali jeho normálový vektor, ktorý bol vďaka vhodnému rozmiestneniu vrcholov zároveň aj normálovým vektorom pôvodnej roviny.

Výber vhodného algoritmu pre danú aplikáciu už závisí od skúseností a intuície vývojára.  Výsledky ICP algoritmu aj napriek experimetovaniu s rôznymi modelmi mriežky a nastavením parametrov neboli dostatočne robusté a v tomto prípade sa viac osvedčil „Convex Hull“ prístup. Výstup vrátane normálového vektora, RPY roviny a Quaternionu definujúceho výslednú orientáciu efektora sú v tvare:

terminal_plane

Normálový vektor má okrem detekcie náklonu roviny ešte jednu funkciu: ak chceme aby efektor prišiel ku danej rovine v kolmom smere, potrebujeme okrem bodu dotyku zadefinovať aj ďalší „približovací“ bod, v ktorom už bude mať efektor správnu orientáciu ale zároveň ešte bude od roviny v určitej vzdialenosti. A práve na výpočet tohto „približovacieho“ waypointu nám výborne poslúžia zložky normálového vektora nx,ny a nz, ktoré aj vzhľadom na matematickú definíciu vektora stačí vo vhodnom pomere pripočítať k bodu dotyku.

Experimetovanie s reálnym robotom

Týmto spôsobom je teda možné jednoznačne identifikovať 5 zo 6 tich parametrov definujúcich polohu roviny v priestore. Posledný parameter „yaw“ hovorí o natočení nástroja (prísavky) alebo objektu okolo svojej osi. Zaujímavý začína byť vo chvíli keď máme na efektore napríklad dvojprstové chápadlo, prípadne ak potrebujeme uchopený objekt správne zorientovať pri položení. Finálnu orientáciu objektu zatiaľ prakticky vyriešenú nemáme ale v najbližšom období sa na tento problém určite pozrieme.

František Ďurovský