Function BOOST_AUTO_TEST_CASE

tests/forward/mujoco-humanoid.cpp:87–147

}

BOOST_AUTO_TEST_CASE(mujoco_humanoid)
{
  ModelHandle model_handle(new Model());
  Model & model = ::pinocchio::helper::get_ref(model_handle);
  GeometryModelHandle geom_model_handle(new GeometryModel());
  GeometryModel & geom_model = ::pinocchio::helper::get_ref(geom_model_handle);

  const bool verbose = false;
  ::pinocchio::mjcf::buildModel(SIMPLE_TEST_DATA_DIR "/mujoco_humanoid.xml", model, verbose);
  ::pinocchio::mjcf::buildGeom(model, SIMPLE_TEST_DATA_DIR "/mujoco_humanoid.xml", pinocchio::COLLISION, geom_model);
  addFloorToGeomModel(geom_model);

  // sanity checks
  assert(model.nv == 27);
  assert(geom_model.geometryObjects.size() == 20);

  // initial state
  const Eigen::VectorXd q0 = model.referenceConfigurations["qpos0"];
  const Eigen::VectorXd v0 = Eigen::VectorXd::Zero(model.nv);
  const Eigen::VectorXd tau0 = Eigen::VectorXd::Zero(model.nv);

  // add collision pairs
  addSystemCollisionPairs(model, geom_model, q0);
  assert(geom_model.collisionPairs.size() == 175);

  // run simulation
  model.lowerPositionLimit.setConstant(-std::numeric_limits<double>::infinity());
  model.upperPositionLimit.setConstant(std::numeric_limits<double>::infinity());
  model.lowerDryFrictionLimit.setZero();
  model.upperDryFrictionLimit.setZero();
  const double dt = 1e-3;
  const Eigen::VectorXd zero_torque = Eigen::VectorXd::Zero(model.nv);

  {
    Simulator sim(model_handle, geom_model_handle);
    Eigen::VectorXd q = q0;
    Eigen::VectorXd v = v0;
    for (size_t i = 0; i < 100; ++i)
    {
      BOOST_CHECK_NO_THROW(sim.step<ADMM>(q, v, zero_torque, dt));
      BOOST_CHECK(sim.admm_constraint_solver.getIterationCount() < sim.admm_constraint_solver_settings.max_iter);
      BOOST_CHECK(sim.pgs_constraint_solver.getIterationCount() == 0); // make sure pgs didnt run
      q = sim.qnew;
      v = sim.vnew;
    }
  }

  {
    Simulator sim(model_handle, geom_model_handle);
    Eigen::VectorXd q = q0;
    Eigen::VectorXd v = v0;
    for (size_t i = 0; i < 100; ++i)
    {
      BOOST_CHECK_NO_THROW(sim.step<PGS>(q, v, zero_torque, dt));
      BOOST_CHECK(sim.pgs_constraint_solver.getIterationCount() < sim.pgs_constraint_solver_settings.max_iter);
      BOOST_CHECK(sim.admm_constraint_solver.getIterationCount() == 0); // make sure admm didnt run
      q = sim.qnew;
      v = sim.vnew;