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

71
python/helicalscan.py
View File

@@ -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))