initialvalues = {}
initialvalues[x] = 1
initialvalues[x_d] = 0
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
statevariables = system.get_state_variables()
ini = [initialvalues[item] for item in statevariables]
N = Frame('N', system)
O = Frame('O', system)
A1 = Frame('A1', system)
B1 = Frame('B1', system)
C1 = Frame('C1', system)
D1 = Frame('D1', system)
A2 = Frame('A2', system)
B2 = Frame('B2', system)
C2 = Frame('C2', system)
D2 = Frame('D2', system)
system.set_newtonian(N)
O.rotate_fixed_axis(N, [0, 0, 1], qO, system)
A1.rotate_fixed_axis(N, [0, 0, 1], qA1, system)
B1.rotate_fixed_axis(N, [0, 0, 1], qB1, system)
C1.rotate_fixed_axis(N, [0, 0, 1], qC1, system)
D1.rotate_fixed_axis(N, [0, 0, 1], qD1, system)
A2.rotate_fixed_axis(N, [0, 0, 1], qA2, system)
B2.rotate_fixed_axis(N, [0, 0, 1], qB2, system)
C2.rotate_fixed_axis(N, [0, 0, 1], qC2, system)
D2.rotate_fixed_axis(N, [0, 0, 1], qD2, system)
pOrigin = 0 * N.x + 0 * N.y
pOcm = x * N.x + y * N.y
pOA = pOcm + lO / 2 * O.x
pOC = pOcm - lO / 2 * O.x
initialvalues = {}
initialvalues[qA]=0*math.pi/180
initialvalues[qA_d]=1
initialvalues[qB]=0*math.pi/180
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)
initialvalues[qMC_d] = 0
statevariables = system.get_state_variables()
ini = [initialvalues[item] for item in statevariables]
N = Frame('N', system)
O = Frame('O', system)
MA = Frame('MA', system)
A = Frame('A', system)
B = Frame('B', system)
MC = Frame('MC', system)
C = Frame('C', system)
D = Frame('D', system)
system.set_newtonian(N)
O.rotate_fixed_axis(N, [0, 0, 1], qO, system)
MA.rotate_fixed_axis(N, [0, 0, 1], qMA, system)
A.rotate_fixed_axis(N, [0, 0, 1], qA, system)
B.rotate_fixed_axis(N, [0, 0, 1], qB, system)
MC.rotate_fixed_axis(N, [0, 0, 1], qMC, system)
C.rotate_fixed_axis(N, [0, 0, 1], qC, system)
D.rotate_fixed_axis(N, [0, 0, 1], qD, system)
pOrigin = 0 * N.x + 0 * N.y
pOcm = x * N.x + y * N.y
pOA = pOcm + lO / 2 * O.x
pOC = pOcm - lO / 2 * O.x
pAB = pOA + lA * A.x
pBtip = pAB + lB * B.x
vBtip = pBtip.time_derivative(N, system)
initialvalues[qB_d] = 0 * pi / 180
#initialvalues[wB]=0*pi/180
#initialvalues[a]=0
initialvalues[i] = 0
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,
initialvalues[qB_d]=0*pi/180
initialvalues[qC]=150*pi/180
initialvalues[qC_d]=0*pi/180
initialvalues[qD]=-30*pi/180
initialvalues[qD_d]=0*pi/180
statevariables = system.get_state_variables()
N = Frame('N',system)
A = Frame('A',system)
B = Frame('B',system)
C = Frame('C',system)
D = Frame('D',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(N,[0,0,1],qC,system)
D.rotate_fixed_axis(C,[0,0,1],qD,system)
pNA = 0*N.x
pAB = pNA + lA*A.x
pBD = pAB + lB*B.x
pCD = pNA + lC*C.x
pDB = pCD + lD*D.x
points = [pBD,pAB,pNA,pCD,pDB]
statevariables = system.get_state_variables()
ini0 = [initialvalues[item] for item in statevariables]
initialvalues[wx_d] = 0
initialvalues[wy] = 0
initialvalues[wy_d] = 0
initialvalues[wz] = 0
initialvalues[wz_d] = 0
statevariables = system.get_state_variables()
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)
initialvalues[qM_d] = 0 * pi / 180
#initialvalues[wB]=0*pi/180
#initialvalues[a]=0
initialvalues[i] = 0
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)
Z = Frame('Z', 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)
M.rotate_fixed_axis(A, [0, 0, 1], qM, system)
pO = 0 * N.x
pAcm = x * N.x + y * N.y
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
A2 = Frame('A2', system)
A23 = Frame('A23', system)
A3 = Frame('A3', system)
A34 = Frame('A34', system)
NB1 = Frame('NB1', system)
B1 = Frame('B1', system)
B12 = Frame('B12', system)
B2 = Frame('B2', system)
B23 = Frame('B23', system)
################################################
#Relative frame rotations from newtonian out to distal frames
system.set_newtonian(N)
A1.rotate_fixed_axis(N, [1, 0, 0], qA1, system)
A12.rotate_fixed_axis(A1, [0, 0, 1], t1, system)
A2.rotate_fixed_axis(A12, [1, 0, 0], qA2, system)
A23.rotate_fixed_axis(A2, [0, 0, 1], t2, system)
A3.rotate_fixed_axis(A23, [1, 0, 0], qA3, system)
# A34.rotate_fixed_axis(A3,[0,0,1],t3,system)
#
NB1.rotate_fixed_axis(N, [0, 0, 1], -t0, system)
B1.rotate_fixed_axis(NB1, [1, 0, 0], qB1, system)
B12.rotate_fixed_axis(B1, [0, 0, 1], -t4, system)
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
N = Frame('N', system)
O = Frame('O', system)
A1 = Frame('A1', system)
C1 = Frame('C1', system)
A2 = Frame('A2', system)
C2 = Frame('C2', system)
MA = Frame('MA', system)
A = Frame('A', system)
B = Frame('B', system)
MC = Frame('MC', system)
C = Frame('C', system)
D = Frame('D', system)
system.set_newtonian(N)
O.rotate_fixed_axis(N, [0, 0, 1], qO, system)
A1.rotate_fixed_axis(N, [0, 0, 1], qA1, system)
C1.rotate_fixed_axis(N, [0, 0, 1], qC1, system)
A2.rotate_fixed_axis(N, [0, 0, 1], qA2, system)
C2.rotate_fixed_axis(N, [0, 0, 1], qC2, system)
MA.rotate_fixed_axis(N, [0, 0, 1], qMA, system)
B.rotate_fixed_axis(N, [0, 0, 1], qB, system)
MC.rotate_fixed_axis(N, [0, 0, 1], qMC, system)
D.rotate_fixed_axis(N, [0, 0, 1], qD, system)
pOrigin = 0 * N.x + 0 * N.y
pOcm = x * N.x + y * N.y
pOA = pOcm + lO / 2 * O.x
pOC = pOcm - lO / 2 * O.x
if __name__=='__main__':
from sympy import sin, cos
from math import pi
sys = System()
N = Frame('N',system)
A = Frame('A',system)
B = Frame('B',system)
C = Frame('C',system)
D = Frame('D',system)
E = Frame('E',system)
a = Constant('a',4,sys)
# a = sympy.Symbol('a')
# a,b,c,d,e,f,q = sympy.symbols('a b c d e f q')
qA,qA_d,qA_dd = Differentiable('qA',sys)
qB,qB_d,qB_dd = Differentiable('qB',sys)
qC,qC_d,qC_dd = Differentiable('qC',sys)
qD,qD_d,qD_dd = Differentiable('qD',sys)
qE,qE_d,qE_dd = Differentiable('qE',sys)
sys.set_newtonian(N)
A.rotate_fixed_axis(N,[0,0,1],qA,sys)
B.rotate_fixed_axis(A,[0,0,1],qB,sys)
C.rotate_fixed_axis(B,[0,0,1],qC,sys)
D.rotate_fixed_axis(C,[0,0,1],qD,sys)
E.rotate_fixed_axis(D,[1,0,0],qE,sys)
a= N.efficient_rep(B,'dot')[frozenset((N.x_sym,B.x_sym))]
N = Frame('N', system)
A1 = Frame('A1', system)
A2 = Frame('A2', system)
A3 = Frame('A3', system)
B1 = Frame('B1', system)
B2 = Frame('B2', system)
B3 = Frame('B3', system)
C1 = Frame('C1', system)
C2 = Frame('C2', system)
C3 = Frame('C3', system)
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)
