Laboratorium czwarte
- stworzenie węzła ROS2 interpolującego trajektorię w przestrzeni konfiguracyjnej (w przestrzeni złącz manipulatora)
- przy realizacji zadania należy użyć mechanizmu komunikacji klient - serwer 💻
- węzeł jint (joints interpolation) ma przyjmować położenia zadane stawów, czas ruchu oraz inne niezbędne parametry 📃
- węzeł informuje o zakończeniu interpolacji lub o niemożności jej wykonania ( np. położenia zadane poza wartościami granicznymi lub zerowy czas ruchu ⏲️ )
- węzeł jint publikuje kolejne położenia zadane w stawach i przesyła je do węzła robot_state_publisher
- wizualizacja poruszającego się robota w programie RViz2 wraz z zaznaczeniem trajektorii końcówki roboczej 🔧 przy użyciu markerów🖌️
1) W wersji podstawowej - interpolacja liniowa 📏
2) W wersji rozszerzonej - interpolacja z ciągłym (np. trapezowym) rozkładem prędkości 🏎️
Oznaczenia:
q(t) - położenie w chwili t
q0 - położenie początkowe
qf - położenie końcowe
tf - czas ruchu/czas zakończenia ruchu (zakładamy, że ruch zaczyna się w chwili t=0)
ℹ️ W przypadku interpolacji liniowej prędkość jest stała i równa wartości współczynnika stojącego przy zmiennej t, a przyspieszenie jest zerowe. Jest to najprostszy rodzaj interpolacji - niestety cechuje się on zrywnym, mało płynnym ruchem członu dołączonego do złącza ( szczególnie w startowej i końcowej fazie ruchu).
Zdecydowaliśmy się na implementację interpolacji wielomianem 3-go stopnia. Możemy na niego nałożyć warunki graniczne na prędkość i położenie ( w naszym przypadku założymy, że ruch rozpoczyna się i kończy z prędkością równą zero).
ℹ️ W tym przypadku prędkość jest kwadratową funkcją czasu ( nie jest stała, jak miało to miejsce w interpolacji liniowej) - dzięki temu przebiegi trajektorii położenia cechują się gładkością i płynnością.
- jint.py - plik zawierający węzeł klienta serwisu obsługującego interpolację. Przy wywoływaniu w konsoli przyjmuje konieczne parametry takie jak: położenia zadane w stawach, czas oraz rodzaj interpolacji 📈. Obsługuje również wszystkie wyjątki związane z np. podaniem położenia zadanego poza limitem, podaniem zerowego czasu ruchu, podaniem niepoprawnej liczby parametrów. Po zakończeniu interpolacji wyświetla komunikat o parametrach procesu interpolacji oraz o jej pomyślnym zakończeniu 👍
- jint_control_srv.py - plik z węzłem serwisu obsługującym interpolację. W zależności od wyboru typu interpolacji publikowane są kolejne położenia stawów ( co okres próbkowania 0.1s). Cały opis mechanizmu interpolacji został już opisany powyżej (odpowiednio dla interpolacji liniowej i wielomianowej). W tym węźle znajduje się również MarkerPublisher - dzięki niemu możemy obserwować trajektorię w programie RViz2.
W repozytorium oprócz pakietów z poszczególnych laboratoriów znajduje się także pakiet interpolation_interfaces. Jest to osobny pakiet zawierający folder _/srv, w którym są przechowywane pliki z definicją struktur komunikacji w ramach serwisów. Pakiet ten został wydzielony, ponieważ utworzenie customowego serwisu wymaga podania odpowiedniej zależności w pliku CMake.txt (a do wszystkich innych pakietów używamy setup.py).
- Zbudowanie paczki z definicjami struktur komunikacji
colcon build --symlink-install --packages-select interpolation_interfaces - Zbudowanie właściwego pakietu dla zadania czwartego
colcon build --symlink-install --packages-select lab4_interpolacja - Uruchomienie Rviz2 wraz z konfiguracją
ros2 launch lab4_interpolacja lab3.launch.py - Uruchomienie serwisu
ros2 run lab4_interpolacja service - Podanie parametrów przy uruchamianiu węzła klienta serwisu
ros2 run lab4_interpolacja client [x y z czas typ] - Rezultaty interpolacji można obserwować w Rviz'ie lub w konsoli
Dla konsoli: ros2 topic echo /joint_states
| Test 1 | Test 2 |
|---|---|
 |
 |
