System planowania i realizacji chwytów za pomocą robotycznych rąk

Transkrypt

1 Politechnika Warszawska Wydział Elektroniki i Technik Informacyjnych Instytut Automatyki i Informatyki Stosowanej Praca magisterska System planowania i realizacji chwytów za pomocą robotycznych rąk Krzysztof Czajkowski Opiekun naukowy: dr inż. Wojciech Szynkiewicz Warszawa, 2010

2

3 Streszczenie Celem tej pracy było zaprojektowanie oraz implementacja Systemu Planowania i Realizacji Chwytów (ang. Grasp Planning and Execution System - GPES) za pomocą robotycznych rąk. Ma on służyć lokalizacji zadanego obiektu, obliczenia dla niego najlepszego, w sensie zadanego kryterium, chwytu oraz realizacji tego chwytu w środowisku rzeczywistym. Głównymi narzędziami jakimi się posługiwano były symulator chwytania GraspIt!, program do modelowania trójwymiarowego Blender oraz struktura ramowa MRROC++. Sprzęt jaki wykorzystano to manipulator IRp6, sztuczna ręka Bird Hand oraz urządzenie do lokalizacji obiektów pcbird. Do obliczeń wykorzystano algorytm brutalnej siły i przybliżania prymitywami zaimplementowany w GraspIt! Polega on na użyciu brutalnej siły i ocenie określonej liczby chwytów. Chwyty oceniane są przy użyciu zadanego modelu ręki oraz przybliżonego prymitywami kształtu chwytanego obiektu. Obliczony chwyt przekazywany jest do realizacji przy użyciu struktury ramowej MRROC++. Chwyty zrealizowane za pomocą systemu GPES zbadano następnie w środowisku rzeczywistym. Zaprezentowano i oceniono wyniki tych badań. Słowa kluczowe: GraspIt, MRROC++, pcbird, GPES, chwytanie, sztuczna ręka, manipulator

4 Abstract Title: Grasp planning and execution system using robotic hands The aim of this thesis was to design and implement a Grasp Planning and Execution System (GPES) using robotic hands. Its purpose is to locate a specified object, calculate the best grasp for it according to a given criterion, and execute the grasp in a real environment. The main software used in this work is a grasp simulator GraspIt!, a 3D modeling program Blender and MRROC++ framework. The hardware used is a manipulator IRp6, a Bird Hand robotic hand, and a pcbird object locating device. An algorithm using brute force and shape primitives implemented in GraspIt! was used for planning. It is based on the brute force method and evaluation of a given number of grasps. Grasps are evaluated using a given hand model and a shape-primitive approximation of an object. Calculated grasp is executed using an application implemented in MRROC++ framework. The grasps executed using the GPES system were examined in a real environment. The results of the experiments and their evaluations are presented in this thesis. Key words: GraspIt, MRROC++, pcbird, GPES, grasping, artificial hand, manipulator

5 Spis treści Spis treści i 1 Wprowadzenie Cel i zakres pracy Założenia Zawartość rozdziałów Problematyka chwytania Sztuczne ręce Rodzaje chwytów Wybrane metody planowania chwytów Brutalna siła Symulowane wyżarzanie Przeszukiwanie bazy danych System planowania i realizacji chwytów - GPES Sformułowanie zadania Projekt GPES Ogólna struktura Funkcje realizowane przez moduły Komunikacja Wykorzystane oprogramowanie i sprzęt GraspIt! MRROC i

6 ii SPIS TREŚCI 4.3 IRp6 wraz z chwytakiem PCbird Planowanie chwytu Modele komputerowe Środowisko pracy Chwytak dwupalczasty - TFG Chwytak trójpalczasty - Bird Hand Manipulator IRp Obiekty do chwytania Algorytm planowania chwytu Wybór algorytmu i uzasadnienie Opis algorytmu Realizacja chwytu Podest dla podejmowanego obiektu Metoda realizacji chwytów Implementacja Analiza wymagań Planowanie chwytu Komunikacja z MRROC Komunikacja z PCbird Realizacja chwytu Komunikacja z GraspIt! Nowa aplikacja Kinematyka chwytaka Bird Hand Badania w środowisku rzeczywistym Wyniki Ocena wyników eksperymentalnych Podsumowanie Najważniejsze osiągnięcia Możliwości dalszego rozwoju

7 SPIS TREŚCI iii Bibliografia 93 A Zawartość załączonej płyty CD 97 Spis symboli i skrótów 99 Spis rysunków 100 Spis tabel 103

8

9 Rozdział 1 Wprowadzenie Chwytanie obiektów z zastosowaniem sztucznych rąk robotów jest stosunkowo nowym zagadnieniem badawczym. Znalezienie optymalnych chwytów, w sensie zadanego kryterium, jest problemem trudnym i dlatego stanowi bardzo ciekawy obszar badań. Ograniczenia technologiczne budowanych rąk, ich wysokie ceny i złożoność skutkują jednak stosunkowo małym tempem rozwoju tych badań. Większość proponowanych rozwiązań dla szukania chwytów to, wynikające z intuicji, heurystyki. Najważniejszym obszarem zastosowań robotów potrafiących chwytać wydają się być usługi. Roboty usługowe muszą mieć duże możliwości chwytania różnych przedmiotów. Są wykorzystywane na przykład do karmienia osób niepełnosprawnych, serwowania potraw w restauracjach lub różnych czynności sprzątania i czyszczenia. Potrafią wyręczać ludzi zarówno w domu jak i w pracy. Montaż samochodów, praca przy kasie w sklepie i pomoc przy transporcie bagażu w hotelu to tylko niewielka część obszarów ich zastosowań. Zadanie chwytania, ponieważ jest problemem bardzo złożonym, zazwyczaj dekomponuje się na mniejsze podproblemy. Są to zarówno zagadnienia związane ze sprzętem jak i z oprogramowaniem. Przykładami są odpowiednia budowa sztucznej ręki, czujników używanych do chwytania oraz implementacja efektywnych algorytmów szukających i realizujących chwyty. Rozwiązania części podproblemów związanych z 1

10 2 ROZDZIAŁ 1. WPROWADZENIE chwytaniem istnieją, a informacje o nich są łatwo dostępne. Wyzwaniem staje się więc odpowiednia modyfikacja i połączenie tych rozwiązań w jedną, spójną całość. Całość ta, czyli system planowania i realizacji chwytów, byłby kolejnym krokiem naprzód w robotyce. Dostarczałby robotom nowej umiejętności jaką jest samodzielne chwytanie. Próba stworzenia takiego systemu jest tematem tej pracy. 1.1 Cel i zakres pracy Celem pracy jest stworzenie systemu, który potrafiłby: zlokalizować zadany obiekt znaleźć dla niego najlepszy, względem zadanego kryterium, chwyt wykonać ten chwyt za pomocą dostępnej, sztucznej ręki System ten ma korzystać z dostępnego już, otwartego oprogramowania: systemu planowania chwytów GraspIt! programowej struktury ramowej MRROC++ W ogólnym zarysie zadaniem GraspIt! jest znalezienie odpowiedniego chwytu, natomiast do MRROC++ należy wykonanie go. MRROC++ wykorzystywany jest do tworzenia sterowników dla odpowiedniego sprzętu - robota IRp6 oraz sztucznej ręki zdolnej chwytać obiekty. Do GraspIt! podłączony jest czujnik położenia i orientacji PCbird pozwalający lokalizować obiekty.

11 1.2. ZAŁOŻENIA Założenia Przyjęto następujące założenia przy projektowaniu systemu: Układ odniesienia wirtualnego świata GraspIt! jest układem odniesienia robota IRp6 Znane i niezmienne są położenie i orientacja: układu odniesienia robota, otaczających go obiektów (przeszkód) Kształt chwytanego obiektu jest znany i niezmienny Znane są współczynniki tarcia statycznego wszystkich używanych materiałów dla chwytanych obiektów i chwytaków Położenie i orientacja chwytanego obiektu są nieznane 1.3 Zawartość rozdziałów Rozdział 2 zawiera krótki przegląd zagadnień związanych z konstrukcją sztucznych rąk oraz z problematyką chwytania. W rozdziale 3 przedstawiono projekt systemu planowania i realizacji chwytów dla robotycznych rąk. W rozdziale 4 przedstawiono wykorzystane w pracy oprogramowanie i sprzęt. W rozdziale 5 opisano zagadnienia związane z planowaniem chwytów - budowa modeli komputerowych oraz algorytmy planowania. W rozdziale 6 opisano zagadnienia związane z realizacją chwytów - budowa trajektorii ruchu oraz badania w środowisku rzeczywistym. W rozdziale 7 przedstawiono implementację systemu planowania i realizacji chwytów. W rozdziale 8 przedstawiono i omówiono wyniki badań w środowisku rzeczywistym. Rozdział 9 zawiera podsumowanie, wyciągnięte na podstawie rezultatów pracy wnioski i przedstawienie możliwości dalszych badań.

12

13 Rozdział 2 Problematyka chwytania 2.1 Sztuczne ręce W mowie potocznej termin ręka i dłoń często są ze sobą utożsamiane. W tej pracy przez słowo ręka będziemy rozumieli dłoń i palce. Dłoń natomiast oznaczać będzie śródręcze oraz nadgarstek. Ręce można podzielić na zintegrowane, czyli złączone na stałe z ramieniem, co wynika z ich architektury np. UB Hand (rys. 2.1a), oraz modularne, będące odrębną całością, np. DLR Hand (rys. 2.1b). Istotną zaletą rąk modularnych są szerokie możliwości integracji z różnymi ramionami. Wadą są ich rozmiary. Ręce zintegrowane są mniejsze co sprawia, że są wykorzystywane do zadań wymagających większej precyzji. Wraz z układem zasilającym i sterującym ich gabaryty są jednak duże a ramię niewymienialne. Ze względu na sposób napędu dla poszczególnych stopni swobody ręce możemy podzielić na napędzane wewnętrznie oraz zewnętrznie. Podział ten jest silnie związany z modularnością ręki. Przy napędzie wewnętrznym staramy się umieścić silniki, lub inne urządzenia wykonawcze, jak najbliżej lub nawet w napędzanym przegubie. Znacznie upraszcza to konstrukcję przegubów. Nie trzeba też rozwiązywać problemu przenoszenia napędu i budowania odpowiednich przekładni. Wadą jest jednak rozmiar tak zbudowanej ręki. Rozmiary palców muszą być zwiększone co najmniej do rozmiarów uży- 5

14 6 ROZDZIAŁ 2. PROBLEMATYKA CHWYTANIA (a) (b) Rysunek 2.1: Sztuczne ręce: UB Hand (a), DLR Hand (b) tych elementów wykonawczych. Te z kolei są duże z czego wynikają problemy konstrukcyjne. Istnieje oczywiście możliwość wybrania odpowiednio małych elementów, ale to wiąże się ze zmniejszeniem siły chwytów realizowanych przez rękę. To podejście zostało zrealizowane we wspomnianej wcześniej DLR Hand (rys. 2.1b). W napędzie zewnętrznym elementy wykonawcze są umieszczone poza stopniami swobody, np. w dłoni lub przedramieniu i są do nich podłączone. Skutkuje to mniejszymi rozmiarami ręki. Jest to również podejście, które można zaobserwować w naturze, np. w ludzkiej ręce. Związany z tym problem to konieczność brania pod uwagę faktu, że napędy będą wpływać na siebie. Siła napędowa oddziałująca na pewien stopień swobody i ogniwo musi być przeniesiona przez ogniwa poprzedzające. Można wyróżnić trzy główne kategorie sposobów sterowania stopniami swobody. Przyjmujemy, że W to liczba napędów ręki, a S to liczba przegubów: W < S niektóre przeguby są bierne lub sprzężone z innymi W = S każdy przegub ma swój napęd

15 2.2. RODZAJE CHWYTÓW 7 W > S niektóre z przegubów są napędzane przez więcej niż jeden napęd W ostatnim przypadku działanie jest podobne do ludzkiej ręki, gdzie jeden mięsień odpowiada za zginanie palca, a inny za prostowanie. W rękach robotowych, tak jak i w innych urządzeniach robotowych, czujniki można podzielić na proprioceptywne oraz eksteroceptywne. Te pierwsze mierzą wielkości wewnętrzne robota (np. pozycja, prędkość), a te drugie wielkości związane z interakcją ze środowiskiem (np. tarcie, kształt dotykanego obiektu). Wyróżniamy następujące cztery klasy czujników: czujniki pozycji/prędkości napędzanych przegubów czujniki napięcia ścięgien i momentów obrotowych przegubów czujniki dotyku dla końcówki palca (zakładany kontakt punktowy) czujniki dotyku (zakładany kontakt powierzchniowy, np. sztuczna skóra) 2.2 Rodzaje chwytów Chwyt jest to relacja ręki z chwytanym obiektem [KS08]. Na obiekt są nałożone więzy wynikające z jego kontaktu z ręką. Przez punkt kontaktu rozumiemy punkt wspólny ręki oraz schwyconego obiektu. Znając momenty sił działające na przeguby palca można znaleźć siłę z jaką palec działa na punkt kontaktu. Znając siły działające w punktach kontaktów można znaleźć wypadkową siłę działającą na chwytany obiekt oraz jego moment, np. względem jego środka ciężkości. Przekształcenia są też możliwe w drugą stronę, czyli dla zadanej siły działającej na obiekt można wyznaczyć odpowiednie momenty sił działające na przeguby.

16 8 ROZDZIAŁ 2. PROBLEMATYKA CHWYTANIA Możliwe do wykonania chwyty to takie, które spełniają 3 typy ograniczeń [Cut89]. Są to ograniczenia zadania (np. ruch i siły jakie muszą działać na obiekt), ograniczenia obiektu (np. kształt i kruchość) i ograniczenia ręki (np. maksymalna siła jaką można przyłożyć i maksymalny kąt rozwarcia palców). Mając zbiór możliwych chwytów można wybierać lepsze kierując się wybranymi ich cechami. Istnieje wiele takich cech używanych w języku potocznym, które są trudne do sprecyzowania i obiektywnej oceny. Są to np. bezpieczeństwo, stabilność, zręczność i precyzja. W celu analizy należy wybrać i zdefiniować cechę, która zostanie następnie użyta w zadanej funkcji celu. Przykładem może być odporność na wyślizgiwanie się obiektu, czyli jak duże mogą być siły i momenty przyłożone do obiektu zanim zacznie się on przemieszczać względem ręki. W zależności od użytej ręki można zastosować różnego rodzaju chwyty. Przykłady chwytów pokazano na rysunku 2.2. Znajomość kształtu i rozmiarów chwytanego obiektu jest bardzo pomocna w wyznaczeniu odpowiednich punktów przyłożenia siły dla każdego z palców. Im większa ich liczba tym potencjalnie bardziej skomplikowane i adekwatne do obiektu chwyty można zastosować. Łatwiej jest też przy użyciu takiej ręki manipulować obiektem. Czwarty i piąty palec nie są niezbędne, ale pełnią one rolę pomocniczą. Podtrzymują dodatkowo obiekt i zwiększają stabilność chwytu. Większa liczba palców komplikuje jednak algorytm chwytania, gdyż należy brać pod uwagę większą liczbę stopni swobody. Chwyty możemy ogólnie podzielić na domknięte siłowo (rys. 2.3b) oraz domknięte kształtowo (rys. 2.3a) [KS08]. Chwyty domknięte kształtowo mają miejsce wtedy gdy dłoń i palce ręki zamykają się na obiekcie dopasowując do jego kształtu. W wyniku czego obiekt nie może się poruszyć w żadnym kierunku. W domknięciu siłowym znaczącą rolę odgrywają siły działające na obiekt. Wymagane jest, aby obiekt pozostawał w równowadze nawet pod działaniem sił zewnętrznych. W związku z tym należy zauważyć, że wszystkie chwyty domknięte kształtowo są również domknięte siłowo. Nie mniej jednak te drugie są możliwe przy mniejszej liczbie

17 2.2. RODZAJE CHWYTÓW 9 Rysunek 2.2: Przykładowa klasyfikacja chwytów (a) (b) Rysunek 2.3: Domknięcie: kształtowe (a), siłowe (b) [KS08] punktów kontaktu, ponieważ siły tarcia odgrywają w nich większą rolę. Siły tarcia dodatkowo przeciwdziałają siłom zewnętrznym i ograniczają ruch obiektu w kierunkach prostopadłych do normalnej do obiektu w punktach kontaktu. Chwyty mogą być również mieszane, tzn. niektóre stopnie swobody możemy ograniczyć kształtowo, a inne siłowo. Rozróżniamy trzy rodzaje kontaktu przydatne przy analizie chwytu [KS08]:

18 10 ROZDZIAŁ 2. PROBLEMATYKA CHWYTANIA punktowe bez tarcia miękki palec twardy palec Pierwszy jest używany wtedy gdy obszar kontaktu z obiektem jest bardzo mały oraz gdy tarcie jest pomijalne. Jedynie składowa prostopadła do powierzchni obiektu jest brana pod uwagę. Siły tarcia i momenty można zaniedbać. Model twardego palca ma zastosowanie wtedy gdy siły tarcia podczas kontaktu są na tyle duże, że nie można ich pominąć. Obszar kontaktu jest jednak ciągle punktowy. Momenty są pomijane. Model miękkiego palca jest używany wtedy gdy zarówno tarcie jak i powierzchnia kontaktu są duże. Rozważane są wszystkie siły i momenty działające na obiekt. Na rys. 2.4 pokazano przybliżony stożek tarcia, czyli sposób modelowania tarcia między ręką a obiektem dla twardego palca. Kąt rozwarcia stożka β = 2 arc tg µ, gdzie µ to współczynnik tarcia statycznego. Dozwolone siły, przykładane w danym punkcie, które nie spowodują przesuwania się palca na obiekcie, muszą zawierać się wewnątrz tego stożka. Rysunek 2.4: Aproksymacja stożka tarcia

19 2.3. WYBRANE METODY PLANOWANIA CHWYTÓW 11 Dla miękkiego palca tarcie zależy od współczynnika Younga oraz rodzaju i pola powierzchni stykających się ze sobą materiałów. Można wyróżnić trzy rodzaje kontaktu ze względu na liczbę punktów kontaktu: kontakt powierzchniowy kontakt krawędziowy kontakt punktowy Tarcie między płaskimi powierzchniami będzie większe niż między stykającymi się krawędziami i dużo większe niż między stykającymi się punktami. 2.3 Wybrane metody planowania chwytów Najwcześniej proponowanymi metodami planowania chwytów były metody analityczne (ang. model-based analytical grasp planning). Opierały się na znajdowaniu punktów kontaktu na zadanym konturze według pewnego, zdefiniowanego kryterium jakości. W większości tych metod nie brano jednak pod uwagę ograniczeń związanych z parametrami ręki oraz zakładano dwuwymiarowość środowiska działania robota. Nie miały one więc wielu praktycznych zastosowań. W jednej z nowszych prac z tej klasy metod [MSdPF06] wzięto pod uwagę ograniczenia ręki podczas planowania, ale problem nie został uogólniony na trzy wymiary. W związku z czym w dalszym ciągu ta klasa metod nie ma praktycznego zastosowania w tej pracy. Kolejnym krokiem w rozwoju systemów planowania było pojawienie się metod opierających się na symulacji (ang. simulation-based grasp planning) opisanych w [Mil01, ACKM03]. Polegają one na dostarczeniu modeli obiektów 3D do chwytania, generowanych ręcznie lub automatycznie [GALP07], oraz modeli ręki i przeszkód. Następnie przeszukiwana jest wielowymiarowa przestrzeń chwytów składająca się z 3 parametrów określających położenie, 3 określających rotację oraz tylu dodatkowych parametrów ile zmiennych przegubowych posiada rozważana ręka. Kryterium jakości może zależeć nie tylko od liczby i roz-

20 12 ROZDZIAŁ 2. PROBLEMATYKA CHWYTANIA mieszczenia punktów kontaktu na obiekcie, ale również w przypadku bardziej złożonych scen, od odległości od innych obiektów oraz położenia robota do którego przymocowana jest ręka [BDN + 07]. W rozdziale tym przedstawiono dwie metody przeszukiwania tych przestrzeni lub ograniczania ich rozmiarów w środowisku symulacyjnym. Są to metoda brutalnej siły z przybliżaniem prymitywami kształtu oraz metoda symulowanego wyżarzania. Najbardziej zaawansowaną klasą metod planowania oraz najszybciej rozwijającą się jest klasa metod bazujących na wiedzy (ang. knowledgebased / data-driven grasp planning). W podejściu tym zbierana i przechowywana jest wiedza o chwytach związanych z przeanalizowanymi kształtami. Podczas planowania chwytu dla nowego obiektu znajdowane są chwyty analogiczne do tych jakie znaleziono już dla znanych obiektów. Pojawiły się też propozycje przechowywania informacji semantycznej o obiektach [XKZD09], takiej jak możliwość odkręcenia korka butelki czy nie chwytania za brzeg filiżanki który może stykać się z ustami pijącego z niej człowieka. W rozdziale tym przedstawiono metodę planowania wykorzystującą bazę danych do przechowywania wiedzy [GCDA09] Brutalna siła Sposób działania algorytmu polega na wyznaczaniu wielu pozycji ręki wokół obiektu przybliżonego prymitywami. Następnie algorytm sprawdza po kolei wszystkie wyznaczone pozycje. Ręka jest zbliżana z tych pozycji w kierunku obiektu aż do uzyskania kontaktu. Kolejnym krokiem jest zaciśnięcie ręki i ocena chwytu [ACKM03]. Sieć działań algorytmu pokazano na rysunku 2.5. Wczytanie modelu obiektu polega na załadowaniu jego pliku konfiguracyjnego.iv w formacie OpenInventor [Pin96]. Obiekt musi być przybliżony prymitywami kształtu. Wczytanie modelu ręki polega na załadowaniu jego pliku konfiguracyjnego.cfg w formacie GraspIt! Następnie generowane są punkty testowe których liczba i położenie zależą od zadanego parametru, kształtu oraz rozmiarów wczyta-

21 2.3. WYBRANE METODY PLANOWANIA CHWYTÓW 13 Rysunek 2.5: Sieć działań algorytmu przybliżania prymitywami nego obiektu. Na płaszczyźnie prymitywów wyznaczana jest zadana liczba, jednakowo od siebie odległych punktów. Następnie punkty te są odsuwane od płaszczyzny wzdłuż normalnych do tej płaszczyzny w wyznaczonych punktach. Długość przesunięcia jest dana. Ostatnim krokiem jest sprawdzenie chwytów dla wszystkich wyznaczonych punktów testowych. Ręka jest przesuwana w kierunku obiektu z punktu i aż do uzyskania kontaktu. Następnie wszystkie palce są zaciskane na obiekcie i obliczana jest jakość chwytu. Wynikiem jest wektor jakości chwytów dla wszystkich sprawdzonych punktów. Przykłady badań przeprowadzonych z użyciem tego algorytmu można znaleźć w pracy [Cza09]. Jakość znalezionych chwytów jest mierzona według kryterium opisanego przez Ferrari i Canny w [FC92].

22 14 ROZDZIAŁ 2. PROBLEMATYKA CHWYTANIA Najważniejszą zaletą algorytmu brutalnej siły jest to, że znalezione przez niego chwyty są chwytami w których ręka faktycznie wchodzi w kontakt z obiektem. Punkty rozpoczynania chwytu są rozłożone równomiernie wokół całego obiektu do chwytania, więc obszar przeszukiwania jest dość dobrze pokryty. Znalezione chwyty są chwytami głównie siłowymi. Istotną wadą jest jednak szybkość działania algorytmu. Funkcja obliczająca jakość chwytu, wielokrotnie używana, jest dość wolna i stanowi wąskie gardło całego procesu. W zależności od wybranej liczby punktów do sprawdzenia algorytm może wykonywać się od kilku do kilkunastu minut Symulowane wyżarzanie Symulowane wyżarzanie jest to algorytm optymalizacji globalnej użyty w metodzie poszukiwania chwytu. Metoda ta jest zaimplementowana w GraspIt! dopiero od końca 2009, czyli od ukazania się wersji 2.0. Umożliwia znajdowanie chwytów nie tylko siłowych, ale również, o ile pozwala na to konstrukcja reki, kształtowych. Istotną zaletą jest możliwość używania jej z dokładnymi modelami obiektów, a nie tylko z uproszczonymi przez prymitywy wersjami. Zadanym kryterium poszukiwań jest minimalizacja wartości funkcji którą autor nazywa energią. Na powierzchni ręki zdefiniowane są pożądane punkty jej kontaktu z obiektami. Energia natomiast określona jest przez autora jako funkcja sumy kątów dla każdego punktu kontaktu między normalną w punkcie a prostą łączącą ten punkt z najbliższym punktem na obiekcie wraz z sumą odległości tych punktów od obiektów. Zmiennymi do manipulacji dla algorytmu symulowanego wyżarzania są położenie (x, y, z), orientacja ręki (α, β, γ) oraz wartości stopni swobody jej palców (q1,..., qn - dla Bird Hand N = 8). W sumie jest to 14 zmiennych dla Bird Hand. Algorytm ma za zadanie zminimalizowanie energii poprzez manipulowanie wartościami tych zmiennych [CGA07]. Główną cechą, odróżniającą tę metodę od brutalnej siły jest fakt, że znalezione chwyty nie są równoważne z zaciśnięciem ręki na obiekcie.

