RUNNING VERSION STAND END OF BEAMTIME AT X06SA

python_algorithms/Calibration_Routines/create_LUT_omega.py

@@ -11,9 +11,9 @@ import matplotlib.pyplot as plt
 # Read in CSV File into DataFrame rawdf:
 rawdf = read_csv('measure_omega_rot_OUTPUT.csv')
 rawdf['time']=rawdf['DMS_Secs']+rawdf['DMS_Nsecs']*1e-9 #Joins s & ns columns to one float
-rawdf['X']=(rawdf['DMS_X']+0.0015)  #Set X Offset and scale
-rawdf['Y']=(rawdf['DMS_Y']+0.0028)   #Set Y Offset and scale
-rawdf['Z']=(rawdf['DMS_Z']-0.0044)   #Set Z Offset and scale
+rawdf['X']=(rawdf['DMS_X']+0.00)  #Set X Offset and scale
+rawdf['Y']=(rawdf['DMS_Y']+0.0005)   #Set Y Offset and scale
+rawdf['Z']=(rawdf['DMS_Z']-0.00)   #Set Z Offset and scale
 
 # Select Data Window and save to new DataFrame df:
 #df = rawdf[615:2219]

python_algorithms/Dominik/1_check_DMS_Alignment/210804_measure_repeatability_aerotech_2.py

