sf-mx/PBSwissMX
0
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

using new optimized control parameters

Browse Source
This commit is contained in:
zamofing_t
2019-03-06 10:32:08 +01:00
parent 0b677bdd23
commit 9eda474e61
12 changed files with 14890 additions and 543 deletions
Download Patch File Download Diff File Expand all files Collapse all files

73
python/trajectory.py
View File

@@ -16,6 +16,7 @@ ift: inverse fourier transformation
import numpy as np
import matplotlib as mpl
import matplotlib.pyplot as plt
import os
np.set_printoptions(precision=3, suppress=True)
@@ -63,11 +64,15 @@ def gen_pvt(p,v,t,ts):
return pvt
(a,b)=os.path.split(__file__)
fnBase=os.path.abspath(os.path.join(a,'../MXfastStageDoc'))
#derivate_fft_test()
w=40. # ms step between samples
ts=.2 # sampling time
n=int(w/ts)# servo cycle between samples
k=32 #number of unique samples
k=320 #number of unique samples
t = np.arange(0, w*(k+1), w) #time array of trajectory
@@ -80,9 +85,12 @@ p=np.random.random(k+1)*4. #position array of trajectory
p[-1]=p[0] # put the first position at the end
#p=np.array([1,-1]*16+[1,]);
#p+=3
tt = np.arange(t[0],t[-1], ts) #time array of servo cycles
ax=plt.figure().add_subplot(1, 1, 1)
fig=plt.figure()
ax=fig.add_subplot(1, 1, 1)
ax.xaxis.set_ticks(t)
markerline, stemlines, baseline = ax.stem(t, p, '-')
@@ -91,14 +99,8 @@ markerline, stemlines, baseline = ax.stem(t, p, '-')
p_iftf=np.fft.fft(p[:-1])
ft=np.hstack((p_iftf[:k/2],np.zeros((n-1)*k),p_iftf[k/2:]))
pp_ift=np.fft.ifft(ft)*n
ax.plot(tt,pp_ift,'-b',label='ift')
#plt.figure()
#ax=plt.gca()
#ax.xaxis.set_ticks(x)
#markerline, stemlines, baseline = ax.stem(x, y, '-')
### PVT move ###
p2=np.hstack((p[-2],p,p[1]))
v=(p2[2:]-p2[:-2])/(w*2)
@@ -110,15 +112,16 @@ ax.plot(tt,pp_pvt,'-g',label='pvt')
v*=0
pp_p0t=gen_pvt(p,v,t,ts)
ax.plot(tt,pp_p0t,'-r',label='p0t')
ax.set_xlim(0,400)
### PVT with ift velocity move -> PFT ###
f=np.fft.fftfreq(k, d=1./k)
p_pftf=np.fft.fft(p[:-1])
p_pftfd=p_pftf*f*1j # differentiate in fourier
print (p_pftfd)
#print (p_pftfd)
p_pftd=np.fft.ifft(p_pftfd)
print (p_pftd)
#print (p_pftd)
p_pftd=np.hstack((p_pftd,p_pftd[0]))
#ax2=plt.figure().add_subplot(1,1,1)
@@ -128,8 +131,12 @@ v=p_pftd.real/(k*2*np.pi)
pp_pft=gen_pvt(p,v,t,ts)
ax.plot(tt,pp_pft,'-c',label='pft')
ax.xaxis.set_label_text('time(ms)')
ax.yaxis.set_label_text('position')
ax.legend(loc='best')
plt.show(block=False)
fig.savefig(os.path.join(fnBase,'traj1.eps'))
### frequency plots ###
@@ -146,22 +153,54 @@ pp_pftf=np.fft.rfft(pp_pft)/(2*n)
f=np.fft.rfftfreq(pp_ift.shape[0], d=ts*1E-3)
f=f[1:] #remove dc value frequency
mag=abs(pp_iftf[1:])#; mag=20*np.log10(abs(mag))
def FreqSpecDb(spec,w=20.):
#spec = abs(spec)
spec = np.convolve(spec, np.ones(w) / w, 'same');
spec = 20 * np.log10(spec)
return spec
def FreqSpec(spec,w=20.):
#spec = abs(spec)
spec = np.convolve(spec, np.ones(w) / w, 'same');
return spec
mag=FreqSpecDb(abs(pp_iftf[1:]))
ax.semilogx(f,mag,'-b',label='ift') # Bode magnitude plot
mag=abs(pp_pvtf[1:])#; mag=20*np.log10(abs(mag))
mag=FreqSpecDb(abs(pp_pvtf[1:]))
ax.semilogx(f,mag,'-g',label='pvt') # Bode magnitude plot
mag=abs(pp_p0tf[1:])#; mag=20*np.log10(abs(mag))
mag=FreqSpecDb(abs(pp_p0tf[1:]))
ax.semilogx(f,mag,'-r',label='p0t') # Bode magnitude plot
mag=abs(pp_pftf[1:])#; mag=20*np.log10(abs(mag))
mag=FreqSpecDb(abs(pp_pftf[1:]))
ax.semilogx(f,mag,'-c',label='pft') # Bode magnitude plot
#ax.yaxis.set_label_text('dB ampl')
ax.yaxis.set_label_text('ampl')
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')
ax.set_xlim(1,100)
plt.show(block=False)
fig.savefig(os.path.join(fnBase,'traj2.eps'))
fig=plt.figure()
ax=fig.add_subplot(1,1,1)#ax=plt.gca()
magift=abs(pp_iftf[1:])
mag=FreqSpec(abs(pp_pvtf[1:])-magift)
ax.semilogx(f,mag,'-g',label='pvt-ift') # Bode magnitude plot
mag=FreqSpec(abs(pp_p0tf[1:])-magift)
ax.semilogx(f,mag,'-r',label='p0t-ift') # Bode magnitude plot
mag=FreqSpec(abs(pp_pftf[1:])-magift)
ax.semilogx(f,mag,'-c',label='pft-ift') # Bode magnitude plot
#ax.yaxis.set_label_text('dB ampl')
ax.yaxis.set_label_text('ampl')
ax.xaxis.set_label_text('frequency [Hz]')
ax.set_xlim(1,100)
plt.grid(True)
ax.legend(loc='best')
plt.show(block=False)
fig.savefig(os.path.join(fnBase,'traj3.eps'))
plt.show()