Added Dominik's Python Scripts

python_algorithms/Calibration_Routines/create_LUT_omega_OUTPUT.csv

@@ -0,0 +1,37 @@
+,OMEGA,X,Y,Z
+0,5.0,5.909392537313434e-05,9.868161194029852e-06,-0.00039495467462686564
+1,15.0,0.0001290561801801802,0.0001035904084084084,-0.0007849931951951953
+2,25.0,0.00017507500000000001,-2.269573192771084e-05,-0.0013272097710843374
+3,35.0,0.0002545680575757576,-0.00013335097878787878,-0.002011397887878788
+4,45.0,0.00030339992514970065,-0.0002967246796407186,-0.0025266904281437126
+5,55.0,0.00040550968072289165,-0.0005867319969879519,-0.002738596189759036
+6,65.0,0.0005440292335329341,-0.0011674720928143711,-0.002663125511976048
+7,75.0,0.0007441960634441086,-0.0017789477432024168,-0.0025174750332326284
+8,85.0,0.0009327745074626867,-0.0024226592716417913,-0.002197805223880597
+9,95.0,0.0011406777627627628,-0.0028108397867867866,-0.001957066045045045
+10,105.0,0.0012695368761329305,-0.002815012160120846,-0.001523616259818731
+11,115.0,0.0014212054504504504,-0.0025032628828828827,-0.000752324822822823
+12,125.0,0.0015787265225225226,-0.0021586821651651653,1.187065765765764e-05
+13,135.0,0.0017429303987915408,-0.0018317292779456195,0.0006209941329305135
+14,145.0,0.0018899760476190478,-0.0013783368095238095,0.0008465621458333333
+15,155.0,0.001979165918674699,-0.0006346651987951807,0.0007060866626506024
+16,165.0,0.0019819500271084337,0.00012890914457831324,0.0005609045451807229
+17,175.0,0.0020824341060606062,0.0009598767393939394,0.0002693357060606061
+18,185.0,0.002061735329305136,0.001451296848942598,-0.0001240640996978852
+19,195.0,0.001989431587878788,0.0015961538272727272,-0.0006776197393939394
+20,205.0,0.0019664731164179107,0.0014465387343283581,-0.0014275834925373135
+21,215.0,0.0018614367530120478,0.0012564003222891567,-0.0022541468373493976
+22,225.0,0.0018020323592814374,0.0009859418083832333,-0.002981173386227545
+23,235.0,0.001697820469879518,0.0004888292590361445,-0.0033421301024096385
+24,245.0,0.001574653459459459,-9.0471993993994e-05,-0.0033992601771771773
+25,255.0,0.001418204341389728,-0.000882334655589124,-0.003456446915407855
+26,265.0,0.001222578087613293,-0.001608071912386707,-0.003335449474320242
+27,275.0,0.0010579737027027027,-0.0020967322162162164,-0.0031538016516516514
+28,285.0,0.0009059466000000001,-0.0022713038727272725,-0.0027494998878787877
+29,295.0,0.0006926335030120481,-0.002189087629518072,-0.0020717855542168677
+30,305.0,0.0005690860209580839,-0.0021271746616766465,-0.0013094347964071856
+31,315.0,0.00042595616516516525,-0.002061734339339339,-0.0007213954684684686
+32,325.0,0.00030983462839879154,-0.0018158917280966767,-0.0003932621540785499
+33,335.0,0.0003028431411411411,-0.0013482420900900903,-0.000353129093093093
+34,345.0,0.00029773215868263473,-0.0006864582245508982,-0.0004993741706586826
+35,355.0,0.00021540581393372982,0.00010695226508071366,-0.0007016237553101104

python_algorithms/Dominik/1_check_DMS_Alignment/1_1_AerotechMotion/210827_measure_DMS_Tool_linearity.py

@@ -0,0 +1,223 @@
+#!/usr/bin/env python
+
+# Dieses Script verfaehrt den Aerotech translativ mit GMX,GMY und GMZ.
+# Der Ausgangswert des Scripts beschreibt den Winkelversatz zwischen Aerotech und
+# dem Kalibrier Tool.
+
+# Dominik Buntschu, 2.8.2021
+
+import rospy
+from sensor_msgs.msg import JointState
+import requests
+import time
+from datetime import date
+from datetime import datetime
+import matplotlib.pyplot as plt
+from mpl_toolkits.mplot3d import Axes3D
+import numpy as np
+import epics
+import csv
+import math
+
+DMS_X=[];
+DMS_Y=[];
+DMS_Z=[];
+DMS_Seq=[];
+DMS_Secs=[];
+DMS_Nsecs=[];
+OMEGA_inst=0
+OMEGA=[];
+
+# Aktueller Tag
+today = date.today()
+current_day = today.strftime("%Y_%m_%d_")
+#print(current_day)
+
+# Aktuelle Zeit
+now = datetime.now()
+current_time = now.strftime("%H:%M:%S_")
+#print(current_time)
+#print(current_day+current_time)
+
+# Auslesen der Omega Achse
+def callback_LJUE9_JointState(data):
+
+    global OMEGA_inst
+    OMEGA_inst = data.position[0]
+
+# Auselsen des Kalibriertools
+# Auslesen Hinzufuegen eines Zeitstempels
+# Einbinden des Aerotech Omegawinkels (OMEGA_inst)    
+def callback_readbackCAL_JointState(data):
+
+    global OMEGA,DMS_X,DMS_Y,DMS_Z,DMS_Seq,DMS_Secs,DMS_Nsecs
+
+    DMS_X.append(data.position[0])
+    DMS_Y.append(data.position[1])
+    DMS_Z.append(data.position[2])
+    DMS_Secs.append(data.header.stamp.secs)
+    DMS_Nsecs.append(data.header.stamp.nsecs)
+    DMS_Seq.append(data.header.seq)
+    OMEGA.append(OMEGA_inst)
+
+# Funktion um aktueller Werte des Kaliobriertools zu speichern
+# Wird fuer Makierung im 3D Plot verwendet    
+def getCurrentPoint():
+    return [DMS_X[-1],DMS_Y[-1],DMS_Z[-1]]
+
+# Stellt Seitenlaengen des 3D Plot in Relation   
+def set_axes_equal(ax):
+
+    x_limits = ax.get_xlim3d()
+    y_limits = ax.get_ylim3d()
+    z_limits = ax.get_zlim3d()
+
+    x_range = abs(x_limits[1] - x_limits[0])
+    x_middle = np.mean(x_limits)
+    y_range = abs(y_limits[1] - y_limits[0])
+    y_middle = np.mean(y_limits)
+    z_range = abs(z_limits[1] - z_limits[0])
+    z_middle = np.mean(z_limits)
+
+    print (f"x_range: {x_range}")
+    print (f"y_range: {y_range}")
+    print (f"z_range: {z_range}")
+    print (f"x_middle: {x_middle}")
+    print (f"y_middle: {y_middle}")
+    print (f"z_middle: {z_middle}")
+
+    plot_radius = 0.5*max([x_range, y_range, z_range])
+
+    ax.set_xlim3d([x_middle - plot_radius, x_middle + plot_radius])
+    ax.set_ylim3d([y_middle - plot_radius, y_middle + plot_radius])
+    ax.set_zlim3d([z_middle - plot_radius, z_middle + plot_radius])
+
+
+def calculate_correction(VECT):
+    current = VECT[0]
+    centre = (max(VECT) + min(VECT))/2.
+    correction = -(current-centre)
+    print (f"MAX= {max(VECT)}, MIN= {min(VECT)}")
+    print (f"current {current}")
+    print (f"centre {centre}")
+    print (f"CORRECTION: {correction}")
+
+### START SCRIPT HERE #########################################################
+
+#AEROTECH_EPICS_RECORD = "X06MX-ES-DF1"
+AEROTECH_EPICS_RECORD = "X06SA-ES-DF1"
+
+print ("This script moves Aerotech GMX GMY and GMZ by aprox. 2mm")
+print ("and records the calibration tool position during this motion.")
+print ("***** Calibration Tool is ready, in contact and feedback is active *******")
+
+key=input ("OK to continue? (y/n) ")
+if (key != "y"):
+    print ('Stopping script.')
+    exit(code=None)
+
+# starten der Datenaufzeichnung
+rospy.init_node('DMS_Recorder', anonymous=True)
+subsOMEGA=rospy.Subscriber("/LJUE9_JointState", JointState, callback_LJUE9_JointState)
+subsDMS  =rospy.Subscriber("/readbackCAL", JointState, callback_readbackCAL_JointState)
+print ("Starting data collection...")
+
+# smargopolo_server = "http://smargopolo:3000"
+
+# Setzt die Geschwidigkeit der Aerotech Achsen
+epics.caput("X06MX-ES-DF1:GMX-SETV",2)
+epics.caput("X06MX-ES-DF1:GMZ-SETV",2)
+epics.caput("X06MX-ES-DF1:GMY-SETV",2)
+
+# Macht X Iterationen eines Rechteckes mit jeweils einem Versatz
+# Veranschaulicht den Winkelversatz zwischen Aerotech und Kalibriertool        
+for i in range(0,5,1):
+    print("Move GMZ to +2")
+    epics.caput("X06MX-ES-DF1:GMZ-INCP",2)
+    time.sleep(2)
+    Point1 = getCurrentPoint()
+    print("Move GMY to +2")
+    epics.caput("X06MX-ES-DF1:GMY-INCP",2)
+    time.sleep(2)
+    Point2 = getCurrentPoint()
+    print("Move GMZ to -2")
+    epics.caput("X06MX-ES-DF1:GMZ-INCP",-2)
+    time.sleep(2)
+    Point3 = getCurrentPoint()
+    print("Move GMY to -2")
+    epics.caput("X06MX-ES-DF1:GMY-INCP",-2)
+    time.sleep(2)
+    print("Move GMX to +1")
+    epics.caput("X06MX-ES-DF1:GMX-INCP",1)
+    time.sleep(2)
+
+
+
+# beenden der Datenaufzeichnung
+rospy.signal_shutdown('finished measuring')
+################################################################################
+
+# 3D Plot der Bewegung mit den Werten des Kalibirertools
+# Start der Bewegung = x Punkt
+fig = plt.figure()
+ax=fig.add_subplot(111, projection='3d')
+ax.plot(DMS_Z,DMS_X,DMS_Y)
+ax.set_xlabel("DMS_Z [mm]")
+ax.set_ylabel("DMS_X [mm]")
+ax.set_zlabel("DMS_Y [mm]")
+ax.set_title("3D Plot: Measure Linearity to DMS Tool",fontsize=14,fontweight="bold")
+ax.plot(DMS_Z[0:1],DMS_X[0],DMS_Y[0],label='StartPoint',marker=(5,0))
+ax.plot([Point1[2]],[Point1[0]],[Point1[1]],label='Point1',marker=(5,1))
+ax.plot([Point2[2]],  [Point2[0]],  [Point2[1]],label='Point2',  marker=(5,2))
+ax.plot([Point3[2]],  [Point3[0]],  [Point3[1]],label='Point3',  marker=(5,3))
+ax.legend()
+set_axes_equal(ax)
+fig.show()
+
+# 2D Plot der Bewegung mit den Werten des Kaibriertools
+#fig2 = plt.figure()
+#ax2=fig2.add_subplot(111)
+#ax2.plot(OMEGA, DMS_X, label='DMS_X')
+#ax2.plot(OMEGA, DMS_Y, label='DMS_Y')
+#ax2.plot(OMEGA, DMS_Z, label='DMS_Z')
+#ax2.set_xlabel("OMEGA")
+#ax2.legend()
+#fig2.show()
+
+#Berrechnung des Versatzes zwischen Aerotech und Kalibirertool-> Rotation um GMX
+YOffset= Point2[1]-Point1[1]
+ZOffset= Point2[2]-Point1[2]
+GMXOffset = (math.tan(ZOffset/YOffset))*(180/math.pi)
+print (f"Winkelversatz um GMX: {GMXOffset}")
+
+#Berrechnung des Versatzes zwischen Aerotech und Kalibirertool-> Rotation um GMZ
+YOffset= Point2[1]-Point1[1]
+XOffset= Point2[0]-Point1[0]
+GMZOffset = (math.tan(XOffset/YOffset))*(180/math.pi)
+print (f"Winkelversatz um GMZ: {GMZOffset}")
+
+#Berrechnung des Versatzes zwischen Aerotech und Kalibirertool-> Rotation um GMY
+ZOffset= Point2[2]-Point3[2]
+XOffset= Point2[0]-Point3[0]
+GMYOffset = (math.tan(XOffset/ZOffset))*(180/math.pi)
+print (f"Winkelversatz um GMY: {GMYOffset}")
+
+##########################################i######################################
+
+# Speichern der Daten in ein CSV.
+# Bennenung des Files mit Datum und Zeitstempel
+rows = zip(OMEGA, DMS_X, DMS_Y, DMS_Z, DMS_Seq, DMS_Secs, DMS_Nsecs)
+with open(current_day + current_time +'measure_DMS_Tool_linearity.csv', 'w', newline='') as file:
+    writer = csv.writer(file)
+    writer.writerow(["OMEGA", "DMS_X", "DMS_Y", "DMS_Z", "DMS_Seq", "DMS_Secs", "DMS_Nsecs"])
+    for row in rows:
+        writer.writerow(row)
+
+# Bewegung der GMX Achse zurueck auf den Startwert
+print("Move GMX back to Start Position")
+epics.caput("X06MX-ES-DF1:GMX-INCP",-5)
+time.sleep(2)
+
+input("done.")
+
+

