TFY 4345 Numerical model of Trebuchet
#importing pacages to be used
import numpy as np
import matplotlib.pyplot as plt
def f(u, dT): #RHS of the equation to be solved by RK_4
w = np.zeros(4)
w[0] = u[2]
w[1] = u[3]
w[2] = 0
w[3] = -g
return w
def RK_4(f , u , dT): #one step of RK_4
F1 = f(u, dT)
F2 = f(u + dT/2 * F1, dT)
F3 = f(u + dT/2 *F2, dT)
F4 = f(u + dT/2 *F3, dT)
return u + dT/6*(F1 + 2*F2 + 2*F3 + F4)
def solver(f, h, v0, theta): #solving RK_4
u = decompose(v0,theta)
x = np.zeros(1)
y = np.zeros(1)
while u[1] >= 0:
u = RK_4(f, u , h)
x = np.append(x,u[0])
y = np.append(y,u[1])
return x , y
#Initial conditions
m1 = 2000E3
m2 = 15E3
l1 = 1.2
l2 = 5.7
l3 = 3.2
l4 = 5
g = 9.81
theta0 = 0.7*np.pi
dTheta0 = 0
psi0 = theta0 - 0.5*np.pi
dPsi0 = 0
u = [theta0, psi0, dTheta0, dPsi0]
def A_Matrix(u):
a11 = -((l1**2)*m1)-((l2**2)*m2)+(2*l2*l4*m2*np.cos(u[1]))-((l4**2)*m2)
a12 = -l4*m2*(l2*np.cos(u[1])-l4)
a22 = -(l4**2)*m2
return np.matrix([[a11, a12], [a12, a22]])
def b(u):
return[g*m1*l1*np.sin(u[0]) - g*m2* (l2*np.sin(u[0]) + l4*np.sin(u[1]-u[0]))
-l2*l4*m2*(u[3]-2*u[2])*np.sin(u[1])*u[3],
l4*m2*(g*np.sin(u[1] - u[0]) - l2 * np.sin(u[1])*(u[2]**2))]
#mega stygg ligning
def RHST(u, dT):
A = A_Matrix(u)
temp = np.zeros(4)#temporary vector holding position and radial velocity
b1 = b(u)
dPT = np.dot((A.I), b1)
temp[2] = dPT[0,0]
temp[3] = dPT[0,1]
temp[0] = u[2]
temp[1] = u[3]
return temp
def solverTrebuch(f, dT, u, time): #solving RK_4
t = np.array([0])
theta = np.array([theta0])
psi = np.array([psi0])
dtheta = np.array([dTheta0])
dPsi = np.array([dPsi0])
i = 0
while t[i] <= time:
u = RK_4(f, u , dT)
theta = np.append(theta,u[0])
psi = np.append(psi,u[1])
dtheta = np.append(dtheta, u[2])
dPsi = np.append(dPsi, u[3])
t = np.append(t, t[i]+dT)
i+=1
return theta, psi, dtheta, dPsi, t
theta, psi, dtheta, dPsi, t = solverTrebuch(RHST, 0.01, u, 10)
plt.title("Theta and Dtheta as a function of t")
plt.plot(t, theta, label = "theta")
plt.plot(t, dtheta, label = "dtheta")
plt.ylabel("theta [rad]")
plt.xlabel("t[s]")
plt.legend()
plt.grid()
plt.show()
plt.title("Psi as a function of t")
plt.plot(t, psi, label = "psi")
plt.plot(t,dPsi, label = " dpsi")
plt.ylabel("psi [rad]")
plt.xlabel("t[s]")
plt.legend()
plt.grid()
plt.show()
theta, psi, dtheta, dPsi, t = solverTrebuch(RHST, 0.01, u, 50)
plt.title("Theta as a function of t")
plt.plot(t, theta, label = "theta")
plt.plot(t,dtheta, label = "dtheta")
plt.ylabel("theta [rad]")
plt.xlabel("t[s]")
plt.legend()
plt.grid()
plt.show()
plt.title("Psi as a function of t")
plt.plot(t, psi, label = "psi")
plt.plot(t, dPsi, label = "dpsi")
plt.ylabel("psi [rad]")
plt.xlabel("t[s]")
plt.legend()
plt.grid()
plt.show()
def convertCoordinates(theta, psi):
r_1 = np.zeros((2, len(theta)))
r_2b = np.zeros([2, len(theta)])
v_2b = np.zeros([2, len(theta)])
r_1[0,:] = l1*np.sin(theta)
r_1[1,:] = -l1*np.cos(theta) + l3
r_2b[0, :] = -l2*np.sin(theta) - l4*np.sin(psi-theta)
r_2b[1, :] = l2*np.cos(theta) + l3 - l4*np.cos(psi- theta)
return r_1, r_2b
r_1, r_2b = convertCoordinates(theta, psi)
plt.plot(r_1[0,:], r_1[1,:])
plt.plot(r_2b[0,:], r_2b[1,:])
def solverTrebuchet2(dT, u, thetaR): #solving RK_4
t = np.array([0])
theta = np.array([theta0])
psi = np.array([psi0])
dtheta = np.array([dTheta0])
dPsi = np.array([dPsi0])
i = 0
while theta[i] >= thetaR :
u = RK_4(RHST, u , dT)
theta = np.append(theta,u[0])
psi = np.append(psi,u[1])
dtheta = np.append(dtheta, u[2])
dPsi = np.append(dPsi, u[3])
t = np.append(t, t[i]+dT)
i+=1
return theta, psi, dtheta, dPsi, t
def RK_42(u , dT): #one step of RK_4
F1 = f(u, dT)
F2 = f(u + dT/2 * F1, dT)
F3 = f(u + dT/2 *F2, dT)
F4 = f(u + dT/2 *F3, dT)
return u + dT/6*(F1 + 2*F2 + 2*F3 + F4)
def f(u, dT): #RHS of the equation to be solved by RK_4
w = np.zeros(4)
w[0] = u[2]
w[1] = u[3]
w[2] = 0
w[3] = -g
return w
def solver(h, v0x, v0y, x0, y0): #solving RK_4
R = np.zeros(4)
R[0] = x0
R[1] = y0
R[2] = v0x
R[3] = v0y
x = np.array((x0))
y = np.array((y0))
vx = np.array((v0x))
vy = np.array((v0y))
while R[1] >= 0:
R = RK_42(R , h)
x = np.append(x,R[0])
y = np.append(y,R[1])
vx = np.append(vx, R[2])
vy = np.append(vy, R[3])
return x , y, vx, vy
def treb2Projectile(theta0, psi0, dtheta0, dpsi0, dT , thetaR):
u = [theta0, psi0, dtheta0, dpsi0]
theta, psi, dtheta, dpsi, t = solverTrebuchet2(dT, u, thetaR)
r_1, r_2b1 = convertCoordinates(theta, psi)
v_0x = -l2*dtheta[-1]*np.cos(theta[-1])-l4*np.cos(psi[-1] - theta[-1])*(dpsi[-1]-dtheta[-1])
v_0y = -l2*dtheta[-1]*np.sin(theta[-1]) + l4*np.sin(psi[-1] - theta[-1])*(dpsi[-1]-dtheta[-1])
xproj, yproj, vx, vy = solver(0.01, v_0x, v_0y, r_2b1[0,-1], r_2b1[1,-1])
return r_2b1, xproj, yproj, vx, vy
r_2b2, xproj, yproj, vx, vy = treb2Projectile(theta0, psi0, dTheta0, dPsi0, 0.01 , 0.1*np.pi)
plt.plot(r_2b2[0,:],r_2b2[1,:])
plt.plot(xproj,yproj)
plt.xlabel("x[m]")
plt.ylabel("y[m]")
plt.title("Tradejtory of projectile ")
for i in range(0, 40, 5):
r_2i, xproj1, yproj1, vx, vy = treb2Projectile(theta0, psi0, dTheta0, dPsi0, 0.01 , np.deg2rad(i))
plt.plot(xproj1, yproj1, label= ("thetaR = " + str(i) + " deg" ))
plt.xlabel("x[m]")
plt.ylabel("y[m]")
plt.title("Trajectory of projectile for different release angel"+"\n"+"theta0 = 0.7pi and psi0 = theta0 - 0.5pi")
plt.legend()
for i in range(0, 40, 5):
r_2i, xproj12, yproj12, vx, vy = treb2Projectile(theta0, 0, dTheta0, dPsi0, 0.01 , np.deg2rad(i))
plt.plot(xproj12, yproj12, label= ("thetaR = " + str(i) + " deg" ))
plt.xlabel("x[m]")
plt.ylabel("y[m]")
plt.title("Trajectory of projectile for different release angel"+"\n"+"theta0 = 0.7pi and psi0 = 0")
plt.legend()
theta01 = 0.9*np.pi
psi01 = theta01 - 0.5*np.pi
for i in range(0, 40, 5):
r_2i, xproj1, yproj1, vx, vy = treb2Projectile(theta01, psi01, dTheta0, dPsi0, 0.01 , np.deg2rad(i))
plt.plot(xproj1, yproj1, label= ("thetaR = " + str(i) + " deg" ))
plt.xlabel("x[m]")
plt.ylabel("y[m]")
plt.title("Trajectory of projectile for different release angel"+"\n"+"theta0 = 0.9pi and psi0 = theta0 - 0.5pi")
plt.legend()
for i in range(0, 40, 5):
r_2i, xproj1, yproj1, vx, vy = treb2Projectile(theta01, 0, dTheta0, dPsi0, 0.01 , np.deg2rad(i))
plt.plot(xproj1, yproj1, label= ("theta R = " + str(i) + " deg" ))
plt.xlabel("x[m]")
plt.ylabel("y[m]")
plt.title("Trajectory of projectile for different release angel"+"\n"+"theta0 = 0.9pi and psi0 = 0")
plt.legend()
def Range(x, y):
r = - y[-2]/y[-1]
return ((x[-2] + r*x[-1])/(r+1))
def impEnergy(vx, vy):
impE = 0.5*m1*(vx[-1]**2 + vy[-1]**2)
return impE
impact = []
impactEnr = []
realeasAng = np.linspace(-5, 60, 100)
for i in realeasAng:
r_2i, xproj3, yproj3, vx, vy = treb2Projectile(theta0, psi0, dTheta0, dPsi0, 0.01 , np.deg2rad(i))
impact.append(Range(xproj3, yproj3))
impactEnr.append(impEnergy(vx, vy))
plt.title("Range as a function of angel")
plt.plot(realeasAng, impact)
plt.xlabel("thetaR[Deg]")
plt.ylabel("range[m]")
plt.show()
plt.title("Impact energy as a function of angel")
plt.plot(realeasAng, impactEnr)
plt.xlabel("thetaR[Deg]")
plt.ylabel("impact energy [J]")
plt.show()
print("Optimal release angle for range: ", realeasAng[np.argmax(impact)])
print("Optimal release angle for impactEnergy: ", realeasAng[np.argmax(impactEnr)])
r_2Imp, xprojImp, yprojImp, vxImp, vyImp = treb2Projectile(theta0, psi0, dTheta0, dPsi0, 0.01 , np.deg2rad(realeasAng[np.argmax(impact)]))
r_2Enr, xprojEnr, yprojEnr, vxEnr, vyEnr = treb2Projectile(theta0, psi0, dTheta0, dPsi0, 0.01 , np.deg2rad(realeasAng[np.argmax(impactEnr)]))
plt.plot(r_2Imp[0,:],r_2Imp[1,:])
plt.plot(xprojImp, yprojImp)
plt.title("Tradejtory of projectile longest range")
plt.show()
plt.plot(r_2Enr[0,:],r_2Enr[1,:])
plt.plot(xprojEnr, yprojEnr)
plt.title("Tradejtory of projectile highest impact energy")
plt.show()