Symulacja robota mobilnego z wykorzystaniem Scipy ODE (0-10 punktów)

Cel

Zmodyfikuj symulację robota mobilnego (z poprzedniego zadania) tak, aby wykorzystywała solver ODE z biblioteki SciPy (Python).

Opis

Stwórz system, który będzie symulował ruch robota mobilnego używającego solver ODE (SciPy).

\left\{\begin{array}{rcl} \dot{x} & = & \cos{\phi} \cdot u_1,\\ \dot{y} & = & \sin{\phi} \cdot u_1,\\ \dot{\phi} & = & u_2,\end{array}\right.

gdzie:
x, y, $\phi$ reprezentują pozę robota (położenie i orientację),
$u_1$ to sygnał sterowania prędkością liniową,
$u_2$ to sygnał sterowania prędkością kątową.

Na diagramie przedstawiono przykładowy system (w najprostszym przypadku, sterowanie w otwartej pętli)

Symulator mobilnego robota

Ten węzeł powinien obliczać kolejne stany robota, zakładając otrzymane wartości sterowania. Jeśli nie ma wartości sterowania w konkretnym punkcie czasowym, powinien przyjąć wartości sterowania równe 0 i opublikować nowy stan.

Kontroler

Powinien generować funkcje sterowania dla symulatora mobilnego robota. Wartości sterowania powinny obejmować parametry prędkości liniowej ( $u_1$ ) i prędkości kątowej ( $u_2$ ). Rozważ użycie wiadomości geometry_msgs/Twist jako wyjścia z kontrolera.

Możliwym rozszerzeniem może być dodanie kontrolera zamkniętej pętli (z trajektorią generatora).

Uwagi:

Załóż, że robot porusza się na płaszczyźnie XY, a z jest równe 0.
Załóż, że na początku robot znajduje się w punkcie (0,0,0).
Opublikuj trajektorię robota jako serię wiadomości PoseStamped.
Publikuj położenie robota również z tf2 jako transformację między ramą world a base_link. Możesz założyć, że rama world jest stała.
Zwizualizuj ścieżkę i ramki w rviz (alternatywnie w foxglove).
Symulowany robot powinien poruszać się w sensowny sposób, na przykład wzdłuż kwadratu.
Stwórz nowy pakiet ROS 2 dla swojego projektu symulacji robota.
Stwórz launcher (oparty na Pythonie), który uruchomi wszsytkie węzły systemu, a także wczyta parametry z plików. System powinien być uruchamiany jednym poleceniem.
```
ros2 launch nazwa_pakietu nazwa_launcher
```
Udokumentuj kod.

Przykład użycia solvera ODE (SciPy)

from scipy.integrate import solve_ivp
import matplotlib.pyplot as plt

# RHS of the ODE dot(x) = f(t, x, p), where
# t -- time
# x -- state
# p -- parameter
def f(t, x, p):
  return -p * x

T = [0, 10] # Time horizon
x0 = [2] # Initial state
p = 0.5 # Parameter

# Solve
sol = solve_ivp(f, T, x0, args=[p])

# Plot the solution
plt.plot(sol.t, sol.y[0], label='x(t)')
plt.title('ODE Solution')
plt.xlabel('Time (t)')
plt.ylabel('State (x)')
plt.legend()
plt.show()

Instalacja SciPy

pip install scipy

Rozproszone Systemy Sterowania: L11