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))