Commit f4a64b31 authored by Anders Blomdell's avatar Anders Blomdell
Browse files

Add Kp_ref to saved signals, update analyze.py accordingly

parent 890426c1
......@@ -16,6 +16,10 @@ timingtest: timingtest.cpp defs.h \
%.o: %.cpp %.h
gcc -O3 -std=c++11 -c -o $@ $<
measurement: timingtest
./timingtest > measurement_$$(date +"%Y-%m-%d_%H:%M").txt
clean:
rm -f *~
rm -f *.o
......
......@@ -2,30 +2,30 @@
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] == 32, "Unexpected # columns %d" % data.shape[1]
assert data.shape[1] == 39, "Unexpected # columns %d" % data.shape[1]
seq = data[...,0]
kp = data[...,4:11]
d_kp = kp[1:,...]-kp[0:-1,...]
pos = (data[...,11:25] - data[0,11:25]) * 180 / math.pi
torque = (data[...,25:32:] - data[0,25:32])
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 = 4000 - 500 * i
n = 4500 - 500 * (i + 1)
cut = slice(1000,1000+n)
f, axarr = plt.subplots(3, sharex=True)
axarr[0].set_title(r'$\Delta K_p$')
axarr[0].plot(seq[1:][cut],d_kp[...,i][cut])
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('$\Delta Position$')
axarr[1].plot(seq[cut],(pos[...,i*2]-pos[...,i*2+1])[cut])
axarr[1].plot(seq[cut],(pos[...,i*2+1]-pos[...,i*2])[cut])
axarr[2].set_title(r'Torque [Nm]')
axarr[2].plot(seq[cut],torque[...,i][cut])
plt.show()
......@@ -149,9 +149,11 @@ int main(int argc, char* argv[])
printf("%d.%06d %d.%06d ",
timing[pos].start.tv_sec, timing[pos].start.tv_nsec / 1000,
timing[pos].end.tv_sec, timing[pos].end.tv_nsec / 1000);
// Kp
// Kp (ref and act)
for (int i = 0 ; i < 7 ; i++) {
printf("%12.8f ",
printf("%12.8f %12.8f ",
(timing[pos].to_robot.joint[i].pid.kp /
origin.joint[i].pid.kp) - 1.0,
(timing[pos].from_robot.joint[i].pid.kp /
origin.joint[i].pid.kp) - 1.0);
}
......
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment