def callback(self, q):
if self.viz is None:
return
vizutils.applyViewerConfiguration(self.viz, 'world/ball',
pin.SE3ToXYZQUATtuple(M))
vizutils.applyViewerConfiguration(self.viz, 'world/box',
pin.SE3ToXYZQUATtuple(self.Mtarget))
self.viz.display(q)
time.sleep(1e-2)
def callback(self, q):
if self.viz is None:
return
pin.framesForwardKinematics(self.rmodel, self.rdata, q)
M = self.rdata.oMf[self.frameIndex]
vizutils.applyViewerConfiguration(self.viz, 'world/ball',
pin.SE3ToXYZQUATtuple(M))
vizutils.applyViewerConfiguration(self.viz, 'world/box',
self.ptarget.tolist() + [1, 0, 0, 0])
self.viz.display(q)
time.sleep(1e-2)
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 and t <= Walk_phases.getFinalTime(
len(Walk_phases.p) - 1) + time_offset * conf.dt:
print('current com', tsid.robot.com(tsid.formulation.data()))
print('desired com', CurveSet.com_traj[i - time_offset])
def run_simu():
global push_robot_active
i, t = 0, 0.0
q, v = tsid.q, tsid.v
time_avg = 0.0
while True:
time_start = time.time()
tsid.comTask.setReference(tsid.trajCom.computeNext())
tsid.postureTask.setReference(tsid.trajPosture.computeNext())
tsid.rightFootTask.setReference(tsid.trajRF.computeNext())
tsid.leftFootTask.setReference(tsid.trajLF.computeNext())
HQPData = tsid.formulation.computeProblemData(t, q, v)
sol = tsid.solver.solve(HQPData)
if sol.status != 0:
print("QP problem could not be solved! Error code:", sol.status)
break
# tau = tsid.formulation.getActuatorForces(sol)
dv = tsid.formulation.getAccelerations(sol)
q, v = tsid.integrate_dv(q, v, dv, conf.dt)
i, t = i + 1, t + conf.dt
if push_robot_active:
push_robot_active = False
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
A = np.vstack((J_com, J))
b = np.concatenate(
(np.array(push_robot_com_vel), np.zeros(J.shape[0])))
v = np.linalg.lstsq(A, b, rcond=-1)[0]
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 % 1000 == 0:
print("Average loop time: %.1f (expected is %.1f)" %
(1e3 * time_avg, 1e3 * conf.dt))
time_spent = time.time() - time_start
time_avg = (i * time_avg + time_spent) / (i + 1)
if time_avg < 0.9 * conf.dt:
time.sleep(10 * (conf.dt - time_avg))