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)
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)
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
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)
ParticleB = Particle(pAB2, mA, 'ParticleB', system)
system.addforce(-b * vAB, vAB)
system.addforce(-b * vAB2, vAB2)
system.addforcegravity(-g * N.y)
v = pAB2 - pNA2
u = (v.dot(v))**.5
eq1 = [(v.dot(v)) - lA**2]
eq1_dd = system.derivative(system.derivative(eq1[0]))
eq = [eq1_dd]
f, ma = system.getdynamics()
func = system.state_space_post_invert(f,
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)
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)
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)
#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)
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)
#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(pAcm, mA, 'ParticleA', system)
ParticleB = Particle(pBcm, mB, 'ParticleB', system)
ParticleC = Particle(pCcm, mC, 'ParticleC', system)
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_force1(k, (qA - preload1) * N.z, wNA)
system.add_spring_force1(k, (qB - preload2) * N.z, wAB)
system.add_spring_force1(k, (qC - preload3) * N.z, wBC)
statevariables = system.get_state_variables()
ini = [initialvalues[item] for item in statevariables]
N = Frame('N')
system.set_newtonian(N)
A = Frame('A')
A.rotate_fixed_axis_directed(N, [0, 0, 1], q1)
pNA = 0 * N.x
pm1 = x1 * N.x + y1 * N.y
pm2 = pm1 + (l1 - l0) * A.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)
f4 = Frame(,system)
#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)
initialvalues[y]=.1
initialvalues[y_d]=0
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)
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)
system.addforce(-b*vm1,vm1)
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 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
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)
initialvalues[x] = 1
initialvalues[y] = 0
initialvalues[x_d] = 0
initialvalues[y_d] = 0
statevariables = system.get_state_variables()
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(pAB, mA, 'ParticleA', system)
system.addforce(-b * vAB, vAB)
system.addforcegravity(-g * N.y)
x1 = ParticleA.pCM.dot(N.x)
y1 = ParticleA.pCM.dot(N.y)
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(item) for item in eq1_d]
#
a = [(v.dot(v)) - lA**2]
initialvalues[y] = .1
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
pAcm = pNA + x * N.x + y * N.y
vAcm = pAcm.time_derivative(N, system)
ParticleA = Particle(system, pAcm, mA, 'ParticleA')
system.addforce(-b * vAcm, vAcm)
stretch = y
stretched1 = (stretch + abs(stretch)) / 2
stretched2 = -(-stretch + abs(-stretch)) / 2
#system.add_spring_force(k,(stretched1)*N.y,vAcm)
system.add_spring_force(k, (stretched2) * N.y, vAcm)
system.addforcegravity(-g * N.y)
x1 = ParticleA.pCM.dot(N.x)
y1 = ParticleA.pCM.dot(N.y)
KE = system.KE
initialvalues[x] = 1
initialvalues[y] = 0
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]
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)
BodyC = Particle(pCcm,mC,'ParticleC',system)
vSoil = -1*N.y
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
# 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)
toeforce = k_constraint * -stretch1_s
stretch2 = -pC2.dot(N.y)
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)
# # system.addforce(-b*wBC,wBC)
# # 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)
# # system.addforcegravity(-g*N.y)
# # vCtip = pCtip.time_derivative(N,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 = system.get_state_variables()
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], q1, system)
pOrigin = 0 * N.x
pm1 = x1 * N.x + y1 * N.y
pm2 = pm1 + a * A.x - y2 * A.y
IA = Dyadic.build(A, Ixx_A, Iyy_A, Izz_A)
BodyA = Body('BodyA', A, pm1, m1, IA, 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 = l - l0
ul_ = l_ * (l**-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)
