diff --git a/python/coordTrfTest.py b/python/coordTrfTest.py
new file mode 100755
index 0000000..7e0227a
--- /dev/null
+++ b/python/coordTrfTest.py
@@ -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()
+