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