Laboratorium trzecie

Kinematyka prosta 🤖

Zakres laboratorium ✏️ :

• stworzenie węzła NONKDL_DKIN, który będzie rozwiązywał prostą kinematykę manipulatora metodą analityczną bez użycia KDL
• stworzenie drugiego, analogicznego węzła KDL_DKIN z użyciem KDL (Kinematics and Dynamics Library)
• porównanie działania dwóch autorskich węzłów NONKDL_DKIN i KDL_DKIN w programie RViz2

⚠️ Węzły otrzymują położenie zadane stawów z węzła joint_state_publisher, rezultat (pozycja końcówki geometry_msgs/PoseStamped) jest publikowany na temacie i wizualizowany w RVIZ). Nie interpolujemy ruchu.

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

nonkdl_dkin.py 📈

• węzeł obliczający zadanie kinematyki prostej na podstawie operacji na macierzach przekształcenia jednorodnego (MPJ)
• subskrybuje temat /joint_state_publisher (konkretnie Joint State Publisher GUI) z którego otrzymuje położenia zadane w poszczególnych złączach
• parametry potrzebne do utworzenia MPJ wczytywane są z pliku Tablica_MD-H.json
• 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
• macierz rotacji przekształcana jest do kwaternionu ponieważ jest to wymagany format w wiadomości geometry_msgs/Pose.msg
• pozycję końcówki w formacie geometry_msgs/PoseStamped publikujemy na temacie i wizualizujemy w Rviz2 👓

kdl_dkin.py 📚

• węzeł obliczający zadanie kinematyki prostej wykorzystując bibliotekę KDL
• tworzymy obiekt klasy Chain() i dodajemy do niego kolejne elementy (Segments)
• Segment definiowany jest przez dwa parametry

1. Rodzaj złącza ( przesuwne/translacyjne oraz względem której osi)
2. Pozycję układu współrzędnych związanego ze złączem ( położenie -> Vector() oraz orientacja ->Rotation.RPY() )

• tworzymy w sumie 3 segmenty i kolejno dodajemy je do obiektu klasy Chain() ⛓️
• parametry potrzebne do określenia zależności pomiędzy kolejnymi układami wczytywane są z pliku urdf_wartosci.yaml będącego wynikiem działania konwertera tablicy DH z poprzedniego zadania laboratoryjnego
• tworzymy listę jako obiekt klasy JntArray() - elementy tej listy to zadane położenia złączowe z węzła /joint_state_publisher
• program wylicza położenie i pozycje końcówki manipulatora korzystając z rekursywnego solvera kinematyki prostej 💻
• podobnie jak w węźle nonkdl_dkin.py przekształcamy orientację do postaci kwaternionu i publikujemy na odpowiednim temacie

kartman.rviz 🔧

• plik z zapisaną konfiguracją wyświetlania układu związanego z końcówką manipulatora ( nie trzeba już nic ręcznie dodawać w RViz2)

⚠️ W obu wersjach programu dodajemy do końcowego położenia manipulatora offset, aby uwzględnić położenie narzędzia w stosunku do ostatniego złącza.

Instrukcja uruchomienia 🚀 :

1. Budujemy pakiet komendą: colcon build --symlink-install --packages-select lab3_kinematyka_prosta
2. Uruchamiamy plik launch : ros2 launch lab3_kinematyka_prosta lab3.launch.py ( uruchamiane przez plik launch węzły to robot_state_publisher , RViz2, joint_state_publisher_gui)
3. W oddzielnych konsolach uruchamiamy węzły obliczające PZK:

• w wersji analitycznej: ros2 run lab3_kinematyka_prosta nonkdl
• z wykorzystaniem KDL: ros2 run lab3_kinematyka_prosta kdl

4. W każdej otwartej konsoli przed uruchamianiem węzłów należy wpisać komendę: source install/setup.bash ❗

Schemat mechanizmu komunikacji 📞 :

Opis przebiegu testów 🧪 :

Metodologia testów: Obserwujemy układy związane z końcówką jednocześnie poruszając suwakami w /joint_states_publisher_gui.

1. Sprawdzamy zgodność pozycji końcówki manipulatora z poszczególnych węzłów, odczytując pozycje w programie RVIZ2 🧑‍🔬

⚠️ Wyraźnie widać, że pozycja końcówki obliczona przez oba, autorskie węzły jest identyczna. Pozycja układu generowana przez /tf (Transformations) nie jest zgodna z dwoma pozostałymi. Wynika to z faktu, że ten układ związany jest z ostatnim złączem (link_3__tool), które jest typu fixed. W autorskich węzłach długość końcówki została uwzględniona jako offset - z tego wynika widoczne na powyższej grafice przesunięcie.

2. Wydruki położenia w przestrzeni konfiguracyjnej i operacyjnej z wykorzystaniem programu rqt_plot 📊 :

Położenie:

Orientacja:

Obserwacje i wnioski na podstawie przeprowadzonych testów 🤔:

➡️ Orientacja wyliczona przez oba węzły pokrywa się - w tym miejscu należy zauważyć, że orientacja narzędzia(końcówki) jest stała niezależnie od wartości zmiennych złączowych w poszczególnych stawach. Narzędzie zawsze będzie zorientowane poziomo w dodatnim kierunku osi X układu bazowego.

➡️Położenia końcówki wyliczone przez autorskie węzły są zgodne. Na wykresach położenia widać jednak pewne opóźnienie wyznaczania pozycji w węźle wykorzystującym KDL. Podejrzewamy, że wynika to najprawdopodobniej z faktu, iż rekursywny solver kinematyki potrzebuje więcej czasu na obliczenia. W stanach ustalonych położenia pokrywają się.