1010 lines
43 KiB
Python
Executable File
1010 lines
43 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:
|
|
0x01 basic info
|
|
0x02 plot sorting steps
|
|
0x04 list program
|
|
0x08 upload progress
|
|
0x10 plot gather path
|
|
0x20 plot pvt trajectory (before motion)
|
|
0x40 print sync details
|
|
|
|
Gather motor order
|
|
|
|
idx 0 1 2 3 4 5 6
|
|
OLD Motor[3].ActPos Motor[2].ActPos Motor[1].ActPos Motor[3].DesPos Motor[2].DesPos Motor[1].DesPos Gate3[1].Chan[1].UserFlag
|
|
NEW Motor[1].ActPos Motor[2].ActPos Motor[1].DesPos Motor[2].DesPos Gate3[1].Chan[1].UserFlag
|
|
NEW y.ActPos x.ActPos y.DesPos x.DesPos Gate3[1].Chan[1].UserFlag
|
|
OLD->NEW
|
|
0->none
|
|
1->1
|
|
2->0
|
|
3->none
|
|
4->3
|
|
5->2
|
|
|
|
Mot 1: Stage Y Parker MX80L D11 25mm one pole cycle = 13mm = 2048 phase_step
|
|
Mot 2: Stage X Parker MX80L D11 25mm one pole cycle = 13mm = 2048 phase_step
|
|
Mot 3: Rotation stage LS Mecapion MDM-DC06DNC0H 32 poles = 1 rev = 16*2048=32768 phase_step
|
|
Mot 4: Stage X Stada Stepper 670mA 200 poles 1 rev = 100*2048 phase_step (2 stepper motor)
|
|
Mot 5: Stage Z Stada Stepper 670mA 200 poles 1 rev = 100*2048 phase_step (2 stepper motor)
|
|
Enc 6: Interferometer Y
|
|
Enc 7: Interferometer X
|
|
|
|
|
|
tunnel PowerBrick port locally
|
|
------------------------------
|
|
Port 22 on PowerBrick is the ssh server port
|
|
Port 2332 on PowerBrick is the gather port of gather_server
|
|
|
|
PPMAC=SAR-CPPM-EXPMX1
|
|
|
|
rsync -vai ~/Documents/prj/SwissFEL/PBTools/pbtools/gather/gather_server root@$PPMAC:/tmp/
|
|
ssh root@$PPMAC
|
|
LD_LIBRARY_PATH=/opt/ppmac/libppmac/ /tmp/gather_server
|
|
|
|
|
|
|
|
rsync -vai ~/Documents/prj/SwissFEL/PBTools/pbtools/gather/gather_server root@$PPMAC:/tmp
|
|
|
|
ssh -L 10001:localhost:22 root@$PPMAC 'uname -a'
|
|
ssh -L 10002:localhost:2332 root@$PPMAC 'uname -a'
|
|
-> this tunnels port 22 and 2332 of $PPMAC to 10001 and 10002 of localhost
|
|
('uname -a' is just to execute a command and return, because the port remains open)
|
|
|
|
Tests:
|
|
nc localhost 10001
|
|
nc localhost 10002
|
|
|
|
list close ssh tunnel
|
|
---------------------
|
|
lsof -i -n | egrep 'ssh'
|
|
lsof -i -n | grep '127.0.0.1:1000'
|
|
|
|
|
|
|
|
'''
|
|
from __future__ import print_function
|
|
#from __future__ import absolute_import,division,generators,nested_scopes,print_function,unicode_literals,with_statement
|
|
try: raw_input;input=raw_input
|
|
except NameError: pass
|
|
|
|
import os, sys, time
|
|
import numpy as np
|
|
import matplotlib as mpl
|
|
import matplotlib.pyplot as plt
|
|
import subprocess as sprc
|
|
|
|
sys.path.insert(0,os.path.expanduser('~/Documents/prj/SwissFEL/PBTools/'))
|
|
sys.path.insert(0,os.path.expanduser('/tmp/zamofing_t/PBTools/'))
|
|
from pbtools.misc.pp_comm import PPComm
|
|
from pbtools.misc.gather import Gather
|
|
from MXMotion import MotionBase
|
|
|
|
def gen_pvt(p,v, p2pt, ts):
|
|
'''generates a pvt motion
|
|
p: position array
|
|
v: velocity array
|
|
t: time array
|
|
ts: servo cycle time
|
|
!!! it is assumed, that the time intervals are constant !!!
|
|
'''
|
|
n=int(p2pt/ts)
|
|
pvt=np.ndarray((p.shape[0]-1)*n)
|
|
|
|
tt=np.arange(0,p2pt,ts)[:n]
|
|
for i in range(p.shape[0]-1):
|
|
d=p[i]
|
|
c=v[i]
|
|
a=(-2*(p[i+1]-p[i]-v[i]*p2pt)+p2pt*(v[i+1]-v[i]))/p2pt**3
|
|
b=(3*p2pt*(p[i+1]-p[i]-v[i]*p2pt)-p2pt**2*(v[i+1]-v[i]))/p2pt**3
|
|
pvt[i*n:(i+1)*n]=a*tt**3+b*tt**2+c*tt+d
|
|
|
|
return pvt
|
|
|
|
class DebugPlot:
|
|
def __init__(self,obj=None):
|
|
if obj is None:
|
|
self.load_npz()
|
|
elif type(obj)==str:
|
|
self.load_npz(obj)
|
|
else:
|
|
self.set_data(obj)
|
|
|
|
def plot_gen_pvt(self,pv):
|
|
# pv is an array of posx posy velx vely
|
|
#pv=pv[5:10,:]
|
|
#pv=pv[5:-4,:]
|
|
p2pt=self.meta['pt2pt_time'] # ms step between samples
|
|
ts=self.meta['srv_per'] # sampling time in ms
|
|
n=int(p2pt/ts) # servo cycle between samples
|
|
k=pv.shape[0] # number of unique samples
|
|
t=np.arange(0, p2pt*k, p2pt) # time array of trajectory
|
|
ppx=gen_pvt(pv[:,0], pv[:,2], p2pt, ts)
|
|
ppy=gen_pvt(pv[:,1], pv[:,3], p2pt, ts)
|
|
tt=np.arange(0, n*(k-1))*ts # time array of trajectory
|
|
|
|
fig=plt.figure()
|
|
ax1=fig.add_subplot(2, 1, 1)
|
|
ax2=fig.add_subplot(2, 1, 2)
|
|
#ax.xaxis.set_ticks(t)
|
|
ax1.stem(t, pv[:,0], '-r',use_line_collection=True)
|
|
ax2.stem(t, pv[:,1], '-g',use_line_collection=True)
|
|
|
|
ax1.plot(tt, ppx, '-r', label='x')
|
|
ax2.plot(tt, ppy, '-g', label='y')
|
|
#ax.legend(loc='best')
|
|
|
|
ax=plt.figure().add_subplot(1, 1, 1)
|
|
ax.plot(pv[:,0], pv[:,1], '.r', label='pft')
|
|
ax.plot(ppx, ppy, '-c', label='pft')
|
|
ax.invert_xaxis()
|
|
ax.invert_yaxis()
|
|
plt.axis('equal')
|
|
|
|
ax.legend(loc='best')
|
|
plt.show(block=False)
|
|
|
|
# ### frequency plots ###
|
|
# fig=plt.figure()
|
|
# ax=fig.add_subplot(1,1,1)#ax=plt.gca()
|
|
#
|
|
# #remove linear slope
|
|
# sx=ppx-(pv[-1,0]-pv[0,0])*np.arange(ppx.shape[0])
|
|
# sy=ppy-(pv[-1,1]-pv[0,1])*np.arange(ppy.shape[0])
|
|
#
|
|
# #normalize with l -> value of k means amplitude of k at a given frequency
|
|
# ppxf=np.fft.rfft(sx)/(2*n)
|
|
# ppyf=np.fft.rfft(sy)/(2*n)
|
|
#
|
|
# f=np.fft.rfftfreq(ppx.shape[0], d=ts*1E-3)
|
|
# f=f[1:] #remove dc value frequency
|
|
#
|
|
# mag=abs(ppxf[1:])#; mag=20*np.log10(abs(mag))
|
|
# ax.semilogx(f,mag,'-b',label='ppx') # Bode magnitude plot
|
|
# mag=abs(ppyf[1:])#; mag=20*np.log10(abs(mag))
|
|
# ax.semilogx(f,mag,'-g',label='ppy') # Bode magnitude plot
|
|
# #ax.yaxis.set_label_text('dB ampl')
|
|
# ax.yaxis.set_label_text('ampl')
|
|
# ax.xaxis.set_label_text('frequency [Hz]')
|
|
# plt.grid(True)
|
|
#
|
|
# ax.legend(loc='best')
|
|
# plt.show(block=False)
|
|
return (tt,ppx,ppy)
|
|
|
|
#@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
|
|
|
|
@staticmethod
|
|
def plot_points(pts):
|
|
fig=plt.figure()
|
|
ax = fig.add_subplot(1,1,1)
|
|
ax.invert_xaxis();ax.invert_yaxis()
|
|
#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
|
|
plt.axis('equal')
|
|
#self.ax=ax
|
|
#self.hl=hl
|
|
|
|
|
|
def analyze_trigger(self):
|
|
if hasattr(self,'idxTrigger'): return
|
|
pts=self.pts # X,Y array
|
|
rec=self.rec # yA,xA,yD,xD,trig
|
|
lenRec=rec.shape[0]
|
|
lenPts=pts.shape[0]
|
|
|
|
idxTrigger=rec[:,4]
|
|
idxTrigger=np.where(np.diff(idxTrigger)==1)[0]+1
|
|
idxInPos=[] #first point at idx 0
|
|
idx=0
|
|
for i in range(lenPts):
|
|
l=rec[idx:,(3,2)]-pts[i,:]
|
|
l2=l[:,0]**2+l[:,1]**2
|
|
try:
|
|
ofs=l2.argmin()
|
|
except ValueError as e:
|
|
raise e #should never happen
|
|
break
|
|
idx+=ofs
|
|
idxInPos.append(idx)
|
|
|
|
idxInPos = np.array(idxInPos)
|
|
#select only triggers on a target point
|
|
i=max(0,np.abs(idxTrigger-idxInPos[1]).argmin()-1)
|
|
j=i+idxInPos.shape[0]
|
|
idxTrigger=idxTrigger[i:j]
|
|
self.idxInPos=idxInPos
|
|
self.idxTrigger=idxTrigger
|
|
|
|
def plot_trigger_jitter(self):
|
|
self.analyze_trigger()
|
|
ts=self.meta['srv_per']*self.meta['acq_per']
|
|
idxTrigger=self.idxTrigger
|
|
idxInPos=self.idxInPos
|
|
n=min(idxTrigger.shape[0],idxInPos.shape[0])-1
|
|
jitter = idxTrigger[1:n]-idxInPos[1:n]
|
|
pts=self.pts # X,Y array
|
|
rec=self.rec # yA,xA,yD,xD,trig
|
|
|
|
fig = plt.figure('trigger jitter')
|
|
ax = fig.add_subplot(1, 1, 1)
|
|
hl = []
|
|
hl += ax.plot(jitter * ts, 'b-', label='jitter')
|
|
ax.xaxis.set_label_text('position idx')
|
|
ax.yaxis.set_label_text('jitter motion (ms)')
|
|
|
|
|
|
rec = self.rec # yA,xA,yD,xD,trig
|
|
ts=self.meta['srv_per']*self.meta['acq_per']
|
|
fig = plt.figure('shot position error')
|
|
ax = fig.add_subplot(1, 1, 1)
|
|
|
|
#errx = rec[idxTrigger, 1] - rec[idxInPos, 3]
|
|
#erry = rec[idxTrigger, 0] - rec[idxInPos, 2]
|
|
n=idxTrigger.shape[0]
|
|
errx = rec[idxTrigger, 1] - pts[:n, 0]
|
|
erry = rec[idxTrigger, 0] - pts[:n, 1]
|
|
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('target point index')
|
|
ax.yaxis.set_label_text('pos-error um')
|
|
legend = ax.legend(loc='best', shadow=True)
|
|
print('shot average error x %g um, y %g um, %g um' % (np.abs(errx).mean(), np.abs(erry).mean(), err.mean()))
|
|
plt.show(block=False)
|
|
|
|
|
|
def plot_pos_error(self):
|
|
rec = self.rec # yA,xA,yD,xD,trig
|
|
ts=self.meta['srv_per']*self.meta['acq_per']
|
|
fig = plt.figure('position error')
|
|
ax = fig.add_subplot(1, 1, 1)
|
|
|
|
t=np.arange(rec.shape[0],dtype=np.uint32)
|
|
errx = rec[:, 1] - rec[:, 3]
|
|
erry = rec[:, 0] - rec[:, 2]
|
|
err = np.sqrt(errx ** 2 + erry ** 2)
|
|
|
|
hl = []
|
|
hl += ax.plot(t, errx, 'b-', label='x-error')
|
|
hl += ax.plot(t, erry, 'g-', label='y-error')
|
|
hl += ax.plot(t, err, 'r-', label='error')
|
|
ax.xaxis.set_label_text('ms (timebase: %g ms per data point)' % ts)
|
|
ax.yaxis.set_label_text('pos-error um')
|
|
legend = ax.legend(loc='best', shadow=True)
|
|
print('motion average error x %g um, y %g um, %g um' % (np.abs(errx).mean(), np.abs(erry).mean(), err.mean()))
|
|
plt.show(block=False)
|
|
|
|
def plot_trajectory(self):
|
|
pts = self.pts # X,Y array
|
|
rec = self.rec # yA,xA,yD,xD,trig
|
|
fig = plt.figure('trajectory')
|
|
ax = fig.add_subplot(1, 1, 1)
|
|
ax.invert_xaxis()
|
|
ax.invert_yaxis()
|
|
# hl=ax[0].plot(x, y, color=col)
|
|
hl = ax.plot(pts[:, 0], pts[:, 1], 'r.', label='points')
|
|
hl += ax.plot(pts[:, 0], pts[:, 1], 'y--', label='direct')
|
|
hl += ax.plot(rec[:, 3], rec[:, 2], 'b-', label='DesPos') # desired path
|
|
hl += ax.plot(rec[:, 1], rec[:, 0], 'g-', label='ActPos') # actual path
|
|
try:
|
|
pvt = self.pvt
|
|
except AttributeError:
|
|
pass
|
|
else:
|
|
hl = ax.plot(pvt[1], pvt[2], 'c--', label='SimPos') # simulated path
|
|
|
|
fig2 = plt.figure('time line')
|
|
ax2 = fig2.add_subplot(1, 1, 1)
|
|
hl2 = ax2.plot(rec[:, 2], 'r-', label='desPos Mot1')
|
|
hl2 += ax2.plot(rec[:, 3], 'g-', label='desPos Mot2')
|
|
|
|
idxTrigger = rec[:, 4]
|
|
idxTrigger = np.where(np.diff(idxTrigger) == 1)[0] + 1
|
|
if idxTrigger.shape[0] > 0:
|
|
hl += ax.plot(rec[idxTrigger, 1], rec[idxTrigger, 0], 'xr', label='trig') # actual path
|
|
hl2 += ax2.plot(rec[:, 4]*10, 'b-', label='trigger')
|
|
|
|
ax.xaxis.set_label_text('x-pos um')
|
|
ax.yaxis.set_label_text('y-pos um')
|
|
ax.axis('equal')
|
|
ax.legend(loc='best')
|
|
# cid = fig.canvas.mpl_connect('button_press_event', self.onclick)
|
|
# fig.obj=self
|
|
ax2.legend(loc='best')
|
|
plt.show(block=False)
|
|
|
|
def plot_gather(self,mode=255):
|
|
try:
|
|
meta=self.meta
|
|
pts=self.pts # X,Y array
|
|
rec = self.rec # yA,xA,yD,xD,trig
|
|
except AttributeError as e:
|
|
print('plot_gather(): '+str(e)+': no data acquired yet')
|
|
return
|
|
|
|
if mode&1:
|
|
self.plot_trajectory()
|
|
|
|
if mode&2:
|
|
self.plot_pos_error()
|
|
|
|
if mode&4:
|
|
self.plot_bode(xy=(3,1),mode=31,db=True) # FX
|
|
self.plot_bode(xy=(2,0),mode=31,db=True) # FY
|
|
|
|
if mode&8:
|
|
self.plot_trigger_jitter()
|
|
|
|
plt.show(block=False)
|
|
|
|
def plot_bode(self,xy=(0,1),mode=25,db=True):
|
|
'''displays a bode plot of the data
|
|
Y(s)=G(s)*X(s)
|
|
Y= output signal
|
|
X= input signal
|
|
xy are the row indexes of input and output signal
|
|
meta= meta information (dictionary) of data
|
|
|
|
mode bits: (+mean default)
|
|
+1: display time signal
|
|
2: display bode of X(s) signal
|
|
4: display bode of Y(s) signal
|
|
+8: display bode of G(s) signal
|
|
+16: clip frequencies out of minFrq,maxFrq
|
|
'''
|
|
meta=self.meta
|
|
pts=self.pts # X,Y array
|
|
rec = self.rec # yA,xA,yD,xD,trig
|
|
strMot=('FY.act','FX.act','FY.des','FX.des')
|
|
ts=self.meta['srv_per']*self.meta['acq_per']*1E-3 #0.2ms
|
|
num=rec.shape[0]
|
|
#rngMin=int(.01/ts);rngMax=int(num-1.001/ts) #0.01s from start 1.01 sec before end
|
|
rngMin=int(.01/ts);rngMax=rngMin+int((pts.shape[0]-2)*.01/ts)
|
|
|
|
num=rngMax-rngMin
|
|
|
|
minFrq=1/(num*ts)#minimal frq to show bode
|
|
maxFrq=1/(2*ts) #maximal frq to show bode
|
|
xIdx,yIdx=xy
|
|
#remove DC value
|
|
x=rec[rngMin:rngMax,xIdx]-rec[rngMin,xIdx]
|
|
y=rec[rngMin:rngMax,yIdx]-rec[rngMin,yIdx]
|
|
#make last value same as first (nice periodicity)
|
|
x=x-(x[-1]*np.arange(num)/(num-1.))
|
|
y=y-(y[-1]*np.arange(num)/(num-1.))
|
|
if mode&1:
|
|
t = ts*np.arange(num)
|
|
fig=plt.figure('raw {}->{}'.format(strMot[xIdx],strMot[yIdx]))
|
|
ax=fig.gca()
|
|
ax.plot(t,x,'b')
|
|
ax.plot(t,y,'g')
|
|
|
|
fig=plt.figure('bode {}->{}'.format(strMot[xIdx],strMot[yIdx]))
|
|
ax1=fig.add_subplot(2,1,1)
|
|
ax1.grid(True)
|
|
ax1.yaxis.set_label_text('Amplitude'+ (' [dB]' if db else ''))
|
|
ax1.axvline(minFrq,c='k');ax1.axvline(maxFrq,c='k')
|
|
|
|
ax2=fig.add_subplot(2,1,2, sharex = ax1)
|
|
ax2.grid(True)
|
|
ax2.xaxis.set_label_text('Frequency [Hz]')
|
|
ax2.yaxis.set_label_text('Phase [degree]')
|
|
|
|
ftX=np.fft.rfft(x)
|
|
ftY=np.fft.rfft(y)
|
|
|
|
fMax=.5/ts #fs=1/ts, fMax=1/2fs
|
|
n=ftX.shape[0]
|
|
f=np.arange(n)*fMax/(n-1)
|
|
|
|
if mode&16:
|
|
i=int(minFrq*num*ts); j=int(maxFrq*num*ts); #print(w[i],w[j])
|
|
f=f[i:j+1]
|
|
ftX=ftX[i:j+1]
|
|
ftY=ftY[i:j+1]
|
|
ftLst=[]
|
|
if mode&2:
|
|
ftLst.append((ftX,'b','input'))
|
|
if mode&4:
|
|
ftLst.append((ftY,'g','output'))
|
|
if mode&8:
|
|
ftLst.append((ftY/ftX,'r','out/inp'))
|
|
|
|
for ft,c,s in ftLst:
|
|
phase=np.angle(ft)
|
|
phase=np.degrees(np.unwrap(phase))
|
|
mag=np.abs(ft) #ftY)/np.abs(ftX)
|
|
if db:
|
|
magDb=20*np.log10(mag) #in decibel (20=10*2: factor 2 because rfft only half)
|
|
ax1.semilogx(f,magDb,c,label=s) # Bode magnitude plot
|
|
else:
|
|
ax1.semilogx(f, mag, c,label=s) # Bode magnitude plot
|
|
ax2.semilogx(f,phase,c,label=s) # Bode phase plot
|
|
ax2.set_ylim(-360,360)
|
|
ax2.legend(loc='best')
|
|
plt.show(block=False)
|
|
|
|
def load_npz(self,fn='/tmp/shapepath.npz'):
|
|
fh=np.load(fn)
|
|
for k,v in fh.iteritems():
|
|
setattr(self,k,v)
|
|
self.meta=self.meta.item()
|
|
|
|
def set_data(self,spObj):
|
|
self.meta=spObj.meta
|
|
self.pts=spObj.points
|
|
try: self.rec=spObj.rec
|
|
except AttributeError: pass
|
|
try: self.pvt=spObj.pvt
|
|
except AttributeError: pass
|
|
|
|
|
|
class ShapePath(MotionBase):
|
|
def __init__(self,comm, gather, verbose,**kwargs):
|
|
MotionBase.__init__(self,comm, gather, verbose, **kwargs)
|
|
|
|
def gen_swissmx_points(self,flipx=False,flipy=False,ofs=(0,0),width=1000):
|
|
'generathe a path that writes swissfel'
|
|
#string from inkscape path of the drawing
|
|
d="m 524.7061,637.31536 3.54883,0 3.54882,0 3.54883,0 0,-4.20801 0,-4.20801 0,-4.208 0,-4.20801 4.22949,0 4.22949,0 4.2295,0 4.22949,0 0,-3.55957 0,-3.55957 0,-3.55957 0,-3.55957 -4.22949,0 -4.2295,0 -4.22949,0 -4.22949,0 0,-4.22949 0,-4.2295 0,-4.22949 0,-4.22949 -3.54883,0 -3.54882,0 -3.54883,0 -3.54883,0 0,4.22949 0,4.22949 0,4.2295 0,4.22949 -4.20752,0 -4.20752,0 -4.20752,0 -4.20752,0 0,3.55957 0,3.55957 0,3.55957 0,3.55957 4.20752,0 4.20752,0 4.20752,0 4.20752,0 0,4.20801 0,4.208 0,4.20801 0,4.20801 -11.87126,0.36152 -12.12171,-0.13934 -2.52941,3.93977 -2.57238,3.94369 -2.50854,3.88614 -2.50731,3.91767 -2.49035,3.88268 -2.50987,3.91244 -2.50453,3.88732 -2.51897,3.9189 -6.39782,5.72802 -6.63782,6.70894 -3.21517,5.11464 -3.3404,5.32333 -3.08995,5.11464 -3.17343,5.15637 -16.69223,0.0698 5.55908,0 5.55909,0 5.55908,0 3.18604,-5.17432 3.18603,-5.17431 3.18604,-5.17432 3.18603,-5.17431 3.17481,5.17431 3.1748,5.17432 3.17481,5.17431 3.1748,5.17432 5.59229,0 5.59228,0 5.59229,0 5.59228,0 -2.74121,-4.15283 -2.74121,-4.15283 -2.74121,-4.15283 -2.74121,-4.15284 -2.74122,-4.15283 -2.74121,-4.15283 -2.74121,-4.15283 -2.74121,-4.15283 2.50488,-3.90015 2.50489,-3.90015 2.50488,-3.90014 2.50488,-3.90015 2.50488,-3.90015 2.50489,-3.90015 2.50488,-3.90014 2.50488,-3.90015 -5.42724,0 -5.42725,0 -5.42724,0 -5.42725,0 -2.76855,4.95508 -2.76856,4.95508 -2.76855,4.95508 -2.76856,4.95508 -2.85644,-4.95508 -2.85645,-4.95508 -2.85644,-4.95508 -2.85645,-4.95508 -5.48193,0 -5.48194,0 -5.48194,0 -5.48193,0 2.52686,3.8562 2.52685,3.8562 2.52686,3.8562 2.52686,3.85621 2.52685,3.8562 2.52686,3.8562 2.52685,3.8562 2.52686,3.8562 -2.77954,4.19678 -2.77954,4.19678 -2.77954,4.19677 -2.77955,4.19678 -2.77954,4.19678 -2.77954,4.19678 -2.77954,4.19677 -2.77954,4.19678 -4.91638,0 -4.91638,0 -4.91639,0 -4.91638,0 -4.91638,0 -4.91638,0 -4.91638,0 -4.91638,0 -4.91639,0 -4.91638,0 -4.91638,0 -4.91638,0 -4.91638,0 -4.91639,0 -4.91638,0 -4.91638,0 4.07568,0 4.07568,0 4.07569,0 4.07568,0 0,-6.14136 0,-6.14135 0,-6.14136 0,-6.14136 0,-6.14136 0,-6.14135 0,-6.14136 0,-6.14136 1.57105,6.14136 1.57104,6.14136 1.57104,6.14135 1.57105,6.14136 1.57105,6.14136 1.57104,6.14136 1.57104,6.14135 1.57105,6.14136 3.68066,0 3.68067,0 3.68067,0 3.68066,0 1.57642,-6.14136 1.57641,-6.14135 1.57642,-6.14136 1.57641,-6.14136 1.57642,-6.14136 1.57642,-6.14135 1.57641,-6.14136 1.57642,-6.14136 0,6.14136 0,6.14136 0,6.14135 0,6.14136 0,6.14136 0,6.14136 0,6.14135 0,6.14136 4.06494,0 4.06494,0 4.06494,0 4.06494,0 0,-8.05298 0,-8.05298 0,-8.05298 0,-8.05297 0,-8.05298 0,-8.05298 0,-8.05298 0,-8.05298 -6.52588,0 -6.52588,0 -6.52587,0 -6.52588,0 -1.25781,4.8999 -1.25782,4.89991 -1.25781,4.8999 -1.25781,4.8999 -1.25781,4.8999 -1.25782,4.89991 -1.25781,4.8999 -1.25781,4.8999 -1.26343,-4.8999 -1.26343,-4.8999 -1.26343,-4.89991 -1.26343,-4.8999 -1.26342,-4.8999 -1.26343,-4.8999 -1.26343,-4.89991 -1.26343,-4.8999 -6.54785,0 -6.54785,0 -6.54785,0 -6.54785,0 0,8.05298 0,8.05298 0,8.05298 0,8.05298 0,8.05297 0,8.05298 0,8.05298 -4.25755,8.13646 -8.40743,0.19687 -8.40743,0.19687 -8.40743,0.19687 -8.40743,0.19687 5.93521,0.22812 8.09742,-0.56079 6.18579,-1.6814 4.55883,-2.66919 3.13062,-3.43823 1.84571,-3.87866 0.61523,-3.98853 -0.58179,-3.83373 -1.74634,-3.50416 -2.802,-2.95581 -3.83472,-2.18676 -5.49316,-1.60401 -7.77832,-1.20849 -7.64649,-1.58204 -1.75781,-2.59179 1.36328,-2.59375 4.4375,-1.09766 5.09766,1.40625 2.19727,3.29492 4.24072,-0.41748 4.24073,-0.41748 4.24072,-0.41748 4.24072,-0.41748 -1.98804,-4.09741 -2.44946,-3.15259 -2.97778,-2.3291 -3.65894,-1.62598 -5.05371,-0.95629 -7.25098,-0.3191 -7.10766,0.41748 -5.50367,1.25244 -4.19677,2.05494 -3.18604,2.91186 -2.01099,3.65796 -0.67065,4.29517 0.61523,3.98852 1.84571,3.5271 2.78002,2.823 3.32935,1.87817 5.06421,1.42822 7.89868,1.56006 7.69141,1.84571 2.02148,2.98828 -1.53906,2.85742 -5.58008,1.53711 -5.27344,-1.36133 -3.07617,-4.52734 -4.43847,0.41748 -4.43848,0.41748 -4.43848,0.41748 -4.43847,0.41748 2.50488,5.95459 4.43848,4.4165 3.18313,1.59592 4.10031,1.14017 -3.65979,0.0939 -5.9713,6e-5 -5.97131,5e-5 -5.9713,6e-5 -5.9713,6e-5 -5.9713,5e-5 -5.97131,6e-5 -5.9713,5e-5 -5.9713,6e-5 5.34491,0.81842 8.09742,-0.56079 6.18579,-1.6814 4.55883,-2.66919 3.13062,-3.43823 1.84571,-3.87866 0.61523,-3.98853 -0.58179,-3.83373 -1.74634,-3.50416 -2.802,-2.95581 -3.83472,-2.18676 -5.49316,-1.60401 -7.77832,-1.20849 -7.64649,-1.58204 -1.75781,-2.59179 1.36328,-2.59375 4.4375,-1.09766 5.09766,1.40625 2.19727,3.29492 4.24072,-0.41748 4.24073,-0.41748 4.24072,-0.41748 4.24072,-0.41748 -1.98804,-4.09741 -2.44946,-3.15259 -2.97778,-2.3291 -3.65894,-1.62598 -5.05371,-0.95629 -7.25098,-0.3191 -7.10766,0.41748 -5.50367,1.25244 -4.19677,2.05494 -3.18604,2.91186 -2.01099,3.65796 -0.67065,4.29517 0.61523,3.98852 1.84571,3.5271 2.78002,2.823 3.32935,1.87817 5.06421,1.42822 7.89868,1.56006 7.69141,1.84571 2.02148,2.98828 -1.53906,2.85742 -5.58008,1.53711 -5.27344,-1.36133 -3.07617,-4.52734 -4.43847,0.41748 -4.43848,0.41748 -4.43848,0.41748 -4.43847,0.41748 2.50488,5.95459 4.43848,4.4165 3.18313,1.59592 4.10031,1.14017 -3.06953,-0.0416 -3.06952,-0.0416 -8.58102,-0.0261 -10.12782,-0.0261 -7.03422,-0.0261 -8.58102,-0.0261 4.47168,0 6.6151,0 2.32826,0 4.47168,0 0,-5.83374 0,-5.83374 0,-5.83374 0,-5.83374 0,-5.83374 0,-5.83374 0,-5.83374 0,-5.83374 -4.47168,0 -4.47168,0 -4.47168,0 0,-5.5796 4.47168,0 4.47168,0 4.47168,0 0,-6.08691 0,-6.08692 -4.47168,0 -4.47168,0 -4.47168,0 -4.47168,0 0,6.08692 0,6.08691 0,5.5796 0,5.83374 0,5.83374 0,5.83374 0,5.83374 0,5.83374 0,5.83374 0,5.83374 -3.67318,5.83374 -8.7308,0 -10.73079,0 -6.7308,0 -9.10563,0 -2.25201,0.007 -8.72971,0.0266 -7.53755,-0.0442 -9.68477,0.0107 -6.3443,0 3.99902,0 3.99902,0 3.99903,0 3.99902,0 2.28516,-7.02002 2.28516,-7.02002 2.28516,-7.02002 2.28516,-7.02002 2.36181,7.02002 2.36182,7.02002 2.36181,7.02002 2.36182,7.02002 3.97705,0 3.97705,0 3.97705,0 3.97705,0 2.14795,-5.83374 2.14795,-5.83374 2.14795,-5.83374 2.14795,-5.83374 2.14795,-5.83374 2.14795,-5.83374 2.14795,-5.83374 2.14795,-5.83374 -4.2959,0 -4.2959,0 -4.2959,0 -4.2959,0 -0.93921,3.67505 -0.93921,3.67505 -0.93921,3.67505 -0.93921,3.67505 -0.9392,3.67504 -0.93921,3.67505 -0.93921,3.67505 -0.93921,3.67505 -1.23047,-3.67505 -1.23047,-3.67505 -1.23047,-3.67505 -1.23047,-3.67504 -1.23046,-3.67505 -1.23047,-3.67505 -1.23047,-3.67505 -1.23047,-3.67505 -4.03223,0 -4.03222,0 -4.03223,0 -4.03223,0 -1.18652,3.67505 -1.18653,3.67505 -1.18652,3.67505 -1.18653,3.67505 -1.18652,3.67504 -1.18652,3.67505 -1.18653,3.67505 -1.18652,3.67505 -0.93921,-3.67505 -0.93921,-3.67505 -0.93921,-3.67505 -0.93921,-3.67504 -0.9392,-3.67505 -0.93921,-3.67505 -0.93921,-3.67505 -0.93921,-3.67505 -4.32862,0 -4.32861,0 -4.32862,0 -4.32861,0 2.16431,5.83374 2.1643,5.83374 2.16431,5.83374 2.16431,5.83374 2.16431,5.83374 2.1643,5.83374 2.16431,5.83374 -3.84635,5.83374 -5.60781,0.003 -5.6078,0.003 -5.60781,0.003 -5.6078,0.003 -5.4839,-1.59358 0,0 5.47119,-3.35034 4.10888,-4.60278 2.5708,-5.4712 0.85694,-5.95459 -0.64868,-5.02123 -1.94507,-4.51587 -3.32837,-3.91114 -4.88843,-3.20801 -7.482173,-2.87842 -5.1337,-1.42273 -6.06186,-1.41174 -6.67969,-2.37304 -1.44922,-2.76758 1.75782,-3.56055 5.22851,-1.49414 6.5918,1.97852 1.99951,2.5708 1.16455,3.75732 4.69141,-0.2749 4.691403,-0.2749 4.6914,-0.27491 4.69141,-0.2749 -0.94483,-4.66918 -1.604,-3.98804 -2.26318,-3.30688 -2.92236,-2.62574 -3.59802,-2.01858 -4.334103,-1.44162 -5.0702,-0.86484 -5.80627,-0.28824 -4.76547,0.1593 -4.23282,0.47791 -6.86695,1.91162 -5.04223,2.98828 -3.61401,3.95507 -2.14283,4.53687 -0.7146,4.82251 1.40625,6.88892 4.21875,5.54858 3.26035,2.31812 4.19986,2.07641 5.13919,1.83472 6.07834,1.59302 6.54785,1.81226 3.64746,1.92211 2.19727,4.48242 -2.33008,4.65821 -6.54688,1.97851 -5.05371,-0.97827 -3.73535,-2.93384 -1.57153,-2.9663 -0.93433,-4.06495 -4.73486,0.29688 -4.73487,0.29687 -4.73486,0.29688 -4.73486,0.29687 0.76065,4.6637 1.44711,4.23523 2.13376,3.80676 2.82059,3.3783 3.79577,2.76855 5.0592,1.97754 6.32264,1.18652 7.58606,0.39551 9.481626,-0.95145 -7.224723,-0.043 -7.224724,-0.043 -7.224723,-0.043 -7.224723,-0.043 -7.224723,-0.043 -7.224723,-0.043 -7.224724,-0.043 -7.224723,-0.043"
|
|
d=d.split()
|
|
pts=np.ndarray((len(d)-1,2),dtype=np.float)
|
|
for i in xrange(pts.shape[0]):
|
|
pts[i,:]=map(float,d[i+1].split(','))
|
|
|
|
pts[0,:]=(0,0)
|
|
pts=pts.cumsum(0)
|
|
pts=pts[::-1,:]
|
|
pts=pts-pts[0]
|
|
pts*=width/pts[:,0].max()
|
|
if flipx: pts[:,0]=-pts[:,0]
|
|
if not flipy: pts[:,1]=-pts[:,1]
|
|
pts+=ofs
|
|
self.points=pts
|
|
|
|
verb=self.verbose
|
|
if verb&0x02:
|
|
self.plot_points(pts)
|
|
plt.show(block=False)
|
|
|
|
|
|
def gen_swissfel_points(self,flipx=False,flipy=False,ofs=(0,0),width=1000):
|
|
'generathe a path that writes swissfel'
|
|
#string from inkscape path of the drawing
|
|
d="m 15.801613,951.54022 -1.655274,-0.17578 -1.809082,-0.52002 0,-1.52344 1.765137,0.76172 1.699219,0.25635 1.955566,-0.49805 0.688477,-1.4209 -0.498047,-1.25976 -1.618652,-0.68115 -0.900879,-0.17578 -1.426392,-0.42298 -0.968628,-0.60974 -0.739746,-1.95557 0.254516,-1.29638 0.76355,-0.98877 1.217652,-0.62622 1.602173,-0.20874 1.567383,0.13916 1.6333,0.41748 0,1.44287 -1.589355,-0.60059 -1.442871,-0.19775 -1.071167,0.11719 -0.796509,0.35156 -0.651856,1.33301 0.432129,1.09863 1.655274,0.59326 0.893555,0.18311 1.437377,0.43579 1.001587,0.67749 0.593262,0.92651 0.197754,1.19751 -0.267334,1.3971 -0.802002,1.01257 -1.311035,0.61523 2.995605,-0.004 2.995606,-0.004 -0.536499,-2.05078 -0.536499,-2.05078 -0.536499,-2.05079 -0.536499,-2.05078 1.347656,0 0.421142,1.60034 0.421143,1.60034 0.421143,1.60035 0.421142,1.60034 0.419312,-1.60034 0.419311,-1.60035 0.419312,-1.60034 0.419311,-1.60034 1.589356,0 0.421142,1.60034 0.421143,1.60034 0.421143,1.60035 0.421142,1.60034 0.419312,-1.60034 0.419311,-1.60035 0.419312,-1.60034 0.419311,-1.60034 1.347657,0 -0.536499,2.05078 -0.5365,2.05079 -0.536499,2.05078 -0.536499,2.05078 -1.589355,0 -0.441284,-1.68091 -0.441285,-1.68091 -0.441284,-1.6809 -0.441284,-1.68091 -0.443115,1.68091 -0.443116,1.6809 -0.443115,1.68091 -0.443115,1.68091 2.330933,-8e-5 2.330933,-8e-5 2.330932,-8e-5 2.330933,-8e-5 0,-2.05078 0,-2.05078 0,-2.05079 0,-2.05078 0.673828,-1.48681 0.673828,0 0,-0.85327 0,-0.85327 -0.673828,0 -0.673828,0 0,0.85327 0,0.85327 0.673828,1.48681 0.673828,0 0,2.05078 0,2.05079 0,2.05078 0,2.05078 2.545166,0.1062 2.545166,0.1062 -1.376953,-0.13183 -1.501465,-0.38086 0,-1.3916 1.472168,0.58593 1.435547,0.19043 1.464844,-0.32226 0.512695,-0.92285 -0.373535,-0.84229 -1.038789,-0.41935 -1.048614,-0.25448 -1.078491,-0.33325 -0.730591,-0.47241 -0.55664,-1.50147 0.205078,-1.02539 0.615234,-0.76172 0.99243,-0.47241 1.336672,-0.15747 1.40625,0.10986 1.21582,0.32959 0,1.27442 -1.186523,-0.43946 -1.274414,-0.14648 -1.508789,0.30762 -0.498047,0.92285 0.358886,0.73975 0.763094,0.30189 0.76857,0.22893 0.785513,0.20842 0.813927,0.24037 0.809326,0.49988 0.455932,0.66833 0.151978,0.89173 -0.227051,1.02355 -0.681152,0.78553 -1.071167,0.49988 3.205262,0.0833 3.205261,0.0833 -1.376953,-0.13183 -1.501465,-0.38086 0,-1.3916 1.472168,0.58593 1.435547,0.19043 1.464843,-0.32226 0.512696,-0.92285 -0.373536,-0.84229 -1.038789,-0.41935 -1.048613,-0.25448 -1.078491,-0.33325 -0.730591,-0.47241 -0.556641,-1.50147 0.205078,-1.02539 0.615235,-0.76172 0.99243,-0.47241 1.336672,-0.15747 1.40625,0.10986 1.21582,0.32959 0,1.27442 -1.186524,-0.43946 -1.274414,-0.14648 -1.508789,0.30762 -0.498047,0.92285 0.358887,0.73975 0.805073,0.3095 0.76935,0.22988 0.764915,0.20509 0.791765,0.23514 0.809327,0.49988 0.455932,0.66833 0.151978,0.89173 -0.227051,1.02355 -0.681152,0.78553 -1.071167,0.49988 2.095642,-0.0229 2.095642,-0.0229 0,-1.36688 0,-1.36688 0,-1.36689 0,-1.36688 0,-1.36688 0,-1.36688 0,-1.36689 0,-1.36688 1.571045,0 1.571045,0 1.571045,0 1.571045,0 0,1.24512 -1.201172,0 -1.201172,0 -1.201172,0 -1.201172,0 0,0.80566 0,0.80567 0,0.80566 0,0.80566 1.083984,0 1.083985,0 1.083984,0 1.083985,0 0,1.24512 -1.083985,0 -1.083984,0 -1.083985,0 -1.083984,0 0,1.30554 0,1.30555 0,1.30554 0,1.30554 3.581543,0 3.581543,0 0,-1.36688 0,-1.36688 0,-1.36689 0,-1.36688 0,-1.36688 0,-1.36688 0,-1.36689 0,-1.36688 1.728516,0 1.728516,0 1.728515,0 1.728516,0 0,1.24512 -1.358643,0 -1.358642,0 -1.358643,0 -1.358643,0 0,0.80932 0,0.80933 0,0.80933 0,0.80932 1.30188,0 1.30188,0 1.30188,0 1.30188,0 0,1.24512 -1.30188,0 -1.30188,0 -1.30188,0 -1.30188,0 0,0.9906 0,0.9906 0,0.9906 0,0.9906 1.391602,0 1.391601,0 1.391602,0 1.391602,0 0,1.24512 -3.587581,3.8e-4 -1.964972,0 3.702844,0 4.295998,0 0,-1.36733 0,-1.36688 0,-1.36689 0,-1.36688 0,-1.36688 0,-1.36689 0,-1.36688 0,-1.36688 1.479492,0 0,1.21124 0,1.21124 0,1.21125 0,1.21124 0,1.21124 0,1.21124 0,1.21125 0,1.21124 1.331177,0 1.331177,0 1.331176,0 1.331177,0 0,1.24512 -1.70105,0 -1.701049,0 -1.70105,0"
|
|
d=d.split()
|
|
pts=np.ndarray((len(d)-1,2),dtype=np.float)
|
|
for i in xrange(pts.shape[0]):
|
|
pts[i,:]=map(float,d[i+1].split(','))
|
|
|
|
pts[0,:]=(0,0)
|
|
pts=pts.cumsum(0)
|
|
pts=pts[::-1,:]
|
|
pts=pts-pts[0]
|
|
pts*=width/pts[:,0].max()
|
|
if flipx: pts[:,0]=-pts[:,0]
|
|
if not flipy: pts[:,1]=-pts[:,1]
|
|
pts+=ofs
|
|
self.points=pts
|
|
|
|
verb=self.verbose
|
|
if verb&0x02:
|
|
self.plot_points(pts)
|
|
plt.show(block=False)
|
|
|
|
def gen_rand_points(self,n=107,scale=1000,ofs=(0,0)):
|
|
'generate random distributed points'
|
|
np.random.seed(0)
|
|
#data=np.random.randint(0,1000,(30,2))
|
|
pts=np.random.rand(n,2)*scale
|
|
pts+=ofs
|
|
self.points=pts
|
|
|
|
def gen_grid_points(self,w=10,h=10,pitch=100,rnd=.2,ofs=(0,0)):
|
|
'generates points in a grid with a given pitch and a bit randomness'
|
|
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))
|
|
pts+=ofs
|
|
self.points=pts
|
|
|
|
def gen_spiral_points(self,rStart=1.,rInc=.2,numSeg=4,numCir=6, phase=0, ofs=(0, 0)):
|
|
#rInc radius increment per circle
|
|
r=rStart+np.arange(numSeg*numCir)*(float(rInc)/numSeg)
|
|
ang=2.*np.pi/numSeg*np.arange(numSeg*numCir)+phase*np.pi/180
|
|
|
|
pts=np.vstack((np.sin(ang)*r,np.cos(ang)*r)).T
|
|
pts+=ofs
|
|
self.points=pts
|
|
|
|
def gen_closed_shifted(self,pitch=100,shift=5,mult=3):
|
|
'from the given points, close the path, and runs 9 times with small pitch'
|
|
|
|
pts=self.points
|
|
pts = np.vstack((pts, pts[-1]+(0,-50)))#add a new point outside of the grid
|
|
mn=pts.min(0)
|
|
mx=pts.max(0)
|
|
d=pts[0,:]-pts[-1,:]
|
|
l=np.sum(d)
|
|
bk=[]
|
|
if abs(d[1])>pitch: #make a vertical back move
|
|
s=np.sign(d[1])
|
|
n=np.ceil(np.abs(d[1])/pitch)
|
|
p=np.ndarray((n,2))
|
|
p[:,0]=pts[-1, 0]
|
|
p[:,1]=pts[-1, 1]+np.arange(1,n+1)*s*pitch
|
|
pts=np.vstack((pts,p))
|
|
|
|
if abs(d[0])>pitch: #make a horizonlat back move
|
|
s=np.sign(d[0])
|
|
n=np.ceil(np.abs(d[0])/pitch)
|
|
p=np.ndarray((n,2))
|
|
p[:,0]=pts[-1, 0]+np.arange(1,n+1)*s*pitch
|
|
p[:,1]=pts[-1, 1]
|
|
pts[-1, :]
|
|
pts=np.vstack((pts,p))
|
|
|
|
stack=[]
|
|
for y in np.arange(mult)*shift:
|
|
for x in np.arange(mult)*shift:
|
|
stack.append(pts+(x,y))
|
|
pts=np.vstack(stack)
|
|
|
|
|
|
#xx,yy=np.meshgrid(range(w), range(h))
|
|
#pts=np.array([xx.reshape(-1),yy.reshape(-1)],dtype=np.float).transpose()*pitch
|
|
#if xy:
|
|
#else:
|
|
|
|
# smlpitch
|
|
#pts+=ofs
|
|
self.points=pts
|
|
|
|
|
|
def sort_points(self,xy=False,grp_sz=None):
|
|
pts=self.points
|
|
verb=self.verbose
|
|
|
|
cnt=pts.shape[0]
|
|
idx=np.ndarray(cnt,dtype=np.int32)
|
|
if grp_sz is None:
|
|
grp_cnt=int(np.sqrt(cnt))
|
|
grp_sz=int(np.ceil(float(cnt)/grp_cnt))
|
|
else:
|
|
grp_sz=int(grp_sz)
|
|
grp_cnt=int(np.ceil(float(cnt)/grp_sz))
|
|
|
|
if xy==True:
|
|
idxA=1;idxB=0
|
|
else:
|
|
idxA=0;idxB=1
|
|
|
|
#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&0x02:
|
|
DebugPlot.plot_points(pts)
|
|
plt.show(block=False)
|
|
self.points=pts
|
|
|
|
|
|
def opt_pts(self,fn):
|
|
'''
|
|
trial to optimize path by mofing trajectory, uload real path and move the points
|
|
to finally go trough the desired points
|
|
'''
|
|
fh=np.load(fn)
|
|
#res=rot.ActPos,x.ActPos,y.ActPos,rot.DesPos,x.DesPos,y.DesPos
|
|
#idx 0 1 2 3 4 5
|
|
rec=fh['rec']
|
|
pts=fh['pts']
|
|
desPos=rec[:,(3,2)]
|
|
|
|
idx=np.ndarray(shape=len(pts),dtype=np.int32)
|
|
for i in range(len(pts)):
|
|
l=desPos-pts[i,:]
|
|
l2=l[:,0]**2+l[:,1]**2
|
|
idx[i]=np.argmin(l2)
|
|
|
|
recPts=rec[idx,:]
|
|
ptsCorr=(pts-recPts[:,(1,0)]+recPts[:,(3,2)])
|
|
self.points=pts
|
|
self.ptsCorr=ptsCorr
|
|
print(ptsCorr)
|
|
|
|
def setup_gather(self,acq_per=None):
|
|
'''
|
|
setup the channels to gather
|
|
kwargs:
|
|
acq_per : acquire period: acquire data all acq_per servo loops (default=None->calc best, =1-> fastest)
|
|
'''
|
|
if self.comm is None: return
|
|
comm=self.comm
|
|
gt=self.gather
|
|
gt.set_phasemode(False)
|
|
if self.meta['sync_flag']&2:
|
|
address=("Motor[1].ActPos","Motor[2].ActPos","Motor[1].DesPos","Motor[2].DesPos","Coord[1].Q[11]")
|
|
else:
|
|
address=("Motor[1].ActPos","Motor[2].ActPos","Motor[1].DesPos","Motor[2].DesPos","Gate3[1].Chan[1].UserFlag")
|
|
|
|
gtMaxLn=gt.set_address(*address)
|
|
if acq_per is None:
|
|
ovhdTime=100
|
|
acq_per=int(np.ceil((self.meta['pt2pt_time']*self.points.shape[0]+ovhdTime)/(gtMaxLn*self.meta['srv_per'])))
|
|
gt.set_property(MaxSamples=1000000, Period=acq_per)
|
|
#gt.set_property(Period=acq_per)
|
|
self.meta.update({'acq_per':acq_per,'address':address})
|
|
|
|
def setup_coord_trf(self,fx='X',fy='Y',cz='0'):
|
|
# FY:1, FX:2, ROT_Y:3, CX:4, CZ:5,
|
|
if self.comm is None: return
|
|
comm = self.comm
|
|
gpascii = comm.gpascii
|
|
prg = '''&1a
|
|
#1-> Y
|
|
#2-> X
|
|
#3-> A
|
|
&0#4->0
|
|
&0#5->0
|
|
&0#6->0
|
|
&0#7->0
|
|
#1..3j/
|
|
//#8->0
|
|
//#1..8j/
|
|
'''
|
|
prg=f'''&1a
|
|
&1#3->0
|
|
&1#4->0
|
|
&1#6->0
|
|
&1#7->0
|
|
&0#3->0
|
|
&0#4->0
|
|
&0#6->0
|
|
&0#7->0
|
|
&1
|
|
#1->{fy}
|
|
#2->{fx}
|
|
#5->{cz}
|
|
#1,2,5j/
|
|
'''
|
|
|
|
gpascii.send_block(prg)
|
|
|
|
def setup_motion(self,prgId=2,fnPrg=None,mode=0,**kwargs):
|
|
'''
|
|
1. generates program <prgId> and saves to fnPrg
|
|
the type of generated program is defined by <mode>
|
|
2. runs the program on the deltatau
|
|
|
|
mode=1 pvt motion
|
|
kwargs:
|
|
scale : scaling velocity (default=1. value=0 would stop at the point
|
|
cnt : move path multiple times (default=1)
|
|
dwell : dwell time at end (default=100ms)
|
|
|
|
mode=3 pvt motion using inverse fft velocity
|
|
kwargs: same as pvt motion
|
|
numPad : number of padding points to reduce aliasing (default=16)
|
|
'''
|
|
prg=['close all buffers','open prog %d'%(prgId)]
|
|
verb=self.verbose
|
|
comm=self.comm
|
|
if comm is not None:
|
|
gpascii=comm.gpascii
|
|
# this uses Coord[1].Tm and limits with MaxSpeed
|
|
if mode in (1,3): #### pvt motion
|
|
pt2pt_time=self.meta['pt2pt_time']
|
|
ts=self.meta['srv_per']
|
|
scale=kwargs.get('scale', 1.)
|
|
cnt=kwargs.get('cnt', 1) # move path multiple times
|
|
dwell=kwargs.get('dwell', 100) # wait time at end of motion
|
|
CoordFeedTime=1000. #Defaut deltatau value
|
|
try:
|
|
pt=self.ptsCorr
|
|
except AttributeError:
|
|
pt=self.points
|
|
#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)]=pt[0,:]
|
|
pv[ 1:-1,(0,1)]=pt
|
|
pv[ -1,(0,1)]=pt[-1,:]
|
|
pv[(0,0,-1,-1),(2,3,2,3)]=0
|
|
if mode==1: # set velocity to average from prev to next point
|
|
dist=pv[2:,(0,1)] - pv[:-2,(0,1)]
|
|
pv[ 1:-1,(2,3)] = dist/(2.*pt2pt_time)*scale #um/ms
|
|
else: #mode=3: set velocity to the reconstructed inverse fourier transformation
|
|
numPad=kwargs.get('numPad', 16)
|
|
p=np.hstack((pt.T,pt[-1,:].repeat(numPad).reshape(2,-1)))
|
|
k=p.shape[1]
|
|
stp=((p[:,-1]-p[:,0])/(k-1)) #calculate steepness point to point
|
|
#stp*=0
|
|
p[0,:]-=stp[0]*np.arange(k)
|
|
p[1,:]-=stp[1]*np.arange(k)
|
|
f=np.fft.fftfreq(k, d=1.)
|
|
pf=np.fft.fft(p)
|
|
pfd=pf*f*1j # differentiate in fourier
|
|
pd=np.fft.ifft(pfd)
|
|
v=pd.real.T/pt2pt_time*np.pi*2+stp/pt2pt_time
|
|
if numPad==0:
|
|
n=None
|
|
else:
|
|
n=-numPad
|
|
pv[ 1:-1,(2,3)] = v[:n]*scale
|
|
if verb&0x20:
|
|
dp=DebugPlot(self);self.pvt=dp.plot_gen_pvt(pv)
|
|
plt.show(block=False)
|
|
|
|
pv[1:-1, (2, 3)]*=CoordFeedTime #scaling for Deltatau
|
|
prg.append(' linear abs')
|
|
prg.append('X%g Y%g' % tuple(pv[0, (0,1)]))
|
|
prg.append('dwell 10')
|
|
try: prg.extend(self.sync_prg.split('\n'))
|
|
except AttributeError:
|
|
#print('no sync code available')
|
|
prg.append('Gather.Enable=2')
|
|
if cnt>1:
|
|
prg.append('P100=%d'%cnt)
|
|
prg.append('N100:')
|
|
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)]))
|
|
if cnt>1:
|
|
prg.append('dwell 10')
|
|
prg.append('P100=P100-1')
|
|
prg.append('if(P100>0)')
|
|
prg.append('{')
|
|
prg.append(' linear abs')
|
|
prg.append('X%g Y%g' % tuple(pv[0, (0,1)]))
|
|
prg.append('dwell %d' % dwell)
|
|
prg.append('goto 100')
|
|
prg.append('}')
|
|
else:
|
|
prg.append('dwell %d'%dwell)
|
|
prg.append('Gather.Enable=0')
|
|
|
|
prg.append('close')
|
|
#prg.append('&1\nb%dr\n'%prgId)
|
|
if verb&0x04:
|
|
for ln in prg:
|
|
print(ln)
|
|
|
|
if fnPrg is not None:
|
|
fh=open(fnPrg,'w')
|
|
fh.write('\n'.join(prg))
|
|
fh.close()
|
|
if comm is not None:
|
|
gpascii.send_block(prg,verb&0x08)
|
|
|
|
self.prg=prg
|
|
|
|
def gather_upload(self,fnRec=None):
|
|
gt=self.gather
|
|
gt.wait_stopped(verbose=True)
|
|
self.rec=rec=gt.upload()
|
|
|
|
try:
|
|
syncShell=self.syncShell
|
|
except AttributeError: pass
|
|
else:
|
|
print(syncShell.sync())
|
|
del self.syncShell
|
|
|
|
pts=self.points
|
|
ofsy=-rec[0,2]+pts[0,1]
|
|
ofsx=-rec[0,3]+pts[0,0]
|
|
rec[:,(1,3)]+=ofsx
|
|
rec[:,(0,2)]+=ofsy
|
|
if fnRec:
|
|
np.savez_compressed(fnRec, rec=rec, pts=pts, meta=self.meta)
|
|
|
|
|
|
if __name__=='__main__':
|
|
#import logging
|
|
#logger = logging.getLogger(__name__)
|
|
#logger = logging.getLogger('pbtools.misc.pp_comm')
|
|
#logger.setLevel(logging.DEBUG)
|
|
#logging.basicConfig(format=('%(asctime)s %(name)-12s '
|
|
# '%(levelname)-8s %(message)s'),
|
|
# datefmt='%m-%d %H:%M',
|
|
# )
|
|
|
|
def unique_filename(fnBase):
|
|
i = 0;
|
|
while (True):
|
|
fn=fnBase+('%0.3d'%i)
|
|
i+=1
|
|
if not os.path.exists(fn+'.npz'):
|
|
print('save to '+fn+'.*')
|
|
break
|
|
return fn
|
|
|
|
|
|
def run_test(args):
|
|
#dp=DebugPlot();dp.plot_gather();return
|
|
#import socket
|
|
#s = socket.socket(socket.AF_INET, socket.SOCK_STREAM) # create an INET, STREAMing socket
|
|
#s.settimeout(.1)
|
|
#try:
|
|
# port=22 #ssh port
|
|
# s.connect((args.host, port)) # try to connect to port
|
|
# s.close()
|
|
#except (socket.error, socket.gaierror) as e:
|
|
# comm=gather=None
|
|
#else:
|
|
# comm = PPComm(host=args.host)
|
|
# gather = Gather(comm)
|
|
|
|
hpp=args.host.split(':')
|
|
param={'host':hpp[0]}
|
|
if len(hpp)>1:
|
|
param['port']=int(hpp[1])
|
|
if len(hpp)>2:
|
|
param['fast_gather_port']=int(hpp[2])
|
|
print(' -> ssh-tunneling PPComm({host}:{port} {host}:{fast_gather_port})'.format(**param))
|
|
comm=PPComm(**param)
|
|
gather = Gather(comm)
|
|
|
|
#real start and frame trigger with sync
|
|
#sp = ShapePath(comm, gather, args.verbose)
|
|
|
|
# direct start
|
|
#sp = ShapePath(comm, gather, args.verbose,sync_mode=0,pt2pt_time=10)
|
|
|
|
#simulated start and frame trigger no sync
|
|
sp = ShapePath(comm, gather, args.verbose,sync_mode=1,sync_flag=3)
|
|
|
|
#simulated start and frame trigger with sync
|
|
#sp = ShapePath(comm, gather, args.verbose,sync_mode=2,sync_flag=3)
|
|
|
|
#simulated start real frame trigger no sync
|
|
#sp = ShapePath(comm, gather, args.verbose,sync_mode=1,sync_flag=1)
|
|
|
|
#simulated start real frame trigger with sync
|
|
#sp = ShapePath(comm, gather, args.verbose,sync_mode=2,sync_flag=1)
|
|
|
|
fn='/tmp/shapepath'
|
|
#fn =unique_filename('ShapePathAnalyser/records/19_01_24/spiral')
|
|
|
|
# Gather.MaxLines=116508
|
|
# ts=0.2ms
|
|
# max_num_points=(MaxLines*ts-1000ms)/(+acq_per*pt2pt_time*ts)
|
|
# pt2pt_time acq_per maxpts
|
|
# 40ms 1 555
|
|
# 40ms 2 1135
|
|
# 40ms 3 1715
|
|
# 40ms 4 2295
|
|
# 10ms 1 2220
|
|
# 10ms 2 4540
|
|
# 10ms 3 6860
|
|
# 10ms 4 9180
|
|
|
|
#sp.gen_grid_points(w=6,h=6,pitch=100,rnd=0,ofs=(0,0));sp.sort_points(False);
|
|
#sp.gen_grid_points(w=100,h=100,pitch=10,rnd=.2)
|
|
#sp.gen_swissfel_points(width=1000,ofs=(-500,0));sp.sort_points(xy=xy)
|
|
#sp.gen_grid_points(w=10,h=10,pitch=50,rnd=.2)
|
|
#sp.gen_grid_points(w=100,h=100,pitch=50,rnd=.2)
|
|
#sp.gen_closed_shifted()
|
|
#sp.gen_swissmx_points(width=1000,ofs=(-500,0))
|
|
#sp.gen_swissfel_points(width=1000,ofs=(-500,0))
|
|
#sp.gen_rand_points(n=20, scale=100,ofs=(-950,+2780));sp.sort_points(xy=False)
|
|
#sp.gen_rand_points(n=200, scale=100,ofs=(0,+2000));sp.sort_points(xy=False)
|
|
#sp.gen_swissmx_points(width=1000, ofs=(-500, 0));
|
|
#sp.gen_spiral_points(rStart=100,rInc=10,numSeg=4,numCir=60, ofs=(0, 0))
|
|
#sp.gen_spiral_points(rStart=100,rInc=130,numSeg=4,numCir=2, ofs=(0, 0))
|
|
#sp.gen_grid_points(w=20,h=20,pitch=100,rnd=0,ofs=(0,+2000));sp.sort_points(False);
|
|
#sp.gen_grid_points(w=5,h=10,pitch=100,rnd=0,ofs=(0,+2000));sp.sort_points(False,10);
|
|
sp.gen_grid_points(w=125,h=125,pitch=3,rnd=0,ofs=(0,+2000));sp.sort_points(False,125); sp.meta['pt2pt_time']=5
|
|
|
|
#sp.gen_grid_points(w=1,h=10,pitch=100,rnd=0,ofs=(0,0))
|
|
#sp.gen_spiral_points(rStart=100,rInc=20,numSeg=8,numCir=32, ofs=(0, 0))
|
|
#sp.gen_spiral_points(rStart=100,rInc=10,numSeg=2,numCir=32, phase=45, ofs=(0, 0))
|
|
#sp.gen_spiral_points(rStart=100,rInc=10,numSeg=4,numCir=32, ofs=(0, 0))
|
|
#sp.gen_closed_shifted()
|
|
|
|
gtMaxLn=116508;ovhdTime=100
|
|
acq_per=int(np.ceil((sp.meta['pt2pt_time']*sp.points.shape[0]+ovhdTime)/(gtMaxLn*sp.meta['srv_per'])))
|
|
sp.setup_gather(acq_per=acq_per)
|
|
sp.setup_sync(verbose=args.verbose&32,timeOfs=0.05)
|
|
sp.setup_coord_trf() # reset to shape path system
|
|
#sp.meta['pt2pt_time']=10 #put between setup_sync and setup_motion to have more motion points than FEL syncs
|
|
sp.setup_motion(fnPrg=fn+'.prg', mode=3, scale=1.,dwell=10)
|
|
#sp.setup_motion(fnPrg=fn + '.prg', mode=1, scale=1,dwell=10)
|
|
#sp.setup_motion(fnPrg=fn + '.prg', mode=1, scale=0,dwell=10)
|
|
sp.homing() #homing if needed
|
|
sp.run() #start motion program
|
|
sp.wait_armed() # wait until motors are at first position
|
|
sp.trigger(0.5) #send a start trigger (if needed) ater given time
|
|
if not comm is None:
|
|
while True:
|
|
p=sp.progress()
|
|
if p<0: break
|
|
print('progress %d/%d'%(p,sp.points.shape[0]));time.sleep(.1)
|
|
sp.gather_upload(fnRec=fn+'.npz')
|
|
dp=DebugPlot(sp);dp.plot_gather(mode=11)
|
|
|
|
print('done')
|
|
plt.show(block=False)
|
|
input('press return')
|
|
|
|
|
|
#sp.plot_points(sp.points);plt.show()
|
|
|
|
|
|
|
|
#>>>run gather and plot trajectory<<<
|
|
#return
|
|
|
|
|
|
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=('-v15',
|
|
'--host=localhost:10001:10002'
|
|
)
|
|
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('--host', help='hostname', default='SAR-CPPM-EXPMX1')
|
|
#parser.add_option('--host', help='hostname', default='localhost:10001:10002')
|
|
#parser.add_option('--host', help='hostname')
|
|
|
|
(args, other)=parser.parse_args()
|
|
args.other=other
|
|
run_test(args)
|
|
#------------------ Main Code ----------------------------------
|
|
#ssh_test()
|
|
ret=parse_args()
|
|
exit(ret)
|