23 2.3. WYBRANE METODY PLANOWANIA CHWYTÓW 15 Są one, w pewnym sensie, nie wykonane, ale można je łatwo wykonać uruchamiając funkcją automatycznego zaciśnięcia ręki. Cecha ta również przyspiesza wyszukiwanie. Jest to metoda ciągle rozwijana, ale powyższe cechy sugerują, że jest ona bardzo wartościowa Przeszukiwanie bazy danych Pomysł wykorzystania bazy danych w poszukiwaniu chwytów wynikł z potrzeby użycia wiedzy na temat chwytów już znanych obiektów. Podobną strategią posługują się ludzie. Na przykład uczymy się, że jednym z najlepszych chwytów dla wszystkich rodzajów kubków jest chwyt za jego ucho. Liczne zasady savoir vivre określają na przykład właściwy sposób trzymania sztućców. Uczymy się również właściwych sposobów trzymania zwierząt domowych. Brak informacji o takich chwytach może powodować na przykład uszkodzenia skóry lub sierści zwierząt. Jak z tego wynika, aby znajdować chwyty które ludzie określaliby jako dobre, konieczna jest wiedza o otaczającym nas świecie. Algorytm ten reprezentuje nowe podejście do problemu szukania chwytów, zwane chwytaniem sterowanym danymi (ang. data-driven grasping). Aby zbudować bazę chwytów konieczna jest baza trójwymiarowych modeli obiektów, będących reprezentacją obiektów świata rzeczywistego. Taka baza istnieje już, jest szeroko stosowana i rozwijana na uniwersytecie w Princeton [SMKF04]. Baza powstałą poprzez działanie zautomatyzowanych pająków internetowych, przeszukujących Internet w poszukiwaniu odpowiednich obiektów. Trzy takie pająki były uruchamiane na przestrzeni dwóch lat, aby wybrać obiekty używane w obecnej wersji bazy. Znaleziono ich Baza to została wykorzystana do zbudowania bazy chwytów. Jest to baza łącząca informacje o pozycji ręki oraz jej wartości zmiennych przegubowych z modelem z bazy Princeton. Baza chwytów została stworzona na uniwersytecie Columbia [GCDA09]. Chwyty w niej zapisane zostały znalezione za pomocą algorytmu wyżarzania (rozdział 2.3.2) lub poprzez manualne działania człowieka - operatora.

24 16 ROZDZIAŁ 2. PROBLEMATYKA CHWYTANIA Algorytm znajdowania chwytu bazuje na intuicyjnym założeniu, że obiekty o podobnym kształcie będą miały podobne chwyty. W związku z tym, dla danego nieznanego wcześniej obiektu, wystarczy znaleźć w bazie danych obiekt podobny do niego, aby znaleźć chwyty które również powinny pasować [GCP + 09]. Algorytm ten korzysta z informacji o kształcie obiektu pobranej z bazy modeli do znajdowania podobnych modeli. Po znalezieniu, obiekty są ustawiane w położeniu i orientacji najbardziej odwzorowującej kształt obu obiektów. Następnie realizowany jest chwyt na nowym obiekcie zgodnie z informacjami z bazy chwytów o obiekcie podobnym. Rozwiązanie to charakteryzuje się najkrótszym czasem działania i wydaje się być najbardziej intuicyjne. Wykorzystuje ono również wiedzę o świecie rzeczywistym, bez której trudno sobie wyobrazić znalezienie poprawnych, z punktu widzenia człowieka, chwytów. Podsumowanie Algorytmy opisane w tym rozdziale zostały wybrane z uwagi na działające i przetestowane już implementacje w GraspIt! Dobrze odzwierciedlają też dwie popularne klasy algorytmów poszukujących chwytów. Metoda brutalnej siły z przybliżaniem prymitywami kształtu oraz metoda symulowanego wyżarzania reprezentują metody opierające się na symulacji. Metoda przeszukiwania bazy danych opiera się na wykorzystywaniu wiedzy. Metoda brutalnej siły działa najwolniej i wymaga dostarczania modelu przybliżonego prymitywami kształtu do chwytania. Chwyty znalezione dzięki niej mogą być jednak bezpośrednio przekazane do wykonania. Symulowane wyżarzanie jest bardziej skomplikowaną metodą. Za jej pomocą znajdowane są niedokończone chwyty, ale nie ma potrzeby upraszania modelu. Można dostarczyć dokładny model geometryczny. Przeszukiwanie bazy danych działa najszybciej, ale wymagane jest przy niej dostarczenie wypełnionej informacjami o chwytach bazy danych.

25 Rozdział 3 System planowania i realizacji chwytów - GPES 3.1 Sformułowanie zadania Głównym zadaniem stawianym w niniejszej pracy jest opracowanie systemu planowania i realizacji chwytów GPES (Grasp Planning and Execution System). Za jego pomocą możliwe są lokalizacja zadanego obiektu, planowanie oraz realizacja dla niego chwytu używając danej, sztucznej ręki. Problemy przemieszczania lub manipulacji schwyconym obiektem są tutaj pomijane. GPES jest systemem deliberacyjnym - opartym na modelu, działającym sekwencyjnie na zasadzie wyczuwaj-planuj-działaj (ang. SPA - sense-plan-act). Rysunek 3.1: Zasada działania GPES 17

26 18 ROZDZIAŁ 3. SYSTEM PLANOWANIA I REALIZACJI CHWYTÓW - GPES W pierwszym kroku konieczne są lokalizacja i określenie kształtu obiektu. Powinien się on znajdować w przestrzeni roboczej manipulatora. Kolejnym krokiem jest obliczenie najlepszego chwytu, w sensie zadanego kryterium, dla dostępnego chwytaka. Przy planowaniu uwzględnione są ograniczenia konstrukcyjne chwytaka oraz znajdujące się w jego otoczeniu przeszkody. W ostatnim kroku następuje przejście do realizacji planu, czyli wykonania obliczonego chwytu. Realizacja może uwzględniać informacje na bieżąco napływające z czujników chwytaka. W rezultacie oczekiwany jest układ obiektu i zaciśniętego na nim chwytaka. Chwyt ten powinien być odporny na działanie niewielkich sił zewnętrznych działających na obiekt, podobnych do tych, z którymi spotykają się ludzie. Są to np. wpływ grawitacji, wiejący wiatr lub siła bezwładności, pojawiająca się przy poruszaniu obiektem. E T G 0 T E E T G E G G P T G 0 T G 0 P 0 T P Rysunek 3.2: Opis geometryczny zadania chwytania

27 3.1. SFORMUŁOWANIE ZADANIA 19 Nie oczekujemy jednak, aby chwyty realizowane za pomocą GPES były zgodnie z jakimikolwiek panującymi normami życia społecznego. Przykładem może być niewkładanie palców do kubka, nawet jeżeli jest on pusty, lub chwytanie za specjalny uchwyt znajdujący się na pokrywce od garnka. Chociaż takie chwyty są pożądane, nie są jednak wymagane do pomyślnego wykonania zleconego zadania. Problem chwytania jest bardzo złożony i w niektórych przypadkach może sprawiać problemy nawet ludziom. Z tego powodu konieczna będzie dekompozycja zadania na mniejsze podzadania. Będą to podzadanie lokalizacji obiektu, planowania i podzadanie realizacji chwytu. Planowanie powinno zajmować możliwie mało czasu, rzędu kilkunastu sekund, aby nie zmniejszać komfortu pracy użytkownika. Realizacja może odbywać się wolniej z uwagi na ograniczenia sprzętowe. W miarę możliwości do budowy GPES należy używać już gotowych i sprawdzonych rozwiązań. Ostatnim wymaganiem jest weryfikacja działania GPES w warunkach laboratoryjnych, przy użyciu rzeczywistych robotów i chwytanych obiektów. Na rysunku 3.2 przedstawiono układy współrzędnych związane z robotem 0, chwytakiem E, obiektem do chwytania G, pozycją ręki w momencie chwytu G i bazą czujnika pozycji P. Zadanie systemu GPES polega na znalezieniu pozycji G, przekształcenia E T G oraz wartości wszystkich zmiennych przegubowych chwytaka, tak aby po wykonaniu tego przekształcenia i ustawieniu zmiennych chwytaka doszło do najlepszego, względem zadanego kryterium, chwytu. E T G jest macierzą przekształcenia jednorodnego [Szy09]. Przekształcenie 0 T E jest dane i wynika z kinematyki robota. Przekształcenie 0 T G = 0 T P P T G również jest znane, gdyż 0 T P jest otrzymywane za pomocą procesu kalibracji, natomiast dane o P T G przesyła do systemu sam czujnik pozycji. Proces kalibracji przeprowadza się, wspierając się programem kalibracji, opracowanym przez J. Kosiorka w [Kos10].

28 20 ROZDZIAŁ 3. SYSTEM PLANOWANIA I REALIZACJI CHWYTÓW - GPES 3.2 Projekt GPES Ogólna struktura System będzie składał się ze współdziałających symulatora GraspIt! oraz programu zaimplementowanego na platformie MRROC++. GraspIt! ma własny wirtualny świat, odwzorowujący świat rzeczywisty i umożliwione jest w nim podłączenie zewnętrznego czujnika jakim jest PCbird. W symulatorze tym dokonywane będą wszystkie obliczenia. MRROC++ natomiast jest strukturą, która działa w świecie rzeczywistym i umożliwia sterowanie podłączonymi robotami. Rysunek 3.3 przedstawia poglądowy schemat połączeń między modułami GPES. Rysunek 3.3: Projekt GPES - schemat blokowy Najpierw dokonywana jest lokalizacja obiektu, następnie planowany jest dla niego chwyt. Na koniec chwyt ten jest realizowany przez roboty Funkcje realizowane przez moduły Moduł PCbird służy do lokalizacji obiektu, który został przymocowany do tego czujnika. Pozycja tego obiektu jest przekazywana względem

29 3.2. PROJEKT GPES 21 bazy czujnika. Moduł ten można będzie w przyszłości wymieniać na dowolny inny pozwalający na lokalizację obiektów, np. dwie kamery i stereowizję. Moduł GraspIt! służy do planowanie chwytu. Moduł ten ma zdefiniowane wcześniej modele robota, sztucznej ręki, obiektu do chwytania i model otoczenia w którym ma działać robot. Dostarczony jest też odpowiedni algorytm planowania, który operując na wymienionych modelach oraz danych z PCbird oblicza najlepszy chwyt, zgodny z zadanym kryterium. Moduł MRROC++ służy do realizacji chwytu przez komunikację z robotem i sztuczną ręką. Moduł ten komunikuje się też z GraspIt! w celu pobrania danych o chwycie do realizacji. MRROC++ jest platformą programową wykorzystywaną w Politechnice Warszawskiej, ale może być wykorzystana też dowolna inna. Głównym zadaniem do zrealizowania w tym module jest wykonanie trajektorii ruchu robota i sztucznej ręki jaką zaplanowano za pomocą GraspIt! Dokładny opis wykorzystanych narzędzi znajduje się w rozdziale 4. Opis scenariusza poprawnego działania systemu: 1. Użytkownik ustawia obiekt do chwycenia z przyklejonym czujnikiem PCbird w obszarze roboczym manipulatora. 2. Użytkownik wysyła rozkaz chwycenia za pośrednictwem interfejsu graficznego GraspIt! 3. Następuje komunikacja GraspIt! z PCbird w celu uzyskania danych o położeniu i orientacji obiektu. 4. Za pomocą GraspIt! obliczany jest chwyt w wirtualnym świecie zgodnie z danymi uzyskanymi z czujnika. Wynikiem są docelowe wartości zmiennych przegubowych używanego robota i chwytaka. Generowane są również pośrednie wartości tych zmiennych, które będę używane w trakcie realizacji trajektorii ruchu. 5. Z GraspIt! kolejno wysyłane są obliczone wektory zmiennych przegubowych do sterownika MRROC++, za pomocą którego

