if i % conf.DISPLAY_N == 0:
tsid.robot_display.display(q[:, i])
tsid.gui.applyConfiguration('world/ee',
ee_pos[:, i].tolist() + [0, 0, 0, 1.])
tsid.gui.applyConfiguration('world/ee_ref',
ee_pos_ref[:, i].tolist() + [0, 0, 0, 1.])
time_spent = time.time() - time_start
if (time_spent < conf.dt): time.sleep(conf.dt - time_spent)
# PLOT STUFF
time = np.arange(0.0, N * conf.dt, conf.dt)
if (PLOT_EE_POS):
(f, ax) = plut.create_empty_figure(3, 1)
for i in range(3):
ax[i].plot(time, ee_pos[i, :], label=r'$x$')
ax[i].plot(time, ee_pos_ref[i, :], 'r:', label=r'$x^{ref}$')
ax[i].set_xlabel('Time [s]')
ax[i].set_ylabel(r'$x_%d$ [m]' % i)
leg = ax[0].legend()
leg.get_frame().set_alpha(0.5)
if (PLOT_EE_VEL):
(f, ax) = plut.create_empty_figure(3, 1)
for i in range(3):
ax[i].plot(time, ee_vel[i, :], label=r'$\dot{x}$')
ax[i].plot(time, ee_vel_ref[i, :], 'r:', label=r'$\dot{x}^{ref}$')
ax[i].set_xlabel('Time [s]')
ax[i].set_ylabel(r'$\dot{x}_%d$ [m/s]' % i)
err = 0.
for i in range(N+1):
t = i*conf.dt
time[i] = t
traj.compute(t)
H = robot.framePlacement(problem.X[i,:robot.nq],problem.frame_id,True)
x_real[i,:] = H.translation
x_traj[i,:] = traj.pos
err += norm(traj.pos - x_real[i,:])
err_mean = err/(N+1)
print('Mean tracking error : ', err_mean)
max_plots = 3
if(x_real.shape[0] == 1):
nplot = 1
(f, ax) = plut.create_empty_figure()
ax = [ax]
else:
nplot = min(max_plots, x_real.shape[0])
(f, ax) = plut.create_empty_figure(nplot, 1)
i_ls = 0
for i in range(len(ax)):
ls = linestyles[0]
ax[i].plot(time, x_real[:, i], ls, label='real', alpha=0.7)
ls = linestyles[1]
ax[i].plot(time, x_traj[:, i], ls, label='traj', alpha=0.7)
ax[i].set_xlabel('Time [s]')
if i==0:
ax[i].set_ylabel(r'$x$')
if i==1:
ax[i].set_ylabel(r'$y$')
if i % int(1 / conf.dt_RC) == 0:
print("Time %.3f\n" % (t))
t += conf.dt_RC
time_spent = tim.time() - time_start
if (conf.simulate_real_time and time_spent < conf.dt_RC):
tim.sleep(3 * conf.dt_RC - time_spent)
# ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ #
# ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ PLOT STUFF ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ #
if (conf.PLOT_TRACK_OCP):
max_plots = 3
if (x_real.shape[0] == 1):
nplot = 1
(ff, ax) = plut.create_empty_figure()
ax = [ax]
else:
nplot = min(max_plots, x_real.shape[0])
(ff, ax) = plut.create_empty_figure(nplot, 1)
i_ls = 0
ax[0].set_title("Trajectories")
for i in range(len(ax)):
ls = linestyles[0]
ax[i].plot(time1, x_real[:, i], ls, label='traj OCP', alpha=0.7)
ls = linestyles[1]
ax[i].plot(time1, x_traj[:, i], ls, label='traj ref', alpha=0.7)
ax[i].set_xlabel('Time [s]')
if i == 0:
ax[i].set_ylabel(r'$x$ [m]')
if i == 1:
desc = test['controller'] + ' kp=' + str(test['kp']) + ' fri=' + str(
test['friction'])
if (test['controller'] == 'OSC'):
tracking_err_osc += [{'value': tracking_err, 'description': desc}]
elif (test['controller'] == 'IC'):
tracking_err_ic += [{'value': tracking_err, 'description': desc}]
else:
print('ERROR: Unknown controller', test['controller'])
print('Average tracking error %.3f m\n' % (tracking_err))
# PLOT STUFF
tt = np.arange(0.0, N * conf.dt, conf.dt)
if (PLOT_EE_POS):
(f, ax) = plut.create_empty_figure(nx)
ax = ax.reshape(nx)
for i in range(nx):
ax[i].plot(tt, x[i, :], label='x')
ax[i].plot(tt, x_ref[i, :], '--', label='x ref')
ax[i].set_xlabel('Time [s]')
ax[i].set_ylabel(r'x_' + str(i) + ' [m]')
leg = ax[0].legend()
leg.get_frame().set_alpha(0.5)
ax[0].set_title(description)
if (PLOT_EE_VEL):
(f, ax) = plut.create_empty_figure(nx)
ax = ax.reshape(nx)
for i in range(nx):
ax[i].plot(tt, dx[i, :], label='dx')
mat_mult_expm[d.method_name] = []
total_err[d.method_name] = []
ndt[d.method_name] = []
mat_norm_expm[d.method_name] += [np.mean(d.mat_norm_expm)]
mat_mult_expm[d.method_name] += [np.mean(d.mat_mult_expm)]
total_err[d.method_name] += [err]
err_traj[name] = err_per_time
ndt[d.method_name] += [d.ndt]
# PLOT STUFF
line_styles = 10 * ['-o', '--o', '-.o', ':o']
tt = np.arange(0.0, (N_SIMULATION + 1) * dt, dt)[:N_SIMULATION + 1]
# PLOT INTEGRATION ERRORS
if (PLOT_INTEGRATION_ERRORS):
(ff, ax) = plut.create_empty_figure(1)
j = 0
for name in sorted(total_err.keys()):
err = total_err[name]
ax.plot(ndt[name],
total_err[name],
line_styles[j],
alpha=0.7,
label=name)
j += 1
ax.set_xlabel('Number of time steps')
ax.set_ylabel('Error norm')
ax.set_xscale('log')
ax.set_yscale('log')
leg = ax.legend()
if (leg): leg.get_frame().set_alpha(0.5)