148 lines
4.1 KiB
Python
Executable File
148 lines
4.1 KiB
Python
Executable File
#!/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
|
|
|
|
|
|
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(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__':
|
|
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(DMS_X)
|
|
calculate_correction(DMS_Y)
|
|
calculate_correction(DMS_Z)
|
|
|
|
|