30 22 ROZDZIAŁ 3. SYSTEM PLANOWANIA I REALIZACJI CHWYTÓW - GPES obliczane są sterowania dla rzeczywistego robota i chwytaka. W rezultacie rzeczywisty robot i chwytak realizują chwyt znaleziony w wirtualnym świecie Komunikacja Najważniejszą cechą nowego systemu jest elastyczność i modularność. W GraspIt! istnieje możliwość zmiany algorytmu znajdującego chwyty na dowolny inny. Można również dowolnie zmieniać czujnik położenia i orientacji obiektu na np. kamerę lub czujnik laserowy. W każdym przypadku rodzaj danych z tego czujnika przetwarzany w GraspIt! pozostaje taki sam. Jest to położenie i orientacja. Format danych o znalezionym chwycie wysyłany do MRROC++ również pozostaje niezmieniony. Wymiana robota i chwytaka realizujących chwyty wymaga zmiany jedynie sterownika sprzętowego podłączonego do MRROC++ i odpowiedniego modelu w wirtualnym świecie GraspIt! Schemat połączeń i dane przesyłane między podsystemami pozostają w każdym przypadku niezmienione. Diagram sekwencji dla tego scenariusza znajduje się na rysunku 3.4.

31 3.2. PROJEKT GPES 23 Rysunek 3.4: Projekt GPES - diagram sekwencji

32

33 Rozdział 4 Wykorzystane oprogramowanie i sprzęt Rozdział ten zawiera opis wszystkich wykorzystanych w tej pracy narzędzi oraz ich rozszerzeń i modyfikacji stworzonych na potrzeby niniejszej pracy. 4.1 GraspIt! GraspIt! został stworzony na uniwersytecie Columbia jako narzędzie do badań dotyczących chwytania [Mil01]. Jest to symulator, w którym udostępniono wirtualny świat gdzie można załadować obiekty o dowolnym kształcie. Symulator ten pozwala na ustawienie i wizualizację w środowisku 3D dowolnego chwytu. Chwyty można symulować przy użyciu dostępnych już kilku modeli rąk i obiektów jak również dodawać własne. Dodatkowo możliwy jest pomiar jakości uzyskanych chwytów, na którego podstawie wybierany jest najlepszy. Dostępne są różne modele sztucznych rąk, włączając w to rękę Barretta oraz rękę Robonauty, najbardziej przypominający ludzką rękę model NASA. Obiekty te zapisane są w formacie Open Inventor. Dzięki temu można je modyfikować oraz dodawać własne obiekty dowolnym programem obsługującym ten format, np. Autodesk Maya, 3D Max Studio lub Blender. Do mode- 25

34 26 ROZDZIAŁ 4. WYKORZYSTANE OPROGRAMOWANIE I SPRZĘT Rysunek 4.1: Przykładowy ekran z GraspIt! lu 3D należy dodać też plik z dodatkową informacją o obiekcie taką jak materiał, masa i środek ciężkości. Do wirtualnego świata GraspIt! można dodać obiekty zwane przeszkodami, których jedyna rola polega na ograniczaniu przestrzeni roboczej manipulatora. Jest to przydatna cecha, gdyż chwytane obiekty prawdopodobnie będę znajdowały się na stole lub podobnej powierzchni. Na rysunku 4.2 przedstawiono hierarchię klas elementów świata w GraspIt! Implementacje tych klas można dowolnie zmieniać, lub dodawać własne. W GraspIt! zaimplementowano algorytm poszukiwania najlepszego chwytu dla ręki Barretta oraz sztucznej, 3-palczastej ręki zbudowanej na Politechnice Warszawskiej. Polega on na sprawdzeniu i ocenie dużej liczby chwytów. Można go określić jako algorytm brutalnej siły. Przetestowanie odpowiednio dużej liczby chwytów może niestety trwać nawet kilka godzin. W GraspIt! można obejrzeć wizualizację sił przyłożonych i sił tarcia. Pozwala nam to w pewnym stopniu zobaczyć jakość chwytu i jego słabe punkty. W punkcie kontaktu palca robota z

35 4.1. GRASPIT! 27 Rysunek 4.2: Hierarchia elementów wirtualnego świata w GraspIt! obiektem pojawiają się stożki tarcia. Cały przebieg chwytu w czasie, to jest zachowanie obiektów w kontakcie, można zaobserwować wciskając przycisk symulacji. Symulator został napisany w C++, w środowisku QT, co zapewnia przenoszalność tworzonego kodu na różne platformy.

36 28 ROZDZIAŁ 4. WYKORZYSTANE OPROGRAMOWANIE I SPRZĘT 4.2 MRROC++ MRROC++ (Multi-Robot Research Oriented Controller) został stworzony i jest rozwijany w Politechnice Warszawskiej. Został on napisany w języku C++ i działa pod nadzorem systemu czasu rzeczywistego QNX 6.5 [ZSWK07]. Algorytmy sterowania chwytakiem będą również implementowane w C++. System MRROC++ jest wielowątkową, wielowarstwową i modularną strukturą ramową napisaną w C++. Składa się ona z trzech warstw: Procesy interfejsu użytkownika (UI) warstwa zależna od OS, nie zmieniana przez programistę Procesy sterowania (MP, ECP) warstwa zależna od zadania, tutaj programista podpina własne procesy wykonujące pewne zadania dla określonego manipulatora Procesy sterowników (EDP, VSP) warstwa zależna od sprzętu, nie zmieniana przez programistę Struktura ta jest łatwo rozszerzalna i wygodna w użyciu. Programista, który chce użyć robota do wykonania nowego zadania nie musi martwić się o komunikację międzyprocesową, komunikację z użytkownikiem czy szczegóły komunikacji ze sprzętem. Po prostu używa istniejących procesów EDP/VSP oraz UI. Sam pisze jedynie kod procesu ECP/MP. W systemie istnieją również zaimplementowane generatory ruchu, które mogą automatycznie zajmować się o ustawieniem odpowiednich prędkości i przyspieszeń ruchu robota dla zadanej trajektorii. 4.3 IRp6 wraz z chwytakiem Roboty przemysłowe IRp-6 (rysunek 4.4) są pomocne podczas automatyzacji procesów przemysłowych, a przede wszystkim procesów uciążliwych lub trudnych do wykonania przez człowieka. Roboty mogą być

37 4.3. IRP6 WRAZ Z CHWYTAKIEM 29 Rysunek 4.3: Schemat struktury ramowej MRROC++ oraz wyjaśnienia użytych skrótów stosowane do automatyzacji prac wykonywanych przez maszyny lub mogą same wykonywać pewne prace przy użyciu narzędzi. Roboty IRp-6 dostępne w laboratorium mogą być wyposażone w dowolne narzędzia, umożliwiające np. rysowanie lub kamery, pomocne np. w grze w warcaby. Najciekawszymi narzędziami są jednak chwytaki. Jednym z nich jest widoczny na rysunku 4.5 TFG. Chwytak ten jest dość prymitywny, ale pozwala na chwytanie niektórych, przystosowanych do tego obiektów. Posiada tylko jeden stopień swobody i składa się z dwóch równoległych palców mogących się do siebie zbliżać i oddalać. Bardziej zaawansowanym chwytakiem jest widoczny na rysunku 4.6 chwytak trójpalczasty - Bird Hand. Został on stworzony w Politechnice Warszawskiej w 2010 roku. Praca ta w głównej mierze opiera się na korzystaniu właśnie z niego, chociaż TFG jest również używany w celach testowych. Bird Hand ma trzy palce, dwa z

38 30 ROZDZIAŁ 4. WYKORZYSTANE OPROGRAMOWANIE I SPRZĘT Rysunek 4.4: Roboty IRp6 [Win08] Rysunek 4.5: Chwytak dwupalczasty - TFG [Win08] nich mają trzy stopnie swobody a trzeci dwa stopnie swobody. W sumie jest to osiem stopni swobody. Ręka ta wyglądem przypomina szpon i w porównaniu z ludzką ręką oferuje niewielkie możliwości manipulacji. Bardzo dobrze nadaje się jednak do chwytania różnych obiektów o

39 4.4. PCBIRD 31 nieregularnym kształcie. Pod tym względem ma dużo większe zastosowanie niż wspomniany TFG. Rysunek 4.6: Chwytak trójpalczasty - Bird Hand Stworzony system jest niezależny od używanej ręki. Przymocowaną do manipulatora rękę można wymieniać na inne, w miarę potrzeb. Dokładne modele opisanych tu chwytaków można znaleźć w rozdziale PCbird PCbird, zbudowny przez Ascension Technology Corporation, jest urządzeniem pomiarowym o 6 stopniach swobody, podłączanym do komputera PC [Cor00]. Mierzy ono położenie i orientację małego czujni-

40 32 ROZDZIAŁ 4. WYKORZYSTANE OPROGRAMOWANIE I SPRZĘT ka znajdującego się w odległości do około 1,2 metra od swojej bazy. PCbird potrafi dokonywać tych pomiarów dzięki polu magnetycznemu emitowanemu przez bazę. Jego charakterystyka jest mierzona przez czujnik, a następnie informacja ta jest przekształcana na położenie i orientację czujnika. Obliczenia te są dokonywane dzięki dedykowanej karcie ISA. Kartę, bazę i czujnik pokazano na rysunku 4.7. Rysunek 4.7: Zestaw pomiarowy PCbird Zasięg czujnika wynosi około 120cm od bazy. Dokładność pomiarów to około 2mm dla położenia i około 1 dla orientacji [Cor00]. Pomiary są bardzo wrażliwe na zakłócenia wynikające z bliskości metali w pobliżu czujnika lub bazy. Wykresy reakcji na bliskość metalu pokazano na rysunku 4.8, gdzie w górnej część okna odwzorowano pomiary położenia (x, y, z), a w dolnej orientacji (azimuth, elevation, roll). Podczas doświadczenia czujnik nie był przemieszczany, a jedynie zbliżano metalową płytkę do niego i do bazy. Na wykresach widać nieznaczne wahania odczytów z czujnika, które powinny być stałe.

41 4.4. PCBIRD 33 Rysunek 4.8: Wykresy reakcji czujnika na bliskość metali

42

43 Rozdział 5 Planowanie chwytu Aby poprawnie zaplanować chwyt, niezbędne są dwa elementy. Są to odpowiednie modele komputerowe obiektów oraz algorytm planowania, który będzie z nich korzystał. 5.1 Modele komputerowe Celem tworzenia modeli komputerowych jest dostarczenie przybliżenia świata rzeczywistego w świecie wirtualnym na potrzeby symulacji. Modele mają przybliżać zarówno obiekty jakie GPES ma za zadanie chwytać, sztuczne ręce z jakich będzie korzystał, jak i pozostałe obiekty, zwane przeszkodami, których ma unikać. Wszystkie pokazane tu modele zostały stworzone za pomocą programu Blender i umieszczone na dołączonej do pracy płycie CD (spis zawartości w dodatku A) Środowisko pracy Środowiskiem pracy robotów jest laboratorium robotyki Instytutu Automatyki i Informatyki Stosowanej Politechniki Warszawskiej. Elementy znajdujące się w obszarze roboczym manipulatora to stół w kształcie litery T, różne poręcze znajdujące się wokół niego, oraz drugi manipulator. Zostało to pokazane na rysunku 5.1. Obiekty do chwytania będą umieszczane na tym stole lub bezpośrednio nad nim. 35

44 36 ROZDZIAŁ 5. PLANOWANIE CHWYTU Rysunek 5.1: Środowisko rzeczywiste działania GPES Ponieważ robot podczas ruchu może kolidować jedynie z tymi elementami to tylko one zostały uwzględnione podczas tworzenia modelu. Model końcowy pokazano na rysunku 5.2. Rysunek 5.2: Model komputerowy środowiska działania GPES

45 5.1. MODELE KOMPUTEROWE 37 Rysunek 5.3: Model komputerowy stołu stworzony w programie Blender Chwytak dwupalczasty - TFG Dla chwytaka opisanego w rozdziale 4.3 i pokazanego na rysunku 4.5 zbudowano model komputerowy w środowisku GraspIt! Powstał on z myślą o testowaniu możliwości wykorzystania różnych chwytaków w GPES. Z X Rysunek 5.4: Model komputerowy chwytaka TFG

