test/integration/joint_controller_system.cc
/////////////////////////////////////////////////
// Tests the JointController using joint force commands
Tests the JointController using joint force commands
Tests the JointController using joint force commands
TEST_F(JointControllerTestFixture, JointVelocityCommandWithForce)
{
  using namespace std::chrono_literals;
  ServerConfig serverConfig;
  const auto sdfFile = std::string(PROJECT_SOURCE_PATH) +
    "/test/worlds/joint_controller.sdf";
  serverConfig.SetSdfFile(sdfFile);
  Server server(serverConfig);
  EXPECT_FALSE(server.Running());
  EXPECT_FALSE(*server.Running(0));
  server.SetUpdatePeriod(0ns);
  const std::string linkName = "rotor2";
  test::Relay testSystem;
  math::Vector3d angularVelocity;
  testSystem.OnPreUpdate(
      [&](const gazebo::UpdateInfo &, gazebo::EntityComponentManager &_ecm)
      {
        auto link = _ecm.EntityByComponents(components::Link(),
                                            components::Name(linkName));
        if (nullptr == _ecm.Component<components::AngularVelocity>(link))
        {
          _ecm.CreateComponent(link, components::AngularVelocity());
        }
      });
  testSystem.OnPostUpdate([&](const gazebo::UpdateInfo &,
                              const gazebo::EntityComponentManager &_ecm)
      {
        _ecm.Each<components::Link, components::Name,
                  components::AngularVelocity>(
            [&](const ignition::gazebo::Entity &,
                const components::Link *,
                const components::Name *_name,
                const components::AngularVelocity *_angularVel) -> bool
            {
              EXPECT_EQ(_name->Data(), linkName);
              angularVelocity = _angularVel->Data();
              return true;
            });
      });
  server.AddSystem(testSystem.systemPtr);
  const std::size_t initIters = 10;
  server.Run(true, initIters, false);
  EXPECT_NEAR(0, angularVelocity.Length(), TOL);
  transport::Node node;
  auto pub = node.Advertise<msgs::Double>(
      "/model/joint_controller_test_2/joint/j1/cmd_vel");
  const double testAngVel{20.0};
  msgs::Double msg;
  msg.set_data(testAngVel);
  pub.Publish(msg);
  std::this_thread::sleep_for(100ms);
  const std::size_t testIters = 3000;
  server.Run(true, testIters , false);
  EXPECT_NEAR(0, angularVelocity.X(), 1e-2);
  EXPECT_NEAR(0, angularVelocity.Y(), 1e-2);
  EXPECT_NEAR(testAngVel, angularVelocity.Z(), 1e-2);
}
TEST_F ( JointControllerTestFixture , JointVelocityCommandWithForce ) { using namespace std :: chrono_literals ; ServerConfig serverConfig ; const auto sdfFile = std :: string ( PROJECT_SOURCE_PATH ) + " " ; serverConfig . SetSdfFile ( sdfFile ) ; Server server ( serverConfig ) ; EXPECT_FALSE ( server . Running ( ) ) ; EXPECT_FALSE ( * server . Running ( 0 ) ) ; server . SetUpdatePeriod ( 0 ns ) ; const std :: string linkName = " " ; test :: Relay testSystem ; math :: Vector3d angularVelocity ; testSystem . OnPreUpdate ( [ & ] ( const gazebo :: UpdateInfo & , gazebo :: EntityComponentManager & _ecm ) { auto link = _ecm . EntityByComponents ( components :: Link ( ) , components :: Name ( linkName ) ) ; if ( nullptr == _ecm . Component < components :: AngularVelocity > ( link ) ) { _ecm . CreateComponent ( link , components :: AngularVelocity ( ) ) ; } } ) ; testSystem . OnPostUpdate ( [ & ] ( const gazebo :: UpdateInfo & , const gazebo :: EntityComponentManager & _ecm ) { _ecm . Each < components :: Link , components :: Name , components :: AngularVelocity > ( [ & ] ( const ignition :: gazebo :: Entity & , const components :: Link * , const components :: Name * _name , const components :: AngularVelocity * _angularVel ) -> bool { EXPECT_EQ ( _name -> Data ( ) , linkName ) ; angularVelocity = _angularVel -> Data ( ) ; return true ; } ) ; } ) ; server . AddSystem ( testSystem . systemPtr ) ; const std :: size_t initIters = 10 ; server . Run ( true , initIters , false ) ; EXPECT_NEAR ( 0 , angularVelocity . Length ( ) , TOL ) ; transport :: Node node ; auto pub = node . Advertise < msgs :: Double > ( " " ) ; const double testAngVel { 20.0 } ; msgs :: Double msg ; msg . set_data ( testAngVel ) ; pub . Publish ( msg ) ; std :: this_thread :: sleep_for ( 100 ms ) ; const std :: size_t testIters = 3000 ; server . Run ( true , testIters , false ) ; EXPECT_NEAR ( 0 , angularVelocity . X ( ) , 1e-2 ) ; EXPECT_NEAR ( 0 , angularVelocity . Y ( ) , 1e-2 ) ; EXPECT_NEAR ( testAngVel , angularVelocity . Z ( ) , 1e-2 ) ; }
Tests the JointController using joint force commands
Tests the JointController using joint force commands
// Start server // Create an AngularVelocity component if it doesn't exist. This signals // physics system to populate the component // Publish command and check that the joint velocity is set // Wait for the message to be published