t,
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])
stretched2 = -(-stretch+abs(-stretch))/2
#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()
system.addforce(-k*qB*A.z,wAB)
#system.addforce(fNAx*N.x+fNAy*N.y,vNA)
#system.addforce(-fNAx*N.x+-fNAy*N.y,vAN)
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])
#integrator = RK4(func,ini,t)
#integrator = DoPri(func,ini,t)
#states = integrator.run()
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()
#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.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) - .5 * k * (qA - preload1)**2 - .5 * k * (qB - preload2)**2
t = numpy.r_[0:10:.001]
outputs = Output([x1, y1, x2, y2, KE, PE, qA, qB], system.constants)
pynamics.tic()
print('solving dynamics...')
var_dd = system.solvedynamics('LU', True)
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,
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(pOrigin) - system.getPESprings()
output = Output([x1, x2, l, KE - PE], system)
y = output.calc(states)
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])
