def vel(theta1, case):
rB, rC, rD, rE, rD2 = pos(theta1, case)
vB = kA.vel(rB, case[-1])
v2_T, omg3 = kA.vCPA1(0, rB-rC, rB, case[-1])
vD, vD2 = kA.vel(rD-rC, omg3), kA.vel(rD2-rC, omg3)
vE, omg4 = kA.vCPA2(-vD, rE-rC, rE-rD)
return vB, vD, vE, vD2, omg3, omg4, v2_T
def vel(theta1, case):
rB, rC, rD, rE, rC2 = pos(theta1, case)
vB = kA.vel(rB, case[-1])
v2_T, omg2 = kA.vCPA1(0, rB-rE, rB, case[-1])
vC, vC2 = kA.vel(rC-rE, omg2, v2_T), kA.vel(rC2-rE, omg2, v2_T)
v5_T, omg4 = kA.vCPA2(-vC, 1j, rD-rC)
vD = kA.vel(1j, v_T=v5_T)
return vB, vC, vD, vC2, omg2, omg4, v2_T, v5_T
def vel(theta1, case):
rB, rC, rE, rD, rE, rF, rB_max = pos(theta1, case)
vB = kA.vel(rB, case[-1])
v2_T, omg2 = kA.vCPA1(0, rB-rD, rB, case[-1])
vB2 = kA.vel(rB_max-rD, omg2, v2_T)
vC = kA.vel(rC-rD, omg2, v2_T)
vE = vC + kA.vel(rE-rC, omg2)
vF, omg4 = kA.vCPA2(-vE, rF-case[-3], rF-rE)
return vB, vC, vE, vF, vB2, omg2, omg4, v2_T
def vel(phi1, case):
rB, rC, rD, rE, rF = pos(phi1, case)
omg1 = case[-1] * np.pi / 30
vB = kA.vel(rB, omg1)
omg2, omg3 = kA.vCPA4(vB, rC - rB, rC - rD)
vC, vE = kA.vel(rC - rD, omg3), kA.vel(rE - rD, omg3)
v5_T, omg4 = kA.vCPA2(-vE, rF - rD, rF - rE)
vF = kA.vel(rF - rD, v_T=v5_T)
return vB, vC, vE, vF, omg2, omg3, omg4, v5_T
def vel(phi1, omg1, AB=0.15, AC=0.1, CD=0.15, DF=0.4, AG=0.3):
rA, rB, rC, rD, rF, rG = pos(phi1, AB, AC, CD, DF, AG)
v2 = kA.vel(rB, omg1, 0)
v3_T, omg3 = kA.vCPA1(0, rB - rC, rB, omg1, 0)
v5_T, omg5 = kA.vCPA1(0, rD, rD - rC, omg3, 0)
v4 = kA.vel(rD - rC, omg3, 0)
vF = kA.vel(rF - rC, omg3, 0)
vG = kA.vel(rG, omg5, 0)
return v2, v3_T, v5_T, v4, vF, vG, omg3, omg5
def vel(phi1, case):
rB, rC, rD, rD_max, rE, rC_max = pos(phi1, case)
omg1 = case[-1]*np.pi/30
# find vB
vB = kA.vel(rB, omg1)
# angular velocity of link 2
v2_T, omg2 = kA.vCPA1(0, rB-rD, rB, omg1)
vC = kA.vel(rC-rD, omg2, v2_T)
vE = vC_max = np.imag(vC)*1j
vD_max = kA.vel(rD_max-rD, omg2, v2_T)
return vB, vC, vE, vD_max, vC_max, omg2, v2_T
def vel(phi1, case):
rB, rC, rD, rE, rF = pos(phi1, case)
omg1 = case[-1] * np.pi / 30
# find vB
vB = kA.vel(rB, omg1)
# find v2_T, omg2
v2_T, omg2 = kA.vCPA1(0, rB - rC, rB, omg1)
# find vE = vB + vEB
vD = vB + kA.vel(rD - rB, omg2) #kA.vel(rD-rC, omg2, v2_T)
omg4, omg5 = kA.vCPA4(vD, rE - rD, rE - rF)
vE = kA.vel(rE - rF, omg5)
return vB, vD, vE, omg2, omg4, omg5, v2_T
def vel(phi1, case):
rB, rC, rD, rE, rF, rcF = pos(phi1, case)
omg1 = case[-1] * np.pi / 30
# find vB
vB = kA.vel(rB, omg1)
# find angular velocity of link2, link3
omg2, omg3 = kA.vCPA4(vB, rC - rB, rC - rD)
# find vE = vB + vEB
vE = kA.vel(rE - rD, omg3)
v5_T, omg4 = kA.vCPA2(vE, rcF, rE - rF)
vF = kA.vel(rcF, v_T=v5_T)
return vB, vF, omg2, omg3, omg4, v5_T
def vel(phi1, omg1, variation=0, AB=1, BC=1):
"""
Calculate velocities and angular velocities
"""
rB, rC, rBC = pos(phi1, AB, BC)[variation]
vB = kA.vel(rB, omg1, 0)
vC, omg2 = kA.vCPA2(vB, -rC, rBC, 0, 0)
return vB, vC, omg2
rED = kA.toExp(case[2] + case[3], CD_angle)
rC, rE = rD + rCD, rD + rED
res1, res2 = kA.CPA2(np.pi / 2, case[4], case[7] + rD + rED)
r0F_1, rFE_1 = kA.toExp(res1[0], np.pi / 2), kA.toExp(case[4], res1[1])
r0F_2, rFE_2 = kA.toExp(res2[0], np.pi / 2), kA.toExp(case[4], res2[1])
return (rB, rC, rD, rE, rCD, rED, r0F_1, rFE_1), \
(rB, rC, rD, rE, rCD, rED, r0F_2, rFE_2)
def vel(theta1, case, variation=0):
rB, rC, rD, rE, rCD, rED, r0F, rFE = pos(theta1, case)[variation]
omg1 = case[-1] * np.pi / 30
vB = kA.vel(rB, omg1)
omg2, omg3 = kA.vCPA4(vB, rC - rB, rCD)
vE = kA.vel(rED, omg3, 0)
v05, omg4 = kA.vCPA2(-vE, r0F, rFE)
v5 = kA.vel(r0F, v_T=v05)
return vB, v05, v5, omg2, omg3, omg4
def acc(theta1, case, variation=0):
rB, rC, rD, rE, rCD, rED, r0F, rFE = pos(theta1, case)[variation]
v05, v5, omg2, omg3, omg4 = vel(theta1, case, variation)[1:]
omg1 = case[-1] * np.pi / 30
aB = kA.acc(rB, omg1)
alp2, alp3 = kA.aCPA4(aB, rC - rB, rCD, omg2, omg3)
F23 = FGs[2] - Gs[2] - F03 - (-F34)
# find F01
F01 = FGs[0] - Gs[0] - (-F12)
# find drive moment of crank 1
M_eq = dA.toCross(rB / 2, FGs[0] - Gs[0]) - dA.toCross(rB, -F12)
return F05, F45, F23, F03, F12, F01, M_eq
def virtual_work(phi1, case, ms, F_ext, g=-9.807j):
omg1, omg3 = case[-1] * np.pi / 30, vel(phi1, case)[3]
rB, rC, rD, rE, rF, rcF = pos(phi1, case)
vC, vE = kA.vel(rC - rD, omg3), kA.vel(rE - rD, omg3)
vB, vF = vel(phi1, case)[:2]
F_ext *= -np.sign(vF)
vGs = [vB / 2, (vB + vC) / 2, (vC + vE) / 2, (vE + vF) / 2, vF]
M_eq = np.sum(dA.toDot(ms * g, vGs) + dA.toDot(F_ext, vF)) / omg1
return M_eq
def animate(case):
from matplotlib.animation import ArtistAnimation
phi1s = np.linspace(0, 2 * np.pi, 300)
fig, ax = plt.subplots()
rA, container = 0, []
def vel(theta1, omg1, AB, AD, BC, CD, variation=0):
rB, rC = pos(theta1, AB, AD, BC, CD)[variation]
vB = kA.vel(rB, omg1)
omg2, omg3 = kA.vCPA4(vB, rC - rB, rC - AD)
vC = kA.vel(rC - AD, omg3)
return vB, vC, omg2, omg3