#IB = Dyadic.build(B,Ixx_B,Iyy_B,Izz_B) #IC = Dyadic.build(C,Ixx_C,Iyy_C,Izz_C) BodyA = Body('BodyA',A,pm1,m1,IA,system) #Particle1 = Particle(system,pm1,m1,'Particle1') Particle2 = Particle(system,pm2,m2,'Particle2') s1 = pk1.dot(N.y)*N.y s2 = pk2.dot(N.y)*N.y s3 = (q2-q2_command)*A.z wNA = A.getw_(N) wNB = B.getw_(N) #switch1 = system.add_spring_force(k,s1,vk1) system.add_spring_force(k,s2,vk2) system.add_spring_force(k_controller,s3,wNA) system.add_spring_force(k_controller,-s3,wNB) system.addforce(-b*vm1,vm1) system.addforcegravity(-g*N.y) #system.addforcegravity(-g*N.y) #system.addforcegravity(-g*N.y) x1 = BodyA.pCM.dot(N.x) y1 = BodyA.pCM.dot(N.y) x2 = Particle2.pCM.dot(N.x) y2 = Particle2.pCM.dot(N.y)
#BodyA = Body('BodyA',A,pAcm,mA,IA,system) #BodyB = Body('BodyB',B,pBcm,mB,IB,system) #BodyC = Body('BodyC',C,pCcm,mC,IC,system) ParticleA = Particle(system, pAcm, mA, 'ParticleA') ParticleB = Particle(system, pBcm, mB, 'ParticleB') ParticleC = Particle(system, pCcm, mC, 'ParticleC') system.addforce(-b * wNA, wNA) system.addforce(-b * wAB, wAB) system.addforce(-b * wBC, wBC) #system.addforce(-k*(qA-preload1)*N.z,wNA) #system.addforce(-k*(qB-preload2)*A.z,wAB) #system.addforce(-k*(qC-preload3)*B.z,wBC) system.add_spring_force(k, (qA - preload1) * N.z, wNA) system.add_spring_force(k, (qB - preload2) * N.z, wAB) system.add_spring_force(k, (qC - preload3) * N.z, wBC) system.addforcegravity(-g * N.y) vCtip = pCtip.time_derivative(N, system) eq1_d = [vCtip.dot(N.y)] eq1_dd = [(system.derivative(item)) for item in eq1_d] eq = eq1_dd x1 = ParticleA.pCM.dot(N.x) y1 = ParticleA.pCM.dot(N.y) x2 = ParticleB.pCM.dot(N.x) y2 = ParticleB.pCM.dot(N.y) x3 = ParticleC.pCM.dot(N.x)
pNA = 0 * N.x pAcm = pNA + x * N.x + y * N.y vAcm = pAcm.time_derivative(N, system) ParticleA = Particle(system, pAcm, mA, 'ParticleA') system.addforce(-b * vAcm, vAcm) stretch = y stretched1 = (stretch + abs(stretch)) / 2 stretched2 = -(-stretch + abs(-stretch)) / 2 #system.add_spring_force(k,(stretched1)*N.y,vAcm) system.add_spring_force(k, (stretched2) * N.y, vAcm) system.addforcegravity(-g * N.y) x1 = ParticleA.pCM.dot(N.x) y1 = ParticleA.pCM.dot(N.y) KE = system.KE PE = system.getPEGravity(pNA) - system.getPESprings() pynamics.tic() print('solving dynamics...') f, ma = system.getdynamics() print('creating second order function...') func1 = system.state_space_post_invert(f, ma) print('integrating...') states = scipy.integrate.odeint(func1,