test/integration/lift_drag_system.cc
/////////////////////////////////////////////////
/// Measure / verify force torques against analytical answers.
Measure / verify force torques against analytical answers.
Measure / verify force torques against analytical answers .
TEST_F(LiftDragTestFixture, VerifyVerticalForce)
{
  using namespace std::chrono_literals;
  ServerConfig serverConfig;
  const auto sdfFile =
      std::string(PROJECT_SOURCE_PATH) + "/test/worlds/lift_drag.sdf";
  serverConfig.SetSdfFile(sdfFile);
  Server server(serverConfig);
  EXPECT_FALSE(server.Running());
  EXPECT_FALSE(*server.Running(0));
  server.SetUpdatePeriod(0ns);
  const std::string bodyName = "body";
  const std::string bladeName = "wing_1";
  const std::string jointName = "body_joint";
  const double desiredVel = -0.2;
  test::Relay testSystem;
  std::vector<math::Vector3d> linearVelocities;
  std::vector<math::Vector3d> forces;
  testSystem.OnPreUpdate(
      [&](const gazebo::UpdateInfo &, gazebo::EntityComponentManager &_ecm)
      {
        auto bladeLink = _ecm.EntityByComponents(components::Link(),
                                                 components::Name(bladeName));
        if (nullptr == _ecm.Component<components::AngularVelocity>(bladeLink))
        {
          _ecm.CreateComponent(bladeLink, components::AngularVelocity());
        }
        auto bodyLink = _ecm.EntityByComponents(components::Link(),
                                                components::Name(bodyName));
        if (nullptr ==
            _ecm.Component<components::WorldLinearVelocity>(bodyLink))
        {
          _ecm.CreateComponent(bodyLink, components::WorldLinearVelocity());
        }
        if (nullptr ==
            _ecm.Component<components::WorldLinearAcceleration>(bodyLink))
        {
          _ecm.CreateComponent(bodyLink, components::WorldLinearAcceleration());
        }
      });
  server.AddSystem(testSystem.systemPtr);
  server.Run(true, 1, false);
  const double kp = 100.0;
  testSystem.OnPreUpdate(
      [&](const gazebo::UpdateInfo &, gazebo::EntityComponentManager &_ecm)
      {
        auto joint = _ecm.EntityByComponents(components::Joint(),
                                             components::Name(jointName));
        auto bodyLink = _ecm.EntityByComponents(components::Link(),
                                                components::Name(bodyName));
        auto linVelComp =
            _ecm.Component<components::WorldLinearVelocity>(bodyLink);
        if (!linVelComp)
          return;
        auto jointCmd = kp * (desiredVel - linVelComp->Data().X());
        if (nullptr == _ecm.Component<components::JointForceCmd>(joint))
        {
          _ecm.CreateComponent(joint,
                               components::JointForceCmd({jointCmd}));
        }
        else
        {
          _ecm.Component<components::JointForceCmd>(joint)->Data()[0] =
              jointCmd;
        }
      });
  test::Relay wrenchRecorder;
  wrenchRecorder.OnPreUpdate([&](const gazebo::UpdateInfo &,
                              const gazebo::EntityComponentManager &_ecm)
      {
        auto bladeLink = _ecm.EntityByComponents(components::Link(),
                                                 components::Name(bladeName));
        auto bodyLink = _ecm.EntityByComponents(components::Link(),
                                                components::Name(bodyName));
        auto linVelComp =
            _ecm.Component<components::WorldLinearVelocity>(bodyLink);
        auto wrenchComp =
            _ecm.Component<components::ExternalWorldWrenchCmd>(bladeLink);
        if (linVelComp)
        {
          linearVelocities.push_back(linVelComp->Data());
        }
        else
        {
          linearVelocities.push_back(math::Vector3d::Zero);
        }
        if (wrenchComp)
        {
          math::Vector3d force = msgs::Convert(wrenchComp->Data().force());
          forces.push_back(force);
        }
        else
        {
          forces.push_back(math::Vector3d::Zero);
        }
      });
  server.AddSystem(wrenchRecorder.systemPtr);
  const double a0 = 0.1;
  const double cla = 4.0;
  const double dihedral = 0.1;
  const double rho = 1.2041;
  const double area = 10;
  const std::size_t testIters = 1000;
  server.Run(true, testIters , false);
  EXPECT_EQ(testIters, forces.size());
  EXPECT_EQ(testIters, linearVelocities.size());
  for (std::size_t i = forces.size() - 15; i < forces.size(); ++i)
  {
    const double v = linearVelocities[i].X();
    const double q = 0.5 * rho * v * v;
    const double cl = cla * a0 * q * area;
    const double vertForce = forces[i].Z();
    const double expVertForce = cl * cos(dihedral);
    EXPECT_NEAR(expVertForce, vertForce, TOL);
    EXPECT_GT(vertForce, 0);
  }
}
TEST_F ( LiftDragTestFixture , VerifyVerticalForce ) { 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 bodyName = " " ; const std :: string bladeName = " " ; const std :: string jointName = " " ; const double desiredVel = -0.2 ; test :: Relay testSystem ; std :: vector < math :: Vector3d > linearVelocities ; std :: vector < math :: Vector3d > forces ; testSystem . OnPreUpdate ( [ & ] ( const gazebo :: UpdateInfo & , gazebo :: EntityComponentManager & _ecm ) { auto bladeLink = _ecm . EntityByComponents ( components :: Link ( ) , components :: Name ( bladeName ) ) ; if ( nullptr == _ecm . Component < components :: AngularVelocity > ( bladeLink ) ) { _ecm . CreateComponent ( bladeLink , components :: AngularVelocity ( ) ) ; } auto bodyLink = _ecm . EntityByComponents ( components :: Link ( ) , components :: Name ( bodyName ) ) ; if ( nullptr == _ecm . Component < components :: WorldLinearVelocity > ( bodyLink ) ) { _ecm . CreateComponent ( bodyLink , components :: WorldLinearVelocity ( ) ) ; } if ( nullptr == _ecm . Component < components :: WorldLinearAcceleration > ( bodyLink ) ) { _ecm . CreateComponent ( bodyLink , components :: WorldLinearAcceleration ( ) ) ; } } ) ; server . AddSystem ( testSystem . systemPtr ) ; server . Run ( true , 1 , false ) ; const double kp = 100.0 ; testSystem . OnPreUpdate ( [ & ] ( const gazebo :: UpdateInfo & , gazebo :: EntityComponentManager & _ecm ) { auto joint = _ecm . EntityByComponents ( components :: Joint ( ) , components :: Name ( jointName ) ) ; auto bodyLink = _ecm . EntityByComponents ( components :: Link ( ) , components :: Name ( bodyName ) ) ; auto linVelComp = _ecm . Component < components :: WorldLinearVelocity > ( bodyLink ) ; if ( ! linVelComp ) return ; auto jointCmd = kp * ( desiredVel - linVelComp -> Data ( ) . X ( ) ) ; if ( nullptr == _ecm . Component < components :: JointForceCmd > ( joint ) ) { _ecm . CreateComponent ( joint , components :: JointForceCmd ( { jointCmd } ) ) ; } else { _ecm . Component < components :: JointForceCmd > ( joint ) -> Data ( ) [ 0 ] = jointCmd ; } } ) ; test :: Relay wrenchRecorder ; wrenchRecorder . OnPreUpdate ( [ & ] ( const gazebo :: UpdateInfo & , const gazebo :: EntityComponentManager & _ecm ) { auto bladeLink = _ecm . EntityByComponents ( components :: Link ( ) , components :: Name ( bladeName ) ) ; auto bodyLink = _ecm . EntityByComponents ( components :: Link ( ) , components :: Name ( bodyName ) ) ; auto linVelComp = _ecm . Component < components :: WorldLinearVelocity > ( bodyLink ) ; auto wrenchComp = _ecm . Component < components :: ExternalWorldWrenchCmd > ( bladeLink ) ; if ( linVelComp ) { linearVelocities . push_back ( linVelComp -> Data ( ) ) ; } else { linearVelocities . push_back ( math :: Vector3d :: Zero ) ; } if ( wrenchComp ) { math :: Vector3d force = msgs :: Convert ( wrenchComp -> Data ( ) . force ( ) ) ; forces . push_back ( force ) ; } else { forces . push_back ( math :: Vector3d :: Zero ) ; } } ) ; server . AddSystem ( wrenchRecorder . systemPtr ) ; const double a0 = 0.1 ; const double cla = 4.0 ; const double dihedral = 0.1 ; const double rho = 1.2041 ; const double area = 10 ; const std :: size_t testIters = 1000 ; server . Run ( true , testIters , false ) ; EXPECT_EQ ( testIters , forces . size ( ) ) ; EXPECT_EQ ( testIters , linearVelocities . size ( ) ) ; for ( std :: size_t i = forces . size ( ) - 15 ; i < forces . size ( ) ; ++ i ) { const double v = linearVelocities [ i ] . X ( ) ; const double q = 0.5 * rho * v * v ; const double cl = cla * a0 * q * area ; const double vertForce = forces [ i ] . Z ( ) ; const double expVertForce = cl * cos ( dihedral ) ; EXPECT_NEAR ( expVertForce , vertForce , TOL ) ; EXPECT_GT ( vertForce , 0 ) ; } }
Measure / verify force torques against analytical answers.
Measure / verify force torques against analytical answers .
// Start server // Create velocity and acceleration components if they dont't exist. // This signals physics system to populate the component // Set a constant velocity to the prismatic joint // \todo(addisu) This assumes that the this system will run after the lift // drag system. This is needed to capture the wrench set by the lift drag // system. This assumption may not hold when systems are run in parallel. // parameters from SDF // This the angle the blade makes with the horizontal axis. It's set in the // <pose> of the link as the roll // It takes a few iterations before the system reaches a steady state // The test above passes if the force is zero (which can happen if the // system is not functioning properly), so we check here to make sure it's // not zero