def acc(theta1, omg1, alp1, AB, AD, BC, CD, variation=0):
rB, rC = pos(theta1, AB, AD, BC, CD)[variation]
omg2, omg3 = vel(theta1, omg1, AB, AD, BC, CD, variation)[2:]
aB = kA.acc(rB, omg1, alp1)
alp2, alp3 = kA.aCPA4(aB, rC - rB, rC - AD, omg2, omg3)
aC = kA.acc(rC - AD, omg3, alp3)
return aB, aC, alp2, alp3
def acc(theta1, case):
rB, rC, rD, rE, rD2 = pos(theta1, case)
vB, vD, vE, vD2, omg3, omg4, v2_T = vel(theta1, case)
aB = kA.acc(rB, case[-1])
a2_T, alp3 = kA.aCPA1(0, rB-rC, rB, omg3, case[-1], v1_T=v2_T)
aD, aD2 = kA.acc(rD-rC, omg3, alp3), kA.acc(rD2-rC, omg3, alp3)
aE, alp4 = kA.aCPA2(-aD, rE-rC, rE-rD, 0, omg4, v1_T=vE)
return aB, aD, aE, aD2, alp3, alp4
def acc(theta1, case):
rB, rC, rE, rD, rE, rF, rB_max = pos(theta1, case)
omg2, omg4, v2_T = vel(theta1, case)[-3:]
aB = kA.acc(rB, case[-1])
a2_T, alp2 = kA.aCPA1(0, rB-rD, rB, omg2, case[-1], v1_T=v2_T)
aB2 = kA.acc(rB_max-rD, omg2, alp2, v2_T, a2_T)
aC = kA.acc(rC-rD, omg2, alp2, v2_T, a2_T)
aE = aC + kA.acc(rE-rC, omg2, alp2)
aF, alp4 = kA.aCPA2(-aE, rF-case[-3], rF-rE, 0, omg4, 0, vel(theta1, case)[3])
return aB, aC, aE, aF, aB2, alp2, alp4
def acc(theta1, case):
rB, rC, rD, rE, rC2 = pos(theta1, case)
omg2, omg4, v2_T, v5_T = vel(theta1, case)[-4:]
aB = kA.acc(rB, case[-1])
a2_T, alp2 = kA.aCPA1(0, rB-rE, rB, omg2, case[-1], v1_T=v2_T)
aC = kA.acc(rC-rE, omg2, alp2, v2_T, a2_T)
aC2 = kA.acc(rC2-rE, omg2, alp2, v2_T, a2_T)
a5_T, alp4 = kA.aCPA2(-aC, 1j, rD-rC, omg2=omg4, v1_T=v5_T)
aD = kA.acc(1j, v_T=v5_T, a_T=a5_T)
return aB, aC, aD, aC2, alp2, alp4
def acc(phi1, case):
rB, rC, rD, rE, rF = pos(phi1, case)
omg2, omg3, omg4, v5_T = vel(phi1, case)[-4:]
omg1 = case[-1] * np.pi / 30
aB = kA.acc(rB, omg1)
alp2, alp3 = kA.aCPA4(aB, rC - rB, rC - rD, omg2, omg3)
aC, aE = kA.acc(rC - rD, omg3, alp3), kA.acc(rE - rD, omg3, alp3)
a5_T, alp4 = kA.aCPA2(-aE, rF - rD, rF - rE, omg2=omg4, v1_T=v5_T)
aF = kA.acc(rF - rD, v_T=v5_T, a_T=a5_T)
return aB, aC, aE, aF, alp2, alp3, alp4
def acc(phi1, omg1, alp1, 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)
v3_T, v5_T = vel(phi1, omg1, AB, AC, CD, DF, AG)[1:3]
omg3, omg5 = vel(phi1, omg1, AB, AC, CD, DF, AG)[-2:]
a2 = kA.acc(rB, omg1, 0, 0, alp1)
a3_T, alp3 = kA.aCPA1(0, rB - rC, rB, omg3, omg1, v3_T, 0, 0, alp1)
a5_T, alp5 = kA.aCPA1(0, rD, rD - rC, omg5, omg3, v5_T, 0, 0, alp3)
a4 = kA.acc(rD - rC, omg3, 0, 0, alp3)
aF = kA.acc(rF - rC, omg3, 0, 0, alp3)
aG = kA.acc(rG, omg5, 0, 0, alp5)
return a2, a3_T, a5_T, a4, aF, aG, alp3, alp5
def acc(phi1, case):
rB, rC, rD, rD_max, rE, rC_max = pos(phi1, case)
vB, vC, vE, vD_max, vC_max, omg2, v2_T = vel(phi1, case)
omg1 = case[-1]*np.pi/30
# find aB
aB = kA.acc(rB, omg1)
# find angular acceleration of link2, link3
a2_T, alp2 = kA.aCPA1(0, rB-rD, rB, omg2, omg1, v1_T=v2_T)
# find aC, aE
aC = kA.acc(rC-rD, omg2, alp2, v2_T, a2_T)
aE = aC_max = np.imag(aC)*1j
aD_max = kA.acc(rD_max-rD, omg2, alp2, v2_T, a2_T)
return aB, aC, aE, aD_max, aC_max, alp2, a2_T
def acc(phi1, case):
rB, rC, rD, rE, rF = pos(phi1, case)
omg2, omg4, omg5, v2_T = vel(phi1, case)[3:]
omg1 = case[-1] * np.pi / 30
# find aB
aB = kA.acc(rB, omg1)
# find angular acceleration of link2, link3
a2_T, alp2 = kA.aCPA1(0, rB - rC, rB, omg2, omg1, v1_T=v2_T)
# find aC, aE
aD = aB + kA.acc(rD - rB, omg2,
alp2) #kA.acc(rD-rC, omg2, alp2, v2_T, a2_T)
alp4, alp5 = kA.aCPA4(aD, rE - rD, rE - rF, omg4, omg5)
aE = kA.acc(rE - rF, omg5, alp5)
return aB, aD, aE, alp2, alp4, alp5
def acc(phi1, case):
rB, rC, rD, rE, rF, rcF = pos(phi1, case)
vF, omg2, omg3, omg4, v5_T = vel(phi1, case)[1:]
omg1 = case[-1] * np.pi / 30
# find aB
aB = kA.acc(rB, omg1)
# find angular acceleration of link2, link3
alp2, alp3 = kA.aCPA4(aB, rC - rB, rC - rD, omg2, omg3)
# find aC, aE
aC = kA.acc(rC - rD, omg3, alp3)
aE = kA.acc(rE - rD, omg3, alp3)
a5_T, alp4 = kA.aCPA2(aE, rcF, rE - rF, omg2=omg4, v1_T=v5_T)
aF = kA.acc(rcF, v_T=v5_T, a_T=a5_T)
return aB, aC, aE, aF, alp2, alp3, alp4
def acc(phi1, omg1, alp1, variation=0, AB=1, BC=1):
"""
Calculate accelerations and angular acceleration
"""
rB, rC, rBC = pos(phi1, AB, BC)[variation]
vC, omg2 = vel(phi1, omg1, variation, AB, BC)[-2:]
aB = kA.acc(rB, omg1, 0, 0, alp1)
aC, alp2 = kA.aCPA2(aB, -rC, rBC, 0, omg2, vC, 0, 0, 0)
return aB, aC, alp2
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)
aC = kA.acc(rCD, omg3, alp3)
aE = kA.acc(rED, omg3, alp3)
a05, alp4 = kA.aCPA2(-aE, r0F, rFE, omg2=omg4, v1_T=v05)
a5 = kA.acc(r0F, v_T=v05, a_T=a05)
return aB, aC, aE, a5, a05, alp2, alp3, alp4
def reactions(theta1, case, ms, Is, F_ext, g=-9.807j, variation=0):
rB, rC, rD, rE, rCD, rED, r0F, rFE = pos(theta1, case)[variation]
aB, aC, aE, a5, a05, alp2, alp3, alp4 = acc(theta1, case, variation)
F = F_ext * -np.sign(vel(theta1, case, variation)[2])
aGs = np.array([aB / 2, (aB + aC) / 2, aE / 2, (aE + a5) / 2, a5])
