I have data-text-file with lines like:
+45.16, +1254.067, +4377.328, -1107.861, -235.2307434, 266.4469299, -261.4131470
We should take 6 numbers, except of the first one. Further we have a filter, which transforms this 6 values into quaternion with 4 values.
def FilterUpdate(w_x, w_y, w_z, a_x, a_y, a_z):
...
return SEq_1, SEq_2, SEq_3, SEq_4
Then I make function to get data from file:
def file():
q = []
logs_test = open("log_124.txt", "r")
while True:
line = logs_test.readline()
if not line:
break
w_1 = float(line.split()[1].replace("+", "").replace(",", "")) / 7200
w_2 = float(line.split()[2].replace("+", "").replace(",", "")) / 7200
w_3 = float(line.split()[3].replace("+", "").replace(",", "")) / 7200
a_1 = float(line.split()[4].replace("+", "").replace(",", ""))
a_2 = float(line.split()[5].replace("+", "").replace(",", ""))
a_3 = float(line.split()[6].replace("+", "").replace(",", ""))
q.append(FilterUpdate(w_1, w_2, w_3, a_1, a_2, a_3))
logs_test.close()
return q
And the main task is to do animation of system coordinate axes. Data is written with frequency 250 Hz. I try to draw even without animation, and it doesn't work. I can only draw basic axes:
ox = [1., 0., 0.]
oy = [0., 1., 0.]
oz = [0., 0., 1.]
def data_gen():
ax.cla()
ax.quiver(0, 0, 0, ox[0], ox[1], ox[2], length=1.0, color='blue')
ax.quiver(0, 0, 0, oy[0], oy[1], oy[2], length=1.0, color='blue')
ax.quiver(0, 0, 0, oz[0], oz[1], oz[2], length=1.0, color='blue')
# for i in range(4):
# q_0 = Quaternion(file()[i])
# ox_i = q_0.rotate(ox)
# oy_i = q_0.rotate(oy)
# oz_i = q_0.rotate(oz)
# ax.quiver(0, 0, 0, ox_i[0], ox_i[1], ox_i[2], length=1.0, color='red')
# ax.quiver(0, 0, 0, oy_i[0], oy_i[1], oy_i[2], length=1.0, color='red')
# ax.quiver(0, 0, 0, oz_i[0], oz_i[1], oz_i[2], length=1.0, color='red')
ax.set_xlim(-1, 1)
ax.set_ylim(-1, 1)
ax.set_zlim(-1, 1)
ax.view_init(elev=30, azim=60)
fig = plt.figure()
ax = fig.add_subplot(projection='3d')
data_gen()
ax.set_xlabel('X')
ax.set_ylabel('Y')
ax.set_zlabel('Z')
ax.grid()
plt.show()