!apt update -y
!apt install ffmpeg -y
import numpy as np
import sympy
import matplotlib.pyplot as plt
from matplotlib.patches import Circle
from matplotlib.patches import Rectangle
from matplotlib.animation import FFMpegWriter
# Parameters:
L = 1 # m
m1 = 1 # kg
m2 = 2 # kg
k = 10 # N/m
g = 9.8 # m/s^2
lo = 1 # rest spring length (m)
l_pend_hinge = 1.5 # length from ground to pendulum hinge (m)
# Time Vector:
dt = 0.001
t_vec = np.arange(0,10,dt)
y_offset = -1.5
# Initize State Vectors:
x_vec = np.zeros(len(t_vec)) # x position vector
dx_vec = np.zeros(len(t_vec)) # x velocity vector
theta_vec = np.zeros(len(t_vec)) # theta angle vector
dtheta_vec = np.zeros(len(t_vec)) # angular velocity vector
x_spring = np.zeros(len(t_vec)) # spring x location
y_spring = np.zeros(len(t_vec)) # spring y location
length_spring = np.zeros(len(t_vec))
ddx = 0
ddtheta = 0
# Initial Conditions:
x_vec[0] = 0 # x inital poisition (m)
dx_vec[0] = 0 # x initial velocity (m/s)
theta_vec[0] = 1 # initial angle (rad)
dtheta_vec[0] = 0 # initial angular acceleration (rad/sec)
# Euler Method
for i in range(1, len(t_vec)):
x_vec[i] = x_vec[i-1] + dx_vec[i-1]*dt
theta_vec[i] = theta_vec[i-1] + dtheta_vec[i-1]*dt
# Solving for ddx and ddtheta
ddx = (-k*(x_vec[i-1]-L*np.sin(theta_vec[i-1]))*(np.sqrt((x_vec[i-1]-L*np.sin(theta_vec[i-1]))**2 + (L*np.cos(theta_vec[i-1])-l_pend_hinge)**2)-lo))/(m2*np.sqrt((x_vec[i-1]-L*np.sin(theta_vec[i-1]))**2 + (L*np.cos(theta_vec[i-1])-l_pend_hinge)**2))
ddtheta = (-g/L)*np.sin(theta_vec[i-1]) - (k*(3*L*np.sin(theta_vec[i-1]) - 2*L*x_vec[i-1]*np.cos(theta_vec[i-1]))*(np.sqrt((x_vec[i-1]-L*np.sin(theta_vec[i-1]))**2 + (L*np.cos(theta_vec[i-1])-l_pend_hinge)**2)-lo))/(2*np.sqrt((x_vec[i-1]-L*np.sin(theta_vec[i-1]))**2 + (L*np.cos(theta_vec[i-1])-l_pend_hinge)**2))
dx_vec[i] = dx_vec[i-1] + ddx*dt
dtheta_vec[i] = dtheta_vec[i-1] + ddtheta*dt
# Plotting X and Theta
# Set up X and Theta Plots
plt.plot(t_vec,x_vec)
plt.xlabel('Time (sec)') # X Label
plt.ylabel('X Position (m)') # Y Label
plt.title('X Position')
plt.show()
plt.plot(t_vec,theta_vec)
plt.xlabel('Time (sec)') # X Label
plt.ylabel('Theta Angle (rad)') # Y Label
plt.title('Theta')
plt.show()
# Animation
fig, ax = plt.subplots() # Initialize Figure and Axes
p, = ax.plot([],[], color = 'royalblue')
ax.axis('equal')
ax.set_xlim([-3,3])
ax.set_ylim([-3,3])
ax.set_xlabel('X') # x-label
ax.set_ylabel('Y') # y-label
ax.set_title('Question 2 Simulation') # Plot Title
video_title = 'Q2Simulation' # name of mp4
# Initialize Patch
c1 = Circle((0,0),radius=0.05,color='black')
ax.add_patch(c1) # Add patch to axes
#Initialize Patch: (Mass 2)
width = 0.3 # Width of m2
height = width/2 # Height of m2
xy_cart = (x_vec[0]-width/2, y_offset - height/2)
r = Rectangle(xy_cart, width, height, color='cornflowerblue')
ax.add_patch(r) # Add Patch to Plot
# Draw the Ground:
ground = ax.hlines(-1.5-height/2, -1, 1, colors='black')
# Animation Writer
FPS = 20
sample_rate = int(1/(dt*FPS))
dpi = 300
writerObj = FFMpegWriter(fps=FPS)
# Initalize Array
simulation_size = len(t_vec)
x_pendulum_arm = np.zeros(simulation_size)
y_pendulum_arm = np.zeros(simulation_size)
origin = (0,0) # Pendulum Hinge Point
# X and Y trajectories of pendulum:
for i in range(0,simulation_size):
x_pendulum_arm[i] = L * np.sin(theta_vec[i]) + origin[0]
y_pendulum_arm[i] = origin[1] - L*np.cos(theta_vec[i])
# Plot and Create Animation
with writerObj.saving(fig,video_title+".mp4",dpi):
for i in range(0,simulation_size, sample_rate):
r.set(xy=(x_vec[i]-width/2, y_offset-height/2)) # Update m2 position
x_data_points = [origin[0],x_pendulum_arm[i], x_vec[i]]
y_data_points = [origin[0], y_pendulum_arm[i], -1.5]
p.set_data(x_data_points, y_data_points)
patch_center1 = x_pendulum_arm[i], y_pendulum_arm[i]
c1.center = patch_center1
fig.canvas.draw()
writerObj.grab_frame()
# Video
from IPython.display import Video
Video("/work/Q2Simulation.mp4", embed=True, width=640, height=480)