diff --git a/examples/worlds/lift_drag_nested.sdf b/examples/worlds/lift_drag_nested.sdf new file mode 100644 index 0000000000..1587410928 --- /dev/null +++ b/examples/worlds/lift_drag_nested.sdf @@ -0,0 +1,293 @@ + + + + + + + + + + + + + true + 0 0 10 0 0 0 + 1 1 1 1 + 0.5 0.5 0.5 1 + + 1000 + 0.9 + 0.01 + 0.001 + + -0.5 0.1 -0.9 + + + + true + + + + + 0 0 1 + 100 100 + + + + + + + 0 0 1 + 100 100 + + + + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + + + + + 0 0 0 0 0 0 + + 0.0 0 0.5 0 0 0 + + 0.0 0.0 0.0 0.0 0.0 0.0 + + 1.8 + 0.0 + 0.0 + 1.8 + 0.0 + 1.8 + + 10.0 + + + + + 1 1 1 + + + + + + 0.1 + 0.1 + + + + + + + + 1 1 1 + + + + 0.2 0.2 0.2 1.0 + .421 0.225 0.0 1.0 + + + + + 0 0 1.25 0 0 0 + + 0.0 0.0 0 0.0 0.0 0.0 + + 0.012 + 0.0 + 0.0 + 0.012 + 0.0 + 0.0012 + + 1.0 + + + + + 0.02 0.02 0.5 + + + + + + + 0.02 0.02 0.5 + + + + 0.3 0.3 0.3 1.0 + .421 0.225 0.0 1.0 + + + + + + 0 0 1.5 0.0 0 0 + + 0.0 0.5 0 0.0 0.0 0.0 + + 0.0000465 + 0.0 + 0.0 + 0.0000006 + 0.0 + 0.0000470 + + 0.01 + + + 0.0 0.5 0 0.0 0.0 0.0 + + + 0.2 1.0 0.01 + + + + + 0.0 0.5 0 0.0 0.0 0.0 + + + 0.2 1.0 0.01 + + + + 0.5 0.2 0.2 1.0 + .421 0.225 0.0 1.0 + + + + + + + 0 0 1.5 0.0 0 3.141593 + + 0.0 0.5 0 0.0 0.0 0.0 + + 0.0000465 + 0.0 + 0.0 + 0.0000006 + 0.0 + 0.0000470 + + 0.01 + + + 0.0 0.5 0 0.0 0.0 0.0 + + + 0.2 1.0 0.01 + + + + + 0.0 0.5 0 0.0 0.0 0 + + + 0.2 1.0 0.01 + + + + 0.2 0.5 0.2 1.0 + .421 0.225 0.0 1.0 + + + + + + body + rod_1 + 0.0 0.0 0.0 0.0 0.0 0.0 + + + -1e16 + 1e16 + + 0.0 0.0 -1.0 + + 0.0001 + + + + + 1 + + + + + rod_1 + blade_1 + 0.0 0.0 0.0 0.0 0.0 0.0 + + + rod_1 + blade_2 + 0.0 0.0 0.0 0.0 0.0 0.0 + + + + + + 0.1 + 0.1 + 0.001 + 0.0 + 0.0 0.5 0 + 0.2 + 1.2041 + 1 0 0 + 0 0 1 + blade_1::link + + + 0.1 + 0.1 + 0.001 + 0.0 + 0.0 0.5 0 + 0.2 + 1.2041 + 1 0 0 + 0 0 1 + blade_2::link + + + rod_1_joint + + + + diff --git a/src/systems/lift_drag/LiftDrag.cc b/src/systems/lift_drag/LiftDrag.cc index a9e63d1764..841858cc55 100644 --- a/src/systems/lift_drag/LiftDrag.cc +++ b/src/systems/lift_drag/LiftDrag.cc @@ -33,6 +33,7 @@ #include "ignition/gazebo/components/AngularVelocity.hh" #include "ignition/gazebo/components/Inertial.hh" +#include "ignition/gazebo/components/Joint.hh" #include "ignition/gazebo/components/JointPosition.hh" #include "ignition/gazebo/components/LinearVelocity.hh" #include "ignition/gazebo/components/Link.hh" @@ -172,33 +173,67 @@ void LiftDragPrivate::Load(const EntityComponentManager &_ecm, { sdf::ElementPtr elem = _sdf->GetElement("link_name"); auto linkName = elem->Get(); - this->linkEntity = this->model.LinkByName(_ecm, linkName); + auto entities = + entitiesFromScopedName(linkName, _ecm, this->model.Entity()); - if (this->linkEntity == kNullEntity) + if (entities.empty()) { ignerr << "Link with name[" << linkName << "] not found. " << "The LiftDrag will not generate forces\n"; this->validConfig = false; return; } + else if (entities.size() > 1) + { + ignwarn << "Multiple link entities with name[" << linkName << "] found. " + << "Using the first one.\n"; + } + + this->linkEntity = *entities.begin(); + if (!_ecm.EntityHasComponentType(this->linkEntity, + components::Link::typeId)) + { + this->linkEntity = kNullEntity; + ignerr << "Entity with name[" << linkName << "] is not a link\n"; + this->validConfig = false; + return; + } } else { + ignerr << "The LiftDrag system requires the 'link_name' parameter\n"; this->validConfig = false; + return; } if (_sdf->HasElement("control_joint_name")) { auto controlJointName = _sdf->Get("control_joint_name"); - this->controlJointEntity = this->model.JointByName(_ecm, controlJointName); - if (this->controlJointEntity == kNullEntity) + auto entities = + entitiesFromScopedName(controlJointName, _ecm, this->model.Entity()); + + if (entities.empty()) + { + ignerr << "Joint with name[" << controlJointName << "] not found. " + << "The LiftDrag will not generate forces\n"; + this->validConfig = false; + return; + } + else if (entities.size() > 1) { - ignerr << "Joint with name[" << controlJointName << "] does not exist.\n"; + ignwarn << "Multiple joint entities with name[" << controlJointName + << "] found. Using the first one.\n"; } - else + + this->controlJointEntity = *entities.begin(); + if (!_ecm.EntityHasComponentType(this->controlJointEntity, + components::Joint::typeId)) { + this->controlJointEntity = kNullEntity; + ignerr << "Entity with name[" << controlJointName << "] is not a joint\n"; this->validConfig = false; + return; } } diff --git a/src/systems/lift_drag/LiftDrag.hh b/src/systems/lift_drag/LiftDrag.hh index c03eab6f15..b79e55616d 100644 --- a/src/systems/lift_drag/LiftDrag.hh +++ b/src/systems/lift_drag/LiftDrag.hh @@ -38,7 +38,9 @@ namespace systems /// The following parameters are used by the system: /// /// link_name : Name of the link affected by the group of lift/drag - /// properties. + /// properties. This can be a scoped name to reference links in + /// nested models. \sa entitiesFromScopedName to learn more + /// about scoped names. /// air_density : Density of the fluid this model is suspended in. /// area : Surface area of the link. /// a0 : The initial "alpha" or initial angle of attack. a0 is also @@ -59,6 +61,8 @@ namespace systems /// coefficient curve. /// cda_stall : The ratio of coefficient of drag and alpha slope after /// stall. + /// control_joint_name: Name of joint that actuates a control surface for this + /// lifting body (Optional) class LiftDrag : public System, public ISystemConfigure, diff --git a/test/integration/lift_drag_system.cc b/test/integration/lift_drag_system.cc index 72ecf28d98..4b1e3181ea 100644 --- a/test/integration/lift_drag_system.cc +++ b/test/integration/lift_drag_system.cc @@ -18,6 +18,7 @@ #include #include +#include #include #include @@ -36,6 +37,7 @@ #include "ignition/gazebo/Server.hh" #include "ignition/gazebo/SystemLoader.hh" +#include "ignition/gazebo/Util.hh" #include "ignition/gazebo/test_config.hh" #include "../helpers/Relay.hh" @@ -45,28 +47,45 @@ using namespace ignition; using namespace gazebo; +struct VerticalForceTestParam +{ + const std::string fileName; + const std::string bladeName; +}; + +std::ostream &operator<<(std::ostream &_out, const VerticalForceTestParam &_val) +{ + _out << "[" << _val.fileName << ", " << _val.bladeName << "]"; + return _out; +} + + /// \brief Test fixture for LiftDrag system -class LiftDragTestFixture : public ::testing::Test +class VerticalForceParamFixture: + public ::testing::TestWithParam { - // Documentation inherited protected: void SetUp() override { ignition::common::Console::SetVerbosity(4); - ignition::common::setenv("IGN_GAZEBO_SYSTEM_PLUGIN_PATH", - (std::string(PROJECT_BINARY_PATH) + "/lib").c_str()); + ignition::common::setenv( + "IGN_GAZEBO_SYSTEM_PLUGIN_PATH", + common::joinPaths(PROJECT_BINARY_PATH, "lib").c_str()); } }; ///////////////////////////////////////////////// /// Measure / verify force torques against analytical answers. -TEST_F(LiftDragTestFixture, VerifyVerticalForce) +TEST_P(VerticalForceParamFixture, VerifyVerticalForce) { using namespace std::chrono_literals; + ignition::common::setenv( + "IGN_GAZEBO_RESOURCE_PATH", + common::joinPaths(PROJECT_SOURCE_PATH, "test", "worlds", "models")); // Start server ServerConfig serverConfig; const auto sdfFile = - std::string(PROJECT_SOURCE_PATH) + "/test/worlds/lift_drag.sdf"; + common::joinPaths(PROJECT_SOURCE_PATH, GetParam().fileName); serverConfig.SetSdfFile(sdfFile); Server server(serverConfig); @@ -76,10 +95,20 @@ TEST_F(LiftDragTestFixture, VerifyVerticalForce) server.SetUpdatePeriod(0ns); const std::string bodyName = "body"; - const std::string bladeName = "wing_1"; + const std::string bladeName = GetParam().bladeName; const std::string jointName = "body_joint"; const double desiredVel = -0.2; + auto firstEntityFromScopedName = [](const std::string &_scopedName, + const EntityComponentManager &_ecm) -> Entity + { + auto entities = entitiesFromScopedName(_scopedName, _ecm); + if (entities.size() > 0) + return *entities.begin(); + else + return kNullEntity; + }; + test::Relay testSystem; std::vector linearVelocities; std::vector forces; @@ -88,27 +117,14 @@ TEST_F(LiftDragTestFixture, VerifyVerticalForce) { // Create velocity and acceleration components if they dont't exist. // This signals physics system to populate the component - auto bladeLink = _ecm.EntityByComponents(components::Link(), - components::Name(bladeName)); + auto bladeLink = firstEntityFromScopedName(bladeName, _ecm); - if (nullptr == _ecm.Component(bladeLink)) - { - _ecm.CreateComponent(bladeLink, components::AngularVelocity()); - } + enableComponent(_ecm, bladeLink); - auto bodyLink = _ecm.EntityByComponents(components::Link(), - components::Name(bodyName)); + auto bodyLink = firstEntityFromScopedName(bodyName, _ecm); - if (nullptr == - _ecm.Component(bodyLink)) - { - _ecm.CreateComponent(bodyLink, components::WorldLinearVelocity()); - } - if (nullptr == - _ecm.Component(bodyLink)) - { - _ecm.CreateComponent(bodyLink, components::WorldLinearAcceleration()); - } + enableComponent(_ecm, bodyLink); + enableComponent(_ecm, bodyLink); }); server.AddSystem(testSystem.systemPtr); @@ -122,8 +138,8 @@ TEST_F(LiftDragTestFixture, VerifyVerticalForce) auto joint = _ecm.EntityByComponents(components::Joint(), components::Name(jointName)); - auto bodyLink = _ecm.EntityByComponents(components::Link(), - components::Name(bodyName)); + auto bodyLink = firstEntityFromScopedName(bodyName, _ecm); + auto linVelComp = _ecm.Component(bodyLink); @@ -151,10 +167,9 @@ TEST_F(LiftDragTestFixture, VerifyVerticalForce) 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 bladeLink = firstEntityFromScopedName(bladeName, _ecm); + auto bodyLink = firstEntityFromScopedName(bodyName, _ecm); + auto linVelComp = _ecm.Component(bodyLink); auto wrenchComp = @@ -213,3 +228,12 @@ TEST_F(LiftDragTestFixture, VerifyVerticalForce) EXPECT_GT(vertForce, 0); } } + +INSTANTIATE_TEST_SUITE_P( + LiftDragTests, VerticalForceParamFixture, + ::testing::Values( + VerticalForceTestParam{ + common::joinPaths("test", "worlds", "lift_drag.sdf"), "wing_1"}, + VerticalForceTestParam{ + common::joinPaths("test", "worlds", "lift_drag_nested_model.sdf"), + "wing_1::base_link"})); diff --git a/test/worlds/lift_drag_nested_model.sdf b/test/worlds/lift_drag_nested_model.sdf new file mode 100644 index 0000000000..1838aabc7a --- /dev/null +++ b/test/worlds/lift_drag_nested_model.sdf @@ -0,0 +1,149 @@ + + + + + + + + + + true + + + + + 0 0 1 + 100 100 + + + + + + + 0 0 1 + 100 100 + + + + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + + + + + 0 0 0.5 0 0 0 + + + 0.0 0 0 0.0 0.0 0.0 + + 0.465 + 0.0 + 0.0 + 0.006 + 0.0 + 0.470 + + 1.0 + + + 0.0 0 0 0.0 0.0 0.0 + + + 0.2 + + + + + 0.0 0 0 0.0 0.0 0.0 + + + 0.2 + + + + 0.5 0.2 0.2 1.0 + .421 0.225 0.0 1.0 + + + + + + + file://lift_drag_wing + wing_1 + 0 5.5 0.5 0.1 0 0 + + + + file://lift_drag_wing + wing_2 + 0 -5.5 0.5 -0.1 0 0 + + + + + world + body + 0.0 0.0 0.0 0.0 0.0 0.0 + + 1.0 0.0 0.0 + + 0.000000 + + + + + + body + wing_1::base_link + + + body + wing_2::base_link + + + + 0.1 + 4.000 + 20.0 + 0.00 + 10.0 + -0.2 + 1.0 + 0.0 + 0.0 5.0 0 + 10 + 1.2041 + -1 0 0 + 0 0 1 + wing_1::base_link + + + 0.1 + 4.000 + 20.0 + 0.00 + 10.0 + -0.2 + 1.0 + 0.0 + 0.0 -5.0 0 + 10 + 1.2041 + -1 0 0 + 0 0 1 + wing_2::base_link + + + + diff --git a/test/worlds/models/lift_drag_wing/model.config b/test/worlds/models/lift_drag_wing/model.config new file mode 100644 index 0000000000..5cb5ee1ec5 --- /dev/null +++ b/test/worlds/models/lift_drag_wing/model.config @@ -0,0 +1,7 @@ + + + lift_drag_wing + 1.0 + model.sdf + + diff --git a/test/worlds/models/lift_drag_wing/model.sdf b/test/worlds/models/lift_drag_wing/model.sdf new file mode 100644 index 0000000000..9e2d6e83c0 --- /dev/null +++ b/test/worlds/models/lift_drag_wing/model.sdf @@ -0,0 +1,36 @@ + + + + + + + 0.465 + 0.0 + 0.0 + 0.006 + 0.0 + 0.470 + + 1.0 + + + + + 1.0 10.0 0.01 + + + + + + + 1.0 10.0 0.01 + + + + 0.5 0.2 0.2 1.0 + .421 0.225 0.0 1.0 + + + + +