python_algorithms/Dominik/1_check_DMS_Alignment/1_1_AerotechMotion/Test_Aerotech_Resolution.py

@@ -0,0 +1,57 @@
+#!/usr/bin/env python
+
+# Dieses Script verfaehrt den Aerotech translativ mit GMX,GMY und GMZ.
+# Der Ausgangswert des Scripts beschreibt den Winkelversatz zwischen Aerotech und
+# dem Kalibrier Tool.
+
+# Dominik Buntschu, 2.8.2021
+
+import epics
+import time
+
+# Setzt die Geschwidigkeit der Aerotech Achsen
+epics.caput("X06MX-ES-DF1:GMX-SETV",2)
+epics.caput("X06MX-ES-DF1:GMZ-SETV",2)
+epics.caput("X06MX-ES-DF1:GMY-SETV",2)
+
+# Faehrt Aerotech Achsen in Startpositionen
+epics.caput("X06MX-ES-DF1:GMX-SETP",85)
+epics.caput("X06MX-ES-DF1:GMY-SETP",4)
+epics.caput("X06MX-ES-DF1:GMZ-SETP",23)
+time.sleep(3)
+# Macht X Iterationen eines Rechteckes mit jeweils einem Versatz
+# Veranschaulicht den Winkelversatz zwischen Aerotech und Kalibriertool 
+Step1 = 5
+Step3 = 3       
+for i in range(0,Step1,1):
+    print("Move GMY to +0.001")
+    epics.caput("X06MX-ES-DF1:GMY-INCP",0.001)
+    time.sleep(2.5)
+
+for i in range(0,Step1*2,1):
+    print("Move GMY to -0.001")
+    epics.caput("X06MX-ES-DF1:GMY-INCP",-0.001)
+    time.sleep(2.5)
+    
+for i in range(0,Step1,1):
+    print("Move GMY to +0.001")
+    epics.caput("X06MX-ES-DF1:GMY-INCP",0.001)
+    time.sleep(2.5)
+
+for i in range(0,Step3,1):
+    print("Move GMY to +0.003")
+    epics.caput("X06MX-ES-DF1:GMY-INCP",0.003)
+    time.sleep(2.5)
+
+for i in range(0,Step3*2,1):
+    print("Move GMY to -0.003")
+    epics.caput("X06MX-ES-DF1:GMY-INCP",-0.003)
+    time.sleep(2.5)
+
+for i in range(0,Step3,1):
+    print("Move GMY to +0.003")
+    epics.caput("X06MX-ES-DF1:GMY-INCP",0.003)
+    time.sleep(2.5)
+
+
+

python_algorithms/Dominik/1_check_DMS_Alignment/1_1_AerotechMotion/Test_Cap_Sensor_Accuracy.py

@@ -0,0 +1,56 @@
+#!/usr/bin/env python
+
+# Dieses Script verfaehrt den Aerotech translativ mit GMY.
+# Anhand des Scripts wir die Genauigkeit der Sensoren ermittelt.
+
+# Dominik Buntschu, 2.8.2021
+
+import epics
+import time
+
+# Setzt die Geschwidigkeit der Aerotech Achsen
+epics.caput("X06MX-ES-DF1:GMX-SETV",2)
+epics.caput("X06MX-ES-DF1:GMZ-SETV",2)
+epics.caput("X06MX-ES-DF1:GMY-SETV",2)
+
+# Faehrt Aerotech Achsen in Startpositionen
+epics.caput("X06MX-ES-DF1:GMX-SETP",85)
+epics.caput("X06MX-ES-DF1:GMY-SETP",4)
+epics.caput("X06MX-ES-DF1:GMZ-SETP",23)
+time.sleep(3)
+# Macht X Iterationen eines Rechteckes mit jeweils einem Versatz
+# Veranschaulicht den Winkelversatz zwischen Aerotech und Kalibriertool 
+Step1 = 5
+Step3 = 3       
+for i in range(0,Step1,1):
+    print("Move GMY to +0.001")
+    epics.caput("X06MX-ES-DF1:GMY-INCP",0.001)
+    time.sleep(2.5)
+
+for i in range(0,Step1*2,1):
+    print("Move GMY to -0.001")
+    epics.caput("X06MX-ES-DF1:GMY-INCP",-0.001)
+    time.sleep(2.5)
+    
+for i in range(0,Step1,1):
+    print("Move GMY to +0.001")
+    epics.caput("X06MX-ES-DF1:GMY-INCP",0.001)
+    time.sleep(2.5)
+
+for i in range(0,Step3,1):
+    print("Move GMY to +0.003")
+    epics.caput("X06MX-ES-DF1:GMY-INCP",0.003)
+    time.sleep(2.5)
+
+for i in range(0,Step3*2,1):
+    print("Move GMY to -0.003")
+    epics.caput("X06MX-ES-DF1:GMY-INCP",-0.003)
+    time.sleep(2.5)
+
+for i in range(0,Step3,1):
+    print("Move GMY to +0.003")
+    epics.caput("X06MX-ES-DF1:GMY-INCP",0.003)
+    time.sleep(2.5)
+
+
+

python_algorithms/Dominik/1_check_DMS_Alignment/1_1_AerotechMotion/Test_Cap_Sensor_Accuracy_2.py

@@ -0,0 +1,56 @@
+#!/usr/bin/env python
+
+# Dieses Script verfaehrt den Aerotech translativ mit GMY.
+# Anhand des Scripts wir die Genauigkeit der Sensoren ermittelt.
+
+# Dominik Buntschu, 2.8.2021
+
+import epics
+import time
+
+# Setzt die Geschwidigkeit der Aerotech Achsen
+epics.caput("X06MX-ES-DF1:GMX-SETV",2)
+epics.caput("X06MX-ES-DF1:GMZ-SETV",2)
+epics.caput("X06MX-ES-DF1:GMY-SETV",2)
+
+# Faehrt Aerotech Achsen in Startpositionen
+epics.caput("X06MX-ES-DF1:GMX-SETP",85)
+epics.caput("X06MX-ES-DF1:GMY-SETP",4)
+epics.caput("X06MX-ES-DF1:GMZ-SETP",23)
+time.sleep(3)
+# Macht X Iterationen eines Rechteckes mit jeweils einem Versatz
+# Veranschaulicht den Winkelversatz zwischen Aerotech und Kalibriertool 
+Step1 = 5
+Step3 = 3       
+for i in range(0,Step1,1):
+    print("Move GMY to +0.001")
+    epics.caput("X06MX-ES-DF1:GMY-INCP",0.001)
+    time.sleep(2.5)
+
+for i in range(0,Step1*2,1):
+    print("Move GMY to -0.001")
+    epics.caput("X06MX-ES-DF1:GMY-INCP",-0.001)
+    time.sleep(2.5)
+    
+for i in range(0,Step1,1):
+    print("Move GMY to +0.001")
+    epics.caput("X06MX-ES-DF1:GMY-INCP",0.001)
+    time.sleep(2.5)
+
+for i in range(0,Step3,1):
+    print("Move GMY to +0.002")
+    epics.caput("X06MX-ES-DF1:GMY-INCP",0.002)
+    time.sleep(2.5)
+
+for i in range(0,Step3*2,1):
+    print("Move GMY to -0.002")
+    epics.caput("X06MX-ES-DF1:GMY-INCP",-0.002)
+    time.sleep(2.5)
+
+for i in range(0,Step3,1):
+    print("Move GMY to +0.002")
+    epics.caput("X06MX-ES-DF1:GMY-INCP",0.002)
+    time.sleep(2.5)
+
+
+

