-
Notifications
You must be signed in to change notification settings - Fork 0
/
read_table.py
103 lines (81 loc) · 3.11 KB
/
read_table.py
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
import pickle
import matplotlib.pyplot as plt
import matplotlib as mpl
import numpy as np
# dic = ''
# with open(r'test.txt','r') as f:
# for i in f.readlines():
# dic=i #string
# dic = eval(dic) # this is orignal dict with instace dict
dic = pickle.load(open('results/abb/abb6640/ABB6640_acc.pickle','rb'))
# dic = pickle.load(open('results/motoman/MA2010/MA2010_acc.pickle','rb'))
# dic = pickle.load(open('results/test.pickle','rb'))
###surface plots of accleration limits, x as q2, y as q3
x=[]
y=[]
x2=[]
y2=[]
x3=[]
y3=[]
q1_acc=[]
q2_acc_p=[]
q3_acc_p=[]
q2_acc_n=[]
q3_acc_n=[]
for key, value in dic.items():
x.append(key[0])
y.append(key[1])
q1_acc.append(value[0])
###manual thresholding the boundary for bad data
if key[0]>-1.6 and key[0]<2.1:
x2.append(key[0])
y2.append(key[1])
q2_acc_n.append(value[2])
q2_acc_p.append(value[3])
if key[1]<1 and key[1]>-3.2:
x3.append(key[0])
y3.append(key[1])
q3_acc_n.append(value[4])
q3_acc_p.append(value[5])
#####################################################################get acc from q###########################################################
# q=np.array([2,0,-1,1,3,4])
# xy=np.array([x,y]).T
# idx=np.argmin(np.linalg.norm(xy-q[1:3],axis=1))
# print('q2,q3 at: ',x[idx],y[idx])
# print('acc: ',q1_acc[idx],q2_acc[idx],q3_acc[idx],47.29253791291949,39.49167516506145,54.32806813314554)
#####################################################################surface plots##########################################################
fig = plt.figure()
ax = fig.add_subplot(111, projection='3d')
surf = ax.plot_trisurf(x, y, q1_acc, linewidth=0, antialiased=False)
ax.set_xlabel('q2 (rad)')
ax.set_ylabel('q3 (rad)')
ax.set_zlabel('q1 acc (rad/s^2)')
plt.title('Joint1 Acceleration Limit', x=0.5, y=0.95)
plt.savefig('results/j1_acc.png')
plt.show()
fig = plt.figure()
ax = fig.add_subplot(111, projection='3d')
surf = ax.plot_trisurf(x2, y2, q2_acc_n, linewidth=0, antialiased=False, label='-')
surf = ax.plot_trisurf(x2, y2, q2_acc_p, linewidth=0, antialiased=False, label='+')
ax.set_xlabel('q2 (rad)')
ax.set_ylabel('q3 (rad)')
ax.set_zlabel('q2 acc (rad/s^2)')
plt.title('Joint2 Acceleration Limit')
fake2Dline = mpl.lines.Line2D([0],[0], linestyle="none", c='b', marker = 'o')
fake2Dline2 = mpl.lines.Line2D([0],[0], linestyle="none", c='orange', marker = 'o')
ax.legend([fake2Dline,fake2Dline2], ['- direction','+ direction'], numpoints = 1)
plt.savefig('results/j2_acc.png')
plt.show()
fig = plt.figure()
ax = fig.add_subplot(111, projection='3d')
surf = ax.plot_trisurf(x3, y3, q3_acc_n, linewidth=0, antialiased=False)
surf = ax.plot_trisurf(x3, y3, q3_acc_p, linewidth=0, antialiased=False)
ax.set_xlabel('q2 (rad)')
ax.set_ylabel('q3 (rad)')
ax.set_zlabel('q3 acc (rad/s^2)')
plt.title('Joint3 Acceleration Limit')
fake2Dline = mpl.lines.Line2D([0],[0], linestyle="none", c='b', marker = 'o')
fake2Dline2 = mpl.lines.Line2D([0],[0], linestyle="none", c='orange', marker = 'o')
ax.legend([fake2Dline,fake2Dline2], ['- direction','+ direction'], numpoints = 1)
plt.savefig('results/j3_acc.png')
plt.show()