#!/usr/bin/python3 import sys import numpy from scipy.fftpack import fft import matplotlib.pyplot as plt import math if __name__ == '__main__': data = numpy.loadtxt(sys.argv[1]) print(data.shape) assert data.shape[1] == 39, "Unexpected # columns %d" % data.shape[1] seq = data[...,0] 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 t_nom = numpy.linspace(0,1,len(t_sensor)) * t_tot 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]) for i in range(0,7): n = int(6.5/(i + 1)*1000) cut = slice(1000,1000+n) f, axarr = plt.subplots(4, sharex=True) f.set_size_inches(15, 12) axarr[0].set_title(r'\$K_p^{act}-K_p^{ref}\$') axarr[0].plot(seq[cut],(kp[...,i*2]-kp[...,i*2+1])[cut]) axarr[1].set_title('\$pos^{act}-pos^{ref}\$') axarr[1].plot(seq[cut],(pos[...,i*2]-pos[...,i*2+1])[cut]) axarr[2].set_title(r'Torque [Nm]') axarr[2].plot(seq[cut],torque[...,i][cut]) axarr[3].set_title(r'\$t_{robot} - t_{sensor}\$') axarr[3].plot(seq[cut], numpy.stack(((t_robot - t_sensor), (t_robot - t_nom)), axis=1)[cut]) f.savefig('axis_%d_%s.jpg' % (i + 1, sys.argv[1])) plt.show()