367 lines
9.9 KiB
Python
Executable File
367 lines
9.9 KiB
Python
Executable File
#!/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
|
|
''')
|
|
if mode==2:
|
|
gather=False
|
|
prg.append('''
|
|
//motors CX CZ RY FY
|
|
// 4 5 3 1
|
|
&1
|
|
a
|
|
#1->0
|
|
#2->0
|
|
#3->0
|
|
#4->0
|
|
#5->0
|
|
#6->0
|
|
#7->0
|
|
#8->0
|
|
|
|
#1->Y
|
|
open forward
|
|
close
|
|
open inverse
|
|
close
|
|
''')
|
|
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
|
|
#1->0
|
|
#2->0
|
|
#3->0
|
|
#4->0
|
|
#5->0
|
|
#6->0
|
|
#7->0
|
|
#8->0
|
|
|
|
#1->I
|
|
|
|
open forward
|
|
define(q1='L1')
|
|
define(Y='C7')
|
|
//coord Y
|
|
send 1"forward D0: %f \\n",D0
|
|
if(D0>0) callsub 100
|
|
D0=$00000080; //B=$2 X=$40 Y=$80 Z=$100 hex(2+int('40',16)+int('80',16)+int('100',16)) -> 0x1c2
|
|
N100:
|
|
Y=q1
|
|
send 1"forward result %f\\n",Y
|
|
P1001+=1
|
|
close
|
|
|
|
open inverse
|
|
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(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
|
|
''')
|
|
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(['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()
|
|
|