python_algorithms/Dominik/1_check_DMS_Alignment/1_1_AerotechMotion/Test_Cap_Sensor_Accuracy_GMX.py

@@ -0,0 +1,56 @@
+#!/usr/bin/env python
+
+# Dieses Script verfaehrt den Aerotech translativ mit GMY.
+# Anhand des Scripts wir die Genauigkeit der Sensoren ermittelt.
+
+# Dominik Buntschu, 2.8.2021
+
+import epics
+import time
+
+# Setzt die Geschwidigkeit der Aerotech Achsen
+epics.caput("X06MX-ES-DF1:GMX-SETV",2)
+epics.caput("X06MX-ES-DF1:GMZ-SETV",2)
+epics.caput("X06MX-ES-DF1:GMY-SETV",2)
+
+# Faehrt Aerotech Achsen in Startpositionen
+epics.caput("X06MX-ES-DF1:GMX-SETP",85)
+epics.caput("X06MX-ES-DF1:GMY-SETP",4)
+epics.caput("X06MX-ES-DF1:GMZ-SETP",23)
+time.sleep(3)
+# Macht X Iterationen eines Rechteckes mit jeweils einem Versatz
+# Veranschaulicht den Winkelversatz zwischen Aerotech und Kalibriertool 
+Step1 = 5
+Step3 = 3       
+for i in range(0,Step1,1):
+    print("Move GMX to +0.001")
+    epics.caput("X06MX-ES-DF1:GMX-INCP",0.001)
+    time.sleep(2.5)
+
+for i in range(0,Step1*2,1):
+    print("Move GMX to -0.001")
+    epics.caput("X06MX-ES-DF1:GMX-INCP",-0.001)
+    time.sleep(2.5)
+    
+for i in range(0,Step1,1):
+    print("Move GMX to +0.001")
+    epics.caput("X06MX-ES-DF1:GMX-INCP",0.001)
+    time.sleep(2.5)
+
+for i in range(0,Step3,1):
+    print("Move GMX to +0.003")
+    epics.caput("X06MX-ES-DF1:GMX-INCP",0.003)
+    time.sleep(2.5)
+
+for i in range(0,Step3*2,1):
+    print("Move GMX to -0.003")
+    epics.caput("X06MX-ES-DF1:GMX-INCP",-0.003)
+    time.sleep(2.5)
+
+for i in range(0,Step3,1):
+    print("Move GMX to +0.003")
+    epics.caput("X06MX-ES-DF1:GMX-INCP",0.003)
+    time.sleep(2.5)
+
+
+

python_algorithms/Dominik/1_check_DMS_Alignment/1_1_AerotechMotion/Test_Cap_Sensor_Accuracy_GMX_2.py

@@ -0,0 +1,55 @@
+#!/usr/bin/env python
+
+# Dieses Script verfaehrt den Aerotech translativ mit GMY.
+# Anhand des Scripts wir die Genauigkeit der Sensoren ermittelt.
+
+# Dominik Buntschu, 2.8.2021
+
+import epics
+import time
+
+# Setzt die Geschwidigkeit der Aerotech Achsen
+epics.caput("X06MX-ES-DF1:GMX-SETV",2)
+epics.caput("X06MX-ES-DF1:GMZ-SETV",2)
+epics.caput("X06MX-ES-DF1:GMY-SETV",2)
+
+# Faehrt Aerotech Achsen in Startpositionen
+epics.caput("X06MX-ES-DF1:GMX-SETP",85)
+epics.caput("X06MX-ES-DF1:GMY-SETP",4)
+epics.caput("X06MX-ES-DF1:GMZ-SETP",23)
+time.sleep(3)
+# Macht X Iterationen eines Rechteckes mit jeweils einem Versatz
+# Veranschaulicht den Winkelversatz zwischen Aerotech und Kalibriertool 
+Step1 = 4
+Step2 = 3
+      
+for i in range(0,Step1,1):
+    print("Move GMX to +0.001")
+    epics.caput("X06MX-ES-DF1:GMX-INCP",0.0005)
+    time.sleep(2.5)
+
+for i in range(0,Step1*2,1):
+    print("Move GMX to -0.001")
+    epics.caput("X06MX-ES-DF1:GMX-INCP",-0.0005)
+    time.sleep(2.5)
+    
+for i in range(0,Step1,1):
+    print("Move GMX to +0.001")
+    epics.caput("X06MX-ES-DF1:GMX-INCP",0.0005)
+    time.sleep(2.5)
+
+for i in range(0,Step2,1):
+    print("Move GMX to +0.001")
+    epics.caput("X06MX-ES-DF1:GMX-INCP",0.0005)
+    time.sleep(2.5)
+
+for i in range(0,Step2*2,1):
+    print("Move GMX to -0.001")
+    epics.caput("X06MX-ES-DF1:GMX-INCP",-0.0005)
+    time.sleep(2.5)
+    
+for i in range(0,Step2,1):
+    print("Move GMX to +0.001")
+    epics.caput("X06MX-ES-DF1:GMX-INCP",0.0005)
+    time.sleep(2.5)
+

python_algorithms/Dominik/1_check_DMS_Alignment/1_1_AerotechMotion/Test_Cap_Sensor_Accuracy_GMY.py

@@ -0,0 +1,56 @@
+#!/usr/bin/env python
+
+# Dieses Script verfaehrt den Aerotech translativ mit GMY.
+# Anhand des Scripts wir die Genauigkeit der Sensoren ermittelt.
+
+# Dominik Buntschu, 2.8.2021
+
+import epics
+import time
+
+# Setzt die Geschwidigkeit der Aerotech Achsen
+epics.caput("X06MX-ES-DF1:GMX-SETV",2)
+epics.caput("X06MX-ES-DF1:GMZ-SETV",2)
+epics.caput("X06MX-ES-DF1:GMY-SETV",2)
+
+# Faehrt Aerotech Achsen in Startpositionen
+epics.caput("X06MX-ES-DF1:GMX-SETP",85)
+epics.caput("X06MX-ES-DF1:GMY-SETP",4)
+epics.caput("X06MX-ES-DF1:GMZ-SETP",23)
+time.sleep(3)
+# Macht X Iterationen eines Rechteckes mit jeweils einem Versatz
+# Veranschaulicht den Winkelversatz zwischen Aerotech und Kalibriertool 
+Step1 = 5
+Step3 = 3       
+for i in range(0,Step1,1):
+    print("Move GMY to +0.001")
+    epics.caput("X06MX-ES-DF1:GMY-INCP",0.001)
+    time.sleep(2.5)
+
+for i in range(0,Step1*2,1):
+    print("Move GMY to -0.001")
+    epics.caput("X06MX-ES-DF1:GMY-INCP",-0.001)
+    time.sleep(2.5)
+    
+for i in range(0,Step1,1):
+    print("Move GMY to +0.001")
+    epics.caput("X06MX-ES-DF1:GMY-INCP",0.001)
+    time.sleep(2.5)
+
+for i in range(0,Step3,1):
+    print("Move GMY to +0.003")
+    epics.caput("X06MX-ES-DF1:GMY-INCP",0.003)
+    time.sleep(2.5)
+
+for i in range(0,Step3*2,1):
+    print("Move GMY to -0.003")
+    epics.caput("X06MX-ES-DF1:GMY-INCP",-0.003)
+    time.sleep(2.5)
+
+for i in range(0,Step3,1):
+    print("Move GMY to +0.003")
+    epics.caput("X06MX-ES-DF1:GMY-INCP",0.003)
+    time.sleep(2.5)
+
+
+

python_algorithms/Dominik/1_check_DMS_Alignment/1_1_AerotechMotion/measure_DMS_Tool_linearity.py

