Function BOOST_AUTO_TEST_CASE

tests/forward/simulation-robots.cpp:39–88

BOOST_AUTO_TEST_SUITE(BOOST_TEST_MODULE)

BOOST_AUTO_TEST_CASE(humanoid_with_bilateral_constraint)
{
  // Construct humanoid
  ModelHandle model(new Model());
  buildModels::humanoid(*model, true);
  model->lowerPositionLimit = Eigen::VectorXd::Constant(model->nq, -1.0) * std::numeric_limits<double>::max();
  model->upperPositionLimit = Eigen::VectorXd::Constant(model->nq, 1.0) * std::numeric_limits<double>::max();
  DataHandle data = std::make_shared<Data>(*model);

  GeometryModelHandle geom_model(new GeometryModel());

  // Initial state
  Eigen::VectorXd q = pinocchio::neutral(*model);

  // Bilateral constraint on the robot right wrist
  BilateralPointConstraintModelVector bilateral_constraint_models;
  pinocchio::framesForwardKinematics(*model, *data, q);
  const JointIndex joint1_id = 0;
  const GeomIndex joint2_id = 13;
  ::pinocchio::SE3 Mc = data->oMi[joint2_id];
  const SE3 joint1_placement = Mc;
  const SE3 joint2_placement = SE3::Identity();
  bilateral_constraint_models.push_back(BilateralPointConstraintModel(*model, joint1_id, joint1_placement, joint2_id, joint2_placement));
  bilateral_constraint_models[0].baumgarte_corrector_parameters().Kp = 0.1;

  // The humanoid's freeflyer's height should remain the same
  Simulator sim(model, geom_model, bilateral_constraint_models);
  sim.admm_constraint_solver_settings.absolute_precision = 1e-9;
  sim.admm_constraint_solver_settings.relative_precision = 1e-9;
  sim.admm_constraint_solver_settings.max_iter = 1000;
  const double dt = 1e-3;
  Eigen::VectorXd v = Eigen::VectorXd::Zero(model->nv);
  Eigen::VectorXd tau = Eigen::VectorXd::Zero(model->nv);
  BOOST_CHECK_NO_THROW(sim.step<ADMM>(q, v, tau, dt));
  pinocchio::framesForwardKinematics(*model, *data, sim.qnew);
  EIGEN_VECTOR_IS_APPROX(Mc.translation(), data->oMi[joint2_id].translation(), 1e-3);

  // Calling the simulator twice to test warmstart
  sim.step<ADMM>(q, v, tau, dt);
  INDEX_EQUALITY_CHECK(sim.admm_constraint_solver.getIterationCount(), 0);

  for (int i = 0; i < 10; ++i)
  {
    Eigen::VectorXd q = sim.qnew;
    Eigen::VectorXd v = sim.vnew;
    BOOST_CHECK_NO_THROW(sim.step<ADMM>(q, v, tau, dt));
    pinocchio::framesForwardKinematics(*model, *data, sim.qnew);
    EIGEN_VECTOR_IS_APPROX(Mc.translation(), data->oMi[joint2_id].translation(), 1e-3);
  }
}

BOOST_AUTO_TEST_CASE(simulator_instance_step_with_friction_on_joints)
{