test/integration/apply_joint_force_system.cc
/////////////////////////////////////////////////
// Tests that the ApplyJointForce accepts joint velocity commands
Tests that the ApplyJointForce accepts joint velocity commands
Tests that the ApplyJointForce accepts joint velocity commands
TEST_F(ApplyJointForceTestFixture, JointVelocityCommand)
{
  using namespace std::chrono_literals;
  ServerConfig serverConfig;
  const auto sdfFile = std::string(PROJECT_SOURCE_PATH) +
    "/test/worlds/apply_joint_force.sdf";
  serverConfig.SetSdfFile(sdfFile);
  Server server(serverConfig);
  EXPECT_FALSE(server.Running());
  EXPECT_FALSE(*server.Running(0));
  server.SetUpdatePeriod(1ms);
  const std::string jointName = "j1";
  test::Relay testSystem;
  std::vector<double> jointForceCmd;
  testSystem.OnPreUpdate(
      [&](const gazebo::UpdateInfo &, gazebo::EntityComponentManager &_ecm)
      {
        auto joint = _ecm.EntityByComponents(components::Joint(),
                                             components::Name(jointName));
        auto forceComp = _ecm.Component<components::JointForceCmd>(joint);
        if (forceComp)
        {
          jointForceCmd.push_back(forceComp->Data()[0]);
        }
      });
  server.AddSystem(testSystem.systemPtr);
  const std::size_t initIters = 10;
  server.Run(true, initIters, false);
  EXPECT_EQ(initIters, jointForceCmd.size());
  for (const auto &jointForce : jointForceCmd)
  {
    EXPECT_NEAR(0, jointForce, TOL);
  }
  jointForceCmd.clear();
  transport::Node node;
  auto pub = node.Advertise<msgs::Double>(
      "/model/joint_force_test/joint/j1/cmd_force");
  const double testJointForce{0.001};
  msgs::Double msg;
  msg.set_data(testJointForce);
  pub.Publish(msg);
  const std::size_t maxIters = 1000;
  for (std::size_t i = 0; i < maxIters; ++i)
  {
    server.Run(true, 1, false);
    if (std::abs(jointForceCmd.back() - testJointForce) < 1e-6)
    {
      break;
    }
  }
  EXPECT_DOUBLE_EQ(jointForceCmd.back(), testJointForce);
}
TEST_F ( ApplyJointForceTestFixture , JointVelocityCommand ) { 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 ( 1 ms ) ; const std :: string jointName = " " ; test :: Relay testSystem ; std :: vector < double > jointForceCmd ; testSystem . OnPreUpdate ( [ & ] ( const gazebo :: UpdateInfo & , gazebo :: EntityComponentManager & _ecm ) { auto joint = _ecm . EntityByComponents ( components :: Joint ( ) , components :: Name ( jointName ) ) ; auto forceComp = _ecm . Component < components :: JointForceCmd > ( joint ) ; if ( forceComp ) { jointForceCmd . push_back ( forceComp -> Data ( ) [ 0 ] ) ; } } ) ; server . AddSystem ( testSystem . systemPtr ) ; const std :: size_t initIters = 10 ; server . Run ( true , initIters , false ) ; EXPECT_EQ ( initIters , jointForceCmd . size ( ) ) ; for ( const auto & jointForce : jointForceCmd ) { EXPECT_NEAR ( 0 , jointForce , TOL ) ; } jointForceCmd . clear ( ) ; transport :: Node node ; auto pub = node . Advertise < msgs :: Double > ( " " ) ; const double testJointForce { 0.001 } ; msgs :: Double msg ; msg . set_data ( testJointForce ) ; pub . Publish ( msg ) ; const std :: size_t maxIters = 1000 ; for ( std :: size_t i = 0 ; i < maxIters ; ++ i ) { server . Run ( true , 1 , false ) ; if ( std :: abs ( jointForceCmd . back ( ) - testJointForce ) < 1e-6 ) { break ; } } EXPECT_DOUBLE_EQ ( jointForceCmd . back ( ) , testJointForce ) ; }
Tests that the ApplyJointForce accepts joint velocity commands
Tests that the ApplyJointForce accepts joint velocity commands
// Start server // Slow update period to increase the likelihood of published data arriving // before iterations are completed // Publish command and check that the joint force is set // Wait for the message to be published