46 38 ROZDZIAŁ 5. PLANOWANIE CHWYTU Rysunek 5.5: Elementy modelu komputerowego chwytaka TFG Chwytak trójpalczasty - Bird Hand Dla chwytaka opisanego w rozdziale 4.3 i pokazanego na rysunku 4.6 poprawiono model komputerowy w środowisku GraspIt! Model ten został stworzony i opisany w pracy [Cza09]. W wyniku różnych usprawnień rzeczywistej ręki, które miały miejsce od momentu powstania tamtej pracy, model również musiał zostać zaktualizowany. Aktualny model mechaniczny znajduje się na rysunku 5.6.

47 5.1. MODELE KOMPUTEROWE 39 Rysunek 5.6: Model mechaniczny chwytaka Bird Hand Poprawiony model komputerowy znajduje się na rysunku 5.7. Model komputerowy ręki składa się z podstawy oraz trzech łańcuchów kinematycznych, tworzących palce. Łańcuchy te można opisać

48 40 ROZDZIAŁ 5. PLANOWANIE CHWYTU Z X Rysunek 5.7: Model komputerowy chwytaka Bird Hand Rysunek 5.8: Elementy modelu komputerowego chwytaka Bird Hand w GraspIt! za pomocą parametrów Denavita-Hartenberga. Tabela 5.1 przedstawia te parametry dla jednego z palców.

49 5.1. MODELE KOMPUTEROWE 41 Θ i d i a i α i Θ mm 0 Θ mm 0 Θ mm 0 Tabela 5.1: Parametry Denavita-Hartenberga dla palca Bird Hand, notacja zmodyfikowana Manipulator IRp6 Model komputerowy IRp6 został stworzony w ramach pracy [Cza09] i przedstawiono go na rysunku 5.9. X 3 X 2 Z 4 Z 2 X 4 =Z 3 X 1 Z 5 X 5 Z 1 Z 0 X 0 Rysunek 5.9: Model komputerowy robota IRp Obiekty do chwytania Obiektami wybranymi do chwytania są butelka po wodzie mineralnej, kostka Rubika oraz piłka do tenisa, przedstawione na rysunku Ich

50 42 ROZDZIAŁ 5. PLANOWANIE CHWYTU modele można obejrzeć na rysunku Dodatkowo zbudowano ich wersje przybliżone prymitywami. Istnieją 4 dozwolone rodzaje prymitywów wraz z odpowiednimi zestawami parametrów: Cube { width W h e i g h t H depth D } C y l i n d e r { r a d i u s R h e i g h t H } Sphere { r a d i u s R } Cone { bottomradius BR h e i g h t H } Rysunek 5.10: Obiekty rzeczywiste - butelka, kostka i piłka Definicje plików konfiguracyjnych GraspIt! znajdują się poniżej. Jednostkami są milimetry. dla kostki: //W = 55mm, H = 55mm, D = 55mm Cube { width 55 h e i g h t 55 depth 55 } dla butelki:

51 5.1. MODELE KOMPUTEROWE 43 Rysunek 5.11: Dokładne modele komputerowe obiektów - butelka, kostka i piłka Rysunek 5.12: przybliżone modele komputerowe obiektów - butelka, kostka i piłka //R = 30mm, H = 200mm C y l i n d e r { r a d i u s 30 h e i g h t 200 } dla piłki: //R = 31mm Sphere { r a d i u s 31 }

52 44 ROZDZIAŁ 5. PLANOWANIE CHWYTU 5.2 Algorytm planowania chwytu Wybór algorytmu i uzasadnienie W symulatorze GraspIt! zaimplementowane zostały trzy algorytmy planowania chwytu. Reprezentują one metody opisane w rozdziale 2.3. Wszystkie znajdują się w fazie rozwoju. Algorytmem wybranym do zastosowania w systemie GPES jest algorytm brutalnej siły. Chociaż nie jest on najszybszy to pozwala znaleźć chwyty, które można bezpośrednio przesłać do wykonania do systemu sterującego robotami. Algorytm symulowanego wyżarzania potrafi znajdować jedynie przybliżone chwyty, w których ręka nie wchodzi w kontakt z obiektem. Algorytm przeszukiwania bazy danych wydaje się lepszym rozwiązaniem, ale pod warunkiem, że posiadamy już odpowiednio dużą bazę wypełnioną chwytami. Wypełnienie jej wymaga jednak żmudnej pracy operatora lub innego algorytmu, który by automatycznie znajdował chwyty i dodawał je do bazy. W tym drugim przypadku pojawia się jednak problem błędnego koła, gdyż właśnie takiego algorytmu szukamy. Argumenty przemawiające za algorytmem brutalnej siły są również związane z neurologią i wczesnym rozwojem dzieci. Jak pokazały badania nad niemowlakami w ich pierwszym roku życia, chwyt był głównie realizowany poprzez wyciąganie otwartej ręki w kierunku chwytanego obiektu. Następnie kontakt obiektu z dłonią powodował odruch jednoczesnego zginania wszystkich palców aż do całkowitego ich zaciśnięcia na obiekcie. Wraz z rozwojem układu nerwowego dziecka rola dłoni w chwytaniu malała, a na znaczeniu zyskiwały palce. Chwyty stawały się bardziej precyzyjne, a dzieci zyskiwały możliwość chwytania małych obiektów z użyciem jedynie opuszek palców. Zauważono również, że w późniejszych etapach rozwoju dziecka dłoń zbliżając się do obiektu nie była w pełni rozwarta, ale przyjmowała kształt zależny od rodzaju i rozmiarów obiektu [LMF96, OA01]. W związku z tym można przyjąć, że działanie algorytmu brutalnej siły przypomina zachowania kilkumiesięcznego niemowlaka, a więc i że w udany sposób naśladuje sposób myślenia człowieka.

53 5.2. ALGORYTM PLANOWANIA CHWYTU Opis algorytmu Działanie algorytmu polega na wyznaczaniu wielu pozycji ręki wokół obiektu przybliżonego prymitywami. Następnie algorytm sprawdza po kolei wszystkie wyznaczone pozycje. Ręka jest zbliżana z tych pozycji w kierunku obiektu, aż do uzyskania kontaktu. Kolejnym krokiem jest zaciśnięcie ręki i ocena chwytu [ACKM03]. Sieć działań algorytmu pokazano na rysunku Rysunek 5.13: Sieć działań algorytmu przybliżania prymitywami Działanie algorytmu rozpoczyna się od odczytania prymitywów kształtu z zadanych, uproszczonych obiektów. Wywoływany następnie jest proces planowania z zadanymi prymitywami jako parametrami. W wyniku jego działania zwracane są zestawy punktów testowych repre-

54 46 ROZDZIAŁ 5. PLANOWANIE CHWYTU zentujących pozycje sztucznej ręki (rysunek 5.14), które są z kolei przekazywane do ewaluacji. Punkty są wyznaczane w zadanej odległości od płaszczyzn prymitywów. Ich liczba zależy od rozmiaru i kształtu prymitywów oraz zadanego parametru dokładności pomiarów. Przykłady obrazów wygenerowanych punktów można obejrzeć na rysunku 5.15 i Model komputerowy ręki jest ustawiany w punkcie testowym, i zbliżany do obiektu wzdłuż osi przybliżania. Następnie wszystkie palce ręki są automatycznie zaciskane na obiekcie aż do uzyskania kontaktu. Punkty kontaktu są następnie używane do obliczenia jakości chwytu. W przypadku gdy chwyt jest uznany za niestabilny ręka oddala się o zadaną wartość i palce są ponownie zaciskane. Zadanymi parametrami są tu liczba ponownych prób chwytu oraz odległość oddalenia w przypadku niepowodzenia. K 1 K 2 N Rysunek 5.14: Reprezentacja położenia i orientacji sztucznej ręki Rysunek 5.14 przedstawia położenie i orientację ręki. Położenie jest oznaczone jako środek kuli i w tym miejscu zostanie umieszczony początek układu współrzędnych dłoni. Jest to punkt rozpoczynania chwytu. Żadne inne punkty położenia ręki lub palców nie są zadawane. Zmienne przegubowe przyjmują wartości domyślne. Oś N jest normalną dłoni i jednocześnie kierunkiem zbliżania się ręki do obiektu. Oś K natomiast oznacza kierunek kciuka. Na tym rysunku widzimy dwie osie

55 5.2. ALGORYTM PLANOWANIA CHWYTU 47 K 1 oraz K 2 i oznaczają one dwa kierunki kciuka, a więc i dwie możliwe orientacje ręki. Pierwsza oś oznacza kciuk skierowany w prawo, a druga w dół. Współczynnik jakości chwytu jest obliczany zgodnie z opisem w [FC92, MA99]. Wyznaczana jest 6-wymiarowa powłoka wypukła, składająca się z wartości sił i momentów sił w 3-wymiarowej przestrzeni dla każdego z punktów kontaktu. Początkiem układu współrzędnych jest środek ciężkości obiektu. Następnie obliczany jest maksymalny promień 6-wymiarowej kuli, o środku w tym układzie, pozostający wewnątrz tej powłoki wypukłej. Można wyobrazić sobie, że kula ta ma na początku minimalny promień, który jest zwiększany aż do zetknięcia się jej powierzchni z powierzchnią powłoki. Jeżeli środek ciężkości znajduje się na zewnątrz tej powłoki to chwyt jest oznaczany jako niestabilny z wartością jakości równą -1. Implementacja algorytmu została dostosowana do trójpalczastej ręki - Bird Hand oraz dwupalczastej - TFG. Na rysunku 5.15 można zauważyć, że kubek został przybliżony dwoma walcami, a na rysunku 5.16 słuchawka telefonu trzema prostopadłościanami. Widać również punkty położenia i orientacji ręki z rysunku 5.14, z których będą rozpoczynane chwyty.

56 48 ROZDZIAŁ 5. PLANOWANIE CHWYTU Rysunek 5.15: Wyznaczone punkty rozpoczynania chwytu dla kubka Rysunek 5.16: Wyznaczone punkty rozpoczynania chwytu dla słuchawki telefonicznej

57 Rozdział 6 Realizacja chwytu Po udanym zakończeniu procesu planowania (rozdział 5) następuje proces realizacji chwytu. Jest za niego w całości odpowiedzialny sterownik MRROC++. Do realizacji chwytu trzeba jednak przygotować również środowisko rzeczywiste w którym działają roboty. 6.1 Podest dla podejmowanego obiektu W laboratorium, w obszarze roboczym używanych robotów, znajduje się stół na którym można kłaść obiekty do chwytania. Ponieważ system ciągle znajduje się jednak w fazie rozwoju bierzemy pod uwagę możliwość nieprawidłowej realizacji chwytów. To natomiast może skutkować kolizją chwytaka ze stołem, a w rezultacie uszkodzeniami chwytaka. Chcąc tego uniknąć zbudowano, na wzór statywu mikrofonowego, podest do podejmowania obiektów, który widać na rysunku 6.1. Podest ten zbudowany jest z drewnianej deseczki, aby nie zakłócała działania czujnika PCbird. Podest jest lekki i podatny na uderzenia. Łatwo jest go przesunąć i przewrócić, co zmniejsza ryzyko uszkodzenia chwytaka podczas ewentualnej jego kolizji z podestem lub chwytanym obiektem. 49

58 50 ROZDZIAŁ 6. REALIZACJA CHWYTU Rysunek 6.1: Podest dla podejmowanego obiektu 6.2 Metoda realizacji chwytów Za pomocą sterownika MRROC++, zgodnie z planem z rozdziału 3.2, przekazywane są pozycje końcowa i pośrednia, które zostają następnie zrealizowane. Trajektoria przemieszczania się chwytaka przechodzi przez trzy punkty: początkowa, aktualna pozycja chwytaka - nieznana podczas planowania pośrednia pozycja chwytaka - obliczona podczas planowania na podstawie pozycji końcowej końcowa, docelowa pozycja chwytaka - obliczona podczas planowania Problem generowania odpowiedniej trajektorii ruchu ogólnie jest bardzo złożony. Chociaż problem ten nie jest tematem tej pracy, to musi zostać rozwiązany. W tym celu wprowadzamy odpowiednie uproszczenia, tzn. zakładamy, że w większości przypadków można przejść od pozycji początkowej do końcowej przechodząc jedynie przez jeden punkt pośredni. Punkt pośredni natomiast można łatwo obliczyć poruszając się o zadaną odległość z pozycji końcowej ręki wzdłuż normalnej do powierzchni obiektu przechodzącej przez tą pozycję końcową. Jeśli

