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

work on fwd/inv kinematics for PVT motion

Browse Source
This commit is contained in:
zamofing_t
2018-01-05 15:48:06 +01:00
parent e84c2020cd
commit f3e01ce33c
4 changed files with 127 additions and 74 deletions
Download Patch File Download Diff File Expand all files Collapse all files

25
Readme.md
View File

@@ -160,7 +160,8 @@ PBInspect --host $PPMAC --cfg PBInspect2.pbi
cd /home/zamofing_t/Documents/prj/SwissFEL/epics_ioc_modules/ESB_MX/cfg cd /home/zamofing_t/Documents/prj/SwissFEL/epics_ioc_modules/ESB_MX/cfg
gpasciiCommander --host $PPMAC sim_8_motors.cfg -i gpasciiCommander --host $PPMAC sim_8_motors.cfg -i
ssh root@$PPMAC sendgetsends -1 ssh root@$PPMAC
sendgetsends -1
cpx send 1"SampleMessage\n" cpx send 1"SampleMessage\n"
cd /home/zamofing_t/Documents/prj/SwissFEL/epics_ioc_modules/ESB_MX/python cd /home/zamofing_t/Documents/prj/SwissFEL/epics_ioc_modules/ESB_MX/python
@@ -266,3 +267,25 @@ export CAQTDM_DISPLAY_PATH=/net/slsfs-crtl/export/sf/common/config/qt/:/net/slsf
caqtdm -macro "P=SAR-EXPMX" ESB_MX_exp.ui caqtdm -macro "P=SAR-EXPMX" ESB_MX_exp.ui
``` ```
TODO 5.1.18
-----------
```
Fwd/inv kinematic is not yet fully correct for PVT moves:
Inverse kinematic need to calculate velocities:
Now a simplified approach with no dependencies is implemented:
vqCX=vDX
vqCZ=vDZ
vqW=vW
vqFY=vY
To see the differences compare:
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=(120000,120000),pt2pt_time=100,smt=0)
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,cntHor=1,cntVert=5,wRng=(0,360000),smt=0)
When using segmented mode (SegMoveTime>0) the velocities are not needed.
With SegMoveTime=0 these velocities are needed and will result in a non optimal trajectory (with the current buggy calculation).
```

4
cfg/sim_8_motors.cfg
View File

@@ -37,6 +37,6 @@ Motor[7].pLimits=0;Motor[7].AmpFaultLevel=0;Motor[7].pAmpEnable=0;Motor[7].pAmpF
!motor(mot=8,dirCur=0,contCur=100,peakCur=1000,timeAtPeak=1,JogSpeed=8.,numPhase=3,invDir=True) !motor(mot=8,dirCur=0,contCur=100,peakCur=1000,timeAtPeak=1,JogSpeed=8.,numPhase=3,invDir=True)
Motor[8].pLimits=0;Motor[8].AmpFaultLevel=0;Motor[8].pAmpEnable=0;Motor[8].pAmpFault=0 Motor[8].pLimits=0;Motor[8].AmpFaultLevel=0;Motor[8].pAmpEnable=0;Motor[8].pAmpFault=0
Motor[3].JogSpeed=1000 # used for joging Motor[3].JogSpeed=1000 //used for joging
Motor[3].MaxSpeed=1000 # used for motion program Motor[3].MaxSpeed=1000 //used for motion program
#1..8j/ #1..8j/

101
python/coordTrfTest.py
View File

