i, t = i + 1, t + conf.dt
data = tsid.formulation.data()
if tsid.contact_LF_active:
J_LF = tsid.contactLF.computeMotionTask(0.0, q, v, data).matrix
else:
J_LF = np.zeros((0, tsid.model.nv))
if tsid.contact_RF_active:
J_RF = tsid.contactRF.computeMotionTask(0.0, q, v, data).matrix
else:
J_RF = np.zeros((0, tsid.model.nv))
J = np.vstack((J_LF, J_RF))
J_com = tsid.comTask.compute(t, q, v, data).matrix
if i % conf.DISPLAY_N == 0:
tsid.display(q)
x_com = tsid.robot.com(tsid.formulation.data())
x_com_ref = tsid.trajCom.getSample(t).pos()
H_lf = tsid.robot.framePosition(tsid.formulation.data(), tsid.LF)
H_rf = tsid.robot.framePosition(tsid.formulation.data(), tsid.RF)
x_lf_ref = tsid.trajLF.getSample(t).pos()[:3]
x_rf_ref = tsid.trajRF.getSample(t).pos()[:3]
vizutils.applyViewerConfiguration(tsid.viz, 'world/com', x_com.tolist() + [0, 0, 0, 1.])
vizutils.applyViewerConfiguration(tsid.viz, 'world/com_ref', x_com_ref.tolist() + [0, 0, 0, 1.])
vizutils.applyViewerConfiguration(tsid.viz, 'world/rf', pin.SE3ToXYZQUATtuple(H_rf))
vizutils.applyViewerConfiguration(tsid.viz, 'world/lf', pin.SE3ToXYZQUATtuple(H_lf))
vizutils.applyViewerConfiguration(tsid.viz, 'world/rf_ref', x_rf_ref.tolist() + [0, 0, 0, 1.])
vizutils.applyViewerConfiguration(tsid.viz, 'world/lf_ref', x_lf_ref.tolist() + [0, 0, 0, 1.])
if i % 100 == 0 and t>4:
print ('current com', tsid.robot.com(tsid.formulation.data()))
if tsid.formulation.checkContact(tsid.contactRF.name, sol) and i >= 0:
print("\tnormal force %s: %.1f" %
(tsid.contactRF.name.ljust(20, '.'), f_RF[2, i]))
if tsid.formulation.checkContact(tsid.contactLF.name, sol) and i >= 0:
print("\tnormal force %s: %.1f" %
(tsid.contactLF.name.ljust(20, '.'), f_LF[2, i]))
print("\ttracking err %s: %.3f" % (tsid.comTask.name.ljust(
20, '.'), norm(tsid.comTask.position_error, 2)))
print("\t||v||: %.3f\t ||dv||: %.3f" % (norm(v, 2), norm(dv)))
q, v = tsid.integrate_dv(q, v, dv, conf.dt)
t += conf.dt
if i % conf.DISPLAY_N == 0: tsid.display(q)
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 + N_post) * conf.dt, conf.dt)
if PLOT_COM:
(f, ax) = plut.create_empty_figure(3, 1)
for i in range(3):
ax[i].plot(time, com_pos[i, :], label='CoM ' + str(i))
ax[i].plot(time[:N],
com_pos_ref[i, :],
'r:',
label='CoM Ref ' + str(i))