ini = [initialvalues[item] for item in statevariables] A = Frame('A', system) B = Frame('B', system) C = Frame('C', system) D = Frame('D', system) system.set_newtonian(A) B.rotate_fixed_axis(A, [0, 0, 1], H, system) C.rotate_fixed_axis(B, [1, 0, 0], -L, system) D.rotate_fixed_axis(C, [0, 1, 0], Q, system) pNA = 0 * A.x pAD = pNA + x * A.x + y * A.y pBcm = pAD + r * C.z pDA = pBcm - r * D.z wAD = A.get_w_to(D) II = Dyadic.build(B, J, I, J) BodyD = Body('BodyD', D, pBcm, m, II, system) #ParticleA = Particle(pAcm,mA,'ParticleA',system) #ParticleB = Particle(pBcm,mB,'ParticleB',system) #ParticleC = Particle(pCcm,mC,'ParticleC',system) system.addforcegravity(-g * A.z) f, ma = system.getdynamics()
wOA = O.get_w_to(A) wAB = A.get_w_to(B) wOMC = O.get_w_to(MC) wOC = O.get_w_to(C) wCD = C.get_w_to(D) wBD = B.get_w_to(D) wNMA = N.get_w_to(MA) aNMA = wNMA.time_derivative() wNMC = N.get_w_to(MC) aNMC = wNMC.time_derivative() I_motorA = Dyadic.build(MA, Im, Im, Im) I_motorC = Dyadic.build(MC, Im, Im, Im) BodyO = Body('BodyO', O, pOcm, mO, Dyadic.build(O, I_main, I_main, I_main), system) #BodyA = Body('BodyA',A,pAcm,mA,Dyadic.build(A,I_leg,I_leg,I_leg),system) #BodyB = Body('BodyB',B,pBcm,mB,Dyadic.build(B,I_leg,I_leg,I_leg),system) #BodyC = Body('BodyC',C,pCcm,mC,Dyadic.build(C,I_leg,I_leg,I_leg),system) #BodyD = Body('BodyD',D,pDcm,mD,Dyadic.build(D,I_leg,I_leg,I_leg),system) MotorA = Body('MotorA', MA, pOA, m_motor, I_motorA, system) MotorA = Body('MotorC', MC, pOC, m_motor, I_motorC, system) InductorA = PseudoParticle(0 * MA.x, L, name='InductorA', vCM=iA * MA.x, aCM=iA_d * MA.x) InductorC = PseudoParticle(0 * MC.x, L,
plt.plot(*(item.T)) # for item,value in zip(system.get_state_variables(),result.x): # initialvalues[item]=value pAcm=pOA+lA/2*A.x pBcm=pAB+lB/2*B.x pCcm=pOC+lC/2*C.x pDcm=pCD+lD/2*D.x wOA = O.getw_(A) wAB = A.getw_(B) wOC = O.getw_(C) wCD = C.getw_(D) wBD = B.getw_(D) BodyO = Body('BodyO',O,pOcm,mO,Dyadic.build(O,I_main,I_main,I_main),system) #BodyA = Body('BodyA',A,pAcm,mA,Dyadic.build(A,I_leg,I_leg,I_leg),system) #BodyB = Body('BodyB',B,pBcm,mB,Dyadic.build(B,I_leg,I_leg,I_leg),system) #BodyC = Body('BodyC',C,pCcm,mC,Dyadic.build(C,I_leg,I_leg,I_leg),system) #BodyD = Body('BodyD',D,pDcm,mD,Dyadic.build(D,I_leg,I_leg,I_leg),system) ParticleA = Particle(pAcm,mA,'ParticleA') ParticleB = Particle(pBcm,mB,'ParticleB') ParticleC = Particle(pCcm,mC,'ParticleC') ParticleD = Particle(pDcm,mD,'ParticleD') system.addforce(-b*wOA,wOA) system.addforce(-b*wAB,wAB) system.addforce(-b*wOC,wOC) system.addforce(-b*wCD,wCD) system.addforce(-b*wBD,wBD)
N = Frame('N', system) A = Frame('A', system) system.set_newtonian(N) A.rotate_fixed_axis(N, [0, 0, 1], qA, system) pNA = 0 * N.x pAcm = pNA + lA / 2 * A.x pAtip = pNA + lA * A.x vAcm = pAcm.time_derivative(N, system) wNA = N.get_w_to(A) IA_motor = Dyadic.build(A, Ixx_motor, Iyy_motor, Izz_motor) IA_plate = Dyadic.build(A, Ixx_plate, Iyy_plate, Izz_plate) BodyMotor = Body('BodyMotor', A, pNA, mA, IA_motor) BodyPlate = Body('BodyPlate', A, pAcm, mA, IA_plate) f_aero_C2 = rho * vAcm.length() * (vAcm.dot(A.y)) * Area * A.y system.addforce(-f_aero_C2, vAcm) system.add_spring_force1(k, qA * N.z, wNA) tin = torque * sympy.sin(2 * sympy.pi * freq * system.t) system.addforce(tin * N.z, wNA) f, ma = system.getdynamics() changing_constants = [freq] static_constants = system.constant_values.copy() for key in changing_constants: del static_constants[key]
A = Frame('A',system) B = Frame('B',system) C = Frame('C',system) system.set_newtonian(N) A.rotate_fixed_axis(N,[1,0,0],qA,system) B.rotate_fixed_axis(A,[0,1,0],qB,system) C.rotate_fixed_axis(B,[0,0,1],qC,system) 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},))
# # ### Angular Velocity wA3B3 = A3.get_w_to(B3) wB3C3 = B3.get_w_to(C3) wA3S = A3.get_w_to(S) # ### Define Inertias and Bodies # The next several lines compute the inertia dyadics of each body and define a rigid body on each frame. In the case of frame C, we represent the mass as a particle located at point pCcm. IA = Dyadic.build(A3, Ixx_A, Iyy_A, Izz_A) IB = Dyadic.build(B3, Ixx_B, Iyy_B, Izz_B) IC = Dyadic.build(C3, Ixx_C, Iyy_C, Izz_C) IS = Dyadic.build(S, Ixx_S, Iyy_S, Izz_S) BodyA = Body('BodyA', A3, pAcm, mA, IA, system) BodyB = Body('BodyB', B3, pBcm, mB, IB, system) BodyC = Body('BodyC', C3, pCcm, mC, IC, system) BodyS = Body('BodyS', S, pScm, mS, IS, system) # ## Forces and Torques # Forces and torques are added to the system with the generic ```addforce``` method. The first parameter supplied is a vector describing the force applied at a point or the torque applied along a given rotational axis. The second parameter is the vector describing the linear speed (for an applied force) or the angular velocity(for an applied torque) system.addforce(torque * sympy.sin(freq * 2 * sympy.pi * system.t) * A3.z, wA3S) # ### Spring Forces # # Spring forces are a special case because the energy stored in springs is conservative and should be considered when calculating the system's potential energy. To do this, use the ```add_spring_force``` command. In this method, the first value is the linear spring constant. The second value is the "stretch" vector, indicating the amount of deflection from the neutral point of the spring. The final parameter is, as above, the linear or angluar velocity vector (depending on whether your spring is a linear or torsional spring) # # In this case, the torques applied to each joint are dependent upon whether qA, qB, and qC are absolute or relative rotations, as defined above.
wNA = N.get_w_to(A) wAB = A.get_w_to(B) wAM = A.get_w_to(M) pBcm = pAcm + l * B.x vAcm = pAcm.time_derivative() #wNA = G*wNB #aNA = wNA.time_derivative() #wNB = wB*B.z #aNB = wB_d*B.z I_motor = Dyadic.build(M, Im, Im, Im) I_body = Dyadic.build(A, Ib, Ib, Ib) I_load = Dyadic.build(B, Il, Il, Il) Motor = Body('Motor', M, pAcm, 0, I_motor, system) main_body = Body('main_body', A, pAcm, m_body, I_body, system) Load = Body('Load', B, pBcm, m_pendulum, I_load, system) Inductor = PseudoParticle(0 * Z.x, L, name='Inductor', vCM=i * Z.x, aCM=i_d * Z.x) #Load = Body('Load',B,pO,0,I_load,system,wNBody = wNB,alNBody = aNB) #T = kt*(V/R)-kv*G*qB_d T = kt * i system.addforce(T * A.z, wAM) system.addforce((V - i * R - kv * G * qB_d) * Z.x, i * Z.x)
#constraint1 = pNA-pAN #constraint1_d = vectorderivative(constraint1,N) #constraint1_dd = vectorderivative(constraint1_d,N) # constraint2 = pAB-pBA constraint2_d = constraint2.time_derivative(N,system) constraint2_dd = constraint2_d.time_derivative(N,system) wNA = N.getw_(A) wAB = A.getw_(B) IA = Dyadic.build(A,Ixx_A,Iyy_A,Izz_A) IB = Dyadic.build(A,Ixx_B,Iyy_B,Izz_B) Body('BodyA',A,pAcm,mA,IA,system) Body('BodyB',B,pBcm,mB,IB,system) system.addforce(-b*wNA,wNA) system.addforce(-b*wAB,wAB) system.addforce(-k*qA*N.z,wNA) 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)
wNMA = N.get_w_to(MA) aNMA = wNMA.time_derivative() wNMC = N.get_w_to(MC) aNMC = wNMC.time_derivative() I_motorA = Dyadic.build(MA, Im, Im, Im) I_motorC = Dyadic.build(MC, Im, Im, Im) #BodyO = Body('BodyO',O,pOcm,mO,Dyadic.build(O,I_main,I_main,I_main),system) #BodyA = Body('BodyA',A,pAcm,mA,Dyadic.build(A,I_leg,I_leg,I_leg),system) #BodyB = Body('BodyB',B,pBcm,mB,Dyadic.build(B,I_leg,I_leg,I_leg),system) #BodyC = Body('BodyC',C,pCcm,mC,Dyadic.build(C,I_leg,I_leg,I_leg),system) #BodyD = Body('BodyD',D,pDcm,mD,Dyadic.build(D,I_leg,I_leg,I_leg),system) Particle0 = Particle(pOcm, mO, 'ParticleO') MotorA = Body('MotorA', MA, pOA, m_motor, I_motorA, system) MotorA = Body('MotorC', MC, pOC, m_motor, I_motorC, system) ParticleA1 = Particle(pA1cm, mA / 2, 'ParticleA1') ParticleC1 = Particle(pC1cm, mC / 2, 'ParticleC1') InductorA = PseudoParticle(0 * MA.x, L, name='InductorA', vCM=iA * MA.x, aCM=iA_d * MA.x) InductorC = PseudoParticle(0 * MC.x, L, name='InductorC', vCM=iC * MC.x, aCM=iC_d * MC.x)
vk2 = pk2.time_derivative(N,system) Ixx_A = Constant('Ixx_A',1e-4,system) Iyy_A = Constant('Iyy_A',1e-4,system) Izz_A = Constant('Izz_A',1e-4,system) #Ixx_B = Constant('Ixx_B',6.27600676796613e-07,system) #Iyy_B = Constant('Iyy_B',1.98358014762822e-06,system) #Izz_B = Constant('Izz_B',1.98358014762822e-06,system) #Ixx_C = Constant('Ixx_C',4.39320316677997e-07,system) #Iyy_C = Constant('Iyy_C',7.9239401855911e-07,system) #Izz_C = Constant('Izz_C',7.9239401855911e-07,system) IA = Dyadic.build(A,Ixx_A,Iyy_A,Izz_A) #IB = Dyadic.build(B,Ixx_B,Iyy_B,Izz_B) #IC = Dyadic.build(C,Ixx_C,Iyy_C,Izz_C) BodyA = Body('BodyA',A,pm1,m1,IA,system) #Particle1 = Particle(system,pm1,m1,'Particle1') Particle2 = Particle(system,pm2,m2,'Particle2') s1 = pk1.dot(N.y)*N.y s2 = pk2.dot(N.y)*N.y s3 = (q2-q2_command)*A.z wNA = A.getw_(N) wNB = B.getw_(N) #switch1 = system.add_spring_force(k,s1,vk1) system.add_spring_force(k,s2,vk2) system.add_spring_force(k_controller,s3,wNA) system.add_spring_force(k_controller,-s3,wNB)
def Cal_system(initial_states, drag_direction, tinitial, tstep, tfinal, fit_vel, f1, f2): g_k, g_b_damping, g_b_damping1 = [0.30867935, 1.42946955, 1.08464536] system = System() pynamics.set_system(__name__, system) global_q = True lO = Constant(7 / 1000, 'lO', system) lA = Constant(33 / 1000, 'lA', system) lB = Constant(33 / 1000, 'lB', system) lC = Constant(33 / 1000, 'lC', system) mO = Constant(10 / 1000, 'mA', system) mA = Constant(2.89 / 1000, 'mA', system) mB = Constant(2.89 / 1000, 'mB', system) mC = Constant(2.89 / 1000, 'mC', system) k = Constant(g_k, 'k', system) k1 = Constant(0.4, 'k1', system) friction_perp = Constant(f1, 'f_perp', system) friction_par = Constant(f2, 'f_par', system) b_damping = Constant(g_b_damping, 'b_damping', system) b_damping1 = Constant(g_b_damping1, 'b_damping1', system) preload0 = Constant(0 * pi / 180, 'preload0', system) preload1 = Constant(0 * pi / 180, 'preload1', system) preload2 = Constant(0 * pi / 180, 'preload2', system) preload3 = Constant(0 * pi / 180, 'preload3', system) Ixx_O = Constant(1, 'Ixx_O', system) Iyy_O = Constant(1, 'Iyy_O', system) Izz_O = Constant(1, 'Izz_O', system) Ixx_A = Constant(1, 'Ixx_A', system) Iyy_A = Constant(1, 'Iyy_A', system) Izz_A = Constant(1, 'Izz_A', system) Ixx_B = Constant(1, 'Ixx_B', system) Iyy_B = Constant(1, 'Iyy_B', system) Izz_B = Constant(1, 'Izz_B', system) Ixx_C = Constant(1, 'Ixx_C', system) Iyy_C = Constant(1, 'Iyy_C', system) Izz_C = Constant(1, 'Izz_C', system) y, y_d, y_dd = Differentiable('y', system) qO, qO_d, qO_dd = Differentiable('qO', system) qA, qA_d, qA_dd = Differentiable('qA', system) qB, qB_d, qB_dd = Differentiable('qB', system) qC, qC_d, qC_dd = Differentiable('qC', system) fit_states = initial_states initialvalues = {} initialvalues[y] = fit_states[0] initialvalues[y_d] = fit_states[5] initialvalues[qO] = 0 initialvalues[qO_d] = 0 initialvalues[qA] = fit_states[2] initialvalues[qA_d] = fit_states[7] initialvalues[qB] = fit_states[3] initialvalues[qB_d] = fit_states[8] initialvalues[qC] = fit_states[4] initialvalues[qC_d] = fit_states[9] statevariables = system.get_state_variables() ini = [initialvalues[item] for item in statevariables] N = Frame('N') O = Frame('O') A = Frame('A') B = Frame('B') C = Frame('C') system.set_newtonian(N) if not global_q: O.rotate_fixed_axis_directed(N, [0, 0, 1], qO, system) A.rotate_fixed_axis_directed(O, [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) else: O.rotate_fixed_axis_directed(N, [0, 0, 1], qO, system) A.rotate_fixed_axis_directed(N, [0, 0, 1], qA, system) B.rotate_fixed_axis_directed(N, [0, 0, 1], qB, system) C.rotate_fixed_axis_directed(N, [0, 0, 1], qC, system) pNO = 0 * N.x + y * N.y pOA = lO * N.x + y * N.y pAB = pOA + lA * A.x pBC = pAB + lB * B.x pCtip = pBC + lC * C.x pOcm = pNO + lO / 2 * N.x pAcm = pOA + lA / 2 * A.x pBcm = pAB + lB / 2 * B.x pCcm = pBC + lC / 2 * C.x wNO = N.getw_(O) wOA = N.getw_(A) wAB = A.getw_(B) wBC = B.getw_(C) IO = Dyadic.build(O, Ixx_O, Iyy_O, Izz_O) IA = Dyadic.build(A, Ixx_A, Iyy_A, Izz_A) IB = Dyadic.build(B, Ixx_B, Iyy_B, Izz_B) IC = Dyadic.build(C, Ixx_C, Iyy_C, Izz_C) BodyO = Body('BodyO', O, pOcm, mO, IO, system) BodyA = Body('BodyA', A, pAcm, mA, IA, system) BodyB = Body('BodyB', B, pBcm, mB, IB, system) BodyC = Body('BodyC', C, pCcm, mC, IC, system) # vOcm = pOcm.time_derivative() vAcm = pAcm.time_derivative() vBcm = pBcm.time_derivative() vCcm = pCcm.time_derivative() # system.add_spring_force1(k1+10000*(qA+abs(qA)),(qA-qO-preload1)*N.z,wOA) # system.add_spring_force1(k+10000*(qB+abs(qB)),(qB-qA-preload2)*N.z,wAB) # system.add_spring_force1(k+10000*(qC+abs(qC)),(qC-qB-preload3)*N.z,wBC) system.add_spring_force1(k1, (qA - qO - preload1) * N.z, wOA) system.add_spring_force1(k, (qB - qA - preload2) * N.z, wAB) system.add_spring_force1(k, (qC - qB - preload3) * N.z, wBC) #new Method use nJoint nvAcm = 1 / vAcm.length() * vAcm nvBcm = 1 / vBcm.length() * vBcm nvCcm = 1 / vCcm.length() * vCcm vSoil = drag_direction * 1 * N.y nSoil = 1 / vSoil.length() * vSoil if fit_vel == 0: vSoil = 1 * 1 * N.y nSoil = 1 / vSoil.length() * vSoil faperp = friction_perp * nSoil.dot(A.y) * A.y fapar = friction_par * nSoil.dot(A.x) * A.x system.addforce(-(faperp + fapar), vAcm) fbperp = friction_perp * nSoil.dot(B.y) * B.y fbpar = friction_par * nSoil.dot(B.x) * B.x system.addforce(-(fbperp + fbpar), vBcm) fcperp = friction_perp * nSoil.dot(C.y) * C.y fcpar = friction_par * nSoil.dot(C.x) * C.x system.addforce(-(fcperp + fcpar), vCcm) else: faperp = friction_perp * nvAcm.dot(A.y) * A.y fapar = friction_par * nvAcm.dot(A.x) * A.x system.addforce(-(faperp + fapar), vAcm) fbperp = friction_perp * nvBcm.dot(B.y) * B.y fbpar = friction_par * nvBcm.dot(B.x) * B.x system.addforce(-(fbperp + fbpar), vBcm) fcperp = friction_perp * nvCcm.dot(C.y) * C.y fcpar = friction_par * nvCcm.dot(C.x) * C.x system.addforce(-(fcperp + fcpar), vCcm) system.addforce(-b_damping1 * wOA, wOA) system.addforce(-b_damping * wAB, wAB) system.addforce(-b_damping * wBC, wBC) eq = [] eq_d = [(system.derivative(item)) for item in eq] eq_d.append(y_d - fit_vel) eq_dd = [(system.derivative(item)) for item in eq_d] f, ma = system.getdynamics() func1 = system.state_space_post_invert(f, ma, eq_dd) points = [pNO, pOA, pAB, pBC, pCtip] constants = system.constant_values states = pynamics.integration.integrate_odeint(func1, ini, t, args=({ 'constants': constants }, )) points_output = PointsOutput(points, system, constant_values=constants) y = points_output.calc(states) final = numpy.asarray(states[-1, :]) time1 = time.time() points_output.animate(fps=30, movie_name=str(time1) + 'video_1.mp4', lw=2, marker='o', color=(1, 0, 0, 1), linestyle='-') return final, states, y, system
def Cal_robot(direction, given_l, omega1, t1, t2, ini_states, name1, system, video_on, x1): time_a = time.time() pynamics.set_system(__name__, system) given_k, given_b = x1 global_q = True damping_r = 0 tinitial = 0 tfinal = (t1 - t2) / omega1 tstep = 1 / 30 t = numpy.r_[tinitial:tfinal:tstep] tol_1 = 1e-6 tol_2 = 1e-6 lO = Constant(27.5 / 1000, 'lO', system) lR = Constant(40.5 / 1000, 'lR', system) lA = Constant(given_l / 1000, 'lA', system) lB = Constant(given_l / 1000, 'lB', system) lC = Constant(given_l / 1000, 'lC', system) mO = Constant(154.5 / 1000, 'mO', system) mR = Constant(9.282 / 1000, 'mR', system) mA = Constant(given_l * 2.75 * 0.14450000000000002 / 1000, 'mA', system) mB = Constant(given_l * 2.75 * 0.14450000000000002 / 1000, 'mB', system) mC = Constant(given_l * 2.75 * 0.14450000000000002 / 1000, 'mC', system) k = Constant(given_k, 'k', system) friction_perp = Constant(13 / 3, 'f_perp', system) friction_par = Constant(-2 / 3, 'f_par', system) friction_arm_perp = Constant(5.6, 'fr_perp', system) friction_arm_par = Constant(-0.2, 'fr_par', system) b_damping = Constant(given_b, 'b_damping', system) preload0 = Constant(0 * pi / 180, 'preload0', system) preload1 = Constant(0 * pi / 180, 'preload1', system) preload2 = Constant(0 * pi / 180, 'preload2', system) preload3 = Constant(0 * pi / 180, 'preload3', system) Ixx_O = Constant(1, 'Ixx_O', system) Iyy_O = Constant(1, 'Iyy_O', system) Izz_O = Constant(1, 'Izz_O', system) Ixx_R = Constant(1, 'Ixx_R', system) Iyy_R = Constant(1, 'Iyy_R', system) Izz_R = Constant(1, 'Izz_R', system) Ixx_A = Constant(1, 'Ixx_A', system) Iyy_A = Constant(1, 'Iyy_A', system) Izz_A = Constant(1, 'Izz_A', system) Ixx_B = Constant(1, 'Ixx_B', system) Iyy_B = Constant(1, 'Iyy_B', system) Izz_B = Constant(1, 'Izz_B', system) Ixx_C = Constant(1, 'Ixx_C', system) Iyy_C = Constant(1, 'Iyy_C', system) Izz_C = Constant(1, 'Izz_C', system) y, y_d, y_dd = Differentiable('y', system) qO, qO_d, qO_dd = Differentiable('qO', system) qR, qR_d, qR_dd = Differentiable('qR', system) qA, qA_d, qA_dd = Differentiable('qA', system) qB, qB_d, qB_dd = Differentiable('qB', system) qC, qC_d, qC_dd = Differentiable('qC', system) initialvalues = {} initialvalues[y] = ini_states[0] + tol_1 initialvalues[qO] = ini_states[1] + tol_1 initialvalues[qR] = ini_states[2] + tol_1 initialvalues[qA] = ini_states[3] + tol_1 initialvalues[qB] = ini_states[4] + tol_1 initialvalues[qC] = ini_states[5] + tol_1 initialvalues[y_d] = ini_states[6] + tol_1 initialvalues[qO_d] = ini_states[7] + tol_1 initialvalues[qR_d] = ini_states[8] + tol_1 initialvalues[qA_d] = ini_states[9] + tol_1 initialvalues[qB_d] = ini_states[10] + tol_1 initialvalues[qC_d] = ini_states[11] + tol_1 statevariables = system.get_state_variables() ini = [initialvalues[item] for item in statevariables] N = Frame('N') O = Frame('O') R = Frame('R') A = Frame('A') B = Frame('B') C = Frame('C') system.set_newtonian(N) if not global_q: O.rotate_fixed_axis_directed(N, [0, 0, 1], qO, system) R.rotate_fixed_axis_directed(O, [0, 0, 1], qR, system) A.rotate_fixed_axis_directed(R, [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) else: O.rotate_fixed_axis_directed(N, [0, 0, 1], qO, system) R.rotate_fixed_axis_directed(N, [0, 0, 1], qR, system) A.rotate_fixed_axis_directed(N, [0, 0, 1], qA, system) B.rotate_fixed_axis_directed(N, [0, 0, 1], qB, system) C.rotate_fixed_axis_directed(N, [0, 0, 1], qC, system) pNO = 0 * N.x + y * N.y pOR = pNO + lO * N.x pRA = pOR + lR * R.x pAB = pRA + lA * A.x pBC = pAB + lB * B.x pCtip = pBC + lC * C.x pOcm = pNO + lO / 2 * N.x pRcm = pOR + lR / 2 * R.x pAcm = pRA + lA / 2 * A.x pBcm = pAB + lB / 2 * B.x pCcm = pBC + lC / 2 * C.x wNO = N.getw_(O) wOR = N.getw_(R) wRA = R.getw_(A) wAB = A.getw_(B) wBC = B.getw_(C) IO = Dyadic.build(O, Ixx_O, Iyy_O, Izz_O) IR = Dyadic.build(R, Ixx_R, Iyy_R, Izz_R) IA = Dyadic.build(A, Ixx_A, Iyy_A, Izz_A) IB = Dyadic.build(B, Ixx_B, Iyy_B, Izz_B) IC = Dyadic.build(C, Ixx_C, Iyy_C, Izz_C) BodyO = Body('BodyO', O, pOcm, mO, IO, system) BodyR = Body('BodyR', R, pRcm, mR, IR, system) BodyA = Body('BodyA', A, pAcm, mA, IA, system) BodyB = Body('BodyB', B, pBcm, mB, IB, system) BodyC = Body('BodyC', C, pCcm, mC, IC, system) j_tol = 3 * pi / 180 inv_k = 10 system.add_spring_force1(k + inv_k * (qA - qR + abs(qA - qR - j_tol)), (qA - qR - preload1) * N.z, wRA) system.add_spring_force1(k + inv_k * (qB - qA + abs(qB - qA - j_tol)), (qB - qA - preload2) * N.z, wAB) system.add_spring_force1(k + inv_k * (qC - qB + abs(qC - qB - j_tol)), (qC - qB - preload3) * N.z, wBC) vOcm = y_d * N.y vRcm = pRcm.time_derivative() vAcm = pAcm.time_derivative() vBcm = pBcm.time_derivative() vCcm = pCcm.time_derivative() nvRcm = 1 / (vRcm.length() + tol_1) * vRcm nvAcm = 1 / (vAcm.length() + tol_1) * vAcm nvBcm = 1 / (vBcm.length() + tol_1) * vBcm nvCcm = 1 / (vCcm.length() + tol_1) * vCcm vSoil = -direction * 1 * N.y nSoil = 1 / vSoil.length() * vSoil foperp = 8 * nSoil system.addforce(-foperp, vOcm) frperp = friction_arm_perp * nvRcm.dot(R.y) * R.y frpar = friction_arm_par * nvRcm.dot(R.x) * R.x system.addforce(-(frperp + frpar), vRcm) faperp = friction_perp * nvAcm.dot(A.y) * A.y fapar = friction_par * nvAcm.dot(A.x) * A.x system.addforce(-(faperp + fapar), vAcm) fbperp = friction_perp * nvBcm.dot(B.y) * B.y fbpar = friction_par * nvBcm.dot(B.x) * B.x system.addforce(-(fbperp + fbpar), vBcm) fcperp = friction_perp * nvCcm.dot(C.y) * C.y fcpar = friction_par * nvCcm.dot(C.x) * C.x system.addforce(-(fcperp + fcpar), vCcm) system.addforce(-b_damping * 1 * wRA, wRA) system.addforce(-b_damping * 1 * wAB, wAB) system.addforce(-b_damping * 1 * wBC, wBC) eq = [] eq_d = [(system.derivative(item)) for item in eq] eq_d.append(qR_d - omega1) eq_dd = [(system.derivative(item)) for item in eq_d] f, ma = system.getdynamics() func1 = system.state_space_post_invert(f, ma, eq_dd) points = [pNO, pOR, pRA, pAB, pBC, pCtip] constants = system.constant_values states = pynamics.integration.integrate_odeint(func1, ini, t, args=({ 'constants': constants }, )) final = numpy.asarray(states[-1, :]) logger1 = logging.getLogger('pynamics.system') logger2 = logging.getLogger('pynamics.integration') logger3 = logging.getLogger('pynamics.output') logger1.disabled = True logger2.disabled = True logger3.disabled = True points_output = PointsOutput(points, system, constant_values=constants) y1 = points_output.calc(states) if video_on == 1: plt.figure() plt.plot(*(y1[::int(len(y1) / 20)].T) * 1000) plt.axis('equal') plt.axis('equal') plt.title("Plate Configuration vs Distance") plt.xlabel("Configuration") plt.ylabel("Distance (mm)") plt.figure() plt.plot(t, numpy.rad2deg(states[:, 2])) plt.plot(t, numpy.rad2deg(states[:, 8])) plt.legend(["qR", "qR_d"]) plt.hlines(numpy.rad2deg(t1), tinitial, tfinal) plt.hlines(numpy.rad2deg(t2), tinitial, tfinal) plt.title("Robot Arm angle and velocitues (qR and qR_d) over Time") plt.xlabel("Time (s)") plt.ylabel("Angles,Velocities (deg, deg/s)") plt.figure() q_states = numpy.c_[(states[:, 2], states[:, 3], states[:, 4], states[:, 5])] plt.plot(t, numpy.rad2deg(q_states)) plt.title("Joint Angule over Time") plt.xlabel("Time (s)") plt.ylabel("Joint Angles (deg)") plt.legend(["Arm", "Joint 1", "Joint 2", "Joint 3"]) plt.figure() qd_states = numpy.c_[(states[:, 8], states[:, 9], states[:, 10], states[:, 11])] plt.plot(t, numpy.rad2deg(qd_states)) plt.legend(["qR_d", "qA_d", "qB_d", "qC_d"]) plt.title("Joint Angular Velocities over Time") plt.xlabel("Time (s)") plt.ylabel("Joint Angular Velocities (deg/s)") plt.legend(["Arm", "Joint 1", "Joint 2", "Joint 3"]) plt.figure() plt.plot(t, states[:, 0], '--') plt.plot(t, states[:, 6]) plt.title("Robot Distance and Velocity over time") plt.xlabel("Time (s)") plt.ylabel("Distance (mm)") plt.legend(["Distance", "Velocity of the robot"]) points_output.animate(fps=1 / tstep, movie_name=name1, lw=2, marker='o', color=(1, 0, 0, 1), linestyle='-') else: pass return final, states, y1
system.set_newtonian(N) A.rotate_fixed_axis_directed(N,[0,0,1],qA,system) B.rotate_fixed_axis_directed(N,[0,0,1],qB,system) pO = 0*N.x wNA = N.getw_(A) wNB = N.getw_(B) #wNA = G*wNB #aNA = wNA.time_derivative() #wNB = wB*B.z #aNB = wB_d*B.z I_motor = Dyadic.build(A,Im,Im,Im) I_load = Dyadic.build(B,Il,Il,Il) Motor = Body('Motor',A,pO,0,I_motor,system) #Motor = Body('Motor',B,pO,0,I_motor,system,wNBody = wNA,alNBody = aNA) Inductor = PseudoParticle(0*M.x,L,name='Inductor',vCM = i*M.x,aCM = i_d*M.x) #Load = Body('Load',B,pO,0,I_load,system,wNBody = wNB,alNBody = aNB) Load = Body('Load',B,pO,m,I_load,system) #T = kt*(V/R)-kv*G*qB_d T = kt*i system.addforce(T*N.z,wNA) system.addforce(-b*wNA,wNA) system.addforce(-Tl*B.z,wNB) system.addforce((V-i*R - kv*G*qB_d)*M.x,i*M.x) eq_d = [] eq_d = [wNA.dot(N.z) - G*wNB.dot(N.z)] #eq_d = [N.getw_(A).dot(N.z) - G*N.getw_(B).dot(N.z)]
#A.rotate_fixed_axis(N,[0,0,1],qA,system) B.rotate_fixed_axis(N, [0, 0, 1], qB, system) pO = 0 * N.x #wNA = N.get_w_to(A) wNB = N.get_w_to(B) wNA = G * wNB aNA = wNA.time_derivative() #wNB = wB*B.z #aNB = wB_d*B.z I_motor = Dyadic.build(B, Im, Im, Im) I_load = Dyadic.build(B, Il, Il, Il) #Motor = Body('Motor',A,pO,0,I_motor,system) Motor = Body('Motor', B, pO, 0, I_motor, system, wNBody=wNA, alNBody=aNA) Inductor = PseudoParticle(0 * M.x, L, name='Inductor', vCM=i * M.x, aCM=i_d * M.x) #Load = Body('Load',B,pO,0,I_load,system,wNBody = wNB,alNBody = aNB) Load = Body('Load', B, pO, m, I_load, system) #T = kt*(V/R)-kv*G*qB_d T = kt * i system.addforce(T * N.z, wNA) system.addforce(-b * wNA, wNA) system.addforce(-Tl * B.z, wNB) system.addforce((V - i * R - kv * G * qB_d) * M.x, i * M.x)
def init_system(v, drag_direction, time_step): import pynamics from pynamics.frame import Frame from pynamics.variable_types import Differentiable, Constant from pynamics.system import System from pynamics.body import Body from pynamics.dyadic import Dyadic from pynamics.output import Output, PointsOutput from pynamics.particle import Particle import pynamics.integration import logging import sympy import numpy import matplotlib.pyplot as plt from math import pi from scipy import optimize from sympy import sin import pynamics.tanh as tanh from fit_qs import exp_fit import fit_qs # time_step = tstep x = numpy.zeros((7, 1)) friction_perp = x[0] friction_par = x[1] given_b = x[2] given_k = x[3] given_k1 = x[4] given_b1 = x[4] system = System() pynamics.set_system(__name__, system) global_q = True lO = Constant(7 / 1000, 'lO', system) lA = Constant(33 / 1000, 'lA', system) lB = Constant(33 / 1000, 'lB', system) lC = Constant(33 / 1000, 'lC', system) mO = Constant(10 / 1000, 'mA', system) mA = Constant(2.89 / 1000, 'mA', system) mB = Constant(2.89 / 1000, 'mB', system) mC = Constant(2.89 / 1000, 'mC', system) k = Constant(0.209, 'k', system) k1 = Constant(0.209, 'k1', system) friction_perp = Constant(1.2, 'f_perp', system) friction_par = Constant(-0.2, 'f_par', system) b_damping = Constant(given_b, 'b_damping', system) # time_step = 1/00 if v == 0: [ t, tinitial, tfinal, tstep, qAa1, qAb1, qAc1, qAa2, qAb2, qAc2, qAa3, qAb3, qAc3, qBa1, qBb1, qBc1, qBa2, qBb2, qBc2, qBa3, qBb3, qBc3, qCa1, qCb1, qCc1, qCa2, qCb2, qCc2, qCa3, qCb3, qCc3 ] = fit_qs.fit_0_amount(time_step) elif v == 10: [ t, tinitial, tfinal, tstep, qAa1, qAb1, qAc1, qAa2, qAb2, qAc2, qAa3, qAb3, qAc3, qBa1, qBb1, qBc1, qBa2, qBb2, qBc2, qBa3, qBb3, qBc3, qCa1, qCb1, qCc1, qCa2, qCb2, qCc2, qCa3, qCb3, qCc3 ] = fit_qs.fit_10_amount(time_step) elif v == 20: [ t, tinitial, tfinal, tstep, qAa1, qAb1, qAc1, qAa2, qAb2, qAc2, qAa3, qAb3, qAc3, qBa1, qBb1, qBc1, qBa2, qBb2, qBc2, qBa3, qBb3, qBc3, qCa1, qCb1, qCc1, qCa2, qCb2, qCc2, qCa3, qCb3, qCc3 ] = fit_qs.fit_20_amount(time_step) elif v == 30: [ t, tinitial, tfinal, tstep, qAa1, qAb1, qAc1, qAa2, qAb2, qAc2, qAa3, qAb3, qAc3, qBa1, qBb1, qBc1, qBa2, qBb2, qBc2, qBa3, qBb3, qBc3, qCa1, qCb1, qCc1, qCa2, qCb2, qCc2, qCa3, qCb3, qCc3 ] = fit_qs.fit_30_amount(time_step) elif v == 40: [ t, tinitial, tfinal, tstep, qAa1, qAb1, qAc1, qAa2, qAb2, qAc2, qAa3, qAb3, qAc3, qBa1, qBb1, qBc1, qBa2, qBb2, qBc2, qBa3, qBb3, qBc3, qCa1, qCb1, qCc1, qCa2, qCb2, qCc2, qCa3, qCb3, qCc3 ] = fit_qs.fit_40_amount(time_step) elif v == 50: [ t, tinitial, tfinal, tstep, qAa1, qAb1, qAc1, qAa2, qAb2, qAc2, qAa3, qAb3, qAc3, qBa1, qBb1, qBc1, qBa2, qBb2, qBc2, qBa3, qBb3, qBc3, qCa1, qCb1, qCc1, qCa2, qCb2, qCc2, qCa3, qCb3, qCc3 ] = fit_qs.fit_50_amount(time_step) distance = 200 / 1000 nums = int(tfinal / tstep) array_num = numpy.arange(0, nums) array_num1 = numpy.repeat(array_num, nums, axis=0) array_num1.shape = (nums, nums) error_k = array_num1 / 8000 + numpy.ones((nums, nums)) fit_t = t fit_qA = exp_fit(fit_t, qAa1, qAb1, qAc1, qAa2, qAb2, qAc2, qAa3, qAb3, qAc3) fit_qB = exp_fit(fit_t, qBa1, qBb1, qBc1, qBa2, qBb2, qBc2, qBa3, qBb3, qBc3) fit_qC = exp_fit(fit_t, qCa1, qCb1, qCc1, qCa2, qCb2, qCc2, qCa3, qCb3, qCc3) fit_qAd1 = numpy.diff(fit_qA) / numpy.diff(fit_t) fit_qAd = numpy.append(fit_qAd1[0], fit_qAd1) fit_qBd1 = numpy.diff(fit_qB) / numpy.diff(fit_t) fit_qBd = numpy.append(fit_qBd1[0], fit_qBd1) fit_qCd1 = numpy.diff(fit_qC) / numpy.diff(fit_t) fit_qCd = numpy.append(fit_qCd1[0], fit_qCd1) fit_states1 = numpy.stack( (fit_qA, fit_qB, fit_qC, fit_qAd, fit_qBd, fit_qCd), axis=1) fit_states1[:, 0:3] = fit_states1[:, 0:3] - fit_states1[0, 0:3] fit_states = -drag_direction * numpy.deg2rad(fit_states1) # plt.plot(t,fit_states) if drag_direction == -1: zero_shape = fit_states.shape fit_states = numpy.zeros(zero_shape) fit_vel = drag_direction * distance / (tfinal) if qAa1 == 0: fit_vel = 0 fit_v = numpy.ones(t.shape) * fit_vel if qAa1 == 0: fit_d = numpy.ones(t.shape) * fit_vel else: fit_d = drag_direction * numpy.r_[tinitial:distance:tstep * abs(fit_vel)] preload0 = Constant(0 * pi / 180, 'preload0', system) preload1 = Constant(0 * pi / 180, 'preload1', system) preload2 = Constant(0 * pi / 180, 'preload2', system) preload3 = Constant(0 * pi / 180, 'preload3', system) Ixx_O = Constant(1, 'Ixx_O', system) Iyy_O = Constant(1, 'Iyy_O', system) Izz_O = Constant(1, 'Izz_O', system) Ixx_A = Constant(1, 'Ixx_A', system) Iyy_A = Constant(1, 'Iyy_A', system) Izz_A = Constant(1, 'Izz_A', system) Ixx_B = Constant(1, 'Ixx_B', system) Iyy_B = Constant(1, 'Iyy_B', system) Izz_B = Constant(1, 'Izz_B', system) Ixx_C = Constant(1, 'Ixx_C', system) Iyy_C = Constant(1, 'Iyy_C', system) Izz_C = Constant(1, 'Izz_C', system) y, y_d, y_dd = Differentiable('y', system) qO, qO_d, qO_dd = Differentiable('qO', system) qA, qA_d, qA_dd = Differentiable('qA', system) qB, qB_d, qB_dd = Differentiable('qB', system) qC, qC_d, qC_dd = Differentiable('qC', system) initialvalues = {} initialvalues[y] = 0 + 1e-14 initialvalues[y_d] = fit_vel + 1e-14 initialvalues[qO] = 0 + 1e-14 initialvalues[qO_d] = 0 + 1e-14 initialvalues[qA] = fit_states[0, 0] + 1e-14 initialvalues[qA_d] = fit_states[0, 3] + 1e-14 initialvalues[qB] = fit_states[0, 1] + 1e-14 initialvalues[qB_d] = fit_states[0, 4] + 1e-14 initialvalues[qC] = fit_states[0, 2] + 1e-14 initialvalues[qC_d] = fit_states[0, 5] + 1e-14 statevariables = system.get_state_variables() ini = [initialvalues[item] for item in statevariables] N = Frame('N') O = Frame('O') A = Frame('A') B = Frame('B') C = Frame('C') drag_direction = drag_direction velocity = 200 / tfinal / 1000 vSoil = drag_direction * velocity * N.y nSoil = 1 / vSoil.length() * vSoil system.set_newtonian(N) if not global_q: O.rotate_fixed_axis_directed(N, [0, 0, 1], qO, system) A.rotate_fixed_axis_directed(O, [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) else: O.rotate_fixed_axis_directed(N, [0, 0, 1], qO, system) A.rotate_fixed_axis_directed(N, [0, 0, 1], qA, system) B.rotate_fixed_axis_directed(N, [0, 0, 1], qB, system) C.rotate_fixed_axis_directed(N, [0, 0, 1], qC, system) pNO = 0 * N.x + y * N.y pOA = lO * N.x + y * N.y pAB = pOA + lA * A.x pBC = pAB + lB * B.x pCtip = pBC + lC * C.x pOcm = pNO + lO / 2 * N.x pAcm = pOA + lA / 2 * A.x pBcm = pAB + lB / 2 * B.x pCcm = pBC + lC / 2 * C.x wNO = N.getw_(O) wOA = N.getw_(A) wAB = A.getw_(B) wBC = B.getw_(C) IO = Dyadic.build(O, Ixx_O, Iyy_O, Izz_O) IA = Dyadic.build(A, Ixx_A, Iyy_A, Izz_A) IB = Dyadic.build(B, Ixx_B, Iyy_B, Izz_B) IC = Dyadic.build(C, Ixx_C, Iyy_C, Izz_C) BodyO = Body('BodyO', O, pOcm, mO, IO, system) BodyA = Body('BodyA', A, pAcm, mA, IA, system) BodyB = Body('BodyB', B, pBcm, mB, IB, system) BodyC = Body('BodyC', C, pCcm, mC, IC, system) # BodyC = Particle(pCcm,mC,'ParticleC',system) vOcm = pOcm.time_derivative() vAcm = pAcm.time_derivative() vBcm = pBcm.time_derivative() vCcm = pCcm.time_derivative() system.add_spring_force1(k1 + 10000 * (qA + abs(qA)), (qA - qO - preload1) * N.z, wOA) system.add_spring_force1(k + 10000 * (qB + abs(qB)), (qB - qA - preload2) * N.z, wAB) system.add_spring_force1(k + 10000 * (qC + abs(qC)), (qC - qB - preload3) * N.z, wBC) #new Method use nJoint nvAcm = 1 / vAcm.length() * vAcm nvBcm = 1 / vBcm.length() * vBcm nvCcm = 1 / vCcm.length() * vCcm faperp = friction_perp * nvAcm.dot(A.y) * A.y fapar = friction_par * nvAcm.dot(A.x) * A.x system.addforce(-(faperp + fapar), vAcm) fbperp = friction_perp * nvBcm.dot(B.y) * B.y fbpar = friction_par * nvBcm.dot(B.x) * B.x system.addforce(-(fbperp + fbpar), vBcm) fcperp = friction_perp * nvCcm.dot(C.y) * C.y fcpar = friction_par * nvCcm.dot(C.x) * C.x system.addforce(-(fcperp + fcpar), vCcm) system.addforce(-b_damping * wOA, wOA) system.addforce(-b_damping * wAB, wAB) system.addforce(-b_damping * wBC, wBC) eq = [] eq_d = [(system.derivative(item)) for item in eq] eq_d.append(y_d - fit_vel) eq_dd = [(system.derivative(item)) for item in eq_d] f, ma = system.getdynamics() func1 = system.state_space_post_invert(f, ma, eq_dd) points = [pNO, pOA, pAB, pBC, pCtip] constants = system.constant_values return system, f, ma, func1, points, t, ini, constants, b_damping, k, k1, tstep, fit_states
pEcm = pBtip - .1 * E.y pE1 = pEcm + lE / 2 * E.x vE1 = pE1.time_derivative(N, system) pE2 = pEcm - lE / 2 * E.x vE2 = pE2.time_derivative(N, system) wOA = O.getw_(A) wAB = A.getw_(B) wOC = O.getw_(C) wCD = C.getw_(D) wBD = B.getw_(D) wOE = O.getw_(E) BodyO = Body('BodyO', O, pOcm, mO, Dyadic.build(O, I_main, I_main, I_main), system) #BodyA = Body('BodyA',A,pAcm,mA,Dyadic.build(A,I_leg,I_leg,I_leg),system) #BodyB = Body('BodyB',B,pBcm,mB,Dyadic.build(B,I_leg,I_leg,I_leg),system) #BodyC = Body('BodyC',C,pCcm,mC,Dyadic.build(C,I_leg,I_leg,I_leg),system) #BodyD = Body('BodyD',D,pDcm,mD,Dyadic.build(D,I_leg,I_leg,I_leg),system) BodyE = Body('BodyE', E, pEcm, mE, Dyadic.build(D, I_leg, I_leg, I_leg), system) ParticleA = Particle(pAcm, mA, 'ParticleA') ParticleB = Particle(pBcm, mB, 'ParticleB') ParticleC = Particle(pCcm, mC, 'ParticleC') ParticleD = Particle(pDcm, mD, 'ParticleD') #ParticleE = Particle(pEcm,mE,'ParticleE') system.addforce(-b * wOA, wOA) system.addforce(-b * wAB, wAB)
#f5 = Frame(,system) system.set_newtonian(f1) f2.rotate_fixed_axis(f1,[0,0,1],q1,system) f3.rotate_fixed_axis(f2,[1,0,0],q2,system) f4.rotate_fixed_axis(f3,[0,1,0],q3,system) p0 = 0*f1.x p1 = p0-l1*f4.x v1=p1.time_derivative(f1) wNA = f1.get_w_to(f2) 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)
# ### Vector derivatives # The time derivatives of vectors may also be # vCtip = pCtip.time_derivative(N,system) # ### Define Inertias and Bodies # The next several lines compute the inertia dyadics of each body and define a rigid body on each frame. In the case of frame C, we represent the mass as a particle located at point pCcm. # In[17]: IA = Dyadic.build(A, Ixx_A, Iyy_A, Izz_A) IB = Dyadic.build(B, Ixx_B, Iyy_B, Izz_B) IC = Dyadic.build(B, Ixx_C, Iyy_C, Izz_C) BodyA = Body('BodyA', A, pAcm, mA, IA, system) BodyB = Body('BodyB', B, pBcm, mB, IB, system) BodyC = Body('BodyC', C, pCcm, mC, IC, system) ParticleM = Particle(pm1, m1, 'ParticleM', system) # ## Forces and Torques # Forces and torques are added to the system with the generic ```addforce``` method. The first parameter supplied is a vector describing the force applied at a point or the torque applied along a given rotational axis. The second parameter is the vector describing the linear speed (for an applied force) or the angular velocity(for an applied torque) # In[18]: stretch1 = -pC1.dot(N.y) stretch1_s = (stretch1 + abs(stretch1)) on = stretch1_s / (2 * stretch1 + 1e-10) system.add_spring_force1(k_constraint, -stretch1_s * N.y, vC1) system.addforce(-b_constraint * vC1 * on, vC1)
N = Frame('N') A = Frame('A') system.set_newtonian(N) A.rotate_fixed_axis_directed(N, [0, 0, 1], q, system) pNA = 0 * N.x pAB = pNA - x * A.y vNA = pNA.time_derivative(N, system) vAB = pAB.time_derivative(N, system) aAB = vAB.time_derivative(N, system) #ParticleA = Particle(pAB,mA,'ParticleA',system) IA = Dyadic.build(A, Ixx_A, Iyy_A, Izz_A) BodyA = Body('BodyA', A, pAB, mA, IA, system) stretch = x - lA direction = -A.y #system.addforce(-b*vAB,vAB) system.addforce(-k * stretch * A.y, vNA) system.addforce(k * stretch * A.y, vAB) #system.add_springforce(k*stretch*A.y,vAB) #system.addforce(b*x_d*A.y,vAB) system.addforcegravity(-g * N.y) x1 = BodyA.pCM.dot(N.x) y1 = BodyA.pCM.dot(N.y) f, ma = system.getdynamics() func = system.state_space_post_invert(f, ma)
A = Frame('A', system) system.set_newtonian(N) A.rotate_fixed_axis(N, [0, 0, 1], q, system) p1 = x * N.x p2 = p1 - l * A.y wNA = N.get_w_to(A) v1 = p1.time_derivative(N, system) v2 = p2.time_derivative(N, system) I = Dyadic.build(A, I_xx, I_yy, I_zz) BodyA = Body('BodyA', A, p2, m, I, system) ParticleO = Particle(p2, M, 'ParticleO', system) stretch = q - q0 system.add_spring_force1(k, (stretch) * N.z, wNA) system.addforce(-b * v2, v2) system.addforcegravity(-g * N.y) pos = sympy.cos(system.t * 2 * pi / 2) eq = pos * N.x - p1 eq_d = eq.time_derivative() eq_dd = eq_d.time_derivative() eq_dd_scalar = [] eq_dd_scalar.append(eq_dd.dot(N.x)) system.add_constraint(AccelerationConstraint(eq_dd_scalar))
statevariables = [qA, qA_d] ini = [initialvalues[item] for item in statevariables] N = Frame('N') A = Frame('A') system.set_newtonian(N) A.rotate_fixed_axis_directed(N, [0, 0, 1], qA, system) pNA = 0 * N.x pAB = pNA + lA * A.x pAcm = pNA + lA / 2 * A.x wNA = N.getw_(A) BodyA = Body('BodyA', A, pAcm, mA, Dyadic.build(A, Ixx_A, Iyy_A, Izz_A), system) system.addforce(-b * wNA, wNA) system.addforce(-k * qA * N.z, wNA) system.addforcegravity(-g * N.y) 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),