PD Control of a Single Arm Pendulum

# Pendulum Animation # Install ffmpeg (allows us to create videos) !apt update -y !apt install ffmpeg -y

# Import Libraries: import numpy as np import matplotlib.pyplot as plt from matplotlib.animation import FFMpegWriter from matplotlib.patches import Circle # Pendulum Parameters g = 9.8 # [m/s^2] L = 1 # [m] m = 1 # [kg] # We need an array of time points from 0 to 10 in increments of 0.01 seconds dt = 0.001 t_vec = np.arange(0,10,dt) # Initialize a vector of zeros theta_vec = np.zeros(len(t_vec)) dtheta_vec = np.zeros(len(t_vec)) # Set our initial condition theta_vec[0] = np.pi/4 # initial angle dtheta_vec[0] = 0 # initial angular velocity # Control Parameters kp = 15 kd = 5 theta_des = np.pi/2 # This is where we want it to go dtheta_des = 0 # Desired velocity at the end # Loop through time # Euler's Method (approximately integrates the differential equation) for i in range(1, len(t_vec)): tau = m*L**2*(kp*(theta_des - theta_vec[i-1]) + kd*(dtheta_des - dtheta_vec[i-1]) + g/L*np.sin(theta_vec[i-1])) theta_vec[i] = theta_vec[i-1] + dtheta_vec[i-1]*dt dtheta_vec[i] = dtheta_vec[i-1] + (-g/L*np.sin(theta_vec[i-1]) + tau/(m*L**2))*dt # Plotting theta values plt.plot(t_vec,theta_vec) plt.show() # Set up our Figure for drawing our Pendulum fig, ax = plt.subplots() # Create a plot on those axes, which is currently empty p, = ax.plot([],[], color='cornflowerblue') # initializes an empty plot ax.set_xlim([-3, 3]) # X Lim ax.set_ylim([-3, 3]) # Y Lim ax.axis('equal') ax.set_xlabel('X') ax.set_ylabel('Y') ax.set_title('Pendulum Simulation') video_title = "simulation" # Now we want to plot stuff on those axes c = Circle((0, 0), radius=0.1, color='cornflowerblue') ax.add_patch(c) # Define information for our Animation FPS = 20 sample_rate = int(1 / (FPS*dt)) dpi = 300 # Quality of the Video writerObj = FFMpegWriter(fps=FPS) # Now we're putting the rod and bob in the right place # Initialize an array containing the positions of the pendulum over time simulation_size = len(t_vec) # number of sim time points x_pendulum_arm = np.zeros(simulation_size) y_pendulum_arm = np.zeros(simulation_size) for i in range(0, simulation_size): x_pendulum_arm[i] = L*np.sin(theta_vec[i]) y_pendulum_arm[i] = -L*np.cos(theta_vec[i]) # We've computed all the pendulum positions # Now we need to update the plot in a loop # and store each of the frames in a video # Plot and Create Animation with writerObj.saving(fig, video_title+".mp4", dpi): # Loop through the frames we want to animate for i in range(0, simulation_size, sample_rate): # Update the pendulum arm (rod) x_data_points = [0, x_pendulum_arm[i]] y_data_points = [0, y_pendulum_arm[i]] # Updated the data in the plotting object, p p.set_data(x_data_points, y_data_points) # Update our bob circle (path) patch_center = x_pendulum_arm[i], y_pendulum_arm[i] c.center = patch_center # Update Drawing fig.canvas.draw() # Grab and save the frame writerObj.grab_frame() # Importing the video we just made and runing it on the notebook from IPython.display import Video Video("/work/simulation.mp4", embed=True, width=640, height=480)