From f3e01ce33cb46708c8950d54879cac077411faf1 Mon Sep 17 00:00:00 2001 From: Thierry Zamofing Date: Fri, 5 Jan 2018 15:48:06 +0100 Subject: [PATCH] work on fwd/inv kinematics for PVT motion --- Readme.md | 25 +++++++++- cfg/sim_8_motors.cfg | 4 +- python/coordTrfTest.py | 101 +++++++++++++++++++---------------------- python/helicalscan.py | 71 ++++++++++++++++++++++------- 4 files changed, 127 insertions(+), 74 deletions(-) diff --git a/Readme.md b/Readme.md index 980bf09..efe2c93 100644 --- a/Readme.md +++ b/Readme.md @@ -160,7 +160,8 @@ PBInspect --host $PPMAC --cfg PBInspect2.pbi cd /home/zamofing_t/Documents/prj/SwissFEL/epics_ioc_modules/ESB_MX/cfg gpasciiCommander --host $PPMAC sim_8_motors.cfg -i -ssh root@$PPMAC sendgetsends -1 +ssh root@$PPMAC +sendgetsends -1 cpx send 1"SampleMessage\n" 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 ``` + +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). +``` diff --git a/cfg/sim_8_motors.cfg b/cfg/sim_8_motors.cfg index d8dfa7c..765bbdd 100644 --- a/cfg/sim_8_motors.cfg +++ b/cfg/sim_8_motors.cfg @@ -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[8].pLimits=0;Motor[8].AmpFaultLevel=0;Motor[8].pAmpEnable=0;Motor[8].pAmpFault=0 -Motor[3].JogSpeed=1000 # used for joging -Motor[3].MaxSpeed=1000 # used for motion program +Motor[3].JogSpeed=1000 //used for joging +Motor[3].MaxSpeed=1000 //used for motion program #1..8j/ diff --git a/python/coordTrfTest.py b/python/coordTrfTest.py index 7e0227a..ebc58f7 100755 --- a/python/coordTrfTest.py +++ b/python/coordTrfTest.py @@ -258,32 +258,6 @@ open prog 2 Gather.Enable=0 P1000=1 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: gather=False @@ -292,15 +266,20 @@ close // 4 5 3 1 &1 a +#1->0 +#2->0 +#3->0 #4->0 #5->0 -#3->0 -#1->0 +#6->0 +#7->0 +#8->0 -#4->X -#5->Z -#3->B #1->Y +open forward +close +open inverse +close ''') if mode==3: gather=False @@ -310,42 +289,47 @@ a // 4 5 3 1 &1 a +#1->0 +#2->0 +#3->0 #4->0 #5->0 -#3->0 -#1->0 +#6->0 +#7->0 +#8->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 + define(q1='L1') + define(Y='C7') + //coord 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 + D0=$00000080; //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 + Y=q1 + send 1"forward result %f\\n",Y P1001+=1 close open inverse - define(DX='C6', DZ='C8', W='C1', Y='C7') - //coord X Z B Y + define(q1='L1', vq1='R1') + define(Y='C7',vY='C39') + //coord Y + send 1"inverse D0:%f _Y:%f:%f\\n",D0,Y,vY //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 + define(q1='L1') + q1=Y + if(D0>0) + { + vq1=vY // THIS LINE IS USED FOR PVT MOVE !!! + send 1"inverse result _q1:%f:%f\\n",q1,vq1 + } + else + { + send 1"inverse result %f\\n",q1 + } P1002+=1 close ''') @@ -353,7 +337,7 @@ close def test(self): 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(2) #simple coord trf @@ -365,10 +349,17 @@ close #prg.append(' Coord[1].SegMoveTime=0') # turn off segmented mode #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) +# *** READ USER MANUAL P 455 *** +# Inverse-Kinematic Program for PVT Mode, No Segmentation +# + if __name__=='__main__': ct=CoordTrf(verbose=255,host='MOTTEST-CPPM-CRM0485') ct.test() diff --git a/python/helicalscan.py b/python/helicalscan.py index 4a4d3fe..1a2678b 100755 --- a/python/helicalscan.py +++ b/python/helicalscan.py @@ -689,7 +689,7 @@ open forward 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''') + send 1"fwd_inp(%f) %f %f %f %f\\n",D0,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] @@ -711,7 +711,7 @@ open forward DX=qCX-p0_x DZ=qCZ-p0_z 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 D0=$000001c2; //B=$2 X=$40 Y=$80 Z=$100 hex(2+int('40',16)+int('80',16)+int('100',16)) -> 0x1c2 close @@ -719,14 +719,22 @@ close prg.append(''' open inverse - define(DX='C6', DZ='C8', W='C1', Y='C7') - //coord X Z B Y - //D0 is set to $000001c2 + //coord X Z B Y + define( DX='C6' , DZ='C8' , W='C1', Y='C7') + 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(p0_x='L10', p0_y='L11', p0_z='L12') define(p1_x='L13', p1_y='L14', p1_z='L15') 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): # 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] @@ -749,7 +757,16 @@ open inverse qCX=DX+p0_x qCZ=DZ+p0_z 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 close ''') @@ -806,10 +823,11 @@ a #1->I open forward + //define(KinVelEna='D0',KinAxisUsed='D0') 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 + send 1"fwd_inp(%f) %f %f %f %f\\n",D0,qCX,qCZ,qW,qFY 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: @@ -817,20 +835,38 @@ N100: DZ=qCZ W=qW 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 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') + //coord X Z B Y + define( DX='C6' , DZ='C8' , W='C1', Y='C7') + 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') + 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 qCZ=DZ qW=W 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 close ''') @@ -943,7 +979,7 @@ close 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 + prg.append(' Coord[1].SegMoveTime=0') #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') @@ -959,6 +995,7 @@ close wRng = kwargs.get('wRng', (0,360000)) yRng = kwargs.get('yRng', (2.3,6.2)) 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 pt=np.zeros((numPt,4)) if cntHor>1: @@ -991,7 +1028,7 @@ close 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(' Coord[1].SegMoveTime=%d'%smt) #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) @@ -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,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,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) #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))