Skip to content

Commit

Permalink
bullet-featherstoneFix how links are flattened in ConstructSdfModel (#…
Browse files Browse the repository at this point in the history 
…562)

* Add test to show seg-fault with unsorted links

Signed-off-by: Shameek Ganguly <shameekarcanesphinx@gmail.com>
Signed-off-by: Steve Peters <scpeters@openrobotics.org>
Signed-off-by: Ian Chen <ichen@openrobotics.org>
Co-authored-by: Steve Peters <scpeters@openrobotics.org>
Co-authored-by: Ian Chen <ichen@openrobotics.org>
  • Loading branch information
@shameekganguly @scpeters @iche033
3 people authored Nov 8, 2023
1 parent 3b7a7c8 commit 1260ef2
Show file tree
Hide file tree
Showing 3 changed files with 134 additions and 64 deletions.
87 changes: 42 additions & 45 deletions bullet-featherstone/src/SDFFeatures.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -135,9 +135,12 @@ std::optional<Structure> ValidateModel(const ::sdf::Model &_sdfModel)
const ::sdf::Joint *rootJoint = nullptr;
bool fixed = false;
const std::string &rootModelName = _sdfModel.Name();
// a map of parent link to a vector of its child links
std::unordered_map<const ::sdf::Link*, std::vector<const ::sdf::Link*>>
linkTree;

const auto checkModel =
[&rootLink, &rootJoint, &fixed, &parentOf, &rootModelName](
[&rootLink, &rootJoint, &fixed, &parentOf, &rootModelName, &linkTree](
const ::sdf::Model &model) -> bool
{
for (std::size_t i = 0; i < model.JointCount(); ++i)
Expand Down Expand Up @@ -217,6 +220,10 @@ std::optional<Structure> ValidateModel(const ::sdf::Model &_sdfModel)
<< rootModelName << "] has multiple parent joints. That is not "
<< "supported by the gz-physics-bullet-featherstone plugin.\n";
}
if (parent != nullptr)
{
linkTree[parent].push_back(child);
}
}

return true;
Expand All @@ -231,11 +238,10 @@ std::optional<Structure> ValidateModel(const ::sdf::Model &_sdfModel)
return std::nullopt;
}

std::vector<const ::sdf::Link*> flatLinks;
std::unordered_map<const ::sdf::Link*, std::size_t> linkIndex;
const auto flattenLinks =
[&rootLink, &parentOf, &rootModelName, &flatLinks, &linkIndex](
const ::sdf::Model &model) -> bool
// Find root link in model and verify that there is only one root link in
// the model. Returns false if more than one root link is found
const auto findRootLink =
[&rootLink, &parentOf, &rootModelName](const ::sdf::Model &model) -> bool
{
for (std::size_t i = 0; i < model.LinkCount(); ++i)
{
Expand All @@ -254,23 +260,35 @@ std::optional<Structure> ValidateModel(const ::sdf::Model &_sdfModel)
}

rootLink = link;
continue;
}

linkIndex[link] = linkIndex.size();
flatLinks.push_back(link);
}

return true;
};

if (!flattenLinks(_sdfModel))
if (rootLink == nullptr && !findRootLink(_sdfModel))
{
// No root link was found in this model
return std::nullopt;
}

// find root link in nested models if one was not already found
for (std::size_t i = 0; i < _sdfModel.ModelCount(); ++i)
{
if (!flattenLinks(*_sdfModel.ModelByIndex(i)))
if (rootLink != nullptr)
{
break;
}
if (!findRootLink(*_sdfModel.ModelByIndex(i)))
{
return std::nullopt;
}
}

if (!rootLink)
{
gzerr << "No root link was found for model [" << _sdfModel.Name() << "\n";
return std::nullopt;
}

// The documentation for bullet does not mention whether parent links must
Expand All @@ -280,44 +298,23 @@ std::optional<Structure> ValidateModel(const ::sdf::Model &_sdfModel)
// come before their children in the array, and do not work correctly if that
// ordering is not held. Out of an abundance of caution we will assume that
// ordering is necessary.
for (std::size_t i = 0; i < flatLinks.size(); ++i)
std::vector<const ::sdf::Link*> flatLinks;
std::function<void(const ::sdf::Link *)> flattenLinkTree =
[&](const ::sdf::Link *link)
{
// Every element in flatLinks should have a parent if the earlier validation
// was done correctly.
if (parentOf.size() == 0)
if (link != rootLink)
{
break;
flatLinks.push_back(link);
}
const auto *parentJoint = parentOf.at(flatLinks[i]).joint;

const auto *parentLink =
_sdfModel.LinkByName(parentJoint->ParentName());
const auto p_index_it = linkIndex.find(parentLink);
if (p_index_it == linkIndex.end())
if (auto it = linkTree.find(link); it != linkTree.end())
{
// If the parent index cannot be found, that must mean the parent is the
// root link, so this link can go anywhere in the list as long as it is
// before its own children.
assert(parentLink == rootLink);
continue;
}

auto &p_index = p_index_it->second;
if (i < p_index)
{
// The current link is in front of its parent link in the array. We must
// swap their places.
std::swap(flatLinks[i], flatLinks[p_index]);
p_index = i;
linkIndex[flatLinks[p_index]] = p_index;
for (const auto &child_link : it->second)
{
flattenLinkTree(child_link);
}
}
}

if (!rootLink)
{
gzerr << "No root link was found for model [" << _sdfModel.Name() << "\n";
return std::nullopt;
}
};
flattenLinkTree(rootLink);

