0

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()
Trenton McKinney
  • 56,955
  • 33
  • 144
  • 158
dthedog
  • 3
  • 2

0 Answers0