initialvalues[qB_d]=1 initialvalues[qC]=0*math.pi/180 initialvalues[qC_d]=0 N = Frame('N',system) 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)
initialvalues[q] = 30 * pi / 180 initialvalues[q_d] = 0 * pi / 180 statevariables = system.get_state_variables() ini = [initialvalues[item] for item in statevariables] N = Frame('N', system) 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)
for item in y: plt.plot(*(item.T)) # for item,value in zip(system.get_state_variables(),result.x): # initialvalues[item]=value pA1cm = pOA + lA / 4 * A1.x pB1cm = pAB + lB / 4 * B1.x pC1cm = pOC + lC / 4 * C1.x pD1cm = pCD + lD / 4 * D1.x pA2cm = pA1A2 + lA / 4 * A2.x pB2cm = pB1B2 + lB / 4 * B2.x pC2cm = pC1C2 + lC / 4 * C2.x pD2cm = pD1D2 + lD / 4 * D2.x wOA1 = O.get_w_to(A1) wA1A2 = A1.get_w_to(A2) wA2B1 = A2.get_w_to(B1) wB1B2 = B1.get_w_to(B2) wOC1 = O.get_w_to(C1) wC1C2 = C1.get_w_to(C2) wC2D1 = C2.get_w_to(D1) wD1D2 = D1.get_w_to(D2) wB2D2 = B2.get_w_to(D2) #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)
system.set_newtonian(N) A.rotate_fixed_axis(N, [0, 0, 1], qA, system) B.rotate_fixed_axis(A, [0, 0, 1], qB, system) C.rotate_fixed_axis(B, [0, 0, 1], qC, system) pNA = 0 * N.x pAB = pNA + lA * A.x pBC = pAB + lB * B.x pCtip = pBC + lC * C.x pAcm = pNA + lA / 2 * A.x pBcm = pAB + lB / 2 * B.x pCcm = pBC + lC / 2 * C.x wNA = N.get_w_to(A) wAB = A.get_w_to(B) wBC = B.get_w_to(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) system.addforce(-b * wNA, wNA) system.addforce(-b * wAB, wAB) system.addforce(-b * wBC, wBC)
state = numpy.array([ini, result.x]) ini1 = list(result.x) y = points.calc(state) y = y.reshape((-1, 6, 2)) plt.figure() for item in y: 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 wOMA = O.get_w_to(MA) 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)
statevariables = system.get_state_variables() ini = [initialvalues[item] for item in statevariables] N = Frame('N', system) #A = Frame('A',system) B = Frame('B', system) M = Frame('M', system) system.set_newtonian(N) #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)
vAB = pAB.time_derivative(N, system) aAB = vAB.time_derivative(N, system) vBA = pBA.time_derivative(N, system) aBA = vBA.time_derivative(N, system) #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.get_w_to(A) wAB = A.get_w_to(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)
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()
B2.rotate_fixed_axis(B12, [1, 0, 0], qB2, system) B23.rotate_fixed_axis(B2, [0, 0, 1], -t3, system) ################################################ #Define particles at the center of mass of each body pNO = 0 * N.x ParticleA1 = Particle(A1.x + A12.x, m, 'ParticleA1', system) 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)
# J.simplify() # qd2 = J*qi # subs = dict([(ii,jj) for ii,jj in zip(qd,qd2)]) pAcm=pNA+lA/2*A.x pBcm=pAB+lB/2*B.x pCcm=pNA+lC/2*C.x pDcm=pCD+lD/2*D.x # # wNA = N.get_w_to(A) # # wAB = A.get_w_to(B) # # wBC = B.get_w_to(C) wND = N.get_w_to(D) # # 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) BodyA = Particle(pAcm,m,'ParticleA',system) BodyB = Particle(pBcm,m,'ParticleB',system) BodyC = Particle(pCcm,m,'ParticleC',system) BodyD = Particle(pDcm,m,'ParticleD',system) # # system.addforce(-b*wNA,wNA) # # system.addforce(-b*wAB,wAB)
y = y.reshape((-1, len(points), 2)) plt.figure() for item in y: plt.plot(*(item.T)) # for item,value in zip(system.get_state_variables(),result.x): # initialvalues[item]=value pA1cm = pOA + lA / 4 * A1.x pC1cm = pOC + lC / 4 * C1.x pBcm = pAB + lB / 2 * B.x pDcm = pCD + lD / 2 * D.x pA2cm = pA1A2 + lA / 4 * A2.x pC2cm = pC1C2 + lC / 4 * C2.x wOMA = O.get_w_to(MA) wOMC = O.get_w_to(MC) wOA1 = O.get_w_to(A1) wA1A2 = A1.get_w_to(A2) wA2B = A2.get_w_to(B) #wB1B2 = B1.get_w_to(B2) wOC1 = O.get_w_to(C1) wC1C2 = C1.get_w_to(C2) wC2D = C2.get_w_to(D) #wD1D2 = D1.get_w_to(D2) wBD = B.get_w_to(D) wNMA = N.get_w_to(MA) aNMA = wNMA.time_derivative() wNMC = N.get_w_to(MC)
for item in system.get_state_variables(): if item in variables: ini.append(result[item]) else: ini.append(initialvalues[item]) points = PointsOutput(points, constant_values=constants) points.calc(numpy.array([ini0, ini]), [0, 1]) points.plot_time() 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.get_w_to(A) wAB = A.get_w_to(B) wOC = O.get_w_to(C) wCD = C.get_w_to(D) wBD = B.get_w_to(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')
S = Frame('S', system) system.set_newtonian(N) A1.rotate_fixed_axis(N, [1, 0, 0], qA1, system) A2.rotate_fixed_axis(A1, [0, 1, 0], qA2, system) A3.rotate_fixed_axis(A2, [0, 0, 1], qA3, system) B1.rotate_fixed_axis(N, [1, 0, 0], qB1, system) B2.rotate_fixed_axis(B1, [0, 1, 0], qB2, system) B3.rotate_fixed_axis(B2, [0, 0, 1], qB3, system) C1.rotate_fixed_axis(N, [1, 0, 0], qC1, system) C2.rotate_fixed_axis(C1, [0, 1, 0], qC2, system) C3.rotate_fixed_axis(C2, [0, 0, 1], qC3, system) S.rotate_fixed_axis(A3, [0, 0, 1], qS, system) wA1 = N.get_w_to(A3) wA2 = wAx * A3.x + wAy * A3.y + wAz * A3.z N.set_w(A3, wA2) wB1 = N.get_w_to(B3) wB2 = wBx * B3.x + wBy * B3.y + wBz * B3.z N.set_w(B3, wB2) wC1 = N.get_w_to(C3) wC2 = wCx * C3.x + wCy * C3.y + wCz * C3.z N.set_w(C3, wC2) ### Vectors # Define the vectors that describe the kinematics of a series of connected lengths pAcm = x * A3.x + y * A3.y + z * A3.z
# S = Frame('S',system) system.set_newtonian(N) A1.rotate_fixed_axis(N, [1, 0, 0], qA1, system) A2.rotate_fixed_axis(A1, [0, 1, 0], qA2, system) A3.rotate_fixed_axis(A2, [0, 0, 1], qA3, system) B1.rotate_fixed_axis(N, [1, 0, 0], qB1, system) B2.rotate_fixed_axis(B1, [0, 1, 0], qB2, system) B3.rotate_fixed_axis(B2, [0, 0, 1], qB3, system) C1.rotate_fixed_axis(N, [1, 0, 0], qC1, system) C2.rotate_fixed_axis(C1, [0, 1, 0], qC2, system) C3.rotate_fixed_axis(C2, [0, 0, 1], qC3, system) if simplify_w: wA1 = N.get_w_to(A3) wA2 = wAx * A3.x + wAy * A3.y + wAz * A3.z N.set_w(A3, wA2) wB1 = N.get_w_to(B3) wB2 = wBx * B3.x + wBy * B3.y + wBz * B3.z N.set_w(B3, wB2) wC1 = N.get_w_to(C3) wC2 = wCx * C3.x + wCy * C3.y + wCz * C3.z N.set_w(C3, wC2) pAcm = x * A3.x + y * A3.y + z * A3.z pBase = pAcm - lA / 2 * A3.x pAB = pAcm + lA / 2 * A3.x
po1.calc(numpy.array([ini0,ini]),[0,1]) po1.plot_time() # po1.calc(numpy.array([ini,ini]),[0,1]) # po1.plot_time() pAcm = pNA+lA/2*A.x pBcm = pAB+lA/2*B.x pCcm = pNC+lA/2*C.x pDcm = pCD+lA/2*D.x vBD = pBD.time_derivative() vDB = pDB.time_derivative() wNA = N.get_w_to(A) wAB = A.get_w_to(B) wNC = N.get_w_to(C) wCD = C.get_w_to(D) wNB = N.get_w_to(B) uBD = 1/(vBD.length())*vBD uDB = 1/(vDB.length())*vDB # 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)