271 lines
9.4 KiB
Python
Executable File
271 lines
9.4 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="/tmp/shapepath.prg")',
|
|
'plot_gather()']
|
|
}
|
|
|
|
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
|
|
- plot gathered data
|
|
plot_gather()
|
|
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:
|
|
#cfg={"points": [[100,523],[635,632],[756,213]],"sequencer":['sort_points()','gen_prog(file="/tmp/shapepath.prg",host="SAROP11-CPPM-MOT6871")','plot_gather()']}
|
|
#cfg={"sequencer":['gen_rand_points(n=107, scale=1000)','sort_points()','gen_prog(file="/tmp/shapepath.prg",host="SAROP11-CPPM-MOT6871")','plot_gather()']}
|
|
#cfg={"sequencer":['gen_grid_points(w=10,h=10,pitch=100,rnd=.2)','sort_points()','gen_prog(file="/tmp/shapepath.prg",host="SAROP11-CPPM-MOT6871")','plot_gather()']}
|
|
#cfg={"sequencer":['gen_grid_points(w=10,h=10,pitch=100,rnd=0.2)','sort_points()','gen_prog(file="/tmp/shapepath.prg")','plot_gather()']}
|
|
#cfg = {"sequencer": ['gen_rand_points(n=107, scale=1000)', 'sort_points()','plot_gather()']}
|
|
cfg={"sequencer":['gen_grid_points(w=20,h=20,pitch=50,rnd=.2)','sort_points()','gen_prog(file="/tmp/shapepath.prg",host="SAROP11-CPPM-MOT6871")','plot_gather()']}
|
|
cfg={"sequencer":['gen_grid_points(w=20,h=20,pitch=50,rnd=.2)','sort_points()','gen_prog(file="/tmp/shapepath.prg")','plot_gather()']}
|
|
#cfg={"sequencer":['gen_rand_points(n=400, scale=1000)','sort_points()','gen_prog(file="/tmp/shapepath.prg",host="SAROP11-CPPM-MOT6871")','plot_gather()']}
|
|
|
|
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):
|
|
prg=[]
|
|
gather={"MaxSamples":100000, "Period":10 }
|
|
#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
|
|
prg.append('Gather.Enable=2')
|
|
if mode==0:
|
|
prg.append(' linear abs')
|
|
data=self.points
|
|
for idx in range(data.shape[0]):
|
|
prg.append('X(%f) Y(%f)'%tuple(data[idx,:]))
|
|
prg.append('dwell 100')
|
|
if mode==1:
|
|
pass
|
|
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)
|
|
|
|
#sort points along y
|
|
pts=pts[pts[:, 1].argsort()]
|
|
#if verb&2:
|
|
# self.plot_points(pts)
|
|
|
|
#group sorting
|
|
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))
|
|
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,0].argsort()[::-1]
|
|
else:
|
|
idx[a:a+grp_sz]=a+pts[a:a+grp_sz,0].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,fnLoc='/tmp/gather.txt'):
|
|
pts=self.points
|
|
rec = np.genfromtxt(fnLoc, delimiter=' ')
|
|
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[:, 4], rec[:, 5], 'b-')
|
|
hl=ax.plot(rec[:,1],rec[:,2],'g-')
|
|
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)
|
|
err=np.sqrt((rec[:,1]-rec[:,4])**2+(rec[:,1]-rec[:,4])**2)
|
|
hl = ax.plot(err, 'r-')
|
|
ax.xaxis.set_label_text('time (10x servo cycle)')
|
|
ax.yaxis.set_label_text('pos-error um')
|
|
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('--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)
|