Zad. 6: Sterowanie manipulatorem – przypadek 2D

Zad. 6: Sterowanie manipulatorem – przypadek 2D
1
Cel ćwiczenia
Wykształcenie umiej˛etności korzystania z szablonu vector<> oraz dalsze rozwijanie umiej˛etności projektowania struktur danych oraz definiowanie odpowiednich przecia˛żeń operatorów.
2
Program zaj˛eć
• Ocena realizacji zadania z poprzedniego laboratorium – ocenie podlega poprawność realizacji zadania, styl pisania programu oraz dokumentacja wygenerowana za pomoca˛ systemu doxygen.
• Ocena przygotowania do zaj˛eć – ocenie podlega diagram klas i diagram czynności (patrz
rozdział 4).
• Modyfikacja programu wg wskazań osoby prowadzacej
˛ – ocenie b˛edzie podlegała poprawność realizacji modyfikacji. Prac˛e nad modyfikacja˛ programu (wszystkie operacje
należy wykonywać na kopii) należy rozpoczać
˛ już w trakcie pierwszej fazy laboratorium,
gdyż prowadzacy
˛ nie b˛edzie w stanie ocenić wcześniejszego programu wszystkim jednocześnie.
• Realizacja wst˛epnej fazy prac nad nowym zadaniem – należy stworzyć uproszczona˛ wersj˛e programu, która rysuje zmiany postury manipulatora składajacego
˛
si˛e z pojedynczego
ogniwa. Głównym celem jest przećwiczenie użycia modułu lacze_do_gnuplota.
• Ocena realizacji wst˛epnej fazy zadania
3
Opis zadania programowego
Należy napisać program, który umożliwi wyznaczenie postury manipulatora. Zakładamy, że
manipulator przedstawiony jest schematycznie w postaci łamanej (patrz rysunek poniżej). Każdy
z segmentów łamanej ma symbolizować pojedyncze ogniowo ramienia manipulatora. Tak wi˛ec
w ten sposób postawione zadanie sprowadza si˛e do wyznaczenia położenia poszczególnych
przegubów manipulatora i jego efektora. Postura manipulatora jest zadana poprzez wartości
katów
˛
q0 , q1 , q2 , . . . mi˛edzy kolejnymi ogniwami ramienia. Dla uproszczenia, postury manipulatora b˛eda˛ rozważane tylko w jednej płaszczyźnie XOY . Zakłada si˛e, że liczba ogniw ramienia
nie jest ograniczona Przyjmuje si˛e, że długość każdego z ogniw może być różna. Konfiguracja
manipulatora (tzn. liczba ogniw oraz ich długości) b˛eda˛ zadane w pliku tekstowym. Podobnie postury manipulatora b˛eda˛ również zapisane osobnym pliku tekstowym. Program powinien
umieć wczytać wspomniane dane i nast˛epnie wyliczyć oraz zobrazować postur˛e manipulatora.
Program nie powinien posiadać żadnego menu.
4
Przygotowanie do zaj˛eć
Należy przygotować wst˛epny diagram czynności obrazujacy
˛ sposób wyliczenia postury manipulatora. Ponadto należy stworzyć diagram klas dla problemu rozwiazywanego
˛
w ramach tego
1
Rysunek 1: Szkielet manipulatora i zaznaczonymi katami
˛
pozwalajacymi
˛
sterowanie nachyleniem poszczególnych ogniw manipulatora.
zadania. Diagramy powinny być przygotowane w wersji elektronicznej.
5
Wyznaczanie współrz˛ednych przegubów
Istnieje ogólna procedura wyznaczania współrz˛ednych kolejnych ogniw łańcucha kinematycznego. Została ona opisana w materiałach dodatkowych (patrz strona kursu → tabela zadań →
wiersz tego zadania → kolumna Dod.). Osoby, którym programowanie nie sprawia problemu,
zach˛eca si˛e do zastosowania podejścia ogólnego. Niemniej w wersji podstawowej można skorzystać prostszej metody, która˛ można zastosować dla manipulatora planarnego dla przypadku
2D. Tak można postapić
˛ pod warunkiem, że osoba prowadzaca
˛ laboratorium nie zdecyduje inaczej. W dalszej cz˛eści przedstawiona jest wspomniana prostsza metoda.
Tak jak w metodzie ogólnej, z każdym przegubem zwia˛żmy lokalny układ współrz˛ednych.
Inaczej niż w metodzie ogólnej, wybierzmy osie OX oraz OY w taki sposób, aby były one
równoległe do analogicznych osi układu globalnego (patrz rys. 2). Współrz˛edne punktu P0 w
Rysunek 2: Szkielet manipulatora
układzie globalnym sa˛ oczywiście równe (0, 0). Łatwo zauważyć, że współrz˛edne punktu P1
możemy wyznaczyć bezpośrednio jako
P1 = (l1 cos q0 , l1 sin q0 )
2
gdzie l1 jest długościa˛ pierwszego ogniwa. Jednakże należy pami˛etać, że jest to uproszczenie
wynikajace
˛ z faktu, że przegub P0 znajduje si˛e w środku układu współrz˛ednych i lokalny układ
współrz˛ednych zwiazany
˛
z tym przegubem pokrywa si˛e z układem współrz˛ednych globalnych.
Jeśli tak nie jest, trzeba to uwzgl˛ednić dodajac
˛ współrz˛edne tego punktu, a wi˛ec ogólnie jest:
P1 = P10 + P0 ,
gdzie P10 = (l1 cos q1 , l1 sin q1 ).
Współrz˛edne punktu P2 w lokalnym układzie współrz˛ednych X1 OY1 możemy wyrazić analogicznie jako
P21 = (l2 cos α, l2 sin α)
(1)
gdzie l2 to długość drugiego ogniwa. Z faktu, że odpowiadajace
˛ sobie osie układów współrz˛ednych XOY oraz X1 OY1 sa˛ równoległe wynika bezpośrednio, że kat
˛ α to
α = q0 + q1 .
(2)
Co widać bezpośrednio na rys. 2. Należy przy tym pami˛etać, że dodatni kierunek miary kata
˛
przyjmujemy jako przeciwny do ruchu wskazówek zegara. Tak wi˛ec w tym przypadku q0 ∼
110◦ , zaś q1 ∼ −70◦ (kat
˛ q1 ma wartość ujemna,˛ gdyż na rys. 2 mierzony jest on w kierunku
zgodnym z ruchem wskazówek zegara). Tak wi˛ec kat
˛ α to ok. 40◦ .
Uwzgl˛edniajac
˛ (2) wzór (1) możemy przepisać w postaci
P21 = (l2 cos(q0 + q1 ), l2 sin(q0 + q1 )).
Aby wyznaczyć współrz˛edne punktu P2 w globalnym układzie XOY , należy zauważyć, że
układy XOY oraz X1 OY1 sa˛ wzgl˛edem siebie tylko przesuni˛ete, a współrz˛edne wektora translacji, to nic innego jak współrz˛edne punktu P1 . Tak wi˛ec
P2 = P21 + P1 .
Obliczenie współrz˛ednych punktu P3 realizujemy w analogiczny sposób. Najpierw wyznaczamy je w lokalnym układzie współrz˛ednych X2 OY2 . Wykonujac
˛ analogiczna˛ analiz˛e możemy
stwierdzić, że
β = q0 + q1 + q2 .
Tak wi˛ec
P32 = (l3 cos(q0 + q1 + q2 ), l3 sin(q0 + q1 + q2 )).
Chcac
˛ przejść ze współrz˛ednych lokalnych układu X2 OY2 do współrz˛ednych globalnego układu
XOY korzystamy z faktu, że układy te sa˛ przesuni˛ete mi˛edzy soba˛ o wektor ~T2 , którego współrz˛ednymi sa˛ współrz˛edne punktu P2 . Tak wi˛ec
P3 = P32 + P2 .
Z nast˛epnymi przegubami post˛epujemy analogicznie. Zaleta˛ tej procedury jest to, że w nast˛epnym kroku, w którym wyznaczmy współrz˛edne przegubu Pi możemy wykorzystać sum˛e katów
˛
q0 , qq , . . . , qi−1 oraz współrz˛edne przegubu Pi−1 wyznaczone w kroku poprzednim.
Uwaga: Należy zauważyć, że
P = (l sin α, l cos α) = l(cos α, sin α).
Mamy tu do czynienia z mnożeniem wektora przez liczb˛e. Pozwala to w bardziej zwarty
sposób zapisać operacje w prezentowanej metodzie i tak należy postapić.
˛
3
Rysunek 3: Powiazanie
˛
mi˛edzy układami współrz˛ednych XOY oraz X2 OY2
6
Działanie programu
Program ma nie mieć żadnego menu. Wszystkie dane (tzn. konfiguracja manipulatora i postury)
przekazywane sa˛ poprzez pliki. Nazwy plików umieszczane sa˛ w linii wywołania programu.
Składnia wywołania programu:
./a.out plik_konfiguracji.dat
plik_postury1.dat [ plik_postury2.dat ... ]
Pierwsza˛ nazwa˛ jest nazwa pliku zawierajacego
˛
konfiguracj˛e manipulatora. Jest to plik tekstowy zawierajacy
˛ liczby określajace
˛ długość poszczególnych ogniw w kolejności od pierwszego ogniwa. Przykład zawartości pliku plik_konfiguracji.dat:
40 45.5
64.5
32
Poszczególne liczby moga˛ być oddzielone dowolna˛ ilościa˛ białych znaków. Ilość liczb określa
jednocześnie ilość ogniw manipulatora.
Oprócz wspomnianego pliku może wystapić
˛
dowolna ilość plików reprezentujacych
˛
poszczególne postury manipulatora. Zawieraja˛ one wartości katów
˛
dla poszczególnych przegubów poczawszy
˛
od kata
˛ q0 . Wszystkie wartości wyrażone sa˛ w stopniach. Zapisane sa˛ w jednej
linii i moga˛ być rozdzielone dowolna˛ ilościa˛ spacji lub znaków tabulacji. Przykład zawartości
pliku plik_postury1.dat:
75.2
-20
-31
-15
Program po wczytaniu pliku konfiguracji manipulatora powinien wyświetlić rysunek manipulatora w posturze poczatkowej
˛
przyjmujac
˛ wartości katów:
˛
q0 = 90◦ , q1 = 0◦ , . . . , qn = 0◦ , gdzie
n jest ilościa˛ ogniw. Oprócz tego na ekranie terminala powinien wypisać wczytana˛ konfiguracj˛e. Nast˛epnie powinien odczekać 2s i wczytać plik z katami
˛
dla pierwszej postury, wyliczyć
położenia przegubów i wyświetlić rysunek manipulatora. Na ekranie terminala powinien natomiast wypisać katy
˛ i współrz˛edne przegubów. Nast˛epnie powinien odczekać tym razem 1s i
wczytać katy
˛ z nast˛epnego pliku dla kolejnej postury. Dalsze operacje powtarzaja˛ si˛e. Po wczytaniu katów
˛
z ostatniego pliku i wyświetlenie ostatniej postury program powinien oczekiwać na
przyciśni˛ecie przycisku ENTER, aby zakończyć działanie.
4
6.1
Reakcja na bł˛edy
Jeżeli plik z konfiguracja˛ manipulatora lub kolejna˛ jego postura˛ nie b˛edzie mógł zostać otwarty,
to program powinien w tym momencie przerwać działanie i wyświetlić stosowny komunikat.
W analogiczny sposób należy zareagować, jeśli w pliku pojawia˛ si˛e inne napisy niż te, które
oznaczaja˛ liczby.
Jeżeli w pliku opisujacego
˛
postur˛e manipulatora jest mniej danych niż przegubów, katy
˛
pozostałych przegubów nie należy zmieniać, zaś na wyjściu standardowym powinno zostać
wyświetlone ostrzeżenie. Powinna również zostać podana ilość przegubów, dla których brakuje
wartości katów.
˛
Po wyświetleniu ostrzeżenia program powinien normalnie działać i wyznaczyć
nowa˛ postur˛e, dla tych danych, które posiada.
7
Przykład działania programu
Niniejszy przykład nie obejmuje widoku okienka z rysunkiem wygenerowanym przez program
gnuplot. Przedstawiona˛ form˛e interakcji z użytkownikiem należy traktować jako obowiazu˛
jac
˛ a.˛
jkowalsk@panamint: rozwiazanie> ./manipulator konfig.dat postura1.dat postura2.dat
Konfiguracje wczytano z pliku: plik_konfiguracji.dat
Konfiguracja:
Dlugos ogniwa
Dlugos ogniwa
Dlugos ogniwa
Dlugos ogniwa
Postura:
q0 = 90.0
q1 =
0.0
q2 =
0.0
q3 =
0.0
0:
1:
2:
3:
40
45.5
64.5
32
Przegub 0:
Przegub 1:
Przegub 2:
Przegub 3:
Efektor:
(
(
(
(
(
0.0,
0.0)
-0.0, 40.0)
-0.0, 85.5)
-0.0, 150.0)
-0.0, 182.0)
Postura wczytana z pliku: plik_postury1.dat
Postura:
q0 = 75.2
q1 = -20.0
q2 = -31.0
q3 = -15.0
Przegub 0:
Przegub 1:
Przegub 2:
Przegub 3:
Efektor:
(
0.0,
0.0)
( 10.2, 38.7)
( 36.2, 76.0)
( 95.0, 102.5)
( 126.6, 107.6)
Postura wczytana z pliku: plik_postury2.dat
Postura:
5
q0
q1
q2
q3
=
=
=
=
100.0
-30.0
-41.0
-25.0
Przegub 0:
Przegub 1:
Przegub 2:
Przegub 3:
Efektor:
(
(
(
(
(
0.0,
0.0)
-6.9, 39.4)
8.6, 82.1)
65.0, 113.4)
97.0, 115.7)
Aby zakonczyc nacisnij ENTER ...
Przedstawiony przykład należy uznać za obligatoryjny pod wzgl˛edem rodzaju i sposobu wyświetlania informacji.
8
Wymagania i zarys programu zaj˛eć w okresie realizacji zadania
Oprócz samego programu należy stworzyć i wygenerować za pomoca˛ programu doxygen dokumentacj˛e programu. Powinna ona zawierać diagramy wymagane w ramach przygotowania
do zaj˛eć. Diagramy powinny zostać odpowiednio zaktualizowane, aby odzwierciedlały stan
faktyczny zaimplementowanego rozwiazania.
˛
Oprócz tego pozostaja˛ w mocy wszystkie wcześniejsze wymagania dotyczace
˛ struktury katalogów, pliku Makefile, modułowej struktury programu, jak też opisów.
8.1
Tydzień 0
Wymagania co do tygodnia 0 zostały opisane w rozdziale 4.
8.2
Tydzień 1
Należy stworzyć odpowiedni diagram czynności dla implementowanej procedury obliczania
współrz˛ednych poszczególnych przegubów. Należy również skorygować odpowiednio wcześniejszy diagram klas, tak aby odzwierciedlał aktualny koncepcj˛e projektu struktur danych.
Oczekuje si˛e, że b˛eda˛ stworzone moduły z niezb˛ednymi klasami, które pozwola˛ zamodelować poj˛ecie manipulatora i przeprowadzić niezb˛edne obliczenia. W klasach tych nie musza˛ być
jeszcze w pełni zdefiniowane metody odpowiedzialne za wyliczenie współrz˛ednych przegubów.
Stworzona cz˛eść kodu musi być zdokumentowana w systemie doxygen, a sama dokumentacja
musi być wygenerowana i przedstawiona osobie prowadzacej
˛ na zaj˛eciach.
Minimalna˛ funkcjonalnościa,˛ która˛ musi dysponować na tym etapie program jest otwieranie
plików, których nazwy podane sa˛ w linii polecenia, i wyświetlenie wczytywanych liczb (w
trakcie czytania).
8.3
Tydzień 2
Rozliczenie si˛e z gotowego programu i rozpocz˛ecie nast˛epnego zadania.
6
9
9.1
Warianty zadania
Wersja podstawowa
W tej wersji proponuje si˛e użycie zewn˛etrznego modułu lacze_do_gnuplota. Znajduje si˛e
on w kartotece ˜bk/edu/kpo/zad/z6/przyklad_2D wraz z przykładem jego wykorzystania.
Wspomniany moduł pozwala uruchomić program gnuplot z poziomu własnego programu i
przesłać do niego polecenie wykonania rysunku. Ponadto umożliwia on ustalenie zakresu osi
współrz˛ednych OX i OY .
9.2
Wersja uproszczona
W tej wersji zadania można ograniczyć si˛e do r˛ecznego uruchamiania programu gnuplot po
każdorazowym przeliczeniu współrz˛ednych przegubów. Program gnuplot uruchamiamy w
osobnym okienku terminala graficznego po wcześniejszym przejściu do podkatalogu, w którym
znajduje si˛e plik manipulator.dat. Realizacja zadania w tym wariancie nie b˛edzie mogła być
wyżej oceniona niż na ocen˛e 4, 0.
9.2.1
Uruchomienie rysowania z linii polecenia
Przykładowy sposób uruchomienia programu:
echo "plot ’manipulator.dat’; pause 999" | gnuplot
Przerwanie działania programu realizujemy poprzez naciśni˛ecie przycisków Ctrl-C, gdy focus
znajduje si˛e w okienku terminala, z poziomu którego został on uruchomiony.
9.3
Wersja rozszerzona
W wersji rozszerzonej proponuje si˛e, aby zmiana pozycji ogniw była animowana i odbywała
si˛e możliwie płynnie.
9.4
Wersja bardziej rozszerzona
Program powinien umożliwiać jednoczesne sterowanie dowolnej ilości manipulatorów. W tym
przypadku wywołanie programu powinno mieć postać:
./a.out konf_rob1.dat konf_rob2 -p rob1_post1.dat rob2_post2.dat -p rob1_post2.dat rob2_post2.dat [ -p ... ]
Opcja -p została tu użyta, aby oddzielić kolejny zestaw postur.
10
Materiały pomocnicze
W podkatalogu ∼bk/edu/kpo/zad/z6/przyklad_2D znajduje si˛e przykład użycia modułu
lacze_do_gnuplota, który pozwala wizualizować postur˛e manipulatora. W podkatalogu
∼bk/edu/kpo/zad/z6/bin.panamint znajduje si˛e przykładowa realizacja programu w wersji podstawowej. Program można uruchomić po zalogowaniu si˛e na panaminta.
7