@@ -0,0 +1,223 @@
+#!/usr/bin/env python
+
+# Dieses Script verfaehrt den Aerotech translativ mit GMX,GMY und GMZ.
+# Der Ausgangswert des Scripts beschreibt den Winkelversatz zwischen Aerotech und
+# dem Kalibrier Tool.
+
+# Dominik Buntschu, 2.8.2021
+
+import rospy
+from sensor_msgs.msg import JointState
+import requests
+import time
+from datetime import date
+from datetime import datetime
+import matplotlib.pyplot as plt
+from mpl_toolkits.mplot3d import Axes3D
+import numpy as np
+import epics
+import csv
+import math
+
+DMS_X=[];
+DMS_Y=[];
+DMS_Z=[];
+DMS_Seq=[];
+DMS_Secs=[];
+DMS_Nsecs=[];
+OMEGA_inst=0
+OMEGA=[];
+
+# Aktueller Tag
+today = date.today()
+current_day = today.strftime("%Y_%m_%d_")
+#print(current_day)
+
+# Aktuelle Zeit
+now = datetime.now()
+current_time = now.strftime("%H:%M:%S_")
+#print(current_time)
+#print(current_day+current_time)
+
+# Auslesen der Omega Achse
+def callback_LJUE9_JointState(data):
+
+    global OMEGA_inst
+    OMEGA_inst = data.position[0]
+
+# Auselsen des Kalibriertools
+# Auslesen Hinzufuegen eines Zeitstempels
+# Einbinden des Aerotech Omegawinkels (OMEGA_inst)    
+def callback_readbackCAL_JointState(data):
+
+    global OMEGA,DMS_X,DMS_Y,DMS_Z,DMS_Seq,DMS_Secs,DMS_Nsecs
+
+    DMS_X.append(data.position[0])
+    DMS_Y.append(data.position[1])
+    DMS_Z.append(data.position[2])
+    DMS_Secs.append(data.header.stamp.secs)
+    DMS_Nsecs.append(data.header.stamp.nsecs)
+    DMS_Seq.append(data.header.seq)
+    OMEGA.append(OMEGA_inst)
+
+# Funktion um aktueller Werte des Kaliobriertools zu speichern
+# Wird fuer Makierung im 3D Plot verwendet    
+def getCurrentPoint():
+    return [DMS_X[-1],DMS_Y[-1],DMS_Z[-1]]
+
+# Stellt Seitenlaengen des 3D Plot in Relation   
+def set_axes_equal(ax):
+
+    x_limits = ax.get_xlim3d()
+    y_limits = ax.get_ylim3d()
+    z_limits = ax.get_zlim3d()
+
+    x_range = abs(x_limits[1] - x_limits[0])
+    x_middle = np.mean(x_limits)
+    y_range = abs(y_limits[1] - y_limits[0])
+    y_middle = np.mean(y_limits)
+    z_range = abs(z_limits[1] - z_limits[0])
+    z_middle = np.mean(z_limits)
+
+    print (f"x_range: {x_range}")
+    print (f"y_range: {y_range}")
+    print (f"z_range: {z_range}")
+    print (f"x_middle: {x_middle}")
+    print (f"y_middle: {y_middle}")
+    print (f"z_middle: {z_middle}")
+
+    plot_radius = 0.5*max([x_range, y_range, z_range])
+
+    ax.set_xlim3d([x_middle - plot_radius, x_middle + plot_radius])
+    ax.set_ylim3d([y_middle - plot_radius, y_middle + plot_radius])
+    ax.set_zlim3d([z_middle - plot_radius, z_middle + plot_radius])
+
+
+def calculate_correction(VECT):
+    current = VECT[0]
+    centre = (max(VECT) + min(VECT))/2.
+    correction = -(current-centre)
+    print (f"MAX= {max(VECT)}, MIN= {min(VECT)}")
+    print (f"current {current}")
+    print (f"centre {centre}")
+    print (f"CORRECTION: {correction}")
+
+### START SCRIPT HERE #########################################################
+
+#AEROTECH_EPICS_RECORD = "X06MX-ES-DF1"
+AEROTECH_EPICS_RECORD = "X06SA-ES-DF1"
+
+print ("This script moves Aerotech GMX GMY and GMZ by aprox. 2mm")
+print ("and records the calibration tool position during this motion.")
+print ("***** Calibration Tool is ready, in contact and feedback is active *******")
+
+key=input ("OK to continue? (y/n) ")
+if (key != "y"):
+    print ('Stopping script.')
+    exit(code=None)
+
+# starten der Datenaufzeichnung
+rospy.init_node('DMS_Recorder', anonymous=True)
+subsOMEGA=rospy.Subscriber("/LJUE9_JointState", JointState, callback_LJUE9_JointState)
+subsDMS  =rospy.Subscriber("/readbackCAL", JointState, callback_readbackCAL_JointState)
+print ("Starting data collection...")
+
+# smargopolo_server = "http://smargopolo:3000"
+
+# Setzt die Geschwidigkeit der Aerotech Achsen
+epics.caput("X06MX-ES-DF1:GMX-SETV",2)
+epics.caput("X06MX-ES-DF1:GMZ-SETV",2)
+epics.caput("X06MX-ES-DF1:GMY-SETV",2)
+
+# Macht X Iterationen eines Rechteckes mit jeweils einem Versatz
+# Veranschaulicht den Winkelversatz zwischen Aerotech und Kalibriertool        
+for i in range(0,5,1):
+    print("Move GMZ to +2")
+    epics.caput("X06MX-ES-DF1:GMZ-INCP",2)
+    time.sleep(2)
+    Point1 = getCurrentPoint()
+    print("Move GMY to +2")
+    epics.caput("X06MX-ES-DF1:GMY-INCP",2)
+    time.sleep(2)
+    Point2 = getCurrentPoint()
+    print("Move GMZ to -2")
+    epics.caput("X06MX-ES-DF1:GMZ-INCP",-2)
+    time.sleep(2)
+    Point3 = getCurrentPoint()
+    print("Move GMY to -2")
+    epics.caput("X06MX-ES-DF1:GMY-INCP",-2)
+    time.sleep(2)
+    print("Move GMX to +1")
+    epics.caput("X06MX-ES-DF1:GMX-INCP",1)
+    time.sleep(2)
+
+
+
+# beenden der Datenaufzeichnung
+rospy.signal_shutdown('finished measuring')
+################################################################################
+
+# 3D Plot der Bewegung mit den Werten des Kalibirertools
+# Start der Bewegung = x Punkt
+fig = plt.figure()
+ax=fig.add_subplot(111, projection='3d')
+ax.plot(DMS_Z,DMS_X,DMS_Y)
+ax.set_xlabel("DMS_Z [mm]")
+ax.set_ylabel("DMS_X [mm]")
+ax.set_zlabel("DMS_Y [mm]")
+ax.set_title("3D Plot: Measure Linearity to DMS Tool",fontsize=14,fontweight="bold")
+ax.plot(DMS_Z[0:1],DMS_X[0],DMS_Y[0],label='StartPoint',marker=(5,0))
+ax.plot([Point1[2]],[Point1[0]],[Point1[1]],label='Point1',marker=(5,1))
+ax.plot([Point2[2]],  [Point2[0]],  [Point2[1]],label='Point2',  marker=(5,2))
+ax.plot([Point3[2]],  [Point3[0]],  [Point3[1]],label='Point3',  marker=(5,3))
+ax.legend()
+set_axes_equal(ax)
+fig.show()
+
+# 2D Plot der Bewegung mit den Werten des Kaibriertools
+#fig2 = plt.figure()
+#ax2=fig2.add_subplot(111)
+#ax2.plot(OMEGA, DMS_X, label='DMS_X')
+#ax2.plot(OMEGA, DMS_Y, label='DMS_Y')
+#ax2.plot(OMEGA, DMS_Z, label='DMS_Z')
+#ax2.set_xlabel("OMEGA")
+#ax2.legend()
+#fig2.show()
+
+#Berrechnung des Versatzes zwischen Aerotech und Kalibirertool-> Rotation um GMX
+YOffset= Point2[1]-Point1[1]
+ZOffset= Point2[2]-Point1[2]
+GMXOffset = (math.tan(ZOffset/YOffset))*(180/math.pi)
+print (f"Winkelversatz um GMX: {GMXOffset}")
+
+#Berrechnung des Versatzes zwischen Aerotech und Kalibirertool-> Rotation um GMZ
+YOffset= Point2[1]-Point1[1]
+XOffset= Point2[0]-Point1[0]
+GMZOffset = (math.tan(XOffset/YOffset))*(180/math.pi)
+print (f"Winkelversatz um GMZ: {GMZOffset}")
+
+#Berrechnung des Versatzes zwischen Aerotech und Kalibirertool-> Rotation um GMY
+ZOffset= Point2[2]-Point3[2]
+XOffset= Point2[0]-Point3[0]
+GMYOffset = (math.tan(XOffset/ZOffset))*(180/math.pi)
+print (f"Winkelversatz um GMY: {GMYOffset}")
+
+##########################################i######################################
+
+# Speichern der Daten in ein CSV.
+# Bennenung des Files mit Datum und Zeitstempel
+rows = zip(OMEGA, DMS_X, DMS_Y, DMS_Z, DMS_Seq, DMS_Secs, DMS_Nsecs)
+with open(current_day + current_time +'measure_DMS_Tool_linearity.csv', 'w', newline='') as file:
+    writer = csv.writer(file)
+    writer.writerow(["OMEGA", "DMS_X", "DMS_Y", "DMS_Z", "DMS_Seq", "DMS_Secs", "DMS_Nsecs"])
+    for row in rows:
+        writer.writerow(row)
+
+# Bewegung der GMX Achse zurueck auf den Startwert
+print("Move GMX back to Start Position")
+epics.caput("X06MX-ES-DF1:GMX-INCP",-5)
+time.sleep(2)
+
+input("done.")
+
+

python_algorithms/Dominik/1_check_DMS_Alignment/1_1_AerotechMotion/old/210804_measure_repeatability_aerotech_2.py

