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, 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
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)
aC = kA.acc(rCD, omg3, alp3)
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