Laboratorium piąte

Kinematyka odwrotna 🤖

⚠️ Stworzony system opiera się na pakietach ROS2 utworzonych w ramach poprzednich laboratoriów.

Zakres laboratorium ✏️ :

• Tworzymy węzeł IKIN rozwiązujący kinematykę odwrotną manipulatora, na którym dotychczas pracowaliśmy.
• Opracowujmy algorytm analityczny (bez użycia KDL ❗).
• Przy wyznaczaniu położenia stawów nie uwzględniamy orientacji końcówki - w naszym robocie jest ona stała.
• Węzeł IKIN powinien korzystać z parametrów zdefiniowanych w rosparam.
• Rozkazy dla węzła IKIN mają pochodzić z węzła interpolacji trajektorii w przestrzeni operacyjnej.
• Wyznaczone położenie stawów przesyłamy do robot_state_publisher 🤖
• Jeżeli wyznaczenie położenia nie jest możliwe, węzeł IKIN nie publikuje nowej pozycji zadanej oraz sygnalizuje błąd w ROS Log ⚠️
• W wersji rozszerzonej należy uwzględnić ograniczenia kinematyki odwrotnej co do przestrzeni roboczej manipulatora.
• Porównujemy w RVIZ2, dwie pozycje 👓:

1. wyznaczoną przez węzeł interpolacji trajektorii w przestrzeni operacyjnej,
2. pochodzącą z robot_state_publisher.

Metodologia testowania systemu: 🤔🔬

• Wysyłamy szereg rozkazów dla węzła interpolacji trajektorii w przestrzeni operacyjnej sprawdzając poprawność wykonania zadania,
• W szczególności zadajemy z węzła ocmd ruch końcówki po prostokącie 🔲 i elipsie ⚪
• Ruch ma wykonywać się cyklicznie 🔁 Trajektoria ma pozostawiać ślad w postaci smugi ⚡
• Ruch powinien wykonywać się poprawnie niezależnie od tego, jaka będzie początkowa konfiguracja robota (położenie stawów).
• Początkową konfigurację można zadawać korzystając ze struktury systemu z poprzedniego zadania (interpolacji w przestrzeni złącz).

Algorytm analityczny 🎰 :

Nasz manipulator opisywany jest zgodnie z notacją DH przez 4 parametry. Tabela DH prezentuje się następująco: (dla ustalenia uwagi przyjęto, że układ zerowy ma oś z położoną w płaszczyźnie poziomej i oś x skierowaną w dół)

• macierz opisująca wzajemne położenie dwóch układów współrzędnych tworzona jest zgodnie ze wzorem:
  
• pozycję końcówki otrzymujemy poprzez przemnożenie MPJ (u nas n = 3, gdyż taka jest liczba wierszy w tabeli DH 📓 )
• wynikowa macierz zawiera wszystkie potrzebne informacje tj. wektor położenia oraz macierz rotacji
  

Trajektoria testowa 📉 :

Elipsa	Prostokąt

Legenda:

⚪ - trajektoria testowa wyznaczana przez oint_control_srv

Opis plików źródłowych 📄 :

ikin.py - w pliku zaimplementowaliśmy algorytm obliczający wartości zmiennych złączowych dla zadanego położenia końcówki manipulatora (odwrotne zadanie kinematyki). Parametry takie jak długości członów wczytywane są z serwera parametrów 💻. W przypadku gdy zmienna złączowa wychodzi poza dozwolony limit generowany jest błąd w ROS Log ⚠️. Końcówka manipulatora podczas przemieszczania pozostawia ślad swojej trajektorii w postaci smugi 🖍️ (obsługa przy pomocy Marker Arrays).

oint.py - plik zawiera węzeł klienta serwisu 📞 odpowiedzialnego za generowanie trajektorii testowej 〽️. Sprawdza poprawność parametrów wywołania serwisu. Wspomniane parametry to kolejno:

• czas zakreślenia pełnej trajektorii testowej ⌚
• kształt trajektorii testowej ➡️ prostokąt lub elipsa
• parametr a figury - długość pierwszego boku prostokąta lub półoś wielka elipsy
• parametr b figury - długość drugiego boku prostokąta lub półoś mała elipsy

✅ Po pozytywnym zakończeniu wykreślania trajektorii serwis zwraca wiadomość z parametrami trajektorii oraz z potwierdzeniem wykonania zlecenia.

oint_control_srv.py - plik z węzłem serwisu generującego trajektorię testową. Wyróżniamy dwa typy interpolacji:

• rectangle - prostokątna
• ellipse - eliptyczna

Serwis wykorzystuje wartości długości członów manipulatora wczytywane z pliku .json. Kolejne punkty obu figur zadawane są z krokiem dyskretnym. Gęstość punktów na obwodzie figury zależy od ilorazu zadanego czasu generacji trajektorii przez czasu próbkowania (100 ms).

Uwzględnienie ograniczeń manipulatora podczas śledzenia trajektorii 🛑 :

Elipsa	Prostokąt

Legenda:

🔴 - część trajektorii testowej poza zasięgiem manipulatora
🟢 - część trajektorii testowej przebyta przez końcówkę manipulatora (będąca w jego zasięgu)

Struktura systemu 📞 :

Instrukcja uruchomienia systemu 🚀 :

1. Zbudowanie paczki z definicjami struktur komunikacji
   colcon build --symlink-install --packages-select interpolation_interfaces
2. Zbudowanie właściwego pakietu dla zadania piątego
   colcon build --symlink-install --packages-select lab5_kinematyka_odwrotna
3. Uruchomienie Rviz2 wraz z konfiguracją
   ros2 launch lab5_kinematyka_odwrotna lab3.launch.py
4. Uruchomienie serwisu
   ros2 run lab5_kinematyka_odwrotna service2
5. Podanie parametrów przy uruchamianiu węzła klienta serwisu
   ros2 run lab5_kinematyka_odwrotna client2 [ czas rodzaj_trajektorii_testowej parametr_a_figury parametr_b_figury]
6. Uruchomienie właściwego węzła IKIN
   ros2 run lab5_kinematyka_odwrotna ikin
7. Uruchomienie węzła zaznaczającego pozycję narzędzia (do celów testowo - porównawczych)
   ros2 run lab5_kinematyka_odwrotna kdl

EWENTUALNIE ⬇️ - zadanie początkowej pozycji startowej innej niż domyślna po odpaleniu RViz2
8) Uruchomienie serwisu interpolacji trajektorii
ros2 run lab5_kinematyka_odwrotna service
9) Podanie parametrów przy uruchamianiu węzła klienta serwisu interpolacji trajektorii
ros2 run lab5_kinematyka_odwrotna client [przesunięcie w złączu #1 przesunięcie w złączu #2 przesunięcie w złączu #3 czas typ]

⚠️ Przy uruchamianiu kolejnych węzłów w nowych oknach konsoli należy wskazać na pliki źródłowe ROS2: source install/setup.bash ❗

Przebieg testów 👨‍🔬 :

Porównanie trajektorii testowej oraz trajektorii końcówki manipulatora 🕵️ :

Elipsa	Prostokąt

Legenda:

⚪ - trajektoria testowa wyznaczana przez oint_control_srv
🟢 - część trajektorii testowej przebyta przez końcówkę manipulatora

Wykresy rqt_graph 📊:

Porównanie położenia końcówki manipulatora dla trajektorii testowej

✅ Przebiegi zmiennych z rqt_plot potwierdzają zgodność pozycji końcówki robota z poszczególnych potoków przetwarzania pozycji zadanej przez interpolator (z użyciem odwrotnej kinematyki i bez).