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='-')
