def plot_multi_x_vs_y_rate_of_change(y,
x,
ylabel,
xlabel='Number of time steps',
logy=True,
logx=True):
line_styles = 10 * ['-o', '--o', '-.o', ':o']
(ff, ax) = plut.create_empty_figure(1)
j = 0
for name in sorted(y.keys()):
if (len(y[name]) > 0):
ax.plot(x[name][:-1],
np.diff(y[name]) / np.diff(x[name]),
line_styles[j],
alpha=0.7,
markerSize=10,
label=name)
j += 1
ax.set_xlabel(xlabel)
ax.set_ylabel(ylabel)
if (logx):
ax.set_xscale('log')
if (logy):
ax.set_yscale('log')
leg = ax.legend(loc='best')
if (leg): leg.get_frame().set_alpha(0.5)
return (ff, ax)
# COMPUTE INTEGRATION ERRORS:
res = compute_integration_errors(data, robot, dt)
# PLOT STUFF
line_styles = 10 * ['-o', '--o', '-.o', ':o']
tt = np.arange(0.0, (N_SIMULATION + 1) * dt, dt)[:N_SIMULATION + 1]
if (PLOT_INTEGRATION_ERRORS):
plot_multi_x_vs_y_log_scale(res.err_infnorm_avg, res.dt,
'Mean error inf-norm', 'Time step [s]')
# plot_multi_x_vs_y_log_scale(res.err_infnorm_avg, res.comp_time, 'Mean error inf-norm', 'Computation time per step')
# plot_multi_x_vs_y_log_scale(res.err_infnorm_avg, res.realtime_factor, 'Mean error inf-norm', 'Real-time factor')
if (PLOT_INTEGRATION_ERROR_TRAJECTORIES):
(ff, ax) = plut.create_empty_figure(1)
for (j, name) in enumerate(sorted(res.err_traj_infnorm.keys())):
if (len(res.err_traj_infnorm[name]) > 0):
ax.plot(tt,
res.err_traj_infnorm[name],
line_styles[j],
alpha=0.7,
label=name)
ax.set_xlabel('Time [s]')
ax.set_ylabel('Error inf-norm')
ax.set_yscale('log')
leg = ax.legend()
if (leg): leg.get_frame().set_alpha(0.5)
# PLOT THE CONTACT FORCES OF ALL INTEGRATION METHODS ON THE SAME PLOT
if (PLOT_FORCES):
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)
if(PLOT_INTEGRATION_ERROR_TRAJECTORIES):
(ff, ax) = plut.create_empty_figure(1)
j = 0
simu.display(q[-1], dt)
t += dt
# end simulation loop
time_spent = time.time() - time_start
print("Real-time factor:", t / time_spent)
# plot base trajectory
plt.figure('base_height')
plt.plot(tt, sim_q[:, 2], line_styles[i_ls], alpha=0.7, label=name)
plt.legend()
plt.title('Base Height vs time ')
# plot contact forces
for ci, ci_name in enumerate(conf.contact_frames):
(ff, ax) = plut.create_empty_figure(3,
1,
name=ci_name + " normal force")
ax = ax.reshape(3)
for i in range(3):
ax[i].plot(tt,
sim_f[:, ci, i],
line_styles[i_ls],
alpha=0.7,
label=name)
ax[i].set_xlabel('Time [s]')
ax[i].set_ylabel('Force [N]')
leg = ax[-1].legend()
if (leg): leg.get_frame().set_alpha(0.5)
ax[0].set_title(ci_name + " normal force vs time")
i_ls += 1
data[name] = run_simulation(q0, v0, simu_params, data['ground-truth-euler'])
# COMPUTE INTEGRATION ERRORS:
res = compute_integration_errors(data, robot, dt)
# 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):
plot_multi_x_vs_y_log_scale(res.err_infnorm_avg, res.ndt, 'Mean error inf-norm')
plot_multi_x_vs_y_log_scale(res.err_infnorm_max, res.ndt, 'Max error inf-norm')
if(PLOT_INTEGRATION_ERROR_TRAJECTORIES):
(ff, ax) = plut.create_empty_figure(1)
for (j,name) in enumerate(sorted(res.err_traj_infnorm.keys())):
ax.plot(tt, res.err_traj_infnorm[name], line_styles[j], alpha=0.7, label=name)
ax.set_xlabel('Time [s]')
ax.set_ylabel('Error inf-norm')
ax.set_yscale('log')
leg = ax.legend()
if(leg): leg.get_frame().set_alpha(0.5)
# FOR EACH INTEGRATION METHOD PLOT THE FORCE PREDICTIONS
if(PLOT_FORCE_PREDICTIONS):
T = N_SIMULATION*dt
tt = np.arange(0.0, (N_SIMULATION+1)*dt, dt)[:N_SIMULATION+1]
# for (name,d) in data.items():
