Function createBallsScene

tests/forward/simulation-combine-constraints.cpp:39–118

\brief Creates a scene of 10 balls, all lying on a floor. \param reduced_collision_pairs activate only the collision pair between balls and floor.

/// \brief Creates a scene of 10 balls, all lying on a floor.
/// \param reduced_collision_pairs activate only the collision pair between balls and floor.
std::tuple<ModelHandle, GeometryModelHandle, Eigen::VectorXd>
createBallsScene(bool prismatic_joint, bool collision_plane, bool inter_colision)
{
  Model model;
  GeometryModel geom_model;

  // Add plane
  hpp::fcl::CollisionGeometryPtr_t plane_ptr(new hpp::fcl::Halfspace(0., 0., 1., 0.));
  const GeometryObject plane_geom = GeometryObject("plane", 0, SE3::Identity(), plane_ptr);
  GeomIndex plane_id = geom_model.addGeometryObject(plane_geom);

  // Add balls
  const std::size_t nballs = 10;
  const double radius = 1.0;
  const double mass = 1.0;
  for (std::size_t i = 0; i < nballs; ++i)
  {
    const std::string name = "ball_" + std::to_string(i);
    const std::string joint_name = "joint_" + std::to_string(i);
    const std::string frame_name = "frame_" + std::to_string(i);
    const Inertia inertia = Inertia::FromSphere(mass, radius);
    const JointIndex parent = 0;
    JointIndex joint_id;
    SE3 placement = SE3::Identity();
    ;
    if (prismatic_joint)
    {
      placement.translation() = Eigen::Vector3d(static_cast<double>(i) * 2 * radius * 1.1, 0, 0);
      const JointModelPZ joint = JointModelPZ();
      joint_id = model.addJoint(parent, joint, placement, joint_name);
    }
    else
    {
      const JointModelFreeFlyer joint = JointModelFreeFlyer();
      joint_id = model.addJoint(parent, joint, placement, joint_name);
    }
    model.appendBodyToJoint(joint_id, inertia);
    hpp::fcl::CollisionGeometryPtr_t ball_ptr(new hpp::fcl::Sphere(radius));
    const GeometryObject ball_geom = GeometryObject(name, joint_id, placement, ball_ptr);
    GeomIndex ball_id = geom_model.addGeometryObject(ball_geom);
    if (collision_plane)
    {
      CollisionPair col_pair(plane_id, ball_id);
      geom_model.addCollisionPair(col_pair);
    }
  }

  if (inter_colision)
  {
    for (size_t i = 0; i < geom_model.geometryObjects.size(); ++i)
    {
      for (size_t j = i + 1; j < geom_model.geometryObjects.size(); ++j)
      {
        CollisionPair col_pair(i, j);
        geom_model.addCollisionPair(col_pair);
      }
    }
  }