@@ -0,0 +1,200 @@
+#!/usr/bin/env python
+
+# This Script creates a lookup table from the measured error CSV
+# Dominik Buntschu, 2.8.2021
+
+import rospy
+from sensor_msgs.msg import JointState
+import requests
+import time
+from datetime import date
+from datetime import datetime
+import matplotlib.pyplot as plt
+from mpl_toolkits.mplot3d import Axes3D
+import numpy as np
+import epics
+import csv
+import math
+
+DMS_X=[];
+DMS_Y=[];
+DMS_Z=[];
+DMS_Seq=[];
+DMS_Secs=[];
+DMS_Nsecs=[];
+OMEGA_inst=0
+OMEGA=[];
+
+today = date.today()
+current_day = today.strftime("%Y_%m_%d_")
+#print(current_day)
+
+now = datetime.now()
+current_time = now.strftime("%H:%M:%S_")
+#print(current_time)
+#print(current_day+current_time)
+
+def callback_LJUE9_JointState(data):
+
+    global OMEGA_inst
+    OMEGA_inst = data.position[0]
+
+
+def callback_readbackCAL_JointState(data):
+
+    global OMEGA,DMS_X,DMS_Y,DMS_Z,DMS_Seq,DMS_Secs,DMS_Nsecs
+
+    DMS_X.append(data.position[0])
+    DMS_Y.append(data.position[1])
+    DMS_Z.append(data.position[2])
+    DMS_Secs.append(data.header.stamp.secs)
+    DMS_Nsecs.append(data.header.stamp.nsecs)
+    DMS_Seq.append(data.header.seq)
+    OMEGA.append(OMEGA_inst)
+
+def getCurrentPoint():
+    return [DMS_X[-1],DMS_Y[-1],DMS_Z[-1]]
+
+
+def set_axes_equal(ax):
+    '''Make axes of 3D plot have equal scale so that spheres appear as spheres,
+    cubes as cubes, etc..  This is one possible solution to Matplotlib's
+    ax.set_aspect('equal') and ax.axis('equal') not working for 3D.
+
+    Input
+      ax: a matplotlib axis, e.g., as output from plt.gca().
+    '''
+
+    x_limits = ax.get_xlim3d()
+    y_limits = ax.get_ylim3d()
+    z_limits = ax.get_zlim3d()
+
+    x_range = abs(x_limits[1] - x_limits[0])
+    x_middle = np.mean(x_limits)
+    y_range = abs(y_limits[1] - y_limits[0])
+    y_middle = np.mean(y_limits)
+    z_range = abs(z_limits[1] - z_limits[0])
+    z_middle = np.mean(z_limits)
+
+    print (f"x_range: {x_range}")
+    print (f"y_range: {y_range}")
+    print (f"z_range: {z_range}")
+    print (f"x_middle: {x_middle}")
+    print (f"y_middle: {y_middle}")
+    print (f"z_middle: {z_middle}")
+    # The plot bounding box is a sphere in the sense of the infinity
+    # norm, hence I call half the max range the plot radius.
+    plot_radius = 0.5*max([x_range, y_range, z_range])
+
+    ax.set_xlim3d([x_middle - plot_radius, x_middle + plot_radius])
+    ax.set_ylim3d([y_middle - plot_radius, y_middle + plot_radius])
+    ax.set_zlim3d([z_middle - plot_radius, z_middle + plot_radius])
+
+
+def calculate_correction(VECT):
+    current = VECT[0]
+    centre = (max(VECT) + min(VECT))/2.
+    correction = -(current-centre)
+    print (f"MAX= {max(VECT)}, MIN= {min(VECT)}")
+    print (f"current {current}")
+    print (f"centre {centre}")
+    print (f"CORRECTION: {correction}")
+
+#if __name__ == '__main__':
+### START SCRIPT HERE #########################################################
+#AEROTECH_EPICS_RECORD = "X06MX-ES-DF1"
+AEROTECH_EPICS_RECORD = "X06SA-ES-DF1"
+
+print ("This script rotates the Aerotech OMEGA axis from 0-360deg,")
+print ("and records the calibration tool position during this motion.")
+print ("Make sure:")
+print ("* OMEGA is ready to turn freely.")
+print ("* Calibration Tool is ready, in contact and feedback is active.")
+key=input ("OK to continue? (y/n) ")
+if (key != "y"):
+    print ('Stopping script.')
+    exit(code=None)
+
+
+print ("Setting up ROS")
+#connect to ROS topics for OMEGA and DMS values:
+rospy.init_node('DMS_Recorder', anonymous=True)
+subsOMEGA=rospy.Subscriber("/LJUE9_JointState", JointState, callback_LJUE9_JointState)
+subsDMS  =rospy.Subscriber("/readbackCAL", JointState, callback_readbackCAL_JointState)
+print ("Starting data collection...")
+
+smargopolo_server = "http://smargopolo:3000"
+epics.caput("X06MX-ES-DF1:GMX-SETV",2)
+epics.caput("X06MX-ES-DF1:GMZ-SETV",2)
+epics.caput("X06MX-ES-DF1:GMY-SETV",2)
+        
+for i in range(0,5,1):
+    print("Move GMZ to +2")
+    epics.caput("X06MX-ES-DF1:GMZ-INCP",2)
+    time.sleep(2)
+    startPoint = getCurrentPoint()
+    print("Move GMY to +2")
+    epics.caput("X06MX-ES-DF1:GMY-INCP",2)
+    time.sleep(2)
+    secondPoint = getCurrentPoint()
+    print("Move GMZ to -2")
+    epics.caput("X06MX-ES-DF1:GMZ-INCP",-2)
+    time.sleep(2)
+    print("Move GMY to -2")
+    epics.caput("X06MX-ES-DF1:GMY-INCP",-2)
+    time.sleep(2)
+    print("Move GMX to +1")
+    epics.caput("X06MX-ES-DF1:GMX-INCP",1)
+    time.sleep(2)
+
+
+
+#stop ROS to stop measuring.
+rospy.signal_shutdown('finished measuring')
+################################################################################
+
+fig = plt.figure()
+ax=fig.add_subplot(111, projection='3d')
+ax.plot(DMS_Z,DMS_X,DMS_Y)
+ax.set_xlabel("DMS_Z")
+ax.set_ylabel("DMS_X")
+ax.set_zlabel("DMS_Y")
+ax.plot(DMS_Z[0:1],DMS_X[0],DMS_Y[0], 'rx')
+ax.plot([startPoint[2]],[startPoint[0]],[startPoint[1]],'rx')
+ax.plot([secondPoint[2]],  [secondPoint[0]],  [secondPoint[1]],  'ro')
+set_axes_equal(ax)
+fig.show()
+
+fig2 = plt.figure()
+ax2=fig2.add_subplot(111)
+ax2.plot(OMEGA, DMS_X, label='DMS_X')
+ax2.plot(OMEGA, DMS_Y, label='DMS_Y')
+ax2.plot(OMEGA, DMS_Z, label='DMS_Z')
+ax2.set_xlabel("OMEGA")
+ax2.legend()
+fig2.show()
+
+#Omega Offset in degree
+print ('Omega Offset in degree')
+YOffset= secondPoint[1]-startPoint[1]
+ZOffset= secondPoint[2]-startPoint[2]
+OmegaOffset = (math.tan(ZOffset/YOffset))*(180/math.pi)
+print (f"Omega Offset: {OmegaOffset}")
+##########################################i######################################
+# Save Data to CSV
+
+rows = zip(OMEGA, DMS_X, DMS_Y, DMS_Z, DMS_Seq, DMS_Secs, DMS_Nsecs)
+with open(current_day + current_time +'measure_repeatability_aerotech_2.csv', 'w', newline='') as file:
+    writer = csv.writer(file)
+    writer.writerow(["OMEGA", "DMS_X", "DMS_Y", "DMS_Z", "DMS_Seq", "DMS_Secs", "DMS_Nsecs"])
+    for row in rows:
+        writer.writerow(row)
+
+
+print("Move GMX to +1")
+epics.caput("X06MX-ES-DF1:GMX-INCP",-5)
+time.sleep(2)
+
+input("done.")
+
+

python_algorithms/Dominik/1_check_DMS_Alignment/1_1_AerotechMotion/old/210827_measure_DMS_Tool_linearity (copy).py

