v_mean = v[:, i] + 0.5 * dt * dv[:, i]
v[:, i + 1] = v[:, i] + dt * dv[:, i]
q[:, i + 1] = se3.integrate(model, q[:, i], dt * v_mean)
t += conf.dt
if i % conf.DISPLAY_N == 0:
robot_display.display(q[:, i])
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_JOINT_POS):
(f, ax) = plut.create_empty_figure(int(robot.nv / 2), 2)
ax = ax.reshape(robot.nv)
for i in range(robot.nv):
ax[i].plot(time, q[i, :-1], label=r'$q$ ')
ax[i].plot(time, q_ref[i, :], '--', label=r'$q^{ref}$ ')
ax[i].set_xlabel('Time [s]')
ax[i].set_ylabel(r'$q_%d$ [rad]' % i)
leg = ax[0].legend()
leg.get_frame().set_alpha(0.5)
if (PLOT_JOINT_VEL):
(f, ax) = plut.create_empty_figure(int(robot.nv / 2), 2)
ax = ax.reshape(robot.nv)
for i in range(robot.nv):
ax[i].plot(time, v[i, :-1], label=r'$\dot{q}$ ')
ax[i].plot(time, v_ref[i, :], '--', label=r'$\dot{q}^{ref}$ ')
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)