@@ -0,0 +1,181 @@
+#!/usr/bin/env python
+
+# This Script creates a lookup table from the measured error CSV
+# Wayne Glettig, 16.7.2021
+
+import rospy
+from sensor_msgs.msg import JointState
+import matplotlib.pyplot as plt
+import requests
+import time
+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=[];
+
+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 SHY to +2")
+    epics.caput("X06SA-ES-DF1:GMZ-INCP",2)
+    time.sleep(2)
+    startPoint = getCurrentPoint()
+    print("Move SHX to +2")
+    epics.caput("X06SA-ES-DF1:GMY-INCP",2)
+    time.sleep(2)
+    secondPoint = getCurrentPoint()
+    print("Move SHY to -2")
+    epics.caput("X06SA-ES-DF1:GMZ-INCP",-2)
+    time.sleep(2)
+    print("Move SHX to -2")
+    epics.caput("X06SA-ES-DF1:GMY-INCP",-2)
+    time.sleep(2)
+    print("Move SHZ to +1")
+    epics.caput("X06SA-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('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)
+
+
+input("done.")

python_algorithms/Dominik/1_check_DMS_Alignment/210804_measure_repeatability_smargon_2.py

@@ -0,0 +1,184 @@
+#!/usr/bin/env pythoni
+
+# This Script creates a lookup table from the measured error CSV
+# Wayne Glettig, 16.7.2021
+
+import rospy
+from sensor_msgs.msg import JointState
+import matplotlib.pyplot as plt
+import requests
+import time
+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=[];
+
+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 SHY to +2")
+    response = requests.put(smargopolo_server+'/nudgeBCS?BZ=2')
+    time.sleep(2)
+    startPoint = getCurrentPoint()
+    print("Move SHX to +1")
+    response = requests.put(smargopolo_server+'/nudgeBCS?BY=2')
+    time.sleep(2)
+    secondPoint = getCurrentPoint()
+    print("Move SHY to +1")
+    response = requests.put(smargopolo_server+'/nudgeBCS?BZ=-2')
+    time.sleep(2)
+    print("Move SHX to +1")
+    response = requests.put(smargopolo_server+'/nudgeBCS?BY=-2')
+    time.sleep(2)
+    print("Move SHZ 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('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)
+
+
+input("done.")

python_algorithms/Dominik/210804_measure_repeatability_aerotech_2.py

@@ -0,0 +1,181 @@
+#!/usr/bin/env python
+
+# This Script creates a lookup table from the measured error CSV
+# Wayne Glettig, 16.7.2021
+
+import rospy
+from sensor_msgs.msg import JointState
+import matplotlib.pyplot as plt
+import requests
+import time
+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=[];
+
+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 SHY to +2")
+    epics.caput("X06SA-ES-DF1:GMZ-INCP",2)
+    time.sleep(2)
+    startPoint = getCurrentPoint()
+    print("Move SHX to +2")
+    epics.caput("X06SA-ES-DF1:GMY-INCP",2)
+    time.sleep(2)
+    secondPoint = getCurrentPoint()
+    print("Move SHY to -2")
+    epics.caput("X06SA-ES-DF1:GMZ-INCP",-2)
+    time.sleep(2)
+    print("Move SHX to -2")
+    epics.caput("X06SA-ES-DF1:GMY-INCP",-2)
+    time.sleep(2)
+    print("Move SHZ to +1")
+    epics.caput("X06SA-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('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)
+
+
+input("done.")

python_algorithms/Dominik/210804_measure_repeatability_smargon_2.py

@@ -0,0 +1,184 @@
+#!/usr/bin/env pythoni
+
+# This Script creates a lookup table from the measured error CSV
+# Wayne Glettig, 16.7.2021
+
+import rospy
+from sensor_msgs.msg import JointState
+import matplotlib.pyplot as plt
+import requests
+import time
+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=[];
+
+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 SHY to +2")
+    response = requests.put(smargopolo_server+'/nudgeBCS?BZ=2')
+    time.sleep(2)
+    startPoint = getCurrentPoint()
+    print("Move SHX to +1")
+    response = requests.put(smargopolo_server+'/nudgeBCS?BY=2')
+    time.sleep(2)
+    secondPoint = getCurrentPoint()
+    print("Move SHY to +1")
+    response = requests.put(smargopolo_server+'/nudgeBCS?BZ=-2')
+    time.sleep(2)
+    print("Move SHX to +1")
+    response = requests.put(smargopolo_server+'/nudgeBCS?BY=-2')
+    time.sleep(2)
+    print("Move SHZ 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('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)
+
+
+input("done.")

python_algorithms/Dominik/2_1_get_Calibrationpin_Geometry_SHX&SHY/measure_phi_rot.py

@@ -0,0 +1,166 @@
+#!/usr/bin/env python
+
+# This Script creates a lookup table from the measured error CSV
+# Wayne Glettig, 16.7.2021
+
+import rospy
+from sensor_msgs.msg import JointState
+import matplotlib.pyplot as plt
+import requests
+import time
+import matplotlib.pyplot as plt
+from mpl_toolkits.mplot3d import Axes3D
+import numpy as np
+import csv
+
+
+DMS_X=[];
+DMS_Y=[];
+DMS_Z=[];
+DMS_Seq=[];
+DMS_Secs=[];
+DMS_Nsecs=[];
+OMEGA =0;
+
+def callback_LJUE9_JointState(data):
+
+    global OMEGA
+    OMEGA = data.position[0]
+
+
+def callback_readbackCAL_JointState(data):
+
+    global 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)
+    
+
+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_SHX(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 value for SHX: 	{correction}")
+    
+def calculate_correction_SHY(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 value for SHY:	-{correction}")
+    
+        
+#if __name__ == '__main__':
+smargopolo_server = "http://smargopolo:3000"
+
+response = requests.put(smargopolo_server+"/targetSCS?PHI=0")
+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)
+time.sleep(1)
+
+print ("Moving phi to -180deg")
+response = requests.put(smargopolo_server+"/targetSCS?PHI=-90")
+time.sleep(5)
+response = requests.put(smargopolo_server+"/targetSCS?PHI=-180")
+time.sleep(5)
+print ("moving to phi=180deg")
+response = requests.put(smargopolo_server+"/targetSCS?PHI=-90")
+time.sleep(5)
+response = requests.put(smargopolo_server+"/targetSCS?PHI=0")
+time.sleep(5)
+response = requests.put(smargopolo_server+"/targetSCS?PHI=90")
+time.sleep(5)
+response = requests.put(smargopolo_server+"/targetSCS?PHI=180")
+time.sleep(5)
+
+print ("moving to phi=0deg")
+response = requests.put(smargopolo_server+"/targetSCS?PHI=0")
+time.sleep(8)
+
+#stop ROS to stop measuring.
+rospy.signal_shutdown('finished measuring')
+print ("Stopped collecting data.")
+
+################################################################################
+
+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')
+set_axes_equal(ax)
+
+fig.show()
+
+fig2 = plt.figure()
+ax2=fig2.add_subplot(111)
+ax2.plot(DMS_X, label='DMS_X')
+ax2.plot(DMS_Y, label='DMS_Y')
+ax2.plot(DMS_Z, label='DMS_Z')
+ax2.legend()
+fig2.show()
+
+################################################################################
+
+calculate_correction_SHX(DMS_Y)
+calculate_correction_SHY(DMS_Z)
+
+
+################################################################################
+# Save Data to CSV
+
+rows = zip(DMS_X, DMS_Y, DMS_Z, DMS_Seq, DMS_Secs, DMS_Nsecs)
+with open('measure_Phi_rot.csv', 'w', newline='') as file:
+    writer = csv.writer(file)
+    writer.writerow(["DMS_X", "DMS_Y", "DMS_Z", "DMS_Seq", "DMS_Secs", "DMS_Nsecs"])
+    for row in rows:
+        writer.writerow(row)

python_algorithms/Dominik/2_2_get_CalibrationPin_Geometry_SHZ/measure_chi_rot.py

@@ -0,0 +1,191 @@
+#!/usr/bin/env python
+
+# This Script creates a lookup table from the measured error CSV
+# Wayne Glettig, 16.7.2021
+
+import rospy
+from sensor_msgs.msg import JointState
+import matplotlib.pyplot as plt
+import requests
+import time
+import matplotlib.pyplot as plt
+from mpl_toolkits.mplot3d import Axes3D
+import numpy as np
+import csv
+
+DMS_X=[];
+DMS_Y=[];
+DMS_Z=[];
+DMS_Seq=[];
+DMS_Secs=[];
+DMS_Nsecs=[];
+SCS_CHI=[];
+#SCS_Seq=[];
+#SCS_Secs=[];
+#SCS_Nsecs=[];
+OMEGA =0
+CHI =0
+
+def callback_LJUE9_JointState(data):
+    global OMEGA
+    OMEGA = data.position[0]
+
+def callback_readbackCAL_JointState(data):
+    global DMS_X,DMS_Y,DMS_Z,DMS_Seq,DMS_Secs,DMS_Nsecs,CHI
+    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)
+    
+    SCS_CHI.append(CHI)
+
+def callback_readbackSCS_JointState(data):
+    global CHI
+    CHI = data.position[4]
+
+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 value for __: 	{correction}")
+
+#######################################################################################
+#if __name__ == '__main__':
+smargopolo_server = "http://smargopolo:3000"
+
+response = requests.put(smargopolo_server+"/targetSCS?CHI=0")
+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)
+subsSCS  =rospy.Subscriber("/readbackSCS", JointState, callback_readbackSCS_JointState)
+time.sleep(1)
+
+startPoint = getCurrentPoint()
+print ("Moving chi to 90deg")
+response = requests.put(smargopolo_server+"/targetSCS?CHI=30")
+time.sleep(5)
+response = requests.put(smargopolo_server+"/targetSCS?CHI=60")
+time.sleep(5)
+response = requests.put(smargopolo_server+"/targetSCS?CHI=90")
+time.sleep(5)
+
+endPoint = getCurrentPoint()
+print ("Moving chi to 0deg")
+response = requests.put(smargopolo_server+"/targetSCS?CHI=60")
+time.sleep(5)
+response = requests.put(smargopolo_server+"/targetSCS?CHI=30")
+time.sleep(5)
+response = requests.put(smargopolo_server+"/targetSCS?CHI=0")
+time.sleep(7)
+
+#response = requests.put(smargopolo_server+"/targetSCS?CHI=0")
+#time.sleep(8)
+
+#stop ROS to stop measuring.
+rospy.signal_shutdown('finished measuring')
+print ("Stopped collecting data.")
+################################################################################
+
+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([endPoint[2]],  [endPoint[0]],  [endPoint[1]],  'ro')
+set_axes_equal(ax)
+
+fig.show()
+
+fig2 = plt.figure()
+ax2=fig2.add_subplot(111)
+ax2.plot(DMS_X, label='DMS_X')
+ax2.plot(DMS_Y, label='DMS_Y')
+ax2.plot(DMS_Z, label='DMS_Z')
+ax2.legend()
+fig2.show()
+
+################################################################################
+#calculate_correction(DMS_X)
+#calculate_correction(DMS_Y)
+#calculate_correction(DMS_Z)
+
+################################################################################
+#Find Centre point of CHI rotation in DMS_X DMS_Y plane (2D projection)
+
+P0 = [startPoint[0], startPoint[1]]
+P1 = [endPoint[0],   endPoint[1]]
+
+v01half   = [ (P1[0]-P0[0]) / 2. , (P1[1]-P0[1]) / 2. ]
+v01half90 = [ -v01half[1]  , v01half[0] ]
+v0c = [v01half[0]+v01half90[0], v01half[1]+v01half90[1]]
+
+PC = [P0[0]+v0c[0], P0[1]+v0c[1]]
+
+ax.plot([startPoint[2]], [PC[0]], [PC[1]], 'g+')
+fig.show()
+
+print (f"Centre Point of Quarter Circle: DMS_X: {PC[0]} DMS_Y: {PC[1]}")
+
+print (f"Correction from current postion: in DMS_X: {v0c[0]} in DMS_Y: {v0c[1]}")
+print (f"CORRECTION value for SHZ: 	{v0c[0]}")
+
+################################################################################
+# Save Data to CSV
+
+rows = zip(SCS_CHI, DMS_X, DMS_Y, DMS_Z, DMS_Seq, DMS_Secs, DMS_Nsecs)
+with open('measure_chi_rot_OUTPUT.csv', 'w', newline='') as file:
+    writer = csv.writer(file)
+    writer.writerow(["SCS_CHI", "DMS_X", "DMS_Y", "DMS_Z", "DMS_Seq", "DMS_Secs", "DMS_Nsecs"])
+    for row in rows:
+        writer.writerow(row)
+
+################################################################################
+

python_algorithms/Dominik/3_get_Smargon_centeredToOmega_OX&OY/measure_omega_rot.py

@@ -0,0 +1,180 @@
+#!/usr/bin/env python
+
+# This Script creates a lookup table from the measured error CSV
+# Wayne Glettig, 16.7.2021
+
+import rospy
+from sensor_msgs.msg import JointState
+import matplotlib.pyplot as plt
+import requests
+import time
+import matplotlib.pyplot as plt
+from mpl_toolkits.mplot3d import Axes3D
+import numpy as np
+import epics
+import csv
+
+DMS_X=[];
+DMS_Y=[];
+DMS_Z=[];
+DMS_Seq=[];
+DMS_Secs=[];
+DMS_Nsecs=[];
+OMEGA_inst=0
+OMEGA=[];
+
+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 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_OX(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 value for OX: 	{correction}")
+    
+def calculate_correction_OY(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 value for OY: 	{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)
+
+#make sure OMEGA is at 0deg
+print ("Moving OMEGA to 0deg")
+movebackSpeed = 40 # deg/s
+currentOMEGA = epics.caget(AEROTECH_EPICS_RECORD + ":OMEGA-GETP")
+epics.caput(AEROTECH_EPICS_RECORD + ":OMEGA-SETV", movebackSpeed)
+epics.caput(AEROTECH_EPICS_RECORD + ":OMEGA-SETP", 0)
+time.sleep(currentOMEGA/movebackSpeed + 2)
+
+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...")
+
+
+print ("Moving OMEGA to 360deg ")
+dataCollectionSpeed = 5 #deg/s
+print (f"Data collection will take approx {360/dataCollectionSpeed+5} [s]")
+epics.caput(AEROTECH_EPICS_RECORD + ":OMEGA-SETV",dataCollectionSpeed)
+epics.caput(AEROTECH_EPICS_RECORD + ":OMEGA-SETP",360)
+time.sleep(360/dataCollectionSpeed + 5)
+
+
+#stop ROS to stop measuring.
+rospy.signal_shutdown('finished measuring')
+epics.caput(AEROTECH_EPICS_RECORD + ":OMEGA-SETV", movebackSpeed)
+epics.caput(AEROTECH_EPICS_RECORD + ":OMEGA-SETP",0)
+print ("Stopped collecting data.")
+################################################################################
+
+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')
+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()
+
+################################################################################
+
+calculate_correction_OX(DMS_Y)
+calculate_correction_OY(DMS_Z)
+
+################################################################################
+# Save Data to CSV
+
+rows = zip(OMEGA, DMS_X, DMS_Y, DMS_Z, DMS_Seq, DMS_Secs, DMS_Nsecs)
+with open('measure_omega_rot_OUTPUT.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)
+
+
+input("done.")

python_algorithms/Dominik/old/2021_08_03_getToolError.csv

@@ -0,0 +1,2 @@
+OMEGA,Radius,AI0,AI1,Sequenz1,Sequenz2,Sekunden1,Sekunden1,NaoSek1,NanoSek2,DMS1,DMS2,DMS3,CHI,PHI
+359.9558410644531,4.995263576507568,4.878664970397949,0.03724929690361023,11175596,"[9309989, 9309990, 9309991, 9309992, 9309993, 9309994]",1627972405,"[1627972405, 1627972405, 1627972405, 1627972405, 1627972405, 1627972405]",440182566,"[421151158, 427159517, 435216056, 442293471, 447063496, 453410680]","[0.037002233, 0.03695152, 0.036472274, 0.036271394000000005, 0.036139387, 0.036115252]","[-0.021702699000000002, -0.021391857, -0.020976190000000002, -0.020631673, -0.020461014, -0.020461594]","[-0.193922072, -0.194173316, -0.194375181, -0.194658395, -0.19493586400000001, -0.195165256]",73.2072906860778,9.970000000000001e-05

python_algorithms/Dominik/old/210701_Tool_Error_SHX_SHY_New.py

@@ -0,0 +1,330 @@
+#!/usr/bin/env python3
+# -*- coding: utf-8 -*-
+#########EPICS COMMANDS
+
+#read speed Omega Achse -> epics.caget("X06MX-ES-DF1:OMEGA-SETV")
+#write speed Omega Achse -> epics.caput("X06MX-ES-DF1:OMEGA-SETV",20)
+#
+#write relative Omega Achse -> epics.caput("X06MX-ES-DF1:OMEGA-INCP",90)
+#write absolute Omega Achse -> epics.caput("X06MX-ES-DF1:OMEGA-SETP",20)
+#
+#read ReadBAck Omega Achse -> epics.caget("X06MX-ES-DF1:OMEGA-RBV")
+#read User ReadBAck Omega Achse -> epics.caget("X06MX-ES-DF1:OMEGA-GETP")
+
+#Created on Tue May 18 18:03:02 2021
+#Buntschu Dominik
+#
+
+import rospy
+from std_msgs.msg import Float32
+from std_msgs.msg import Int32
+from sensor_msgs.msg import JointState
+import requests
+import json 
+import epics
+import time
+import csv
+from datetime import date
+import sys
+import math
+import smaract.ctl as ctl
+import matplotlib.pyplot as plt
+import numpy as np
+from mpl_toolkits.mplot3d import Axes3D
+
+#temporary subscriber to check function
+DMS=[]
+Position=[]
+Secs=[]
+Secs2=[]
+Nsecs=[]
+Nsecs2=[]
+Seq=[]
+Seq2=[]
+Liste=[]
+LJU6_JointStateOmega=0
+CHI_Cal=0
+PHI_Cal=0
+OMEGA=0
+OmegaRDB=0
+
+#neccessary subscriber to put in csv
+CHI=[]
+PHI=[]
+SHX=[]
+SHY=[]
+SHZ=[]
+OX=[]
+OY=[]
+OZ=[]
+
+DMS_X = []
+DMS_Y = []
+DMS_Z = []
+#d_handle
+    
+Counter = 0
+motion=True
+
+#now = date.time()
+today = date.today()
+# dd/mm/YY
+day = today.strftime("%Y_%m_%d_")
+#print("d1 =", d1)
+smargopolo_server = "http://smargopolo:3000"
+
+def init_Smargon():
+    smargopolo_server = "http://smargopolo:3000"
+    response = requests.get(smargopolo_server+"/readbackSCS")
+
+def create_CSV_File():
+    #Writing CSV File from List 
+    Liste.extend([Seq,Seq2,Secs,Secs2,Nsecs,Nsecs2,DMS_X,DMS_Y,DMS_Z,CHI,PHI])
+    with open(day+'getToolError.csv', 'w', newline='') as file:
+        writer = csv.writer(file)
+        writer.writerow(["OMEGA","Radius","AI0","AI1","Sequenz1","Sequenz2","Sekunden1","Sekunden1","NaoSek1","NanoSek2","DMS1","DMS2","DMS3","CHI","PHI"])
+        writer.writerow(Liste)
+        #for row in Liste:
+         #   writer.writerow(row)
+
+def move_to_CHI90_Start():
+    print("==Move to ZERO at Start============")
+    
+    smargopolo_server = "http://smargopolo:3000"
+    response = requests.get(smargopolo_server+"/readbackSCS")
+    #Motion moving to yero Position
+    epics.caput("X06SA-ES-DF1:OMEGA-SETV",50)
+    OmegaRDB=epics.caget("X06SA-ES-DF1:OMEGA-RBV")
+    
+    if OmegaRDB > 180 or OmegaRDB < -180:
+        print("Omega > 180")
+        epics.caput("X06SA-ES-DF1:OMEGA-SETP",0)
+        response = requests.put(smargopolo_server+'/targetSCS?CHI=90')
+        response = requests.put(smargopolo_server+'/targetSCS?PHI=0')
+        time.sleep(40)
+    
+    else:
+        print("Omega < 180")
+        epics.caput("X06SA-ES-DF1:OMEGA-SETP",0)
+        response = requests.put(smargopolo_server+'/targetSCS?CHI=90')
+        response = requests.put(smargopolo_server+'/targetSCS?PHI=0')
+        time.sleep(5)
+        
+    #get Position Feedback
+    readbackSCS = json.loads(response.text)
+    get_CHI=readbackSCS['CHI']
+    get_PHI=readbackSCS['PHI']
+    OmegaRDB=epics.caget("X06SA-ES-DF1:OMEGA-RBV")
+    OmegaRDB=round(OmegaRDB, 2)
+    print("Omega:", OmegaRDB,"    CHI: ",get_CHI,"    PHI: ",get_PHI)
+    print("")
+    
+def move_to_CHI90_Stopp():
+    print("==Move to ZERO at Stopp============")
+    
+    smargopolo_server = "http://smargopolo:3000"
+    response = requests.get(smargopolo_server+"/readbackSCS")
+    #Motion moving to yero Position
+    epics.caput("X06SA-ES-DF1:OMEGA-SETV",50)
+    OmegaRDB=epics.caget("X06SA-ES-DF1:OMEGA-RBV")
+    
+    if OmegaRDB > 180 or OmegaRDB < -180:
+        print("Omega > 180")
+        epics.caput("X06SA-ES-DF1:OMEGA-SETP",0)
+        response = requests.put(smargopolo_server+'/targetSCS?CHI=90')
+        response = requests.put(smargopolo_server+'/targetSCS?PHI=0')
+        time.sleep(20)
+    
+    else:
+        print("Omega < 180")
+        epics.caput("X06SA-ES-DF1:OMEGA-SETP",0)
+        response = requests.put(smargopolo_server+'/targetSCS?CHI=90')
+        response = requests.put(smargopolo_server+'/targetSCS?PHI=0')
+        time.sleep(5)
+        
+    #get Position Feedback
+    readbackSCS = json.loads(response.text)
+    get_CHI=readbackSCS['CHI']
+    get_PHI=readbackSCS['PHI']
+    OmegaRDB=epics.caget("X06SA-ES-DF1:OMEGA-RBV")
+    OmegaRDB=round(OmegaRDB, 2)
+    print("Omega:", OmegaRDB,"    CHI: ",get_CHI,"    PHI: ",get_PHI)
+    print("")
+    
+    
+def callback_LJUE9_JointState(data):
+    
+    global Nsecs,Secs,Seq,Liste,CHI,PHI,Position,Counter,OMEGA  
+                
+    Position=(data.position)
+    OMEGA=(data.position[0])
+    Secs=(data.header.stamp.secs)
+    Nsecs=(data.header.stamp.nsecs)
+    Seq=(data.header.seq)
+    Liste=list(Position) 
+    
+    response = requests.get(smargopolo_server+"/readbackSCS")
+    readbackSCS = json.loads(response.text)
+    CHI=readbackSCS['CHI']
+    PHI=readbackSCS['PHI']
+        
+def callback_readbackCAL_JointState(data):   
+    
+    global DMS_X,DMS_Y,DMS_Z,Seq2,Secs2,Nsecs2
+    
+    DMS_X.append(data.position[0])
+    DMS_Y.append(data.position[1])
+    DMS_Z.append(data.position[2])
+    Secs2.append(data.header.stamp.secs)
+    Nsecs2.append(data.header.stamp.nsecs)
+    Seq2.append(data.header.seq)
+
+def plot_graph():
+
+    #3D PLot
+    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')
+    set_axes_equal(ax)
+    fig.show()
+
+    #2D PLot
+    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()
+
+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}")
+        
+def start_ToolError_routine():
+    
+    print()
+    print("============First Motion===========")
+    
+    #get Smargon
+    smargopolo_server = "http://smargopolo:3000"
+    response = requests.get(smargopolo_server+"/readbackSCS")
+    readbackSCS = json.loads(response.text)
+    get_CHI=round(readbackSCS['CHI'])
+    get_PHI=round(readbackSCS['PHI'])
+    
+    #get Aerotech
+    OmegaRDB=epics.caget("X06SA-ES-DF1:OMEGA-RBV")
+    OmegaRDB=round(OmegaRDB, 2)
+
+    #set amount of rotations in degree
+    PHI_Rotation = 360
+    
+    if get_CHI==90 and OmegaRDB == 0:
+            
+        print("Move PHI to " +str(PHI_Rotation) +" degree")
+        response = requests.put(smargopolo_server+'/targetSCS?PHI='+str(PHI_Rotation))
+        time.sleep(15)
+        print("Move PHI to 0 degree")
+        response = requests.put(smargopolo_server+'/targetSCS?PHI='+str(0))
+        time.sleep(15)
+            
+    #get Position Feedback
+    response = requests.get(smargopolo_server+"/readbackSCS")
+    readbackSCS = json.loads(response.text) 
+    get_CHI=readbackSCS['CHI']
+    get_PHI=readbackSCS['PHI']
+    OmegaRDB=epics.caget("X06SA-ES-DF1:OMEGA-RBV")
+    OmegaRDB=round(OmegaRDB, 2)
+    print("Omega:", OmegaRDB,"    CHI: ",get_CHI,"    PHI: ",get_PHI)
+    print("")
+    
+    
+if __name__ == '__main__':   
+    
+    init_Smargon()
+    move_to_CHI90_Start()
+    rospy.init_node('listener', anonymous=True)
+    subs1=rospy.Subscriber("/LJUE9_JointState", JointState, callback_LJUE9_JointState)
+    subs2=rospy.Subscriber("/readbackCAL", JointState, callback_readbackCAL_JointState)
+    start_ToolError_routine()
+    subs1.unregister()
+    subs2.unregister()
+    create_CSV_File()
+    plot_graph()
+    calculate_correction(DMS_X)
+    calculate_correction(DMS_Y)
+    calculate_correction(DMS_Z)
+    move_to_CHI90_Stopp()
+
+    
+
+
+        
+        
+        
+
+
+
+
+
+        
+        
+        
+        
+        
+        
+
+
+        
+        
+        
+
+
+
+