@@ -258,32 +258,6 @@ open prog 2
Gather.Enable=0 Gather.Enable=0
P1000=1 P1000=1
close close
open prog 3
P1000=0
nofrax
Coord[1].AltFeedRate=0
Coord[1].SegMoveTime=0
linear abs
X0 Z0 B120000 Y2.3
dwell 10
Gather.Enable=2
Coord[1].AltFeedRate=0
Coord[1].SegMoveTime=0
pvt100 abs
X0:0 Z0:0 B120000:0 Y2.3:4.875
X0:0 Z0:0 B120000:0 Y3.275:9.75
X0:0 Z0:0 B120000:0 Y4.25:9.75
X0:0 Z0:0 B120000:0 Y5.225:9.75
X0:0 Z0:0 B120000:0 Y6.2:4.875
X0:0 Z0:0 B120000:0 Y6.2:0
dwell 1000
Gather.Enable=0
P1000=1
close
''') ''')
if mode==2: if mode==2:
gather=False gather=False
@@ -292,15 +266,20 @@ close
// 4 5 3 1 // 4 5 3 1
&1 &1
a a
#1->0
#2->0
#3->0
#4->0 #4->0
#5->0 #5->0
#3->0 #6->0
#1->0 #7->0
#8->0
#4->X
#5->Z
#3->B
#1->Y #1->Y
open forward
close
open inverse
close
''') ''')
if mode==3: if mode==3:
gather=False gather=False
@@ -310,42 +289,47 @@ a
// 4 5 3 1 // 4 5 3 1
&1 &1
a a
#1->0
#2->0
#3->0
#4->0 #4->0
#5->0 #5->0
#3->0 #6->0
#1->0 #7->0
#8->0
#4->I
#5->I
#3->I
#1->I #1->I
open forward open forward
define(qCX='L4', qCZ='L5', qW='L3', qFY='L1') define(q1='L1')
define(DX='C6', DZ='C8', W='C1', Y='C7') define(Y='C7')
//coord X Z B Y //coord Y
send 1"forward D0: %f \\n",D0 send 1"forward D0: %f \\n",D0
if(D0>0) callsub 100 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 D0=$00000080; //B=$2 X=$40 Y=$80 Z=$100 hex(2+int('40',16)+int('80',16)+int('100',16)) -> 0x1c2
N100: N100:
DX=qCX Y=q1
DZ=qCZ send 1"forward result %f\\n",Y
W=qW
Y=qFY
send 1"forward result %f %f %f %f\\n",DX,DZ,W,Y
P1001+=1 P1001+=1
close close
open inverse open inverse
define(DX='C6', DZ='C8', W='C1', Y='C7') define(q1='L1', vq1='R1')
//coord X Z B Y define(Y='C7',vY='C39')
//coord Y
send 1"inverse D0:%f _Y:%f:%f\\n",D0,Y,vY
//D0 is set to $000001c2 //D0 is set to $000001c2
define(qCX='L4', qCZ='L5', qW='L3', qFY='L1') define(q1='L1')
qCX=DX q1=Y
qCZ=DZ if(D0>0)
qW=W {
qFY=Y vq1=vY // THIS LINE IS USED FOR PVT MOVE !!!
send 1"inverse result %f %f %f %f\\n",qCX,qCZ,qW,qFY send 1"inverse result _q1:%f:%f\\n",q1,vq1
}
else
{
send 1"inverse result %f\\n",q1
}
P1002+=1 P1002+=1
close close
''') ''')
@@ -353,7 +337,7 @@ close
def test(self): def test(self):
file='/tmp/prg.cfg' file='/tmp/prg.cfg'
self.gen_code(0) #motor config #self.gen_code(0) #motor config
self.gen_code(1) #program code self.gen_code(1) #program code
self.gen_code(2) #simple coord trf self.gen_code(2) #simple coord trf
@@ -365,10 +349,17 @@ close
#prg.append(' Coord[1].SegMoveTime=0') # turn off segmented mode #prg.append(' Coord[1].SegMoveTime=0') # turn off segmented mode
#self.download_code(['&1;b1r',], file, True) #self.download_code(['&1;b1r',], file, True)
self.download_code(['&1;b2r',], file, True) #self.download_code(['Coord[1].AltFeedRate=100','Coord[1].SegMoveTime=0','&1;b2r',], file, True)
self.download_code(['Coord[1].AltFeedRate=0','Coord[1].SegMoveTime=0','&1;b2r',], file, True)
#self.download_code(['Coord[1].AltFeedRate=1','Coord[1].SegMoveTime=.1','&1;b2r',], file, True)
#self.download_code(['Coord[1].AltFeedRate=0','Coord[1].SegMoveTime=.1','&1;b2r',], file, True)
#self.download_code(['&1;b3r',], file, True) #self.download_code(['&1;b3r',], file, True)
# *** READ USER MANUAL P 455 ***
# Inverse-Kinematic Program for PVT Mode, No Segmentation
#
if __name__=='__main__': if __name__=='__main__':
ct=CoordTrf(verbose=255,host='MOTTEST-CPPM-CRM0485') ct=CoordTrf(verbose=255,host='MOTTEST-CPPM-CRM0485')
ct.test() ct.test()

71
python/helicalscan.py
View File

