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)
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))
tfinal = 5
tstep = .001
t = numpy.r_[tinitial:tfinal:tstep]
preload1 = Constant('preload1', 0 * pi / 180, system)
x, x_d, x_dd = Differentiable(system, 'x')
y, y_d, y_dd = Differentiable(system, 'y')
initialvalues = {}
initialvalues[x] = 1
initialvalues[y] = 0
initialvalues[x_d] = 0
initialvalues[y_d] = 0
statevariables = system.get_q(0) + system.get_q(1)
ini = [initialvalues[item] for item in statevariables]
N = Frame('N')
system.set_newtonian(N)
pNA = 0 * N.x
pAB = pNA + x * N.x + y * N.y
vAB = pAB.time_derivative(N, system)
ParticleA = Particle(system, pAB, mA, 'ParticleA')
system.addforce(-b * vAB, vAB)
system.addforcegravity(-g * N.y)
x1 = ParticleA.pCM.dot(N.x)
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()
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,
t,
rtol=1e-12,
atol=1e-12,
hmin=1e-14)
pynamics.toc()
print('calculating outputs..')
outputs.calc(statevariables, states)
pynamics.toc()
plt.figure(1)
plt.plot(outputs(x1), outputs(y1))
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)
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)
preload2 = Constant('preload2',0*pi/180,system)
preload3 = Constant('preload3',0*pi/180,system)
qA,qA_d,qA_dd = Differentiable(system,'qA')
qB,qB_d,qB_dd = Differentiable(system,'qB')
qC,qC_d,qC_dd = Differentiable(system,'qC')
initialvalues = {}
initialvalues[qA]=0*pi/180
initialvalues[qA_d]=0*pi/180
initialvalues[qB]=0*pi/180
initialvalues[qB_d]=0*pi/180
initialvalues[qC]=0*pi/180
initialvalues[qC_d]=0*pi/180
statevariables = system.get_q(0)+system.get_q(1)
ini = [initialvalues[item] for item in statevariables]
N = Frame('N')
A = Frame('A')
B = Frame('B')
C = Frame('C')
system.set_newtonian(N)
A.rotate_fixed_axis_directed(N,[0,0,1],qA,system)
B.rotate_fixed_axis_directed(A,[0,0,1],qB,system)
C.rotate_fixed_axis_directed(B,[0,0,1],qC,system)
pNA=0*N.x
pAB=pNA+lA*A.x
pBC = pAB + lB*B.x
statevariables = system.get_state_variables()
ini0 = [initialvalues[item] for item in statevariables]
eq = []
eq.append(pBD-pDB)
eq_d = [item.time_derivative() for item in eq]
eq_scalar = []
eq_scalar.append(eq[0].dot(N.x))
eq_scalar.append(eq[0].dot(N.y))
c=KinematicConstraint(eq_scalar)
variables = [qB,qD]
constant_states = list(set(system.get_q(0))-set(variables))
constants = system.constant_values.copy()
for key in constant_states:
constants[key] = initialvalues[key]
guess = [initialvalues[item] for item in variables]
result = c.solve_numeric(variables,guess,constants)
ini = []
for item in system.get_state_variables():
if item in variables:
ini.append(result[item])
else:
ini.append(initialvalues[item])
#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_q(0) + system.get_q(1)
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])
#Iyy_C = Constant('Iyy_C',7.9239401855911e-07,system)
#Izz_C = Constant('Izz_C',7.9239401855911e-07,system)
qA, qA_d, qA_dd = Differentiable(system, 'qA')
qB, qB_d, qB_dd = Differentiable(system, 'qB')
qC, qC_d, qC_dd = Differentiable(system, 'qC')
initialvalues = {}
initialvalues[qA] = 0 * pi / 180
initialvalues[qA_d] = 0 * pi / 180
initialvalues[qB] = 0 * pi / 180
initialvalues[qB_d] = 0 * pi / 180
initialvalues[qC] = 0 * pi / 180
initialvalues[qC_d] = 0 * pi / 180
statevariables = system.get_q(0) + system.get_q(1)
ini = [initialvalues[item] for item in statevariables]
N = Frame('N')
A = Frame('A')
B = Frame('B')
C = Frame('C')
system.set_newtonian(N)
A.rotate_fixed_axis_directed(N, [0, 0, 1], qA, system)
B.rotate_fixed_axis_directed(A, [0, 0, 1], qB, system)
C.rotate_fixed_axis_directed(B, [0, 0, 1], qC, system)
pNA = 0 * N.x
pAB = pNA + lA * A.x
pBC = pAB + lB * B.x
x2 = ParticleB.pCM.dot(N.x)
y2 = ParticleB.pCM.dot(N.y)
x3 = ParticleC.pCM.dot(N.x)
y3 = ParticleC.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...')
a = [aCtip.dot(N.x), aCtip.dot(N.y)]
import sympy
b = sympy.Matrix(a)
b.jacobian(system.get_q(2))
c = b.jacobian(system.get_q(2))
d = sympy.lambdify(system.get_state_variables(), c)
func1 = system.createsecondorderfunction4(f, ma, d)
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, KE - PE, qA, qB, qC], system)
y = output.calc(states)
ParticleA = Particle(pAB, mA, 'ParticleA', system)
#ParticleB = Particle(pAB2,mA,'ParticleB',system)
system.addforce(-b * vAB, vAB)
#system.addforce(-b*vAB2,vAB2)
system.addforcegravity(-g * N.y)
#v = pAB2-pNA2
#u = (v.dot(v))**.5
#
#eq1 = [(v.dot(v)) - lA**2]
#eq1_dd=system.derivative(system.derivative(eq1[0]))
#eq = [eq1_dd]
f, ma = system.getdynamics()
func = system.state_space_post_invert(f, ma)
q_dd = system.get_q(2)
func = system.solve_f_ma(f, ma, q_dd)
func = func.subs(system.constant_values)
f = sympy.lambdify([qA, qA_d, system.t], func)
func2 = pynamics.integration.build_step(f)
states = pynamics.integration.integrate_newton(f, ini, t)
#func = system.state_space_post_invert(f,ma,eq)
#states=pynamics.integration.integrate_odeint(func,ini,t,rtol=1e-12,atol=1e-12,hmin=1e-14, args=({'constants':system.constant_values},))
points = [pNA, pAB, pNA]
points_output = PointsOutput(points, system)
points_output.calc(states)
points_output.animate(fps=30, lw=2)
eq_dd=[(system.derivative(item)) for item in eq_d]
f,ma = system.getdynamics()
f = sympy.Matrix(f)
f = f.subs(system.constant_values)
ma = sympy.Matrix(ma)
ma = ma.subs(system.constant_values)
zero = f-ma
torques = (t1,t2,t3)
sol = sympy.solve(zero,torques)
sol2 = [sol[item] for item in torques]
# sol2 = sympy.Matrix([sol[item] for item in torques])
f_torques = sympy.lambdify(system.get_q(0)+system.get_q(1)+system.get_q(2),sol2)
res = numpy.array(f_torques(*(states_exp.T))).T
ft1 = scipy.interpolate.interp1d(t,res[:,0],fill_value = 'extrapolate', kind='quadratic')
ft2 = scipy.interpolate.interp1d(t,res[:,1],fill_value = 'extrapolate', kind='quadratic')
ft3 = scipy.interpolate.interp1d(t,res[:,2],fill_value = 'extrapolate', kind='quadratic')
plt.figure()
plt.plot(t,numpy.array([ft1(t),ft2(t),ft3(t)]).T,'-o')
variable_functions = {t1:ft1,t2:ft2,t3:ft3}
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},) )
f, ma = system.getdynamics()
f = sympy.Matrix(f)
f = f.subs(system.constant_values)
ma = sympy.Matrix(ma)
ma = ma.subs(system.constant_values)
zero = f - ma
torques = (t1, t2, t3)
sol = sympy.solve(zero, torques)
sol2 = [sol[item] for item in torques]
# sol2 = sympy.Matrix([sol[item] for item in torques])
f_torques = sympy.lambdify(
system.get_q(0) + system.get_q(1) + system.get_q(2), sol2)
res = numpy.array(f_torques(*(states_exp.T))).T
ft1 = scipy.interpolate.interp1d(t,
res[:, 0],
fill_value='extrapolate',
kind='quadratic')
ft2 = scipy.interpolate.interp1d(t,
res[:, 1],
fill_value='extrapolate',
kind='quadratic')
ft3 = scipy.interpolate.interp1d(t,
res[:, 2],
fill_value='extrapolate',
kind='quadratic')
