working trajectory generation with ift in shapepath
This commit is contained in:
@@ -9,11 +9,12 @@
|
||||
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
|
||||
1 basic info
|
||||
2 plot sorting steps
|
||||
4 list program
|
||||
4 upload progress
|
||||
8 plot gather path
|
||||
16 plot pvt trajectory (before motion)
|
||||
|
||||
Gather motor order
|
||||
|
||||
@@ -53,6 +54,90 @@ 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)*0
|
||||
|
||||
tt=np.arange(0,p2pt,ts)
|
||||
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
|
||||
|
||||
|
||||
def debugplot_pvt(pv, meta):
|
||||
# pv is an array of posx posy velx vely
|
||||
#pv=pv[5:10,:]
|
||||
#pv=pv[5:-4,:]
|
||||
p2pt=meta['pt2pt_time'] # ms step between samples
|
||||
ts=meta['timebase'] # 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')
|
||||
ax2.stem(t, pv[:,1], '-g')
|
||||
|
||||
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)
|
||||
|
||||
|
||||
class ShapePath(MotionBase):
|
||||
def __init__(self,comm, gather, verbose):
|
||||
MotionBase.__init__(self,comm, gather, verbose)
|
||||
@@ -243,7 +328,8 @@ class ShapePath(MotionBase):
|
||||
'''
|
||||
prg=['close all buffers','open prog %d'%(prgId)]
|
||||
comm=self.comm
|
||||
gpascii=comm.gpascii
|
||||
if comm is not None:
|
||||
gpascii=comm.gpascii
|
||||
# this uses Coord[1].Tm and limits with MaxSpeed
|
||||
if mode==-1: #### jog a 10mm square
|
||||
pos=self.points
|
||||
@@ -273,15 +359,16 @@ class ShapePath(MotionBase):
|
||||
prg.append('Gather.Enable=0')
|
||||
elif mode in (1,3): #### pvt motion
|
||||
pt2pt_time=kwargs.get('pt2pt_time', 100)
|
||||
ts=self.meta['timebase']
|
||||
scale=kwargs.get('scale', 1.)
|
||||
self.meta['pt2pt_time']=pt2pt_time
|
||||
cnt=kwargs.get('cnt', 1) # move path multiple times
|
||||
sync_frq=kwargs.get('sync_frq', 10) # synchronization mark all n points
|
||||
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
|
||||
@@ -291,15 +378,25 @@ class ShapePath(MotionBase):
|
||||
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)] = 1000.*dist/(2.*pt2pt_time)*scale
|
||||
pv[ 1:-1,(2,3)] = dist/(2.*pt2pt_time)*scale #um/ms
|
||||
else: #mode=3: set velocity to the reconstructed inverse fourier transformation
|
||||
k=pt.shape[0]
|
||||
p=pt.T.copy() #copy
|
||||
k=p.shape[1]
|
||||
stp=((p[:,-1]-p[:,0])/(k-1)) #calculate steepness point to point
|
||||
p[0,:]-=stp[0]*np.arange(k)
|
||||
p[1,:]-=stp[1]*np.arange(k)
|
||||
f=np.fft.fftfreq(k, d=1./k)
|
||||
pf=np.fft.fft(pt.T)
|
||||
pf=np.fft.fft(p)
|
||||
pfd=pf*f*1j # differentiate in fourier
|
||||
pd=np.fft.ifft(pfd)
|
||||
v=pd.real/(k*2*np.pi)
|
||||
pv[ 1:-1,(2,3)] = 1000.*v.T/(pt2pt_time)*scale # FACTORS HAS TO BEEN CHECKED
|
||||
v=pd.real.T/(k*2*np.pi)+stp/pt2pt_time
|
||||
pv[ 1:-1,(2,3)] = v*scale
|
||||
verb=self.verbose
|
||||
if verb&16:
|
||||
debugplot_pvt(pv, self.meta)
|
||||
plt.show()
|
||||
|
||||
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')
|
||||
@@ -365,14 +462,9 @@ class ShapePath(MotionBase):
|
||||
fh=open(fnPrg,'w')
|
||||
fh.write('\n'.join(prg))
|
||||
fh.close()
|
||||
gpascii.send_block(prg)
|
||||
if comm is not None:
|
||||
gpascii.send_block(prg)
|
||||
|
||||
#if self.host is not None:
|
||||
# cmd ='gpasciiCommander --host '+self.host+' '+ fnPrg
|
||||
# print(cmd)
|
||||
# p = sprc.Popen(cmd, shell=True)#, stdout=sprc.PIPE, stderr=sprc.STDOUT)
|
||||
# #res=p.stdout.readlines(); print res
|
||||
# retval = p.wait()
|
||||
self.prg=prg
|
||||
|
||||
def gather_upload(self,fnRec=None):
|
||||
@@ -632,7 +724,7 @@ if __name__=='__main__':
|
||||
|
||||
|
||||
def run_test(args):
|
||||
#args.host=None
|
||||
args.host=None
|
||||
if args.host is None:
|
||||
comm=gather=None
|
||||
else:
|
||||
@@ -663,18 +755,35 @@ if __name__=='__main__':
|
||||
#sp.plot_gather()
|
||||
#return
|
||||
fn='/tmp/shapepath'
|
||||
fn='/home/zamofing_t/Documents/prj/SwissFEL/epics_ioc_modules/ESB_MX/python/record/grid_delay_0002'
|
||||
#fn='/home/zamofing_t/Documents/prj/SwissFEL/epics_ioc_modules/ESB_MX/python/record/grid_delay_0002'
|
||||
# /home/zamofing_t/Documents/prj/SwissFEL/epics_ioc_modules/ESB_MX/python/record/chip000_sortedXXXX_9x9'
|
||||
#fn='/sf/bernina/data/p17592/res/20181203/imprints/chip000_preloc_min10um_sortedX_refs_goaround/chip000_sortedY_9x9.npz'
|
||||
#fn='PBMotionAnalyzer/records/rand50um_25Hz'
|
||||
#sp.setup_coord_trf()
|
||||
|
||||
#sp.points=np.zeros((2,2))
|
||||
#sp.meta={'timebase':.2}
|
||||
sp.meta={'timebase':.2}
|
||||
#sp.gather_upload(fnRec=fn+'.npz')
|
||||
#sp.plot_gather()
|
||||
#return
|
||||
xy=False
|
||||
sp.gen_rand_points(n=14, scale=1000);sp.sort_points(xy=xy)
|
||||
#print(sp.points[:,0])
|
||||
#sp.gen_swissmx_points(width=1000, ofs=(-500, 0));
|
||||
#sp.points=np.array([[0.,1.],[1.,0.],[0.,-1.],[-1.,0.]])
|
||||
sp.points=np.array([[0.,1.],[1.,0.],[0.,-1.],[-1.,0.],
|
||||
[0., 1.], [1., 0.], [0., -1.], [-1., 0.],
|
||||
[0., 1.], [1., 0.], [0., -1.], [-1., 0.],
|
||||
[0., 1.], [1., 0.], [0., -1.], [-1., 0.],
|
||||
[0., 1.], [1., 0.], [0., -1.], [-1., 0.],
|
||||
[0., 1.], [1., 0.], [0., -1.], [-1., 0.], [0., 1.]])
|
||||
#sp.gen_grid_points(w=10,h=10,pitch=100,rnd=0,ofs=(0,0));sp.sort_points(False);
|
||||
#sp.gen_grid_points(w=1,h=10,pitch=100,rnd=0,ofs=(0,0))
|
||||
sp.setup_motion(fnPrg=fn + '.prg', mode=1, pt2pt_time=40,scale=0)
|
||||
sp.setup_motion(fnPrg=fn + '.prg', mode=1, pt2pt_time=40,scale=1)
|
||||
sp.setup_motion(fnPrg=fn + '.prg', mode=3, pt2pt_time=40,scale=1)
|
||||
exit(0)
|
||||
|
||||
# fn='/home/zamofing_t/Documents/prj/SwissFEL/epics_ioc_modules/ESB_MX/python/data/'+time.strftime('%y-%m-%d-%H_%M_%S')
|
||||
|
||||
#>>>point source and sorting<<<
|
||||
|
||||
@@ -74,9 +74,9 @@ t = np.arange(0, w*(k+1), w) #time array of trajectory
|
||||
#p=3.*np.cos(t)+4. #position array of trajectory
|
||||
#p=3.*np.sin(1.3+2.*t/(w*k)*2.*np.pi)+10.
|
||||
#p+=np.cos(1.5*t/(w*k)*2.*np.pi)
|
||||
p=np.cos(8*t*np.pi*2./(k*w)) #eine schwingung
|
||||
#np.random.seed(10)
|
||||
#p=np.random.random(k+1)*4. #position array of trajectory
|
||||
#p=np.cos(8*t*np.pi*2./(k*w)) #eine schwingung
|
||||
np.random.seed(10)
|
||||
p=np.random.random(k+1)*4. #position array of trajectory
|
||||
|
||||
|
||||
p[-1]=p[0] # put the first position at the end
|
||||
|
||||
Reference in New Issue
Block a user