A Planar Multirotor Simulator

A practical guide to simulating the planar multirotor.

This tutorial walks through building a planar multirobot simulator to accompany the final project. We’ll model robot dynamics as second-order autonomous systems and visualize their behavior.

System Dynamics

The planar multirotor can be modeled as a second-order system: \begin{align*} m \ddot{x}_1 &= -u_1 \sin \theta \\ m \ddot{x}_2 &= u_1 \cos \theta - mg \\ I \ddot{\theta} &= u_2 \end{align*}

where we take the constants m = 1, I = 1, and g = 9.81. To make the code simpler, these constants can be stored in a dictionary and accessed as needed.

def get_params():
  I = 1.0              # Moment of inertia
  g = 9.8              # gravitational acceleration constant (m/s^2)
  m = 1.0              # multirotor mass
  rw = 0.5            # Rectangle Width (for animation)
  rh = 0.5           # Rectangle Height (for animation)
  
  return I, g, m, rw, rh

Then, the system dynamics can be written as:

import numpy as np

# Controller placeholder
def controller(x):
  return np.array([0.0, 0.0]) # net trust, and rolling moment 

# Differential equations describing the system
def multirotor_system(x, t, controller):    
  I, g, m, rw, rh = get_params()        
        
  dx = np.zeros(6)
  dx[0] = x[2]   
  dx[1] = x[3]   
  dx[4] = x[5]
  
  u = controller(x) # net trust, and rolling moment
  dx[2] = -(1/m)*u[0]*np.sin(x[4])
  dx[3] = (1/m)*u[0]*np.cos(x[4]) - g
  dx[5] = (1/I)*u[1]
  
  return dx

Here, the controller function can be defined to provide the control inputs based on the current state of the system. Notice that for now we set the controller to return zero inputs, which will allow us to visualize the natural dynamics of the system.

Numerical Integration for Simulation

To simulate the system, we can use scipy.integrate.odeint to solve the system of differential equations over a specified time horizon.

from scipy.integrate import odeint

def simulate_multirotor(x0, tfinal, timestep, controller):
  t = np.arange(0, tfinal, timestep)
  sol = odeint(multirotor_system, x0, t, args=(controller,))
  return sol

Running a simulation

Finally, it remains to actually run a simulation for a given initial condition and time horizon.

x0 = [0, 2, 0, 0, 0, 0] # initial conditions. 
# x[0] = x1 - horizontal position of multirobot
# x[1] = x2 - vertical position of multirobot
# x[2] = v1 - horizontal velocity of multirobot
# x[3] = v2 - vertical velocity of multirobot
# x[4] = theta - angle from horizontal
# x[5] = omega - angular velocity

tfinal = 2.0
timestep = 0.01

solvec = simulate_multirotor(x0, tfinal, timestep, controller)

Plotting

We can use matplotlib to plot the trajectories of the system states over time, and also create an animation to visualize the motion of the multirotor in the plane. To help with plotting we will first design a helper function to plot the trajectories of the system states over time.

import matplotlib.pyplot as plt

def plot_trajectories(sol, timestep):
  t = np.arange(0, sol.shape[0]*timestep, timestep)
  
  fig, axes = plt.subplots(2, 1, figsize=(6, 8))
  
  axes[0].plot(t, sol[:, 0], label='x1 (horizontal position)')
  axes[0].plot(t, sol[:, 1], label='x2 (vertical position)')
  # axes[0].plot(t, sol[:, 2], label='v1 (horizontal velocity)')
  # axes[0].plot(t, sol[:, 3], label='v2 (vertical velocity)')
  axes[0].set_xlabel('Time (s)')
  axes[0].set_ylabel('Position (m)')
  axes[0].set_title('Multirotor Position Trajectories')
  axes[0].legend()
  axes[0].grid(True)
    
  axes[1].plot(t, sol[:, 4], label='theta (angle from horizontal)')
  # axes[1].plot(t, sol[:, 5], label='omega (angular velocity)')
  axes[1].set_xlabel('Time (s)')
  axes[1].set_ylabel('Angle (rad)')
  axes[1].set_title('Multirotor Angle Trajectories')
  axes[1].legend()
  axes[1].grid(True)
  
  plt.tight_layout()
  plt.show()

To call this plot, we then simply have to run the following (after the simulate_multirotor line)

plot_trajectories(solvec, timestep)

Animations

