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)
