#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()