| Interpolacja liniowa | Interpolacja wielomianem 3-go stopnia |
|---|---|
 |
 |
- generowanie pozycji układu (niezwiązanego z robotem) poprzez interpolację w przestrzeni operacyjnej
- węzeł interpolujący wysyła do RViz2 wiadomość typu PoseStamped z pakietu /geometry_msgs
- wersja podstawowa - interpolacja położenia ze stałą orientacją 🗺️
- wersja rozszerzona - interpolacja również orientacji 🧭
- wizualizacja poruszającego się układu w RViz2 z zaznaczeniem pozycji 🖊️
- oint.py - plik zawierający węzeł klienta serwisu obsługującego interpolację. Budowa i funkcjonalności zaimplementowane w tym pliku są całkowicie analogiczne względem pliku jint.py, więc nie będziemy ich opisywać ponownie.
- oint_control_srv.py - plik z węzłem serwisu obsługującym interpolację. Również jest on analogiczny do swojego odpowiednika jint_control_srv.py. Rozszerzenie polega na zaimplementowaniu interpolacji trajektorii w przestrzeni operacyjnej 📐. Interpolację wykonujemy operując na kątach R-P-Y podanych przez użytkownika (kąty wyrażone w stopniach). Wyznaczamy kolejne orientacje RPY w dyskretnych chwilach czasu, zamieniamy je na kwaternion i publikujemy na topicu /joint_states razem z informacjami o położeniu.
- Budujemy dwie paczki jak wyżej
colcon build --symlink-install --packages-select interpolation_interfaces
colcon build --symlink-install --packages-select lab4_interpolacja - Uruchamiamy plik launch zawierający Rviz2 z od razu skonfigurowanym PoseStamped, TF, MarkerArrays
ros2 launch lab4_interpolacja lab4.launch.py - Uruchomienie serwisu
ros2 run lab4_interpolacja service2 - Uruchomienie węzła klienta z odpowiednimi parametrami
ros2 run lab4_interpolacja client2 [ x y z rol pitch yaw czas typ ]
| Interpolacja położenia | Interpolacja położenia + orientacji |
|---|---|
 |
 |
| Interpolacja liniowa | Interpolacja wielomianem 3-go stopnia |
|---|---|
 |
 |
| Interpolacja liniowa | Interpolacja wielomianem 3-go stopnia |
|---|---|
 |
 |
Powyższe wykresy nie przypominają kształtem ani linii prostej, ani wielomianu stopnia trzeciego. Dlaczego? - Na wykresach widzimy zależność elementów kwaternionu (w, x, y, z) od czasu, ponieważ to właśnie kwaternion jest wymaganym formatem podawania orientacji w wiadomości PoseStamped. My zdecydowaliśmy się na interpolację w przestrzeni kątów R-P-Y wyrażonych w stopniach. Według nas jest to logiczne i zgodne z intuicją podejście. Nie możemy również wymagać, aby użytkownik korzystający z serwisu podał pożądaną orientację w postaci kwaternionu 🤣 , gdyż jest to o wiele mniej intuicyjne i wygodne niż podanie zestawy kątów R-P-Y.
Poniżej widzimy przykładowe wykresy trajektorii zmiennych (czyli kolejno kątów roll, pitch, yaw) dla orientacji zadanej równej:
roll = 30 stopni
pitch = 60 stopni
yaw = 90 stopni
* Grafiki zawierające wyprowadzenia wzorów wielomianów interpolacyjnych pochodzą z wykładu 9. z przedmiotu Wstęp do Robotyki prowadzonego na Wydziale EiTI PW przez dr hab. inż. Wojciecha Szynkiewicza