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

working so far. have to test helicalscan also on the real system

Browse Source
This commit is contained in:
zamofing_t
2018-01-04 16:04:30 +01:00
parent 57b6ff3a2c
commit e58047f2b0
8 changed files with 483 additions and 146 deletions
Download Patch File Download Diff File Expand all files Collapse all files

247
python/helicalscan.py
View File

@@ -149,12 +149,23 @@ class HelicalScan:
def test_coord_trf(self):
param = self.param
dx, dz, w, y, = (0.2,0.3,0.1,3.3)
print 'input : dx:%.6g dz:%.6g w:%.6g fy:%.6g' % (dx,dz,w/d2r,y)
(cx,cz,w,fy) = self.inv_transform(dx,dz,w,y)
print 'inv_trf: cx:%.6g cz:%.6g w:%.6g fy:%.6g' % (cx,cz,w/d2r,fy)
cx, cz, w, fy, = (0.2,0.3,0.1,0.4)
#cx, cz, w, fy, = (10.,20,3.,40)
print 'input : cx:%.6g cz:%.6g w:%.6g fy:%.6g' % (cx,cz,w/d2r*1000.,fy)
(dx,dz,w,y) = self.fwd_transform(cx,cz,w,fy)
print 'fwd_trf: dx:%.6g dz:%.6g w:%.6g fy:%.6g' % (dx,dz,w/d2r,y)
print 'fwd_trf: dx:%.6g dz:%.6g w:%.6g fy:%.6g' % (dx,dz,w/d2r*1000.,y)
(cx,cz,w,fy) = self.inv_transform(dx,dz,w,y)
print 'inv_trf: cx:%.6g cz:%.6g w:%.6g fy:%.6g' % (cx,cz,w/d2r*1000.,fy)
dx, dz, w, y, = (0.2,0.3,0.1,0.4)
#dx, dz, w, y, = (10.,20,3.,40)
print 'input : dx:%.6g dz:%.6g w:%.6g fy:%.6g' % (dx,dz,w/d2r*1000.,y)
(cx,cz,w,fy) = self.inv_transform(dx,dz,w,y)
print 'inv_trf: cx:%.6g cz:%.6g w:%.6g fy:%.6g' % (cx,cz,w/d2r*1000.,fy)
(dx,dz,w,y) = self.fwd_transform(cx,cz,w,fy)
print 'fwd_trf: dx:%.6g dz:%.6g w:%.6g fy:%.6g' % (dx,dz,w/d2r*1000.,y)
# plt.ion()
# fig = plt.figure()
@@ -164,6 +175,32 @@ class HelicalScan:
# def my_anim_func3(self,idx):
# self.hCrist,pt=self.pltCrist(cx=0,ty=0,cz=0,w=10*idx*d2r,h=self.hCrist)
def mpl_test(self):
plt.ion()
fig = plt.figure()
ax = fig.add_axes([.2, .05, .75, .90])
x=np.arange(0,2*np.pi,.1)
y=np.sin(x)
h=ax.plot(x,y)
# ax.set_position([.2,.05,.75,.90])
#ax2 = ax.twinx()
ax2 = fig.add_axes([.2, .05, .75, .90],sharex=ax)
ax2.set_position([.1,.05,.75,.90])
ax2.spines['top'].set_visible(False)
ax2.spines['bottom'].set_visible(False)
ax2.xaxis.set_visible(False)
ax2.patch.set_visible(False)
y2=y+.1*np.random.random(y.shape)
h+=ax2.plot(x,y2,'g')
ax2.set_position(ax.get_position())
ax2.set_ylabel('mylbl', color='r')
ax2.tick_params(axis='y', colors='b')
ax2.spines['left'].set_position(('axes',-.1))
plt.legend(handels=h)
pass
def show_vel(self):
rec=self.rec
@@ -172,10 +209,62 @@ class HelicalScan:
y = np.diff(rec,axis=0)
mx=y.max(0)
mn=y.min(0)
y=y/(mx-mn)
scl=np.maximum(mx,-mn)
scl=np.where(scl==0, 1, scl) # replace 0 by 1
y/=scl
x = np.arange(0, y.shape[0]);
x= x.reshape(-1,1).dot(np.ones((1,y.shape[1])))
plt.plot(x, y)
lbl=('cx','cz','w','fy')
#plt.plot(x, y,label=lbl)
for i in range(y.shape[1]):
plt.plot(x[:,i], y[:,i],label=lbl[i])
plt.legend()
def show_pos(self):
rec=self.rec
y = rec
#plt.ion()
fig = plt.figure(figsize=(20,6))
c='bgrc'
lbl=('cx','cz','w','fy')
dx=.25/len(lbl)
for i in range(rec.shape[1]):
if i==0:
ax = fig.add_axes([.2, .05, .75, .90])
axl =[ax,]
else:
ax = fig.add_axes(axl[0].get_position(), sharex=ax)
ax.spines['top'].set_visible(False)
ax.spines['bottom'].set_visible(False)
ax.xaxis.set_visible(False)
ax.patch.set_visible(False)
ax.spines['left'].set_position(('axes', -dx*i))
axl.append(ax)
ax.set_ylabel(lbl[i], color=c[i])
ax.tick_params(axis='y', colors=c[i])
x = np.arange(0, y.shape[0]);
h=[]
for i in range(rec.shape[1]):
h+=axl[i].plot(x, y[:,i],c[i],label=lbl[i])
plt.legend(handles=h)
cid = fig.canvas.mpl_connect('motion_notify_event', self.onmove)
fig.obj=self
fig.data=y
@staticmethod
def onmove(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
data = event.canvas.figure.data
if(event.xdata):
idx=round(event.xdata)
msg='%d: cx:%.3f cz:%.3f w:%.1f fy:%.3f'%((idx,)+tuple(data[idx,:]))
#print msg
event.canvas.toolbar.set_message(msg)
def interactive_cx_cz_w_fy(self):
fig = plt.figure()
@@ -578,21 +667,28 @@ class HelicalScan:
prg.append('''
// Set the motors as inverse kinematic axes in CS 1
//motors CX CZ RY FY
// 7 8 1 2
// 4 5 3 1
&1
#7->I
#8->I
a
#4->0
#5->0
#3->0
#1->0
#4->I
#5->I
#3->I
#1->I
#2->I
open forward
define(qCX='L7', qCZ='L8', qW='L1', qFY='L2')
define(qCX='L4', qCZ='L5', qW='L3', qFY='L1')
define(DX='C6', DZ='C8', W='C1', Y='C7')
//coord X Z B Y
define(p0_x='L10', p0_y='L11', p0_z='L12')
define(p1_x='L13', p1_y='L14', p1_z='L15')
define(scale='L16')
//send 1"forward kinematic %f %f %f %f\\n",qCX,qCZ,qW,qFY''')
for i in range(2):
#https://stackoverflow.com/questions/3471999/how-do-i-merge-two-lists-into-a-single-list
@@ -626,7 +722,7 @@ open inverse
define(DX='C6', DZ='C8', W='C1', Y='C7')
//coord X Z B Y
//D0 is set to $000001c2
define(qCX='L7', qCZ='L8', qW='L1', qFY='L2')
define(qCX='L4', qCZ='L5', qW='L3', qFY='L1')
define(p0_x='L10', p0_y='L11', p0_z='L12')
define(p1_x='L13', p1_y='L14', p1_z='L15')
define(scale='L16')
@@ -638,6 +734,7 @@ open inverse
prg.append(" qW=W")
prg.append(" W=W*%g"%(d2r/1000.)) #scale from 1000*deg to rad
prg.append('''
p0_x=x_0+r_0*sin(phi_0+W)
p1_x=x_1+r_1*sin(phi_1+W)
p0_y=y_0
@@ -645,7 +742,7 @@ open inverse
p0_z=z_0+r_0*cos(phi_0+W)
p1_z=z_1+r_1*cos(phi_1+W)
scale=(qFY-y_0)/(y_1-y_0)
scale=(Y-y_0)/(y_1-y_0)
p0_x=p0_x+scale*(p1_x-p0_x)
p0_y=p0_y+scale*(p1_y-p0_y)
p0_z=p0_z+scale*(p1_z-p0_z)
@@ -654,10 +751,90 @@ open inverse
qFY=Y
//send 1"inverse result %f %f %f %f\\n",qCX,qCZ,qW,qFY
//P1002+=1
close
''')
# ****** TESTING CODE ******
#this is testcode to check linear coordinate transformation and
#simple (same as before) fwd and inv-kinematics
#When using pvt mode they do not behave the same:
#(Power PMAC Users Manual P648)
#Enabling Move Segmentation:
#If segmentation mode is disabled, a circle-mode move will execute as a linear-mode move
#directly from the start point to the end point. Segmentation mode must also be enabled to use the
#special lookahead buffer, tool-radius compensation, and kinematic-subroutine calculations.
#set ct to:
# 1 for simple linear transformation
# 2 simple linear fwd/inv kinematics
ct=0;
if ct==1:
#simple linear transformation
prg = []
prg.append('''
//motors CX CZ RY FY
// 4 5 3 1
&1
a
#4->0
#5->0
#3->0
#1->0
#4->X
#5->Z
#3->B
#1->Y
''')
elif ct==2:
#simple linear fwd/inv kinematics
prg = []
prg.append('''
// Set the motors as inverse kinematic axes in CS 1
//motors CX CZ RY FY
// 4 5 3 1
&1
a
#4->0
#5->0
#3->0
#1->0
#4->I
#5->I
#3->I
#1->I
open forward
define(qCX='L4', qCZ='L5', qW='L3', qFY='L1')
define(DX='C6', DZ='C8', W='C1', Y='C7')
//coord X Z B Y
send 1"forward D0: %f \\n",D0
if(D0>0) callsub 100
D0=$000001c2; //B=$2 X=$40 Y=$80 Z=$100 hex(2+int('40',16)+int('80',16)+int('100',16)) -> 0x1c2
N100:
DX=qCX
DZ=qCZ
W=qW
Y=qFY
send 1"forward result %f %f %f %f\\n",DX,DZ,W,Y
P1001+=1
close
open inverse
define(DX='C6', DZ='C8', W='C1', Y='C7')
//coord X Z B Y
//D0 is set to $000001c2
define(qCX='L4', qCZ='L5', qW='L3', qFY='L1')
qCX=DX
qCZ=DZ
qW=W
qFY=Y
send 1"inverse result %f %f %f %f\\n",qCX,qCZ,qW,qFY
P1002+=1
close
''')
if self.args.verbose & 4:
for ln in prg:
print(ln)
@@ -695,7 +872,7 @@ close
yRng : starting and ending height
'''
prg=[]
acq_per=kwargs.get('acq_per',1)
acq_per=kwargs.get('acq_per',10)
gather={"MaxSamples":1000000, "Period":acq_per}
#Sys.ServoPeriod is dependent of !common() macro
ServoPeriod= .2 #0.2ms
@@ -703,7 +880,7 @@ close
self.meta = {'timebase': ServoPeriod*gather['Period']}
#channels=["Motor[1].ActPos","Motor[2].ActPos","Motor[3].ActPos"]
# CX CZ W FY
channels=["Motor[7].ActPos","Motor[8].ActPos","Motor[1].ActPos","Motor[2].ActPos"]
channels=["Motor[4].ActPos","Motor[5].ActPos","Motor[3].ActPos","Motor[1].ActPos"]
prg.append('Gather.Enable=0')
prg.append('Gather.Items=%d'%len(channels))
for k,v in gather.iteritems():
@@ -757,11 +934,16 @@ close
pt[:,2]= np.linspace(wRng[0],wRng[1],numPt) #w
pt[:,3]= np.repeat(np.linspace(yRng[0],yRng[1],cntVert),cntHor) #y
self.points=pt
prg.append(' Coord[1].SegMoveTime=.05') #to calculate every 0.05 ms the inverse kinematics
#prg.append(' Coord[1].SegMoveTime=.05') #to calculate every 0.05 ms the inverse kinematics
prg.append(' nofrax') #needed to set all axis to use AltFeedRate
prg.append(' Coord[1].AltFeedRate=0') # allow maximum speed
prg.append(' Coord[1].SegMoveTime=0') #turn off segmented mode
prg.append(' linear abs')
prg.append(' X%g Z%g B%g Y%g' % tuple(pt[0, :]))
prg.append(' dwell 100')
prg.append(' Gather.Enable=2')
prg.append(' Coord[1].AltFeedRate=0') # allow maximum speed
prg.append(' Coord[1].SegMoveTime=1') #to calculate every 1 ms the inverse kinematics
for i in range(pt.shape[0]):
prg.append(' X%g Z%g B%g Y%g' % tuple(pt[i, :]))
prg.append(' dwell 100')
@@ -776,7 +958,7 @@ close
hRng = kwargs.get('hRng', (-.2,.2))
wRng = kwargs.get('wRng', (0,360000))
yRng = kwargs.get('yRng', (2.3,6.2))
pt2pt_time = kwargs.get('pt2pt_time ', 100)
pt2pt_time = kwargs.get('pt2pt_time', 100)
numPt=cntVert*cntHor
pt=np.zeros((numPt,4))
if cntHor>1:
@@ -798,6 +980,9 @@ close
dist=pv[2:,(0,1,2,3)] - pv[:-2,(0,1,2,3)]
pv[ 1:-1,(4,5,6,7)] = 1000.*dist/(2.*pt2pt_time)
prg.append(' nofrax') #needed to set all axis to use AltFeedRate
prg.append(' Coord[1].AltFeedRate=0') # allow maximum speed
prg.append(' Coord[1].SegMoveTime=0') #turn off segmented mode
prg.append(' linear abs')
prg.append(' X%g Z%g B%g Y%g' % tuple(pv[0, (0,1,2,3)]))
prg.append(' dwell 10')
@@ -805,10 +990,14 @@ close
if cnt>1:
prg.append(' P100=%d'%cnt)
prg.append(' N100:')
prg.append(' Coord[1].AltFeedRate=0') # allow maximum speed
prg.append(' Coord[1].SegMoveTime=1') #to calculate every 1 ms the inverse kinematics
prg.append(' pvt%g abs'%pt2pt_time) #100ms to next position
for idx in range(1,pv.shape[0]):
prg.append(' P2000=%d'%idx)
#prg.append(' P2000=%d'%idx)
prg.append(' X%g:%g Z%g:%g B%g:%g Y%g:%g' % tuple(pv[idx, (0,4,1,5,2,6,3,7)]))
#prg.append('Y%g:%g' % tuple(pv[idx, (5, 7)]))
#prg.append('B%g:%g' %(idx*1000,0))
if cnt>1:
prg.append(' dwell 10')
prg.append(' P100=P100-1')
@@ -847,6 +1036,7 @@ close
ack=GpasciiCommunicator.gpascii_ack
sys.stdout.write('wait execution...');sys.stdout.flush()
while(True):
#Gather.MaxLines calculates maximum numbewr of gathering into memory
com.write('P1000\n')
val=com.read_until(ack)
#print val
@@ -938,20 +1128,27 @@ Examples:'''+''.join(map(lambda s:cmd+s, exampleCmd))+'\n '
#hs.test_find_rot_ctr(n=5. ,per=1.,bias=2.31,ampl=4.12,phi=24.6)
hs.calcParam()
#hs.test_coord_trf()
hs.test_coord_trf()
#hs.interactive_cx_cz_w_fy()
#hs.interactive_dx_dz_w_y()
#SAR-CPPM-EXPMX1 MOTTEST-CPPM-CRM0485
hs.gen_coord_trf_code('/tmp/helicalscan.cfg','MOTTEST-CPPM-CRM0485')
#hs.gen_prog(file='/tmp/prg.cfg',host='MOTTEST-CPPM-CRM0485',mode=-1)
#hs.gen_prog(fnPrg='/tmp/prg.cfg',host='MOTTEST-CPPM-CRM0485',mode=0)
#hs.gen_prog(fnPrg='/tmp/prg.cfg',host='MOTTEST-CPPM-CRM0485',mode=0,cntHor=1,cntVert=3,wRng=(120000,120000))
#hs.gen_prog(fnPrg='/tmp/prg.cfg',host='MOTTEST-CPPM-CRM0485',mode=0,cntHor=1)
#hs.gen_prog(fnPrg='/tmp/prg.cfg',host='MOTTEST-CPPM-CRM0485',mode=0)
#hs.gen_prog(fnPrg='/tmp/prg.cfg',host='MOTTEST-CPPM-CRM0485',mode=0,cntHor=1,cntVert=5,wRng=(120000,120000))
#hs.gen_prog(fnPrg='/tmp/prg.cfg',host='MOTTEST-CPPM-CRM0485',mode=1,cntHor=1,cntVert=5,wRng=(120000,120000),pt2pt_time=100)
#hs.gen_prog(fnPrg='/tmp/prg.cfg',host='MOTTEST-CPPM-CRM0485',mode=1,cntHor=1,cntVert=5,wRng=(0,360000))
#hs.gen_prog(fnPrg='/tmp/prg.cfg',host='MOTTEST-CPPM-CRM0485',mode=1)
#hs.gen_prog(fnPrg='/tmp/prg.cfg',host='MOTTEST-CPPM-CRM0485',mode=1,
# pt2pt_time=100,cnt=1,cntVert=35,cntHor=7,hRng=(-.3,.3),wRng=(0,360000*3),yRng=(6.2,2.3))
hs.gen_prog(fnPrg='/tmp/prg.cfg',host='MOTTEST-CPPM-CRM0485',mode=1,
pt2pt_time=100,cnt=1,cntVert=35,cntHor=7,hRng=(-.3,.3),wRng=(0,360000*3),yRng=(6.2,2.3))
hs.load_rec()
hs.show_pos();hs.show_vel()
hs.interactive_anim()
hs.show_vel(); plt.show()
#hs.show_vel(); plt.show()
#hs.show_pos(); plt.show()
return