@@ -0,0 +1,223 @@
+#!/usr/bin/env python
+
+# Dieses Script verfaehrt den Aerotech translativ mit GMX,GMY und GMZ.
+# Der Ausgangswert des Scripts beschreibt den Winkelversatz zwischen Aerotech und
+# dem Kalibrier Tool.
+
+# Dominik Buntschu, 2.8.2021
+
+import rospy
+from sensor_msgs.msg import JointState
+import requests
+import time
+from datetime import date
+from datetime import datetime
+import matplotlib.pyplot as plt
+from mpl_toolkits.mplot3d import Axes3D
+import numpy as np
+import epics
+import csv
+import math
+
+DMS_X=[];
+DMS_Y=[];
+DMS_Z=[];
+DMS_Seq=[];
+DMS_Secs=[];
+DMS_Nsecs=[];
+OMEGA_inst=0
+OMEGA=[];
+
+# Aktueller Tag
+today = date.today()
+current_day = today.strftime("%Y_%m_%d_")
+#print(current_day)
+
+# Aktuelle Zeit
+now = datetime.now()
+current_time = now.strftime("%H:%M:%S_")
+#print(current_time)
+#print(current_day+current_time)
+
+# Auslesen der Omega Achse
+def callback_LJUE9_JointState(data):
+
+    global OMEGA_inst
+    OMEGA_inst = data.position[0]
+
+# Auselsen des Kalibriertools
+# Auslesen Hinzufuegen eines Zeitstempels
+# Einbinden des Aerotech Omegawinkels (OMEGA_inst)    
+def callback_readbackCAL_JointState(data):
+
+    global OMEGA,DMS_X,DMS_Y,DMS_Z,DMS_Seq,DMS_Secs,DMS_Nsecs
+
+    DMS_X.append(data.position[0])
+    DMS_Y.append(data.position[1])
+    DMS_Z.append(data.position[2])
+    DMS_Secs.append(data.header.stamp.secs)
+    DMS_Nsecs.append(data.header.stamp.nsecs)
+    DMS_Seq.append(data.header.seq)
+    OMEGA.append(OMEGA_inst)
+
+# Funktion um aktueller Werte des Kaliobriertools zu speichern
+# Wird fuer Makierung im 3D Plot verwendet    
+def getCurrentPoint():
+    return [DMS_X[-1],DMS_Y[-1],DMS_Z[-1]]
+
+# Stellt Seitenlaengen des 3D Plot in Relation   
+def set_axes_equal(ax):
+
+    x_limits = ax.get_xlim3d()
+    y_limits = ax.get_ylim3d()
+    z_limits = ax.get_zlim3d()
+
+    x_range = abs(x_limits[1] - x_limits[0])
+    x_middle = np.mean(x_limits)
+    y_range = abs(y_limits[1] - y_limits[0])
+    y_middle = np.mean(y_limits)
+    z_range = abs(z_limits[1] - z_limits[0])
+    z_middle = np.mean(z_limits)
+
+    print (f"x_range: {x_range}")
+    print (f"y_range: {y_range}")
+    print (f"z_range: {z_range}")
+    print (f"x_middle: {x_middle}")
+    print (f"y_middle: {y_middle}")
+    print (f"z_middle: {z_middle}")
+
+    plot_radius = 0.5*max([x_range, y_range, z_range])
+
+    ax.set_xlim3d([x_middle - plot_radius, x_middle + plot_radius])
+    ax.set_ylim3d([y_middle - plot_radius, y_middle + plot_radius])
+    ax.set_zlim3d([z_middle - plot_radius, z_middle + plot_radius])
+
+
+def calculate_correction(VECT):
+    current = VECT[0]
+    centre = (max(VECT) + min(VECT))/2.
+    correction = -(current-centre)
+    print (f"MAX= {max(VECT)}, MIN= {min(VECT)}")
+    print (f"current {current}")
+    print (f"centre {centre}")
+    print (f"CORRECTION: {correction}")
+
+### START SCRIPT HERE #########################################################
+
+#AEROTECH_EPICS_RECORD = "X06MX-ES-DF1"
+AEROTECH_EPICS_RECORD = "X06SA-ES-DF1"
+
+print ("This script moves Aerotech GMX GMY and GMZ by aprox. 2mm")
+print ("and records the calibration tool position during this motion.")
+print ("***** Calibration Tool is ready, in contact and feedback is active *******")
+
+key=input ("OK to continue? (y/n) ")
+if (key != "y"):
+    print ('Stopping script.')
+    exit(code=None)
+
+# starten der Datenaufzeichnung
+rospy.init_node('DMS_Recorder', anonymous=True)
+subsOMEGA=rospy.Subscriber("/LJUE9_JointState", JointState, callback_LJUE9_JointState)
+subsDMS  =rospy.Subscriber("/readbackCAL", JointState, callback_readbackCAL_JointState)
+print ("Starting data collection...")
+
+# smargopolo_server = "http://smargopolo:3000"
+
+# Setzt die Geschwidigkeit der Aerotech Achsen
+epics.caput("X06MX-ES-DF1:GMX-SETV",2)
+epics.caput("X06MX-ES-DF1:GMZ-SETV",2)
+epics.caput("X06MX-ES-DF1:GMY-SETV",2)
+
+# Macht X Iterationen eines Rechteckes mit jeweils einem Versatz
+# Veranschaulicht den Winkelversatz zwischen Aerotech und Kalibriertool        
+for i in range(0,5,1):
+    print("Move GMZ to +2")
+    epics.caput("X06MX-ES-DF1:GMZ-INCP",2)
+    time.sleep(2)
+    Point1 = getCurrentPoint()
+    print("Move GMY to +2")
+    epics.caput("X06MX-ES-DF1:GMY-INCP",2)
+    time.sleep(2)
+    Point2 = getCurrentPoint()
+    print("Move GMZ to -2")
+    epics.caput("X06MX-ES-DF1:GMZ-INCP",-2)
+    time.sleep(2)
+    Point3 = getCurrentPoint()
+    print("Move GMY to -2")
+    epics.caput("X06MX-ES-DF1:GMY-INCP",-2)
+    time.sleep(2)
+    print("Move GMX to +1")
+    epics.caput("X06MX-ES-DF1:GMX-INCP",1)
+    time.sleep(2)
+
+
+
+# beenden der Datenaufzeichnung
+rospy.signal_shutdown('finished measuring')
+################################################################################
+
+# 3D Plot der Bewegung mit den Werten des Kalibirertools
+# Start der Bewegung = x Punkt
+fig = plt.figure()
+ax=fig.add_subplot(111, projection='3d')
+ax.plot(DMS_Z,DMS_X,DMS_Y)
+ax.set_xlabel("DMS_Z")
+ax.set_ylabel("DMS_X")
+ax.set_zlabel("DMS_Y")
+ax.set_title("3D Plot: Measure Linearity to DMS Tool",fontsize=14,fontweight="bold")
+ax.plot(DMS_Z[0:1],DMS_X[0],DMS_Y[0],label='StartPoint',marker=(5,0))
+ax.plot([Point1[2]],[Point1[0]],[Point1[1]],label='Point1',marker=(5,1))
+ax.plot([Point2[2]],  [Point2[0]],  [Point2[1]],label='Point2',  marker=(5,2))
+ax.plot([Point3[2]],  [Point3[0]],  [Point3[1]],label='Point3',  marker=(5,3))
+ax.legend()
+set_axes_equal(ax)
+fig.show()
+
+# 2D Plot der Bewegung mit den Werten des Kaibriertools
+#fig2 = plt.figure()
+#ax2=fig2.add_subplot(111)
+#ax2.plot(OMEGA, DMS_X, label='DMS_X')
+#ax2.plot(OMEGA, DMS_Y, label='DMS_Y')
+#ax2.plot(OMEGA, DMS_Z, label='DMS_Z')
+#ax2.set_xlabel("OMEGA")
+#ax2.legend()
+#fig2.show()
+
+#Berrechnung des Versatzes zwischen Aerotech und Kalibirertool-> Rotation um GMX
+YOffset= Point2[1]-Point1[1]
+ZOffset= Point2[2]-Point1[2]
+GMXOffset = (math.tan(ZOffset/YOffset))*(180/math.pi)
+print (f"Winkelversatz um GMX: {GMXOffset}")
+
+#Berrechnung des Versatzes zwischen Aerotech und Kalibirertool-> Rotation um GMZ
+YOffset= Point2[1]-Point1[1]
+XOffset= Point2[0]-Point1[0]
+GMZOffset = (math.tan(XOffset/YOffset))*(180/math.pi)
+print (f"Winkelversatz um GMZ: {GMZOffset}")
+
+#Berrechnung des Versatzes zwischen Aerotech und Kalibirertool-> Rotation um GMY
+ZOffset= Point2[2]-Point3[2]
+XOffset= Point2[0]-Point3[0]
+GMYOffset = (math.tan(XOffset/ZOffset))*(180/math.pi)
+print (f"Winkelversatz um GMY: {GMYOffset}")
+
+##########################################i######################################
+
+# Speichern der Daten in ein CSV.
+# Bennenung des Files mit Datum und Zeitstempel
+rows = zip(OMEGA, DMS_X, DMS_Y, DMS_Z, DMS_Seq, DMS_Secs, DMS_Nsecs)
+with open(current_day + current_time +'measure_DMS_Tool_linearity.csv', 'w', newline='') as file:
+    writer = csv.writer(file)
+    writer.writerow(["OMEGA", "DMS_X", "DMS_Y", "DMS_Z", "DMS_Seq", "DMS_Secs", "DMS_Nsecs"])
+    for row in rows:
+        writer.writerow(row)
+
+# Bewegung der GMX Achse zurueck auf den Startwert
+print("Move GMX back to Start Position")
+epics.caput("X06MX-ES-DF1:GMX-INCP",-5)
+time.sleep(2)
+
+input("done.")
+
+

python_algorithms/Dominik/1_check_DMS_Alignment/1_2_SmargonMotion/measure_Omega_Offset_to_Smargon.py

@@ -0,0 +1,207 @@
+#!/usr/bin/env pythoni
+
+# Dieses Script verfaehrt den Smargon translativ im Smargon Koordintane System OX,OY,OZ.
+# Der Ausgangswert des Scripts beschreibt den Winkelversatz zwischen der Aerotech Omega Achse und
+# dem Smargon.
+# Ziel ist es einen moeglichst kleinen Winkelfehler zu haben. Smargon muss sich rechtwinklig zum Aeroetch 
+# bewegen
+
+# Dominik Buntschu, 2.8.2021
+
+import rospy
+from sensor_msgs.msg import JointState
+import matplotlib.pyplot as plt
+import requests
+import time
+from datetime import date
+from datetime import datetime
+from mpl_toolkits.mplot3d import Axes3D
+import numpy as np
+import epics
+import csv
+import math
+
+DMS_X=[];
+DMS_Y=[];
+DMS_Z=[];
+DMS_Seq=[];
+DMS_Secs=[];
+DMS_Nsecs=[];
+OMEGA_inst=0
+OMEGA=[];
+
+# Aktueller Tag
+today = date.today()
+current_day = today.strftime("%Y_%m_%d_")
+#print(current_day)
+
+# Aktuelle Zeit
+now = datetime.now()
+current_time = now.strftime("%H:%M:%S_")
+#print(current_time)
+#print(current_day+current_time)
+
+# Auslesen der Omega Achse
+def callback_LJUE9_JointState(data):
+
+    global OMEGA_inst
+    OMEGA_inst = data.position[0]
+
+# Auselsen des Kalibriertools
+# Auslesen Hinzufuegen eines Zeitstempels
+# Einbinden des Aerotech Omegawinkels (OMEGA_inst) 
+def callback_readbackCAL_JointState(data):
+
+    global OMEGA,DMS_X,DMS_Y,DMS_Z,DMS_Seq,DMS_Secs,DMS_Nsecs
+
+    DMS_X.append(data.position[0])
+    DMS_Y.append(data.position[1])
+    DMS_Z.append(data.position[2])
+    DMS_Secs.append(data.header.stamp.secs)
+    DMS_Nsecs.append(data.header.stamp.nsecs)
+    DMS_Seq.append(data.header.seq)
+    OMEGA.append(OMEGA_inst)
+
+# Funktion um aktueller Werte des Kaliobriertools zu speichern
+# Wird fuer Makierung im 3D Plot verwendet 
+def getCurrentPoint():
+    return [DMS_X[-1],DMS_Y[-1],DMS_Z[-1]]
+
+# Stellt Seitenlaengen des 3D Plot in Relation 
+def set_axes_equal(ax):
+
+    x_limits = ax.get_xlim3d()
+    y_limits = ax.get_ylim3d()
+    z_limits = ax.get_zlim3d()
+
+    x_range = abs(x_limits[1] - x_limits[0])
+    x_middle = np.mean(x_limits)
+    y_range = abs(y_limits[1] - y_limits[0])
+    y_middle = np.mean(y_limits)
+    z_range = abs(z_limits[1] - z_limits[0])
+    z_middle = np.mean(z_limits)
+
+    print (f"x_range: {x_range}")
+    print (f"y_range: {y_range}")
+    print (f"z_range: {z_range}")
+    print (f"x_middle: {x_middle}")
+    print (f"y_middle: {y_middle}")
+    print (f"z_middle: {z_middle}")
+
+    plot_radius = 0.5*max([x_range, y_range, z_range])
+
+    ax.set_xlim3d([x_middle - plot_radius, x_middle + plot_radius])
+    ax.set_ylim3d([y_middle - plot_radius, y_middle + plot_radius])
+    ax.set_zlim3d([z_middle - plot_radius, z_middle + plot_radius])
+
+
+def calculate_correction(VECT):
+    current = VECT[0]
+    centre = (max(VECT) + min(VECT))/2.
+    correction = -(current-centre)
+    print (f"MAX= {max(VECT)}, MIN= {min(VECT)}")
+    print (f"current {current}")
+    print (f"centre {centre}")
+    print (f"CORRECTION: {correction}")
+    
+
+### START SCRIPT HERE #########################################################
+
+#AEROTECH_EPICS_RECORD = "X06MX-ES-DF1"
+AEROTECH_EPICS_RECORD = "X06SA-ES-DF1"
+
+print ("This script moves Smargon in Beamline Coordinates OX OY and OZ by approx. 2mm,")
+print ("and records the calibration tool position during this motion.")
+print ("***** Calibration Tool is ready, in contact and feedback is active *******")
+
+key=input ("OK to continue? (y/n) ")
+if (key != "y"):
+    print ('Stopping script.')
+    exit(code=None)
+
+# starten der Datenaufzeichnung
+rospy.init_node('DMS_Recorder', anonymous=True)
+subsOMEGA=rospy.Subscriber("/LJUE9_JointState", JointState, callback_LJUE9_JointState)
+subsDMS  =rospy.Subscriber("/readbackCAL", JointState, callback_readbackCAL_JointState)
+print ("Starting data collection...")
+
+#Definiert Variabel fuer den Smargon Server
+smargopolo_server = "http://smargopolo:3000"
+ 
+# Macht X Iterationen eines Rechteckes mit jeweils einem Versatz
+# Veranschaulicht den Winkelversatz zwischen Aerotech und Smargon         
+for i in range(0,3,1):
+    print("Move OY to +2")
+    response = requests.put(smargopolo_server+'/targetSCS_rel?OY=2')
+    time.sleep(2)
+    Point1 = getCurrentPoint()
+    print("Move OX to +2")
+    response = requests.put(smargopolo_server+'/targetSCS_rel?OX=2')
+    time.sleep(2)
+    Point2 = getCurrentPoint()
+    print("Move OY to -2")
+    response = requests.put(smargopolo_server+'/targetSCS_rel?OY=-2')
+    time.sleep(2)
+    print("Move OX to -2")
+    response = requests.put(smargopolo_server+'/targetSCS_rel?OX=-2')
+    time.sleep(2)
+    print("Move OZ to +1")
+    response = requests.put(smargopolo_server+'/targetSCS_rel?OZ=1')
+    time.sleep(2)
+
+
+
+# beenden der Datenaufzeichnung
+rospy.signal_shutdown('finished measuring')
+################################################################################
+
+# 3D Plot der Bewegung mit den Werten des Kalibirertools
+# Start der Bewegung = x Punkt
+# 3D Plot der Bewegung mit den Werten des Kalibirertools
+# Start der Bewegung = x Punkt
+fig = plt.figure()
+ax=fig.add_subplot(111, projection='3d')
+ax.plot(DMS_Z,DMS_X,DMS_Y)
+ax.set_xlabel("DMS_Z [mm]")
+ax.set_ylabel("DMS_X [mm]")
+ax.set_zlabel("DMS_Y [mm]")
+ax.set_title("3D Plot: Measure Omega Offset to Smargon",fontsize=14,fontweight="bold")
+ax.plot(DMS_Z[0:1],DMS_X[0],DMS_Y[0],label='StartPoint',marker=(5,0))
+ax.plot([Point1[2]],[Point1[0]],[Point1[1]],label='Point1',marker=(5,1))
+ax.plot([Point2[2]],  [Point2[0]],  [Point2[1]],label='Point2',  marker=(5,2))
+ax.legend()
+set_axes_equal(ax)
+fig.show()
+
+# 2D Plot der Bewegung mit den Werten des Kaibriertools
+#fig2 = plt.figure()
+#ax2=fig2.add_subplot(111)
+#ax2.plot(OMEGA, DMS_X, label='DMS_X')
+#ax2.plot(OMEGA, DMS_Y, label='DMS_Y')
+#ax2.plot(OMEGA, DMS_Z, label='DMS_Z')
+#ax2.set_xlabel("OMEGA")
+#ax2.legend()
+#fig2.show()
+
+#Berrechnung des Winkelversatzes zwischen Aerotech und Smargon
+YOffset= Point2[1]-Point1[1]
+ZOffset= Point2[2]-Point1[2]
+OmegaOffset = (math.tan(ZOffset/YOffset))*(180/math.pi)
+print (f"Omega muss um folgenden Winkelfehler korrigiert werden: {OmegaOffset}")
+
+##########################################i######################################
+
+# Speichern der Daten in ein CSV.
+# Bennenung des Files mit Datum und Zeitstempel
+rows = zip(OMEGA, DMS_X, DMS_Y, DMS_Z, DMS_Seq, DMS_Secs, DMS_Nsecs)
+with open(current_day + current_time +'measure_Omega_Offset_to_Smargon.csv', 'w', newline='') as file:
+    writer = csv.writer(file)
+    writer.writerow(["OMEGA", "DMS_X", "DMS_Y", "DMS_Z", "DMS_Seq", "DMS_Secs", "DMS_Nsecs"])
+    for row in rows:
+        writer.writerow(row)
+
+print("Move OZ back to Start Position")
+response = requests.put(smargopolo_server+'/targetSCS_rel?OZ=-3')
+time.sleep(2)
+
+input("done.")

