# BodyA = Particle(pAcm,mA,'ParticleA',system) # BodyB = Particle(pBcm,mB,'ParticleB',system) # BodyC = Particle(pCcm,mC,'ParticleC',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) """# 3. Add Forces: Add the acceleration due to gravity. Add rotational springs in the joints (using k=0 is ok for now) and a damper to at least one rotational joint. You do not need to add external motor/spring forces but you should start planning to collect that data. A damper is added at the input joint (pNA). Other joints are assumed to be fully free (no damping, and no spring). Note that although we have damping and spring formulation in the code the values (b and k) are set to zero in the previous code. """ system.addforce(-b*wNA,wNA) # Spring force if not global_q: system.add_spring_force1(k,(qA-preload1)*N.z,wNA) #system.add_spring_force1(k,(qB-preload2)*A.z,wAB) #system.add_spring_force1(k,(qC-preload3)*B.z,wBC) else: system.add_spring_force1(k,(qA-preload1)*N.z,wNA) #system.add_spring_force1(k,(qB-qA-preload2)*N.z,wAB) #system.add_spring_force1(k,(qC-qB-preload3)*N.z,wBC) # Gravity force system.addforcegravity(-g*N.y) """# 4. Constraints: Keep mechanism constraints in, but follow the pendulum example of double-differentiating all constraint equations. --- If you defined your mechanism as unattached to the Newtonian frame, add enough constraints so that it is fully attached to ground (for now). you will be eventually removing these constraints.
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) y1 = ParticleA.pCM.dot(N.y) KE = system.KE PE = system.getPEGravity(pNA) - system.getPESprings() v = pAB - pNA u = (v.dot(v))**.5 eq1 = [(v.dot(v)) - lA**2] eq1_d = [system.derivative(item) for item in eq1] eq1_dd = [system.derivative(system.derivative(item)) for item in eq1] eq = eq1_dd
statevariables = system.get_state_variables() ini = [initialvalues[item] for item in statevariables] N = Frame('N') system.set_newtonian(N) pNA=0*N.x pAcm=pNA+x*N.x+y*N.y vAcm = pAcm.time_derivative(N,system) ParticleA = Particle(pAcm,mA,'ParticleA',system) system.addforce(-b*vAcm,vAcm) stretch = y stretched1 = (stretch+abs(stretch))/2 stretched2 = -(-stretch+abs(-stretch))/2 #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)
#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') ParticleA1 = Particle(pA1cm, mA / 2, 'ParticleA1') ParticleB1 = Particle(pB1cm, mB / 2, 'ParticleB1') ParticleC1 = Particle(pC1cm, mC / 2, 'ParticleC1') ParticleD1 = Particle(pD1cm, mD / 2, 'ParticleD1') ParticleA2 = Particle(pA2cm, mA / 2, 'ParticleA2') ParticleB2 = Particle(pB2cm, mB / 2, 'ParticleB2') ParticleC2 = Particle(pC2cm, mC / 2, 'ParticleC2') ParticleD2 = Particle(pD2cm, mD / 2, 'ParticleD2') system.addforce(-b_joint * wOA1, wOA1) system.addforce(-b_joint * wA2B1, wA2B1) system.addforce(-b_joint * wOC1, wOC1) system.addforce(-b_joint * wC2D1, wC2D1) system.addforce(-b_joint * wB2D2, wB2D2) #system.addforce(-b_beam*wA1A2,wA1A2) #system.addforce(-b_beam*wB1B2,wB1B2) #system.addforce(-b_beam*wC1C2,wC1C2) #system.addforce(-b_beam*wD1D2,wD1D2) # stretch = -pBtip.dot(N.y) stretch_s = (stretch + abs(stretch)) on = stretch_s / (2 * stretch + 1e-5) system.add_spring_force1(k_constraint, -stretch_s * N.y, vBtip)
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) system.addforce(-b * wOC, wOC) system.addforce(-b * wCD, wCD) system.addforce(-b_ankle * wOE, wOE) # stretch1 = -pE1.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, vE1) system.addforce(-b_constraint * vE1 * on, vE1) toeforce = k_constraint * -stretch1_s stretch2 = -pE2.dot(N.y) stretch2_s = (stretch2 + abs(stretch2))
vpm1 = pm1.time_derivative(N,system) vpm2 = pm2.time_derivative(N,system) l_ = pm1-pm2 l = (l_.dot(l_))**.5 l_d =system.derivative(l) stretch = l - l0 ul_ = l_*((l+error_tol)**-1) vl = l_.time_derivative(N,system) system.add_spring_force1(k,stretch*ul_,vl) #system.addforce(-k*stretch*ul_,vpm1) #system.addforce(k*stretch*ul_,vpm2) system.addforce(-b*l_d*ul_,vpm1) system.addforce(b*l_d*ul_,vpm2) #system.addforce(k*l*ul_,vpm2) #system.addforce(-b*vl,vl) #system.addforce(-b*vl,vl) #system.addforce(-b*vl,vl) system.addforcegravity(-g*N.y) #system.addforcegravity(-g*N.y) #system.addforcegravity(-g*N.y)
#BodyA = Body('BodyA',A,pm1,m1,IA,system) Particle1 = Particle(pm1, m1, 'Particle1', system) Particle2 = Particle(pm2, m2, 'Particle2', system) vpm1 = pm1.time_derivative(N, system) vpm2 = pm2.time_derivative(N, system) #l_ = pm1-pm2 #l = (l_.dot(l_))**.5 #l_d =system.derivative(l) #stretch = l1 #ul_ = l_*((l+error_tol)**-1) #vl = l_.time_derivative(N,system) #system.add_spring_force1(k,stretch*ul_,vpm) system.addforce(k * l1 * A.y, vpm1) system.addforce(-k * l1 * A.y, vpm2) system.addforce(b * l1_d * A.y, vpm1) system.addforce(-b * l1_d * A.y, vpm2) #system.addforce(k*l*ul_,vpm2) #system.addforce(-b*vl,vl) #system.addforce(-b*vl,vl) #system.addforce(-b*vl,vl) system.addforcegravity(-g * N.y) #system.addforcegravity(-g*N.y) #system.addforcegravity(-g*N.y)
pBcm = pAB + lB / 2 * B.x #pCcm=pBC+lC/2*C.x wNA = N.getw_(A) wAB = A.getw_(B) #wBC = B.getw_(C) 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) Body('BodyA', A, pAcm, mA, IA, system) Body('BodyB', B, pBcm, mB, IB, system) #Body('BodyC',C,pCcm,mC,IC,system) system.addforce(-b * wNA, wNA) system.addforce(-b * wAB, wAB) #system.addforce(-b*wBC,wBC) system.addforce(-k * (qA - preload1) * N.z, wNA) system.addforce(-k * (qB - preload2) * A.z, wAB) #system.addforce(-k*(qC-preload3)*B.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)
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) system.addforce(-b*vm1,vm1) system.addforcegravity(-g*N.y) #system.addforcegravity(-g*N.y) #system.addforcegravity(-g*N.y) x1 = BodyA.pCM.dot(N.x) y1 = BodyA.pCM.dot(N.y) x2 = Particle2.pCM.dot(N.x) y2 = Particle2.pCM.dot(N.y) KE = system.KE PE = system.getPEGravity(pNA) - system.getPESprings() pynamics.tic()
pAB = pNA + lA * A.x vAB = pAB.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) wNA = N.get_w_to(A) lab2 = vAB.dot(vAB) uab = vAB * (1 / (lab2**.5 + tol)) #squared term #system.addforce(-b_air*lab2*uab,vAB) #linear term system.addforce(-b_air * vAB, vAB) system.addforce(-b_joint * wNA, wNA) system.addforcegravity(-g * N.y) system.add_spring_force1(k, (qA - preload1) * N.z, wNA) #x1 = ParticleA.pCM.dot(N.x) #y1 = ParticleA.pCM.dot(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) states = pynamics.integration.integrate_odeint(func, ini, t, rtol=1e-12,
pNA = 0 * N.x pm1 = x1 * N.x + y1 * N.y pm2 = pm1 + l0 * A.y Particle1 = Particle(pm1, m1, 'Particle1', system) Particle2 = Particle(pm2, m2, 'Particle2', system) vpm1 = pm1.time_derivative(N, system) vpm2 = pm2.time_derivative(N, system) system.addforcegravity(-g * N.y) y2 = pm2.dot(N.y) f_floor2 = (y2**2)**.5 - y2 system.addforce(k_constraint * f_floor2 * N.y, vpm2) system.addforce(-b_constraint * f_floor2 * vpm2, vpm2) f_floor1 = (y1**2)**.5 - y1 system.addforce(k_constraint * f_floor1 * N.y, vpm1) system.addforce(-b_constraint * f_floor1 * vpm1, vpm1) eq = [] f, ma = system.getdynamics() func = system.state_space_post_invert(f, ma) constants = system.constant_values.copy() # constants[b_constraint]=0 states = pynamics.integration.integrate_odeint(func, ini,
pNA = 0 * N.x pAB = pNA + lA * A.x pAcm = pNA + lA / 2 * A.x vAB = pAB.time_derivative(N, system) # pNA2=2*N.x # pAB2=pNA2+x*N.x+y*N.y # vAB2=pAB2.time_derivative(N,system) # ParticleA = Particle(pAB,mA,'ParticleA',system) BodyA = Body('BodyA', A, pAcm, mA, IA, system) # ParticleB = Particle(pAB2,mA,'ParticleB',system) # system.addforce(t*vAB,vAB) system.addforce(torque * N.z, wNA) # system.addforce(-b*vAB2,vAB2) system.addforcegravity(-g * N.y) # v = pAB2-pNA2 # u = (v.dot(v))**.5 # eq1 = [(v.dot(v)) - lA**2] eq = [] f, ma = system.getdynamics() func = system.state_space_post_invert(f, ma, eq) states = pynamics.integration.integrate_odeint(func, ini, t, rtol=1e-12,
name='InductorC', vCM=iC * MC.x, aCM=iC_d * MC.x) ParticleA2 = Particle(pA2cm, mA / 2, 'ParticleA2') ParticleC2 = Particle(pC2cm, mC / 2, 'ParticleC2') ParticleB = Particle(pBcm, mB, 'ParticleB') ParticleD = Particle(pDcm, mD, 'ParticleD') #system.addforce(-b_joint*wOA1,wOA1) #system.addforce(-b_joint*wA2B1,wA2B1) #system.addforce(-b_joint*wOC1,wOC1) #system.addforce(-b_joint*wC2D1,wC2D1) #system.addforce(-b_joint*wB2D2,wB2D2) #system.addforce(-b*wOA,wOA) system.addforce(-b_joint * wA2B, wA2B) #system.addforce(-b*wOC,wOC) system.addforce(-b_joint * wC2D, wC2D) system.addforce(-b_joint * wBD, wBD) # system.addforce(-b_beam * wA1A2, wA1A2) #system.addforce(-b_beam*wB1B2,wB1B2) system.addforce(-b_beam * wC1C2, wC1C2) #system.addforce(-b_beam*wD1D2,wD1D2) stretch = -pBtip.dot(N.y) stretch_s = (stretch + abs(stretch)) on = stretch_s / (2 * stretch + 1e-5) system.add_spring_force1(k_constraint, -stretch_s * N.y, vBtip) system.addforce(-b_constraint * vBtip * on, vBtip)
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
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] func = system.state_space_post_invert(f, ma, constants=static_constants) statevariables = system.get_state_variables()
ParticleA2 = Particle(A2.x + A23.x, m, 'ParticleA2', system) # ParticleA3 = Particle(A3.x+A34.x,m/2,'ParticleA3',system) ParticleB1 = Particle(B1.x + B12.x, m, 'ParticleB1', system) ParticleB2 = Particle(B2.x + B23.x, m, 'ParticleB2', system) ################################################ #Get the relative rotational velocity between frames wA1 = N.get_w_to(A1) wA2 = A12.get_w_to(A2) wA3 = A23.get_w_to(A3) wB1 = NB1.get_w_to(B1) wB2 = B12.get_w_to(B2) ################################################ #Add damping between joints system.addforce(-b * wA1, wA1) system.addforce(-b * wA2, wA2) system.addforce(-b * wA3, wA3) system.addforce(-b * wB1, wB1) system.addforce(-b * wB2, wB2) #system.addforce(1*A1.x,wA1) ################################################ #Add spring forces to two joints system.add_spring_force1(k, (qA1 - pi / 180 * 45) * A1.x, wA1) system.add_spring_force1(k, (qA2) * A2.x, wA2) system.add_spring_force1(k, (qA3) * A3.x, wA3) system.add_spring_force1(k, (qB1 + pi / 180 * 45) * B1.x, wB1) system.add_spring_force1(k, (qB2) * B2.x, wB2)
vex = ve.dot(E.x) vey = ve.dot(-E.y) angle_of_attack_E = sympy.atan2(vey, vex) #cl = 2*sin(angle_of_attack)*cos(angle_of_attack) #cd = 2*sin(angle_of_attack)**2 #fl = .5*rho*vcp2*cl*A #fd = .5*rho*vcp2*cd*A f_aero_C = rho * vcp2 * sympy.sin(angle_of_attack_C) * Sw * C.y f_aero_E = rho * ve2 * sympy.sin(angle_of_attack_E) * Sw * E.y system.addforcegravity(-g * N.y) system.addforce(f_aero_C, vcp) system.addforce(f_aero_E, ve) points = [pC1, pC2] #ang = [wNC.dot(C.x),wNC.dot(C.y),wNC.dot(C.z)] f, ma = system.getdynamics() func1 = system.state_space_post_invert(f, ma) states = pynamics.integration.integrate_odeint(func1, ini, t, args=({ 'constants': system.constant_values }, ))
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) system.addforce(-b * wAM, wAM) system.addforcegravity(-g * N.y) # eq_d = [] # eq = [pAcm] eq = [] eq_d = [item.time_derivative() for item in eq] eq_d.append(wAM - G * wAB) eq_d.append(vAcm) eq_d.append(wNA) eq_dd = [item.time_derivative() for item in eq_d] eq_dd_scalar = [] eq_dd_scalar.append(eq_dd[0].dot(A.z))
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) 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) # stretch = -pBtip.dot(N.y) stretch_s = (stretch + abs(stretch)) on = stretch_s / (2 * stretch + 1e-10) system.add_spring_force1(k_constraint, -stretch_s * N.y, vBtip) system.addforce(-b_constraint * vBtip * on, vBtip) system.add_spring_force1(k, (qA - qO - preload1) * N.z, wOA) system.add_spring_force1(k, (qB - qA - preload2) * N.z, wAB) system.add_spring_force1(k, (qC - qO - preload3) * N.z, wOC)
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
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) #import sympy #ind = sympy.Matrix([wB]) #dep = sympy.Matrix([qA_d]) # #EQ = sympy.Matrix(eq_d) #A = EQ.jacobian(ind) #B = EQ.jacobian(dep) #dep2 = sympy.simplify(B.solve(-(A),method = 'LU')) f, ma = system.getdynamics() #f,ma = system.getdynamics([qB_d])
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) #from pynamics.integrator import RK4,DoPri #integrator = RK4(func,ini,t) #integrator = DoPri(func,ini,t) #states = integrator.run() states = pynamics.integration.integrate_odeint(func,
wAB = A.getw_(B) wBC = B.getw_(C) #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,pAcm,mA,IA,system) #BodyB = Body('BodyB',B,pBcm,mB,IB,system) #BodyC = Body('BodyC',C,pCcm,mC,IC,system) ParticleA = Particle(system, pAcm, mA, 'ParticleA') ParticleB = Particle(system, pBcm, mB, 'ParticleB') ParticleC = Particle(system, pCcm, mC, 'ParticleC') system.addforce(-b * wNA, wNA) system.addforce(-b * wAB, wAB) system.addforce(-b * wBC, wBC) #system.addforce(-k*(qA-preload1)*N.z,wNA) #system.addforce(-k*(qB-preload2)*A.z,wAB) #system.addforce(-k*(qC-preload3)*B.z,wBC) system.add_spring_force(k, (qA - preload1) * N.z, wNA) system.add_spring_force(k, (qB - preload2) * N.z, wAB) system.add_spring_force(k, (qC - preload3) * N.z, wBC) system.addforcegravity(-g * N.y) vCtip = pCtip.time_derivative(N, system) eq1_d = [vCtip.dot(N.y)] eq1_dd = [(system.derivative(item)) for item in eq1_d]
vAcm = pAcm.time_derivative() vBcm = pBcm.time_derivative() vCcm = pCcm.time_derivative() k_perp = 1 k_par = 1/3 faperp = k_perp*vSoil.dot(A.y)*A.y fapar= k_par*vSoil.dot(A.x)*A.x fbperp = k_perp*vSoil.dot(B.y)*B.y fbpar= k_par*vSoil.dot(B.x)*B.x fcperp = k_perp*vSoil.dot(C.y)*C.y fcpar= k_par*vSoil.dot(C.x)*C.x system.addforce((faperp+fapar),vAcm) system.addforce((fbperp+fbpar),vBcm) system.addforce((fcperp+fcpar),vCcm) system.addforce(-b*wNA,wNA) system.addforce(-b*wAB,wAB) system.addforce(-b*wBC,wBC) system.add_spring_force1(k,(qA-preload1)*N.z,wNA) system.add_spring_force1(k,(qB-qA-preload2)*N.z,wAB) system.add_spring_force1(k,(qC-qB-preload3)*N.z,wBC) # system.addforcegravity(-g*N.y) f,ma = system.getdynamics() func1 = system.state_space_post_invert(f,ma)
#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) x1 = BodyA.pCM.dot(N.x) y1 = BodyA.pCM.dot(N.y)
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) # stretch = -pBtip.dot(N.y) stretch_s = (stretch+abs(stretch)) on = stretch_s/(2*stretch+1e-10) system.add_spring_force1(k_constraint,-stretch_s*N.y,vBtip) system.addforce(-b_constraint*vBtip*on,vBtip) system.add_spring_force1(k,(qA-qO-preload1)*N.z,wOA) system.add_spring_force1(k,(qB-qA-preload2)*N.z,wAB) system.add_spring_force1(k,(qC-qO-preload3)*N.z,wOC) system.add_spring_force1(k,(qD-qC-preload4)*N.z,wCD)
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) toeforce = k_constraint * -stretch1_s stretch2 = -pC2.dot(N.y) stretch2_s = (stretch2 + abs(stretch2)) on = stretch2_s / (2 * stretch2 + 1e-10) system.add_spring_force1(k_constraint, -stretch2_s * N.y, vC2) system.addforce(-b_constraint * vC2 * on, vC2) system.addforce(-b * wNA, wNA) system.addforce(-b * wAB, wAB) system.addforce(-b * wBC, wBC) # system.addforce(force_var*N.x,vNA)
system.set_newtonian(N) A.rotate_fixed_axis(N, [0, 0, 1], q, system) p1 = x * N.x p2 = p1 - l * A.y 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 = x - xO system.add_spring_force1(k, (stretch) * N.x, v1) system.addforce(-b * v1, v1) system.addforcegravity(-g * N.y) f, ma = system.getdynamics() func1 = system.state_space_post_invert(f, ma, constants=system.constant_values) states = pynamics.integration.integrate_odeint(func1, ini, t, rtol=tol, atol=tol, args=({ 'constants': {}, 'alpha': 1e2, 'beta': 1e1 }, ))
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)) f, ma = system.getdynamics() func1, lambda1 = system.state_space_post_invert( f, ma, constants=system.constant_values, return_lambda=True) states = pynamics.integration.integrate_odeint(func1,
wAB = A.getw_(B) wBC = B.getw_(C) #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,pAcm,mA,IA,system) #BodyB = Body('BodyB',B,pBcm,mB,IB,system) #BodyC = Body('BodyC',C,pCcm,mC,IC,system) ParticleA = Particle(system, pAcm, mA, 'ParticleA') ParticleB = Particle(system, pBcm, mB, 'ParticleB') ParticleC = Particle(system, pCcm, mC, 'ParticleC') system.addforce(-b * wNA, wNA) system.addforce(-b * wAB, wAB) system.addforce(-b * wBC, wBC) system.addforce(fx * N.x, vCtip) system.addforce(fy * N.y, vCtip) #system.addforce(-k*(qA-preload1)*N.z,wNA) #system.addforce(-k*(qB-preload2)*A.z,wAB) #system.addforce(-k*(qC-preload3)*B.z,wBC) system.add_spring_force(k, (qA - preload1) * N.z, wNA) system.add_spring_force(k, (qB - preload2) * N.z, wAB) system.add_spring_force(k, (qC - preload3) * N.z, wBC) system.addforcegravity(-g * N.y) x1 = ParticleA.pCM.dot(N.x)