We can create an animation to accompany this simulation. The easiest way to do this is to create the helper function make_animation. This animation will require an image of the multirotor, which is provided here.

import matplotlib.image as mpimg
from matplotlib import animation
from matplotlib.offsetbox import OffsetImage, AnnotationBbox
from scipy.ndimage import rotate

def make_animation(dt, solvec):
  posx_vec = solvec[:,0]
  posy_vec = solvec[:,1]
  theta_vec = solvec[:,4]    
  Nt = solvec.shape[0]
  
  fig, ax = plt.subplots()
  I, g, m, rw, rh = get_params()
  global ab, imagebox
  
  multirotor_image = mpimg.imread("figures/multirotor.png")  # Replace with your image path
  imagebox = OffsetImage(multirotor_image, zoom=0.2)  # Adjust zoom for size
  ab = AnnotationBbox(imagebox, (0, 0), frameon=False)
  ax.add_artist(ab)
  
  line1, = ax.plot([], [], '-',color = '#49baff',markersize = 12, markerfacecolor = '#0077BE',lw=2, markevery=10000, markeredgecolor = 'k')   # line for Earth

      
  # initialization function: plot the background of each frame
  def init():
      line1.set_data([], [])
      time_string.set_text('')
      return  ab, line1, time_string
  
  # animation function.  This is called sequentially
  def animate(i):
      I, g, m, rw, rh = get_params()
      # Motion trail sizes. Defined in terms of indices. Length will vary with the time step, dt. E.g. 5 indices will span a lower distance if the time step is reduced.
      trail1 = 100              # length of motion trail of weight 1 
      
      # multirotor.set_xy((posx_vec[i]-rw/2, posy_vec[i]-rh/2))
      # transform = mpl.transforms.Affine2D().rotate_around(posx_vec[i],posy_vec[i],theta_vec[i]) + ax.transData
      # multirotor.set_transform(transform)

      # Rotate the image array
      rotated_img = rotate(multirotor_image, np.degrees(theta_vec[i]), reshape=True)
      rotated_img = np.clip(rotated_img, 0, 1)  # If image is float (0 to 1 range)
      imagebox.set_data(rotated_img)

      # Update position
      ab.xybox = (posx_vec[i], posy_vec[i])
      
      line1.set_data(posx_vec[i:max(1,i-trail1):-1], posy_vec[i:max(1,i-trail1):-1])   # marker + line of pendulum mass

      time_string.set_text(time_template % (i*dt))
      return ab, line1, time_string

  # call the animator. blit=True means only re-draw the parts that have changed.
  time_template = 'Time = %.1f s'
  time_string = ax.text(0.05, 0.9, '', transform=ax.transAxes)
  
  ax.set_xlim(min(posx_vec) - rw, max(posx_vec) + rw)
  ax.set_ylim(min(posy_vec) - rh, max(posy_vec) + rh)
  ax.get_xaxis().set_ticks([])    # enable this to hide x axis ticks
  ax.get_yaxis().set_ticks([])    # enable this to hide y axis ticks
  ax.set_aspect('equal', adjustable='box') 
  
  # MAIN ANIMATION PLOTTING
  anim = animation.FuncAnimation(fig, animate, init_func=init,
                              frames=Nt, interval=1000*(dt)*0.8, blit=False)
  plt.close(fig)
  return anim

Then, we can call the animation as:

from IPython.display import HTML

anim = make_animation(timestep, solvec)

# Comment if not running in a Jupyter notebook or Quarto document
HTML(anim.to_jshtml())

# Uncomment to save a gif
# anim.save('planar-multirotor.gif', fps=1.0/(timestep), writer = 'imagemagick')

Once Loop Reflect

We can observe that if you inialize the system with nonzero initial velocities, you should see a different balistic behavior:

x0 = [0, 2, 10, 10, 0, 0] # initial conditions. 
# x[0] = x1 - horizontal position of multirobot
# x[1] = x2 - vertical position of multirobot
# x[2] = v1 - horizontal velocity of multirobot
# x[3] = v2 - vertical velocity of multirobot
# x[4] = theta - angle from horizontal
# x[5] = omega - angular velocity

tfinal = 5.0
timestep = 0.01

solvec = simulate_multirotor(x0, tfinal, timestep, controller)
plot_trajectories(solvec, timestep)
anim = make_animation(timestep, solvec)

HTML(anim.to_jshtml())

Once Loop Reflect