const auto &M = rootLink->Inertial().MassMatrix();
const auto mass = static_cast<btScalar>(M.Mass());
Expand Down
75 changes: 56 additions & 19 deletions test/common_test/world_features.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -67,23 +67,20 @@ class WorldFeaturesTest:
public: gz::plugin::Loader loader;
};

using GravityFeatures = gz::physics::FeatureList<
struct GravityFeatures : gz::physics::FeatureList<
gz::physics::GetEngineInfo,
gz::physics::Gravity,
gz::physics::sdf::ConstructSdfWorld,
gz::physics::LinkFrameSemantics,
gz::physics::GetModelFromWorld,
gz::physics::GetLinkFromModel,
gz::physics::ForwardStep
>;
> { };

using GravityFeaturesTestTypes =
::testing::Types<GravityFeatures>;
TYPED_TEST_SUITE(WorldFeaturesTest,
GravityFeatures,);
using WorldFeaturesTestGravity = WorldFeaturesTest<GravityFeatures>;

/////////////////////////////////////////////////
TYPED_TEST(WorldFeaturesTest, GravityFeatures)
TEST_F(WorldFeaturesTestGravity, GravityFeatures)
{
for (const std::string &name : this->pluginNames)
{
Expand Down Expand Up @@ -182,19 +179,59 @@ TYPED_TEST(WorldFeaturesTest, GravityFeatures)
}
}

struct WorldModelFeatureList
: gz::physics::FeatureList<GravityFeatures, gz::physics::WorldModelFeature,
gz::physics::RemoveEntities,
gz::physics::GetNestedModelFromModel,
gz::physics::sdf::ConstructSdfLink,
gz::physics::sdf::ConstructSdfJoint,
gz::physics::sdf::ConstructSdfModel,
gz::physics::sdf::ConstructSdfNestedModel,
gz::physics::ConstructEmptyLinkFeature,
gz::physics::ConstructEmptyNestedModelFeature
>
struct ConstructModelFeatures : gz::physics::FeatureList<
gz::physics::GetEngineInfo,
gz::physics::sdf::ConstructSdfWorld,
gz::physics::sdf::ConstructSdfModel,
gz::physics::GetModelFromWorld,
gz::physics::ForwardStep
> { };

using WorldFeaturesTestConstructModel =
WorldFeaturesTest<ConstructModelFeatures>;

/////////////////////////////////////////////////
TEST_F(WorldFeaturesTestConstructModel, ConstructModelUnsortedLinks)
{
};
for (const std::string &name : this->pluginNames)
{
std::cout << "Testing plugin: " << name << std::endl;
gz::plugin::PluginPtr plugin = this->loader.Instantiate(name);

auto engine =
gz::physics::RequestEngine3d<ConstructModelFeatures>::From(plugin);
ASSERT_NE(nullptr, engine);
EXPECT_TRUE(engine->GetName().find(this->PhysicsEngineName(name)) !=
std::string::npos);

sdf::Root root;
const sdf::Errors errors = root.Load(
gz::common::joinPaths(TEST_WORLD_DIR, "world_unsorted_links.sdf"));
EXPECT_TRUE(errors.empty()) << errors;
const sdf::World *sdfWorld = root.WorldByIndex(0);
ASSERT_NE(nullptr, sdfWorld);

// Per https://github.com/gazebosim/gz-physics/pull/562, there is a
// seg-fault in the bullet-featherstone implementation of ConstructSdfModel
// (called by ConstructSdfWorld). So loading the world successfully
// is enough for this test.
auto world = engine->ConstructWorld(*sdfWorld);
EXPECT_NE(nullptr, world);
}
}

struct WorldModelFeatureList : gz::physics::FeatureList<
GravityFeatures,
gz::physics::WorldModelFeature,
gz::physics::RemoveEntities,
gz::physics::GetNestedModelFromModel,
gz::physics::sdf::ConstructSdfLink,
gz::physics::sdf::ConstructSdfJoint,
gz::physics::sdf::ConstructSdfModel,
gz::physics::sdf::ConstructSdfNestedModel,
gz::physics::ConstructEmptyLinkFeature,
gz::physics::ConstructEmptyNestedModelFeature
> { };

class WorldModelTest : public WorldFeaturesTest<WorldModelFeatureList>
{
Expand Down
36 changes: 36 additions & 0 deletions test/common_test/worlds/world_unsorted_links.sdf
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,36 @@
<?xml version="1.0" ?>
<sdf version="1.6">
<world name="default">
<model name="unsorted_links">
<link name="child_linkC" />
<link name="parent_link" />
<link name="child_linkA" />
<link name="child_linkB" />
<joint name="world_joint" type="fixed">
<parent>world</parent>
<child>parent_link</child>
</joint>
<joint name="joint1" type="prismatic">
<parent>parent_link</parent>
<child>child_linkA</child>
<axis>
<xyz>0 0 1</xyz>
</axis>
</joint>
<joint name="joint2" type="prismatic">
<parent>child_linkA</parent>
<child>child_linkB</child>
<axis>
<xyz>0 0 1</xyz>
</axis>
</joint>
<joint name="joint3" type="prismatic">
<parent>child_linkB</parent>
<child>child_linkC</child>
<axis>
<xyz>0 0 1</xyz>
</axis>
</joint>
</model>
</world>
</sdf>

0 comments on commit 1260ef2

Please sign in to comment.