diff --git a/python/plots/postprocessing/plotting.py b/python/plots/postprocessing/plotting.py
new file mode 100644
index 0000000000000000000000000000000000000000..a70106273df6f76e07babc090ef9077244e450c0
--- /dev/null
+++ b/python/plots/postprocessing/plotting.py
@@ -0,0 +1,60 @@
+import numpy as np
+import matplotlib.pyplot as plt
+
+
+R_01 = np.load('/home/joschua/Coding/forceControl/master-project/python/plots/postprocessing/R_01.npy')
+
+#experimentLogs = np.hstack((p2, phi_delta, log_compPose_R, log_realPose_R, log_compJoints_R, log_realJoints_R, log_force, p1, log_compJoints_R, log_realPose_L, log_compJoints_L, log_realJoints_L))
+
+data = np.load('./data/experimentLogs300-0,2-1.npy')
+p2_des = data[:, 0:3]
+p2_is = data[:, 12:15]
+p2_comp = data[:, 6:9]
+force = data[:, 32:33]*5-5
+
+# error in workspace
+e = (p2_des - p2_is) * 1000 # m 2 mm
+e_ik = (p2_des - p2_comp) * 1000 # m 2 mm
+
+noSamples = len(e)
+
+# transform error into ee frame
+for i in range(noSamples):
+ e[i, :] = np.transpose(R_01[i] @ e[i, :].T)
+ e_ik[i, :] = np.transpose(R_01[i] @ e_ik[i, :].T)
+
+
+
+
+time = np.linspace(0, round(1.0/80.0 * noSamples), num=noSamples)
+
+
+
+fig = plt.figure()
+
+ax1 = fig.add_subplot(311)
+ax2 = fig.add_subplot(312)
+ax3 = fig.add_subplot(313)
+
+
+ax1.plot(time[0:-10], e[0:-10, 0], label='e_x')
+#ax.plot(time[0:-10], e_ik[0:-10, 0], label='eik_x_ee')
+ax2.plot(time[0:-10], e[0:-10, 1]*(-1), label='e_y')
+ax3.plot(time[0:-10], e[0:-10, 2], label='e_z')
+
+ax4 = ax2.twinx()
+ax4.plot(time[0:-10], force[0:-10], label='force', color='r')
+ax4.set_ylim(-1, 1)
+ax4.set_ylabel('N')
+
+ax1.set_title('position error in EE frame')
+ax1.set_ylabel('mm')
+ax1.set_xlabel('s')
+ax2.set_ylabel('mm')
+ax2.set_xlabel('s')
+ax3.set_ylabel('mm')
+ax3.set_xlabel('s')
+ax1.legend()
+ax2.legend()
+ax3.legend()
+plt.show()
\ No newline at end of file
diff --git a/python/plots/preprocessing/plotting.py b/python/plots/preprocessing/plotting.py
index d76ac8713c14bfe7e35e138b74f06f3dadafe89c..aa92766abbefd1c3ce53a9e638e4bccc71a5d03b 100644
--- a/python/plots/preprocessing/plotting.py
+++ b/python/plots/preprocessing/plotting.py
@@ -13,6 +13,7 @@ pos1 = np.load(import_path+'pos1.npy')
pos2 = np.load(import_path+'pos2.npy')
p2m_ref = np.load(import_path+'p2m_ref.npy')
v1 = np.load(import_path+'v1.npy')
+v2 = np.load(import_path+'v2.npy')
# use self-defined tum-cycler for TUM-blue colors
plt.style.use('mylatex')
@@ -22,11 +23,11 @@ plt.rcParams.update({
"font.family": "serif", # use serif/main font for text elements
"text.usetex": True, # use inline math for ticks
"pgf.rcfonts": False, # don't setup fonts from rc parameters
- "legend.loc": 'upper right',
+ #"legend.loc": 'upper right',
"savefig.directory": '/home/joschua/Documents/Studium/TUM/Thesis/documentation/thesis/myWorkFiles/AMStudentThesis/figures/plots/'
})
-# do plots for tcp1
+""" # do plots for tcp1
#fig = plt.figure()
fig = plt.figure(figsize=(6.25, 2))
ax = fig.add_subplot(111)
@@ -50,14 +51,26 @@ plt.show()
fig.tight_layout()
#fig.savefig('/home/joschua/Documents/Studium/TUM/Thesis/documentation/thesis/myWorkFiles/AMStudentThesis/figures/plots/lefttraj_part.pgf', format='pgf')
+ """
-
-fig = plt.figure()
+fig = plt.figure(figsize=(6.25, 2))
+ax = fig.add_subplot(111)
v1_abs = np.array(list(map(lambda x: norm(x, 2), v1))).reshape(len(v1[:,0]),1)
-plt.plot(np.linspace(0, (len(v1[:,0])-1)*1.0/80.0, len(v1[:,0])).reshape(len(v1[:,0]),1), v1_abs*1000, marker='x')
-plt.ylim(bottom=0)
+v2_abs = np.array(list(map(lambda x: norm(x, 2), v2))).reshape(len(v2[:,0]),1)
+timeline = np.linspace(0, (len(v1[:,0])-1)*1.0/80.0, len(v1[:,0])).reshape(len(v1[:,0]),1)
+v_des = np.ones((len(timeline), 1))* 1000
+ax.plot( timeline, v1_abs*1000*60, label='$v_1$')
+ax.plot( timeline, v2_abs*1000*60, label='$v_2$')
+ax.plot( timeline, v_des, linestyle='dashed', label='$v_f$')
+ax.set_xlabel('s')
+ax.set_ylabel('mm/min')
+ax.legend()
+#plt.ylim(bottom=0)
+fig.tight_layout()
plt.show()
+
+"""
# make a 3D visualization
fig = plt.figure(figsize=(10, 5))
ax = fig.add_subplot(111, projection='3d')
@@ -69,11 +82,11 @@ ax.set_ylim(0, 100)
ax.view_init(elev=25., azim=-115.)
plt.show()
-""" # do plot for tcp2
+# do plot for tcp2
fig = plt.figure()
plt.scatter(p2[:,0], p2[:,1])
plt.scatter(pos2[:,0], pos2[:,1], marker="x")
-plt.show() """
+plt.show()
fig = plt.figure()
@@ -103,4 +116,4 @@ ax.set_zlabel('$y_2$')
ax.legend()
plt.show()
-fig.tight_layout()
\ No newline at end of file
+fig.tight_layout() """
\ No newline at end of file
diff --git a/python/preprocessing/genPaths.py b/python/preprocessing/genPaths.py
index ec376cabac10053c2c470ca1be9d5505adee0da8..40819a58727d0daa9dab1d730d493e5fa50bbf5b 100644
--- a/python/preprocessing/genPaths.py
+++ b/python/preprocessing/genPaths.py
@@ -33,7 +33,7 @@ pos1, pos2 = pos_split[0]*0.001, pos_split[1]*0.001 # transform to SI-units - [m
# TODO: compute trajectory based on path of g-code
# set a cutting speed, sample rate and compute necessary
# position at every time step
-c_speed = 1000 * 0.001 * 1/60 # define cutting speed as 300 mm/min [m/s]
+c_speed = 300 * 0.001 * 1/60 # define cutting speed as 300 mm/min [m/s]
st = 1.0/80.0 # highest possible sample time - 250 Hz
# modificatin to delete in order to check angles in RS
@@ -97,6 +97,7 @@ p2m_ref = np.hstack((p2,z2)) # original coordinates
effLen = np.zeros((pNum,1))
# using the zyx euler convention to store orientation - same in robot studio
ang = np.zeros((pNum,3))
+R_01 = []
for i in range(pNum):
x1, x2 = p1[i,0], p2[i,0]
@@ -141,6 +142,9 @@ for i in range(pNum):
# store values, cut last entry being 1 that was added for the transformation
p2m[i,:] = np.transpose(r0_02)
+
+ # saving rotation matrices for easier transformation in postprocessing for plotting
+ R_01.append(R10)
# compute the respective vecolity of p2 - after doing the interpolation it takes the sample time st to the next point
# slice the first and the last sample
@@ -192,4 +196,7 @@ np.save(plot_path+'pos2', pos2)
np.save(plot_path+'p1m', p1m)
np.save(plot_path+'p2m', p2m)
np.save(plot_path+'p2m_ref', p2m_ref)
-np.save(plot_path+'v1', v1)
\ No newline at end of file
+np.save(plot_path+'v1', v1)
+np.save(plot_path+'v2', v2)
+
+np.save('/home/joschua/Coding/forceControl/master-project/python/plots/postprocessing/R_01', R_01)
\ No newline at end of file