def plotForceTracking(time, f, fRef):
max_f = np.max(np.array([fRef[:, :3], f[:, :3]]))
min_f = np.min(np.array([fRef[:, :3], f[:, :3]]))
max_f += 0.1 * (max_f - min_f)
min_f -= 0.1 * (max_f - min_f)
max_m = np.max(np.array([fRef[:, 3:], f[:, 3:]]))
min_m = np.min(np.array([fRef[:, 3:], f[:, 3:]]))
max_m += 0.1 * (max_m - min_m)
min_m -= 0.1 * (max_m - min_m)
for i in range(6):
print 'Max force tracking error for axis %d: %.2f' % (
i, np.max(np.abs(f[:, i] - fRef[:, i])))
print 'Mean square force tracking error for axis %d: %.2f' % (
i, np.linalg.norm(f[:, i] - fRef[:, i]) / N)
plt.figure()
plt.plot(time, f[:, i])
plt.plot(time, fRef[:, i], 'r--')
# plt.figure(); plt.plot(time, f[:,i]-fRef[:,i],'r-');
plt.xlabel('Time [s]')
plt.ylabel('Force [N]')
if (i < 3):
plt.ylim([min_f, max_f])
else:
plt.ylim([min_m, max_m])
leg = plt.legend(['Force', 'Desired force'], loc='best')
leg.get_frame().set_alpha(plut.LEGEND_ALPHA)
title = 'Force axis ' + str(i) + ' f VS fRef'
plut.saveCurrentFigure(title)
plt.title(title)
def plotForceTracking(time, f, fRef):
max_f = np.max(np.array([fRef[:,:3],f[:,:3]]));
min_f = np.min(np.array([fRef[:,:3],f[:,:3]]));
max_f += 0.1*(max_f-min_f);
min_f -= 0.1*(max_f-min_f);
max_m = np.max(np.array([fRef[:,3:],f[:,3:]]));
min_m = np.min(np.array([fRef[:,3:],f[:,3:]]));
max_m += 0.1*(max_m-min_m);
min_m -= 0.1*(max_m-min_m);
for i in range(6):
print 'Max force tracking error for axis %d: %.2f' % (i, np.max(np.abs(f[:,i]-fRef[:,i])));
print 'Mean square force tracking error for axis %d: %.2f' % (i, np.linalg.norm(f[:,i]-fRef[:,i])/N);
plt.figure(); plt.plot(time, f[:,i]); plt.plot(time, fRef[:,i],'r--');
# plt.figure(); plt.plot(time, f[:,i]-fRef[:,i],'r-');
plt.xlabel('Time [s]');
plt.ylabel('Force [N]');
if(i<3):
plt.ylim([min_f, max_f]);
else:
plt.ylim([min_m, max_m]);
leg = plt.legend(['Force', 'Desired force'], loc='best');
leg.get_frame().set_alpha(plut.LEGEND_ALPHA);
title = 'Force axis '+str(i)+' f VS fRef';
plut.saveCurrentFigure(title);
plt.title(title);
def doplot():
plt.clf()
for idx, i in enumerate([2, 10, 50]): #NACADO_ITER:
#plt.figure()
plt.subplot(1, 3, idx + 1)
treatCheckData(results[i]['extra'],
'After %d OCP iteration' % i) #'Refine in %d iter'%i)
plt.legend(
['Ground truth', 'Policy approx', 'Trajectory approx', 'Cold start'],
loc=4)
plt.subplot(1, 3, 1)
plt.ylabel('Cost')
saveCurrentFigure('warmstart')
A[np.where(tau > 0), 4] = 1
A[np.where(tau < 0), 5] = -1
# mpl.rcParams['axes.labelsize'] = 45;
# mpl.rcParams['font.size'] = 45;
f, ax = plt.subplots(1, 1, sharex=True)
res = np.abs(np.dot(A, x_ls)) - np.abs(delta_q)
binwidth = (max(res) - min(res)) / 20.0
ax.hist(res, bins=np.arange(min(res),
max(res) + binwidth, binwidth))
ax.set_xlabel('Residual [rad]')
ax.set_ylabel('Number of samples')
ax.yaxis.set_ticks([0, 5e3, 10e3, 15e3])
ax.xaxis.set_ticks([-0.004, -0.002, 0, 0.002])
ax.set_xlim([-0.004, 0.002])
saveCurrentFigure('friction_residuals_histogram_symmetric')
f, ax = plt.subplots(1, 1, sharex=True)
res = np.abs(np.dot(A, x)) - np.abs(delta_q)
binwidth = (max(res) - min(res)) / 20.0
ax.hist(res, bins=np.arange(min(res),
max(res) + binwidth, binwidth))
ax.set_xlabel('Residual [rad]')
ax.set_ylabel('Number of samples')
ax.yaxis.set_ticks([0, 5000, 10000, 15000])
ax.xaxis.set_ticks([-0.004, -0.002, 0, 0.002])
ax.set_xlim([-0.004, 0.002])
saveCurrentFigure('friction_residuals_histogram_asymmetric')
# res = np.abs(np.dot(A,x_c1)) - np.abs(delta_q);
# binwidth = (max(res) - min(res))/20.0;
plt.figure(); plt.plot(time, f[:,i]); plt.plot(time, fRef[:,i],'r--',rasterized=True);
plt.xlabel('Time [s]');
plt.ylabel('Force [N]');
if(i<3):
plt.ylim([min_f, max_f]);
else:
plt.ylim([min_m, max_m]);
if(i==2):
loc = 'bottom right';
else:
loc = 'best'
if(SHOW_LEGEND):
leg = plt.legend(['Force', 'Desired force'], loc=loc);
leg.get_frame().set_alpha(plut.LEGEND_ALPHA);
title = 'Force axis '+str(i)+' f VS fRef';
plut.saveCurrentFigure(title);
plt.title(title);
for i in range(len(JOINT_ID)):
# compute delta_q_friction from velocity estimation
delta_q_friction = k_v[i]*dq[:,i];
# compute delta_q_fb from other components of delta_q (there was a bug in the c++ code computing delta_q_fb)
delta_q_fb[:,i] = qDes[:,i]-enc[:,i] - delta_q_ff[:,i] - delta_q_friction;
j = JOINT_ID[i];
delta_q_fb_pos = k_tau[j]*(1+k_p[j])*k_s[j]*(enc[0,i]-enc[:,i]);
delta_q_fb_vel = k_tau[j]*(1+k_p[j])*k_d[j]*(-dq[:,i]);
delta_q_fb_force = delta_q_fb[:,i] - delta_q_fb_pos - delta_q_fb_vel;
plt.figure();
plt.plot(time, 1e3*delta_q_ff[:,i],rasterized=True);
plt.plot(time, 1e3*delta_q_friction,rasterized=True);
if (USE_TORQUE_CTRL):
for i in range(len(JOINT_ID)):
plt.figure()
plt.plot(time, 1e3 * delta_q_ff[:, i])
plt.plot(time, 1e3 * delta_q_friction[:, i])
plt.plot(time, 1e3 * delta_q_fb_force[:, i])
plt.plot(time, 1e3 * delta_q_fb_vel[:, i])
plt.plot(time, 1e3 * (delta_q[:, i]))
plt.xlabel('Time [s]')
plt.ylabel('Delta_q [rad*1000]')
leg = plt.legend(
['ff torque', 'ff friction', 'fb force', 'fb vel', 'total'])
leg.get_frame().set_alpha(plut.LEGEND_ALPHA)
title = 'Joint ' + str(JOINT_ID[i]) + ' delta_q components'
plut.saveCurrentFigure(title)
plt.title(title)
# plt.figure(); plt.plot(enc[:,JOINT_ID[i]]-qDes[:,i]); plt.title('Delta_q '+str(JOINT_ID[i]));
# plt.figure(); plt.plot(dq[:,i]); plt.title('Joint velocity '+str(JOINT_ID[i]));
# plt.figure(); plt.plot(tau[:,i]); plt.title('Joint torque '+str(JOINT_ID[i]));
# for i in range(len(JOINT_ID)):
# f, ax = plt.subplots(1,1,sharex=True);
# ax.plot(tau[:,i], qDes[:,i]-enc[:,JOINT_ID[i]],'r. ');
# tau_model = np.array([tau_min[i], tau_max[i]]);
# delta_q_model = k_tau[i]*tau_model;
# ax.plot(tau_model, delta_q_model,'b');
# ax.set_title('Torque VS delta_q '+str(JOINT_ID[i]));
# ax.set_xlabel('Torque [Nm]');
# ax.set_ylabel('Delta_q [rad]');
A[np.where(dq<-ZERO_VEL_THR),3] = -1;
A[np.where(tau>0),4] = 1;
A[np.where(tau<0),5] = -1;
# mpl.rcParams['axes.labelsize'] = 45;
# mpl.rcParams['font.size'] = 45;
f, ax = plt.subplots(1,1,sharex=True);
res = np.abs(np.dot(A,x_ls)) - np.abs(delta_q);
binwidth = (max(res) - min(res))/20.0;
ax.hist(res, bins=np.arange(min(res), max(res) + binwidth, binwidth));
ax.set_xlabel('Residual [rad]');
ax.set_ylabel('Number of samples');
ax.yaxis.set_ticks([0, 5e3, 10e3, 15e3]);
ax.xaxis.set_ticks([-0.004, -0.002, 0, 0.002]);
ax.set_xlim([-0.004, 0.002]);
saveCurrentFigure('friction_residuals_histogram_symmetric');
f, ax = plt.subplots(1,1,sharex=True);
res = np.abs(np.dot(A,x)) - np.abs(delta_q);
binwidth = (max(res) - min(res))/20.0;
ax.hist(res, bins=np.arange(min(res), max(res) + binwidth, binwidth));
ax.set_xlabel('Residual [rad]');
ax.set_ylabel('Number of samples');
ax.yaxis.set_ticks([0, 5000, 10000, 15000]);
ax.xaxis.set_ticks([-0.004, -0.002, 0, 0.002]);
ax.set_xlim([-0.004, 0.002]);
saveCurrentFigure('friction_residuals_histogram_asymmetric');
# res = np.abs(np.dot(A,x_c1)) - np.abs(delta_q);
# binwidth = (max(res) - min(res))/20.0;
# ax[2].hist(res, bins=np.arange(min(res), max(res) + binwidth, binwidth));