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()
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)
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) print('integrating...') states=scipy.integrate.odeint(func1,ini,t,rtol=1e-8,atol=1e-8) #toc() print('calculating outputs..') outputs.calc(statevariables,states) #toc() plt.figure(1) plt.plot(outputs(x1),outputs(y1)) plt.plot(outputs(x2),outputs(y2)) plt.figure(2) plt.plot(outputs(KE)-outputs(PE)) plt.show() # #link1surface = surfaceclass(A,pNA,system.constants,statevariables,'../../STL/phalanx.stl',1,False) #link2surface = surfaceclass(B,pAB,system.constants,statevariables,'../../STL/phalanx.stl',1,False) # #import DanGL #DanGL.init()
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]) plt.show()
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()
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)) points.animate(fps=30, movie_name='parallel_five_bar_jumper_foot.mp4', lw=2)
tinitial = 0 tfinal = 10 / ff tstep = 1 / 30 t = numpy.r_[tinitial:tfinal:tstep] my_constants[freq] = ff states = pynamics.integration.integrate(func, ini, t, rtol=tol, atol=tol, args=({ 'constants': my_constants }, )) # points_output.calc(states,t) # points_output.animate(fps = 1/tstep,lw=2,movie_name = 'pendulum_in_water.mp4',) # points_output.plot_time() out1.calc(states, t) # plt.figure() # plt.plot(out1.y[:,0],out1.y[:,1]) amp = out1.y[:, 1].max() - out1.y[:, 1].min() amps.append(amp) plt.figure() plt.loglog(freq_sweep, amps) plt.xlabel('freq') plt.ylabel('max amplitude') plt.title('freq vs amplitude')
for constraint in system.constraints: constraint.solve([wx, wy, wz], [qA_d, qB_d, qC_d]) BodyC = Body('BodyC', C, pCcm, mC, IC) system.addforcegravity(-g * N.y) 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, args=({ 'constants': system.constant_values }, )) po = PointsOutput(points, system) po.calc(states) po.animate(fps=30, lw=2) so = Output([qA, qB, qC]) so.calc(states) so.plot_time()
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]) outputs = output.calc(states) fig = plt.figure() ax = fig.add_subplot(111, projection='3d') plt.plot(x,y,z) plt.axis('equal')
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())
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) energy.plot_time() # ============================================================================= positions = Output(system.get_q(0), constant_values=constants) positions.calc(states) positions.plot_time() speeds = Output(system.get_q(1), constant_values=constants) speeds.calc(states) speeds.plot_time() y = Output([G * qB_d], constant_values=constants) y.calc(states) y.plot_time()
states = pynamics.integration.integrate_odeint(func1, ini, t, rtol=tol, atol=tol, args=({ 'constants': {}, 'alpha': 1e2, 'beta': 1e1 }, )) # ============================================================================= KE = system.get_KE() PE = system.getPEGravity(0 * N.x) - system.getPESprings() energy = Output([KE - PE]) energy.calc(states) 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) #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)
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='-')