#Projectile Motion
def projectile_equations():
ProjectileVelocity= np.zeros(sim_length)
x2_vec[store]= l_4 * sympy.cos(beta_vec[store]) + l_2 * sympy.cos(th_vec[store] + sympy.pi)
y2_vec[store]= l_4 * sympy.sin(beta_vec[store]) + l_2 * sympy.sin(th_vec[store] + sympy.pi)
ProjectileVelocityX= dbeta_vec[store] * l_4 * sympy.cos( beta_vec[store] + 90)
ProjectileVelocityY= dbeta_vec[store] * l_4 * sympy.sin( beta_vec[store] + 90)
ProjectileAcceleration= [ 0, -g ]
ymax= 0
for k in range(store+1, sim_length):
x2_vec[k]= x2_vec[k-1] + ProjectileVelocityX*dt + 0.5 * ProjectileAcceleration[0] * dt * dt
y2_vec[k]= y2_vec[k-1] + ProjectileVelocityY*dt + 0.5 * ProjectileAcceleration[1] * dt * dt
ProjectileVelocityX= ProjectileVelocityX + ProjectileAcceleration[0]*dt
ProjectileVelocityY= ProjectileVelocityY + ProjectileAcceleration[1]*dt
if ymax < y2_vec[k] and targetY >= 0:
ymax= y2_vec[k]
elif ymax > y2_vec[k] and targetY < 0:
ymax= y2_vec[k]
if (x2_vec[k] > (targetX + 1) ):
print('Over')
complete= k
return [2, ymax, complete]
if (y2_vec[k] < targetY and ProjectileVelocityY<0):
if (temp1== sympy.pi/4):
print('CANNOT HIT TARGET')
complete= k
return [3, ymax, complete]
print('Under')
complete= k
return [1, ymax, complete]
if (x2_vec[k] < (targetX + 2.5/2) and x2_vec[k] > (targetX - 2.5/2)) and (y2_vec[k] < (targetY + 2.5/2) and y2_vec[k] > (targetY - 2.5/2)):
print('HIT')
complete= k
return [0, ymax, complete]
!apt update -y
!apt install ffmpeg -y
import numpy as np
import matplotlib.pyplot as plt
import sympy
from matplotlib.animation import FFMpegWriter
from matplotlib.patches import Circle
import math
#Target Position
targetX= 60
targetY= -25
# Pendulum Parameters:
m_CW = 25 # mass of counter-weight in kg
m_P = m_CW/133 # mass of projectile in kg
l_1 = 1 # length of the arm betweeen the pivot and counterweight arm in m
l_2 = 4 * l_1 # length of the throwing arm in m
l_3 = .75 # length of the counter-weight arm in m
l_4 = .8 * l_2 # length of the sling m
g = 9.81 # m/s^2
# Create Symbols for Time:
t = sympy.Symbol('t') # Creates symbolic variable t
# Create Generalized Coordinates as a function of time: q = [theta, alpha]
th = sympy.Function('th')(t) #Pivot and Horizontal
alpha = sympy.Function('alpha')(t) #CW and Horizontal
beta = sympy.Function('beta')(t) #Sling and Horizontal
#Create torque as a function of time
tau =sympy.Function('tau')(t) # torque applied to pivot to raise counter weight
rCW = np.array([ l_1*sympy.cos(th) + l_3*sympy.cos(alpha), l_1*sympy.sin(th) + l_3*sympy.sin(alpha) ])
rP = np.array([ l_2*sympy.cos(th+(3.14)) + l_4*sympy.cos(beta), l_2*sympy.sin(th+ (3.14)) + l_4*sympy.sin(beta) ])
vCW = np.array([rCW[0].diff(t), rCW[1].diff(t)]) #Velocity of first pendulum
vP = np.array([rP[0].diff(t), rP[1].diff(t)]) #Velocity of second pendulum
# Energy Equations:
T = 1/2 * m_CW * np.dot(vCW, vCW) + 1/2 * m_P * np.dot(vP, vP) #Kinetic Energy
V = m_CW * g * rCW[1] + m_P * g * rP[1] #Potential Energy
L = T - V #Lagrangian
# Lagrange Terms:
dL_dth = L.diff(th)
dL_dth_dt = L.diff(th.diff(t)).diff(t)
dL_dalpha = L.diff(alpha)
dL_dalpha_dt = L.diff(alpha.diff(t)).diff(t)
dL_dbeta = L.diff(beta)
dL_dbeta_dt = L.diff(beta.diff(t)).diff(t)
# Euler-Lagrange Equations: dL/dq - d/dt(dL/ddq) = torque
# When q=theta (torque only applies to theta eqn)
th_eqn = dL_dth - dL_dth_dt - tau
alpha_eqn = dL_dalpha - dL_dalpha_dt -tau
beta_eqn = dL_dbeta - dL_dbeta_dt -tau
# Replace Time Derivatives and Functions with Symbolic Variables:
replacements = [(th.diff(t).diff(t), sympy.Symbol('ddth')), (th.diff(t), sympy.Symbol('dth')), (th, sympy.Symbol('th')),
(alpha.diff(t).diff(t), sympy.Symbol('ddalpha')), (alpha.diff(t), sympy.Symbol('dalpha')), (alpha, sympy.Symbol('alpha')),
(beta.diff(t).diff(t), sympy.Symbol('ddbeta')), (beta.diff(t), sympy.Symbol('dbeta')), (beta, sympy.Symbol('beta')),(tau,sympy.Symbol('tau'))]
th_eqn = th_eqn.subs(replacements)
alpha_eqn = alpha_eqn.subs(replacements)
beta_eqn = beta_eqn.subs(replacements)
rCW = rCW[0].subs(replacements), rCW[1].subs(replacements)
rP = rP[0].subs(replacements), rP[1].subs(replacements)
# Simplfiy then Force SymPy to Cancel factorization: [Sometimes needed to use .coeff()]
th_eqn = sympy.simplify(th_eqn)
alpha_eqn = sympy.simplify(alpha_eqn)
beta_eqn = sympy.simplify(beta_eqn)
th_eqn = th_eqn.cancel()
alpha_eqn = alpha_eqn.cancel()
beta_eqn = beta_eqn.cancel()
# Solve for Coefficients for A * x = B where x = [ddth ddalpha ddbeta]
A1 = th_eqn.coeff(sympy.Symbol('ddth'))
A2 = th_eqn.coeff(sympy.Symbol('ddalpha'))
A3 = th_eqn.coeff(sympy.Symbol('ddbeta'))
A4 = alpha_eqn.coeff(sympy.Symbol('ddth'))
A5 = alpha_eqn.coeff(sympy.Symbol('ddalpha'))
A6 = alpha_eqn.coeff(sympy.Symbol('ddbeta'))
A7 = beta_eqn.coeff(sympy.Symbol('ddth'))
A8 = beta_eqn.coeff(sympy.Symbol('ddalpha'))
A9 = beta_eqn.coeff(sympy.Symbol('ddbeta'))
# Multiply remaining terms by -1 to switch to other side of equation: A * x - B = 0 -> A * x = B
remainder = [(sympy.Symbol('ddth'), 0), (sympy.Symbol('ddalpha'), 0), (sympy.Symbol('ddbeta'), 0)]
B1 = -1 * th_eqn.subs(remainder)
B2 = -1 * alpha_eqn.subs(remainder)
B3 = -1 * beta_eqn.subs(remainder)
# Generate Lambda Functions for A and B and Position Equations:
replacements = (sympy.Symbol('th'), sympy.Symbol('dth'), sympy.Symbol('alpha'), sympy.Symbol('dalpha'), sympy.Symbol('beta'), sympy.Symbol('dbeta'), sympy.Symbol('tau'))
A1 = sympy.utilities.lambdify(replacements, A1, "numpy")
A2 = sympy.utilities.lambdify(replacements, A2, "numpy")
A3 = sympy.utilities.lambdify(replacements, A3, "numpy")
A4 = sympy.utilities.lambdify(replacements, A4, "numpy")
A5 = sympy.utilities.lambdify(replacements, A5, "numpy")
A6 = sympy.utilities.lambdify(replacements, A6, "numpy")
A7 = sympy.utilities.lambdify(replacements, A7, "numpy")
A8 = sympy.utilities.lambdify(replacements, A8, "numpy")
A9 = sympy.utilities.lambdify(replacements, A9, "numpy")
B1 = sympy.utilities.lambdify(replacements, B1, "numpy")
B2 = sympy.utilities.lambdify(replacements, B2, "numpy")
B3 = sympy.utilities.lambdify(replacements, B3, "numpy")
rCW = sympy.utilities.lambdify(replacements, rCW, "numpy")
rP = sympy.utilities.lambdify(replacements, rP, "numpy")
print(th_eqn) #Prints torque as a symbolic variable - want to change to avaible that can be filled with values
# Simulate System:
x0 = np.pi/4, 0, -np.pi/2, 0, -np.pi/2, 0 # th, dth, alpha, dalpha, beta, dbeta
dt = 0.001
sim_time = 20
time = np.arange(0, sim_time, dt)
sim_length = len(time)
# Initialize Arrays:
th_vec = np.zeros(sim_length)
dth_vec = np.zeros(sim_length)
alpha_vec = np.zeros(sim_length)
dalpha_vec = np.zeros(sim_length)
beta_vec = np.zeros(sim_length)
dbeta_vec = np.zeros(sim_length)
torque_vec = np.zeros(sim_length)
x1_vec = np.zeros(sim_length)
y1_vec = np.zeros(sim_length)
x2_vec = np.zeros(sim_length)
y2_vec = np.zeros(sim_length)
def Simulate(temp1):
# Evaluate Initial Conditions:
th_vec[0] = temp1
dth_vec[0] = x0[1]
alpha_vec[0] = x0[2]
dalpha_vec[0] = x0[3]
beta_vec[0] = x0[4]
dbeta_vec[0] = x0[5]
x1_vec[0], y1_vec[0] = rCW(x0[0], x0[1], x0[2], x0[3], x0[4], x0[5], 0)
x2_vec[0], y2_vec[0] = rP(x0[0], x0[1], x0[2], x0[3], x0[4], x0[5], 0)
# Initialize A and B:
A = np.array([[0, 0, 0], [0, 0, 0], [0, 0, 0]])
B = np.array([0, 0, 0])
#PD controller constants
Kp = 0.1
Kd = 0.1
#Have to used netwon rapson search to find theta desired given an desired distance
#Probably add NR inside Euler Intergration to determine theta desired each interation and d-th desired
# Euler Integration: change theta conditions here
i = 1
j = 0
#Netwon rapson search to find desired theta and d-theta desired
th_desired = temp1
dth_desired = 1
beta_release = 0
alpha_release = 0
while th_vec[i-1]< 100:
#Evaulate torque- PD controller
#If the theta at time t is less than the desired value or greater than 0.2*desired value, the Pd
#controller changes theta
if th_vec[i-1] < th_desired or th_vec[i-1] > (0.2*th_desired):
torque_vec[i-1] = Kp*(th_desired - th_vec[i-1]) - Kd*(dth_desired - dth_vec[i-1])
#Once theta is equal to or less than 0.2*th_desired -> torque turns off
else:
torque_vec[i-1] = 0
# Evaluate Dynamics:
A[0, 0] = A1(th_vec[i-1], dth_vec[i-1], alpha_vec[i-1], dalpha_vec[i-1], beta_vec[i-1], dbeta_vec[i-1], 0)
A[0, 1] = A2(th_vec[i-1], dth_vec[i-1], alpha_vec[i-1], dalpha_vec[i-1], beta_vec[i-1], dbeta_vec[i-1], 0)
A[0, 2] = A3(th_vec[i-1], dth_vec[i-1], alpha_vec[i-1], dalpha_vec[i-1], beta_vec[i-1], dbeta_vec[i-1], 0)
A[1, 0] = A4(th_vec[i-1], dth_vec[i-1], alpha_vec[i-1], dalpha_vec[i-1], beta_vec[i-1], dbeta_vec[i-1], 0)
A[1, 1] = A5(th_vec[i-1], dth_vec[i-1], alpha_vec[i-1], dalpha_vec[i-1], beta_vec[i-1], dbeta_vec[i-1], 0)
A[1, 2] = A6(th_vec[i-1], dth_vec[i-1], alpha_vec[i-1], dalpha_vec[i-1], beta_vec[i-1], dbeta_vec[i-1], 0)
A[2, 0] = A7(th_vec[i-1], dth_vec[i-1], alpha_vec[i-1], dalpha_vec[i-1], beta_vec[i-1], dbeta_vec[i-1], 0)
A[2, 1] = A8(th_vec[i-1], dth_vec[i-1], alpha_vec[i-1], dalpha_vec[i-1], beta_vec[i-1], dbeta_vec[i-1], 0)
A[2, 2] = A9(th_vec[i-1], dth_vec[i-1], alpha_vec[i-1], dalpha_vec[i-1], beta_vec[i-1], dbeta_vec[i-1], 0)
B[0] = B1(th_vec[i-1], dth_vec[i-1], alpha_vec[i-1], dalpha_vec[i-1], beta_vec[i-1], dbeta_vec[i-1], torque_vec[i-1])
B[1] = B2(th_vec[i-1], dth_vec[i-1], alpha_vec[i-1], dalpha_vec[i-1], beta_vec[i-1], dbeta_vec[i-1], 0)
B[2] = B3(th_vec[i-1], dth_vec[i-1], alpha_vec[i-1], dalpha_vec[i-1], beta_vec[i-1], dbeta_vec[i-1], 0)
[ddth, ddalpha, ddbeta] = np.linalg.solve(A, B)
# Euler Step Integration:
th_vec[i] = th_vec[i-1] + dth_vec[i-1] * dt
dth_vec[i] = dth_vec[i-1] + ddth * dt
alpha_vec[i] = alpha_vec[i-1] + dalpha_vec[i-1] * dt
dalpha_vec[i] = dalpha_vec[i-1] + ddalpha * dt
beta_vec[i] = beta_vec[i-1] + dbeta_vec[i-1] * dt
dbeta_vec[i] = dbeta_vec[i-1] + ddbeta * dt
# Animation States:
x1_vec[i], y1_vec[i] = rCW(th_vec[i-1], dth_vec[i-1], alpha_vec[i-1], dalpha_vec[i-1], beta_vec[i-1], dbeta_vec[i-1], 0)
x2_vec[i], y2_vec[i] = rP(th_vec[i-1], dth_vec[i-1], alpha_vec[i-1], dalpha_vec[i-1], beta_vec[i-1], dbeta_vec[i-1], 0)
if (dth_vec[i]) > 0.1: #I'm messing with this value to change timing
beta_release = beta_vec[i]
alpha_release = alpha_vec[i]
break
i=i+1
return i
hit= False
temp1= sympy.pi/4
complete= sim_length
angle= temp1
while (hit == False):
store= Simulate(temp1)
ymax= 0
result= projectile_equations()
if (result[0] == 0):
ymax= result[1]
complete= result[2]
break
elif (result[0] == 1):
temp1= temp1 + 0.5*temp1
elif (result[0] == 2):
temp1= temp1 - 0.5*temp1
elif (result[0] == 3):
break
angle= np.vstack(( angle , temp1) )
color1 = 'green'
color2 = 'red'
plt.plot(x1_vec, y1_vec, color = color1)
plt.plot(x2_vec, y2_vec, color = color2)
plt.xlabel('X position (m)')
plt.ylabel('Y position (m)')
plt.title('Trajectory of Projectile and Counter Weight (Post-Launch)')
plt.show()
ProjectileVelocity= np.zeros(sim_length)
i= store
x2_vec[i]= l_4 * sympy.cos(beta_vec[i]) + l_2 * sympy.cos(th_vec[i] + sympy.pi)
y2_vec[i]= l_4 * sympy.sin(beta_vec[i]) + l_2 * sympy.sin(th_vec[i] + sympy.pi)
ProjectileVelocityX= dbeta_vec[i] * l_4 * sympy.cos( beta_vec[i] + 90)
ProjectileVelocityY= dbeta_vec[i] * l_4 * sympy.sin( beta_vec[i] + 90)
ProjectileAcceleration= [ 0, -g ]
for k in range(i+1, sim_length):
x2_vec[k]= x2_vec[k-1] + ProjectileVelocityX*dt + 0.5 * ProjectileAcceleration[0] * dt * dt
y2_vec[k]= y2_vec[k-1] + ProjectileVelocityY*dt + 0.5 * ProjectileAcceleration[1] * dt * dt
ProjectileVelocityX= ProjectileVelocityX + ProjectileAcceleration[0]*dt
ProjectileVelocityY= ProjectileVelocityY + ProjectileAcceleration[1]*dt
# Create Animation:
# Setup Figure: Initialize Figure / Axe Handles
fig, ax = plt.subplots()
p1, = ax.plot([], [], color='black', linewidth=2)
p2, = ax.plot([], [], color='green', linewidth=2)
p3, = ax.plot([], [], color='blue', linewidth=2)
ax.axis('equal')
if targetY > 0:
if targetY > targetX:
ax.set_ylim([-10, (ymax + 20) ])
else:
ax.set_xlim([-10, (targetX + 20) ])
else:
if -targetY > targetX:
ax.set_ylim([ (ymax - 20), 10 ])
else:
ax.set_xlim([-10, (targetX + 20) ])
ax.set_xlabel('X') # X Label
ax.set_ylabel('Y') # Y Label
ax.set_title('Trebuchet Simulation:')
video_title = "simulation"
# Setup Animation Writer:
FPS = 10
sample_rate = int(1 / (dt * FPS))
dpi = 300
writerObj = FFMpegWriter(fps=FPS)
# Initialize Patch: Pendulum 1 and 2
m_1 = Circle((0, 0), radius=0.5, color='cornflowerblue', zorder=10)
m_2 = Circle((0, 0), radius=0.5, color='cornflowerblue', zorder=10)
ax.add_patch(m_1)
ax.add_patch(m_2)
# Draw Static Objects:
pivot = Circle((0 ,0), radius=0.1, color='red', zorder=10)
ax.add_patch(pivot)
target = Circle((0 ,0), radius=2.5, color='red', zorder=10)
ax.add_patch(target)
# Plot and Create Animation:
with writerObj.saving(fig, video_title+".mp4", dpi):
for k in range(0, complete, sample_rate):
# Draw Pendulum Arm:
if(k<i):
x_coordinates = [x2_vec[k], l_2*sympy.cos(th_vec[k] + 3.14), 0, l_1*sympy.cos(th_vec[k]), x1_vec[k]]
y_coordinates = [y2_vec[k], l_2*sympy.sin(th_vec[k] + 3.14), 0, l_1*sympy.sin(th_vec[k]), y1_vec[k]]
p1.set_data(x_coordinates, y_coordinates)
m_1_center = x1_vec[k], y1_vec[k]
# Update Pendulum Patches:
m_2_center = x2_vec[k], y2_vec[k]
m_1.center = m_1_center
m_2.center = m_2_center
target.center= (targetX, targetY)
# Update Drawing:
fig.canvas.draw()
# Grab and Save Frame:
writerObj.grab_frame()
from IPython.display import Video
Video("/work/simulation.mp4", embed=True, width=640, height=480)
plt.plot(np.arange(0, len(angle), 1), angle, color='blue')
plt.xlabel('Iteration')
plt.ylabel('Radians')
plt.title('Bisection Method Angles')
plt.show()