test coordTrf fwd/inv kinematics
This commit is contained in:
375
python/coordTrfTest.py
Executable file
375
python/coordTrfTest.py
Executable file
@@ -0,0 +1,375 @@
|
||||
#!/usr/bin/env python
|
||||
# *-----------------------------------------------------------------------*
|
||||
# | |
|
||||
# | Copyright (c) 2017 by Paul Scherrer Institute (http://www.psi.ch) |
|
||||
# | |
|
||||
# | Author Thierry Zamofing (thierry.zamofing@psi.ch) |
|
||||
# *-----------------------------------------------------------------------*
|
||||
'''
|
||||
tools to check kinematics
|
||||
'''
|
||||
import os, sys, json
|
||||
import numpy as np
|
||||
import matplotlib as mpl
|
||||
import matplotlib.pyplot as plt
|
||||
import mpl_toolkits.mplot3d as plt3d
|
||||
import matplotlib.animation as anim
|
||||
from matplotlib.widgets import Slider
|
||||
import subprocess as sprc
|
||||
from utilities import *
|
||||
import telnetlib
|
||||
|
||||
class GpasciiCommunicator():
|
||||
'''Communicates with the Delta Tau gpascii programm
|
||||
'''
|
||||
gpascii_ack="\x06\r\n"
|
||||
gpascii_inp='Input\r\n'
|
||||
|
||||
def connect(self, host, username='root', password='deltatau',prompt='ppmac# ',verbose=0):
|
||||
p=telnetlib.Telnet(host)
|
||||
s=p.read_until('login: ')
|
||||
if verbose: print(s)
|
||||
p.write(username+'\n')
|
||||
s =p.read_until('Password: ')
|
||||
if verbose: print(s)
|
||||
p.write(password+'\n')
|
||||
s =p.read_until(prompt) # command prompt
|
||||
if verbose: print(s)
|
||||
p.write('gpascii -2\n') # execute gpascii command
|
||||
s=p.read_until(self.gpascii_inp)
|
||||
if verbose: print(s)
|
||||
return p
|
||||
|
||||
|
||||
class CoordTrf:
|
||||
def __init__(self,**kwargs):
|
||||
for k,v in kwargs.iteritems():
|
||||
setattr(self,k,v)
|
||||
pass
|
||||
|
||||
def show_vel(self):
|
||||
rec=self.rec
|
||||
fig = plt.figure()
|
||||
#y = np.diff(rec[:, 2])
|
||||
y = np.diff(rec,axis=0)
|
||||
mx=y.max(0)
|
||||
mn=y.min(0)
|
||||
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])))
|
||||
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='bg'
|
||||
lbl=('q1','q2')
|
||||
dx=.1/len(lbl)
|
||||
for i in range(rec.shape[1]):
|
||||
if i==0:
|
||||
ax = fig.add_axes([.1, .05, .85, .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: q1:%.3f q2:%.3f'%((idx,)+tuple(data[idx,:]))
|
||||
#print msg
|
||||
event.canvas.toolbar.set_message(msg)
|
||||
|
||||
def download_code(self, prg, file=None, gather=False):
|
||||
host=self.host
|
||||
if self.verbose & 4:
|
||||
for ln in prg:
|
||||
print(ln)
|
||||
if file is not None:
|
||||
fh = open(file, 'w')
|
||||
fh.write('\n'.join(prg))
|
||||
fh.close()
|
||||
if host is not None:
|
||||
cmd = 'gpasciiCommander --host ' + host + ' ' + file
|
||||
print(cmd)
|
||||
p = sprc.Popen(cmd, shell=True) # , stdout=sprc.PIPE, stderr=sprc.STDOUT)
|
||||
# res=p.stdout.readlines(); print res
|
||||
retval = p.wait()
|
||||
# gather -u /var/ftp/gather/out.txt
|
||||
if gather:
|
||||
# ***wait program finished P1000=1***
|
||||
com=GpasciiCommunicator().connect(host,prompt='# ')
|
||||
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
|
||||
val=int(val[val.find('=')+1:].rstrip(ack))
|
||||
if val==1:break
|
||||
#com.write('Gather.Index\n')
|
||||
#val=com.read_until(ack)
|
||||
#print val
|
||||
time.sleep(.2)
|
||||
sys.stdout.write('.');sys.stdout.flush()
|
||||
fnRmt = '/var/ftp/gather/out.txt'
|
||||
fnLoc = '/tmp/gather.txt'
|
||||
print('\ngather data to %s...' % fnRmt)
|
||||
p = sprc.Popen(('ssh', 'root@' + host, 'gather ', '-u', fnRmt), shell=False, stdin=sprc.PIPE, stdout=sprc.PIPE,
|
||||
stderr=sprc.PIPE)
|
||||
res = p.wait()
|
||||
if res:
|
||||
print('ssh failed. ssh root@%s to open a session' % host)
|
||||
return
|
||||
|
||||
print('transfer data to %s...' % fnLoc)
|
||||
p = sprc.Popen(('scp', 'root@' + host + ':' + fnRmt, fnLoc), shell=False, stdin=sprc.PIPE, stdout=sprc.PIPE,
|
||||
stderr=sprc.PIPE)
|
||||
res = p.wait()
|
||||
self.rec = np.genfromtxt(fnLoc, delimiter=' ')
|
||||
#plt.ion()
|
||||
self.show_pos();self.show_vel()
|
||||
plt.show()
|
||||
|
||||
|
||||
|
||||
|
||||
def gen_code(self, mode):
|
||||
prg=[]
|
||||
file='/tmp/prg%d.cfg'%mode
|
||||
gather=True
|
||||
if mode==0:
|
||||
gather=False
|
||||
prg.append('''
|
||||
$$$***
|
||||
!common()
|
||||
!encoder_sim(enc=1,tbl=1,mot=1)
|
||||
!motor(mot=1,dirCur=0,contCur=100,peakCur=100,timeAtPeak=1,JogSpeed=8.,numPhase=3,invDir=True)
|
||||
Motor[1].pLimits=0;Motor[1].AmpFaultLevel=0;Motor[1].pAmpEnable=0;Motor[1].pAmpFault=0
|
||||
|
||||
!encoder_sim(enc=2,tbl=2,mot=2)
|
||||
!motor(mot=2,dirCur=0,contCur=100,peakCur=1000,timeAtPeak=1,JogSpeed=8.,numPhase=3,invDir=True)
|
||||
Motor[2].pLimits=0;Motor[2].AmpFaultLevel=0;Motor[2].pAmpEnable=0;Motor[2].pAmpFault=0
|
||||
|
||||
!encoder_sim(enc=3,tbl=3,mot=3)
|
||||
!motor(mot=3,dirCur=0,contCur=100,peakCur=1000,timeAtPeak=1,JogSpeed=8.,numPhase=3,invDir=True)
|
||||
Motor[3].pLimits=0;Motor[3].AmpFaultLevel=0;Motor[3].pAmpEnable=0;Motor[3].pAmpFault=0
|
||||
|
||||
!encoder_sim(enc=4,tbl=4,mot=4)
|
||||
!motor(mot=4,dirCur=0,contCur=100,peakCur=1000,timeAtPeak=1,JogSpeed=8.,numPhase=3,invDir=True)
|
||||
Motor[4].pLimits=0;Motor[4].AmpFaultLevel=0;Motor[4].pAmpEnable=0;Motor[4].pAmpFault=0
|
||||
|
||||
!encoder_sim(enc=5,tbl=5,mot=5)
|
||||
!motor(mot=5,dirCur=0,contCur=100,peakCur=1000,timeAtPeak=1,JogSpeed=8.,numPhase=3,invDir=True)
|
||||
Motor[5].pLimits=0;Motor[5].AmpFaultLevel=0;Motor[5].pAmpEnable=0;Motor[5].pAmpFault=0
|
||||
|
||||
!encoder_sim(enc=6,tbl=6,mot=6)
|
||||
!motor(mot=6,dirCur=0,contCur=100,peakCur=1000,timeAtPeak=1,JogSpeed=8.,numPhase=3,invDir=True)
|
||||
Motor[6].pLimits=0;Motor[6].AmpFaultLevel=0;Motor[6].pAmpEnable=0;Motor[6].pAmpFault=0
|
||||
|
||||
!encoder_sim(enc=7,tbl=7,mot=7)
|
||||
!motor(mot=7,dirCur=0,contCur=100,peakCur=1000,timeAtPeak=1,JogSpeed=8.,numPhase=3,invDir=True)
|
||||
Motor[7].pLimits=0;Motor[7].AmpFaultLevel=0;Motor[7].pAmpEnable=0;Motor[7].pAmpFault=0
|
||||
|
||||
!encoder_sim(enc=8,tbl=8,mot=8)
|
||||
!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
|
||||
#1..8j/
|
||||
''')
|
||||
if mode==1:
|
||||
gather=False
|
||||
prg.append('''
|
||||
#1..2j=0
|
||||
|
||||
Gather.Enable=0
|
||||
Gather.Items=2
|
||||
Gather.MaxSamples=1000000
|
||||
Gather.Period=10
|
||||
Gather.Addr[0]=Motor[1].ActPos.a
|
||||
Gather.Addr[1]=Motor[2].ActPos.a
|
||||
|
||||
open prog 1
|
||||
jog1=0
|
||||
dwell100
|
||||
P1000=0
|
||||
nofrax
|
||||
linearabs
|
||||
Gather.Enable=2
|
||||
Y2.0
|
||||
dwell100
|
||||
Y4.0
|
||||
dwell100
|
||||
Y6.0
|
||||
dwell100
|
||||
Y8.0
|
||||
dwell100
|
||||
Gather.Enable=0
|
||||
P1000=1
|
||||
close
|
||||
|
||||
open prog 2
|
||||
jog1=0
|
||||
dwell100
|
||||
P1000=0
|
||||
nofrax
|
||||
Gather.Enable=2
|
||||
pvt100abs
|
||||
Y0:0
|
||||
Y2:20
|
||||
Y4:20
|
||||
Y6:20
|
||||
Y8:0
|
||||
dwell100
|
||||
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
|
||||
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
|
||||
''')
|
||||
if mode==3:
|
||||
gather=False
|
||||
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
|
||||
''')
|
||||
self.download_code(prg, file, gather)
|
||||
|
||||
def test(self):
|
||||
file='/tmp/prg.cfg'
|
||||
self.gen_code(0) #motor config
|
||||
self.gen_code(1) #program code
|
||||
|
||||
self.gen_code(2) #simple coord trf
|
||||
#self.download_code(['&1;b1r',], file, True)
|
||||
self.download_code(['&1;b2r',], file, True)
|
||||
|
||||
self.gen_code(3) #fwd/inv kinematics
|
||||
#prg.append(' Coord[1].AltFeedRate=0') # allow maximum speed
|
||||
#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(['&1;b3r',], file, True)
|
||||
|
||||
|
||||
if __name__=='__main__':
|
||||
ct=CoordTrf(verbose=255,host='MOTTEST-CPPM-CRM0485')
|
||||
ct.test()
|
||||
|
||||
Reference in New Issue
Block a user