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.