# 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)
