wip path shaping

Readme.md

@@ -45,11 +45,12 @@ Servo Test Motor QBL 4208-41-04-006
 - 200 pole (100 einraster per rev)
 
 
-************************
+******* Path shaping test *****************
-copy configuration after 'save':
+gpasciiCommander --host SAROP11-CPPM-MOT6871 -i
-scp -r root@SAROP11-CPPM-MOT6871:/opt/ppmac/usrflash /scratch
+!mx-stage()
+#1..3$
+&1
+#1..3j/
 
-restore
+cd /home/zamofing_t/Documents/prj/SwissFEL/epics_ioc_modules/ESB_MX/cfg
-scp -r /scratch/usrflash.IDE/* root@SAROP11-CPPM-MOT6871:/opt/ppmac/usrflash 
+./shapepath.py -v255
-$$$
-#4$

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
-

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
-
+from utilities import *
-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
-
 
 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","???"]}
+        #cfg={"points": [[100,523],[635,632],[756,213]],"sequencer":['sort_points()','gen_prog(file="/tmp/shapepath.prg",host="SAROP11-CPPM-MOT6871")','plot_gather()']}
-        #args.cfg={"points": [[100,523],[635,632],[756,213]],"mode": ["plot","program","gather","???"]}
+        #cfg={"sequencer":['gen_rand_points(n=107, scale=1000)','sort_points()','gen_prog(file="/tmp/shapepath.prg",host="SAROP11-CPPM-MOT6871")','plot_gather()']}
-      self.cfg=cfg
+        #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
+      try:
-      if mode&1:
+        self.points=np.array(self.cfg.points)
-        #generate random points and sort
+      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
+        pts=np.random.rand(n,2)*scale
-        self.points=data
+        self.points=pts
-        self.sort_points()
+
-      if mode&2:
+    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))
+        xx,yy=np.meshgrid(range(w), range(h))
-        data=np.array([xx.reshape(-1),yy.reshape(-1)],dtype=np.float).transpose()*100
+        pts=np.array([xx.reshape(-1),yy.reshape(-1)],dtype=np.float).transpose()*pitch
-        data+=np.random.rand(100,2)*20
+        if rnd != 0:
-        self.points=data
+          pts+=(np.random.rand(pts.shape[0],2)*(rnd*pitch))
-        self.sort_points()
+        self.points=pts
-      if mode&4:
+
+    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:
+      #if verb&2:
-        self.plot_points(data)
+      #  self.plot_points(pts)
 
       #sort points along y
-      data=data[data[:, 1].argsort()]
+      pts=pts[pts[:, 1].argsort()]
-      if verb&2:
+      #if verb&2:
-        self.plot_points(data)
+      #  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(pts[:,0],pts[:,1],'r.')
-      hl=ax.plot(data[:,0],data[:,1],'y--')
+      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()

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
+