python_algorithms/Dominik/1_check_DMS_Alignment/1_2_SmargonMotion/old/210804_measure_repeatability_smargon_2.py

@@ -0,0 +1,198 @@
+#!/usr/bin/env pythoni
+
+# This Script creates a lookup table from the measured error CSV
+# Dominik Buntschu, 2.8.2021
+
+import rospy
+from sensor_msgs.msg import JointState
+import matplotlib.pyplot as plt
+import requests
+import time
+from datetime import date
+from datetime import datetime
+import matplotlib.pyplot as plt
+from mpl_toolkits.mplot3d import Axes3D
+import numpy as np
+import epics
+import csv
+import math
+
+DMS_X=[];
+DMS_Y=[];
+DMS_Z=[];
+DMS_Seq=[];
+DMS_Secs=[];
+DMS_Nsecs=[];
+OMEGA_inst=0
+OMEGA=[];
+
+today = date.today()
+current_day = today.strftime("%Y_%m_%d_")
+#print(current_day)
+
+now = datetime.now()
+current_time = now.strftime("%H:%M:%S_")
+#print(current_time)
+#print(current_day+current_time)
+
+def callback_LJUE9_JointState(data):
+
+    global OMEGA_inst
+    OMEGA_inst = data.position[0]
+
+
+def callback_readbackCAL_JointState(data):
+
+    global OMEGA,DMS_X,DMS_Y,DMS_Z,DMS_Seq,DMS_Secs,DMS_Nsecs
+
+    DMS_X.append(data.position[0])
+    DMS_Y.append(data.position[1])
+    DMS_Z.append(data.position[2])
+    DMS_Secs.append(data.header.stamp.secs)
+    DMS_Nsecs.append(data.header.stamp.nsecs)
+    DMS_Seq.append(data.header.seq)
+    OMEGA.append(OMEGA_inst)
+
+def getCurrentPoint():
+    return [DMS_X[-1],DMS_Y[-1],DMS_Z[-1]]
+
+
+def set_axes_equal(ax):
+    '''Make axes of 3D plot have equal scale so that spheres appear as spheres,
+    cubes as cubes, etc..  This is one possible solution to Matplotlib's
+    ax.set_aspect('equal') and ax.axis('equal') not working for 3D.
+
+    Input
+      ax: a matplotlib axis, e.g., as output from plt.gca().
+    '''
+
+    x_limits = ax.get_xlim3d()
+    y_limits = ax.get_ylim3d()
+    z_limits = ax.get_zlim3d()
+
+    x_range = abs(x_limits[1] - x_limits[0])
+    x_middle = np.mean(x_limits)
+    y_range = abs(y_limits[1] - y_limits[0])
+    y_middle = np.mean(y_limits)
+    z_range = abs(z_limits[1] - z_limits[0])
+    z_middle = np.mean(z_limits)
+
+    print (f"x_range: {x_range}")
+    print (f"y_range: {y_range}")
+    print (f"z_range: {z_range}")
+    print (f"x_middle: {x_middle}")
+    print (f"y_middle: {y_middle}")
+    print (f"z_middle: {z_middle}")
+    # The plot bounding box is a sphere in the sense of the infinity
+    # norm, hence I call half the max range the plot radius.
+    plot_radius = 0.5*max([x_range, y_range, z_range])
+
+    ax.set_xlim3d([x_middle - plot_radius, x_middle + plot_radius])
+    ax.set_ylim3d([y_middle - plot_radius, y_middle + plot_radius])
+    ax.set_zlim3d([z_middle - plot_radius, z_middle + plot_radius])
+
+
+def calculate_correction(VECT):
+    current = VECT[0]
+    centre = (max(VECT) + min(VECT))/2.
+    correction = -(current-centre)
+    print (f"MAX= {max(VECT)}, MIN= {min(VECT)}")
+    print (f"current {current}")
+    print (f"centre {centre}")
+    print (f"CORRECTION: {correction}")
+    
+
+
+#if __name__ == '__main__':
+### START SCRIPT HERE #########################################################
+#AEROTECH_EPICS_RECORD = "X06MX-ES-DF1"
+AEROTECH_EPICS_RECORD = "X06SA-ES-DF1"
+
+print ("This script rotates the Aerotech OMEGA axis from 0-360deg,")
+print ("and records the calibration tool position during this motion.")
+print ("Make sure:")
+print ("* OMEGA is ready to turn freely.")
+print ("* Calibration Tool is ready, in contact and feedback is active.")
+key=input ("OK to continue? (y/n) ")
+if (key != "y"):
+    print ('Stopping script.')
+    exit(code=None)
+
+
+print ("Setting up ROS")
+#connect to ROS topics for OMEGA and DMS values:
+rospy.init_node('DMS_Recorder', anonymous=True)
+subsOMEGA=rospy.Subscriber("/LJUE9_JointState", JointState, callback_LJUE9_JointState)
+subsDMS  =rospy.Subscriber("/readbackCAL", JointState, callback_readbackCAL_JointState)
+print ("Starting data collection...")
+
+smargopolo_server = "http://smargopolo:3000"
+        
+for i in range(0,5,1):
+    print("Move BZ to +2")
+    response = requests.put(smargopolo_server+'/nudgeBCS?BZ=2')
+    time.sleep(2)
+    startPoint = getCurrentPoint()
+    print("Move BY to +1")
+    response = requests.put(smargopolo_server+'/nudgeBCS?BY=2')
+    time.sleep(2)
+    secondPoint = getCurrentPoint()
+    print("Move BZ to +1")
+    response = requests.put(smargopolo_server+'/nudgeBCS?BZ=-2')
+    time.sleep(2)
+    print("Move BY to +1")
+    response = requests.put(smargopolo_server+'/nudgeBCS?BY=-2')
+    time.sleep(2)
+    print("Move BX to +1")
+    response = requests.put(smargopolo_server+'/nudgeBCS?BX=1')
+    time.sleep(2)
+
+
+
+#stop ROS to stop measuring.
+rospy.signal_shutdown('finished measuring')
+################################################################################
+
+fig = plt.figure()
+ax=fig.add_subplot(111, projection='3d')
+ax.plot(DMS_Z,DMS_X,DMS_Y)
+ax.set_xlabel("DMS_Z")
+ax.set_ylabel("DMS_X")
+ax.set_zlabel("DMS_Y")
+ax.plot(DMS_Z[0:1],DMS_X[0],DMS_Y[0], 'rx')
+ax.plot([startPoint[2]],[startPoint[0]],[startPoint[1]],'rx')
+ax.plot([secondPoint[2]],  [secondPoint[0]],  [secondPoint[1]],  'ro')
+set_axes_equal(ax)
+fig.show()
+
+fig2 = plt.figure()
+ax2=fig2.add_subplot(111)
+ax2.plot(OMEGA, DMS_X, label='DMS_X')
+ax2.plot(OMEGA, DMS_Y, label='DMS_Y')
+ax2.plot(OMEGA, DMS_Z, label='DMS_Z')
+ax2.set_xlabel("OMEGA")
+ax2.legend()
+fig2.show()
+
+#Omega Offset in degree
+print ('Omega Offset in degree')
+YOffset= secondPoint[1]-startPoint[1]
+ZOffset= secondPoint[2]-startPoint[2]
+OmegaOffset = (math.tan(ZOffset/YOffset))*(180/math.pi)
+print (f"Omega Offset: {OmegaOffset}")
+
+##########################################i######################################
+# Save Data to CSV
+
+rows = zip(OMEGA, DMS_X, DMS_Y, DMS_Z, DMS_Seq, DMS_Secs, DMS_Nsecs)
+with open(current_day + current_time +'measure_repeatability_smargon_2.csv', 'w', newline='') as file:
+    writer = csv.writer(file)
+    writer.writerow(["OMEGA", "DMS_X", "DMS_Y", "DMS_Z", "DMS_Seq", "DMS_Secs", "DMS_Nsecs"])
+    for row in rows:
+        writer.writerow(row)
+
+print("Move BX to +1")
+response = requests.put(smargopolo_server+'/nudgeBCS?BX=-5')
+time.sleep(2)
+
+input("done.")

