def retimage(yaw_all, pitch_all,leftxyz_all,phileft_all,rightxyz_all, phiright_all, pmat): #have to tie the record number to each para coordinate in the list so you can compare the stimulus on either eye with heading. i.e. if you want #to say something about where the para was wrt heading, transformed_plist_r = [] transformed_plist_l = [] para_wrt_heading = [] headingvec = [] for frame in range(pmat.shape[1]): print(frame) yaw = np.radians(yaw_all[frame]) pitch = np.radians(pitch_all[frame]) phileft = np.radians(phileft_all[frame]) phiright = np.radians(phiright_all[frame]) rightxyz = np.array(rightxyz_all[frame]) leftxyz = np.array(leftxyz_all[frame]) if np.isnan(yaw) or np.isnan(pitch) or np.isnan(phileft) or np.isnan(phiright) or np.any(np.isnan(rightxyz)) or np.any(np.isnan(leftxyz)): transformed_plist_r.append([]) transformed_plist_l.append([]) continue planepoint = (rightxyz+leftxyz) / 2 #this is the midpoint between the eyes. use as the base of the heading vector ufish = np.array([np.cos(yaw)*np.cos(pitch), np.sin(yaw)*np.cos(pitch), np.sin(pitch)]) headingvec.append(ufish) ur_par = np.array([np.cos(yaw + np.pi/2), np.sin(yaw + np.pi/2), 0]) #this is a vector pointing out of the right eye when parallel to the body. u_perp = np.cross(ufish,ur_par) #this will yield a vector normal to the shearing plane of the fish. shearingplane = Plane(Point3D(planepoint[0],planepoint[1],planepoint[2]), normal_vector = (int(u_perp[0]*100), int(u_perp[1]*100), int(u_perp[2]*100))) #normal vector input to a Plane object must be an int. if you don't multiply by 100, decimals are meaningless and just get rounded to 1 by int() ur_xy = np.array([np.cos(yaw+phiright-np.pi/2), np.sin(yaw+phiright-np.pi/2),0]) #xy unit vectors pointing out of eyes ul_xy = np.array([np.cos(yaw-phileft+np.pi/2), np.sin(yaw-phileft+np.pi/2),0]) #project xy vecs onto the shearing plane to give each one a Z coordinate r_xy = rightxyz+ur_xy #point values b/c you have to project a point. l_xy = leftxyz+ul_xy r_xyz = shearingplane.projection(Point3D(r_xy[0],r_xy[1],r_xy[2])) l_xyz = shearingplane.projection(Point3D(l_xy[0],l_xy[1],l_xy[2])) vec3D_r = np.array([r_xyz.x - rightxyz[0], r_xyz.y - rightxyz[1], r_xyz.z - rightxyz[2]]).astype(np.float32) vec3D_l = np.array([l_xyz.x - leftxyz[0], l_xyz.y - leftxyz[1], l_xyz.z - leftxyz[2]]).astype(np.float32) #need to float32 these b/c r_xyz and l_xyz are sympy float types which can't be read out by numpy sqrt function ur_xyz = vec3D_r / np.sqrt(np.dot(vec3D_r, vec3D_r)) #great these are 3D unit vecs pointing out of eyes! ul_xyz = vec3D_l / np.sqrt(np.dot(vec3D_l, vec3D_l)) yaxis_r = np.cross(ur_xyz,u_perp) yaxis_l = np.cross(ul_xyz,u_perp) #NOTE THAT THESE Y AXIS WILL GO IN DIFFERENT DIRECTIONS!! RIGHT TOWARDS TIP OF HEAD, LEFT TOWARDS THE TAIL. ALSO SIMPLY MAKE SURE THAT THIS IS ALL CORRECT. #ke sure all vectors w/ yaxis, ur,ul or uperp are unit at this point. critical for transformation of basis. temp_plist_r = [] temp_plist_l = [] temp_plist_heading = [] #for par_index in range(0,pmat.shape[0], 3): #comment this back in once you figure out why ir times are one too long. for par_index in range(0,pmat.shape[0], 3): para_xyz = pmat[par_index:par_index+3,frame] wrt_heading = np.array([para_xyz[0]-planepoint[0], para_xyz[1]-planepoint[1], para_xyz[2]-planepoint[2]]) wrt_right = np.array([para_xyz[0]-rightxyz[0], para_xyz[1]-rightxyz[1], para_xyz[2]-rightxyz[2]]) wrt_left = np.array([para_xyz[0]-leftxyz[0], para_xyz[1]-leftxyz[1], para_xyz[2]-leftxyz[2]]) # if wrt_right[0] > 0: #this is wrong. if fish eye is at 500 facing hte para at 200, dont want to eliminate!! eliminate at dot stage. new_basis_right = [np.dot(wrt_right, ur_xyz),np.dot(wrt_right,yaxis_r),np.dot(wrt_right,u_perp)] # else: # new_basis_right = [] # if wrt_left[0] > 0: new_basis_left = [np.dot(wrt_left, ul_xyz),np.dot(wrt_left,yaxis_l),np.dot(wrt_left,u_perp)] # else: # new_basis_left = [] if magvector(new_basis_right) < 200 and new_basis_right[0] > 0: temp_plist_r.append(new_basis_right) if magvector(new_basis_left) < 200 and new_basis_left[0] > 0: temp_plist_l.append(new_basis_left) if magvector(wrt_heading) < 200: temp_plist_heading.append(wrt_heading) transformed_plist_r.append(temp_plist_r) transformed_plist_l.append(temp_plist_l) para_wrt_heading.append(temp_plist_heading) return transformed_plist_r,transformed_plist_l,para_wrt_heading
def processBedTower(filename, zProbeOffset, xtarget, ytarget, ztarget, locmarg=0.02): """Reads and analyse a file with four measured bed heights (near towers and in centre). The probe trigger z value (not the actual height) is determined by moving the hot end slowly down and noting the z value where the probe triggers. The distance from the nozzle tip to the probe trigger height is zProbeOffset. The function calculates the number of screw turns required (on M3 screw). This save us a little arithmetic and removes the confusion of which direction to move. . Args: | zProbeOffset (float): the the z value when the probe triggers. friction test in mm. relative to one of the towers, X, Y or Z. B(default: `None`). | locmarg (float): issues a bed titl warning above this threshold Returns: | the axis object for the plot Raises: | No exception is raised. """ print('{}\n{}\n\n'.format(79 * '-', filename)) validlines = [] tdone = False with open(filename, 'r') as fin: lines = fin.readlines() for line in lines: if len(line) > 2: line = line.strip() if '>' in line[0] or '<' in line[0]: line = line[2:] lstl = line.split(' ') # only use lines with Bed X: in them for dataframe if 'Bed X:' in line: # remove unwanted clutter, keep only x,y,z validlines.append([float(lstl[i]) for i in [4, 6, 8]]) # if temperature lines, get values if not tdone and 'ok' in line and 'T:' in line and 'B:' in line: print('Time {} '.format(lstl[0])) print('Bed temperature is {} deg C'.format( lstl[5].split(':')[1])) print('Nozzle temperature is {} deg C'.format( lstl[3].split(':')[1])) tdone = True # make pandas dataframe df = pd.DataFrame(validlines, columns=['x', 'y', 'z']) #name the towers df['Tower'] = df.apply(label_tower, axis=1) df['Target'] = df.apply(target_tower, args=(xtarget, ytarget, ztarget), axis=1) # correct for probe offset to get to metal df['z'] = df['z'] - zProbeOffset # get height at screw df['S'] = df['z'] * 158.6 / (52.85 + np.sqrt(df['x']**2 + df['y']**2)) # now calc offsets to move to target df['dz'] = df['z'] - ztarget df['dz'] = df.apply(delta_tower, args=(xtarget, ytarget, ztarget), axis=1) df['dS'] = df['dz'] * 158.6 / (52.85 + np.sqrt(df['x']**2 + df['y']**2)) # get required turn magnitude df['Trns(deg)'] = 360. * df['dS'] / 0.5 df['Trns/0.25'] = (df['dS'] / 0.5) / 0.25 # from now on work with aggregates dfg = df.groupby(['Tower']) dfr = dfg.aggregate(np.mean) dfr['Std dev'] = dfg.aggregate(np.std)['S'] dfr['Spread'] = dfg.aggregate(np.max)['S'] - dfg.aggregate(np.min)['S'] # centre does not require slant correction dfr['S'].ix['C'] = dfr['z'].ix['C'] # remove value for C outputs dfr.ix['C']['Trns(deg)'] = np.nan dfr.ix['C']['Trns/0.25'] = np.nan dfr.ix['C']['S'] = np.nan dfr.ix['C']['dS'] = np.nan dfr.ix['C']['dz'] = np.nan if 'Xm' in dfr.index: print(dfr.drop(['Xm', 'Ym', 'Zm'], axis=0)) else: print(dfr) print('') # get mean, min and max of the tower averages and tpower and centre twrMean = np.mean((dfr['S']).T[['X', 'Y', 'Z']]) twrMin = np.min((dfr['S']).T[[ 'X', 'Y', 'Z', ]]) twrMax = np.max((dfr['S']).T[['X', 'Y', 'Z']]) twrSpread = twrMax - twrMin bedMean = np.mean((dfr['S']).T[['X', 'Y', 'Z', 'C']]) bedMin = np.min((dfr['S']).T[['X', 'Y', 'Z', 'C']]) bedMax = np.max((dfr['S']).T[['X', 'Y', 'Z', 'C']]) bedSpread = bedMax - bedMin print(' Towers Full bed') print(' (SX,SY,SZ) (SX,SY,SZ,SC)') print('mean {:.3f} {:.3f} '.format(twrMean, bedMean)) print('min {:.3f} {:.3f} '.format(twrMin, bedMin)) print('max {:.3f} {:.3f} '.format(twrMax, bedMax)) print('spread {:.3f} {:.3f} '.format(twrMax - twrMin, bedSpread)) # calculate the plane through the three screws bedplane = Plane(Point3D(dfr.ix['X']['x'], dfr.ix['X']['y'], dfr.ix['X']['z']), Point3D(dfr.ix['Y']['x'], dfr.ix['Y']['y'], dfr.ix['Y']['z']), Point3D(dfr.ix['Z']['x'], dfr.ix['Z']['y'], dfr.ix['Z']['z']), evaluate=False) # process bed normal vector bednormal = Matrix(bedplane.normal_vector).normalized() bnx = float(N(bednormal[0])) bny = float(N(bednormal[1])) bnz = float(N(bednormal[2])) ang = np.arctan2(bny, bnx) norm = np.sqrt(bnx * bnx + bny * bny) print('\nBed normal vector ({:.3e},{:.3e},{:.3e})'.format(bnx, bny, bnz)) print(' projection onto z=0 plane: radius={:.1f} um angle={:.1f} deg'. format(1e6 * norm, 180 * ang / np.pi)) print('\nPrint locus:') print('Plane through SX, SY, and SZ: {}'.format(N(bedplane.equation()))) # measured bed centre bedcentre = Point3D(dfr.ix['C']['x'], dfr.ix['C']['y'], dfr.ix['C']['z']) # displacement between plane at (0,0) and measured z convex = N((N(bedplane.projection(Point3D(0, 0))) - bedcentre).z) # apparent bed direction as seen by printer direc = 'mountain' if convex < 0. else 'valley' slant = 'on a tilted bed' if twrSpread > locmarg else '' print( 'Print locus convexity [plane(0,0) - SC] is {:.3f} mm'.format(convex)) print('The printer perceives the bed as a {} {}'.format(direc, slant)) if twrSpread > locmarg: print( '\n******** WARNING: bed tilt in z {:.3f} mm exceeds {:.3f} mm, {}' .format(twrSpread, locmarg, 'consider levelling'))
x - pointL[_sage_const_0] == sL[_sage_const_0] * t, y - pointL[_sage_const_1] == sL[_sage_const_1] * t, z == sL[_sage_const_2] * t ], x, y, z))[_sage_const_0] P1t = P1.subs(x == Lparam[_sage_const_0].right(), y == Lparam[_sage_const_1].right(), z == Lparam[_sage_const_2].right()) tt = solve(P1t, t)[_sage_const_0].right() xLP1 = Lparam[_sage_const_0].subs(t=tt) yLP1 = Lparam[_sage_const_1].subs(t=tt) zLP1 = Lparam[_sage_const_2].subs(t=tt) # запишем наши объекты через sympy from sympy import Plane, Point, Point3D, Line3D Psp = Plane(Point3D(pointP), normal_vector=n) M1sp = Point3D(M1) prtemp = Psp.projection(M1sp) # проекция M1 на P prM1P = vector([prtemp.x._sage_(), prtemp.y._sage_(), prtemp.z._sage_()]) # наша L через sympy Lsp = Line3D(pointL, point2L) prtemp = Lsp.projection(M1sp) # проекция M1 на L prM1L = vector([prtemp.x._sage_(), prtemp.y._sage_(), prtemp.z._sage_()]) # расстояние от M2 до P2 P2sp = Plane(Point3D(M1), normal_vector=n2) dM2P2 = P2sp.distance(Point3D(M2)) # плоскость через M1 и L P6 = Simp_Plane(Make_Plane(M1, pointL - M1, sL)) # проекция L1 на P # уравнение L1 в параметрическом виде pointL1 = vector([