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

working forward and inverse trf on PowerBrick

Browse Source
This commit is contained in:
zamofing_t
2017-12-08 08:53:10 +01:00
parent 9a663ac251
commit 7548044946
2 changed files with 26 additions and 24 deletions
Download Patch File Download Diff File Expand all files Collapse all files

47
python/helicalscan.py
View File

@@ -149,11 +149,11 @@ class HelicalScan:
def test_coord_trf(self):
self.calcParam()
param = self.param
dx, dz, w, y, = (0.2,0.3,0.1,4.3)
dx, dz, w, y, = (0.2,0.3,0.1,3.3)
print 'input : dx:%.3g dz:%.3g w:%.3g fy:%.3g' % (dx,dz,w/d2r,y)
(cx,cz,w,fy) = self.inv_transform(dx,dz,w,y)
print 'inv_trf: cx:%.3g cz:%.3g w:%.3g fy:%.3g' % (cx,cz,w/d2r,fy)
(dx, dz,w,y) = self.fwd_transform(cx,cz,w,fy)
(dx,dz,w,y) = self.fwd_transform(cx,cz,w,fy)
print 'fwd_trf: dx:%.3g dz:%.3g w:%.3g fy:%.3g' % (dx,dz,w/d2r,y)
# plt.ion()
@@ -483,29 +483,29 @@ open forward
//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(f='L16')
send 1"forward kinematic\\n"''')
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
l=[j for i in zip((i,) * param.shape[1], list(param[i])) for j in i]
prg.append(" define(z_%i=%g, y_%i=%g, x_%i=%g, r_%i=%g, phi_%i=%g)"%tuple(l))
prg.append('''
p0_x=x_0+r_0*sin(phi_0+W)
p1_x=x_1+r_1*sin(phi_1+W)
p0_x=x_0+r_0*sin(phi_0+qW)
p1_x=x_1+r_1*sin(phi_1+qW)
p0_y=y_0
p1_y=y_1
p0_z=z_0+r_0*cos(phi_0+W)
p1_z=z_1+r_1*cos(phi_0+W)
p0_z=z_0+r_0*cos(phi_0+qW)
p1_z=z_1+r_1*cos(phi_1+qW)
f=(qFY-y_0)/(y_1-y_0)
p0_x=p0_x+f*(p1_x-p0_x)
p0_y=p0_y+f*(p1_y-p0_y)
p0_z=p0_z+f*(p1_z-p0_z)
scale=(qFY-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)
DX=qCX-p0_x
DZ=qCZ-p0_z
Y=qFY
W=qW
send 1"forward result %f %f %f %f\\n",DX,DZ,W,Y
D0=$000001c2; //B=$2 X=$40 Y=$80 Z=$100 hex(2+int('40',16)+int('80',16)+int('100',16)) -> 0x1c2
close
@@ -519,9 +519,8 @@ open inverse
define(qCX='L7', qCZ='L8', qW='L1', qFY='L2')
define(p0_x='L10', p0_y='L11', p0_z='L12')
define(p1_x='L13', p1_y='L14', p1_z='L15')
define(f='L16')
send 1"inverse kinematic\\n"''')
define(scale='L16')
send 1"inverse kinematic %f %f %f %f\\n",DX,DZ,W,Y''')
for i in range(2):
# https://stackoverflow.com/questions/3471999/how-do-i-merge-two-lists-into-a-single-list
l = [j for i in zip((i,) * param.shape[1], list(param[i])) for j in i]
@@ -532,16 +531,17 @@ open inverse
p0_y=y_0
p1_y=y_1
p0_z=z_0+r_0*cos(phi_0+W)
p1_z=z_1+r_1*cos(phi_0+W)
p1_z=z_1+r_1*cos(phi_1+W)
f=(qFY-y_0)/(y_1-y_0)
p0_x=p0_x+f*(p1_x-p0_x)
p0_y=p0_y+f*(p1_y-p0_y)
p0_z=p0_z+f*(p1_z-p0_z)
scale=(qFY-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)
qCX=DX+p0_x
qCZ=DZ+p0_z
qFY=Y
qW=W
send 1"inverse result %f %f %f %f\\n",qCX,qCZ,qW,qFY
close
''')
@@ -747,10 +747,11 @@ Examples:'''+''.join(map(lambda s:cmd+s, exampleCmd))+'\n '
hs=HelicalScan(args)
#hs.sequencer()
hs.args.verbose=255;hs.calcParam();hs.gen_coord_trf_code('/tmp/helicalscan.cfg','MOTTEST-CPPM-CRM0485')
#return
hs.test_coord_trf()
return
hs.test_find_rot_ctr()
hs.test_find_rot_ctr(n=5. ,per=1.,bias=2.31,ampl=4.12,phi=24.6)
hs.test_coord_trf()
hs.interactive_cx_cz_w_fy()
hs.interactive_dx_dz_w_y()