sf-mx/PBSwissMX
0
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

wip

Browse Source
This commit is contained in:
zamofing_t
2017-12-06 13:46:21 +01:00
parent d771169639
commit b625bab5bf
3 changed files with 603 additions and 220 deletions
Download Patch File Download Diff File Expand all files Collapse all files

659
python/helicalscan.py
View File

@@ -16,9 +16,13 @@ import matplotlib as mpl
import matplotlib.pyplot as plt
import mpl_toolkits.mplot3d as plt3d
import matplotlib.animation as anim
from matplotlib.widgets import Slider
from utilities import *
d2r=2*np.pi/360
#ax.set_xlabel('Z');ax.set_ylabel('X');ax.set_zlabel('Y')
#plot coordinates: X Y Z
#data Z X Y
@@ -40,22 +44,454 @@ class Trf:
c = np.cos(rad)
s = np.sin(rad)
m=np.identity(4)
m[np.ix_((0,2),(0,2))]=((c, -s),(s, c))
m[np.ix_((0,2),(0,2))]=((c, s),(-s, c))
return m
@staticmethod
def trans(v):
def trans(x,y,z):
m=np.identity(4)
m[0:3, 3] = v
m[0:3, 3] =(x,y,z)
return m
class ExtAxes():#mpl.axes._subplots.Axes3DSubplot):
def __init__(self,ax):
self.ax=ax
class HelicalScan:
def __init__(self,args):
if args.cfg:
fh=open(args.cfg,'r')
s=fh.read()
cfg=json.loads(s, object_hook=ConvUtf8)
s=json.dumps(cfg, indent=2, separators=(',', ': '));print(s)
else:
fn='/tmp/shapepath4'
#fn='/home/zamofing_t/Documents/prj/SwissFEL/epics_ioc_modules/ESB_MX/python/data/'+time.strftime('%y-%m-%d-%H_%M_%S')
#cfg = {"sequencer": ['gen_grid_points(w=5,h=5,pitch=100,rnd=0.4)', 'sort_points()','gen_prog(file="'+fn+'.prg",host="SAR-CPPM-EXPMX1",mode=1,pt2pt_time=10,cnt=1)', 'plot_gather("'+fn+'.npz")']}
#cfg = {"sequencer": ['test_find_rot_ctr()']}
#cfg = {"sequencer": ['test_find_rot_ctr(n=5. ,per=1.,bias=2.31,ampl=4.12,phi=24.6)']}
cfg = {"sequencer": ['test_coord_trf()']}
self.cfg=dotdict(cfg)
self.args=args
def sequencer(self):
print('args='+str(self.args))
print('cfg='+str(self.cfg))
#try:
# self.points=np.array(self.cfg.points)
#except AttributeError:
# pass
try:
sequencer= self.cfg.pop('sequencer')
except KeyError:
print('no command sequence to execute')
else:
dryrun=self.args.dryrun
for cmd in sequencer:
print('>'*5+' '+cmd+' '+'<'*5)
if not dryrun:
eval('self.' + cmd)
def calcParam(self):
n = 3.;
per = 1.;
w = 2 * np.pi * per / n * np.arange(n)
p = ((2.3, .71, 4.12, 10.6 * d2r), (6.2, .45, 3.2, 45.28 * d2r)) # (y, bias, ampl, phi)
self.param = param = np.ndarray((len(p), 5))
z = 14.5 # fix z position
for i in range(2):
(y, bias, ampl, phi) = p[i]
x = ampl * np.cos(w + phi) + bias
print('yMeas_%d=' % i + str(y) + ' xMeas_%d=' % i + str(x))
# param[i]=(z_i, y_i, x_i, r_i,phi_i)
param[i, 0] = z
param[i, 1] = y
param[i, 2:] = HelicalScan.meas_rot_ctr(x) # (bias,ampl,phase)
(bias, ampl, phase) = param[i][2:]
print param
def pltOrig(self,m,h=None):
ax=self.ax
# m is a 4x4 matrix. the transformed matrix
r=m[:3,0] #1st
g=m[:3,1] #2nd
b=m[:3,2] #3rd
o=m[:3,3] #origin
if h is None:
hr=ax.plot((o[2],o[2]+r[2]), (o[0],o[0]+r[0]), (o[1],o[1]+r[1]), 'r')
hg=ax.plot((o[2],o[2]+g[2]), (o[0],o[0]+g[0]), (o[1],o[1]+g[1]), 'g')
hb=ax.plot((o[2],o[2]+b[2]), (o[0],o[0]+b[0]), (o[1],o[1]+b[1]), 'b')
return hr + hg + hb # this is a list
else:
hr, hg, hb = h
hr.set_data((o[2], o[2] + r[2]), (o[0], o[0] + r[0]))
hr.set_3d_properties((o[1], o[1] + r[1]))
hg.set_data((o[2], o[2] + g[2]), (o[0], o[0] + g[0]))
hg.set_3d_properties((o[1], o[1] + g[1]))
hb.set_data((o[2], o[2] + b[2]), (o[0], o[0] + b[0]))
hb.set_3d_properties((o[1], o[1] + b[1]))
return h
def pltCrist(self,fy=0,cx=0,cz=0,w=0,h=None):
#h are the handles
if h is None:
h=[] #handels
ax=self.ax
param=self.param
pt = np.ndarray((4, 3))
for i in range(2):
(z, y, x, r, phi) = param[i]
x+=cx;y+=fy;z+=cz;phi+=w
pt[i] = (x, y, z)
pt[i + 2] = (x + r * np.sin(phi), y, z + r * np.cos(phi))
obj = mpl.patches.Circle((z, x), r, facecolor=mpl.colors.colorConverter.to_rgba('r', alpha=0.3))
h1=ax.add_patch(obj)
h2=plt3d.art3d.pathpatch_2d_to_3d(obj, z=y, zdir="z")
h.append(obj)
#hs=ax.scatter(pt[:, 2], pt[:, 0], pt[:, 1])
hs=ax.plot(pt[:, 2], pt[:, 0], pt[:, 1],'.')
hp=ax.plot(pt[2:, 2], pt[2:, 0], pt[2:, 1])
h+=(hs[0],hp[0])
else:
ax=self.ax
param=self.param
pt = np.ndarray((4, 3))
for i in range(2):
(z, y, x, r, phi) = param[i]
x+=cx;y+=fy;z+=cz;phi+=w
pt[i] = (x, y, z)
pt[i + 2] = (x + r * np.sin(phi), y, z + r * np.cos(phi))
h[i].remove()
obj = mpl.patches.Circle((z, x), r, facecolor=mpl.colors.colorConverter.to_rgba('r', alpha=0.3))
ax.add_patch(obj)
plt3d.art3d.pathpatch_2d_to_3d(obj, z=y, zdir="z")
h[i]=obj
h[2].set_data(pt[:, 2], pt[:, 0])#, pt[:, 1]))
h[2].set_3d_properties(pt[:, 1])
h[3].set_data(pt[2:, 2], pt[2:, 0])#, pt[:, 1]))
h[3].set_3d_properties(pt[2:, 1])
#hr.set_data((o[2], o[2] + r[2]), (o[0], o[0] + r[0]))
#hr.set_3d_properties((o[1], o[1] + r[1]))
#h[3].set_3d_properties(zs=pt[:, 1]))
#h[4].set_data((pt[2:, 2], pt[2:, 0]))#, pt[2:, 1]))
#hp=ax.plot(pt[2:, 2], pt[2:, 0], pt[2:, 1], label='zs=0, zdir=z')
#h+=(hs,hp[0])
return (h,pt)
def interactive_fy_cx_cz_w(self):
fig = plt.figure()
self.manip=False#True#False
self.ax=ax=plt3d.Axes3D(fig,[0.02, 0.15, 0.96, 0.83])
ax.set_xlabel('Z');ax.set_ylabel('X');ax.set_zlabel('Y')
ax.view_init(elev=14., azim=10)
self.calcParam()
param=self.param
# param[i]=(z_i, y_i, x_i, r_i,phi_i)
ctr=param[:,0:3].mean(0)[::-1]
self.axSetCenter(ctr,10)
def setCenter(self,v,l):
axFy = plt.axes([0.1, 0.01, 0.8, 0.02])
axCx = plt.axes([0.1, 0.04, 0.8, 0.02])
axCz = plt.axes([0.1, 0.07, 0.8, 0.02])
axW = plt.axes([0.1, 0.10, 0.8, 0.02])
if self.manip:
lz=ax.get_xlim()
lx=ax.get_ylim()
ly=ax.get_zlim()
else:
lx = ly=lz=[-5,5]
self.sldFy = sFy = Slider(axFy, 'fy', ly[0], ly[1], valinit=(ly[0]+ly[1])/2.)
self.sldCx = sCx = Slider(axCx, 'cx', lx[0], lx[1], valinit=(lx[0]+lx[1])/2.)
self.sldCz = sCz = Slider(axCz, 'cz', lz[0], lz[1], valinit=(lz[0]+lz[1])/2.)
self.sldW =sW = Slider(axW, 'ang', -180., 180.0, valinit=0)
sFy.on_changed(self.update_fy_cx_cz_w)
sCx.on_changed(self.update_fy_cx_cz_w)
sCz.on_changed(self.update_fy_cx_cz_w)
sW.on_changed(self.update_fy_cx_cz_w)
hCrist,pt=self.pltCrist()
self.hCrist=hCrist;self.fig=fig
# param[i]=(z_i, y_i, x_i, r_i,phi_i)
p=np.ndarray((param.shape[0], 3))
for i in range(2):
(z_i, y_i, x_i, r_i, phi_i)=param[i]
p[i,0]=x_i+r_i*np.cos(phi_i) # x= x_i+r_i*cos(phi_i+w)+cx
p[i,1]=y_i # y= y_i
p[i,2] =z_i+r_i*np.sin(phi_i) # z= z_i+r_i*sin(phi_i*w)
print p
ofs=(p[1]+p[0])/2. # = center of the cristal
m=Trf.trans(*ofs); self.hOrig=self.pltOrig(m)
plt.show()
def update_fy_cx_cz_w(self,val):
fy = self.sldFy.val
cx = self.sldCx.val
cz = self.sldCz.val
w = self.sldW.val
if self.manip:
param = self.param
# param[i]=(z_i, y_i, x_i, r_i,phi_i)
p = np.ndarray((param.shape[0], 3))
for i in range(2):
(z_i, y_i, x_i, r_i, phi_i) = param[i]
p[i, 0] = x_i + r_i * np.cos(phi_i) # x= x_i+r_i*cos(phi_i+w)+cx
p[i, 1] = y_i # y= y_i
p[i, 2] = z_i + r_i * np.sin(phi_i) # z= z_i+r_i*sin(phi_i*w)
print p
ofs = (p[1] + p[0]) / 2. # = center of the cristal
m = Trf.trans(cx,fy,cz)
m= m.dot(Trf.rotY(w*d2r))
self.hOrig = self.pltOrig(m,self.hOrig)
else:
self.hCrist,pt=self.pltCrist(fy,cx,cz,w*d2r,self.hCrist)
#l.set_ydata(amp * np.sin(2 * np.pi * freq * t))
self.fig.canvas.draw_idle()
def interactive_y_dx_dz_w(self):
fig = plt.figure()
self.manip=False#True#False
self.ax=ax=plt3d.Axes3D(fig,[0.02, 0.15, 0.96, 0.83])
ax.set_xlabel('Z');ax.set_ylabel('X');ax.set_zlabel('Y')
ax.view_init(elev=14., azim=10)
self.calcParam()
param=self.param
# param[i]=(z_i, y_i, x_i, r_i,phi_i)
ctr=param[:,0:3].mean(0)[::-1]
self.axSetCenter(ctr,10)
axFy = plt.axes([0.1, 0.01, 0.8, 0.02])
axCx = plt.axes([0.1, 0.04, 0.8, 0.02])
axCz = plt.axes([0.1, 0.07, 0.8, 0.02])
axW = plt.axes([0.1, 0.10, 0.8, 0.02])
if self.manip:
lz=ax.get_xlim()
lx=ax.get_ylim()
ly=ax.get_zlim()
else:
lx=[-1,1];ly=[0,1];lz=[-1,1]
self.sldFy = sFy = Slider(axFy, 'y', ly[0], ly[1], valinit=(ly[0]+ly[1])/2.)
self.sldCx = sCx = Slider(axCx, 'dx', lx[0], lx[1], valinit=(lx[0]+lx[1])/2.)
self.sldCz = sCz = Slider(axCz, 'dz', lz[0], lz[1], valinit=(lz[0]+lz[1])/2.)
self.sldW =sW = Slider(axW, 'ang', -180., 180.0, valinit=0)
sFy.on_changed(self.update_y_dx_dz_w)
sCx.on_changed(self.update_y_dx_dz_w)
sCz.on_changed(self.update_y_dx_dz_w)
sW.on_changed(self.update_y_dx_dz_w)
hCrist,pt=self.pltCrist()
self.hCrist=hCrist;self.fig=fig
# param[i]=(z_i, y_i, x_i, r_i,phi_i)
p=np.ndarray((param.shape[0], 3))
for i in range(2):
(z_i, y_i, x_i, r_i, phi_i)=param[i]
p[i,0]=x_i+r_i*np.cos(phi_i) # x= x_i+r_i*cos(phi_i+w)+cx
p[i,1]=y_i # y= y_i
p[i,2] =z_i+r_i*np.sin(phi_i) # z= z_i+r_i*sin(phi_i*w)
print p
ofs=(p[1]+p[0])/2. # = center of the cristal
m=Trf.trans(*ofs); self.hOrig=self.pltOrig(m)
plt.show()
def update_y_dx_dz_w(self,val):
fy = self.sldFy.val
cx = self.sldCx.val
cz = self.sldCz.val
w = self.sldW.val
if self.manip:
param = self.param
# param[i]=(z_i, y_i, x_i, r_i,phi_i)
p = np.ndarray((param.shape[0], 3))
for i in range(2):
(z_i, y_i, x_i, r_i, phi_i) = param[i]
p[i, 0] = x_i + r_i * np.cos(phi_i) # x= x_i+r_i*cos(phi_i+w)+cx
p[i, 1] = y_i # y= y_i
p[i, 2] = z_i + r_i * np.sin(phi_i) # z= z_i+r_i*sin(phi_i*w)
print p
ofs = (p[1] + p[0]) / 2. # = center of the cristal
m = Trf.trans(cx,fy,cz)
m= m.dot(Trf.rotY(w*d2r))
self.hOrig = self.pltOrig(m,self.hOrig)
else:
self.hCrist,pt=self.pltCrist(fy,cx,cz,w*d2r,self.hCrist)
#l.set_ydata(amp * np.sin(2 * np.pi * freq * t))
self.fig.canvas.draw_idle()
def test_coord_trf(self):
plt.ion()
fig = plt.figure()
#self.ax=ax=fig.add_subplot(1,1,1,projection='3d')
self.ax=ax=plt3d.Axes3D(fig,[0.02, 0.02, 0.96, 0.96])
#fig.Axes3DSubplot()
ax.set_xlabel('Z');ax.set_ylabel('X');ax.set_zlabel('Y')
ax.view_init(elev=14., azim=10)
self.axSetCenter((0,5,15),10)
self.calcParam()
param=self.param
hCrist,pt=self.pltCrist()
z = 14.5 # fix z position
#self.hCrist=hCrist
#a=anim.FuncAnimation(fig,self.my_anim_func3,100,fargs=(),interval=20,blit=False)
#plt.show()
#plt.show()
#m=np.identity(4); horig=extAx.pltOrig(m)
m=Trf.trans(0,0,z); self.hOrig=self.pltOrig(m)
#self.fwd_transform(y ,w ,cx ,cz)
# y_0 ,0deg ,x_0 ,z_0)
m=self.fwd_transform(param[0][1],0,pt[2,0],pt[2,2])
m=self.fwd_transform(param[0][1],20*d2r,pt[2,0],pt[2,2])
#m=self.fwd_transform(param[0][1],0,pt[2][0],pt[2][2])
pass
#m=self.fwd_transform(param[0][1],20*d2r,pt[2][0],pt[2][2])
#self.pltOrig(m)
#a=anim.FuncAnimation(fig,self.my_anim_func3,100,fargs=(horig,pt),interval=20,blit=False)
#plt.show()
# y_0 ,120deg ,x_0 ,z_0)
self.fwd_transform(param[0][1],2*np.pi/3.,param[0][2],param[0][0])
#self.fwd_transform(param[1][1],0.,param[1][2],param[1][3])
def my_anim_func3(self,idx):
self.hCrist,pt=self.pltCrist(cx=0,ty=0,cz=0,w=10*idx*d2r,h=self.hCrist)
def my_anim_func2(self,idx, horig,pt):
#print('my_anim_func2%d'%idx)
pIdx, aIdx= divmod(idx, 25)
a = aIdx/25. * 2. * np.pi
#print(aIdx,a)
m = Trf.trans(*pt[pIdx])
m = m=m.dot(Trf.rotY(a))
self.pltOrig(m, horig)
def my_anim_func(self,idx, horig):
print('my_anim_func%d'%idx)
a = idx * .01 * 2 * np.pi
m = Trf.rotY(a)
self.pltOrig(m, horig)
def fwd_transform(self,y,w,cx,cz):
#cx,cy: coarse stage
#input: y,w,cx,cz
#output: y,w,dx,dz
# param[i]=(z_i, y_i, x_i, r_i,phi_i)
param=self.param
p=np.ndarray((param.shape[0], 3))
for i in range(2):
(z_i, y_i, x_i, r_i, phi_i)=param[i]
p[i,0]=x_i+r_i*np.cos(phi_i+w) # x= x_i+r_i*cos(phi_i+w)+cx
p[i,1]=y_i # y= y_i
p[i,2] =z_i+r_i*np.sin(phi_i+w) # z= z_i+r_i*sin(phi_i*w)
print p
v=p[1]-p[0]
v=v/np.sqrt(v.dot(v)) # v/|v|
y_0=param[0][1]
y_1=param[1][1]
v=v*(y-y_0)/(y_1-y_0) # v(y)=v*(v-y_0)/(y_1-y_0)
v=p[0]+v
dx=cx-v[0]
dz=cz-v[2]
res=(y,w,dx,dz)
print res
m=Trf.trans(cx,y,cz)
m=m.dot(Trf.rotY(w))
self.pltOrig(m,self.hOrig)
self.axSetCenter(m[0:3,3],10)
return res
def inv_transform(self,y,w,dx=0,dz=0):
#input: y,w,dx,dz
#output: y,w,cx,cz
#dx,dy: deviation from cristal center line
param=self.param
p=np.ndarray((param.shape[0], 3))
for i in range(2):
(z_i, y_i, x_i, r_i, phi_i)=param[i]
p[i,0]=x_i+r_i*np.cos(phi_i+w) # x= x_i+r_i*cos(phi_i+w)+cx
p[i,1]=y_i # y= y_i
p[i,2] =z_i+r_i*np.sin(phi_i+w) # z= z_i+r_i*sin(phi_i*w)
print p
v=p[1]-p[0]
v=v/np.sqrt(v.dot(v)) # v/|v|
y_0=param[0][1]
y_1=param[1][1]
v=v*(y-y_0)/(y_1-y_0) # v(y)=v*(v-y_0)/(y_1-y_0)
v=p[0]+v
dx=cx-v[0]
dz=cz-v[2]
res=(y,w,dx,dz)
print res
pass
@staticmethod
def meas_rot_ctr(y,per=1):
# find the amplitude bias and phase of an equidistant sampled sinus
# it needs at least 3 measurements e.g. at 0,120 240 deg or 0 90 180 270 deg
# per is the number of persiods, default is 1 period =360 deg
n=len(y)
f = np.fft.fft(y)
idx=int(per)
bias=np.absolute(f[0]/n)
phase=np.angle(f[idx])
ampl=np.absolute(f[idx])*2/n
return (bias,ampl,phase)
@staticmethod
def test_find_rot_ctr(n=3.,per=1.,bias=4.1,ampl=2.4,phi=37):
# find the rotation center, amplitude out of n (niminum 3) measurements
# n number of equidistant measurements
# per number of periods (full rotation of all measurements nut be a interger value for precise measurements)
# phi phase
# bias bias value
# ampl amplitude
t = np.arange(n)
w=2*np.pi*per/n*t
y=ampl*np.cos(w+phi*d2r)+bias
plt.figure(1)
plt.subplot(311)
plt.plot(t,y,'b.-')
plt.subplot(312)
f = np.fft.fft(y)
plt.step(t, f.real,'b.-', t, f.imag,'r.-', where='mid')
(bias,ampl,phase)=HelicalScan.meas_rot_ctr(y, per)
print('bias: '+str(bias))
print('amplitude: '+str(ampl))
print('phase: '+str(phase*360./2/np.pi))
plt.subplot(313)
t2 = np.linspace(0,2*np.pi,64)
y2=ampl*np.cos(t2+phase)+bias
plt.plot(t2,y2,'g-')
plt.stem(w,y,'b-')
plt.show()
pass
def axSetCenter(self,v,l):
ax=self.ax
#v=center vector, l= length of each axis
l2=l/2.
@@ -63,209 +499,11 @@ class ExtAxes():#mpl.axes._subplots.Axes3DSubplot):
ax.set_ylim(v[0]-l2, v[0]+l2);
ax.set_zlim(v[1]-l2, v[1]+l2)
def pltOrig(self,m):
ax=self.ax
# m is a 4x4 matrix. the transformed matrix
r=m[:3,0] #1st
g=m[:3,1] #2nd
b=m[:3,2] #3rd
o=m[:3,3] #origin
hr=ax.plot((o[2],o[2]+r[2]), (o[0],o[0]+r[0]), (o[1],o[1]+r[1]), 'r')
hg=ax.plot((o[2],o[2]+g[2]), (o[0],o[0]+g[0]), (o[1],o[1]+g[1]), 'g')
hb=ax.plot((o[2],o[2]+b[2]), (o[0],o[0]+b[0]), (o[1],o[1]+b[1]), 'b')
return hr+hg+hb # this is a list
def my_anim_func(idx,horig):
def my_anim_func(idx,hs,horig):
print('anim')
a=idx*.01*2*np.pi
m=Trf.rotY(a)
r = m[:3, 0] # 1st
g = m[:3, 1] # 2nd
b = m[:3, 2] # 3rd
o = m[:3, 3] # origin
hr,hg,hb=horig
hr.set_data((o[2], o[2] + r[2]), (o[0], o[0] + r[0]))
hr.set_3d_properties((o[1], o[1] + r[1]))
hg.set_data((o[2], o[2] + g[2]), (o[0], o[0] + g[0]))
hg.set_3d_properties((o[1], o[1] + g[1]))
hb.set_data((o[2], o[2] + b[2]), (o[0], o[0] + b[0]))
hb.set_3d_properties((o[1], o[1] + b[1]))
class HelicalScan:
def __init__(self,args):
if args.cfg:
fh=open(args.cfg,'r')
s=fh.read()
cfg=json.loads(s, object_hook=ConvUtf8)
s=json.dumps(cfg, indent=2, separators=(',', ': '));print(s)
else:
fn='/tmp/shapepath4'
#fn='/home/zamofing_t/Documents/prj/SwissFEL/epics_ioc_modules/ESB_MX/python/data/'+time.strftime('%y-%m-%d-%H_%M_%S')
#cfg = {"sequencer": ['gen_grid_points(w=5,h=5,pitch=100,rnd=0.4)', 'sort_points()','gen_prog(file="'+fn+'.prg",host="SAR-CPPM-EXPMX1",mode=1,pt2pt_time=10,cnt=1)', 'plot_gather("'+fn+'.npz")']}
#cfg = {"sequencer": ['test_find_rot_ctr()']}
#cfg = {"sequencer": ['test_find_rot_ctr(n=5. ,per=1.,bias=2.31,ampl=4.12,phi=24.6)']}
cfg = {"sequencer": ['test_coord_trf()']}
self.cfg=dotdict(cfg)
self.args=args
def run(self):
print('args='+str(self.args))
print('cfg='+str(self.cfg))
#try:
# self.points=np.array(self.cfg.points)
#except AttributeError:
# pass
try:
sequencer= self.cfg.pop('sequencer')
except KeyError:
print('no command sequence to execute')
else:
dryrun=self.args.dryrun
for cmd in sequencer:
print('>'*5+' '+cmd+' '+'<'*5)
if not dryrun:
eval('self.' + cmd)
def test_coord_trf(self):
d2r=2*np.pi/360
plt.ion()
fig = plt.figure()
#ax = fig.gca(projection='3d')
ax = fig.add_subplot(1,1,1,projection='3d')
extAx=ExtAxes(ax)
extAx.setCenter((0,5,15),10)
n = 3.; per = 1.; w = 2*np.pi*per/n*np.arange(n)
p=((2.3,0.71,4.12,10.6*d2r),(6.2,.45,3.2,45.28*d2r)) #(y, bias, ampl, phi)
self.param=param=np.ndarray((len(p),5))
z=14.5 # fix z position
pt=np.ndarray((4,3))
for i in range(2):
(y, bias, ampl, phi) =p[i]
x= ampl * np.cos(w+phi) + bias
print('yMeas_%d='%i+str(y)+' xMeas_%d='%i+str(x))
#param[i]=(z_i, y_i, x_i, r_i,phi_i)
param[i,0] =z
param[i,1] =y
param[i,2:]=HelicalScan.meas_rot_ctr(x) #(bias,ampl,phase)
(bias, ampl, phase)=param[i][2:]
pt[i]=(bias, y, z)
pt[i+2]=(bias+ampl*np.cos(phase),y, z+ampl*np.sin(phase))
obj = mpl.patches.Circle((z,bias), ampl, facecolor=mpl.colors.colorConverter.to_rgba('r', alpha=0.3))
ax.add_patch(obj)
plt3d.art3d.pathpatch_2d_to_3d(obj, z=y, zdir="z")
ax.scatter(pt[:,2], pt[:,0], pt[:,1])
ax.plot(pt[2:,2], pt[2:,0], pt[2:,1], label='zs=0, zdir=z')
print param
#plt.show()
#m=np.identity(4); horig=extAx.pltOrig(m)
m=Trf.trans((0,0,z)); horig=extAx.pltOrig(m)
#self.fwd_transform(y ,w ,cx ,cz)
# y_0 ,0deg ,x_0 ,z_0)
m=self.fwd_transform(param[0][1],0,pt[2][0],pt[2][2],extAx)
m=self.fwd_transform(param[0][1],0,pt[2][0],pt[2][2],extAx)
m=self.fwd_transform(param[0][1],20*d2r,pt[2][0],pt[2][2])
extAx.pltOrig(m)
#my_anim_func(0,horig)
a=anim.FuncAnimation(fig,my_anim_func,25,fargs=(horig,),interval=50,blit=False)
plt.show()
# y_0 ,120deg ,x_0 ,z_0)
self.fwd_transform(param[0][1],2*np.pi/3.,param[0][2],param[0][0])
#self.fwd_transform(param[1][1],0.,param[1][2],param[1][3])
def fwd_transform(self,y,w,cx,cz,extAx):
#cx,cy: coarse stage
#input: y,w,cx,cz
#output: y,w,dx,dz
m=Trf.trans((cx,y,cz))
m=m.dot(Trf.rotY(w))
extAx.pltOrig(m)
extAx.setCenter(m[0:3,3],1)
#TODO: NOT WORKING AT ALL NOW...
param=self.param
# param[i]=(z_i, y_i, x_i, r_i,phi_i)
p=np.ndarray((param.shape[0], 3))
for i in range(2):
(z_i, y_i, x_i, r_i, phi_i)=param[i]
p[i,0]=x_i+r_i*np.cos(phi_i+w) # x= x_i+r_i*cos(phi_i+w)+cx
p[i,1]=y_i # y= y_i
p[i,2] =z_i+r_i*np.sin(phi_i+w) # z= z_i+r_i*sin(phi_i*w)
print p
v=p[1]-p[0]
v=v/np.sqrt(v.dot(v)) # v/|v|
y_0=param[0][1]
y_1=param[1][1]
v=v*(y-y_0)/(y_1-y_0) # v(y)=v*(v-y_0)/(y_1-y_0)
v=p[0]+v
cz + cx
#v=v/abs(v)
print v
return m
def inv_transform(y,phi,dx=0,dz=0):
#input: y,w,dx,dz
#output: y,w,cx,cz
m=np.identity(4)
#dx,dy: deviation from cristal center line
#ps= #x,y,z
#returns y,phi,cx,cz
pass
@staticmethod
def meas_rot_ctr(y,per=1):
# find the amplitude bias and phase of an equidistant sampled sinus
# it needs at least 3 measurements e.g. at 0,120 240 deg or 0 90 180 270 deg
# per is the number of persiods, default is 1 period =360 deg
n=len(y)
f = np.fft.fft(y)
idx=int(per)
bias=np.absolute(f[0]/n)
phase=np.angle(f[idx])
ampl=np.absolute(f[idx])*2/n
return (bias,ampl,phase)
@staticmethod
def test_find_rot_ctr(n=3.,per=1.,bias=4.1,ampl=2.4,phi=37):
# find the rotation center, amplitude out of n (niminum 3) measurements
# n number of equidistant measurements
# per number of periods (full rotation of all measurements nut be a interger value for precise measurements)
# phi phase
# bias bias value
# ampl amplitude
d2r=2*np.pi/360
t = np.arange(n)
w=2*np.pi*per/n*t
y=ampl*np.cos(w+phi*d2r)+bias
plt.figure(1)
plt.subplot(311)
plt.plot(t,y,'b.-')
plt.subplot(312)
f = np.fft.fft(y)
plt.step(t, f.real,'b.-', t, f.imag,'r.-', where='mid')
(bias,ampl,phase)=HelicalScan.meas_rot_ctr(y, per)
print('bias: '+str(bias))
print('amplitude: '+str(ampl))
print('phase: '+str(phase*360./2/np.pi))
plt.subplot(313)
t2 = np.linspace(0,2*np.pi,64)
y2=ampl*np.cos(t2+phase)+bias
plt.plot(t2,y2,'g-')
plt.stem(w,y,'b-')
plt.show()
pass
hs.pltOrig(m,horig)
if __name__=='__main__':
from optparse import OptionParser, IndentedHelpFormatter
@@ -302,8 +540,11 @@ Examples:'''+''.join(map(lambda s:cmd+s, exampleCmd))+'\n '
(args, other)=parser.parse_args()
args.other=other
sp=HelicalScan(args)
sp.run()
hs=HelicalScan(args)
#hs.sequencer()
hs.interactive_fy_cx_cz_w()
hs.interactive_y_dx_dz_w()
#hs.test_coord_trf()
#------------------ Main Code ----------------------------------
#ssh_test()
ret=parse_args()