59 6.2. METODA REALIZACJI CHWYTÓW 51 założymy dodatkowo, że obiekt leży na jakieś powierzchni ograniczającej możliwości chwytania go od dołu, np. na stole lub podeście to zauważymy, że ten sposób generowania trajektorii ruchu chwytaka jest w większości przypadków wystarczający. W celu lepszego wyjaśnienia przedstawiono przykład na rysunku 6.2. Wprowadzamy następujące uproszczenia: ścieżka ruchu między zadanymi punktami jest zawsze linią prostą ruch odbywa się ze stałą prędkością Rysunek 6.2: Dozwolony obszar roboczy manipulatora dla realizacji chwytu Czarne koło przedstawia pewien przedmiot, który będziemy chwytać. Leży on na pewnej płaszczyźnie. Czerwony prostokąt oznacza zewnętrzną krawędź obszaru roboczego manipulatora. Linią przerywaną oznaczono punkty odległe od płaszczyzny obiektu o zadaną długość. Są to wszystkie potencjalne punkty pośrednie trajektorii ruchu. Jeżeli tylko punkt początkowy będzie się znajdował poza obszarem oznaczonym na żółto, to możliwe będzie bezkolizyjne przejście od tego punktu początkowego do punktu pośredniego. Przejście od punktu pośredniego do końcowego jest zawsze możliwe i wynika ze sposobu obliczania punktu pośredniego. Na rysunku 6.2 zaznaczono przykładowy punkt

60 52 ROZDZIAŁ 6. REALIZACJA CHWYTU pośredni A. Z rysunku wynika, że obszar żółty jest dużo mniejszy od całego obszary roboczego, więc prawdopodobieństwo, że ta metoda generowania trajektorii doprowadzi do kolizji jest małe. Oczywiście jeżeli punkt pośredni zostanie wybrany bardzo blisko powierzchni na której leży obiekt, to żółty obszar powiększy się. Nie mniej jednak metoda ta jest wystarczająca dla potrzeb tego projektu. Gdy manipulator dotrze już do pozycji końcowej, następuje przejście chwytaka do pozycji końcowej. Przejście do tej pozycji nie wystarcza jednak do prawidłowego zakończenia procesu chwytania. Jeżeli nawet założymy, że chwytak i obiekt chwytany idealnie odwzorowują stan obliczony w symulatorze, a jest to mało prawdopodobne, to i tak chwytak i obiekt będą jedynie się stykać. Nie są przykładane żadne siły, gdyż nie są one brane pod uwagę w procesie planowania. Przyjmujemy, że cały czas chwytakiem sterujemy pozycyjnie. W taki wypadku zwiększamy wartości zmiennych przegubowych chwytaka w pozycji końcowej o pewną, obliczaną doświadczalnie, wartość. Po wykonaniu tego dodatkowego przesunięcia chwyt uznajemy za zrealizowany. Fazy realizacji chwytu P 0 - jest to układ bazowy robota, względem którego przedstawiane są wszystkie współrzędne P 1 - nieznany punkt oznaczający położenie początkowe ręki P 2 - obliczony punkt oznaczający położenie pośrednie ręki P 3 - obliczony punkt oznaczający położenie końcowe ręki czerwone strzałki - oznaczają ruch jaki ma nastąpić w aktualnej fazie W fazie pierwszej, początkowej widocznej na rysunku 6.3 robot przesuwa się po linii prostej do obliczonego punktu pośredniego chwytu. W czasie tego ruchu, zgodnie z założeniami, nie wystąpią żadne kolizje. W fazie drugiej robot przesuwa się po linii prostej do punktu końcowego. W fazie trzeciej następuje zaciśnięcie się palców sztucznej ręki na obiekcie. Wartości zmiennych przegubowych palców ręki przyjmują

61 6.2. METODA REALIZACJI CHWYTÓW 53 wartości równe zaplanowanym. W fazie czwartej istnieje już odpowiedni chwyt. W celu przeprowadzenia dodatkowych badań i weryfikacji chwytu robot przemieszcza chwycony obiekt w bliżej nieokreślone miejsce nad stołem, w swoim obszarze roboczym. W fazie piątej kończy się działanie programu. Wynikiem jest zrealizowany chwyt. P 1 P 2 P 3 P 0 Rysunek 6.3: Faza 1 chwytu - pozycja początkowa

62 54 ROZDZIAŁ 6. REALIZACJA CHWYTU P 2 P 3 P 0 Rysunek 6.4: Faza 2 chwytu - pozycja pośrednia P 0 Rysunek 6.5: Faza 3 chwytu - pozycja końcowa

63 6.2. METODA REALIZACJI CHWYTÓW 55 P 0 Rysunek 6.6: Faza 4 chwytu - zaciśnięte palce ręki P 0 Rysunek 6.7: Faza 5 chwytu - zakończony chwyt

64

65 Rozdział 7 Implementacja 7.1 Analiza wymagań W projekcie przyjmuje się, że przesyłanym położeniem obiektu będą zmienne kartezjańskie (x, y, z), a orientacją kąty Eulera Z-Y-X (α, β, γ - lub w nomenklaturze kątów Eulera ang. Azimuth, Elevation, Roll). Jest to forma podyktowana konwencją przyjętą przez twórców PCbird i opisaną w instrukcji [Cor00]. Danymi przesyłanymi z GraspIt! do MRROC++, czyli de facto rozkazem realizacji chwytu, będą wartości zmiennych przegubowych palców użytego chwytaka oraz dane o położeniu i orientacji chwytaka podłączonego do używanego manipulatora. Może być to zrealizowane na dwa sposoby: 1. przez wysłanie położenia i orientacji chwytaka 2. przez wysłanie wartości zmiennych przegubowych manipulatora Główne różnice między tymi opcjami wynikają z użycia kinematyki odwrotnej. W obu przypadkach GraspIt! korzysta z zaimplementowanej, numerycznie obliczanej kinematyki odwrotnej. Jest ona uruchamiana wielokrotnie w celu znalezienia odpowiedniego chwytu. Rezultatem tych obliczeń jest również konfiguracja manipulatora, w której ani manipulator, ani chwytak nie kolidują z żadnymi przeszkodami. 57

66 58 ROZDZIAŁ 7. IMPLEMENTACJA W przypadku opcji pierwszej w MRROC++ należało będzie wykonać dodatkowe obliczenia związane ze znalezieniem rozwiązania kinematyki odwrotnej dla manipulatora. Są to niepotrzebne obliczenia, gdyż rozwiązanie to można odczytać wprost z systemu GraspIt!, gdzie i tak należało rozwiązać je wcześniej. Kolejnym minusem jest brak gwarancji, że znalezione rozwiązanie nie powoduje kolizji z przeszkodami. Rozwiązanie kinematyki odwrotnej zależy od bieżącej konfiguracji manipulatora, więc najprawdopodobniej będzie ono inne niż znalezione w GraspIt! W symulatorze tym dysonujemy istotnie większą ilością dostarczonych danych niż w MRROC++. Są to dane o przeszkodach znajdujących się w środowisku w którym działają roboty. Dane te są uwzględniane podczas planowania chwytu. Z tego powodu rozwiązania z symulatora powinny mieć wyższy priorytet niż rozwiązania znalezione w MRROC++. Dlatego też opcja druga, wysłania wartości zmiennych przegubowych, wydaje się lepsza. Cechą przemawiającą za opcją pierwszą jest fakt, że kinematyka odwrotna z MRROC++ jest kinematyką obliczaną analitycznie. Dzięki temu jest obliczana szybciej i jest bardziej niezawodna. Można w tym miejscu rozważyć możliwość implementacji kinematyki analitycznej w GraspIt! Wiąże się to jednak z powieleniem implementacji tej kinematyki z MRROC++, co w głównej mierze najprawdopodobniej odbywałoby się metodą kopiuj-wklej. To następnie wiązałoby się z koniecznością utrzymywania bardzo podobnego kodu w dwóch lokalizacjach. Można również zbudować kanał komunikacji pomiędzy systemami, przez który z GraspIt! dostarczanoby danych wejściowych dla funkcji kinematyki odwrotnej, a w MRROC++ odpowiadanoby jego rozwiązaniem. Nie jest to jednak najlepsze rozwiązanie. Z powodu częstego wykorzystania tej funkcji w GraspIt! generowany byłby duży ruch w sieci, a narzuty czasowe związane z komunikacją uniemożliwiałyby sprawne i szybkie planowanie chwytu. Pierwsza opcja jest bardziej intuicyjna, ale wiążą się z nią negatywne konsekwencje. Wybranie drugiej opcji, czyli wysyłania wartości zmiennych przegubowych, wydaje się być lepszą decyzją. Będzie ona więc zaimplementowana w tym projekcie.

67 7.2. PLANOWANIE CHWYTU 59 Bezpośrednie przyjęcie przez robota obliczonych wartości zmiennych przegubowych może być niebezpieczne. Grozi to jego kolizją z różnymi przeszkodami lub nawet z obiektem chwytanym. Może to też spowodować uszkodzenie manipulatora i chwytaka. Konieczne jest więc wygenerowanie odpowiedniej trajektorii ruchu podejścia do obiektu chwytanego. Trajektoria ta powinna być obliczana w GraspIt!, gdyż posiada on informacje o przeszkodach w świecie rzeczywistym. Następnie zostanie ona przesłana do MRROC++, który ją wykona. Obliczanie trajektorii ruchu i omijanie przeszkód nie jest tematem tej pracy, dlatego zostaną przyjęte odpowiednie uproszczenia. Zakładamy, że chwytak w pozycji początkowej znajduje się w pobliżu chwytanego obiektu i wystarczy jeden lub kilka punktów pośrednich trajektorii aby chwytak mógł być bezkolizyjnie przemieszczony do pozycji końcowej. Punkty pośrednie są równoważne wektorom zmiennych przegubowych manipulatora, a wektory te będą wysyłane do realizacji z GraspIt! do MRROC++. Wybór tej formy wysyłania pozycji chwytaka został uzasadniony wcześniej w tym rozdziale. 7.2 Planowanie chwytu W symulatorze wprowadzono dwie główne modyfikacje. Pierwsza umożliwia komunikację z MRROC++, a druga z PCbird Komunikacja z MRROC++ Zakładamy, że chwyt został poprawnie obliczony. Planowanie chwytu opisano w rozdziale 5 oraz w pracy [Cza09]. W tym momencie muszą być przesłane zmienne przegubowe z GraspIt! do MRROC++. Komunikacja ta została zrealizowana w architekturze klient-serwer za pomocą modelu TCP/IP. Rozwiązanie to jest wspierane przez platformę QT, w której GraspIt! został zaimplementowany. Serwerem jest GraspIt!, który po wybraniu odpowiedniej opcji GUI zaczyna nasłuchiwać połączeń przychodzących. W przypadku ustalenia połączenia wysłane zostają wszystkie informacje, a połączenie zakończone. Na-

68 60 ROZDZIAŁ 7. IMPLEMENTACJA słuchiwanie odbywa się aż do momentu wybrania przez użytkownika odpowiedniej opcji GUI. Pierwszą modyfikacją było dodanie przycisku GUI IRp6 Server Start, widocznego na rysunku 7.1, który powoduje uruchomienie serwera nasłuchującego. Po jego naciśnięciu etykieta zmienia się na IRp6 Server Stop, a ponowne naciśnięcie zatrzymuje serwer. Program korzysta z klasy QTcpServer udostępnianej przez QT. Jest to pewna abstrakcja serwera, umożliwiająca korzystanie z niego w ten sam sposób, niezależnie od uruchomionego systemu operacyjnego. Rysunek 7.1: Modyfikacja GraspIt! GUI dla MRROC++ Zgodnie z rozdziałem 6.2 i przyjętymi tam założeniami, trajektoria przejścia do pozycji końcowej będzie składała się tylko z jednego punktu pośredniego. Zgodnie z tym, GraspIt! po zestawieniu nowego połączenia, będzie wysyłał następujące informacje: końcową konfigurację manipulatora (wektor 6 zmiennych przegubowych) pośrednią konfigurację manipulatora (wektor 6 zmiennych przegubowych) końcową konfigurację chwytaka dwupalczastego - TFG - jeśli używany (1 zmienna przegubowa) końcową konfigurację chwytaka trójpalczastego - Bird Hand - jeśli używany (wektor 8 zmiennych przegubowych) Wektory zmiennych przesyłanych przedstawiono poniżej:

