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])
