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

fix display. Behaviour for motion remains compatible.

Browse Source
This commit is contained in:
zamofing_t
2018-10-26 15:57:31 +02:00
parent 00c84235a0
commit 9c8e2a417e
4 changed files with 1524 additions and 258 deletions
Download Patch File Download Diff File Expand all files Collapse all files

474
python/helicalscan.py
View File

@@ -20,23 +20,42 @@ Mot 5: Stage Z Stada Stepper 670mA 200 poles 1 rev = 100*2
Enc 6: Interferometer Y
Enc 7: Interferometer X
verbose bits:
#1 basic info
#2 plot sorting steps
4 list program
#4 upload progress
#8 plot gather path
'''
#motors CX CZ RY FY
# 4 5 3 1
#Dir. - - + +
#There are 2 coordinate systems
#rhs =right hand side coord: system osed in the python code
#pb = power brick coord: cx,cz have inverted directions
#the structures have following conventions:
# param -> pb coord
# rec -> pb coord
# code -> pb coord
# points -> pb coord
#ax.set_xlabel('Z');ax.set_ylabel('X');ax.set_zlabel('Y')
#plot coordinates: X Y Z
#data Z X Y
#param:
#(z_i, y_i, x_i, r_i,phi_i) -> pb coord. be aware of inverted z_i,x_i signs
#rec:
# cx,cz,w,fy -> pb coord. be aware of inverted cx,cz signs
#points:
#dx,dz,w,fy -> pb coord. be aware of inverted dx,dz signs
import os, sys, json,re
import numpy as np
import matplotlib as mpl
@@ -56,10 +75,7 @@ from pbtools.misc.gather import Gather
from MXMotion import MotionBase
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
np.set_printoptions(formatter={'float':lambda x:"{:12.6g}".format(x)})
class Trf:
#https://stackoverflow.com/questions/6802577/python-rotation-of-3d-vector
@@ -184,17 +200,17 @@ class HelicalScanGui():
if manip:
y0=param[:, 1].mean()
ly=sorted(param[:,1])
x0=param[:, 2].mean()
x0=-param[:, 2].mean()
lx=(-l/2+x0,l/2+x0)
z0=param[:, 0].mean()
z0=-param[:, 0].mean()
lz=(-l/2+z0,l/2+z0)
self.axSetCenter((x0,y0,z0),l)
self.axSetCenter((-x0,-y0,-z0),l)
else:
self.axSetCenter((0, 0, 0), l)
ly=sorted(param[:,1])
x0=param[:, 2].mean()
x0=-param[:, 2].mean()
lx=(-l/2+x0,l/2+x0)
z0=param[:, 0].mean()
z0=-param[:, 0].mean()
lz=(-l/2+z0,l/2+z0)
self.sldCx=sCx=Slider(axCx, 'cx', lx[0], lx[1], valinit=(lx[0]+lx[1])/2.)
@@ -225,15 +241,14 @@ class HelicalScanGui():
# param[i]=(z_i, y_i, x_i, r_i,phi_i)
#rx=param[:,2].mean();rz=param[:,0].mean()
#precise caldulation of rotation center dependent of y
(z0, y0, x0, r0, phi0)=param[0]
(z1, y1, x1, r1, phi1)=param[1]
(z0, y0, x0, r0, phi0)=param[0];x0=-x0;z0=-z0
(z1, y1, x1, r1, phi1)=param[1];x1=-x1;z1=-z1
rx = x0+(x1-x0)/(y1-y0)*(fy-y0)
rz = z0+(z1-z0)/(y1-y0)*(fy-y0)
m=Trf.trans(rx,fy,rz).dot(Trf.rotY(-w).dot(Trf.trans(cx-rx,0,cz-rz)))
m=Trf.trans(-rx,-fy,-rz).dot(Trf.rotY(-w).dot(Trf.trans(rx-cx,0,rz-cz)))
self.pltOrig(m)
else:
self.pltCrist(-cx, -cz, w, -fy)
#self.pltCrist(cx,cz,w*d2r,fy)
self.pltCrist(cx, cz, w, fy)
self.fig.canvas.draw_idle()
def interactive_dx_dz_w_y(self,manip=False):
@@ -255,17 +270,15 @@ class HelicalScanGui():
if manip:
y0=param[:, 1].mean()
ly=sorted(param[:,1])
x0=param[:, 2].mean()
x0=-param[:, 2].mean()
lx=(-l/2,l/2)
z0=param[:, 0].mean()
z0=-param[:, 0].mean()
lz=(-l/2,l/2)
self.axSetCenter((x0,y0,z0),l)
self.axSetCenter((-x0,-y0,-z0),l)
else:
self.axSetCenter((0, 0, 0), l)
ly=sorted(param[:,1])
x0=param[:, 2].mean()
lx=(-l/2,l/2)
z0=param[:, 0].mean()
lz=(-l/2,l/2)
self.sldDx=sDx=Slider(axDx, 'dx', lx[0], lx[1], valinit=(lx[0]+lx[1])/2.)
@@ -296,14 +309,14 @@ class HelicalScanGui():
param = self.helScn.param
# param[i]=(z_i, y_i, x_i, r_i,phi_i)
#precise caldulation of rotation center dependent of y
(z0, y0, x0, r0, phi0)=param[0]
(z1, y1, x1, r1, phi1)=param[1]
(z0, y0, x0, r0, phi0)=param[0];x0=-x0;z0=-z0
(z1, y1, x1, r1, phi1)=param[1];x1=-x1;z1=-z1
rx = x0+(x1-x0)/(y1-y0)*(fy-y0)
rz = z0+(z1-z0)/(y1-y0)*(fy-y0)
m=Trf.trans(rx,fy,rz).dot(Trf.rotY(-w).dot(Trf.trans(cx-rx,0,cz-rz)))
m=Trf.trans(-rx,-fy,-rz).dot(Trf.rotY(-w).dot(Trf.trans(rx-cx,0,rz-cz)))
self.pltOrig(m)
else:
self.pltCrist(-cx,-cz,w,-fy)
self.pltCrist(cx,cz,w,fy)
self.fig.canvas.draw_idle()
def interactive_anim(self,manip=False):
@@ -321,10 +334,10 @@ class HelicalScanGui():
axFrm=plt.axes([0.1, 0.01, 0.8, 0.02])
if manip:
y0=param[:, 1].mean()
x0=param[:, 2].mean()
z0=param[:, 0].mean()
self.axSetCenter((x0,y0,z0),l)
y0= param[:, 1].mean()
x0=-param[:, 2].mean()
z0=-param[:, 0].mean()
self.axSetCenter((-x0,-y0,-z0),l)
else:
self.axSetCenter((0, 0, 0), l)
@@ -341,27 +354,24 @@ class HelicalScanGui():
def update_anim(self,frm):
rec=self.helScn.rec
(cx, cz, w, fy)=rec[int(frm),:4]
#data/=. #scale from um to mm
(cx, cz, w, fy)=rec[int(frm),:4];cx=-cx;cz=-cz
w*=d2r/1000 # scale from deg to rad
if self.manip:
param = self.helScn.param
# param[i]=(z_i, y_i, x_i, r_i,phi_i)
#rx=param[:,2].mean();rz=param[:,0].mean()
#precise caldulation of rotation center dependent of y
(z0, y0, x0, r0, phi0)=param[0]
(z1, y1, x1, r1, phi1)=param[1]
(z0, y0, x0, r0, phi0)=param[0];x0=-x0;z0=-z0
(z1, y1, x1, r1, phi1)=param[1];x1=-x1;z1=-z1
rx = x0+(x1-x0)/(y1-y0)*(fy-y0)
rz = z0+(z1-z0)/(y1-y0)*(fy-y0)
m=Trf.trans(rx,fy,rz).dot(Trf.rotY(-w).dot(Trf.trans(cx-rx,0,cz-rz)))
m=Trf.trans(-rx,-fy,-rz).dot(Trf.rotY(-w).dot(Trf.trans(rx-cx,0,rz-cz)))
self.pltOrig(m)
else:
self.pltCrist(-cx,-cz,w,-fy)
self.pltCrist(cx,cz,w,fy)
self.fig.canvas.draw_idle()
def anim_gather_data(self,idx):
rec=self.helScn.rec
(cx, cz, w, fy)=rec[int(idx*self.step),:]
(cx, cz, w, fy)=rec[int(idx*self.step),:];cx=-cx;cz=-cz
w*=d2r/1000 # scale from deg to rad
self.hCrist,pt=self.pltCrist(-cx,-cz,w,-fy,self.hCrist)
#data=self.rec[int(idx*self.step),:]
@@ -406,8 +416,8 @@ class HelicalScanGui():
except AttributeError:
h=[] #handels
for i in range(2):
(z, y, x, r, phi) = param[i]
x+=cx;y+=fy;z+=cz;phi+=w
(z, y, x, r, phi) = param[i];z=-z;x=-x
x=cx-x;y=fy-y;z=cz-z;phi=phi+w
pt[i] = (z, x, y)
pt[i + 2] = (z + r * np.cos(phi), x + r * np.sin(phi), y)
obj = mpl.patches.Circle((z, x), r, facecolor=mpl.colors.colorConverter.to_rgba('y' if i==0 else 'r', alpha=0.2))
@@ -420,7 +430,7 @@ class HelicalScanGui():
hp=ax.plot(pt[2:, 0], pt[2:, 1], pt[2:, 2])
h+=(hs[0],hp[0])
if hasattr(helScn,'points'):
lines = self.get_meas_lines(pt,cx,cz,fy,w)
lines = self.get_meas_lines(pt,w)
p=tuple(lines[:,0,:].T)
hl =plt3d.art3d.Line3D(*p,color='r',marker='.')#, *args[argi:], **kwargs)
ax.add_artist(hl);h.append(hl)
@@ -432,8 +442,8 @@ class HelicalScanGui():
self.hCrist=h
else:
for i in range(2):
(z, y, x, r, phi) = param[i]
x+=cx;y+=fy;z+=cz;phi+=w
(z, y, x, r, phi) = param[i];z=-z;x=-x
x=cx-x;y=fy-y;z=cz-z;phi=phi+w
pt[i] = (z, x, y)
pt[i + 2] = (z + r * np.cos(phi), x + r * np.sin(phi), y)
h[i].remove()
@@ -447,7 +457,7 @@ class HelicalScanGui():
h[3].set_data(pt[2:, 0], pt[2:, 1])#, pt[:, 1]))
h[3].set_3d_properties(pt[2:, 2])
if hasattr(helScn,'points'):
lines = self.get_meas_lines(pt,cx,cz,fy,w)
lines = self.get_meas_lines(pt,w)
hl=h[4]
#hl.set_data(lines[:,0,0], lines[:,0,1]) # , pt[:, 1]))
#hl.set_3d_properties(lines[:,0,2])
@@ -459,7 +469,11 @@ class HelicalScanGui():
return (h,pt)
def get_meas_lines(self,pt,cx,cz,fy,w,arwLen=None):
def get_meas_lines(self,pt,w,arwLen=None):
#pt are the rot-centers,starting,ending points of the crystal
#w is the rotation angle
#the measurement points are used from self.helScn.points
#params is used from self.helScn.param
if arwLen is None:
arwLen=self.scale*.05 # default arrow length=0.01 of crystal size
param = self.helScn.param
@@ -468,16 +482,13 @@ class HelicalScanGui():
dz_ = pts[:, 1] # add 0.2 to test
w_ = pts[:, 2] * (d2r / 1000.)
y_ = pts[:, 3]
# self.inv_transform(self, dx, dz, w, y):
f = (pts[:, 3] - param[0, 1]) / (param[1, 1] - param[0, 1])
f = (pts[:, 3] - param[0, 1]) / (param[1, 1] - param[0, 1]) #part of y move: 0..1
lines = np.ndarray((pts.shape[0], 2, 3))
ofx=dx_*-np.sin(w-w_)+dz_*np.cos(w-w_) # 0.2=dx
ofy=dx_*np.cos(w-w_)+dz_*np.sin(w-w_)
lines[:, 0, 0] = pt[2, 0] + (pt[3, 0] - pt[2, 0]) * f +ofx # z data
lines[:, 0, 1] = pt[2, 1] + (pt[3, 1] - pt[2, 1]) * f +ofy # x data
lines[:, 0, 2] = pts[:, 3] + fy # y data
lines[:, 0, 2] = param[0, 1]+pt[0, 2] -pts[:, 3] # y data
lines[:, 1, 0] = lines[:, 0, 0] + np.cos(w-w_)*arwLen
lines[:, 1, 1] = lines[:, 0, 1] + np.sin(w-w_)*arwLen
lines[:, 1, 2] = lines[:, 0, 2]
@@ -494,18 +505,17 @@ class HelicalScanGui():
class HelicalScanTests():
@staticmethod
def test_find_rot_ctr(n=3.,per=1.,bias=4.1,ampl=2.4,phi=37):
def test_find_rot_ctr(n=3.,per=1.,bias=4.1,ampl=2.4,phi=37*d2r):
# 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
# phi phase (in rad)
# 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)
y=bias+ampl*np.sin(w+phi)
plt.figure('test_find_rot_ctr')
plt.subplot(311)
plt.plot(t,y,'b.-')
@@ -513,19 +523,20 @@ class HelicalScanTests():
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))
inp=np.array((bias,ampl,phi))
(bias_,ampl_,phi_)=out=HelicalScan.meas_rot_ctr(y, per)
print('input bias:{} amplitude:{} phase:{: .4f}'.format(bias,ampl,phi/d2r))
print('output bias:{} amplitude:{} phase:{: .4f}'.format(bias_,ampl_,phi_/d2r))
assert(np.abs((inp-out)).max()<1E-10)
plt.subplot(313)
t2 = np.linspace(0,2*np.pi,64)
y2=ampl*np.cos(t2+phase)+bias
y2=bias+ampl*np.sin(t2+phi_)
plt.plot(t2,y2,'g-')
plt.stem(w,y,'b-')
plt.show()
pass
@staticmethod
def test_coord_trf(helScn):
@@ -575,31 +586,54 @@ class HelicalScanTests():
pass
@staticmethod
def calcParamSim():
def calcParamSim(hs):
#simulated test values
n = 3.;
per = 1.;
w = 2 * np.pi * per / n * np.arange(n)
#(z_i, y_i, x_i, r_i, phi_i)=param[i]
# param[i]=(z_i, y_i, x_i, r_i,phi_i)
#paramSim = ((14.5, 2.3, .71, 4.12, 10.6 * d2r),(14.5, 6.2, .45, 3.2, 45.28 * d2r)) # (y, bias, ampl, phi)
#paramSim = ((14.5, 2.3, -100., 10, 10. * d2r),(14.5, 6.2, 100., 10., -10. * d2r)) # (y, bias, ampl, phi)
paramSim = ((14.5, 2.3, -100., 10, 0. * d2r),(14.5, 6.2, 100., 10., 90. * d2r)) # (y, bias, ampl, phi)
#paramSim = ((14.5, 2.3, 102., 15, 0. * d2r),(14.8, 16.2, 103.4, 10., 90. * d2r)) # (y, bias, ampl, phi)
paramSim = ((-945., 730., -102., 150, 10. * d2r),(-958., 327., -108, 202., 90. * d2r)) # (y, bias, ampl, phi)
paramSim=np.array(paramSim)
param = np.ndarray(paramSim.shape)
for i in range(param.shape[0]):
(z,y,bias,ampl,phi) = paramSim[i]
measX = ampl * np.sin(w + phi) + bias
measZ = ampl * np.cos(w + phi) + bias
print('xMeas_%d=' % i + str(measX) + ' zMeas_%d=' % i + str(measZ))
# 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(measX) # (bias,ampl,phase)
(bias, ampl, phase) = param[i][2:]
print(param)
measX=np.ndarray((paramSim.shape[0],w.size))
measZ=np.ndarray((paramSim.shape[0],w.size))
for i in range(paramSim.shape[0]):
(z_i,y_i,x_i,r_i,phi_i) = paramSim[i]
measX[i,:] = x_i+r_i*np.sin(w+phi_i)
measZ[i,:] = z_i+r_i*np.cos(w+phi_i)
if i==0:
print('measX='+str(measX[i,:])+' measZ='+str(measZ[i,:]))
else:
print(' '+str(measX[i,:])+' '+str(measZ[i,:]))
measY=paramSim[:, 1]
print('measY='+str(measY))
hs.calcParam(measX,measY,measZ)
print(hs.param)
print(paramSim)
assert((paramSim-param).max()<1E-10)
return param
assert(abs(paramSim-hs.param).max()<1E-10)
inp=np.array(((0,0,0, measY[0]),
(0,0,np.pi*2/3, measY[0]),
(0,0,np.pi*4/3, measY[0]),
(0,0,0, measY[1]),
(0,0,np.pi*2/3, measY[1]),
(0,0,np.pi*4/3, measY[1])))
outCX=(measX[0,0],measX[0,1],measX[0,2],measX[1,0],measX[1,1],measX[1,2])
outCZ=(measZ[0,0],measZ[0,1],measZ[0,2],measZ[1,0],measZ[1,1],measZ[1,2])
for i in range(inp.shape[0]):
(dx,dz,w_,y_) = inp[i,:]
print('input : dx:%.6g dz:%.6g w:%.6g fy:%.6g' % (dx,dz,w_/d2r*1000.,y_))
(cx,cz,w,fy) = out = hs.inv_transform(dx,dz,w_,y_)
#assert(abs(cx-outCX[i])<1E-10)
#assert(abs(cz-outCZ[i])<1E-10)
print('inv_trf: cx:%.6g cz:%.6g w:%.6g fy:%.6g' % (cx,cz,w/d2r*1000.,fy))
(dx,dz,w_,y_)=out= hs.fwd_transform(cx,cz,w,fy)
print('fwd_trf: dx:%.6g dz:%.6g w:%.6g y:%.6g' % (dx,dz,w_/d2r*1000.,y_))
assert(np.abs(inp[i,:]-out).max()<1E-10)
class HelicalScan(MotionBase):
@@ -628,10 +662,10 @@ class HelicalScan(MotionBase):
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.sin(phi_i+w) # x= x_i+r_i*cos(phi_i+w)+cx
(z_i, y_i, x_i, r_i, phi_i)=param[i];z_i=-z_i;x_i=-x_i
p[i,0]=x_i-r_i*np.sin(w+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.cos(phi_i+w) # z= z_i+r_i*sin(phi_i*w)
p[i,2]=z_i-r_i*np.cos(w+phi_i) # z= z_i+r_i*sin(phi_i*w)
v=p[1]-p[0]
#for y = 0..1:
#v=v*y
@@ -652,10 +686,10 @@ class HelicalScan(MotionBase):
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.sin(phi_i+w) # x= x_i+r_i*cos(phi_i+w)+cx
(z_i, y_i, x_i, r_i, phi_i)=param[i];z_i=-z_i;x_i=-x_i
p[i,0]=x_i-r_i*np.sin(w+phi_i)
p[i,1]=y_i # y= y_i
p[i,2]=z_i+r_i*np.cos(phi_i+w) # z= z_i+r_i*sin(phi_i*w)
p[i,2]=z_i-r_i*np.cos(w+phi_i)
v=p[1]-p[0]
#for y = 0..1:
#v=v*y
@@ -672,23 +706,11 @@ class HelicalScan(MotionBase):
def calcParam(self,x=((-241.,96.,-53.),(-162.,-293.,246.)),
y=(575.,175.),
z=((-1401.,-1401.,-1802.),(-1802.,-1303.,-1402.))):
# #1,3,4,5p
# point 1 0,120,240 deg
# 575.5 0 -241.5 -1401.3
# 575.5 120000 96.7 -1401.7
# 575.5 240000 -53.8 -1802.4
#
# point 2 0,120,240 deg
# 175.5 0 -162.3 -1802.5
# 175.5 120000 -293.2 -1303.7
# 175.5 240000 246.4 -1402.25
#real measured values:
#y : 2x1 array : y position were the measurements were taken
#x : 3x2 array : 3 measurements at angle 0,120,240 for y[0] and y[1]
#z : 3x2 array : 3 measurements at angle 0,120,240 for y[0] and y[1]
# the z value is only used to find a rought bias of z
assert(len(y)==2)
n = float(len(x[0])) #number of angles
per = 1 #number of rotations
@@ -700,35 +722,26 @@ class HelicalScan(MotionBase):
param[i, 2:] = HelicalScan.meas_rot_ctr(x[i]) # (bias,ampl,phase)
(bias, ampl, phase) = param[i][2:]
#check correctness of center:
w = 2 * np.pi * per / n * np.arange(n)
x_ = ampl * np.cos(w + phase) + bias
print(x_)
(dx,dz,w,y_) = (0,0,0,y[0])
print('input : dx:%.6g dz:%.6g w:%.6g fy:%.6g' % (dx,dz,w/d2r*1000.,y_))
(cx,cz,w,fy) = self.inv_transform(dx,dz,w,y_)
print('inv_trf: cx:%.6g cz:%.6g w:%.6g fy:%.6g' % (cx,cz,w/d2r*1000.,fy))
(dx, dz, w, y_) = (0,0,0,y[1])
print('input : dx:%.6g dz:%.6g w:%.6g fy:%.6g' % (dx, dz, w / d2r * 1000., y_))
(cx, cz, w, fy) = self.inv_transform(dx, dz, w, y_)
print('inv_trf: cx:%.6g cz:%.6g w:%.6g fy:%.6g' % (cx, cz, w / d2r * 1000., fy))
print(param)
# check correctness of center:
w=2*np.pi*per/n*np.arange(n)
x_=bias+ampl*np.sin(w+phase)
#print(x_)
assert ((x[i]-x_).max()<1E-10)
@staticmethod
def meas_rot_ctr(y,per=1):
# find the amplitude bias and phase of an equidistant sampled sinus
# find the amplitude bias and phase of an equidistant sampled values
# the phase is amout the matching sine is shifted to the left (delayed) in radians
# 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)
f = np.fft.rfft(y)
idx=int(per)
#bias=np.absolute(f[0]/n)
assert(np.imag(f[0])==0.)
bias=np.real(f[0]/n)
#phase=np.angle(f[idx]) #f(w)=bias+ampl*cos(w+phase)
phase=np.angle(f[idx]*1j) #f(w)=bias+ampl*sin(w+phase)
phase=np.angle(f[idx]*1j) #f(w)=bias+ampl*sin(w+phase)
ampl=np.absolute(f[idx])*2/n
return (bias,ampl,phase)
@@ -739,6 +752,7 @@ class HelicalScan(MotionBase):
acq_per : acquire period: acquire data all acq_per servo loops (default=1)
'''
comm=self.comm
if comm is None:return
gt=self.gather
gt.set_phasemode(False)
#gt.set_address("Motor[4].ActPos","Motor[5].ActPos","Motor[3].ActPos","Motor[1].ActPos")
@@ -749,8 +763,6 @@ class HelicalScan(MotionBase):
self.meta = {'timebase': ServoPeriod*acq_per}
def setup_coord_trf(self,fnCrdTrf='/tmp/coordTrf.cfg'):
comm = self.comm
gpascii = comm.gpascii
param=self.param
prg = '''
// Set the motors as inverse kinematic axes in CS 1
@@ -768,13 +780,17 @@ a
#3->I
#1->I
'''
pbParam=param.copy()
pbParam[:,(0,2)]=-pbParam[:,(0,2)] #change sign of x_0,x_1,z_0,z_1, because axes have neg direction
sh=pbParam.shape
s = ('z', 'y', 'x', 'r', 'phi')
a = np.ones(param.shape[1], dtype=np.uint8).reshape(1, -1)
b = np.arange(param.shape[0], dtype=np.uint8).reshape(1, -1)
a = np.ones(sh[1], dtype=np.uint8).reshape(1, -1)
b = np.arange(sh[0], dtype=np.uint8).reshape(1, -1)
c = np.matmul(b.T, a).reshape(-1)
subsParam=dict(map(lambda k, i, v: (k + '_' + str(i), v), s * param.shape[0], c, param.reshape(-1)))
subsParam=dict(map(lambda k, i, v: (k + '_' + str(i), v), s * sh[0], c, pbParam.reshape(-1)))
subsParam['d2r']=d2r/1000.
subsParam['r2d']=1000./d2r
subsParam['cmt']='//'
subs={'qCX':'L4', 'qCZ':'L5', 'qW':'L3', 'qFY':'L1',
@@ -787,7 +803,7 @@ a
prg+='''
open forward
send 1"fwd_inp(%f) %f %f %f %f\\n",D0,{qCX},{qCZ},{qW},{qFY}
{cmt}send 1"fwd_inp(%f) %f %f %f %f\\n",D0,{qCX},{qCZ},{qW},{qFY}
{W}={qW}
{qW}={qW}*{d2r} //scale from 1000*deg to rad
@@ -805,8 +821,7 @@ open forward
{DX}={qCX}-{p0_x}
{DZ}={qCZ}-{p0_z}
{Y}={qFY}
//send 1"fwd_res %f %f %f %f\\n",{DX},{DZ},{W},{Y}
//P1001+=1
{cmt}send 1"fwd_res %f %f %f %f\\n",{DX},{DZ},{W},{Y}
D0=$000001c2; //B=$2 X=$40 Y=$80 Z=$100 hex(2+int('40',16)+int('80',16)+int('100',16)) -> 0x1c2
close
'''.format(**subs)
@@ -828,10 +843,10 @@ close
prg+='''
open inverse
//if(D0>0)
// send 1"inv_inp(%f) %f:%f %f:%f %f:%f %f:%f\\n",D0,{DX},{vDX},{DZ},{vDZ},{W},{vW},{Y},{vY}
//else
// send 1"inv_inp(%f) %f %f %f %f\\n",D0,{DX},{DZ},{W},{Y}
{cmt}if(D0>0)
{cmt} send 1"inv_inp(%f) %f:%f %f:%f %f:%f %f:%f\\n",D0,{DX},{vDX},{DZ},{vDZ},{W},{vW},{Y},{vY}
{cmt}else
{cmt} send 1"inv_inp(%f) %f %f %f %f\\n",D0,{DX},{DZ},{W},{Y}
{qW}={W}
{W}={W}*{d2r} //scale from 1000*deg to rad
@@ -851,20 +866,16 @@ open inverse
{qFY}={Y}
if(D0>0)
{{ // calculate velocities for PVT motion
{vW}={vW}*{d2r} //scale from 1000*deg to rad
{p_x}={sclY}*({p1_x}-{p0_x})
{p_z}={sclY}*({p1_z}-{p0_z})
{vqFY}={vY}
{vqCX}={vDX} + ({p1_x}-{p0_x})/({p1_y}-{p0_y})*{vY} //+ vqW*p_z
{vqCZ}={vDZ} + ({p1_z}-{p0_z})/({p1_y}-{p0_y})*{vY} //+ vqW*p_x
//vqW=vqW+(vqCX+(p1_x-p0_x)/(p1_y-p0_y)*vY)*p_z+(vqCZ+(p1_z-p0_z)/(p1_y-p0_y)*vY)*p_x
{vqW}={vW}//+((p1_x-p0_x)/(p1_y-p0_y)*vY)*p_z+((p1_z-p0_z)/(p1_y-p0_y)*vY*p_x
{vqW}={vqW}*{r2d} //scale from rad to 1000*deg
// send 1"inv_res %f:%f %f:%f %f:%f %f:%f\\n",{qCX},{vqCX},{qCZ},{vqCZ},{qW},{vqW},{qFY},{vqFY}
{vqCX}={vDX}+({p1_x}-{p0_x})/({p1_y}-{p0_y})*{vY}
{vqCZ}={vDZ}+({p1_z}-{p0_z})/({p1_y}-{p0_y})*{vY}
{vqW}={vW}
{cmt}send 1"inv_res %f:%f %f:%f %f:%f %f:%f\\n",{qCX},{vqCX},{qCZ},{vqCZ},{qW},{vqW},{qFY},{vqFY}
}}
//else
// send 1"inv_res %f %f %f %f\\n",{qCX},{qCZ},{qW},{qFY}
//P1002+=1
{cmt}else
{cmt} send 1"inv_res %f %f %f %f\\n",{qCX},{qCZ},{qW},{qFY}
close
'''.format(**subs)
@@ -873,19 +884,35 @@ close
# (p1_x-p0_x)/(p1_y-p0_y)*vY velocity part in vqCX direction, when moving in vY
# vqW*p_x velocity part of the rotation (vqW is in rad)
gpascii.send_block('disable plc 0')
time.sleep(.5)
gpascii.send_block(prg)
# {{ // calculate velocities for PVT motion
# {vW}={vW}*{d2r} //scale from 1000*deg to rad
# {p_x}={sclY}*({p1_x}-{p0_x})
# {p_z}={sclY}*({p1_z}-{p0_z})
# {vqFY}={vY}
# {vqCX}={vDX} + ({p1_x}-{p0_x})/({p1_y}-{p0_y})*{vY} //+ vqW*p_z
# {vqCZ}={vDZ} + ({p1_z}-{p0_z})/({p1_y}-{p0_y})*{vY} //+ vqW*p_x
# //vqW=vqW+(vqCX+(p1_x-p0_x)/(p1_y-p0_y)*vY)*p_z+(vqCZ+(p1_z-p0_z)/(p1_y-p0_y)*vY)*p_x
# {vqW}={vW}//+((p1_x-p0_x)/(p1_y-p0_y)*vY)*p_z+((p1_z-p0_z)/(p1_y-p0_y)*vY*p_x
# {vqW}={vqW}*{r2d} //scale from rad to 1000*deg
# // send 1"inv_res %f:%f %f:%f %f:%f %f:%f\\n",{qCX},{vqCX},{qCZ},{vqCZ},{qW},{vqW},{qFY},{vqFY}
comm = self.comm
if comm is not None:
gpascii = comm.gpascii
gpascii.send_block('disable plc 0')
time.sleep(.5)
gpascii.send_block(prg)
if self.verbose & 4:
print(prg)
if fnCrdTrf is not None :
fh=open(fnCrdTrf,'w')
fh.write(prg)
fh.close()
time.sleep(.5)
gpascii.send_block('enable plc 0')
time.sleep(.5)
gpascii.send_block('#1..7j/')
if comm is not None:
time.sleep(.5)
gpascii.send_block('enable plc 0')
time.sleep(.5)
gpascii.send_block('#1..7j/')
def setup_motion(self,prgId=2,fnPrg='/tmp/prg.cfg',mode=0,**kwargs):
'''
@@ -908,8 +935,6 @@ close
wRng : starting and ending angle
yRng : starting and ending height
'''
comm = self.comm
gpascii = comm.gpascii
param=self.param
prg=[]
prg.append('open prog %d'%(prgId))
@@ -1047,7 +1072,10 @@ close
#prg.append('&1\nb%dr\n'%prgId)
prg='\n'.join(prg) + '\n'
gpascii.send_block(prg)
comm = self.comm
if comm is not None:
gpascii = comm.gpascii
gpascii.send_block(prg)
if self.verbose & 4:
print(prg)
if fnPrg is not None :
@@ -1071,127 +1099,78 @@ close
if __name__=='__main__':
def run_test(args):
#args.host=None
test=args.test
args.host=None
if args.host is None:
comm=gather=None
else:
comm = PPComm(host=args.host)
gather = Gather(comm)
gpascii = comm.gpascii
#HelicalScanTests.calcParamSim()
hs=HelicalScan(comm, gather, args.verbose)
#hs.test_find_rot_ctr()
#hs.test_find_rot_ctr(n=5. ,per=1.,bias=2.31,ampl=4.12,phi=24.6)
fn='/tmp/helicalscan'
#hs.load_rec(fn+'.npz')
#hs.param[:4]+=np.pi/2.#add 90 deg
if test==1:
### test find rotation center ###
HelicalScanTests.test_find_rot_ctr(n=32. ,per=1.,bias=2.31,ampl=4.12,phi=0*d2r)
HelicalScanTests.test_find_rot_ctr(n=32. ,per=1.,bias=2.31,ampl=4.12,phi=30.*d2r)
HelicalScanTests.test_find_rot_ctr(n=3.,per=1.,bias=-4.1,ampl=2.4,phi=37*d2r)
HelicalScanTests.test_find_rot_ctr(n=5. ,per=1.,bias=7.25,ampl=6.51,phi=-124.6*d2r)
return
#hsg=HelicalScanGui(hs);hsg.interactive_anim()
#hsg=HelicalScanGui(hs);hsg.interactive_anim(manip=True)
#while True:hsg.update_anim(0)
#hs.param = np.ndarray((2,5))
#hs.param[0]=(15,2,0,3,0)#(z_i, y_i, x_i, r_i,phi_i)
#hs.param[1]=(15,4,0,2,np.pi/4)#(z_i, y_i, x_i, r_i,phi_i)
elif test==2:
### test param calculation and display the gui###
HelicalScanTests.calcParamSim(hs)
hs.setup_motion(mode=1,cntHor=5,cntVert=15,hRng=(-50,50),wRng=(0,120000),smt=0,pt2pt_time=200)
#hsg=HelicalScanGui(hs);hsg.interactive_cx_cz_w_fy()
#hsg=HelicalScanGui(hs);hsg.interactive_cx_cz_w_fy(manip=True)
#while True: hsg.update_cx_cz_w_fy() #for debug purpose
for manip in (False,True):
hsg=HelicalScanGui(hs);hsg.interactive_cx_cz_w_fy(manip=manip)
hsg=HelicalScanGui(hs);hsg.interactive_dx_dz_w_y(manip=manip)
#hsg=HelicalScanGui(hs);hsg.interactive_anim(manip=manip)
return
elif test==3:
### load recorded data, check param calculation not changed and display in the gui###
fn='/home/zamofing_t/Documents/prj/SwissFEL/epics_ioc_modules/ESB_MX/python/helicalscan'
hs.load_rec(fn+'.npz')
paramRec=hs.param
fh=open("/home/zamofing_t/Documents/prj/SwissFEL/epics_ioc_modules/ESB_MX/python/helical.cmd")
s=fh.read();s=s.replace('calcParam','hs.calcParam')
eval(s)
assert (abs(paramRec-hs.param).max()<1E-10)
#hsg=HelicalScanGui(hs);hsg.interactive_dx_dz_w_y()
#hsg=HelicalScanGui(hs);hsg.interactive_dx_dz_w_y(manip=True)
#while True: hsg.update_dx_dz_w_y() #for debug purpose
#return
#TODO: FE Digitizers PBPS117 timing not working!
#hs.calcParam()
for manip in (False,True):
hsg=HelicalScanGui(hs);hsg.interactive_anim(manip=manip)
hsg=HelicalScanGui(hs);hsg.interactive_cx_cz_w_fy(manip=manip)
hsg=HelicalScanGui(hs);hsg.interactive_dx_dz_w_y(manip=manip)
return
#gpasci: #1,4,5p // y,-x ,-z
# 0deg 256.7 -762.5 -396.4
#120deg 258.5 731.7 -1896.9
#240deg 256.2 -1282.8 -2496.5
# 0deg 586.5 -1023.4 -696.8
# 120deg 574.8 619.8 -1797.5
# 240deg 580.0 -900.0 -2496.3
#hs.calcParam(x = ((-762.5, 731.7, -1282.8), (-1023.4, 619.8, -900.0)),
# y = (258.5,580.0),
# z = (( -396.4,-1896.9,-2496.5),(-696.8,-1797.5,-2496.3)))
#...
#&1p
#cpx X0 Z0 B0 Y258
#cpx X0 Z0 B120000 Y258
#cpx X0 Z0 B240000 Y258
#cpx X0 Z0 B0 Y580
#cpx X0 Z0 B120000 Y580
#cpx X0 Z0 B240000 Y580
# 0deg 1405.7 -1154.4 -1309.6
# 120deg 1401.7 216.3 -1010.9
# 240deg 1407.9 -250.7 -2410.3
# 0deg 1053.9 -1330.2 -1219.4
# 120deg 1019.2 340.9 -918.8
# 240deg 984.0 -230.4 -2510.4
#hs.calcParam(x = ((-1154.4, 216.3, -250.7), ( -1330.2, 340.9, -230.4)),
# y = (1405.7,1019.2),
# z = ((-1309.6, -1010.9, -2410.3),( -1219.4, -918.8, -2510.4)))
#&1p
#cpx X0 Z0 B0 Y1405
#cpx X0 Z0 B120000 Y1405
#cpx X0 Z0 B240000 Y1405
#cpx X0 Z0 B0 Y1019
#cpx X0 Z0 B120000 Y1019
#cpx X0 Z0 B240000 Y1019
#1600.5 -1484.0 -1264.2
#1599.0 234.6 -663.3
#1599.9 28.5 -2264.7
# 468.3 629.9 -2663.3
# 472.2 25.3 -164.4
# 470.9 -1841.3 -1965.0
#hs.calcParam(x = ((-1484.0, 234.6, 28.5), ( 629.9, 25.3,-1841.3)),
# y = (1600.,470.),
# z = ((-1264.2, -663.3,-2264.7),(-2663.3, -164.4,-1965.0)))
#cpx X0 Z0 B0 Y1405
#cpx X0 Z0 B120000 Y1405
#cpx X0 Z0 B240000 Y1405
#cpx X0 Z0 B0 Y1019
#cpx X0 Z0 B120000 Y1019
#cpx X0 Z0 B240000 Y1019
fh=open("/sf/bernina/config/swissmx/exchange/helical.cmd")
s=fh.read();s=s.replace('calcParam','hs.calcParam')
eval(s)
#hs.calcParamSim()
#hs.param[0]=(15,2,0,3,0)#(z_i, y_i, x_i, r_i,phi_i)
#hs.param[1]=(15,4,0,3,0)#(z_i, y_i, x_i, r_i,phi_i)
#hs.param[0]=(-100, 100,0,50,0)#(z_i, y_i, x_i, r_i,phi_i)
#hs.param[1]=(-100,-100,0,70,0)#(z_i, y_i, x_i, r_i,phi_i)
#hs.test_coord_trf()
#hs.interactive_cx_cz_w_fy()
#hs.interactive_dx_dz_w_y()
#mode bits:
#0:1 config simulated motors
#1:2 config real motors
#2:4 config coord trf
### use simulation motors ###
#os.chdir(os.path.join(os.path.dirname(__file__),'../cfg'))
#'sim_8_motors.cfg'
#['$$$***','!common()','!SAR-EXPMX1()','#1..7j/','enable plc 1','Motor[1].MaxSpeed=25','Motor[2].MaxSpeed=25'])
# raw_input('press return when homed')
hs.setup_coord_trf()
### hs.calcParam(<data from Zac>) ###
fh=open("/sf/bernina/config/swissmx/exchange/helical.cmd")
#fh=open("/sf/bernina/config/swissmx/exchange/helical_neg.cmd")
s=fh.read();s=s.replace('calcParam','hs.calcParam')
eval(s)
hs.setup_coord_trf()
hs.setup_sync(mode=2) # None: no sync at all mode=1: sync on timing UserFlag
hs.setup_gather()
@@ -1218,13 +1197,11 @@ if __name__=='__main__':
#hs.gen_prog(mode=1,pt2pt_time=100,cnt=1,cntVert=10,cntHor=3,hRng=(-30,30),wRng=(0,36000),yRng=(-50,-100))
#hs.gen_prog(mode=1,cntHor=7,cntVert=2,hRng=(-100,50),wRng=(000,10000),smt=0)
hs.run()
print('wait until gather finished:')
fn='/tmp/helicalscan'
hs.gather_upload(fn+'.npz')
hs.load_rec(fn+'.npz')
hsg=HelicalScanGui(hs)
hsg.show_pos()
hsg.show_vel()
@@ -1259,6 +1236,7 @@ Examples:'''+''.join(map(lambda s:cmd+s, exampleCmd))+'\n '
parser.add_option('-v', '--verbose', type="int", dest='verbose', help='verbosity bits (see below)', default=255)
parser.add_option('--host', help='hostname', default='SAR-CPPM-EXPMX1')
parser.add_option('-t','--test', type="int", default=None)
(args, other)=parser.parse_args()
args.other=other