test/integration/physics_system.cc
TEST_F(PhysicsSystemFixture, RevoluteJoint)
{
  ignition::gazebo::ServerConfig serverConfig;

  const auto sdfFile = std::string(PROJECT_SOURCE_PATH) +
    "/test/worlds/revolute_joint.sdf";

  sdf::Root root;
  root.Load(sdfFile);
  const sdf::World *world = root.WorldByIndex(0);
  ASSERT_TRUE(nullptr != world);

  serverConfig.SetSdfFile(sdfFile);

  gazebo::Server server(serverConfig);

  server.SetUpdatePeriod(1us);

  const std::string modelName{"revolute_demo"};
  const std::string rotatingLinkName{"link2"};

  test::Relay testSystem;

  std::vector<double> armDistances;

  // The test checks if the link connected to the joint swings from side to
  // side. To do this, we check that the maximum and minimum distances of the
  // swinging arm from it's parent model frame. The maximum distance is when the
  // arm is in its initial position. The minimum distance is when the arm is in
  // line with the support arm.
  testSystem.OnPostUpdate(
    [&rotatingLinkName, &armDistances](const gazebo::UpdateInfo &,
    const gazebo::EntityComponentManager &_ecm)
    {
      _ecm.Each<components::Link, components::Name, components::Pose>(
        [&](const ignition::gazebo::Entity &, const components::Link *,
        const components::Name *_name, const components::Pose *_pose)->bool
        {
          if (rotatingLinkName == _name->Data())
          {
            auto pos = _pose->Data().Pos();
            // we ignore the x axis to simplify distance comparisons
            pos.X() = 0;
            armDistances.push_back(pos.Length());
          }
          return true;
        });
    });

  server.AddSystem(testSystem.systemPtr);
  server.Run(true, 1000, false);

  ASSERT_GT(armDistances.size(), 0u);

  const sdf::Model *model = world->ModelByIndex(1);

  auto getCylinderLength = [&model](const std::string &_linkName)
  {
    auto collision = model->LinkByName(_linkName)->CollisionByIndex(0);
    auto cylinder = collision->Geom()->CylinderShape();
    return cylinder->Length();
  };

  const double link1Length = getCylinderLength("link1");
  const double link2Length = getCylinderLength("link2");
  // divide by 2.0 because the position is the center of the link.
  const double expMinDist = link1Length - link2Length/2.0;
  auto link2InitialPos = model->LinkByName("link2")->RawPose().Pos();
  // we ignore the x axis to simplify distance comparisons
  link2InitialPos.X() = 0;
  const double expMaxDist = link2InitialPos.Length();

  auto minmax = std::minmax_element(armDistances.begin(), armDistances.end());

  EXPECT_NEAR(expMinDist, *minmax.first, 1e-3);
  EXPECT_NEAR(expMaxDist, *minmax.second, 1e-3);
}
/////////////////////////////////////////////////
// Test physics integration with revolute joints
Test physics integration with revolute joints
Test physics integration with revolute joints
TEST_F(PhysicsSystemFixture, RevoluteJoint)
{
  ignition::gazebo::ServerConfig serverConfig;
  const auto sdfFile = std::string(PROJECT_SOURCE_PATH) +
    "/test/worlds/revolute_joint.sdf";
  sdf::Root root;
  root.Load(sdfFile);
  const sdf::World *world = root.WorldByIndex(0);
  ASSERT_TRUE(nullptr != world);
  serverConfig.SetSdfFile(sdfFile);
  gazebo::Server server(serverConfig);
  server.SetUpdatePeriod(1us);
  const std::string modelName{"revolute_demo"};
  const std::string rotatingLinkName{"link2"};
  test::Relay testSystem;
  std::vector<double> armDistances;
  testSystem.OnPostUpdate(
    [&rotatingLinkName, &armDistances](const gazebo::UpdateInfo &,
    const gazebo::EntityComponentManager &_ecm)
    {
      _ecm.Each<components::Link, components::Name, components::Pose>(
        [&](const ignition::gazebo::Entity &, const components::Link *,
        const components::Name *_name, const components::Pose *_pose)->bool
        {
          if (rotatingLinkName == _name->Data())
          {
            auto pos = _pose->Data().Pos();
            pos.X() = 0;
            armDistances.push_back(pos.Length());
          }
          return true;
        });
    });
  server.AddSystem(testSystem.systemPtr);
  server.Run(true, 1000, false);
  ASSERT_GT(armDistances.size(), 0u);
  const sdf::Model *model = world->ModelByIndex(1);
  auto getCylinderLength = [&model](const std::string &_linkName)
  {
    auto collision = model->LinkByName(_linkName)->CollisionByIndex(0);
    auto cylinder = collision->Geom()->CylinderShape();
    return cylinder->Length();
  };
  const double link1Length = getCylinderLength("link1");
  const double link2Length = getCylinderLength("link2");
  const double expMinDist = link1Length - link2Length/2.0;
  auto link2InitialPos = model->LinkByName("link2")->RawPose().Pos();
  link2InitialPos.X() = 0;
  const double expMaxDist = link2InitialPos.Length();
  auto minmax = std::minmax_element(armDistances.begin(), armDistances.end());
  EXPECT_NEAR(expMinDist, *minmax.first, 1e-3);
  EXPECT_NEAR(expMaxDist, *minmax.second, 1e-3);
}
TEST_F ( PhysicsSystemFixture , RevoluteJoint ) { ignition :: gazebo :: ServerConfig serverConfig ; const auto sdfFile = std :: string ( PROJECT_SOURCE_PATH ) + " " ; sdf :: Root root ; root . Load ( sdfFile ) ; const sdf :: World * world = root . WorldByIndex ( 0 ) ; ASSERT_TRUE ( nullptr != world ) ; serverConfig . SetSdfFile ( sdfFile ) ; gazebo :: Server server ( serverConfig ) ; server . SetUpdatePeriod ( 1u s ) ; const std :: string modelName { " " } ; const std :: string rotatingLinkName { " " } ; test :: Relay testSystem ; std :: vector < double > armDistances ; testSystem . OnPostUpdate ( [ & rotatingLinkName , & armDistances ] ( const gazebo :: UpdateInfo & , const gazebo :: EntityComponentManager & _ecm ) { _ecm . Each < components :: Link , components :: Name , components :: Pose > ( [ & ] ( const ignition :: gazebo :: Entity & , const components :: Link * , const components :: Name * _name , const components :: Pose * _pose ) -> bool { if ( rotatingLinkName == _name -> Data ( ) ) { auto pos = _pose -> Data ( ) . Pos ( ) ; pos . X ( ) = 0 ; armDistances . push_back ( pos . Length ( ) ) ; } return true ; } ) ; } ) ; server . AddSystem ( testSystem . systemPtr ) ; server . Run ( true , 1000 , false ) ; ASSERT_GT ( armDistances . size ( ) , 0u ) ; const sdf :: Model * model = world -> ModelByIndex ( 1 ) ; auto getCylinderLength = [ & model ] ( const std :: string & _linkName ) { auto collision = model -> LinkByName ( _linkName ) -> CollisionByIndex ( 0 ) ; auto cylinder = collision -> Geom ( ) -> CylinderShape ( ) ; return cylinder -> Length ( ) ; } ; const double link1Length = getCylinderLength ( " " ) ; const double link2Length = getCylinderLength ( " " ) ; const double expMinDist = link1Length - link2Length / 2.0 ; auto link2InitialPos = model -> LinkByName ( " " ) -> RawPose ( ) . Pos ( ) ; link2InitialPos . X ( ) = 0 ; const double expMaxDist = link2InitialPos . Length ( ) ; auto minmax = std :: minmax_element ( armDistances . begin ( ) , armDistances . end ( ) ) ; EXPECT_NEAR ( expMinDist , * minmax . first , 1e-3 ) ; EXPECT_NEAR ( expMaxDist , * minmax . second , 1e-3 ) ; }
Test physics integration with revolute joints
Test physics integration with revolute joints
// The test checks if the link connected to the joint swings from side to // side. To do this, we check that the maximum and minimum distances of the // swinging arm from it's parent model frame. The maximum distance is when the // arm is in its initial position. The minimum distance is when the arm is in // line with the support arm. // we ignore the x axis to simplify distance comparisons // divide by 2.0 because the position is the center of the link. // we ignore the x axis to simplify distance comparisons