#Compute crank arc from thetastart to thetaend
crankpoints = arcpoints(fp1,linka,thetastart,thetaend,numsteps)
plot(crankpoints[:,0],crankpoints[:,1],color = 'g',linewidth=0.5)
#For each crank position, compute joint3 and joint4,
#i.e., the upper and lower intersections of links 'e' and 'd'
#In this linkage they are mirror images of each other.
#joints4 = 1st solution (LHS if traveling to fp2)
#joints3 = 2nd solution (RHS if traveling to fp2)
joints3 = np.zeros((numsteps,2),float)
joints4 = np.zeros((numsteps,2),float)
for i in range(0,numsteps):
intersects = circcirc(crankpoints[i,:],linke,fp2,linkd)
joints4[i,:] = intersects[0,:]
joints3[i,:] = intersects[1,:]
plot(joints3[:,0],joints3[:,1],color = 'g',linewidth=0.5)
plot(joints4[:,0],joints4[:,1],color = 'g',linewidth=0.5)
#Now we can get joints5. The rigid triangles with linkd
#create a bent link for which the coupler is joint5.
#In fact, because fp2 is fixed, it traces an arc like
#joint3, but phase-shifted by (pi-2.97)
joints5 = np.zeros((numsteps,2),float)
for i in range(0,numsteps):
joints5[i,:] = coupler(joints3[i,:],fp2,linkd,gammad)
# Find the angles gamma3 and gamma5 of the "bent links"
# Use arctan2(deltay,deltax) for unambiguous sign convention.
gamma1 = np.arctan2(d3[1],d3[0])
gamma2 = np.arctan2(d36[1],d36[0])
gamma3 = gamma2-gamma1
gamma1 = np.arctan2(d35[1],d35[0])
gamma2 = np.arctan2(d6[1],d6[0])
gamma5 = gamma2-gamma1
# Use circcirc() to solve for the initial location
# of joint34. One of the two solutions returned
# will match the actual initial position.
# This determines which "assembly" we have.
intersections = circcirc(joint23,l3,joint14,l4)
if(np.allclose(intersections[0],joint34)):
assembly = 0
elif(np.allclose(intersections[1],joint34)):
assembly = 1
else:
print('Hmmm, neither solution matches the input point...')
# Use Arcpoints() to get all the positions of joint23,
# as input crank goes from thetastart to thetaend
joints23 = arcpoints(joint12,l2,thetastart,thetaend,numsteps)
# Plot them, with dot at start and square at end:
plot(joints23[:,0],joints23[:,1],color = 'g',linewidth=0.5)
plot(joints23[0,0],joints23[0,1],'o')
plot(joints23[numsteps-1,0],joints23[numsteps-1,1],'s')
l4 = np.linalg.norm(d34)
dc3 = initcoupler - initjoints[1,:]
lc = np.linalg.norm(dc3)
# Find the angle gammac between link3 and coupler
# Use arctan2(deltay,deltax) for unambiguous sign convention.
gamma1 = np.arctan2(d23[1],d23[0])
gamma2 = np.arctan2(dc3[1],dc3[0])
gammac = -gamma1+gamma2
# Use circcirc() to see which solution for joint34
# matches the one we've started with.
# This determines which 'assembly' we have.
# If neither solution matches, something went wrong...
# We use numpy.allclose() to check if negligible difference.
joint34 = circcirc(initjoints[1,:],l3,initjoints[3,:],l4)
if(np.allclose(joint34[0,:],initjoints[2,:])):
assembly = 0
elif(np.allclose(joint34[1,:],initjoints[2,:])):
assembly = 1
else:
print('Hmmm, neither solution matches the input point...')
# Use Arcpoints() to get the positions of joint23,
# i.e., of the joint that connects links 2 and 3, where
# link2 is the input crank.
joints23 = arcpoints(initjoints[0,:],l2,thetastart,thetaend,numsteps)
# For each joint23 location, find the corresponding
# joint34 and coupler locations
joints34 = np.zeros((numsteps,2),float)