From b98456b6adbfde38c68426b34ee0ac81fc315439 Mon Sep 17 00:00:00 2001 From: Thierry Zamofing Date: Thu, 22 Dec 2016 08:09:52 +0100 Subject: [PATCH] wip path shaping --- Readme.md | 15 ++-- cfg/mx-stage.cfg | 74 ---------------- cfg/shapepath.py | 216 +++++++++++++++++++++++++++++++---------------- cfg/utilities.py | 93 ++++++++++++++++++++ 4 files changed, 246 insertions(+), 152 deletions(-) create mode 100755 cfg/utilities.py diff --git a/Readme.md b/Readme.md index f962bec..5c85a88 100644 --- a/Readme.md +++ b/Readme.md @@ -45,11 +45,12 @@ Servo Test Motor QBL 4208-41-04-006 - 200 pole (100 einraster per rev) -************************ -copy configuration after 'save': -scp -r root@SAROP11-CPPM-MOT6871:/opt/ppmac/usrflash /scratch +******* Path shaping test ***************** +gpasciiCommander --host SAROP11-CPPM-MOT6871 -i +!mx-stage() +#1..3$ +&1 +#1..3j/ -restore -scp -r /scratch/usrflash.IDE/* root@SAROP11-CPPM-MOT6871:/opt/ppmac/usrflash -$$$ -#4$ \ No newline at end of file +cd /home/zamofing_t/Documents/prj/SwissFEL/epics_ioc_modules/ESB_MX/cfg +./shapepath.py -v255 diff --git a/cfg/mx-stage.cfg b/cfg/mx-stage.cfg index 710bb9b..dbfa674 100644 --- a/cfg/mx-stage.cfg +++ b/cfg/mx-stage.cfg @@ -76,77 +76,3 @@ open prog 2 X(0) Y(0) A(0) close - - -open prog 2 - //this uses Coord[1].Tm and limits with MaxSpeed - linear abs - X(10000) Y(0) A(0) - X(0) Y(10000) A(0) - X(0) Y(0) A(0) - X(0) Y(0) A(36000) - X(0) Y(0) A(0) -close - -open prog 3 - Gather.Enable=2 - //this uses Coord[1].Tm and limits with MaxSpeed - linear abs - X(10000) //Y(0) - //X(0) Y(0) - //dwell 0 - X(0) Y(10000) - X(0) Y(0) - X(10000) Y(10000) - X(0) Y(0) - dwell 100 - Gather.Enable=0 -close - - -open prog 4 - Gather.Enable=2 - //this uses Coord[1].Tm and limits with MaxSpeed - linear abs - X(100) Y(100) - X(200) Y(110) - X(300) Y(140) - X(400) Y(130) - X(500) Y(134) - X(600) Y(146) - X(700) Y(178) - X(800) Y(143) - X(900) Y(133) - X(1000) Y(147) - X(1100) Y(196) - X(1200) Y(104) - X(1300) Y(183) - X(1400) Y(135) - X(1500) Y(134) - X(1600) Y(195) - X(1700) Y(116) - X(1800) Y(150) - X(1900) Y(127) - X(1900) Y(272) - X(1800) Y(215) - X(1700) Y(232) - X(1600) Y(276) - X(1500) Y(215) - X(1400) Y(298) - X(1300) Y(235) - X(1200) Y(297) - X(1100) Y(256) - X(1000) Y(244) - X(900) Y(279) - X(800) Y(238) - X(700) Y(265) - X(600) Y(279) - X(500) Y(234) - X(400) Y(235) - X(300) Y(297) - X(200) Y(227) - X(100) Y(256) - dwell 100 - Gather.Enable=0 -close - diff --git a/cfg/shapepath.py b/cfg/shapepath.py index f5ed7c1..e1cf7ca 100755 --- a/cfg/shapepath.py +++ b/cfg/shapepath.py @@ -8,26 +8,39 @@ ''' shape an optimal path with given points -#mode bits: - - 0 1 sort and plot random points - 1 2 sort and plot grid(+some random) points - 2 4 generate motion program - 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], - "mode": ["plot","program","gather","???"], + "sequencer":[ + 'gen_grid_points(w=10,h=10,pitch=100,rnd=.2)', + 'sort_points()', + 'gen_prog(file="/tmp/shapepath.prg")', + 'plot_gather()'] } -Acquired time is:MaxSamples*Period*.2 +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 @@ -36,36 +49,7 @@ import matplotlib as mpl import matplotlib.pyplot as plt import subprocess as sprc import telnetlib - -def ConvUtf8(s): - 'convert unicoded json object to ASCII encoded' - #http://stackoverflow.com/questions/956867/how-to-get-string-objects-instead-of-unicode-ones-from-json-in-python - if isinstance(s, dict): - return {ConvUtf8(key): ConvUtf8(value) for key, value in s.iteritems()} - elif isinstance(s, list): - return [ConvUtf8(element) for element in s] - elif isinstance(s, unicode): - return s.encode('utf-8') - else: - return s - -class GpasciiCommunicator(): - '''Communicates with the Delta Tau gpascii programm - ''' - gpascii_ack="\x06\r\n" - gpascii_inp='Input\r\n' - - def connect(self, host, username='root', password='deltatau',prompt='ppmac# '): - p=telnetlib.Telnet(host) - print p.read_until('login: ') - p.write(username+'\n') - print p.read_until('Password: ') - p.write(password+'\n') - print p.read_until(prompt) # command prompt - p.write('gpascii -2\n') # execute gpascii command - print p.read_until(self.gpascii_inp) - return p - +from utilities import * class ShapePath: def __init__(self,args): @@ -75,45 +59,112 @@ class ShapePath: 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]],"mode": ["plot","program","gather","???"]} - #args.cfg={"points": [[100,523],[635,632],[756,213]],"mode": ["plot","program","gather","???"]} - self.cfg=cfg + #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)) - mode=self.args.mode - if mode&1: - #generate random points and sort + 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)) - data=np.random.rand(107,2)*1000 - self.points=data - self.sort_points() - if mode&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(10), range(10)) - data=np.array([xx.reshape(-1),yy.reshape(-1)],dtype=np.float).transpose()*100 - data+=np.random.rand(100,2)*20 - self.points=data - self.sort_points() - if mode&4: + 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): - data=self.points + pts=self.points verb=self.args.verbose - if verb&2: - self.plot_points(data) + #if verb&2: + # self.plot_points(pts) #sort points along y - data=data[data[:, 1].argsort()] - if verb&2: - self.plot_points(data) + pts=pts[pts[:, 1].argsort()] + #if verb&2: + # self.plot_points(pts) #group sorting - cnt=data.shape[0] + 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)) @@ -121,16 +172,16 @@ class ShapePath: a=i*grp_sz #print a,a+grp_sz if i%2: - idx[a:a+grp_sz]=a+data[a:a+grp_sz,0].argsort()[::-1] + idx[a:a+grp_sz]=a+pts[a:a+grp_sz,0].argsort()[::-1] else: - idx[a:a+grp_sz]=a+data[a:a+grp_sz,0].argsort() + idx[a:a+grp_sz]=a+pts[a:a+grp_sz,0].argsort() #print(idx) - data=data[idx] + pts=pts[idx] if verb&2: - self.plot_points(data) + self.plot_points(pts) plt.show() - self.points=data + self.points=pts @staticmethod def onclick(event): @@ -138,18 +189,41 @@ class ShapePath: event.button, event.x, event.y, event.xdata, event.ydata) obj=event.canvas.figure.obj - def plot_points(self,data): + 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(data[:,0],data[:,1],'r.') - hl=ax.plot(data[:,0],data[:,1],'y--') + 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.data=data 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 @@ -182,7 +256,7 @@ Examples:'''+''.join(map(lambda s:cmd+s, exampleCmd))+'\n ' 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('-m', '--mode', type="int", dest='mode', help='mode 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() diff --git a/cfg/utilities.py b/cfg/utilities.py new file mode 100755 index 0000000..fffbd34 --- /dev/null +++ b/cfg/utilities.py @@ -0,0 +1,93 @@ +#!/usr/bin/env python +#*-----------------------------------------------------------------------* +#| | +#| Copyright (c) 2016 by Paul Scherrer Institute (http://www.psi.ch) | +#| | +#| Author Thierry Zamofing (thierry.zamofing@psi.ch) | +#*-----------------------------------------------------------------------* +''' +utilities classes +''' +import logging, h5py, re, zlib, zmq, json +import numpy as np +from libDetXR import * +import time + + +class dotdict(dict): + """dot.notation access to dictionary attributes""" + def __init__(self,arg=None,**kwargs): + if arg!=None: + self.__fill__(arg) + self.__fill__(kwargs) + + def __fill__(self,kw): + for k,v in kw.iteritems(): + if type(v)==dict: + self[k]=dotdict(v) + else: + self[k]=v + if type(v)==list: + for i,w in enumerate(v): + if type(w)==dict: + v[i]=dotdict(w) + pass + + def __dir__(self): + l=dir(object) + #l.extend(self.keys()) + l.extend(map(str,self.keys())) + return l + + def __getattr__(self, attr): + #return self.get(attr) + try: + return self[attr] + except KeyError as e: + raise AttributeError("%r instance has no attribute %r" % (self.__class__, attr)) + + def __repr__(self): + return '<' + dict.__repr__(self)[1:-1] + '>' + + def PrettyPrint(self,indent=0): + for k,v in self.iteritems(): + if type(v)==dotdict: + print ' '*indent,str(k)+':' + v.PrettyPrint(indent+2) + else: + print ' '*indent+str(k)+'\t'+str(v) + + __setattr__= dict.__setitem__ + __delattr__= dict.__delitem__ + #__getattr__= dict.__getattr__ + + +def ConvUtf8(s): + 'convert unicoded json object to ASCII encoded' + #http://stackoverflow.com/questions/956867/how-to-get-string-objects-instead-of-unicode-ones-from-json-in-python + if isinstance(s, dict): + return {ConvUtf8(key): ConvUtf8(value) for key, value in s.iteritems()} + elif isinstance(s, list): + return [ConvUtf8(element) for element in s] + elif isinstance(s, unicode): + return s.encode('utf-8') + else: + return s + +class GpasciiCommunicator(): + '''Communicates with the Delta Tau gpascii programm + ''' + gpascii_ack="\x06\r\n" + gpascii_inp='Input\r\n' + + def connect(self, host, username='root', password='deltatau',prompt='ppmac# '): + p=telnetlib.Telnet(host) + print p.read_until('login: ') + p.write(username+'\n') + print p.read_until('Password: ') + p.write(password+'\n') + print p.read_until(prompt) # command prompt + p.write('gpascii -2\n') # execute gpascii command + print p.read_until(self.gpascii_inp) + return p +