python_algorithms/Dominik/1_check_DMS_Alignment/1_2_SmargonMotion/old/210827_measure_Omega_Offset_to_Smargon (copy).py

@@ -0,0 +1,203 @@
+#!/usr/bin/env pythoni
+
+# Dieses Script verfaehrt den Smargon translativ im Smargon Koordintane System OX,OY,OZ.
+# Der Ausgangswert des Scripts beschreibt den Winkelversatz zwischen der Aerotech Omega Achse und
+# dem Smargon.
+# Ziel ist es einen moeglichst kleinen Winkelfehler zu haben. Smargon muss sich rechtwinklig zum Aeroetch 
+# bewegen
+
+# Dominik Buntschu, 2.8.2021
+
+import rospy
+from sensor_msgs.msg import JointState
+import matplotlib.pyplot as plt
+import requests
+import time
+from datetime import date
+from datetime import datetime
+from mpl_toolkits.mplot3d import Axes3D
+import numpy as np
+import epics
+import csv
+import math
+
+DMS_X=[];
+DMS_Y=[];
+DMS_Z=[];
+DMS_Seq=[];
+DMS_Secs=[];
+DMS_Nsecs=[];
+OMEGA_inst=0
+OMEGA=[];
+
+# Aktueller Tag
+today = date.today()
+current_day = today.strftime("%Y_%m_%d_")
+#print(current_day)
+
+# Aktuelle Zeit
+now = datetime.now()
+current_time = now.strftime("%H:%M:%S_")
+#print(current_time)
+#print(current_day+current_time)
+
+# Auslesen der Omega Achse
+def callback_LJUE9_JointState(data):
+
+    global OMEGA_inst
+    OMEGA_inst = data.position[0]
+
+# Auselsen des Kalibriertools
+# Auslesen Hinzufuegen eines Zeitstempels
+# Einbinden des Aerotech Omegawinkels (OMEGA_inst) 
+def callback_readbackCAL_JointState(data):
+
+    global OMEGA,DMS_X,DMS_Y,DMS_Z,DMS_Seq,DMS_Secs,DMS_Nsecs
+
+    DMS_X.append(data.position[0])
+    DMS_Y.append(data.position[1])
+    DMS_Z.append(data.position[2])
+    DMS_Secs.append(data.header.stamp.secs)
+    DMS_Nsecs.append(data.header.stamp.nsecs)
+    DMS_Seq.append(data.header.seq)
+    OMEGA.append(OMEGA_inst)
+
+# Funktion um aktueller Werte des Kaliobriertools zu speichern
+# Wird fuer Makierung im 3D Plot verwendet 
+def getCurrentPoint():
+    return [DMS_X[-1],DMS_Y[-1],DMS_Z[-1]]
+
+# Stellt Seitenlaengen des 3D Plot in Relation 
+def set_axes_equal(ax):
+
+    x_limits = ax.get_xlim3d()
+    y_limits = ax.get_ylim3d()
+    z_limits = ax.get_zlim3d()
+
+    x_range = abs(x_limits[1] - x_limits[0])
+    x_middle = np.mean(x_limits)
+    y_range = abs(y_limits[1] - y_limits[0])
+    y_middle = np.mean(y_limits)
+    z_range = abs(z_limits[1] - z_limits[0])
+    z_middle = np.mean(z_limits)
+
+    print (f"x_range: {x_range}")
+    print (f"y_range: {y_range}")
+    print (f"z_range: {z_range}")
+    print (f"x_middle: {x_middle}")
+    print (f"y_middle: {y_middle}")
+    print (f"z_middle: {z_middle}")
+
+    plot_radius = 0.5*max([x_range, y_range, z_range])
+
+    ax.set_xlim3d([x_middle - plot_radius, x_middle + plot_radius])
+    ax.set_ylim3d([y_middle - plot_radius, y_middle + plot_radius])
+    ax.set_zlim3d([z_middle - plot_radius, z_middle + plot_radius])
+
+
+def calculate_correction(VECT):
+    current = VECT[0]
+    centre = (max(VECT) + min(VECT))/2.
+    correction = -(current-centre)
+    print (f"MAX= {max(VECT)}, MIN= {min(VECT)}")
+    print (f"current {current}")
+    print (f"centre {centre}")
+    print (f"CORRECTION: {correction}")
+    
+
+### START SCRIPT HERE #########################################################
+
+#AEROTECH_EPICS_RECORD = "X06MX-ES-DF1"
+AEROTECH_EPICS_RECORD = "X06SA-ES-DF1"
+
+print ("This script moves Smargon in Beamline Coordinates BX BY and BZ by approx. 2mm,")
+print ("and records the calibration tool position during this motion.")
+print ("***** Calibration Tool is ready, in contact and feedback is active *******")
+
+key=input ("OK to continue? (y/n) ")
+if (key != "y"):
+    print ('Stopping script.')
+    exit(code=None)
+
+# starten der Datenaufzeichnung
+rospy.init_node('DMS_Recorder', anonymous=True)
+subsOMEGA=rospy.Subscriber("/LJUE9_JointState", JointState, callback_LJUE9_JointState)
+subsDMS  =rospy.Subscriber("/readbackCAL", JointState, callback_readbackCAL_JointState)
+print ("Starting data collection...")
+
+#Definiert Variabel fuer den Smargon Server
+smargopolo_server = "http://smargopolo:3000"
+ 
+# Macht X Iterationen eines Rechteckes mit jeweils einem Versatz
+# Veranschaulicht den Winkelversatz zwischen Aerotech und Smargon         
+for i in range(0,5,1):
+    print("Move OY to +2")
+    response = requests.put(smargopolo_server+'/targetSCS_rel?OY=2')
+    time.sleep(2)
+    startPoint = getCurrentPoint()
+    print("Move OX to +2")
+    response = requests.put(smargopolo_server+'/targetSCS_rel?OX=2')
+    time.sleep(2)
+    secondPoint = getCurrentPoint()
+    print("Move OY to -2")
+    response = requests.put(smargopolo_server+'/targetSCS_rel?OY=-2')
+    time.sleep(2)
+    print("Move OX to -2")
+    response = requests.put(smargopolo_server+'/targetSCS_rel?OX=-2')
+    time.sleep(2)
+    print("Move OZ to +1")
+    response = requests.put(smargopolo_server+'/targetSCS_rel?OZ=1')
+    time.sleep(2)
+
+
+
+# beenden der Datenaufzeichnung
+rospy.signal_shutdown('finished measuring')
+################################################################################
+
+# 3D Plot der Bewegung mit den Werten des Kalibirertools
+# Start der Bewegung = x Punkt
+fig = plt.figure()
+ax=fig.add_subplot(111, projection='3d')
+ax.plot(DMS_Z,DMS_X,DMS_Y)
+ax.set_xlabel("DMS_Z")
+ax.set_ylabel("DMS_X")
+ax.set_zlabel("DMS_Y")
+ax.plot(DMS_Z[0:1],DMS_X[0],DMS_Y[0], 'bx')
+ax.plot([startPoint[2]],[startPoint[0]],[startPoint[1]],'rx')
+ax.plot([secondPoint[2]],  [secondPoint[0]],  [secondPoint[1]],  'ro')
+set_axes_equal(ax)
+fig.show()
+
+# 2D Plot der Bewegung mit den Werten des Kaibriertools
+#fig2 = plt.figure()
+#ax2=fig2.add_subplot(111)
+#ax2.plot(OMEGA, DMS_X, label='DMS_X')
+#ax2.plot(OMEGA, DMS_Y, label='DMS_Y')
+#ax2.plot(OMEGA, DMS_Z, label='DMS_Z')
+#ax2.set_xlabel("OMEGA")
+#ax2.legend()
+#fig2.show()
+
+#Berrechnung des Winkelversatzes zwischen Aerotech und Smargon
+YOffset= secondPoint[1]-startPoint[1]
+ZOffset= secondPoint[2]-startPoint[2]
+OmegaOffset = (math.tan(ZOffset/YOffset))*(180/math.pi)
+print (f"Omega Offset needs to be corrected by: {OmegaOffset}")
+
+##########################################i######################################
+
+# Speichern der Daten in ein CSV.
+# Bennenung des Files mit Datum und Zeitstempel
+rows = zip(OMEGA, DMS_X, DMS_Y, DMS_Z, DMS_Seq, DMS_Secs, DMS_Nsecs)
+with open(current_day + current_time +'measure_Omega_Offset_to_Smargon.csv', 'w', newline='') as file:
+    writer = csv.writer(file)
+    writer.writerow(["OMEGA", "DMS_X", "DMS_Y", "DMS_Z", "DMS_Seq", "DMS_Secs", "DMS_Nsecs"])
+    for row in rows:
+        writer.writerow(row)
+
+print("Move OZ back to Start Position")
+response = requests.put(smargopolo_server+'/targetSCS_rel?OZ=-5')
+time.sleep(2)
+
+input("done.")