SEF Roboter & Tango Control

V predchádzajúcom článku sme si ukázali ako sa dá Tango technológiou oskenovať okolie robota, získaný point cloud natiahnuť do moveit_configu a použiť pri collision avoidance path planningu. Dnes sa bližšie pozrieme na to ako využiť samotnú Pose Estimation funkcionalitu Tango technológie na priame zadávanie žiadanej polohy efektora. A to tak pri point-to-point riadení ako aj v real-time control móde. Opäť využijeme nášho obľúbeného SEF-Robotera, ovládaného po Profinete prostredníctvom ros_control packageu. Technológia Tango od Googla je známa tým že dokáže veľmi presne estimovať svoju polohu v priestore iba na základe IMU jednotky a Motion kamery na zadnej strane smartfónu. Výsledkom je full 6Dof Pose Estimate čiže úplná poloha a orientácia v 3D priestore a to viac ako 200 krát za sekundu. Pomocou ROS-Tango Streamera dokážeme tieto data dostať priamo do ROS, kde už s nimi dokážeme robiť to čo potrebujeme, v našom prípade ich využiť na zadávanie žiadanej polohy SEF Robotera.

V prvom prípade sa zameriame na point-to-point control, čiže polohou Lenova budeme určovať koncové polohy trajektórie. Podmienkou je samozrejme stotožnenie súradnicových systémov, čo je v našej implementácii dosiahnuté virtuálnym stotožnením polohy Lenova a efektora robota hneď po inicializácii Tango Streamera, čím zabezpečíme že Tango a SEF Roboter „o sebe vedia“.  Od tejto chvíle už môžeme Tangom určovať žiadanú cieľovú polohu efektora, ktorá je po zadaní plan & execute commandu použitá priamo ako goal pose. Zvyšok je už MoveIT klasika ako možete vidieť aj na videu nižšie:

V druhom prípade sme si to trocha skomplikovali a pokúsli sme sa dosiahnuť „real time“ ovládanie polohy ramena, v ktorom sa rameno snaží počas celého runtimu sledovať polohu Lenova. Opäť je prvým krokom stotožnenie súradnicových systémov pri inicializácii akurát v tomto prípade už nevyužívame MoveIt na plánovanie trajektórii ako pri point-to-point variante ale každých 10ms počítame inverznú kinematiku ramena podľa aktuálnej polohy Lenova. Výsledok ide priamo do ros_controlu ako nová šestica polohových setpointov pre jednoltivé pohony. Výsledok je vidieť na videu nižšie:

Ešte pred dvoma rokmi by bola na dosiahnutie podobnej funkcionality nutná komplikovaná sústava 3D skenerov navzájom skalibrovaných s ohľadom na umiestnenie robota v priestore. Dnes na to stačí jeden 400 eurový smartfón s Tango technológiou. Magic ….isn’t it?

František Ďurovský