69 7.2. PLANOWANIE CHWYTU 61 double i r p 6 [ 6 ] ; double p r e i r p 6 [ 6 ] ; double t f g ; double b i r d h a n d [ 8 ] ; Najpierw przesyłana jest końcowa konfiguracja manipulatora, potem pośrednia, a na końcu konfiguracja końcowa jednego z chwytaków: out << i r p 6 [ 0 ] ; out << i r p 6 [ 1 ] ; out << i r p 6 [ 2 ] ; out << i r p 6 [ 3 ] ; out << i r p 6 [ 4 ] ; out << i r p 6 [ 5 ] ; out << p r e i r p 6 [ 0 ] ; out << p r e i r p 6 [ 1 ] ; out << p r e i r p 6 [ 2 ] ; out << p r e i r p 6 [ 3 ] ; out << p r e i r p 6 [ 4 ] ; out << p r e i r p 6 [ 5 ] ; i f ( world >getcurrenthand() >getname ( ). c o n t a i n s ( BirdHand ) ) { out << b i r d h a n d [ 0 ] ; out << b i r d h a n d [ 1 ] ; out << b i r d h a n d [ 2 ] ; out << b i r d h a n d [ 3 ] ; out << b i r d h a n d [ 4 ] ; out << b i r d h a n d [ 5 ] ; out << b i r d h a n d [ 6 ] ; out << b i r d h a n d [ 7 ] ; } i f ( world >getcurrenthand() >getname ( ). c o n t a i n s ( 2 F G r i p p e r ) ) { out << t f g ; }

70 62 ROZDZIAŁ 7. IMPLEMENTACJA Komunikacja z PCbird Przed uruchomieniem planowania chwytu GraspIt! potrzebuje danych o położeniu i orientacji obiektu do chwytania. W tym celu należy załadować model obiektu do chwytania, który w swoim pliku konfiguracyjnym będzie zawierał wpis: <u s e F l o c k O f B i r d s>1. 0</ u s e F l o c k O f B i r d s> Informuje on GraspIt! o konieczności jego przemieszczania zgodnie z danymi przesyłanymi z czujnika. Aby uruchomić pobieranie informacji z czujnika otwieramy panel dostępu do czujników zgodnie z rysunkiem 7.2. Rysunek 7.2: Otwieranie panelu GraspIt! GUI dla czujników GUI rozszerzono w następujący sposób. Po pojawieniu się okna, tak jak na rysunku 7.3, możemy uruchomić pobieranie informacji z czujnika poprzez kliknięcie przycisku Start w nowej sekcji pcbird (MRROC++). Etykieta tego przycisku zamieni się na Stop. W tym momencie symulator będzie komunikował się z serwerem uruchomionym na zdalnym komputerze, podłączonym do czujnika PCbird. Wirtualny świat będzie w czasie rzeczywistym odwzorowywał na ekranie komputera pozycję obiektu z przymocowanym czujnikiem. Gdy osiągnie on zadowalającą pozycję, należy w oknie z rysunku 7.3 kliknąć przycisk Stop w tej samej sekcji pcbird (MRROC++). Następnie można przejść do planowania chwytu. Komunikacja ta została zrealizowana w architekturze klient-serwer za pomocą modelu TCP/IP. Rozwiązanie to jest wspierane przez platformę QT, w której GraspIt! został zaimplementowany. Serwerem jest

71 7.2. PLANOWANIE CHWYTU 63 Rysunek 7.3: Rozszerzony o czujnik PCbird panel GraspIt! aplikacja uruchomiona na komputerze podłączonym do czujnika PCbird, a klientem GraspIt! Zarówno ta aplikacja jak i sterownik komunikujący się z czujnikiem zostały stworzone w ramach pracy [ABW08] i działają w środowisku Linux. Po wybraniu odpowiedniej opcji GUI symulator łączy się z serwerem i na bieżąco odpytuje go o dane z czujnika, a następnie przemieszcza obiekt chwytany zgodnie z tymi danymi. Komunikacja zostaje przerwana po wybraniu odpowiedniej opcji z GUI przez użytkownika. Dane przesłane z czujnika są przechowywane w następującej strukturze: // p o z y c j a f l o a t x, y, z ; // katy ( a = azimuth, b = e l e v a t i o n, g = r o l l ) f l o a t a, b, g ; Dane te ulegają odpowiedniej transformacji, uwzględniając różnice między układami bazowym manipulatora (jest to również układ odniesienia związany z wirtualnym światem) oraz układem bazowym czujnika. Dane prezentowane poniżej są przykładowe. Prawdziwe dane otrzymuje się po przeprowadzeniu procesu kalibracji. Konieczna jest też zamiana jednostek odległości z metrów na milimetry, oraz kątów ze stopni na radiany.

72 64 ROZDZIAŁ 7. IMPLEMENTACJA a n g l e s [ 0 ] = 1.0 p. a ; a n g l e s [ 1 ] = 1.0 p. g + 0 ; a n g l e s [ 2 ] = 1.0 p. b + 0 ; p o s i t i o n [ 0 ] = 1. 0 p. x ; p o s i t i o n [ 1 ] = 1.0 p. y ; p o s i t i o n [ 2 ] = 1.0 p. z ; f l o a t Zang = a n g l e s [ 0 ] M PI / 180; f l o a t Yang = a n g l e s [ 1 ] M PI / 180; f l o a t Xang = a n g l e s [ 2 ] M PI / 180; Odwzorowanie przesunięcia może odbyć się wprost. Orientacja natomiast musi być przekształcona z notacji kątów Eulera Z-Y-X do macierzy obrotu. Odbywa się to za pomocą standardowego przekształcenia opisanego w instrukcji obsługi czujnika [Cor00]: r o t a t i o n M a t r i x [ 0 ] = cos ( Yang ) cos ( Zang ) ; r o t a t i o n M a t r i x [ 1 ] = cos ( Xang ) s i n ( Zang)+ s i n ( Xang ) s i n ( Yang ) cos ( Zang ) ; r o t a t i o n M a t r i x [ 2 ] = s i n ( Xang ) s i n ( Zang)+ cos ( Xang ) s i n ( Yang ) cos ( Zang ) ; r o t a t i o n M a t r i x [ 3 ] = cos ( Yang ) s i n ( Zang ) ; r o t a t i o n M a t r i x [ 4 ] = cos ( Xang ) cos ( Zang)+ s i n ( Xang ) s i n ( Yang ) s i n ( Zang ) ; r o t a t i o n M a t r i x [ 5 ] = s i n ( Xang ) cos ( Zang)+ cos ( Xang ) s i n ( Yang ) s i n ( Zang ) ; r o t a t i o n M a t r i x [ 6 ] = s i n ( Yang ) ; r o t a t i o n M a t r i x [ 7 ] = s i n ( Xang ) cos ( Yang ) ; r o t a t i o n M a t r i x [ 8 ] = cos ( Xang ) cos ( Yang ) ; 7.3 Realizacja chwytu Do struktury MRROC++ wprowadzono trzy główne rozszerzenia. Pierwsze umożliwia wymianę informacji z GraspIt!, drugie jest aplikacją sterującą manipulatorem i chwytakiem, a trzecie to oprogramowanie sterownika EDP dla nowego chwytaka Bird Hand, zwane też kinematyką.

73 7.3. REALIZACJA CHWYTU Komunikacja z GraspIt! W celu komunikacji z GraspIt! napisana została klasa czujnika wirtualnego, dziedzicząca po dostarczonej z MRROC++ klasie transmitter. Obiekt tej klasy przechowuje wewnętrznie tablicę: // double g r a s p j o i n t [ 2 6+1]; // t f g double g r a s p j o i n t [ 2 6+8]; // b i r d h a n d Do niej, po wydaniu odpowiedniego polecenia odczytu, zapisywane są informacje przekazane od serwera PCbird. Zgodnie z projektem opisanym w rozdziale 3.2, jest to 20 wartości dla Bird Hand lub 13 dla TFG. Zawierają one 6 zmiennych przegubowych dla manipulatora w pozycji końcowej, 6 w pozycji pośredniej oraz 1 lub 8 zmiennych dla chwytaka w pozycji końcowej. Użycie obiektu tej klasy jest dość łatwe i musi być wykonane jedynie raz podczas całego procesu chwytania. // o dczyt danych z G r a s p I t : // p o d l a c z e n i e t r g r a s p i t >TRconnect ( node name. c s t r ( ), p o r t ) ; // o dczyt t r g r a s p i t >t r e a d ( ) ; // o d l a c z e n i e t r g r a s p i t >TRdisconnect ( ) ; Komunikacja ta została zrealizowana w architekturze klient-serwer za pomocą modelu TCP/IP. Serwerem jest tu symulator GraspIt!, a klientem opisany powyżej obiekt czujnika Nowa aplikacja W celu realizacji zaplanowanego chwytu napisana została aplikacja sterująca robotami. Składa się ona z głównego procesu MP (Master Process), odpowiedzialnego za pobranie informacji o zaplanowanym chwycie (rozkazu), oraz dwóch procesów podrzędnych ECP (Effector Control Process) kontrolujących manipulator i chwytak.

74 66 ROZDZIAŁ 7. IMPLEMENTACJA Procesy ECP działają w ten sposób, że oczekują w nieskończonej pętli na rozkazy z MP. Po otrzymaniu takiego rozkazu, czyli pojedynczego wektora zmiennych przegubowych, komunikują się z przypisanym do nich robotem ustawiając jego zmienne przegubowe na zadane. Następnie sygnalizują procesowi MP, że rozkaz został wykonany i przechodzą w stan oczekiwania na następny. Proces MP po rozpoczęciu działania najpierw komunikuje się z GraspIt! za pomocą obiektu wirtualnego czujnika, opisanego w rozdziale Po otrzymaniu niezbędnych informacji przechodzi do realizacji chwytu, czyli wydawania rozkazów podrzędnym procesom ECP. Schemat działania jest następujący: 1. MP pobiera informacje o chwycie (6 zmiennych przegubowych dla manipulatora w konfiguracji końcowej, 6 w konfiguracji pośredniej oraz 1 lub 8 zmiennych dla chwytaka w konfiguracji końcowej) 2. MP wysyła rozkaz ustawienia manipulatora w konfiguracji pośredniej do jego procesu ECP i czeka na wykonanie 3. MP wysyła rozkaz ustawienia chwytaka w konfiguracji początkowej do jego procesu ECP i czeka na wykonanie 4. MP wysyła rozkaz ustawienia manipulatora w konfiguracji końcowej do jego procesu ECP i czeka na wykonanie 5. MP wysyła rozkaz ustawienia chwytaka w konfiguracji końcowej do jego procesu ECP i czeka na wykonanie 6. MP kończy działanie Wektor zmiennych przegubowych przechowujący konfigurację początkową chwytaka nie jest przesyłany. Zakładamy, że jest on zawsze znany i niezmienny. Konfiguracja ta jest równoważna maksymalnemu rozwarciu palców danego chwytaka.

75 7.3. REALIZACJA CHWYTU Kinematyka chwytaka Bird Hand Wartości zmiennych przegubowych jakie otrzymuje proces MP wyrażone są w radianach. Z punktu widzenia systemu MRROC++ są to zmienne wewnętrzne przydatne podczas przetwarzania danych. W momencie wysyłania wartości tych zmiennych do silników muszą one być przetłumaczone na odpowiednie jednostki. Tą funkcją zajmuje się klasa zwana w nomenklaturze MRROC++ kinematyką. W przypadku nowego chwytaka trójpalczastego implementacja tej klasy była dość prosta. Przeliczenie z wartości zmiennych wewnętrznych (radiany) na wartości w impulsach silnika wygląda następująco: f o r ( i n t i = 0 ; i < 8 ; ++i ) l o c a l d e s i r e d m o t o r p o s n e w [ i ] = l o c a l d e s i r e d j o i n t s [ i ] params. gear [ i ] ; Analogicznie wygląda przekształcenie w odwrotną stronę: f o r ( i n t i = 0 ; i < 8 ; ++i ) l o c a l c u r r e n t j o i n t s [ i ] = l o c a l c u r r e n t m o t o r p o s [ i ] / params. gear [ i ] ; Przełożenia dla poszczególnych stopni swobody wyglądają następująco: g ear [ 0 ] = g ear [ 1 ] = g ear [ 2 ] = g ear [ 3 ] = g ear [ 4 ] = g ear [ 5 ] = ( / / 5. 1 ) /2.0/ M PI ; g ear [ 6 ] = g ear [ 7 ] = /2.0/ M PI ; gdzie to liczba impulsów silnika potrzebna na jego pełny obrót; /2.0/M PI powodują zamianę obrotów silnika na radiany, a pozostałe liczby związane są z liczbą i rodzajem zastosowanych przekładni.

