391 lines
16 KiB
Python
Executable File
391 lines
16 KiB
Python
Executable File
#!/usr/bin/env python
|
|
# *-----------------------------------------------------------------------*
|
|
# | |
|
|
# | Copyright (c) 2016 by Paul Scherrer Institute (http://www.psi.ch) |
|
|
# | |
|
|
# | Author Thierry Zamofing (thierry.zamofing@psi.ch) |
|
|
# *-----------------------------------------------------------------------*
|
|
'''
|
|
shape an optimal path with given points
|
|
|
|
verbose bits:
|
|
1 basic info
|
|
2 plot sorting steps
|
|
4 list program
|
|
4 upload progress
|
|
8 plot gather path
|
|
|
|
#config file example:
|
|
{
|
|
"points": [
|
|
[100,523],[635,632],[756,213],
|
|
"sequencer":[
|
|
'gen_grid_points(w=10,h=10,pitch=100,rnd=.2)',
|
|
'sort_points()',
|
|
'gen_prog(file="'+fn+'.prg")',
|
|
'plot_gather("'+fn+'.npz")']
|
|
}
|
|
|
|
Sequencer functions are:
|
|
- generate points (if not in the 'points' configuration)
|
|
gen_rand_points(self,n=107,scale=1000)
|
|
gen_grid_points(w=10,h=10,pitch=100,rnd=.2)
|
|
- sorting points:
|
|
sort_points(self)
|
|
- generate/download/execute motion progran, upload trace of motors (gather data)
|
|
gen_prog(self,prgId=2,file=None,host=None)
|
|
if host=None nothing will be downloaded/executed and trace of motors will not be uploaded
|
|
if file=None the program will not be saved and nothing will be executed
|
|
gen_prog modes:
|
|
-1 jog a 10mm square
|
|
0 linear motion
|
|
1 pvt motion
|
|
2 spline motion
|
|
|
|
|
|
- plot gathered data
|
|
plot_gather("'+fn+'.npz")
|
|
this makes only sence, if motion has been executed and data can be gathered from the powerbrick
|
|
|
|
Acquired time is:MaxSamples*Period*.2
|
|
'''
|
|
|
|
import os, sys, json
|
|
import numpy as np
|
|
import matplotlib as mpl
|
|
import matplotlib.pyplot as plt
|
|
import subprocess as sprc
|
|
import telnetlib
|
|
from utilities import *
|
|
|
|
class ShapePath:
|
|
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/shapepath'
|
|
#fn='/home/zamofing_t/Documents/prj/SwissFEL/epics_ioc_modules/ESB_MX/python/data/'+time.strftime('%y-%m-%d-%H_%M_%S')
|
|
#cfg={"points": [[100,523],[635,632],[756,213]],"sequencer":['sort_points()','gen_prog(file="'+fn+'.prg",host="SAROP11-CPPM-MOT6871")','plot_gather("'+fn+'.npz")']}
|
|
#cfg={"sequencer":['gen_rand_points(n=107, scale=1000)','sort_points()','gen_prog(file="'+fn+'.prg",host="SAROP11-CPPM-MOT6871")','plot_gather("'+fn+'.npz")']}
|
|
#cfg={"sequencer":['gen_grid_points(w=10,h=10,pitch=100,rnd=.2)','sort_points()','gen_prog(file="'+fn+'.prg",host="SAROP11-CPPM-MOT6871")','plot_gather("'+fn+'.npz")']}
|
|
#cfg={"sequencer":['gen_grid_points(w=10,h=10,pitch=100,rnd=0.2)','sort_points()','gen_prog(file="'+fn+'.prg")','plot_gather("'+fn+'.npz")']}
|
|
#cfg = {"sequencer": ['gen_rand_points(n=107, scale=1000)', 'sort_points()','plot_gather("'+fn+'.npz")']}
|
|
#cfg={"sequencer":['gen_grid_points(w=20,h=20,pitch=50,rnd=.2)','sort_points()','gen_prog(file="'+fn+'.prg",host="SAROP11-CPPM-MOT6871")','plot_gather("'+fn+'.npz")']}
|
|
#cfg={"sequencer":['gen_grid_points(w=20,h=20,pitch=50,rnd=.2)','sort_points()','gen_prog(file="'+fn+'.prg")','plot_gather("'+fn+'.npz")']}
|
|
#cfg={"sequencer":['gen_rand_points(n=400, scale=1000)','sort_points()','gen_prog(file="'+fn+'.prg",host="SAROP11-CPPM-MOT6871")','plot_gather("'+fn+'.npz")']}
|
|
#cfg = {"sequencer": ['gen_grid_points(w=20,h=20,pitch=50,rnd=.2)', 'sort_points()','gen_prog(file="'+fn+'.prg",host="SAROP11-CPPM-MOT6871",mode=0)', 'plot_gather("'+fn+'.npz")']}
|
|
#cfg = {"sequencer": ['gen_grid_points(w=5,h=5,pitch=50,rnd=.2)', 'sort_points()','gen_prog(file="'+fn+'.prg",host="SAROP11-CPPM-MOT6871",mode=1)', 'plot_gather("'+fn+'.npz")']}
|
|
#cfg = {"sequencer": ['gen_grid_points(w=2,h=2,pitch=100,rnd=0)', 'sort_points()','gen_prog(file="'+fn+'.prg",host="SAROP11-CPPM-MOT6871",mode=0)', 'plot_gather("'+fn+'.npz")']}
|
|
#cfg = {"sequencer": ['gen_grid_points(w=5,h=5,pitch=100,rnd=0.2)', 'sort_points()','gen_prog(file="'+fn+'.prg",host="SAROP11-CPPM-MOT6871",mode=0)', 'plot_gather("'+fn+'.npz")']}
|
|
#cfg = {"points": [[0, 0],[100, 0],[200, 0],[300, 0],[400, 0],[400, 100],[300, 100],[200, 100],[100, 100],[0, 100],[10, 200],[100, 200],[200, 200],[300, 200],[400, 200],[410, 300],[300, 300],[200, 300],[100, 300],[0, 300],[0, 400],[100, 400],[200, 400],[300, 400],[400, 400]],"sequencer": ['gen_prog(file="'+fn+'.prg",host="SAROP11-CPPM-MOT6871",mode=0)', 'plot_gather("'+fn+'.npz")']}
|
|
#cfg = {"sequencer": ['gen_grid_points(w=2,h=2,pitch=10000,rnd=0)', 'sort_points()','gen_prog(file="'+fn+'.prg",host="SAROP11-CPPM-MOT6871",mode=-1)', 'plot_gather("'+fn+'.npz")']}
|
|
#cfg = {"sequencer": ['gen_grid_points(w=2,h=2,pitch=10000,rnd=0)', 'sort_points()','gen_prog(file="'+fn+'.prg",host="SAROP11-CPPM-MOT6871",mode=1,pt2pt_time=1000)', 'plot_gather("'+fn+'.npz")']}
|
|
cfg = {"sequencer": ['gen_grid_points(w=20,h=20,pitch=50,rnd=0.2)', 'sort_points()','gen_prog(file="'+fn+'.prg",host="SAROP11-CPPM-MOT6871",mode=1,pt2pt_time=10,acq_per=10)', 'plot_gather("'+fn+'.npz")']}
|
|
#cfg = {"sequencer": ['gen_rand_points(n=400, scale=1000)', 'sort_points()','gen_prog(file="'+fn+'.prg",host="SAROP11-CPPM-MOT6871",mode=1,pt2pt_time=10)', 'plot_gather("'+fn+'.npz")']}
|
|
#cfg = {"sequencer": ['gen_grid_points(w=5,h=5,pitch=100,rnd=0.2)', 'sort_points()','gen_prog(file="'+fn+'.prg",host="SAROP11-CPPM-MOT6871",mode=2,pt2pt_time=10)', 'plot_gather("'+fn+'.npz")']}
|
|
#cfg = {"sequencer":['gen_rand_points(n=400, scale=1000)','sort_points()','gen_prog(file="'+fn+'.prg",host="SAROP11-CPPM-MOT6871",mode=1,pt2pt_time=10,acq_per=1)','plot_gather("'+fn+'.npz")']}
|
|
|
|
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 gen_rand_points(self,n=107,scale=1000):
|
|
np.random.seed(0)
|
|
#data=np.random.randint(0,1000,(30,2))
|
|
pts=np.random.rand(n,2)*scale
|
|
self.points=pts
|
|
|
|
def gen_grid_points(self,w=10,h=10,pitch=100,rnd=.2):
|
|
np.random.seed(0)
|
|
xx,yy=np.meshgrid(range(w), range(h))
|
|
pts=np.array([xx.reshape(-1),yy.reshape(-1)],dtype=np.float).transpose()*pitch
|
|
if rnd != 0:
|
|
pts+=(np.random.rand(pts.shape[0],2)*(rnd*pitch))
|
|
self.points=pts
|
|
|
|
def gen_prog(self,prgId=2,file=None,host=None,mode=0,**kwargs):
|
|
'''
|
|
kwargs:
|
|
acq_per : acquire period: acquire data all acq_per servo loops (default=1)
|
|
pt2pt_time : time to move from one point to the next point
|
|
'''
|
|
prg=[]
|
|
acq_per=kwargs.get('acq_per',1)
|
|
gather={"MaxSamples":1000000, "Period":acq_per}
|
|
#Sys.ServoPeriod is dependent of !common() macro
|
|
ServoPeriod= .2 #0.2ms
|
|
#ServoPeriod = .05
|
|
self.meta = {'timebase': ServoPeriod*gather['Period']}
|
|
#channels=["Motor[1].ActPos","Motor[2].ActPos","Motor[3].ActPos"]
|
|
channels=["Motor[1].ActPos","Motor[2].ActPos","Motor[3].ActPos","Motor[1].DesPos","Motor[2].DesPos","Motor[3].DesPos"]
|
|
prg.append('Gather.Enable=0')
|
|
prg.append('Gather.Items=%d'%len(channels))
|
|
for k,v in gather.iteritems():
|
|
prg.append('Gather.%s=%d'%(k,v))
|
|
for i,c in enumerate(channels):
|
|
prg.append('Gather.Addr[%d]=%s.a'%(i,c))
|
|
|
|
|
|
prg.append('open prog %d'%(prgId))
|
|
# this uses Coord[1].Tm and limits with MaxSpeed
|
|
if mode==-1: #### jog a 10mm square
|
|
pos=self.points
|
|
prg.append(' linear abs')
|
|
prg.append('X(%g) Y(%g)' % tuple(pos[0, :]))
|
|
prg.append('dwell 10')
|
|
prg.append('Gather.Enable=2')
|
|
prg.append('jog2:10000')
|
|
prg.append('dwell 100')
|
|
prg.append('jog3:10000')
|
|
prg.append('dwell 100')
|
|
prg.append('jog2:-10000')
|
|
prg.append('dwell 100')
|
|
prg.append('jog3:-10000')
|
|
prg.append('dwell 100')
|
|
prg.append('Gather.Enable=0')
|
|
elif mode==0: #### linear motion
|
|
pos=self.points
|
|
prg.append(' linear abs')
|
|
prg.append('X(%g) Y(%g)' % tuple(pos[0, :]))
|
|
prg.append('dwell 10')
|
|
prg.append('Gather.Enable=2')
|
|
prg.append(' linear abs')
|
|
for idx in range(pos.shape[0]):
|
|
prg.append('X%g Y%g'%tuple(pos[idx,:]))
|
|
prg.append('dwell 100')
|
|
prg.append('Gather.Enable=0')
|
|
elif mode==1: #### pvt motion
|
|
try:
|
|
pt2pt_time=kwargs['pt2pt_time'] #how many ms to move to next point (pt2pt_time)
|
|
except KeyError:
|
|
print 'missing pt2pt_time, use default=100ms'
|
|
pt2pt_time=100.
|
|
|
|
pt=self.points
|
|
vel=pt[2:,:]-pt[:-2,:]
|
|
#pv is an array of posx posy velx vely
|
|
pv=np.ndarray(shape=(pt.shape[0]+2,4),dtype=pt.dtype)
|
|
pv[:]=np.NaN
|
|
#pv[ 0,(0,1)]=2*pt[0,:]-pt[1,:]
|
|
pv[ 0,(0,1)]=pt[0,:]
|
|
pv[ 1:-1,(0,1)]=pt
|
|
#pv[ -1,(0,1)]=2*pt[-1,:]-pt[-2,:]
|
|
pv[ -1,(0,1)]=pt[-1,:]
|
|
pv[(0,0,-1,-1),(2,3,2,3)]=0
|
|
dist=pv[2:,(0,1)] - pv[:-2,(0,1)]
|
|
pv[ 1:-1,(2,3)] = 1000.*dist/(2.*pt2pt_time)
|
|
|
|
prg.append(' linear abs')
|
|
prg.append('X%g Y%g' % tuple(pv[0, (0,1)]))
|
|
prg.append('dwell 10')
|
|
prg.append('Gather.Enable=2')
|
|
prg.append(' pvt%g abs'%pt2pt_time) #100ms to next position
|
|
for idx in range(1,pv.shape[0]):
|
|
prg.append('X%g:%g Y%g:%g'%tuple(pv[idx,(0,2,1,3)]))
|
|
prg.append('X%g Y%g' % tuple(pv[-1, (0,1)]))
|
|
prg.append('dwell 1000')
|
|
prg.append('Gather.Enable=0')
|
|
elif mode==2: #### spline motion
|
|
try:
|
|
pt2pt_time=kwargs['pt2pt_time'] #how many ms to move to next point (pt2pt_time)
|
|
except KeyError:
|
|
print 'missing pt2pt_time, use default=100ms'
|
|
pt2pt_time=100.
|
|
pos=self.points
|
|
pcor=np.ndarray(pos.shape,dtype=pos.dtype);pcor[:]=np.NaN
|
|
pcor[(0,-1),:]=pos[(0,-1),:]
|
|
pcor[1:-1,:]=(-pos[0:-2,:]+8*pos[1:-1,:]-pos[2:,:])/6.
|
|
#pcor=pos
|
|
prg.append(' linear abs')
|
|
prg.append('X(%g) Y(%g)' % tuple(pcor[0, :]))
|
|
prg.append('dwell 10')
|
|
prg.append('Gather.Enable=2')
|
|
prg.append(' spline%g abs'%pt2pt_time) #100ms to next position
|
|
for idx in range(pcor.shape[0]):
|
|
prg.append('X%g Y%g'%tuple(pcor[idx,:]))
|
|
prg.append('dwell 100')
|
|
prg.append('Gather.Enable=0')
|
|
|
|
prg.append('close')
|
|
prg.append('&1\nb%dr\n'%prgId)
|
|
if self.args.verbose & 4:
|
|
for ln in prg:
|
|
print(ln)
|
|
|
|
if file is not None:
|
|
fh=open(file,'w')
|
|
fh.write('\n'.join(prg))
|
|
fh.close()
|
|
if host is not None:
|
|
cmd ='gpasciiCommander --host '+host+' '+ file
|
|
print cmd
|
|
p = sprc.Popen(cmd, shell=True)#, stdout=sprc.PIPE, stderr=sprc.STDOUT)
|
|
#res=p.stdout.readlines(); print res
|
|
retval = p.wait()
|
|
#gather -u /var/ftp/gather/out.txt
|
|
cmd ='PBGatherPlot -m24 -v7 --host '+host
|
|
print cmd
|
|
p = sprc.Popen(cmd, shell=True)#, stdout=sprc.PIPE, stderr=sprc.STDOUT)
|
|
retval = p.wait()
|
|
self.prg=prg
|
|
|
|
|
|
def sort_points(self):
|
|
pts=self.points
|
|
verb=self.args.verbose
|
|
#if verb&2:
|
|
# self.plot_points(pts)
|
|
|
|
cnt=pts.shape[0]
|
|
idx=np.ndarray(cnt,dtype=np.int32)
|
|
grp_cnt=int(np.sqrt(cnt))
|
|
grp_sz=int(np.ceil(float(cnt)/grp_cnt))
|
|
if self.args.yx==True:
|
|
idxA=0;idxB=1
|
|
else:
|
|
idxA=1;idxB=0
|
|
|
|
#sort points along idxA
|
|
pts=pts[pts[:,idxA].argsort()]
|
|
#group sorting along idxB
|
|
for i in range(grp_cnt):
|
|
a=i*grp_sz
|
|
#print a,a+grp_sz
|
|
if i%2:
|
|
idx[a:a+grp_sz]=a+pts[a:a+grp_sz,idxB].argsort()[::-1]
|
|
else:
|
|
idx[a:a+grp_sz]=a+pts[a:a+grp_sz,idxB].argsort()
|
|
#print(idx)
|
|
pts=pts[idx]
|
|
|
|
if verb&2:
|
|
self.plot_points(pts)
|
|
plt.show()
|
|
self.points=pts
|
|
|
|
@staticmethod
|
|
def onclick(event):
|
|
print 'button=%s, x=%d, y=%d, xdata=%f, ydata=%f'%(
|
|
event.button, event.x, event.y, event.xdata, event.ydata)
|
|
obj=event.canvas.figure.obj
|
|
|
|
def plot_points(self,pts):
|
|
fig=plt.figure()
|
|
ax = fig.add_subplot(1,1,1)
|
|
#hl=ax[0].plot(x, y, color=col)
|
|
hl=ax.plot(pts[:,0],pts[:,1],'r.')
|
|
hl=ax.plot(pts[:,0],pts[:,1],'y--')
|
|
cid = fig.canvas.mpl_connect('button_press_event', self.onclick)
|
|
fig.obj=self
|
|
self.ax=ax
|
|
self.hl=hl
|
|
|
|
def plot_gather(self,fnOut='/tmp/shapepath.npz',fnLoc='/tmp/gather.txt'):
|
|
meta=self.meta
|
|
pts=self.points # X,Y array
|
|
rec = np.genfromtxt(fnLoc, delimiter=' ')
|
|
#rec=Motor[1].ActPos,Motor[2].ActPos,Motor[3].ActPos,Motor[1].DesPos,Motor[2].DesPos,Motor[3].DesPos
|
|
#res=rot.ActPos,y.ActPos,x.ActPos,rot.DesPos,y.DesPos,x.DesPos
|
|
#idx 0 1 2 3 4 5
|
|
if fnOut:
|
|
# time base:
|
|
# Gather.Period=10
|
|
# Sys.ServoPeriod=0.2 -> 5kHz
|
|
# -> aquired data all 10*0.2ms= 2 ms -> 30000 points = 60 sec
|
|
|
|
np.savez_compressed(fnOut, rec=rec, pts=pts, meta=meta)
|
|
fig=plt.figure()
|
|
ax = fig.add_subplot(1,1,1)
|
|
#hl=ax[0].plot(x, y, color=col)
|
|
hl=ax.plot(pts[:,0],pts[:,1],'r.')
|
|
hl=ax.plot(pts[:,0],pts[:,1],'y--')
|
|
hl = ax.plot(rec[:, 5], rec[:, 4], 'b-') # desired path
|
|
hl=ax.plot(rec[:,2],rec[:,1],'g-') # actual path
|
|
ax.xaxis.set_label_text('x-pos um')
|
|
ax.yaxis.set_label_text('y-pos um')
|
|
cid = fig.canvas.mpl_connect('button_press_event', self.onclick)
|
|
fig.obj=self
|
|
self.ax=ax
|
|
self.hl=hl
|
|
|
|
fig = plt.figure()
|
|
ax = fig.add_subplot(1, 1, 1)
|
|
errx=rec[:,2]-rec[:,5]
|
|
erry=rec[:,1]-rec[:,4]
|
|
err=np.sqrt(errx**2+erry**2)
|
|
|
|
hl = []
|
|
hl += ax.plot(errx, 'b-',label='x-error')
|
|
hl += ax.plot(erry, 'g-',label='y-error')
|
|
hl += ax.plot(err, 'r-',label='error')
|
|
ax.xaxis.set_label_text('datapoint (timebase: %g ms per data point)'%meta['timebase'])
|
|
ax.yaxis.set_label_text('pos-error um')
|
|
legend = ax.legend(loc='upper right', shadow=True)
|
|
print('average error x %g um, y %g um, %g um'%(np.abs(errx).mean(),np.abs(erry).mean(),err.mean()))
|
|
plt.show()
|
|
|
|
if __name__=='__main__':
|
|
from optparse import OptionParser, IndentedHelpFormatter
|
|
class MyFormatter(IndentedHelpFormatter):
|
|
'helper class for formating the OptionParser'
|
|
|
|
def __init__(self):
|
|
IndentedHelpFormatter.__init__(self)
|
|
|
|
def format_epilog(self, epilog):
|
|
if epilog:
|
|
return epilog
|
|
else:
|
|
return ""
|
|
|
|
def parse_args():
|
|
'main command line interpreter function'
|
|
#usage: gpasciiCommunicator.py --host=PPMACZT84 myPowerBRICK.cfg
|
|
(h, t)=os.path.split(sys.argv[0]);cmd='\n '+(t if len(h)>3 else sys.argv[0])+' '
|
|
exampleCmd=('--host=PPMAC1391 -m 63 --cfg gather.cfg',
|
|
'samplePowerBrick.cfg',
|
|
'-n stackCheck1.cfg',
|
|
'--host=PPMACZT84 stackCheck1.cfg',
|
|
'--host=PPMACZT84 stackCheck1.cfg -v15',
|
|
)
|
|
epilog=__doc__+'''
|
|
Examples:'''+''.join(map(lambda s:cmd+s, exampleCmd))+'\n '
|
|
|
|
fmt=MyFormatter()
|
|
parser=OptionParser(epilog=epilog, formatter=fmt)
|
|
|
|
parser.add_option('-v', '--verbose', type="int", dest='verbose', help='verbosity bits (see below)', default=0)
|
|
parser.add_option('-n', '--dryrun', action='store_true', help='dryrun to stdout')
|
|
parser.add_option('--yx', action='store_true', help='sort y,x instead x,y')
|
|
parser.add_option('--cfg', help='config file containing json configuration structure')
|
|
|
|
(args, other)=parser.parse_args()
|
|
args.other=other
|
|
|
|
sp=ShapePath(args)
|
|
sp.run()
|
|
#------------------ Main Code ----------------------------------
|
|
#ssh_test()
|
|
ret=parse_args()
|
|
exit(ret)
|