Symulator zachowania prostych robotów mobilnych

Rok akademicki 2011/2012
Politechnika Warszawska
Wydział Elektroniki i Technik Informacyjnych
Instytut Informatyki
PRACA DYPLOMOWA INŻYNIERSKA
Krzysztof Lichocki
Symulator zachowania prostych robotów mobilnych
Opiekun pracy
mgr inż. Krzysztof Gracki
Ocena: .....................................................
.................................................................
Podpis Przewodniczącego
Komisji Egzaminu Dyplomowego
Specjalność:
Inżynieria Systemów Informatycznych
Data urodzenia:
1988.09.04
Data rozpoczęcia studiów:
2007.10.01
Życiorys
Urodziłem się 4 września 1988 roku w Gdańsku. W roku 2004 ukończyłem gimnazjum i
rozpocząłem naukę w I LO im. W. Kętrzyńskiego w Kętrzynie. W roku 2007 zdałem maturę i
rozpocząłem studia dzienne na wydziale Elektroniki i Technik Informacyjnych Politechniki
Warszawskiej na kierunku Informatyka.
.......................................................
Podpis studenta
EGZAMIN DYPLOMOWY
Złożył egzamin dyplomowy w dniu .................................................................................2012 r.
z wynikiem ...................................................................................................................................
Ogólny wynik studiów: ................................................................................................................
Dodatkowe wnioski i uwagi Komisji: ..........................................................................................
.......................................................................................................................................................
.......................................................................................................................................................
STRESZCZENIE
Praca obejmuje stworzenie symulatora służącego do modelowania poruszających się
obiektów z uwzględnieniem ich własności fizycznych, a także umożliwiającego testowanie
algorytmów sterowania. Porusza tematy dotyczące tworzenia modeli poruszających się
obiektów, interakcji między obiektami oraz ich wizualizacji.
Słowa kluczowe: symulacja, robotyka mobilna, algorytmy sterowania
SIMULATOR OF SIMPLE MOBILE ROBOTS BEHAVIOR
Thesis describes creation of the simulator used for modeling moving objects with respect to
their physical properties and for testing of control algorithms. It takes up topics related to
modeling of moving objects, the interaction between objects and their visualization.
Keywords: simulation, mobile robotics, control algorithms
Spis treści
1. Wprowadzenie . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
1
1.1. Problemy zwiazane
˛
z symulacja˛ . . . . . . . . . . . . . . . . . . . . . . .
1
1.2. Zalety symulatorów . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
2
1.3. Zastosowanie symulatorów robotów mobilnych . . . . . . . . . . . . . .
4
1.4. Cel i zakres pracy . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
4
2. Istniejace
˛ rozwiazania
˛
. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
6
2.1. Player Project
. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
6
2.1.1. Player . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
7
2.1.2. Stage . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
7
2.1.3. Gazebo . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
7
2.2. Simbad . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
8
2.3. Webots . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
9
2.4. SimRobot . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 10
2.5. Podsumowanie . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 10
3. Projekt systemu . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 12
3.1. Wymagania . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 12
3.2. Ogólna struktura symulatora . . . . . . . . . . . . . . . . . . . . . . . . 12
4. Implementacja . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 15
4.1. Graf sceny . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 15
4.1.1. X3D - format zapisu
. . . . . . . . . . . . . . . . . . . . . . . . . 15
4.1.2. Edycja drzewa sceny . . . . . . . . . . . . . . . . . . . . . . . . . 17
4.2. Silnik fizyczny . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 18
4.2.1. ODE - Open Dynamics Engine . . . . . . . . . . . . . . . . . . . 18
4.2.2. Własności fizyczne obiektów . . . . . . . . . . . . . . . . . . . . . 19
4.2.3. Detekcja kolizji
. . . . . . . . . . . . . . . . . . . . . . . . . . . . 21
4.3. Wizualizacja . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 22
ii
Spis treści
4.4. Sterowanie . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 23
4.4.1. Architektura klient-serwer . . . . . . . . . . . . . . . . . . . . . . 23
4.4.2. Protokół komunikacji . . . . . . . . . . . . . . . . . . . . . . . . . 24
4.4.3. API . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 25
5. Testowanie oraz przykłady użycia . . . . . . . . . . . . . . . . . . . . . . . 28
5.1. Wydajność systemu . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 28
5.2. Wiarygodność symulacji . . . . . . . . . . . . . . . . . . . . . . . . . . . 30
6. Podsumowanie . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 31
Bibliografia . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 33
A. Plik wallAvoiders.x3d . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 34
B. Plik WallAvoiders.cpp . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 41
C. Plik wallFollower.x3d . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 44
D. Plik WallFollower.cpp . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 50
E. Instrukcja obsługi programu . . . . . . . . . . . . . . . . . . . . . . . . . . . 53
E.1. Okna programu . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 53
E.2. Interfejs programu
. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 54
E.2.1. File menu . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 54
E.2.2. View menu . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 54
E.2.3. Help menu . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 55
E.2.4. Pasek narze˛ dzi . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 55
E.3. Drzewo sceny
. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 56
E.4. Okno wizualizacji 3D . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 57
1. Wprowadzenie
Zwi˛ekszajaca
˛ si˛e moc obliczeniowa komputerów powoduje, że symulacja
znajduje coraz szersze zastosowanie w dziedzinie robotyki mobilnej. Symulatory oferuja˛ coraz dokładniejsze odwzorowanie rzeczywistego świata
zarówno pod wzgl˛edem zjawisk w nim zachodzacych
˛
jak i jego wizualizacji.
Pozwala to na zwi˛ekszenie roli jaka˛ odgrywa symulacja w procesie tworzenia
i testowania modeli robotów oraz algorytmów nimi sterujacych.
˛
[7]
1.1. Problemy zwiazane
˛
z symulacja˛
Mimo systematycznie rosnacej
˛
wydajności komputerów moc obliczeniowa jest nadal sporym ograniczeniem. Istnieja˛ problemy, które z samej
swojej natury sa˛ kosztowne obliczeniowo, takie jak realistyczna symulacja
wody czy efektów aerodynamicznych. W dziedzinie robotyki mobilnej symulatory musza˛ zapewnić zarówno generacj˛e świata jak i odczyty sensorów, w
które zazwyczaj wyposażone sa˛ roboty. Zwykle symulatory zapewniaja˛ także
odwzorowanie fizyki w zakresie kinematyki oraz dynamiki ruchu brył sztywnych. Duża złożoność obliczeniowa jest wobec tego obecnie ich głównym
ograniczeniem. Powoduje, że użytkownicy symulatorów musza˛ przystać na
pewien kompromis mi˛edzy dokładnościa˛ odwzorowania rzeczywistości a wydajnościa˛ symulacji.
Przeprowadzajac
˛ eksperyment przy użyciu symulatora użytkownik oczekuje zwykle czasu wykonania równego lub lepszego od czasu rzeczywistego.
Przy bardziej skomplikowanych scenach spełnienie tego warunku może
w dużym stopniu wpłynać
˛ na inne parametry symulacji np. zmniejszyć
płynność jej wizualizacji, czy nawet wymagać jej wyłaczenia.
˛
W niektórych
przypadkach konieczne może być ponadto zwi˛ekszenie kroku symulacji, a
co za tym idzie zmniejszenie jej dokładności i wiarygodności wyników.
1.2. Zalety symulatorów
2
Kolejnym problemem jest trudność odpowiedniego zamodelowania
świata. Ponieważ możemy uzyskać jedynie przybliżony model świata rzeczywistego konieczne jest ustalenie jaki wycinek oraz jakie cechy tego świata
sa˛ krytyczne dla uzyskania wiarygodnych wyników symulacji. Napotkać
można w tym przypadku ponownie na problem złożoności, gdy system okaże
sie˛ zbyt skomplikowany na jego wierne odtworzenie.
Roboty mobilne, aby podjać
˛ decyzj˛e na temat swojego dalszego działania, zwykle opieraja˛ si˛e na odczytach z sensorów, w które sa˛ wyposażone.
Informacja pochodzaca
˛ z sensorów jest dla robota źródłem wiedzy o jego otoczeniu i jako pełniaca
˛ istotna˛ rol˛e w procesie podejmowania decyzji powinna
być zapewniona także w symulatorze. Rzeczywiste sensory nie sa˛ oczywiście
idealne - wprowadzaja˛ zwykle pewien szum. Symulator, aby jak najwierniej
odwzorować rzeczywistość powinien symulować także te zakłócenia, co nie
jest problemem trywialnym.
Wszystkie powyższe problemy sprowadzaja˛ si˛e do jednej zasadniczej kwestii określajacej
˛
przydatność, badź
˛ też nie, symulatora. Kryterium takim jest
rozbieżność mi˛edzy wynikami osiagni˛
˛ etymi poprzez wykonanie określonego
eksperymentu w rzeczywistości a tymi uzyskanymi za pomoca˛ symulacji.
Jeśli symulacja z wymienionych powodów nie jest odpowiednio dokładnym
odwzorowaniem rzeczywistego modelu, osiagni˛
˛ ete rezultaty moga˛ na tyle odbiegać od rzeczywistych, że symulacja staje si˛e bezużyteczna.
1.2. Zalety symulatorów
Mimo wymienionych problemów, symulatory robotów mobilnych sa˛ bardzo cz˛esto i szeroko wykorzystywane. Przyczyna˛ takiego stanu rzeczy sa˛
oferowane przez nie korzyści, które można sprowadzić do dwóch cech - obniżenie kosztów oraz oszcze˛ dność czasu.
Symulacja pozwala uniknać
˛ typowych problemów spowodowanych przez
fizyczność rzeczywistych modeli robotów. Materiały, z których zbudowane
sa˛ roboty zużywaja˛ si˛e, baterie podczas użytkowania traca˛ na wydajności.
Wirtualne modele pozbawione sa˛ tych niedogodności - pozwala to także
1.2. Zalety symulatorów
3
na przeprowadzenie ryzykownych eksperymentów bez obaw o uszkodzenie
robota.
Znacznie prostsze, albo też wre˛ cz możliwe, staje si˛e przeprowadzanie
eksperymentów wykorzystujacych
˛
wiele robotów.
Podczas gdy rzeczywi-
sty eksperyment z powodu ustalonego budżetu musiałby ograniczać si˛e do
niewielkiej ich liczby, ilość symulowanych robotów ograniczona jest zwykle
jedynie przez możliwości symulatora oraz wydajność komputera. W wi˛ekszości przypadków znacznie przekracza ona liczb˛e możliwa˛ do osiagni˛
˛ ecia
w przypadku rzeczywistego eksperymentu. W przypadku symulacji nie istnieje także typowo logistyczny problem zwiazany
˛
z zasilaniem wi˛ekszej liczby
robotów - czy to poprzez kable, które moga˛ zaczać
˛ si˛e platać,
˛
czy poprzez
baterie, które trzeba ponownie ładować. [6]
Kolejna˛ zaleta˛ pozwalajac
˛ a˛ znacznie obniżyć koszty jest możliwość ponownego użycia stworzonych modeli, a także łatwej modyfikacji już istniejacych.
˛
Oferuje to sposobność taniego testowania różnych konfiguracji robota, co w przypadku modeli rzeczywistych mogłoby okazać si˛e zarówno
kosztowne jak i czasochłonne.
Oszcz˛edność czasu jest druga˛ główna˛ korzyścia˛ oferowana˛ przez symulatory. Przy ich użyciu możliwe staje si˛e np. równoległe przeprowadzenie
wielu symulacji danego eksperymentu z różnymi parametrami wykonania.
Zwykle symulatory oferuja˛ także możliwość wyboru pre˛ dkości z jaka˛ ma być
przeprowadzona symulacja. Cz˛esto wia˛że si˛e to z pewnymi ograniczeniami
np. brakiem prezentacji graficznej, ale dzi˛eki temu wyniki można osiagn
˛ ać
˛
szybciej niż w czasie rzeczywistym.
Symulatory szczególna˛ rol˛e pełnia˛ przy testowaniu algorytmów uczacych
˛
sie˛ np. z wykorzystaniem sztucznych sieci neuronowych czy algorytmów
ewolucyjnych. Specyfika tych algorytmów powoduje, że wymagaja˛ one licznych iteracji, badź
˛ też długiego czasu uczenia, co w przypadku rzeczywistych eksperymentów może być kłopotliwe lub nawet niemożliwe do przeprowadzenia. Symulatory dzi˛eki wyżej wymienionym cechom rozwiazuj
˛ a˛ ten
problem.
4
1.3. Zastosowanie symulatorów robotów mobilnych
1.3. Zastosowanie symulatorów robotów mobilnych
Symulatory robotów mobilnych znajduja˛ zastosowanie wsz˛edzie tam,
gdzie wyniki uzyskane za pomoca˛ symulacji nie odbiegaja˛ zbytnio od tych
możliwych do otrzymania w rzeczywistości. Jest to spowodowane wspomniana˛ już oszcz˛ednościa˛ czasu oraz niższymi kosztami - w przeważajacej
˛
wiekszości
˛
przypadków symulacja jest bardziej opłacalna.
Coraz cz˛eściej zaawansowane systemy CAD (ang.
Computer Aided
Design) pozwalaja˛ na przeprowadzenie symulacji działania zaprojektowanych elementów. Umożliwia to w pewnym stopniu zastosowanie takiego
systemu w dziedzinie robotyki. Jednak specyfika systemów CAD powoduje,
że nadaja˛ si˛e one raczej do pracy nad robotami stacjonarnymi jak np. roboty
przemysłowe. W przypadku robotów mobilnych używa si˛e ich do projektowania szczegółowych modeli robotów, które nast˛epnie importowane sa˛ do
wyspecjalizowanych symulatorów robotów mobilnych i tam testowane.
Symulatory sa˛ również szeroko wykorzystywane w celach dydaktycznych. Sa˛ doskonałym narz˛edziem edukacyjnym przybliżajacym
˛
temat robotyki w przypadku, gdy studenci nie maja˛ dost˛epu do rzeczywistych robotów.
Najcz˛eściej jednak symulacja wykonywana jest z myśla˛ o wykorzystaniu
wyników w dalszym rozwoju rzeczywistego robota. Z tego wzgl˛edu wi˛ekszość
przypadków użycia symulatorów można sprowadzić do dwóch głównych zastosowań: testowania różnych konfiguracji budowy robota oraz testowania
algorytmów sterowania.
1.4. Cel i zakres pracy
Celem pracy jest stworzenie symulatora, który b˛edzie umożliwiał przede
wszystkim tworzenie oraz edytowanie modeli prostych robotów, a także
pozwalał na wygodne testowanie algorytmów sterowania.
Zakres edycji
obejmuje zarówno geometri˛e, wyglad,
˛ jak i podstawowe własności fizyczne
obiektów. System ma umożliwić tworzenie robotów mobilnych kołowych
1.4. Cel i zakres pracy
5
oraz kroczacych.
˛
Praca nie obejmuje modelowania czy symulowania robotów latajacych,
˛
badź
˛ pływajacych.
˛
Jest to powodowane odmiennym charakterem zjawisk fizycznych wyst˛epujacych
˛
w takich przypadkach.
W kolejnym rozdziale przedstawione zostały niektóre z dost˛epnych obecnie na rynku symulatorów. Wybrano je tak, aby zaprezentować jak najbardziej zróżnicowane produkty, a jednocześnie wskazać cechy, które wystepuj
˛
a˛ w wi˛ekszości z nich. W rozdziale trzecim nakreślona została ogólna
struktura projektowanego symulatora, podczas gdy rozdział czwarty opisuje
poszczególne moduły wraz ze szczegółami ich implementacji. W rozdziale
piatym
˛
opisane zostały spsoby testowania zrealizowanego systemu ze szczególnym uwzgl˛ednieniem wydajności oraz wiarygodności symulacji. Ostatni
rozdział stanowi podsumowanie wykonanej pracy oraz rozważania na temat
jej dalszego rozwoju.
2. Istniejace
˛ rozwiazania
˛
Obecnie na rynku dost˛epny jest szeroki wybór zarówno symulatorów
komercyjnych - m.in.
Webots1 , anyKode Marilou2 , Microsoft Robotics
Developer Studio3 czy Cogmation robotSim4 , jak i darmowych - open source - takich jak: Player Project5 , Simbad6 , SimRobot7 czy UchilSim8 . Wiele
symulatorów open-source powstaje jako projekt przygotowywany na studia,
co niesie ze soba˛ pewne niedogodności, takie jak brak dalszego rozwoju projektu, czy brak wsparcia. Istnieje jednak kilka produktów, które moga˛ stanowić alternatyw˛e dla rozwiaza
˛ ń komercyjnych. W dalszej cz˛eści rozdziału
zaprezentowane zostana˛ niektóre z nich, a także komercyjne oprogramowanie Webots.
2.1. Player Project
Jednym z najbardziej popularnych środowisk open-source zwiazanych
˛
z symulacja˛ robotów mobilnych jest Player Project. Projekt ten powstał
w 2000 roku na Uniwersytecie Południowej Kalifornii. Obecnie jest szeroko
używany w celach edukacyjnych jak i naukowych. Jest oparty na licencji
GNU GPL i gromadzi wokół siebie wielu ludzi oferujacych
˛
swój wkład w tworzenie projektu, dzi˛eki czemu dynamicznie si˛e rozwija. Na projekt składaja˛
sie˛ trzy niezależne elementy, które moga˛ ze soba˛ współpracować: serwer
Player, symulator 2D Stage oraz symulator 3D Gazebo.
1
2
3
4
5
6
7
8
http://www.cyberbotics.com/overview
http://www.anykode.com
http://www.microsoft.com/robotics/
http://www.cogmation.com/
http://playerstage.sourceforge.net
http://simbad.sourceforge.net
http://www.informatik.uni-bremen.de/simrobot
http://www.robocup.cl/uchilsim
2.1. Player Project
7
2.1.1. Player
Player to serwer prezentujacy
˛ ujednolicony interfejs sterowania wieloma
sensorami oraz efektorami używanymi w robotach mobilnych. Jest multiplatformowy - działa na wszystkich popularnych systemach operacyjnych
(Linux, Solaris, BSD, Mac OS X, Windows). Komunikacja z programem
klienckim prowadzona jest za pomoca˛ protokołu TCP/IP. Dzi˛eki temu użytkownik ma możliwość tworzenia programu sterujacego
˛
robotem w dowolnym j˛ezyku programowania obsługujacym
˛
gniazda TCP. Dodatkowe wsparcie oferowane jest dla najpopularniejszych j˛ezyków takich jak C, C++, Ruby,
Python czy Java. [9]
Player oferuje obecnie sterowniki dla kilkunastu popularnych robotów
takich jak epuck, Khephera, Mindstorms NXT czy Nomad200. Dzi˛eki modularnej budowie dodawanie nowych sterowników jest stosunkowo łatwe zrealizowane jest to za pomoca˛ systemu wtyczek. Duża˛ zaleta˛ środowiska
Player jest to, że stara si˛e udost˛epniać jeden interfejs dla wielu urzadze
˛
ń.
Umożliwia to wykorzystywanie jednego programu sterujacego
˛
do różnych
modeli robotów. Dzi˛eki temu programy sterujace
˛ dla symulatora Stage czy
Gazebo działaja˛ także z prawdziwymi robotami.
Istotna˛ cecha˛ serwera Player jest obsługa wielu klientów jednocześnie.
Ponieważ do jednego serwera połaczyć
˛
może si˛e wiele programów sterujacych
˛
możliwe jest testowanie algorytmów sterowania rozproszonego czy
monitorowanie eksperymentów.
2.1.2. Stage
Stage jest to wieloagentowy symulator 2D. Przystosowany jest do symulowania wielu mało skomplikowanych modeli. Udost˛epnia zróżnicowane modele sensorów oraz efektorów m.in. sonar, IR, dalmierze laserowe, systemy
wizyjne, odometr, nap˛ed różnicowy.
2.1.3. Gazebo
Gazebo, podobnie jak Stage, jest wieloagentowym symulatorem - posiada
jednak wizualizacj˛e 3D. W przeciwieństwie do Stage najlepiej sprawdza si˛e
8
2.2. Simbad
Rysunek 2.1: Przykładowa scena symulatora Player/Stage
przy symulacji niewielkiej liczby robotów o dużej złożoności.
Zapewnia
dokładne odwzorowanie fizyki - wykorzystuje do tego celu bibliotek˛e ODE
(Open Dynamics Engine). Oprócz sensorów udost˛epnianych przez Stage
oferuje dodatkowo kamery oraz GPS. Poza biblioteka˛ sensorów Gazebo zawiera także bibliotek˛e modeli rzeczywistych robotów m.in.
Pioneer2DX,
Pioneer2AT czy SegwayRMP. Model świata zapisywany jest w pliku XML
co ułatwia jego edytowanie.
2.2. Simbad
Simbad jest stosunkowo prostym symulatorem 3D stworzonym w Javie.
Jest multiplatformowy - wymaga jedynie zainstalowanej Javy oraz biblioteki
Java 3D używanej do generowania grafiki. Także udost˛epnia gotowa˛ biblioteke˛ sensorów m.in. sonary, IR, czujniki dotyku, kamery. Simbad został
stworzony z myśla˛ o testowaniu algorytmów sztucznej inteligencji i uczenia
maszynowego. Dost˛epne sa˛ rozszerzenia zwi˛ekszajace
˛ jego możliwości w postaci bibliotek sieci neuronowych PicoNode oraz algorytmów ewolucyjnych
PicoEvo. [10]
Z powodu określonej funkcji jaka˛ miał pełnić Simbad nie nadaje si˛e do
każdego typu zastosowań. Jednym z głównych ograniczeń jest niedokładne
odwzorowanie rzeczywistości - Simbad nie zapewnia dokładnej symulacji
fizyki. Innym znaczacym
˛
ograniczeniem jest brak edytora. Do dyspozycji
9
2.3. Webots
Rysunek 2.2: Przykładowa scena symulatora Simbad
użytkownika oddane sa˛ dwa gotowe modele, które sa˛ dość proste w budowie. Pewnym problemem może być także możliwość pisania programów
sterujacych
˛
jedynie w j˛ezykach Java oraz Python.
2.3. Webots
Webots to komercyjny produkt firmy Cyberbotics. Pozwala na modelowanie i symulowanie różnych typów robotów mobilnych: jeżdżacych,
˛
kroczacych
˛
oraz latajacych.
˛
Podobnie jak wi˛ekszość symulatorów wykorzy-
stuje ODE do odwzorowywania fizyki. Pozwala na importowanie istnieja˛
cych modeli z innych programów graficznych 3D w standardowym formacie
VRML97 (Virtual Reality Modelling Language) uzupełnionym o w˛ezły opisujace
˛ obiekty zwiazane
˛
z robotyka˛ mobilna.
˛ Posiada także rozbudowany
edytor umożliwiajacy
˛ budow˛e modeli oraz ich otoczenia.
Programy sterujace
˛ moga˛ być tworzone w j˛ezykach C, C++, Java, Python
oraz Matlab, a także innych poprzez protokół TCP/IP. Webots posiada bardzo bogata˛ bibliotek˛e sensorów oraz efektorów m.in. dalmierze, kamery,
czujniki światła, czujniki dotyku, GPS, akcelerometry, żyroskopy, chwytaki,
diody LED, nadajniki i odbiorniki. Oferuje także modele rzeczywistych robotów takie jak e-puck, Nao, Katana czy Hoap-2 oraz możliwość transferu
programów sterujacych
˛
do ich fizycznych odpowiedników. [3]
2.4. SimRobot
10
2.4. SimRobot
SimRobot został stworzony na Uniwersytecie Bremeńskim z myśla˛ o wykorzystaniu na zawodach RoboCup - mi˛edzynarodowych zawodach robotów. W dalszym ciagu
˛ wykorzystywany jest przez niemiecka˛ drużyn˛e w Sony
Four-Legged League.
SimRobot ma budow˛e modułowa.
˛ SimRobotCore - jadro
˛
systemu, odpowiada za symulacj˛e, modeluje obiekty oraz świat, a także symuluje odczyty sensorów. Jest niezależne od platformy, dzi˛eki czemu SimRobot działa
na różnych systemach operacyjnych. Moduł Controller, który jest programem sterujacym
˛
robota, jest wywoływany przez symulator i działa w p˛etli
sense-think-act.
Opis modeli robotów oraz świata ładowany jest z pliku zewn˛etrznego.
Format zapisu to RoSiML (Robot Simulation Markup Language) - j˛ezyk
oparty na XML. Został on stworzony z myśla˛ o standaryzacji zapisu modeli oraz możliwości wymiany komponentów mi˛edzy różnymi symulatorami. Struktura pliku XML dobrze oddaje sposób przechowywania struktury sceny w symulatorze - graf sceny. [11]
2.5. Podsumowanie
Wiele z dost˛epnych obecnie produktów symuluje fizyk˛e w zakresie dynamiki brył sztywnych. Zwykle w tym celu wykorzystuje si˛e zewn˛etrzne silniki
fizyczne - bardzo popularnym rozwiazaniem
˛
jest biblioteka Open Dynamics
Engine (ODE). [4]
Jedna˛ z cech, która charakteryzuje wi˛ekszość dost˛epnych produktów jest
obecność bibliotek gotowych modeli istniejacych
˛
w rzeczywistości robotów,
takich jak np. e-puck, Khephera czy Aibo. Niektóre z symulatorów umożliwiaja˛ oprócz ładowania gotowych modeli ich edycj˛e oraz tworzenie nowych.
Dotyczy to zarówno modeli robotów jak i świata, w którym si˛e poruszaja.
˛
Oprócz bibliotek modeli robotów zwykle udost˛epniane sa˛ także mniej lub
bardziej liczne biblioteki sensorów oraz efektorów, w które można wyposażyć
2.5. Podsumowanie
11
wirtualne roboty. Najcz˛eściej spotykanymi sa˛ m.in. kamery, czujniki IR,
sonary, nap˛edy różnicowe czy diody LED.
Wi˛ekszość symulatorów oferuje także pewien rodzaj wizualizacji świata.
Niektóre ograniczaja˛ si˛e do widoku 2D, ale dużo jest rozwiaza
˛ ń zapewniajacych
˛
realistyczna˛ grafik˛e 3D - zwykle przy użyciu bibliotek grafiki jak
OpenGL czy DirectX.
Głównym ograniczeniem wielu darmowych rozwiaza
˛ ń jest utrudniona
edycja modeli robotów, lub nawet jej brak. Wada˛ niektórych jest także brak
możliwości rozdzielenia środowisk uruchomieniowych symulatora oraz programów sterowania. Jednak głównym problemem jest używanie przez poszczególne produkty zupełnie różnych, cz˛esto autorskich, formatów plików
do zapisu scen. Uniemożliwia to wymian˛e modeli mi˛edzy poszczególnymi
symulatorami.
Tworzony system powinien posiadać podstawowe funkcje dost˛epnych na
rynku symulatorów, a wi˛ec symulacj˛e fizyki oraz wizualizacj˛e świata, a przy
tym umożliwiać łatwa˛ edycj˛e sceny oraz testowanie programów sterowania.
Ważne jest, aby wykorzystywany do zapisu scen format plików miał okazj˛e
stać si˛e w przyszłości standardem w tego typu zastosowaniach.
3. Projekt systemu
3.1. Wymagania
Projektowany system powinien uwzgl˛edniać dwa najcz˛estsze powody
użycia symulatorów robotów mobilnych - testowanie różnych konfiguracji
budowy robota oraz testowanie algorytmów sterujacych.
˛
Oba przypadki narzucaja˛ wiele wymagań funkcjonalnych, które system musi spełnić. Przede
wszystkim powinien umożliwiać tworzenie oraz edytowanie zarówno modeli
robotów, jak i ich środowiska. Aby ułatwić prac˛e użytkownikowi powinien
także umożliwiać ponowne wykorzystanie stworzonych elementów np. poprzez funkcj˛e importu/eksportu do pliku.
W celu uzyskania odpowied-
niego odwzorowania rzeczywistości symulator powinien modelować fizyk˛e
w zakresie dynamiki brył sztywnych, a także pozwalać użytkownikowi na
definiowanie wartości podstawowych własności fizycznych modelowanych
obiektów. System powinien także umożliwiać tworzenie oraz wykonywanie
programów sterujacych
˛
dla poszczególnych robotów, a także wizualizacj˛e
przebiegu symulacji.
System musi spełniać także szereg wymagań niefunkcjonalnych,
w szczególności wydajnościowych. Założono, że symulacja średnio skomplikowanej sceny powinna wykonywać si˛e w czasie rzeczywistym lub szybciej na średniej klasy sprz˛ecie. Wymaga si˛e także, aby symulacja dawała
powtarzalne wyniki dla tych samych warunków poczatkowych.
˛
3.2. Ogólna struktura symulatora
Biorac
˛ pod uwag˛e powyższe wymagania przyj˛eto zaprezentowana˛ na rysunku 3.1 strukture˛ symulatora. Budowa modułowa zapewni możliwość
łatwej modyfikacji oraz dalszego rozwoju systemu.
3.2. Ogólna struktura symulatora
13
Rysunek 3.1: Ogólny schemat systemu
— Graf sceny – Struktura danych przechowujaca
˛
informacje o modelu
świata wraz z obiektami w nim wyst˛epujacymi.
˛
Pełni rol˛e bufora w procesie wymiany informacji mi˛edzy modułem symulatora a modułem renderujacym.
˛
Symulator aktualizuje dane grafu sceny, tak aby odpowiadał
on stanowi symulowanego świata, natomiast moduł renderujacy
˛ na podstawie grafu sceny dokonuje wizualizacji tego świata.
Rysunek 3.2: Diagram klas wezłów
˛
drzewa sceny
— Symulator – Główny moduł systemu, odpowiadajacy
˛ za utworzenie modelu świata fizycznego opisywanego za pomoca˛ dynamiki brył sztywnych
na podstawie grafu sceny. Wykonuje kolejne kroki symulacji, a wi˛ec wyznacza na podstawie aktualnego stanu świata oraz działajacych
˛
w nim
3.2. Ogólna struktura symulatora
14
sił stan kolejny, modelujacy
˛ zadany świat po upływie ustalonego dyskretnego odcinka czasu.
— Renderer – Zapewnia wizualizacj˛e działania symulatora.
— Serwer – Umożliwia podłaczenie
˛
do symulatora programów sterujacych.
˛
— UI – Odpowiada za interfejs użytkownika.
Poszczególne moduły opisane zostały szczegółowo w nast˛epnym rozdziale
pracy.
4. Implementacja
4.1. Graf sceny
Centralnym elementem symulatora jest graf sceny. Jest to struktura
przechowujaca
˛ wszystkie statyczne dane dotyczace
˛ symulowanego świata.
Poprawnie skonstruowana definiuje konkretny stan świata określajac
˛ parametry istniejacych
˛
w nim obiektów - ich pozycj˛e, geometri˛e, wyglad
˛ oraz
właściwości fizyczne. Graf sceny umożliwia odwzorowanie hierarchii obiektów, jest wi˛ec cz˛esto stosowany przy scenach trójwymiarowych, gdzie zwykle
wyst˛epuja˛ składajace
˛ si˛e z prostszych złożone obiekty. Najcz˛eściej implementowany jest jako drzewo, w którym każdy w˛ezeł oprócz korzenia posiada jednego rodzica, lub jako skierowany graf acykliczny. Rozwiazanie
˛
drugie ma t˛e przewag˛e, że informacja o obiektach wyst˛epujacych
˛
w wielu
egzemplarzach nie jest duplikowana [1].
Cz˛esto graf sceny ma bezpośrednie odwzorowanie w formacie w jakim
scena zapisywana jest do pliku. Pewna˛ niedogodnościa˛ jaka˛ odznaczaja˛ si˛e
dostepne
˛
na rynku symulatory, ale i edytory grafiki 3D jest wielość tychże
formatów. Z tego powodu wykorzystanie modeli w innym programie niż
użytym do ich stworzenia jest utrudnione, a w przypadku braku możliwości
konwersji w ogóle niemożliwe.
4.1.1. X3D - format zapisu
Majac
˛ powyższe na uwadze, jako format zapisu danych, a także reprezentacj˛e drzewa sceny wybrano format Extensible 3D (X3D). Jest to otwarty
standard oparty na XML, tworzony przez konsorcjum Web3D i b˛edacy
˛ bezpośrednim nast˛epca˛ formatu Virtual Reality Modeling Language (VRML).
X3D zaprojektowany został z myśla˛ o stworzeniu jednolitego sposobu reprezentacji scen 3D i uzyskał status normy ISO [12]. Ważna˛ zaleta˛ formatu jest
16
4.1. Graf sceny
możliwość jego rozszerzania poprzez definiowanie własnych komponentów,
dzieki
˛ czemu możliwe staje si˛e jego użycie do zapisu danych zwiazanych
˛
z symulacja˛ robotów mobilnych. Na wydruku 4.1 zaprezentowano przykładowy plik x3d, natomiast na rysunku 4.1 wynik renderowania tego pliku.
1
<?xml version= " 1.0 " encoding= "UTF−8" ?>
<X3D>
<head>
</head>
<Scene>
5
<World>
<PointLight l o c a t i o n = " 3 3 1 " />
<Shape>
<Appearance>
<Material d i f f u s e C o l o r = " 0 0 1 " />
10
</Appearance>
<Sphere radius= " 0.5 " />
</Shape>
<Transform r o t a t i o n = " 0 1 0 0 " t r a n s l a t i o n = " 0 −0.5 0 " >
<Shape>
15
<Appearance>
<Material d i f f u s e C o l o r = " 1 0 0 " />
</Appearance>
<Box s i z e = " 5 0.1 5 " />
</Shape>
20
</Transform>
</World>
</Scene>
</X3D>
Wydruk 4.1: Przykładowy plik x3d
Obsług˛e formatu X3D zapewniono poprzez użycie otwartej biblioteki
X3DToolkit1 .
Obsługuje ona podstawowe komponenty standardu X3D,
umożliwia ich rozszerzanie, a także definiowanie własnych. Biblioteka ta
wykorzystywana jest do ładowania oraz zapisywania plików X3D, a także
generowania i edycji drzewa sceny.
1
http://artis.imag.fr/Software/X3D/
17
4.1. Graf sceny
Rysunek 4.1: Wynikowy obraz sceny
4.1.2. Edycja drzewa sceny
Edycja drzewa sceny możliwa jest mi˛edzy innymi poprzez bezpośrednie
edytowanie pliku tekstowego X3D. Jednak aby uprościć i przyspieszyć proces modyfikacji sceny, zaprojektowano oraz zaimplementowano edytor oferujacy
˛ interfejs graficzny. Dzi˛eki temu użytkownik natychmiast widzi efekt
dokonywanych zmian w oknie prezentujacym
˛
wizualizacj˛e sceny. Edytor
umożliwia usuwanie oraz dodawanie nowych w˛ezłów do drzewa, a także
ich edycj˛e. Dodawanie nowego w˛ezła zostało sprowadzone do wyboru przez
użytkownika typu w˛ezła z listy typów dopuszczalnych w danym miejscu
drzewa. Zaznaczenie któregoś z w˛ezłów powoduje wyświetlenie panelu prezentujacego
˛
jego parametry oraz umożliwiajacego
˛
ich edycj˛e. Edytor pozwala także na zapis poszczególnych w˛ezłów do pliku oraz importowanie
ich z plików do drzewa sceny. W celu realizacji edytora użyto biblioteki
wxWidgets, która zapewnia wieloplatformowość, a korzystajac
˛ z natywnych
dla danego środowiska składników także wyglad
˛ charakterystyczny dla danej platformy.
Na rysunku 4.2 zaprezentowano wyglad
˛ edytora drzewa sceny dla prostej sceny zawierajacej
˛
model robota, natomiast na rysunku 4.3 wynikowy
model.
18
4.2. Silnik fizyczny
Rysunek 4.2: Edycja przykładowej sceny
4.2. Silnik fizyczny
4.2.1. ODE - Open Dynamics Engine
Systemy umożliwiajace
˛ symulacj˛e własności fizycznych obiektów zwykle
używaja˛ do tego celu gotowych bibliotek tzw. silników fizyki. W dziedzinie
symulatorów robotów mobilnych szczególnie cz˛esto znajduja˛ zastosowanie
trzy z nich: Bullet2 , PhysX3 oraz ODE (Open Dynamics Engine)4 .
W celu dokonania wyboru spośród dost˛epnych silników fizycznych przeanalizowano prac˛e zawierajac
˛ a˛ wyniki porównania siedmiu z nich [2]. Serie
testów wykazały, że żaden z nich nie wyróżnia si˛e znaczaco
˛ na tle pozostałych pod wzgl˛edem ogólnej oceny. Test sprawdzajacy
˛ stabilność narzuconych w symulacji ograniczeń wykazał, że najmniejszy bład
˛ wyst˛epuje przy
zastosowaniu biblioteki ODE. Ponieważ jest to istotny parametr w symulacji
obiektów składajacych
˛
si˛e z wielu brył sztywnych właśnie ten silnik fizyczny
został użyty w implementacji. Na korzyść ODE przemawia także otwartość
biblioteki oraz użycie jej w wielu istniejacych
˛
symulatorach, co powoduje,
że jest to rozwiazanie
˛
sprawdzone. [4]
2
3
4
http://bulletphysics.org/wordpress/
http://www.geforce.com/Hardware/Technologies/physx
http://www.ode.org/
19
4.2. Silnik fizyczny
Rysunek 4.3: Wynikowy model
4.2.2. Własności fizyczne obiektów
Zakres i dokładność odwzorowania symulowanych własności fizycznych
jest ściśle uzależniony od możliwości oferowanych przez silnik fizyczny ODE.
Sa˛ one jednak na tyle szerokie, że możliwe było uwzgl˛ednienie w symulatorze wszystkich liczacych
˛
si˛e w typowym zastosowaniu parametrów fizycznych.
Masa
Każdy z obiektów znajdujacych
˛
si˛e w drzewie sceny symulowany jest jako
bryła sztywna. W ogólnym przypadku obiekt taki może być zdefiniowany
przez użytkownika jako złożenie dowolnej liczby dost˛epnych brył geometrycznych tzn. sześcianu, walca i kuli. Podczas tworzenia modelu obiektu
dla każdej takiej bryły geometrycznej użytkownik ustala jej g˛estość lub
mase.
˛ W momencie uruchomienia symulacji, w procesie tworzenia świata
fizyki na podstawie drzewa sceny, ustalana jest ostateczna masa tak zdefiniowanego obiektu. Na podstawie obj˛etości brył, ich rodzaju, rozmieszczenia oraz masy lub g˛estości każdej z nich wyznaczana jest macierz tensora
bezwładności odpowiadajacego
˛
rozkładzie masy oraz wielkość masy całego
obiektu.
4.2. Silnik fizyczny
20
Wada˛ tego rozwiazania
˛
jest nieuwzgl˛ednienie przecinajacych
˛
si˛e brył geometrycznych. Masa obiektu wyznaczana jest z założeniem, że tworzace
˛ ja˛
bryły sa˛ rozłaczne.
˛
Drugim przybliżeniem wynikajacym
˛
bezpośrednio z ograniczeń ODE jest konieczność przesuni˛ecia środka masy tak aby odpowiadał
on punktowi odniesienia obiektu. Rozwiazanie
˛
odwrotne, czyli przesuni˛ecie
punktu odniesienia obiektu w miejsce środka masy zostało odrzucone, gdyż
generowałoby niejasne dla użytkownika przesuni˛ecia obiektów w momencie
uruchomienia symulacji.
Tarcie
Tarcie ślizgowe symulowane jest na podstawie przybliżonego modelu tarcia Coulomba. Jest to model zakładajacy,
˛
że siła tarcia zależy liniowo od siły
nacisku, a stosunek tych sił określa współczynnik tarcia zależny od właściwości powierzchni obu ciał m.in. ich chropowatości. Model Coulomba
powstał na drodze empirycznej i nie oddaje w pełni zjawisk zachodzacych
˛
w rzeczywistości. Jednak dla znacznej wi˛ekszości przypadków w wystarczajacym
˛
stopniu przybliża rzeczywistość, a jego mocna˛ strona˛ jest jego prostota.
ODE oferuje dwa modele tarcia b˛edace
˛
przybliżeniem modelu tarcia
Coulomba.
Pierwszy z nich zmienia interpretacj˛e współczynnika tarcia.
W modelu tym współczynnik określa maksymalna˛ sił˛e tarcia jaka może
zaistnieć w miejscu styku danych ciał. Jest to niezbyt dokładne odwzorowanie rzeczywistości, gdyż siła tarcia nie zależy w tym przypadku od siły
nacisku, ale mniej kosztowne obliczeniowo. Drugi model polega na wprowadzeniu przybliżenia poprzez dwa etapy wyznaczania sił - najpierw wyznaczane sa˛ siły nacisku z zupełnym pomini˛eciem tarcia, a nast˛epnie na
podstawie współczynników wyznaczane sa˛ maksymalne siły tarcia według
modelu Coulomba.
Implementujac
˛ system wybrano drugi model jako bardziej odpowiadajacy
˛
rzeczywistości. Wada˛ tego rozwiazania
˛
jest wi˛eksza złożoność obliczeniowa,
ale biorac
˛ pod uwag˛e pozostałe elementy symulacji wpływ na ogólna˛ wydajność jest pomijalny.
21
4.2. Silnik fizyczny
Spre˛ żystość
Dla każdej bryły geometrycznej użytkownik może ustalić wartość parametru określajacego
˛
jej spre˛ żystość. Parametr ten przyjmuje wartości w zakresie od 0 do 1, gdzie 1 oznacza zderzenie idealnie spre˛ żyste natomiast 0
brak odbicia.
Twardość
Użytkownik może także zdefiniować dla każdej bryły dwa parametry majace
˛ bezpośrednie przełożenie na parametry punktu kolizji. Sa˛ to parametr
redukcji bł˛edu - ERP (ang.
error reduction parameter) oraz CFM (ang.
constraint force mixing). W praktyce wartość obu parametrów odpowiada
twardości powierzchni kolidujacych
˛
brył, gdzie wyższe wartości odpowiadaja˛
powierzchniom bardziej mi˛ekkim.
4.2.3. Detekcja kolizji
Silnik fizyczny ODE posiada wbudowany algorytm detekcji kolizji. Algorytm ten stosowany jest przed każdym kolejnym krokiem symulacji. Jego
wynikiem sa˛ struktury danych odpowiadajace
˛ punktom kolizji zawierajace
˛
informacj˛e o kolidujacych
˛
bryłach, gł˛ebokości penetracji obu brył, normalnej punktu kolizji oraz współrz˛ednych tego punktu.
Na podstawie tych
struktur oraz opisanych powyżej współczynników fizycznych do odpowiadajacych
˛
brył sztywnych przypisywane sa˛ siły na nie działajace.
˛
Wynik
działania tych sił i ich wpływ na obiekty obliczany jest nast˛epnie w trakcie
kroku symulacji.
Naiwne sprawdzanie każdej bryły geometrycznej z każda˛ z pozostałych
ma kwadratowa˛ złożoność. Dla bardziej skomplikowanych scen staje si˛e
to zbyt kosztowne obliczeniowo, dlatego ODE umożliwia grupowanie brył
zlokalizowanych blisko siebie w przestrzenie (ang. spaces). Dzi˛eki temu
możliwe staje si˛e znaczne ograniczenie wymaganych testów poprzez stwierdzenie, które przestrzenie na pewno ze soba˛ nie koliduja˛ i pomini˛ecie testów
sprawdzajacych
˛
wzajemne kolizje brył do nich należacych.
˛
4.3. Wizualizacja
22
Stworzony symulator korzysta z tego rozwiazania
˛
poprzez kreowanie nowej przestrzeni dla każdego zdefiniowanego przez użytkownika obiektu. Ponadto przestrzenie moga˛ być zamykane w przestrzeniach wyższego poziomu
dla złożonych obiektów tworzac
˛ w ten sposób hierarchi˛e, która odzwierciedla ich strukture.
˛ Ponieważ specyfika rzeczywistych robotów mobilnych
wymaga zwykle, aby poszczególne elementy danego robota ze soba˛ nie kolidowały przyj˛eto założenie o nieistotności kolizji w ramach jednej przestrzeni.
W dalszym stopniu ogranicza to ilość koniecznych testów detekcji kolizji nie
wpływajac
˛ przy tym na wynik symulacji w typowych zastosowaniach.
4.3. Wizualizacja
W celu wizualizacji działania symulacji użyto biblioteki graficznej
OpenGL. Jej wybór spośród dwóch dominujacych
˛
obecnie rozwiaza
˛ ń jakim jest Direct3D oraz właśnie OpenGL powodowany był posiadana˛ przez
nia˛ zaleta˛ jaka˛ jest wieloplatformowość. Użycie konkurencyjnego Direct3D
ograniczałoby działanie systemu praktycznie jedynie do platformy Windows.
Dodatkowo użyto biblioteki pomocniczej freeglut5 , która jest kontynuacja˛
popularnej, lecz od dłuższego czasu już nie rozwijanej biblioteki GLUT,
a także modułu GLT ZPR6 umożliwiajacego
˛
prosta˛ nawigacj˛e w wizualizowanym świecie za pomoca˛ myszy.
Z uwagi na główne przeznaczenie symulatora jakim jest testowanie algorytmów sterowania postanowiono uprościć wizualizacj˛e świata do niezb˛ednego minimum liczac
˛ na poprawienie w ten sposób ogólnej wydajności systemu. Użytkownik ma możliwość definiowania wygladu
˛
obiektów poprzez
stosowanie podstawowych brył geometrycznych oraz określanie materiału
danej bryły, a wi˛ec m.in. jej koloru, stopnia odbicia światła otoczenia czy połysku. Użytkownik może także dowolnie ustalać oświetlenie sceny poprzez
umieszczanie w niej trzech standardowych typów świateł: kierunkowego,
5
6
http://freeglut.sourceforge.net/
http://www.nigels.com/glt/gltzpr/
23
4.4. Sterowanie
punktowego oraz stożkowego.
Zrezygnowano natomiast zarówno z moż-
liwości teksturowania obiektów jak i generowania cieni jako kosztownych
obliczeniowo i zwykle zb˛ednych przy wizualnej ocenie wyników symulacji.
System umożliwia także użytkownikowi pewna˛ kontrol˛e nad jakościa˛ wyświetlanej grafiki poprzez wybór trzech stopni detali - w praktyce sprowadza
sie˛ to do różnej liczby wierzchołków generowanych brył. Jednak głównym
parametrem majacym
˛
wpływ zarówno na wydajność systemu, jak i prezentacje˛ wizualna˛ jest liczba kroków symulacji przypadajacych
˛
na jedna˛ klatk˛e.
Duża wartość tego parametru zmniejsza płynność animacji umożliwiajac
˛
jednak przeprowadzenie symulacji z wi˛eksza˛ szybkościa.
˛ System udost˛epnia także tryb symulacji czasu rzeczywistego, gdzie średnia szybkość symulacji wynosi 1, a wi˛ec upływ czasu symulatora odpowiada rzeczywistemu.
Zachowanie takie osiagni˛
˛ eto poprzez zastosowanie zmiennej liczby kroków
symulacji na jedna˛ klatk˛e uzależnionej od chwilowej wydajności systemu.
4.4. Sterowanie
4.4.1. Architektura klient-serwer
Podstawowym przeznaczeniem systemu jest testowanie algorytmów sterowania mobilnych robotów. Powinien on zatem udost˛epniać użytkownikom
prosty sposób na odbieranie oraz przekazywanie informacji do symulatora.
W szczególności, aby możliwe było sterowanie robotem, użytkownik powinien mieć możliwość aktualizacji efektorów np.
silnika.
Natomiast aby
sterowanie to nie było przypadkowe, wymagana jest informacja na temat
aktualnego stanu świata, uzyskana np. na podstawie odczytów sensorów.
Symulacja grupy robotów zwykle wymaga ponadto uruchomienia kilku różnych sterowników w zależności od typu robota. Należy zauważyć, że bardziej złożone algorytmy sterujace
˛ moga˛ stanowić znaczace
˛ obcia˛żenie dla
mocy obliczeniowej komputera, co przy wi˛ekszej ilości robotów wpłyn˛ełoby
na wydajność całego systemu oraz szybkość symulacji.
Rozwiazaniem
˛
tego problemu jest rozdzielenie środowiska uruchomieniowego symulatora oraz sterownika, tak aby możliwe stało si˛e wykonywanie
24
4.4. Sterowanie
programu sterujacego
˛
na innej jednostce sprz˛etowej. Osiagni˛
˛ ete zostało to
poprzez zastosowanie architektury klient-serwer, gdzie symulator pełni rol˛e
serwera, natomiast programy sterujace
˛ pełnia˛ rol˛e klientów. Komunikacja
miedzy
˛
klientem a serwerem przebiega za pomoca˛ protokołu TCP/IP. Dodatkowa˛ zaleta˛ tego rozwiazania
˛
jest możliwość zastosowania sterowania
rozproszonego. Nie ma ograniczenia na liczb˛e sterowników podłaczonych
˛
do jednego robota, ani liczb˛e robotów sterowanych przez jeden program.
Funkcjonalność serwera jest osiagni˛
˛ eta przez system poprzez uruchomienie dwóch dodatkowych watków
˛
do obsługi połacze
˛ ń. Jeden z nich nasłuchuje połacze
˛ ń przychodzacych
˛
na porcie 48230. Port ten został wybrany
arbitralnie z rejestru usług i przypisanych im portów jako jeden z wolnych,
co zmniejsza ryzyko używania go przez inne programy. W przypadku udanego połaczenia
˛
watek
˛
nasłuchujacy
˛ przekazuje jego obsług˛e do drugiego
watku,
˛
który pełni rol˛e managera połacze
˛ ń. Rozwiazanie
˛
polegajace
˛ na przydzieleniu nowego watku
˛
do każdego nowego połaczenia
˛
zostało odrzucone,
gdyż przy wi˛ekszej liczbie połacze
˛ ń koszty przełaczania
˛
watków
˛
obniżyłyby
wydajność systemu. Wybierajac
˛ model jeden watek
˛
- wiele połacze
˛ ń udało
sie˛ także uniknać
˛ konieczności synchronizacji watków
˛
w tym konkretnym
przypadku.
4.4.2. Protokół komunikacji
Komunikacja mi˛edzy programami sterujacymi
˛
a serwerem prowadzona
jest według protokołu typu żadanie
˛
- odpowiedź i z założenia przebiega
w sposób synchroniczny. Program kliencki przed wysłaniem kolejnego zapytania zobowiazany
˛
jest odebrać odpowiedź serwera. Protokół definiuje
poprawne wiadomości jakie klient może przesłać do serwera, a także odpowiedzi jakie może od niego uzyskać. Każda z wiadomości posiada nagłówek o
długości 3 bajtów. Pierwszy bajt nagłówka jest identyfikatorem wiadomości
i określa jej typ. Dwa pozostałe bajty określaja˛ rozmiar całej wiadomości w
bajtach. Stosowana jest grubokońcowość (ang. big endian), tzn. najbardziej
znaczacy
˛ bajt przesyłany jest jako pierwszy.
Wyróżnić należy dwa tryby komunikacji, z jakich może korzystać program sterujacy.
˛
Jest to sterowanie synchroniczne oraz asynchroniczne.
4.4. Sterowanie
25
W przypadku sterowania synchronicznego system wykonuje kolejny krok
symulacji dopiero po otrzymaniu odpowiedniej wiadomości od programu
sterujacego
˛
pozostawiajac
˛ pełna˛ kontrol˛e nad przebiegem symulacji użytkownikowi. Sterowanie asynchroniczne wprowadza natomiast pewna˛ losowość poprzez pozwolenie symulatorowi na wykonywanie kolejnych kroków
nie czekajac
˛ na istrukcje od sterownika. Należy zauważyć, że w pewien sposób symuluje to rzeczywiste opóźnienia na drodze sterownik-urzadzenie.
˛
4.4.3. API
W celu ułatwienia tworzenia algorytmów sterujacych
˛
opracowana została
biblioteka zapewniajaca
˛ obsług˛e wyżej opisanego protokołu. Implementuje
ona protokół komunikacji zamykajac
˛ jego obsług˛e w łatwiejszych do stosowania przez użytkownika strukturach. API udost˛epnia mi˛edzy innymi
nastepuj
˛
ace
˛ klasy oraz metody:
— klasa Simulation – stanowi fasad˛e serwera symulatora; zapewnia komunikacj˛e z serwerem
— bool connect(std::string ip, bool synch) – próbuje połaczyć
˛
si˛e z serwerem o danym IP i uruchomić komunikacj˛e w trybie synchronicznym,
badź
˛ też asynchronicznym
— Robot* getRobot(std::string robotName) – zwraca wskaźnik na obiekt
reprezentujacy
˛ robota o danej nazwie
— bool step(int timeStep) – w trybie synchronicznym wykonuje krok symulacji o danej w milisekundach wielkości
— klasa Robot – fasada konkretnego robota; umożliwia jego sterowanie
— DistanceSensor* getDistanceSensor(std::string name) – zwraca wskaźnik na obiekt reprezentujacy
˛ czujnik odległości o danej nazwie
— GPS* getGPS(std::string name) – zwraca wskaźnik na obiekt reprezentujacy
˛ urzadzenie
˛
GPS o danej nazwie
— Motor* getMotor(std::string name) – zwraca wskaźnik na obiekt reprezentujacy
˛ silnik o danej nazwie
— klasa DistanceSensor – fasada czujnika odległości
— bool getDistance(float distance) – zwraca zmierzona˛ odległość
4.4. Sterowanie
26
— klasa GPS – fasada urzadzenia
˛
GPS
— bool getPosition(float x, float y, float z) – zwraca pozycj˛e GPS
— klasa Motor – fasada silnika
— bool getVelocity(float velocity) – zwraca obecna˛ pre˛ dkość
— bool setVelocity(float velocity) – ustawia pożadan
˛
a˛ pre˛ dkość
Korzystajac
˛ z klas oferowanych przez API możliwe jest zatem utworzenie
sterownika bez znajomości wykorzystywanego protokołu. Przykład zastosowania zaprezentowany jest na wydruku 4.2.
1
Simulation simulation = Simulation ( ) ;
simulation . connect ( " 127.0.0.1 " , true ) ;
Robot∗ robot = simulation . getRobot ( " robot1 " ) ;
Motor∗ motorR = robot−>getMotor ( " motor_right " ) ;
5
Motor∗ motorL = robot−>getMotor ( " m o t o r _ l e f t " ) ;
DistanceSensor ∗ dsF = robot−>getDistanceSensor ( " ds_front " ) ;
DistanceSensor ∗ dsL = robot−>getDistanceSensor ( " d s _ l e f t " ) ;
i f ( motorR && motorL && dsL && dsF )
{
10
while ( 1 ) {
i f ( ! simulation . step (200) )
break ;
f l o a t distanceF ;
dsF−>getDistance ( distanceF ) ;
15
f l o a t distanceL ;
dsL−>getDistance ( distanceL ) ;
//przy s c i a n i e − obrot w miejscu
i f ( distanceF < 1 . 0 ) {
motorR−>s e t V e l o c i t y ( −4.0) ;
20
motorL−>s e t V e l o c i t y ( 4 . 0 ) ;
}
//przeszkoda za b l i s k o − skrec w prawo
else i f ( distanceL < 0 . 4 ) {
25
motorR−>s e t V e l o c i t y ( 2 . 0 ) ;
motorL−>s e t V e l o c i t y ( 4 . 0 ) ;
}
//przeszkoda za daleko − skrec w lewo
else i f ( distanceL > 1 . 6 ) {
27
4.4. Sterowanie
motorR−>s e t V e l o c i t y ( 4 . 0 ) ;
30
motorL−>s e t V e l o c i t y ( 2 . 0 ) ;
}
//jedz prosto
else {
motorR−>s e t V e l o c i t y ( 4 . 0 ) ;
35
motorL−>s e t V e l o c i t y ( 4 . 0 ) ;
}
}
}
Wydruk 4.2: Przykład zastosowania API
5. Testowanie oraz przykłady użycia
5.1. Wydajność systemu
Na scenie umieszczony został model robota posiadajacy
˛ dwa poruszajace
˛ si˛e niezależnie od siebie koła na wspólnej osi oraz koło Kastora dla
utrzymania stabilności. W celu przybliżonego odwzorowania ruchu rzeczywistego robota tego typu, wartość współczynnika tarcia koła Kastora ustawiona została na 0. Robot został wyposażony w trzy czujniki odległości jeden umieszczony na przedzie i dwa rozmieszczone symetrycznie po obu
stronach robota pod katem
˛
30 stopni wzgl˛edem jego osi. Plik opisujacy
˛
scene˛ przedstawiony został w załaczniku
˛
A.
Rysunek 5.1: Scena wallAvoiders.x3d
Do symulatora podłaczony
˛
został prosty program sterujacy,
˛
który na
podstawie wskazań czujników odległości tak aktualizuje pre˛ dkości kół, aby
uniknać
˛ zderzenia z przeszkoda.
˛ Kod programu zawarty jest w załaczniku
˛
B.
29
5.1. Wydajność systemu
Tablica 5.1: Wyniki testu dla sterowników działajacych
˛
asynchronicznie
Liczba robotów
1
2
3
5
10
Średnia liczba klatek/s
62
54
49
38
26
Tablica 5.2: Wyniki testu dla sterowników działajacych
˛
synchronicznie
Liczba robotów
1
2
3
5
10
Średnia liczba klatek/s
42
40
33
27
18
Nast˛epnie symulator uruchamiano kolejno dla rosnacej
˛
liczby robotów i
notowano przybliżona˛ średnia˛ osiagni˛
˛ etych klatek na sekund˛e. Symulacj˛e
uruchomiono w trybie czasu rzeczywistego - jej szybkość wynosiła wi˛ec
1. Krok symulacji wynosił 5 ms. Eksperyment zrealizowano zarówno dla
programów sterujacych
˛
podłaczonych
˛
asynchronicznie jak i synchronicznie.
Wyniki zebrano w tabelach 5.1 i 5.2.
Osiagni˛
˛ ete wyniki pokazuja,
˛ że dla prostych modeli system nadaje si˛e do
symulacji środowisk wieloagentowych w czasie rzeczywistym. Pozwala on
przy tym na płynna˛ animacj˛e eksperymentu. Wyraźna różnica mi˛edzy wynikami działania systemu dla obu typów połacze
˛ ń programów sterujacych
˛
wskazuje na znaczne opóźnienia wprowadzane przez komunikacj˛e z serwerem.
W kolejnym teście sprawdzono maksymalna˛ szybkość symulacji możliwa˛ do osiagni˛
˛ ecia przy zachowaniu płynności animacji - za jej granic˛e
przyj˛eto 25 klatek na sekund˛e. Eksperyment uruchomiono z jednym robotem obecnym na scenie i krokiem symulacji wynoszacym
˛
1 ms. Gdy
program sterujacy
˛ połaczony
˛
był w trybie asynchronicznym udało si˛e uzyskać szybkość 7,5-krotnie wyższa˛ od rzeczywistej. W przypadku połaczenia
˛
synchronicznego uzyskana szybkość była wyższa od rzeczywistej około 1,6
razy. Również i w tym teście widoczny jest wyraźny wpływ czasu komunikacji klient-serwer na wyniki. Należy zauważyć, że wpływ ten wzrasta
wraz z szybkościa˛ symulacji, gdyż zwi˛eksza si˛e także odpowiednio liczba
wymienianych komunikatów w jednostce czasu.
30
5.2. Wiarygodność symulacji
5.2. Wiarygodność symulacji
Na scenie umieszczony został robot posiadajacy
˛ dwa poruszajace
˛ si˛e niezależnie od siebie koła na wspólnej osi oraz jedno koło Kastora. Robot został
wyposażony w dwa czujniki odległości - jeden centralnie z przodu, drugi
umieszczony po lewej stronie pod katem
˛
60 stopni wzgl˛edem pierwszego,
a także moduł GPS umożliwiajacy
˛ sprawdzenie globalnych współrz˛ednych
robota. Plik opisujacy
˛ scen˛e przedstawiony został w załaczniku
˛
C.
Rysunek 5.2: Scena wallFollower.x3d
Algorytm sterujacy
˛ robotem na podstawie wskazań czujników odległości szacuje położenie robota wzgl˛edem ściany i tak aktualizuje pre˛ dkości
obu kół, aby utrzymać odległość w zadanym przedziale. Wynikiem takiego
układu sterowania jest robot poda˛żajacy
˛ prawa˛ strona˛ ściany. Program
sterujacy
˛ w zadanych odst˛epach czasu wyświetla uzyskane dane na temat
lokalizacjii robota w symulowanym świecie. Kod programu zawarty jest w
załaczniku
˛
D.
Program sterujacy
˛ podłaczono
˛
do symulatora w trybie synchronicznym,
a nast˛epnie kilkukrotnie uruchomiono symulacj˛e startujac
˛ każdorazowo
z tego samego stanu świata.
Za każdym razem uzyskiwano identyczne
wyniki odczytu pozycji robota co można uznać za potwierdzenie stabilności
oraz powtarzalności symulacji.
6. Podsumowanie
Efektem pracy jest symulator umożliwiajacy
˛ łatwe testowanie algorytmów sterowania, jak również modelowanie prostych obiektów z uwzgl˛ednieniem ich właściwości fizycznych. System oferuje także wizualizacj˛e trójwymiarowego świata, której jakość regulowana jest do pewnego stopnia przez
użytkownika. Dzi˛eki temu użytkownik może dostosować sposób działania
symulatora do przeprowadzanego eksperymentu. Obecnie niemożliwe jest
zupełne wyłaczenie
˛
wizualizacji, co w niektórych zastosowaniach mogłoby
okazać si˛e bardzo przydatne. Warto wi˛ec zastanowić si˛e nad implementacja˛
tej funkcji w przyszłości.
Testy wykazały pewna˛ wad˛e systemu, jaka˛ jest wpływajacy
˛ na efektywność symulacji czas komunikacji programów sterujacych
˛
z serwerem. Zjawisko to jest efektem wyboru architektury zapewniajacej
˛
użytkownikowi jak
najwi˛eksza˛ elastyczność w zakresie testowania algorytmów sterowania. Niestety z tego wzgl˛edu wprowadzane opóźnienie jest niemożliwe do wyeliminowania. Próba˛ złagodzenia tego efektu jest udost˛epnienie trybu połaczenia
˛
asynchronicznego, gdzie opóźnienie w komunikacji nie przekłada si˛e na
sama˛ symulacj˛e lecz na sterownik, podobnie jak to ma miejsce w rzeczywistości.
Autor dołożył wszelkich starań, aby zapewnić otwartość oraz przenośność systemu. Użyte biblioteki wybierane były ze szczególnym naciskiem
na ich wieloplatformowość oraz zgodność z idea˛ wolnego i otwartego oprogramowania. Dzi˛eki temu istnieje możliwość zapewnienia działania symulatora dodatkowo na systemach z rodziny Unix oraz Mac OS X.
Dalszy rozwój systemu możliwy jest zarówno poprzez implementacje˛ dodatkowych sensorów i efektorów, jak np. kamery czy czujników dotyku,
jak i rozszerzenie zakresu stosowania symulatora poprzez udost˛epnienie
6. Podsumowanie
32
użytkownikowi możliwości stosowania mapy wysokości. Dzi˛eki temu możliwe stałoby si˛e symulowanie nierówności podłoża, co obecnie jest znacznie
utrudnione. Dodatkowo prawdopodobne jest zwi˛ekszenie grupy potencjalnych użytkowników systemu poprzez stworzenie API implementujacego
˛
protokół komunikacji w innych j˛ezykach niż obecnie wspomagany C++.
Bibliografia
[1] Bar-Zeev,
Avi.
Ścenegraphs:
Past,
Present,
and
Future",
http://www.realityprime.com/articles/scenegraphs-past-present-and-future
[2] Boeing A., Braunl T.: Evaluation of real-time physics simulation systems. In:
Proceedings of the 5th International Conference on Computer Graphics and
Interactive Techniques in Australia and Southeast Asia - GRAPHITE 2007, vol.
1(212), p. 281, 2007
[3] Cyberbotics Webots, http://www.cyberbotics.com/documentation
[4] Drumwright E., Hsu J., Koenig N., and Shell D., Extending
˛
Open Dynamics
Engine for Robotics Simulation"in Proceedings of SIMPAR 2010 Workshops International Conference on Simulation, Modeling, and Programming for Autonomous Robots, (Darmstadt), p. 38-50, Springer-Verlag, 2010.
[5] Juhasz T., Vajta L.: New challenges in mobile robot simulation systems, Proceedings of 1st CLAWAR/EURON Workshop on Robots in Entertainment, Leisure
and Hobby, 2-4. December 2004, Vienna, Austria.
[6] Reckhaus M., Hochgeschwender N., Paulus J., Shakihimardanov A., and Kraetzschmar G. K., An
˛ Overview about Simulation and Emulation in Robotics"in
Proceedings of SIMPAR 2010 Workshops International Conference on Simulation, Modeling, and Programming for Autonomous Robots, (Darmstadt), pp.
365-374, Springer-Verlag, 2010.
[7] Reckhaus M., Hochgeschwender N., Paulus J., Shakihimardanov A., and Kraetzschmar G. K., Ón the role of simulation in the robot development process"in
Proceedings of SIMPAR 2010 Workshops International Conference on Simulation, Modeling, and Programming for Autonomous Robots, (Darmstadt), pp.
375-384, Springer-Verlag, 2010.
[8] OpenGL API Documentation Overview, http://www.opengl.org/documentation
[9] Player Project, http://playerstage.sourceforge.net
[10] Simbad Project Home, http://www.simbad.sourceforge.net
[11] SimRobot http://www.informatik.uni-bremen.de/simrobot
[12] Web3D consortium: „What is X3D?”, http://www.web3d.org/about/overview/
A. Plik wallAvoiders.x3d
1
<?xml version= " 1.0 " encoding= "UTF−8" ?>
<X3D>
<head>
</head>
5
<Scene>
<World runRealTime= "TRUE" >
<PointLight l o c a t i o n = " 0 10 0 " />
<Group>
<Transform t r a n s l a t i o n = " 0 0.25 −10" >
10
<Shape>
<Appearance>
<Material ambientIntensity= " 0.5 " d i f f u s e C o l o r = " 0.5 0 0 " />
</Appearance>
<Box s i z e = " 20.2 0.5 0.2 " />
15
</Shape>
<Solid>
<Box s i z e = "20 0.5 0.2 " />
</Solid>
</Transform>
20
<Transform t r a n s l a t i o n = " 0 0.25 10" >
<Shape>
<Appearance>
<Material ambientIntensity= " 0.5 " d i f f u s e C o l o r = " 0.5 0 0 " />
</Appearance>
25
<Box s i z e = " 20.2 0.5 0.2 " />
</Shape>
<Solid>
<Box s i z e = "20 0.5 0.2 " />
</Solid>
30
</Transform>
<Transform r o t a t i o n = " 0 1 0 1.5708 " t r a n s l a t i o n = "10 0.25 0 " >
<Shape>
<Appearance>
A. Plik wallAvoiders.x3d
35
<Material ambientIntensity= " 0.5 " d i f f u s e C o l o r = " 0.5 0 0 " />
35
</Appearance>
<Box s i z e = " 20.2 0.5 0.2 " />
</Shape>
<Solid>
<Box s i z e = "20 0.5 0.2 " />
40
</Solid>
</Transform>
<Transform r o t a t i o n = " 0 1 0 1.5708 " t r a n s l a t i o n = "−10 0.25 0 " >
<Shape>
<Appearance>
45
<Material ambientIntensity= " 0.5 " d i f f u s e C o l o r = " 0.5 0 0 " />
</Appearance>
<Box s i z e = " 20.2 0.5 0.2 " />
</Shape>
<Solid>
50
<Box s i z e = "20 0.5 0.2 " />
</Solid>
</Transform>
<Transform t r a n s l a t i o n = "−2 0.25 4 " >
<Shape>
55
<Appearance>
<Material ambientIntensity= " 0.5 " d i f f u s e C o l o r = " 0.5 0 0 " />
</Appearance>
<Box s i z e = " 6 0.5 0.2 " />
</Shape>
60
<Solid>
<Box s i z e = " 6 0.5 0.2 " />
</Solid>
</Transform>
<Transform t r a n s l a t i o n = " 7 0.25 4 " >
65
<Shape>
<Appearance>
<Material ambientIntensity= " 0.5 " d i f f u s e C o l o r = " 0.5 0 0 " />
</Appearance>
<Box s i z e = " 6 0.5 0.2 " />
70
</Shape>
<Solid>
<Box s i z e = " 6 0.5 0.2 " />
A. Plik wallAvoiders.x3d
36
</Solid>
</Transform>
75
<Transform r o t a t i o n = " 0 1 0 1.5708 " t r a n s l a t i o n = "−5 0.25 4 " >
<Shape>
<Appearance>
<Material ambientIntensity= " 0.5 " d i f f u s e C o l o r = " 0.5 0 0 " />
</Appearance>
80
<Box s i z e = " 6 0.5 0.2 " />
</Shape>
<Solid>
<Box s i z e = " 6 0.5 0.2 " />
</Solid>
85
</Transform>
<Transform r o t a t i o n = " 0 1 0 1.5708 " t r a n s l a t i o n = " 3 0.25 −8" >
<Shape>
<Appearance>
<Material ambientIntensity= " 0.5 " d i f f u s e C o l o r = " 0.5 0 0 " />
90
</Appearance>
<Box s i z e = " 4 0.5 0.2 " />
</Shape>
<Solid>
<Box s i z e = " 4 0.5 0.2 " />
95
</Solid>
</Transform>
<Transform t r a n s l a t i o n = " 0 −0.005 0 " >
<Shape>
<Appearance>
100
<Material ambientIntensity= " 1 " d i f f u s e C o l o r = " 0.67 1 0.5 " />
</Appearance>
<Box s i z e = "20 0.01 20" />
</Shape>
</Transform>
105
<Transform r o t a t i o n = " 0 1 0 0 " t r a n s l a t i o n = "−8 0.25 −4" >
<Shape>
<Appearance>
<Material ambientIntensity= " 0.5 " d i f f u s e C o l o r = " 0.5 0 0 " />
</Appearance>
110
<Box s i z e = " 4 0.5 0.2 " />
</Shape>
A. Plik wallAvoiders.x3d
<Solid>
<Box s i z e = " 4 0.5 0.2 " />
</Solid>
115
</Transform>
</Group>
<Robot objectName= " robot1 " r o t a t i o n = " 0 −1 0 0 "
t r a n s l a t i o n = " 0 0.22 0 " >
<Solid>
120
<Box s i z e = " 0.4 0.35 0.4 " />
</Solid>
<Shape>
<Appearance>
<Material d i f f u s e C o l o r = " 0.5 0.5 0.75 " />
125
</Appearance>
<Box s i z e = " 0.4 0.35 0.4 " />
</Shape>
<Transform t r a n s l a t i o n = " 0 0.2 0 " >
<Solid>
130
<Cylinder height= " 0.05 " radius= " 0.3 " />
</Solid>
<Shape>
<Appearance>
<Material d i f f u s e C o l o r = " 0 0.5 0.25 " />
135
</Appearance>
<Cylinder height= " 0.05 " radius= " 0.3 " />
</Shape>
</Transform>
<Motor maxForce= "10" maxStop= " 4 " minStop= "−4"
140
objectName= " m o t o r _ l e f t " r o t a t i o n = "−1 0 0 0 "
t r a n s l a t i o n = " 0.25 −0.02 0 " >
<Transform r o t a t i o n = " 0 0 1 1.5708 " >
<Solid>
<Cylinder height= " 0.02 " radius= " 0.2 " />
145
</Solid>
<Shape>
<Appearance>
<Material ambientIntensity= " 0.8 "
d i f f u s e C o l o r = " 0.87 0.87 0.87 " />
150
</Appearance>
37
A. Plik wallAvoiders.x3d
<Cylinder height= " 0.02 " radius= " 0.2 " side= "FALSE" />
</Shape>
<Shape>
<Appearance>
<Material d i f f u s e C o l o r = " 0 0.5 0.25 " />
155
</Appearance>
<Cylinder bottom= "FALSE" height= " 0.02 " radius= " 0.2 "
top= "FALSE" />
</Shape>
160
</Transform>
</Motor>
<Motor maxForce= "10" maxStop= " 4 " minStop= "−4"
objectName= " motor_right " r o t a t i o n = " 1 0 0 0 "
t r a n s l a t i o n = " −0.25 −0.02 0 " >
165
<Transform r o t a t i o n = " 0 0 1 1.5708 " >
<Solid>
<Cylinder height= " 0.02 " radius= " 0.2 " />
</Solid>
<Shape>
<Appearance>
170
<Material ambientIntensity= " 0.8 "
d i f f u s e C o l o r = " 0.87 0.87 0.87 " />
</Appearance>
<Cylinder height= " 0.02 " radius= " 0.2 " side= "FALSE" />
175
</Shape>
<Shape>
<Appearance>
<Material d i f f u s e C o l o r = " 0 0.5 0.25 " />
</Appearance>
<Cylinder bottom= "FALSE" height= " 0.02 " radius= " 0.2 "
180
top= "FALSE" />
</Shape>
</Transform>
</Motor>
185
<DistanceSensor objectName= " d s _ l e f t " r o t a t i o n = " 0 1 0 0.5236 "
t r a n s l a t i o n = " 0.21 0.2 0.21 " >
<Shape>
<Appearance>
<Material d i f f u s e C o l o r = " 1 0 0 " />
38
A. Plik wallAvoiders.x3d
190
39
</Appearance>
<Sphere radius= " 0.01 " />
</Shape>
</DistanceSensor>
<DistanceSensor DEF= " _x3dtkN0 " objectName= " ds_front "
195
r o t a t i o n = " 0 1 0 0 " t r a n s l a t i o n = " 0 0.2 0.3 " >
<Shape>
<Appearance>
<Material d i f f u s e C o l o r = " 1 0 0 " />
</Appearance>
200
<Sphere radius= " 0.01 " />
</Shape>
</DistanceSensor>
<DistanceSensor objectName= " ds_right " r o t a t i o n = " 0 −1 0 0.5236 "
t r a n s l a t i o n = " −0.21 0.2 0.21 " >
205
<Shape>
<Appearance>
<Material d i f f u s e C o l o r = " 1 0 0 " />
</Appearance>
<Sphere radius= " 0.01 " />
210
</Shape>
</DistanceSensor>
<Motor maxForce= " 0 " objectName= " caster " r o t a t i o n = " 0 0 1 1.5708 "
t r a n s l a t i o n = " 0 0 0.2 " >
<Motor maxForce= " 0 " r o t a t i o n = " 0 0 1 1.5708 "
215
t r a n s l a t i o n = " −0.19 0 0 " >
<Solid mu= " 0 " >
<Sphere radius= " 0.03 " />
</Solid>
<Shape>
<Appearance>
220
<Material ambientIntensity= " 0.4 "
d i f f u s e C o l o r = " 0.87 0.87 0.87 " shininess= " 0 " />
</Appearance>
<Sphere radius= " 0.03 " />
225
</Shape>
</Motor>
</Motor>
</Robot>
A. Plik wallAvoiders.x3d
</World>
230
</Scene>
</X3D>
Wydruk A.1: Plik wallAvoiders.x3d
40
B. Plik WallAvoiders.cpp
1
#include "MoRS_API . h"
#include <iostream >
5
int main ( int argc , char ∗∗ argv )
{
i f ( argc ! = 3) {
std : : cout << " Sposob wywolania programu:\n WallAvoider . exe arg1 arg2\n" ;
std : : cout << " gdzie dla wartosci arg1 \" true \" polaczenie
realizowane j e s t w t r y b i e synchronicznym " ;
10
std : : cout << " a arg2 j e s t nazwa robota\n" ;
system ( " pause " ) ;
return 0;
}
15
using namespace MORS_API;
Simulation simulation = Simulation ( ) ;
bool synch = f a l s e ;
i f ( strcmp ( argv [ 1 ] , " true " ) == 0) {
synch = true ;
std : : cout << " Komunikacja synchroniczna\n" ;
20
}
else
std : : cout << " Komunikacja asynchroniczna\n" ;
std : : cout << " Laczenie z symulatorem . . . \ n" ;
25
try {
bool r = simulation . connect ( " 127.0.0.1 " , synch ) ;
i f ( r==true ) {
std : : cout << " Nawiazano polaczenie\n" ;
Robot∗ robot = simulation . getRobot ( argv [ 2 ] ) ;
30
i f ( robot ) {
std : : cout << " Sterowanie robotem " << argv [ 2 ] << std : : endl ;
Motor∗ motorR = robot−>getMotor ( " motor_right " ) ;
Motor∗ motorL = robot−>getMotor ( " m o t o r _ l e f t " ) ;
B. Plik WallAvoiders.cpp
DistanceSensor ∗ dsR = robot−>getDistanceSensor ( " ds_right " ) ;
35
DistanceSensor ∗ dsL = robot−>getDistanceSensor ( " d s _ l e f t " ) ;
DistanceSensor ∗ dsF = robot−>getDistanceSensor ( " ds_front " ) ;
i f ( motorR && motorL && dsR && dsL && dsF )
{
while ( 1 ) {
i f ( ! simulation . step (200) )
40
break ;
f l o a t distanceL ;
dsL−>getDistance ( distanceL ) ;
f l o a t distanceR ;
dsR−>getDistance ( distanceR ) ;
45
f l o a t distanceF ;
dsF−>getDistance ( distanceF ) ;
//przeszkoda przed robotem − skrec w prawo
i f ( distanceF <= 0.95) {
50
motorR−>s e t V e l o c i t y ( −2.0) ;
motorL−>s e t V e l o c i t y ( 2 . 0 ) ;
}
//przeszkoda po lewej s t r o n i e − skrec w prawo
else i f ( distanceL < distanceR && distanceL < 0 . 5 ) {
55
motorR−>s e t V e l o c i t y ( −2.0) ;
motorL−>s e t V e l o c i t y ( 2 . 0 ) ;
}
//przeszkoda po prawej s t r o n i e − skrec w lewo
else i f ( distanceR < distanceL && distanceR < 0 . 5 ) {
60
motorR−>s e t V e l o c i t y ( 2 . 0 ) ;
motorL−>s e t V e l o c i t y ( −2.0) ;
}
//jedz prosto
else {
65
motorR−>s e t V e l o c i t y ( 5 . 0 ) ;
motorL−>s e t V e l o c i t y ( 5 . 0 ) ;
}
}
70
}
else
std : : cout << " Nie znaleziono urzadzenia
42
B. Plik WallAvoiders.cpp
43
o podanej nazwie " << std : : endl ;
}
else
75
std : : cout << " Nie znaleziono robota o podanej nazwie " << std : : endl ;
}
else {
std : : cout << " Nie udalo s i e nawiazac polaczenia " << std : : endl ;
}
80
}
catch ( std : : exception& e )
{
std : : cout << e . what ( ) << std : : endl ;
}
85
system ( " pause " ) ;
return 0;
}
Wydruk B.1: plik WallAvoiders.cpp
C. Plik wallFollower.x3d
1
<?xml version= " 1.0 " encoding= "UTF−8" ?>
<X3D>
<head>
</head>
5
<Scene>
<World runRealTime= "TRUE" >
<PointLight l o c a t i o n = " 0 10 0 " />
<Transform t r a n s l a t i o n = "−5 0.25 −10" >
<Shape>
10
<Appearance>
<Material ambientIntensity= " 0.5 " d i f f u s e C o l o r = " 0.5 0 0 " />
</Appearance>
<Box s i z e = " 10.2 0.5 0.2 " />
</Shape>
15
<Solid>
<Box s i z e = " 10.2 0.5 0.2 " />
</Solid>
</Transform>
<Transform t r a n s l a t i o n = " 0 0.25 10" >
20
<Shape>
<Appearance>
<Material ambientIntensity= " 0.5 " d i f f u s e C o l o r = " 0.5 0 0 " />
</Appearance>
<Box s i z e = " 20.2 0.5 0.2 " />
25
</Shape>
<Solid>
<Box s i z e = "20 0.5 0.2 " />
</Solid>
</Transform>
30
<Transform r o t a t i o n = " 0 1 0 1.5708 " t r a n s l a t i o n = "10 0.25 5 " >
<Shape>
<Appearance>
<Material ambientIntensity= " 0.5 " d i f f u s e C o l o r = " 0.5 0 0 " />
C. Plik wallFollower.x3d
45
</Appearance>
35
<Box s i z e = " 10.2 0.5 0.2 " />
</Shape>
<Solid>
<Box s i z e = " 10.2 0.5 0.2 " />
</Solid>
40
</Transform>
<Transform r o t a t i o n = " 0 1 0 1.5708 " t r a n s l a t i o n = "−10 0.25 0 " >
<Shape>
<Appearance>
<Material ambientIntensity= " 0.5 " d i f f u s e C o l o r = " 0.5 0 0 " />
45
</Appearance>
<Box s i z e = " 20.2 0.5 0.2 " />
</Shape>
<Solid>
<Box s i z e = "20 0.5 0.2 " />
50
</Solid>
</Transform>
<Transform r o t a t i o n = " 0 1 0 −0.7854 " t r a n s l a t i o n = " 2 0.25 −2" >
<Shape>
<Appearance>
55
<Material ambientIntensity= " 0.5 " d i f f u s e C o l o r = " 0.5 0 0 " />
</Appearance>
<Box s i z e = " 5.75 0.5 0.2 " />
</Shape>
<Solid>
60
<Box s i z e = " 5.75 0.5 0.2 " />
</Solid>
</Transform>
<Transform t r a n s l a t i o n = " 7 0.25 0 " >
<Shape>
65
<Appearance>
<Material ambientIntensity= " 0.5 " d i f f u s e C o l o r = " 0.5 0 0 " />
</Appearance>
<Box s i z e = " 6 0.5 0.2 " />
</Shape>
70
<Solid>
<Box s i z e = " 6 0.5 0.2 " />
</Solid>
C. Plik wallFollower.x3d
46
</Transform>
<Transform " r o t a t i o n = " 0 1 0 1.5708 " t r a n s l a t i o n = " 0 0.25 −7" >
75
<Shape>
<Appearance>
<Material ambientIntensity= " 0.5 " d i f f u s e C o l o r = " 0.5 0 0 " />
</Appearance>
<Box s i z e = " 6 0.5 0.2 " />
80
</Shape>
<Solid >
<Box s i z e = " 6 0.5 0.2 " />
</Solid >
</Transform>
85
<Transform t r a n s l a t i o n = " 0 −0.005 0 " >
<Shape>
<Appearance>
<Material ambientIntensity= " 1 " d i f f u s e C o l o r = " 0.5 0.5 1 " />
</Appearance>
90
<Box s i z e = "20 0.01 20" />
</Shape>
</Transform>
<Robot objectName= " robot2 " r o t a t i o n = " 0 1 0 3.1416 "
t r a n s l a t i o n = "−5 0.22 0 " >
95
<Solid >
<Cylinder height= " 0.35 " radius= " 0.4 " />
</Solid >
<Shape>
<Appearance>
100
<Material d i f f u s e C o l o r = " 0.752941 0.752941 0.752941 "
shininess= "24" specularColor= " 1 1 1 " />
</Appearance>
<Cylinder height= " 0.35 " radius= " 0.4 " />
</Shape>
105
<Motor maxForce= "10" maxStop= " 4 " minStop= "−4" p o s i t i o n = " 0 "
objectName= " m o t o r _ l e f t " r o t a t i o n = " 1 0 0 2.87247 "
t r a n s l a t i o n = " 0.4 −0.02 0 " >
<Transform r o t a t i o n = " 0 0 1 1.5708 " >
<Solid mu= "10" >
110
<Cylinder height= " 0.02 " radius= " 0.2 " />
</Solid >
C. Plik wallFollower.x3d
<Shape>
<Appearance>
<Material ambientIntensity= " 0.4 "
115
d i f f u s e C o l o r = " 0.88 0.88 0.88 " shininess= "24" specularColor= " 1 1 1 " />
</Appearance>
<Cylinder height= " 0.02 " radius= " 0.2 " />
</Shape>
</Transform>
120
</Motor>
<Motor maxForce= "10" maxStop= " 4 " minStop= "−4" p o s i t i o n = " 0 "
objectName= " motor_right " r o t a t i o n = "−1 0 0 1.45873 "
t r a n s l a t i o n = " −0.4 −0.02 0 " >
<Transform r o t a t i o n = " 0 0 1 1.5708 " >
125
<Solid mu= "10" >
<Cylinder height= " 0.02 " radius= " 0.2 " />
</Solid >
<Shape>
<Appearance>
<Material ambientIntensity= " 0.8 "
130
d i f f u s e C o l o r = " 0.88 0.88 0.88 " shininess= "24" specularColor= " 1 1 1 " />
</Appearance>
<Cylinder height= " 0.02 " radius= " 0.2 " />
</Shape>
135
</Transform>
</Motor>
<DistanceSensor length= " 2 " objectName= " d s _ l e f t "
r o t a t i o n = " 0 1 0 1.047 " t r a n s l a t i o n = " 0.28 0 0.28 " >
<Shape>
140
<Appearance>
<Material d i f f u s e C o l o r = " 1 0 0 " />
</Appearance>
<Sphere radius= " 0.01 " />
</Shape>
145
</DistanceSensor>
<DistanceSensor length= " 2 " objectName= " ds_front "
r o t a t i o n = " 0 1 0 0 " t r a n s l a t i o n = " 0 0 0.4 " >
<Shape>
<Appearance>
150
<Material d i f f u s e C o l o r = " 1 0 0 " />
47
C. Plik wallFollower.x3d
</Appearance>
<Sphere radius= " 0.01 " />
</Shape>
</DistanceSensor>
155
<Motor maxForce= " 0 " p o s i t i o n = " 0 " objectName= " caster "
r o t a t i o n = " 0 1 0 0 " t r a n s l a t i o n = " 0 −0.1975 0.4 " >
<Motor maxForce= " 0 " p o s i t i o n = " 0 " r o t a t i o n = " 0 1 0 1.5708 "
t r a n s l a t i o n = " 0 0 −0" >
<Solid mu= " 0.1 " >
<Sphere radius= " 0.0225 " />
160
</Solid >
<Shape>
<Appearance>
<Material ambientIntensity= " 0.4 "
165
d i f f u s e C o l o r = " 0.88 0.88 0.88 " shininess= " 0 " />
</Appearance>
<Sphere radius= " 0.0225 " />
</Shape>
</Motor>
170
<Solid mu= " 0.1 " >
<Sphere radius= " 0.01 " />
</Solid >
</Motor>
<GPS objectName= " gps " />
175
</Robot>
<Transform t r a n s l a t i o n = " 0 0.25 9 " >
<Shape>
<Appearance>
<Material d i f f u s e C o l o r = " 0 0.25 0.5 " />
180
</Appearance>
<Cylinder height= " 0.5 " />
</Shape>
<Solid >
<Cylinder height= " 0.5 " />
185
</Solid >
</Transform>
<Transform t r a n s l a t i o n = "−8 0.25 0 " >
<Shape>
<Appearance>
48
C. Plik wallFollower.x3d
<Material ambientIntensity= " 0.5 "
190
d i f f u s e C o l o r = " 0 0.25 0.5 " />
</Appearance>
<Box s i z e = " 4 0.5 6 " />
</Shape>
<Solid >
195
<Box s i z e = " 4 0.5 6 " />
</Solid >
</Transform>
</World>
200
</Scene>
</X3D>
Wydruk C.1: Plik wallFollower.x3d
49
D. Plik WallFollower.cpp
1
#include "MoRS_API . h"
#include <iostream >
int main ( int argc , char ∗∗ argv )
5
{
i f ( argc ! = 3) {
std : : cout << " Sposob wywolania programu:\n
WallFollower . exe arg1 arg2\n" ;
std : : cout << " gdzie dla wartosci arg1 \" true \" polaczenie
realizowane j e s t w t r y b i e synchronicznym " ;
10
std : : cout << " a arg2 j e s t nazwa robota\n" ;
system ( " pause " ) ;
return 0;
}
15
using namespace MORS_API;
Simulation simulation = Simulation ( ) ;
bool synch = f a l s e ;
i f ( strcmp ( argv [ 1 ] , " true " ) == 0) {
synch = true ;
std : : cout << " Komunikacja synchroniczna\n" ;
20
}
else
std : : cout << " Komunikacja asynchroniczna\n" ;
std : : cout << " Laczenie z symulatorem . . . \ n" ;
25
try {
bool r = simulation . connect ( " 127.0.0.1 " , synch ) ;
i f ( r==true ) {
std : : cout << " Nawiazano polaczenie\n" ;
Robot∗ robot = simulation . getRobot ( argv [ 2 ] ) ;
30
i f ( robot ) {
std : : cout << " Sterowanie robotem " << argv [ 2 ] << std : : endl ;
Motor∗ motorR = robot−>getMotor ( " motor_right " ) ;
Motor∗ motorL = robot−>getMotor ( " m o t o r _ l e f t " ) ;
D. Plik WallFollower.cpp
DistanceSensor ∗ dsF = robot−>getDistanceSensor ( " ds_front " ) ;
35
DistanceSensor ∗ dsL = robot−>getDistanceSensor ( " d s _ l e f t " ) ;
GPS∗ gps = robot−>getGPS ( " gps " ) ;
int count = 0;
i f ( motorR && motorL && dsL && dsF && gps )
{
40
while ( 1 ) {
i f ( ! simulation . step (200) )
break ;
i f ( count % 100 == 0) {
float x , y , z ;
gps−>g e t P o s i t i o n ( x , y , z ) ;
45
std : : cout << " x : " << x << " \ny : " <<
y << " \nz : " << z << " \n" ;
}
count ++;
50
f l o a t distanceF ;
dsF−>getDistance ( distanceF ) ;
f l o a t distanceL ;
55
dsL−>getDistance ( distanceL ) ;
//przy s c i a n i e − obrot w miejscu
i f ( distanceF < 1 . 0 ) {
motorR−>s e t V e l o c i t y ( −4.0) ;
motorL−>s e t V e l o c i t y ( 4 . 0 ) ;
60
}
//przeszkoda za b l i s k o − skrec w prawo
else i f ( distanceL < 0 . 4 ) {
motorR−>s e t V e l o c i t y ( 2 . 0 ) ;
motorL−>s e t V e l o c i t y ( 4 . 0 ) ;
65
}
//przeszkoda za daleko − skrec w lewo
else i f ( distanceL > 1 . 6 ) {
motorR−>s e t V e l o c i t y ( 4 . 0 ) ;
motorL−>s e t V e l o c i t y ( 2 . 0 ) ;
70
}
//jedz prosto
51
52
D. Plik WallFollower.cpp
else {
motorR−>s e t V e l o c i t y ( 4 . 0 ) ;
motorL−>s e t V e l o c i t y ( 4 . 0 ) ;
75
}
}
}
else
std : : cout << " Nie znaleziono urzadzenia
80
o podanej nazwie " << std : : endl ;
}
else
std : : cout << " Nie znaleziono robota o podanej nazwie " << std : : endl ;
}
85
else {
std : : cout << " Nie udalo s i e nawiazac polaczenia " << std : : endl ;
}
}
catch ( std : : exception& e )
90
{
std : : cout << e . what ( ) << std : : endl ;
}
system ( " pause " ) ;
return 0;
95
}
Wydruk D.1: plik WallFollower.cpp
E. Instrukcja obsługi programu
E.1. Okna programu
Program posiada dwa okna: główne okno programu oraz okno wizualizacji 3D. W głównym oknie możemy wyróżnić: menu programu (1), pasek
narzedzi
˛
(2), drzewo sceny (3), panel edycji (4). W oknie wizualizacji: licznik klatek na sekund˛e (5), aktualna pre˛ dkość symulacji (6), osie globalnego
układu współrz˛ednych (7), wizualizacja sceny (8).
Rysunek E.1: główne okno programu
E.2. Interfejs programu
54
Rysunek E.2: okno wizualizacji 3D
E.2. Interfejs programu
E.2.1. File menu
New - tworzy nowy dokument
Open... - pozwala użytkownikowi na otworzenie pliku x3d
Close - zamyka aktualny dokument
Save - zapisuje aktualny dokument
Save as... - pozwala zapisać użytkownikowi aktualny dokument pod
inna˛ nazwa˛
Exit - zamyka program
E.2.2. View menu
Low quality - ustawia niska˛ jakość renderowania
Medium quality - ustawia średnia˛ jakość renderowania
High quality - ustawia wysoka˛ jakość rednerowania
E.2. Interfejs programu
55
E.2.3. Help menu
About... - wyświetla okno z informacja˛ o programie
E.2.4. Pasek narzedzi
˛
New: jak polecenie File->New
Open: jak polecenie File->Open
Save: jak polecenie File->Save
Add node: pozwala użytkownikowi na dodanie w˛ezła do drzewa sceny
Delete node:
usuwa zaznaczony w˛ezeł oraz wszystkie jego w˛ezły
potomne
Import node: importuje w˛ezeł lub drzewo z pliku i wstawia w aktualnym
miejscu w drzewie sceny
E.3. Drzewo sceny
56
Export node: eksportuje zaznaczony w˛ezeł oraz jego w˛ezły potomne do
pliku
Start simulation: rozpoczyna symulacj˛e
Pause simulation: pauzuje symulacj˛e
Stop simulation: zatrzymuje symulacj˛e
E.3. Drzewo sceny
Dodawanie wezła
˛
Aby dodać do drzewa sceny nowy w˛ezeł należy zaznaczyć w˛ezeł, który
bedzie
˛
jego rodzicem, a nast˛epnie wybrać z paska narz˛edzi polecenie Add
˛
node". Pojawi si˛e okno z lista˛ typów w˛ezłów, które moga˛ zostać dodane w
danym miejscu drzewa. Wybór typu w˛ezła powoduje dodanie nowego w˛ezła
do drzewa sceny.
Usuwanie wezła
˛
Aby usunać
˛ w˛ezeł z drzewa należy zaznaczyć wybrany w˛ezeł, a nast˛epnie
wybrać z paska narz˛edzi polecenie "Delete node". Usuni˛ety zostanie zarówno
zaznaczony w˛ezeł, jak i wszystkie jego w˛ezły potomne.
E.4. Okno wizualizacji 3D
57
Edycja wezła
˛
Aby edytować parametry w˛ezła należy zaznaczyć wybrany w˛ezeł.
W
panelu edycji pojawia˛ si˛e nazwy oraz wartości parametrów w˛ezła. Klikni˛ecie
pola z wartościa˛ umożliwia jego edycj˛e. Zatwierdzenie edycji nast˛epuje po
przyciśni˛eciu klawisza ENTER, badź
˛ też poprzez opuszczenie edytowanego
pola np. na skutek klikni˛ecia innego pola.
E.4. Okno wizualizacji 3D
Nawigacja
Nawigacja po scenie możliwa jest za pomoca˛ myszki. Możliwe jest obracanie, przesuwanie oraz przybliżanie sceny.
Obracanie sceny możliwe jest poprzez przyciśni˛ecie LPM i przesuni˛ecie
myszki.
Obrót dokonywany jest wokół poczatku
˛
układu współrz˛ednych
zaznaczonego w˛ezła.
Przesuwanie sceny możliwe jest poprzez przyciśni˛ecie PPM i przesuni˛ecie myszki.
Przybliżanie i oddalanie sceny możliwe jest poprzez wciśni˛ecie LPM i
PPM jednocześnie (lub środkowego przycisku myszy), a nast˛epnie przesuniecie
˛
myszki odpowiednio w góre˛ lub w dół.