Przejdź do treści

Robotic Programming Environments: L11

Mobile robot simulation with SciPy ODE solver (0-10 points)


Modify the mobile robot simulation (previous task) to use an ODE solver (Python SciPy) for the motion equations calculations.


Create a system that will simulate the motion of a mobile robot using an ODE solver in Python to handle the differential equations.

\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.

x, y, \phi represent the pose of the robot (position and orientation),
u_1 is the linear velocity control signal,
u_2 is the angular velocity control signal.

The diagram shows an example system (in the simplest case, open-loop control)


  • Assume that the robot is moving on the XY plane and z is equal to 0.
  • Assume that at the beginning robot is placed in point (0,0,0).
  • Publish trajectory of the robot as a series of geometry_msgs/PoseStamped messages.
  • Publish the robot pose also with tf2 as a transformation between world and base_link frame. You can assume that the world frame is constant.
  • Visualize the path and frames in rviz (alternatively in foxglove).
  • The simulated robot should move in a meaningful way, for example, along a square (not a circle, as the control signals should not remain constant).
  • Create a new ROS 2 package for your robot simulation project.
  • Create a launcher (Python based launcher) that will start all nodes and load parameters from a YAML files. The system should be started with single command.
    ros2 launch package_name launcher_name
  • Document code.

Example of ODE solving with SciPy – solve_ivp

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)')

SciPy installation (if necessary)

pip install scipy