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)