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)