#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,
