linke = 56.
linkd = 77.
linkf = 75.
link1 = 53. #Ground link
alpha1 = 0.085 #Angle given for ground link
gammad = -(np.pi-2.97) #Angle given for rigid triangle
#First fixed joint (for crank) is at (0, 0)
fp1 = np.array([0.0,0.0])
#Get coordinates of the second fixed joint
fp2 = -link1*np.array([np.cos(alpha1),np.sin(alpha1)])
plot((fp2[0],0.0),(fp2[1],0.0),color='k') #plot the ground link
#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,:]
# 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')
# Compute all the locations of joint34 and joint36, treating
# joint36 as a coupler point
joints34 = np.zeros((numsteps,2),float)
joints36 = np.zeros((numsteps,2),float)
for i in range(0,numsteps):
intersections = circcirc(joints23[i,:],l3,joint14,l4)
joints34[i,:] = intersections[assembly,:]
joints36[i,:] = coupler(joints23[i,:],joints34[i,:],c3,gamma3)
# 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)
couplerpts = np.zeros((numsteps,2),float)
for i in range(0,numsteps):
intersects = circcirc(joints23[i,:],l3,initjoints[3,:],l4)
joints34[i,:] = intersects[assembly,:]
couplerpts[i,:] = coupler(joints34[i,:],joints23[i,:],lc,np.pi+gammac)
#Plot the various points - may want to modify this depending
# on what effects we are looking for.
# Currently plots a fat dot at start of angle range and a
# square at end of angle range.
