analyze.py 1.51 KB
Newer Older
Anders Blomdell's avatar
Anders Blomdell committed
1
2
3
4
#!/usr/bin/python3

import sys
import numpy
5
from scipy.fftpack import fft
Anders Blomdell's avatar
Anders Blomdell committed
6
7
8
9
10
11
import matplotlib.pyplot as plt
import math

if __name__ == '__main__':
    data = numpy.loadtxt(sys.argv[1])
    print(data.shape)
12
    assert data.shape[1] == 39, "Unexpected # columns %d" % data.shape[1]
13

Anders Blomdell's avatar
Anders Blomdell committed
14
    seq = data[...,0]
15
16
17
    t_tot = data[-1,2] - data[0,2]
    t_robot = (data[...,1] - data[0,1]) / (data[-1,1] - data[0,1]) * t_tot
    t_sensor = (data[...,2] - data[0,2]) / (data[-1,2] - data[0,2]) * t_tot
18
    t_nom = numpy.linspace(0,1,len(t_sensor)) * t_tot
19

20
21
22
23
    kp = data[...,4:18]
    d_kp = kp[1:,1::2]-kp[0:-1,1::2]
    pos = (data[...,18:32] - data[0,18:32]) * 180 / math.pi
    torque = (data[...,32:39] - data[0,32:39])
Anders Blomdell's avatar
Anders Blomdell committed
24
    for i in range(0,7):
25
        n = int(6.5/(i + 1)*1000)
Anders Blomdell's avatar
Anders Blomdell committed
26
        cut = slice(1000,1000+n)
27
        f, axarr = plt.subplots(4, sharex=True)
28
        f.set_size_inches(15, 12)
29
30
        axarr[0].set_title(r'$K_p^{act}-K_p^{ref}$')
        axarr[0].plot(seq[cut],(kp[...,i*2]-kp[...,i*2+1])[cut])
31
32
        axarr[1].set_title('$pos^{act}-pos^{ref}$')
        axarr[1].plot(seq[cut],(pos[...,i*2]-pos[...,i*2+1])[cut])
Anders Blomdell's avatar
Anders Blomdell committed
33
34
        axarr[2].set_title(r'Torque [Nm]')
        axarr[2].plot(seq[cut],torque[...,i][cut])
35
        axarr[3].set_title(r'$t_{robot} - t_{sensor}$')
36
37
38
        axarr[3].plot(seq[cut], numpy.stack(((t_robot - t_sensor),
                                             (t_robot - t_nom)),
                                            axis=1)[cut])
39
        f.savefig('axis_%d_%s.jpg' % (i + 1, sys.argv[1]))
Anders Blomdell's avatar
Anders Blomdell committed
40
41
42

    plt.show()