@@ -689,7 +689,7 @@ open forward
define(p1_x='L13', p1_y='L14', p1_z='L15') define(p1_x='L13', p1_y='L14', p1_z='L15')
define(scale='L16') define(scale='L16')
//send 1"forward kinematic %f %f %f %f\\n",qCX,qCZ,qW,qFY''') send 1"fwd_inp(%f) %f %f %f %f\\n",D0,qCX,qCZ,qW,qFY''')
for i in range(2): for i in range(2):
#https://stackoverflow.com/questions/3471999/how-do-i-merge-two-lists-into-a-single-list #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] l=[j for i in zip((i,) * param.shape[1], list(param[i])) for j in i]
@@ -711,7 +711,7 @@ open forward
DX=qCX-p0_x DX=qCX-p0_x
DZ=qCZ-p0_z DZ=qCZ-p0_z
Y=qFY Y=qFY
//send 1"forward result %f %f %f %f\\n",DX,DZ,W,Y send 1"fwd_res %f %f %f %f\\n",DX,DZ,W,Y
//P1001+=1 //P1001+=1
D0=$000001c2; //B=$2 X=$40 Y=$80 Z=$100 hex(2+int('40',16)+int('80',16)+int('100',16)) -> 0x1c2 D0=$000001c2; //B=$2 X=$40 Y=$80 Z=$100 hex(2+int('40',16)+int('80',16)+int('100',16)) -> 0x1c2
close close
@@ -719,14 +719,22 @@ close
prg.append(''' prg.append('''
open inverse open inverse
define(DX='C6', DZ='C8', W='C1', Y='C7') //coord X Z B Y
//coord X Z B Y define( DX='C6' , DZ='C8' , W='C1', Y='C7')
//D0 is set to $000001c2 define(vDX='C38', vDZ='C40', vW='C33', vY='C39')
//motor q4 q5 q3 q1
define( qCX='L4', qCZ='L5', qW='L3', qFY='L1')
define(vqCX='R4', vqCZ='R5', vqW='R3', vqFY='R1')
define(qCX='L4', qCZ='L5', qW='L3', qFY='L1') define(qCX='L4', qCZ='L5', qW='L3', qFY='L1')
define(p0_x='L10', p0_y='L11', p0_z='L12') define(p0_x='L10', p0_y='L11', p0_z='L12')
define(p1_x='L13', p1_y='L14', p1_z='L15') define(p1_x='L13', p1_y='L14', p1_z='L15')
define(scale='L16') define(scale='L16')
//send 1"inverse kinematic %f %f %f %f\\n",DX,DZ,W,Y''') if(D0>0)
send 1"inv_inp(%f) %f:%f %f:%f %f:%f %f:%f\\n",D0,DX,vDX,DZ,vDZ,W,vW,Y,vY
else
send 1"inv_inp(%f) %f %f %f %f\\n",D0,DX,DZ,W,Y''')
for i in range(2): for i in range(2):
# https://stackoverflow.com/questions/3471999/how-do-i-merge-two-lists-into-a-single-list # 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] l = [j for i in zip((i,) * param.shape[1], list(param[i])) for j in i]
@@ -749,7 +757,16 @@ open inverse
qCX=DX+p0_x qCX=DX+p0_x
qCZ=DZ+p0_z qCZ=DZ+p0_z
qFY=Y qFY=Y
//send 1"inverse result %f %f %f %f\\n",qCX,qCZ,qW,qFY if(D0>0)
{ // calculate velocities for PVT motion
vqCX=vDX
vqCZ=vDZ
vqW=vW
vqFY=vY
send 1"inv_res %f:%f %f:%f %f:%f %f:%f\\n",qCX,vqCX,qCZ,vqCZ,qW,vqW,qFY,vqFY
}
else
send 1"inv_res %f %f %f %f\\n",qCX,qCZ,qW,qFY
//P1002+=1 //P1002+=1
close close
''') ''')
@@ -806,10 +823,11 @@ a
#1->I #1->I
open forward open forward
//define(KinVelEna='D0',KinAxisUsed='D0')
define(qCX='L4', qCZ='L5', qW='L3', qFY='L1') define(qCX='L4', qCZ='L5', qW='L3', qFY='L1')
define(DX='C6', DZ='C8', W='C1', Y='C7') define(DX='C6', DZ='C8', W='C1', Y='C7')
//coord X Z B Y //coord X Z B Y
send 1"forward D0: %f \\n",D0 send 1"fwd_inp(%f) %f %f %f %f\\n",D0,qCX,qCZ,qW,qFY
if(D0>0) callsub 100 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 D0=$000001c2; //B=$2 X=$40 Y=$80 Z=$100 hex(2+int('40',16)+int('80',16)+int('100',16)) -> 0x1c2
N100: N100:
@@ -817,20 +835,38 @@ N100:
DZ=qCZ DZ=qCZ
W=qW W=qW
Y=qFY Y=qFY
send 1"forward result %f %f %f %f\\n",DX,DZ,W,Y send 1"fwd_res %f %f %f %f\\n",DX,DZ,W,Y
P1001+=1 P1001+=1
close close
open inverse open inverse
define(DX='C6', DZ='C8', W='C1', Y='C7') //coord X Z B Y
//coord X Z B Y define( DX='C6' , DZ='C8' , W='C1', Y='C7')
//D0 is set to $000001c2 define(vDX='C38', vDZ='C40', vW='C33', vY='C39')
define(qCX='L4', qCZ='L5', qW='L3', qFY='L1')
//motor q4 q5 q3 q1
define( qCX='L4', qCZ='L5', qW='L3', qFY='L1')
define(vqCX='R4', vqCZ='R5', vqW='R3', vqFY='R1')
if(D0>0)
send 1"inv_inp(%f) %f:%f %f:%f %f:%f %f:%f\\n",D0,DX,vDX,DZ,vDZ,W,vW,Y,vY
else
send 1"inv_inp(%f) %f %f %f %f\\n",D0,DX,DZ,W,Y
qCX=DX qCX=DX
qCZ=DZ qCZ=DZ
qW=W qW=W
qFY=Y qFY=Y
send 1"inverse result %f %f %f %f\\n",qCX,qCZ,qW,qFY if(D0>0)
{ // calculate velocities for PVT motion
vqCX=vDX
vqCZ=vDZ
vqW=vW
vqFY=vY
send 1"inv_res %f:%f %f:%f %f:%f %f:%f\\n",qCX,vqCX,qCZ,vqCZ,qW,vqW,qFY,vqFY
}
else
send 1"inv_res %f %f %f %f\\n",qCX,qCZ,qW,qFY
P1002+=1 P1002+=1
close close
''') ''')
@@ -943,7 +979,7 @@ close
prg.append(' dwell 100') prg.append(' dwell 100')
prg.append(' Gather.Enable=2') prg.append(' Gather.Enable=2')
prg.append(' Coord[1].AltFeedRate=0') # allow maximum speed 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(' Coord[1].SegMoveTime=0') #to calculate every 1 ms the inverse kinematics
for i in range(pt.shape[0]): for i in range(pt.shape[0]):
prg.append(' X%g Z%g B%g Y%g' % tuple(pt[i, :])) prg.append(' X%g Z%g B%g Y%g' % tuple(pt[i, :]))
prg.append(' dwell 100') prg.append(' dwell 100')
@@ -959,6 +995,7 @@ close
wRng = kwargs.get('wRng', (0,360000)) wRng = kwargs.get('wRng', (0,360000))
yRng = kwargs.get('yRng', (2.3,6.2)) yRng = kwargs.get('yRng', (2.3,6.2))
pt2pt_time = kwargs.get('pt2pt_time', 100) pt2pt_time = kwargs.get('pt2pt_time', 100)
smt = kwargs.get('smt', 1) # SegMoveTime, default = 1ms -> velocity calc not yet 100% correct (smt=0 not 100% working)
numPt=cntVert*cntHor numPt=cntVert*cntHor
pt=np.zeros((numPt,4)) pt=np.zeros((numPt,4))
if cntHor>1: if cntHor>1:
@@ -991,7 +1028,7 @@ close
prg.append(' P100=%d'%cnt) prg.append(' P100=%d'%cnt)
prg.append(' N100:') prg.append(' N100:')
prg.append(' Coord[1].AltFeedRate=0') # allow maximum speed 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(' Coord[1].SegMoveTime=%d'%smt) #to calculate every 1 ms the inverse kinematics
prg.append(' pvt%g abs'%pt2pt_time) #100ms to next position prg.append(' pvt%g abs'%pt2pt_time) #100ms to next position
for idx in range(1,pv.shape[0]): for idx in range(1,pv.shape[0]):
#prg.append(' P2000=%d'%idx) #prg.append(' P2000=%d'%idx)
@@ -1140,7 +1177,9 @@ Examples:'''+''.join(map(lambda s:cmd+s, exampleCmd))+'\n '
#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)
#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=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=(120000,120000),pt2pt_time=100)
#hs.gen_prog(fnPrg='/tmp/prg.cfg',host='MOTTEST-CPPM-CRM0485',mode=1,cntHor=1,cntVert=5,wRng=(120000,120000),pt2pt_time=100,smt=0)
#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,cntHor=1,cntVert=5,wRng=(0,360000))
#hs.gen_prog(fnPrg='/tmp/prg.cfg',host='MOTTEST-CPPM-CRM0485',mode=1,cntHor=1,cntVert=5,wRng=(0,360000),smt=0)
#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)
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)) pt2pt_time=100,cnt=1,cntVert=35,cntHor=7,hRng=(-.3,.3),wRng=(0,360000*3),yRng=(6.2,2.3))