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([