76

77 Rozdział 8 Badania w środowisku rzeczywistym Badania przeprowadzono w laboratorium robotyki Instytutu Automatyki i Informatyki Stosowanej Politechniki Warszawskiej, z wykorzystaniem robotów Bird Hand i IRp6. Chwytak TFG był wykorzystywany jedynie do testów podczas budowy GPES. Z powodu swej prostej budowy i małych możliwości w porównaniu z Bird Hand wyniki badań z jego użyciem nie będą przedstawiane. 8.1 Wyniki Rozdział ten prezentuje wyniki działania systemu planowania i realizacji chwytów GPES. Przedstawiane dane pochodzą z 3 różnych źródeł: zdjęcia chwytów zrealizowanych w środowisku rzeczywistym oraz zaplanowanych w symulatorze. Dodatkowo przedstawiono wizualizacje jakości chwytu dla momentów (z lewej strony) i sił (z prawej strony). Im większa dana bryła przedstawiona na rysunku tym chwyt jest bardziej odporny na zewnętrzne momenty lub siły. odczyty z czujników momentów i położenia umieszczonych w jednym z palców sztucznej ręki. 69

78 70 ROZDZIAŁ 8. BADANIA W ŚRODOWISKU RZECZYWISTYM odczyty z dynamometrów. W doświadczeniach przykładano zewnętrzne siły do trzymanych obiektów równocześnie mierząc je. Jakość chwytu przyjmuje wartości od 0 do 1. W zależności od rodzaju używanej ręki, rozmiarów i rodzaju obiektu chwytanego wartości najlepszych chwytów znajdują się na różnym poziomie. Dla pewnego, danego obiektu najlepsze chwyty mogą przybierać wartości między 0.4, a 0.5, a dla innego między 0.1, a 0.2. Jeżeli dla pierwszego obiektu uda się znaleźć chwyt o jakości np. 0.48, a dla drugiego 0.16 to można powiedzieć, że są one subiektywnie dobre, w kontekście tych obiektów. Im większy i bardziej skomplikowany obiekt tym niższe wartości jego chwytu można uznać za subiektywnie dobre. Chwyty różnych obiektów można jednak ze sobą porównywać i większe wartości liczbowe należy rozumieć jako większą odporności chwyconego obiektu na działające na niego siły zewnętrzne. W praktyce wartości jakości chwytu rzadko zbliżają się do 1, gdyż wymaga to dużej liczby punktów kontaktu ręki z obiektu, a więc i skomplikowanej budowy ręki. Na rysunku 8.1 przedstawiono sposób przykładania siły zewnętrznej i pomiarów za pomocą dynamometru. Siły przykładano za pomocą sznurka którym obwiązywano obiekt, a następnie zaczepiano go o dynamometr. Momenty przykładano zaczepiając końcówkę dynamometru bezpośrednio o obiekt. Siły i momenty przykładano wzdłuż osi układu związanego z obiektem, umieszczone w środku jego masy. Jego pozycję można odczytać z symulatora. Eksperyment ten ma charakter poglądowy, gdyż trudno jest przyłożyć siłę dokładnie tak, aby przecinała środek masy lub przyłożyć moment siły wokół którejkolwiek z osi związanej z obiektem. Niemniej jednak takiej próby dokonano, a odczyt jest wartością siły w której zauważono, że obiekt zaczynał się przemieszczać względem chwytaka. Wartości odczytywane z czujnika siły palca są w nieliniowym związku z wartościami przykładanej do niego siły i zależą od pozycji palca. Z tego powodu zebrano charakterystykę tego palca o dwóch stopniach swobody. DOF 0 oznacza przegub bliższy dłoni, a DOF 1 przegub dalszy. Siłę przykładano w środku paliczka mającego bezpośredni wpływ na dany przegub, prostopadle do jego powierzchni w tym punkcie, i

79 8.1. WYNIKI 71 Rysunek 8.1: Sposób pomiaru maksymalnej siły zewnętrznej mogącej działać na chwycony obiekt Θ 1 -Θ 0 Θ 0 Rysunek 8.2: Schemat budowy palca Bird Hand - stopień swobody DOF 0 dotyczy zmiennej przegubowej Θ 0, a DOF 1 zmiennej przegubowej Θ 1 mierzono ją za pomocą dynamometru. DOF 0 zależy od swojej pozycji, natomiast DOF 1 zależy zarówno od pozycji swojej jak i DOF 0. Zakres pozycji dla DOF 0 wynosi od 0 do 0,55rad, dla DOF 1 od 0 do 0,45rad.

80 72 ROZDZIAŁ 8. BADANIA W ŚRODOWISKU RZECZYWISTYM Rysunek 8.3: Sposób pomiaru charakterystyki czujnika siły dla DOF 0 Rysunek 8.4: Sposób pomiaru charakterystyki czujnika siły dla DOF 1

81 8.1. WYNIKI 73 siła DOF 0\pozycja[rad] DOF 0 (Min) 0,0 0,27 (Max) 0,55 (Min) 4 0,0N 0,0N 0,0N 30 0,4N 0,8N 1,0N 60 1,4N 1,4N 2,2N 90 3,0N 3,0N 4,8N 120 4,8N 5,0N 8,8N 150 7,2N 7,4N 12,6N (Max) ,4N 11,0N 16,0N Tabela 8.1: Charakterystyka DOF 0 (niezależne od DOF 1) Sheet1 18,0N 16,0N 14,0N 12,0N 10,0N 8,0N (Min) 0,0 0,27 (Max) 0,55 6,0N 4,0N 2,0N 0,0N (Min) (Max) 180 Rysunek 8.5: Charakterystyka DOF 0 (niezależne od DOF 1) - przebiegi

82 74 ROZDZIAŁ 8. BADANIA W ŚRODOWISKU RZECZYWISTYM siła DOF 1\pozycja[rad] DOF 0 (Min) 0,0 0,22 (Max) 0,45 (Min) 2 0,0N 0,0N 0,0N 28 5,0N 3,0N 2,6N 56 7,4N 7,0N 7,0N 84 14,8N 10,8N 8,6N ,0N 19,0N 14,0N ,0N 23,0N 18,0N (Max) ,0N 35,0N 26,0N Tabela 8.2: Charakterystyka DOF 1 (dla DOF 0 pozycja = 0,0) Sheet1 40,0N 35,0N 30,0N 25,0N 20,0N (Min) 0,0 0,22 (Max) 0,45 15,0N 10,0N 5,0N 0,0N (Min) (Max) 168 Rysunek 8.6: Charakterystyka DOF 1 (dla DOF 0 pozycja = 0,0) - przebiegi

83 8.1. WYNIKI 75 siła DOF 0\pozycja[rad] DOF 0 (Min) 0,0 0,22 (Max) 0,45 (Min) 2 0,0N 0,0N 0,0N 28 2,4N 4,0N 2,0N 56 2,4N 4,0N 2,0N 84 12,2N 13,6N 6,8N ,0N 20,0N 10,8N ,0N 28,0N 12,0N (Max) ,0N 39,0N 25,0N Tabela 8.3: Charakterystyka DOF 1 (dla DOF 0 pozycja = 0,27) Sheet1 45,0N 40,0N 35,0N 30,0N 25,0N 20,0N (Min) 0,0 0,22 (Max) 0,45 15,0N 10,0N 5,0N 0,0N (Min) (Max) 168 Rysunek 8.7: Charakterystyka DOF 1 (dla DOF 0 pozycja = 0,27) - przebiegi

84 76 ROZDZIAŁ 8. BADANIA W ŚRODOWISKU RZECZYWISTYM siła DOF 0\pozycja[rad] DOF 0 (Min) 0,0 0,22 (Max) 0,45 (Min) 2 0,0N 0,0N 0,0N 28 1,8N 2,8N 2,0N 56 1,8N 2,8N 2,0N 84 12,4N 11,2N 19,0N ,0N 15,0N 34,0N ,0N 21,0N 42,0N (Max) ,0N 25,0N 54,0N Tabela 8.4: Charakterystyka DOF 1 (dla DOF 0 pozycja = 0,55) Sheet1 60,0N 50,0N 40,0N 30,0N (Min) 0,0 0,22 (Max) 0,45 20,0N 10,0N 0,0N (Min) (Max) ,0N Rysunek 8.8: Charakterystyka DOF 1 (dla DOF 0 pozycja = 0,55) - przebiegi

85 8.1. WYNIKI 77 Chwyty w środowisku rzeczywistym i zaplanowane. Kostka Rubika Chwyt 1 - jakość 0,209 Rysunek 8.9: Chwyt rzeczywisty 1 - kostka Rubika Rysunek 8.10: Chwyt zaplanowany 1 - kostka Rubika wielkość\przegub 0 1 siła położenie[rad] 0,1659 0,2078 Tabela 8.5: Odczyty z czujników - chwyt 1 - kostka Rubika

86 78 ROZDZIAŁ 8. BADANIA W ŚRODOWISKU RZECZYWISTYM Chwyt 2 - jakość 0,520 Rysunek 8.11: Chwyt rzeczywisty 2 - kostka Rubika Rysunek 8.12: Chwyt zaplanowany 2 - kostka Rubika wielkość\przegub 0 1 siła położenie[rad] 0,1415 0,1634 Tabela 8.6: Odczyty z czujników - chwyt 2 - kostka Rubika

87 8.1. WYNIKI 79 chwyt\oś X Y Z 0,520 4,0N 3,0N 5,5N 0,209 7,0N 2,0N 0,8N Tabela 8.7: Siły przykładane prostopadle do osi - chwyt kostki Rubika chwyt\oś X Y Z 0,520 12,0N 30,0N 4,0N 0,209 5,0N 4,0N 10,0N Tabela 8.8: Siły przykładane równolegle do osi - chwyt kostki Rubika.

88 80 ROZDZIAŁ 8. BADANIA W ŚRODOWISKU RZECZYWISTYM Piłka do tenisa Chwyt 1 - jakość 0,411 Rysunek 8.13: Chwyt rzeczywisty 1 - piłka Rysunek 8.14: Chwyt zaplanowany 1 - piłka wielkość\przegub 0 1 siła położenie[rad] 0,2261 0,2379 Tabela 8.9: Odczyty z czujników - chwyt 1 - piłka

89 8.1. WYNIKI 81 Chwyt 2 - jakość 0,380 Rysunek 8.15: Chwyt rzeczywisty 2 - piłka Rysunek 8.16: Chwyt zaplanowany 2 - piłka wielkość\przegub 0 1 siła położenie[rad] 0,2190 0,2342 Tabela 8.10: Odczyty z czujników - chwyt 2 - piłka

90 82 ROZDZIAŁ 8. BADANIA W ŚRODOWISKU RZECZYWISTYM chwyt\oś X Y Z 0,411 7,0N 5,0N 5,0N 0,380 9,0N 6,0N 12,0N Tabela 8.11: Siły przykładane prostopadle do osi - chwyt piłki chwyt\oś X Y Z 0,411 5,0N 10,0N 8,0N 0,380 10,0N 25,0N 9,0N Tabela 8.12: Siły przykładane równolegle do osi - chwyt piłki.

91 8.1. WYNIKI 83 Butelka na wodę Chwyt 1 - jakość 0,063 Rysunek 8.17: Chwyt rzeczywisty 1 - butelka Rysunek 8.18: Chwyt zaplanowany 1 - butelka wielkość\przegub 0 1 siła położenie[rad] 0,2186 0,2348 Tabela 8.13: Odczyty z czujników - chwyt 1 - butelka

92 84 ROZDZIAŁ 8. BADANIA W ŚRODOWISKU RZECZYWISTYM Chwyt 2 - jakość 0,059 Rysunek 8.19: Chwyt rzeczywisty 2 - butelka Rysunek 8.20: Chwyt zaplanowany 2 - butelka wielkość\przegub 0 1 siła położenie[rad] 0,2178 0,2338 Tabela 8.14: Odczyty z czujników - chwyt 2 - butelka