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
+
+
+
+
+