func1 = system.state_space_post_invert(f, ma) states = pynamics.integration.integrate_odeint(func1, ini, t, rtol=1e-10, atol=1e-10, args=({ 'constants': constants, 'alpha': 1e2, 'beta': 1e1 }, )) # ============================================================================= KE = system.get_KE() PE = system.getPEGravity(0 * N.x) - system.getPESprings() energy = Output([KE - PE], constant_values=constants) energy.calc(states, t) energy.plot_time() # ============================================================================= positions = Output(system.get_q(0), constant_values=constants) positions.calc(states, t) positions.plot_time() speeds = Output(system.get_q(1), constant_values=constants) speeds.calc(states, t) speeds.plot_time() y = Output([G * qB_d], constant_values=constants) y.calc(states, t) y.plot_time()
system.add_spring_force1(k, (qB - preload2) * N.z, wAB) system.add_spring_force1(k, (qC - preload3) * N.z, wBC) system.addforcegravity(-g * N.y) x1 = BodyA.pCM.dot(N.x) y1 = BodyA.pCM.dot(N.y) x2 = BodyB.pCM.dot(N.x) y2 = BodyB.pCM.dot(N.y) x3 = BodyC.pCM.dot(N.x) y3 = BodyC.pCM.dot(N.y) KE = system.KE PE = system.getPEGravity(pNA) - system.getPESprings() t = numpy.r_[0:5:.001] outputs = Output([x1, y1, x2, y2, x3, y3, KE, PE, qA, qB, qC], system.constants) pynamics.tic() print('solving dynamics...') var_dd = system.solvedynamics('LU', auto_z=True) pynamics.toc() print('substituting constants...') var_dd = var_dd.subs(system.constants) print('creating second order function...') func1 = system.createsecondorderfunction(var_dd, statevariables, system.get_q(1), func_format='odeint') print('integrating...') states = scipy.integrate.odeint(func1, ini,
a = [0-pm2.dot(N.y)] b = [(item+abs(item)) for item in a] x1 = Particle1.pCM.dot(N.y) x2 = Particle2.pCM.dot(N.y) f,ma = system.getdynamics() #func = system.state_space_post_invert(f,ma,eq) func = system.state_space_post_invert2(f,ma,eq1_dd,eq1_d,eq1,eq_active = b) states=pynamics.integration.integrate_odeint(func,ini,t,rtol = error, atol = error, args=({'alpha':alpha,'beta':beta, 'constants':system.constant_values},),full_output = 1,mxstep = int(1e5)) states = states[0] KE = system.get_KE() PE = system.getPEGravity(pNA) - system.getPESprings() output = Output([x1,x2,l, KE-PE],system) y = output.calc(states) pynamics.toc() plt.figure(0) plt.plot(t,y[:,0]) plt.plot(t,y[:,1]) plt.axis('equal') plt.figure(1) plt.plot(t,y[:,2]) plt.axis('equal') plt.figure(2) plt.plot(t,y[:,3]) #plt.axis('equal')
eq_d.append(wOMC.dot(N.z) - G * wOC.dot(N.z)) #eq_d = [N.get_w_to(A).dot(N.z) - G*N.get_w_to(B).dot(N.z)] eq_dd = [system.derivative(item) for item in eq_d] # f, ma = system.getdynamics() func1 = system.state_space_post_invert(f, ma, eq_dd, constants=constants, variable_functions={my_signal: ft2}) states = pynamics.integration.integrate(func1, ini1, t, rtol=tol, atol=tol) KE = system.get_KE() PE = system.getPEGravity(0 * N.x) - system.getPESprings() energy = Output([KE - PE], constant_values=constants) energy.calc(states) energy.plot_time() #torque_plot = Output([torque]) #torque_plot.calc(states) #torque_plot.plot_time() points = [pDtip, pCD, pOC, pOA, pAB, pBtip] points = PointsOutput(points, constant_values=constants) y = points.calc(states) y = y.reshape((-1, 6, 2)) plt.figure() for item in y[::30]: plt.plot(*(item.T))
pCcm=0*N.x w1 = N.get_w_to(C) IC = Dyadic.build(C,Ixx,Iyy,Izz) BodyC = Body('BodyC',C,pCcm,mC,IC) system.addforcegravity(-g*N.y) # system.addforce(1*C.x+2*C.y+3*C.z,w1) points = [1*C.x,0*C.x,1*C.y,0*C.y,1*C.z] f,ma = system.getdynamics() # func1 = system.state_space_pre_invert(f,ma) func1 = system.state_space_post_invert(f,ma) ini = [initialvalues[item] for item in system.get_state_variables()] states=pynamics.integration.integrate_odeint(func1,ini,t,rtol = tol, atol=tol,args=({'constants':system.constant_values},)) po = PointsOutput(points,system) po.calc(states,t) po.animate(fps = 30,lw=2) so = Output([qA,qB,qC]) so.calc(states,t) so.plot_time()
func1 = system.state_space_post_invert(f,ma,variable_functions=variable_functions) # # func1,lambda1 = system.state_space_post_invert(f,ma,eq_dd,return_lambda = True) states=pynamics.integration.integrate_odeint(func1,ini,t,rtol=tol,atol=tol,args=({'constants':system.constant_values},) ) # lambda2 = numpy.array([lambda1(item1,item2,system.constant_values) for item1,item2 in zip(t,states)]) KE = system.get_KE() PE = system.getPEGravity(pNA) - system.getPESprings() points = [pNA,pAB,pBC,pCtip] #points = [item for item2 in points for item in [item2.dot(system.newtonian.x),item2.dot(system.newtonian.y)]] points_output = PointsOutput(points,system) y = points_output.calc(states) #y.resize(y.shape[0],int(y.shape[1]/2),2) plt.figure() plt.plot(t,states[:,:3]) plt.figure() plt.plot(*(y[::int(len(y)/20)].T)) plt.axis('equal') energy_output = Output([KE-PE],system) energy_output.calc(states) plt.figure() plt.plot(energy_output.y) # points_output.animate(fps = 100,movie_name = 'render.mp4',lw=2,marker='o',color=(1,0,0,1),linestyle='-') #a()
t, rtol=tol, atol=tol, args=({ 'alpha': 1e4, 'beta': 1e2, 'constants': system.constant_values }, )) KE = system.get_KE() PE = system.getPEGravity(pNO) - system.getPESprings() output = Output([KE - PE], system) y = output.calc(states, t) plt.figure() plt.plot(y[:]) plt.show() o2 = [pNO, A1.x, A2.x, pNO, A2.x, A3.x, pNO, B1.x, B2.x, pNO, B2.x, B23.x, pNO] points_output = PointsOutput3D(o2, system) y = points_output.calc(states, t) # points_output.plot_time() #points_output.animate(fps = 30,movie_name = 'render.mp4',lw=2,marker='o',color=(1,0,0,1),linestyle='-') # self = points_output # import matplotlib.pyplot as plt # from mpl_toolkits.mplot3d import Axes3D
system.addforce(fABx*N.x+fABy*N.y,vBA) system.addforce(-fABx*N.x+-fABy*N.y,vAB) system.addforcegravity(-g*N.y) x1 = BodyA.pCM.dot(N.x) y1 = BodyA.pCM.dot(N.y) x2 = BodyB.pCM.dot(N.x) y2 = BodyB.pCM.dot(N.y) KE = system.KE PE = system.getPEGravity(pNA) statevariables = system.get_state_variables() ini = [item.subs(initialvalues) for item in statevariables] t = scipy.arange(0,10,.01) outputs = Output([x1,y1,x2,y2,KE,PE],system) #tic() print('solving dynamics...') f,ma = system.getdynamics() #eq_con = [dot(constraint1_dd,N.x),dot(constraint1_dd,N.y),dot(constraint2_dd,N.x),dot(constraint2_dd,N.y)] print('solving constraints...') eq_con = [constraint2_dd.dot(B.x),constraint2_dd.dot(B.y)] var_dd,forces = system.solveconstraineddynamics(list(numpy.array(f) - numpy.array(ma)),eq_con,system.get_q(2),[fABx,fABy]) #toc() print('creating second order function...') var_dd var_dd=var_dd.subs(system.constants) func1 = system.createsecondorderfunction_old(var_dd)
f,ma = system.getdynamics() func1,lambda1 = system.state_space_post_invert(f,ma,eq_dd,return_lambda = True) states=pynamics.integration.integrate(func1,ini,t,rtol=tol,atol=tol, args=({'constants':system.constant_values},)) # Plot --- output plt.figure() artists = plt.plot(t,states[:,:3]) plt.legend(artists,['qA','qB','qC']) # Plot --- energy KE = system.get_KE() PE = system.getPEGravity(pNA) - system.getPESprings() energy_output = Output([KE-PE],system) energy_output.calc(states) energy_output.plot_time() # Motion points = [pNA,pAB,pBtip,pBC, pCD] points_output = PointsOutput(points,system) y = points_output.calc(states) points_output.plot_time(20) points_output.animate(fps = 30,lw=2) from matplotlib import animation, rc from IPython.display import HTML HTML(points_output.anim.to_html5_video())
#import sympy #eq = sympy.Matrix(f)-sympy.Matrix(ma) #sol = sympy.solve(eq,(qA_dd,qB_dd)) # #qadd = sol[qA_dd] #qbdd = sol[qB_dd] # #(Ixx_B*qA_d*qB_d*sin(2*qB) - Iyy_B*qA_d*qB_d*sin(2*qB) - g*lA*mA*sin(qA) - g*lB*mB*sin(qA))/(Ixx_A - Ixx_B*sin(qB)**2 + Ixx_B + Iyy_B*sin(qB)**2 + lA**2*mA + lB**2*mB) print('creating second order function...') func1 = system.state_space_post_invert(f, ma) print('integrating...') states = scipy.integrate.odeint(func1, ini, t, rtol=error, atol=error) pynamics.toc() print('calculating outputs..') output = Output([x1, y1, x2, y2, KE - PE, qA, qB], system) y = output.calc(states) pynamics.toc() plt.figure(1) plt.hold(True) plt.plot(y[:, 0], y[:, 1]) plt.plot(y[:, 2], y[:, 3]) plt.axis('equal') plt.figure(2) plt.plot(y[:, 4]) plt.figure(3) plt.hold(True) plt.plot(t, y[:, 6:] * 180 / pi)
t = scipy.arange(0, 10, .01) pynamics.tic() print('solving dynamics...') var_dd = system.solvedynamics('LU', False) pynamics.toc() print('integrating...') var_dd = var_dd.subs(system.constants) func1 = system.createsecondorderfunction(var_dd, statevariables, system.get_q(1), func_format='odeint') states = scipy.integrate.odeint(func1, ini, t, rtol=1e-8, atol=1e-8) pynamics.toc() print('calculating outputs..') x1 = BodyA.pCM.dot(N.x) y1 = BodyA.pCM.dot(N.y) KE = system.KE PE = system.getPEGravity(pNA) outputs = Output([x1, y1, KE, PE], system.constants) outputs.calc(statevariables, states) pynamics.toc() plt.figure(1) plt.hold(True) plt.plot(outputs(x1), outputs(y1)) plt.figure(2) plt.plot(t, outputs(KE) - outputs(PE)) plt.show()
args=({ 'alpha': alpha, 'beta': beta, 'constants': system.constant_values }, ), full_output=1, mxstep=int(1e5)) states = states[0] KE = system.get_KE() PE = system.getPEGravity(pOrigin) - system.getPESprings() output = Output([x1, x2, l.length(), KE - PE], system) y = output.calc(states, t) plt.figure(0) plt.plot(t, y[:, 0]) plt.plot(t, y[:, 1]) plt.axis('equal') plt.figure(1) plt.plot(t, y[:, 2]) plt.axis('equal') plt.figure(2) plt.plot(t, y[:, 3]) #plt.axis('equal') points = [BodyA.pCM, Particle2.pCM]
statevariables = system.get_state_variables() ini = [initialvalues[item] for item in statevariables] tol = 1e-10 tinitial = 0 tfinal = 10 fps = 30 tstep = 1 / fps t = numpy.r_[tinitial:tfinal:tstep] constants = system.constant_values.copy() states = pynamics.integration.integrate_odeint(func1, ini, t, rtol=tol, atol=tol, args=({ 'constants': constants }, )) states_min = [qNA, qAB, qBC] states_min_out = Output(states_min, system) states_min_out.calc(states, t) states_min_out.plot_time() points = [pBase, pAB, pBC, pCtip] points_output = PointsOutput(points, system) points_output.calc(states, t) points_output.plot_time(20) # points_output.animate(fps = fps,movie_name = 'triple_pendulum_maximal.mp4',lw=2,marker='o',color=(1,0,0,1),linestyle='-')
pynamics.tic() print('solving dynamics...') f, ma = system.getdynamics() print('creating second order function...') func1 = system.state_space_post_invert(f, ma, eq) print('integrating...') states = scipy.integrate.odeint(func1, ini, t, rtol=1e-12, atol=1e-12, hmin=1e-14) pynamics.toc() print('calculating outputs..') output = Output([x1, y1, x2, y2, x3, y3, x4, y4, KE - PE, qA, qB, qC], system) y = output.calc(states) pynamics.toc() plt.figure(1) plt.hold(True) plt.plot(y[:, 0], y[:, 1]) plt.plot(y[:, 2], y[:, 3]) plt.plot(y[:, 4], y[:, 5]) plt.plot(y[:, 6], y[:, 7]) plt.axis('equal') plt.figure(2) plt.plot(y[:, 8]) plt.figure(3)
particle1 = Particle(p1,mp1) body1 = Body('body1',f4,p1,mp1,Dyadic.build(f4,1,1,1),system = None) #system.addforce(-b*v1,v1) system.addforcegravity(-g*f1.z) #system.add_spring_force1(k,(q1)*f1.z,wNA) points = [particle1.pCM] points_x = [item.dot(f1.x) for item in points] points_y = [item.dot(f1.y) for item in points] points_z = [item.dot(f1.z) for item in points] output_x = Output(points_x) output_y = Output(points_y) output_z = Output(points_z) f,ma = system.getdynamics() func = system.state_space_post_invert(f,ma) t = numpy.r_[0:5:.001] states=pynamics.integration.integrate_odeint(func,system.get_ini(),t,atol=1e-5,rtol = 1e-5, args=({'constants':system.constant_values},)) x = output_x.calc(states) y = output_y.calc(states) z = output_z.calc(states) KE = system.get_KE() PE = system.getPEGravity(0*f1.x) - system.getPESprings() output = Output([KE-PE])
# f, ma = system.getdynamics() func1 = system.state_space_post_invert(f, ma, eq_dd, constants=system.constant_values, variable_functions={my_signal: ft2}) states = pynamics.integration.integrate_odeint(func1, ini1, t, rtol=tol, atol=tol) KE = system.get_KE() PE = system.getPEGravity(0 * N.x) - system.getPESprings() energy = Output([KE - PE, toeforce, heelforce]) energy.calc(states) energy.plot_time() #torque_plot = Output([torque]) #torque_plot.calc(states) #torque_plot.plot_time() points = [pDtip, pCD, pOC, pOA, pAB, pBtip, pE1, pE2, pBtip] points = PointsOutput(points) y = points.calc(states) y = y.reshape((-1, 9, 2)) plt.figure() for item in y[::30]: plt.plot(*(item.T))
states = pynamics.integration.integrate_odeint(func, ini, t, rtol=1e-12, atol=1e-12, hmin=1e-14, args=({ 'constants': system.constant_values }, )) KE = system.get_KE() PE = system.getPEGravity( pNA) - system.getPESprings() - 1 / 2 * k * (stretch)**2 output = Output([x1, y1, q, KE - PE], system) y = output.calc(states) plt.figure(1) plt.plot(y[:, 0], y[:, 1]) plt.axis('equal') plt.figure(2) plt.plot(y[:, 3]) #plt.figure(3) #plt.plot(t,y[:,0]) #plt.show() points = [pNA, pAB] #points = [item for item2 in points for item in [item2.dot(system.newtonian.x),item2.dot(system.newtonian.y)]]
#system.add_spring_force1(k,(stretched1)*N.y,vAcm) system.add_spring_force1(k,(stretched2)*N.y,vAcm) system.addforcegravity(-g*N.y) x1 = ParticleA.pCM.dot(N.x) y1 = ParticleA.pCM.dot(N.y) f,ma = system.getdynamics() func1 = system.state_space_post_invert(f,ma) states=pynamics.integration.integrate_odeint(func1,ini,t,rtol=1e-12,atol=1e-12,hmin=1e-14, args=({'constants':system.constant_values},)) KE = system.get_KE() PE = system.getPEGravity(pNA)-system.getPESprings() output = Output([x1,y1,KE-PE,x,y],system) y = output.calc(states) plt.figure(1) plt.plot(y[:,0],y[:,1]) plt.axis('equal') plt.figure(2) plt.plot(y[:,2]) plt.figure(3) plt.plot(t,y[:,4]) plt.show() plt.figure(4) plt.plot(t,states[:,2])
f, ma = system.getdynamics() changing_constants = [freq] static_constants = system.constant_values.copy() for key in changing_constants: del static_constants[key] func = system.state_space_post_invert(f, ma, constants=static_constants) statevariables = system.get_state_variables() ini = [initialvalues[item] for item in statevariables] points = [pNA, pAcm, pAtip] points_output = PointsOutput(points, system) out1 = Output([tin, qA]) my_constants = {} freq_sweep = numpy.r_[-1.5:1:20j] freq_sweep = 1 * 10**freq_sweep amps = [] for ff in freq_sweep: tol = 1e-4 tinitial = 0 tfinal = 10 / ff tstep = 1 / 30 t = numpy.r_[tinitial:tfinal:tstep] my_constants[freq] = ff
rtol=tol, atol=tol, args=({ 'constants': {}, 'alpha': 1e2, 'beta': 1e1 }, )) lambda1_n = numpy.array([lambda1(tt, ss) for tt, ss in zip(t, states)]) plt.figure() plt.plot(t, lambda1_n) # ============================================================================= KE = system.get_KE() PE = system.getPEGravity(0 * N.x) - system.getPESprings() energy = Output([KE - PE]) energy.calc(states, t) energy.plot_time() # ============================================================================= points_list = [p1, p2] #points_list = [item2 for item in points_list for item2 in [item.dot(N.x),item.dot(N.y)]] #points = Output(points_list) #y = points.calc(states,t) #y = y.reshape((-1,2,2)) #plt.figure() #plt.plot(y[:,1,0],y[:,1,1]) #plt.axis('equal') states2 = Output([x, q]) states2.calc(states, t)
states = pynamics.integration.integrate_odeint(func, ini, t, args=({ 'alpha': 1e4, 'beta': 1e2, 'constants': system.constant_values }, )) KE = system.get_KE() PE = system.getPEGravity(pNA) - system.getPESprings() output = Output([x1, y1, KE - PE, x, y] + eq1 + b, system) y = output.calc(states, t) plt.figure(1) plt.plot(y[:, 0], y[:, 1]) plt.axis('equal') plt.figure(2) plt.plot(y[:, 2]) plt.figure(3) plt.plot(t, y[:, 3]) plt.show() plt.figure(5) plt.plot(t, y[:, 5:7])