From 05ae32126155d2bcaa57c8177530ff983f236e71 Mon Sep 17 00:00:00 2001 From: "Addisu Z. Taddese" Date: Thu, 24 Jun 2021 18:10:16 -0500 Subject: [PATCH] Support FreeGroup features for nested models Signed-off-by: Addisu Z. Taddese --- dartsim/src/EntityManagementFeatures.cc | 6 +- dartsim/src/FreeGroupFeatures.cc | 87 +++++++- dartsim/src/FreeGroupFeatures_TEST.cc | 194 ++++++++++++++++++ dartsim/src/SDFFeatures_TEST.cc | 4 +- dartsim/worlds/world_with_nested_model.sdf | 8 + tpe/lib/src/Model.cc | 2 +- tpe/plugin/src/FreeGroupFeatures.cc | 63 ++++-- tpe/plugin/src/FreeGroupFeatures_TEST.cc | 194 ++++++++++++++++++ tpe/plugin/src/SDFFeatures_TEST.cc | 1 + tpe/plugin/src/SimulationFeatures_TEST.cc | 187 ++++++++++++++++- tpe/plugin/worlds/world_with_nested_model.sdf | 48 +++++ 11 files changed, 767 insertions(+), 27 deletions(-) create mode 100644 dartsim/src/FreeGroupFeatures_TEST.cc create mode 100644 tpe/plugin/src/FreeGroupFeatures_TEST.cc create mode 100644 tpe/plugin/worlds/world_with_nested_model.sdf diff --git a/dartsim/src/EntityManagementFeatures.cc b/dartsim/src/EntityManagementFeatures.cc index 57a8a5ac6..ca517de19 100644 --- a/dartsim/src/EntityManagementFeatures.cc +++ b/dartsim/src/EntityManagementFeatures.cc @@ -195,7 +195,11 @@ Identity EntityManagementFeatures::GetEngineOfWorld( std::size_t EntityManagementFeatures::GetModelCount( const Identity &_worldID) const { - return this->ReferenceInterface(_worldID)->getNumSkeletons(); + // dart::simulation::World::getNumSkeletons returns all the skeletons in the + // world, including nested ones. We use the size of the indexInContainerToID + // vector associated with the _worldID to determine the number of models + // that are direct children of the world. + return this->models.indexInContainerToID.at(_worldID).size(); } ///////////////////////////////////////////////// diff --git a/dartsim/src/FreeGroupFeatures.cc b/dartsim/src/FreeGroupFeatures.cc index 4d07f06b3..c802f1bca 100644 --- a/dartsim/src/FreeGroupFeatures.cc +++ b/dartsim/src/FreeGroupFeatures.cc @@ -19,6 +19,7 @@ #include #include +#include namespace ignition { namespace physics { @@ -28,13 +29,13 @@ namespace dartsim { Identity FreeGroupFeatures::FindFreeGroupForModel( const Identity &_modelID) const { + const auto modelInfo = this->models.at(_modelID); // Verify that the model qualifies as a FreeGroup - const dart::dynamics::ConstSkeletonPtr &skeleton = - this->models.at(_modelID)->model; + const dart::dynamics::ConstSkeletonPtr &skeleton = modelInfo->model; // If there are no bodies at all in this model, then the FreeGroup functions // will not work properly, so we'll just reject these cases. - if (skeleton->getNumBodyNodes() == 0) + if (skeleton->getNumBodyNodes() == 0 && modelInfo->nestedModels.size() == 0) return this->GenerateInvalidId(); // Verify that all root joints are FreeJoints @@ -47,6 +48,29 @@ Identity FreeGroupFeatures::FindFreeGroupForModel( } } + for (const auto &nestedModel : modelInfo->nestedModels) + { + // Check if any of the nested models have valid free groups by recursively + // calling FindFreeGroupForModel on all nested models that either have + // BodyNodes or nested models. A nested model that doesn't have any + // BodyNodes or nested models should not exist per the SDFormat spec. + // However, BodyNodes may be moved out of a skeleton when creating joints + // leaving the skeleton without any BodyNodes. In this case, we skip the + // skeleton because the free group of the BodyNodes it used to contain will + // be determined when calling FindFreeGroupForModel for the model that now + // contains them after the move. + auto nestedModelInfo = this->models.at(nestedModel); + if (nestedModelInfo->model->getNumBodyNodes() > 0 || + nestedModelInfo->nestedModels.size() > 0) + { + if (!this->FindFreeGroupForModel( + this->GenerateIdentity(nestedModel, nestedModelInfo))) + { + return this->GenerateInvalidId(); + } + } + } + // TODO(MXG): When the dartsim plugin supports closed-loop constraints, verify // that this model is not attached to the world or any other models. If it's // attached to anything external, then we should return an invalid identity. @@ -95,9 +119,31 @@ FreeGroupFeatures::FreeGroupInfo FreeGroupFeatures::GetCanonicalInfo( const auto model_it = this->models.idToObject.find(_groupID); if (model_it != this->models.idToObject.end()) { - return FreeGroupInfo{ - model_it->second->model->getRootBodyNode(), - model_it->second->model.get()}; + const auto &modelInfo = model_it->second; + if (modelInfo->model->getNumBodyNodes() > 0) + { + return FreeGroupInfo{ + modelInfo->model->getRootBodyNode(), + modelInfo->model.get()}; + } + else + { + // If the skeleton doesn't have any BodyNodes, we recursively search for + // a root BodyNode in the first nested model that has a non-zero number of + // BodyNodes. Since all root joints have been verified to be FreeJoints in + // FindFreeGroupForModel, we are guaranteed that the BodyNode found in + // this way is the root link for the free group. + for (const auto &nestedModel : modelInfo->nestedModels) + { + auto freeGroupInfo = + this->GetCanonicalInfo(this->GenerateIdentity(nestedModel)); + if (freeGroupInfo.model != nullptr || freeGroupInfo.link != nullptr) + return freeGroupInfo; + } + + // Error + return {}; + } } return FreeGroupInfo{this->links.at(_groupID)->link, nullptr}; @@ -111,22 +157,45 @@ void FreeGroupFeatures::SetFreeGroupWorldPose( const FreeGroupInfo &info = GetCanonicalInfo(_groupID); if (!info.model) { - static_cast(info.link->getParentJoint()) + if (nullptr != info.link) + { + static_cast(info.link->getParentJoint()) ->setTransform(_pose); + } + else + { + ignerr << "No link for free group with id [" << _groupID.id + << "] found. SetFreeGroupWorldPose failed." << std::endl; + } return; } - const Eigen::Isometry3d tf_change = + const Eigen::Isometry3d tfChange = _pose * info.link->getWorldTransform().inverse(); for (std::size_t i = 0; i < info.model->getNumTrees(); ++i) { auto *bn = info.model->getRootBodyNode(i); - const Eigen::Isometry3d new_tf = tf_change * bn->getTransform(); + const Eigen::Isometry3d new_tf = tfChange * bn->getTransform(); static_cast(bn->getParentJoint()) ->setTransform(new_tf); } + + auto modelInfo = this->models.at(_groupID); + for (const auto &nestedModel : modelInfo->nestedModels) + { + auto nestedModelIdentity = this->GenerateIdentity(nestedModel); + const FreeGroupInfo &nestedInfo = GetCanonicalInfo(nestedModelIdentity); + // If nestedInfo.link is a nullptr, we skip this model because means the + // BodyNodes in this skeleton have been moved to another skeleton and their + // pose update will be handled there. + if (nullptr != nestedInfo.link && nestedInfo.link != info.link) + { + this->SetFreeGroupWorldPose(nestedModelIdentity, + tfChange * nestedInfo.link->getTransform()); + } + } } ///////////////////////////////////////////////// diff --git a/dartsim/src/FreeGroupFeatures_TEST.cc b/dartsim/src/FreeGroupFeatures_TEST.cc new file mode 100644 index 000000000..9cfd54f8a --- /dev/null +++ b/dartsim/src/FreeGroupFeatures_TEST.cc @@ -0,0 +1,194 @@ +/* + * Copyright (C) 2021 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * +*/ + +#include + +#include + +#include +#include +#include +#include +#include +#include + +#include "ignition/physics/FrameSemantics.hh" +#include "ignition/physics/FreeGroup.hh" +#include "ignition/physics/GetEntities.hh" +#include "ignition/physics/RequestEngine.hh" +#include "ignition/physics/sdf/ConstructModel.hh" +#include "ignition/physics/sdf/ConstructNestedModel.hh" +#include "ignition/physics/sdf/ConstructWorld.hh" +#include "test/Utils.hh" + +struct TestFeatureList : ignition::physics::FeatureList< + ignition::physics::sdf::ConstructSdfWorld, + ignition::physics::sdf::ConstructSdfModel, + ignition::physics::sdf::ConstructSdfNestedModel, + ignition::physics::GetWorldFromEngine, + ignition::physics::GetModelFromWorld, + ignition::physics::GetNestedModelFromModel, + ignition::physics::GetLinkFromModel, + ignition::physics::LinkFrameSemantics, + ignition::physics::FindFreeGroupFeature, + ignition::physics::SetFreeGroupWorldPose > { }; + +using World = ignition::physics::World3d; +using WorldPtr = ignition::physics::World3dPtr; +using ModelPtr = ignition::physics::Model3dPtr; +using LinkPtr = ignition::physics::Link3dPtr; + +///////////////////////////////////////////////// +auto LoadEngine() +{ + ignition::plugin::Loader loader; + loader.LoadLib(dartsim_plugin_LIB); + + ignition::plugin::PluginPtr dartsim = + loader.Instantiate("ignition::physics::dartsim::Plugin"); + + auto engine = + ignition::physics::RequestEngine3d::From(dartsim); + return engine; +} + +///////////////////////////////////////////////// +WorldPtr LoadWorld(const std::string &_world) +{ + auto engine = LoadEngine(); + EXPECT_NE(nullptr, engine); + if (nullptr == engine) + { + return nullptr; + } + + sdf::Root root; + const sdf::Errors &errors = root.Load(_world); + EXPECT_EQ(0u, errors.size()) << errors; + + EXPECT_EQ(1u, root.WorldCount()); + const sdf::World *sdfWorld = root.WorldByIndex(0); + EXPECT_NE(nullptr, sdfWorld); + + auto world = engine->ConstructWorld(*sdfWorld); + EXPECT_NE(nullptr, world); + + return world; +} + +ModelPtr GetModelFromAbsoluteName(const WorldPtr &_world, + const std::string &_absoluteName) +{ + std::vector names = + ignition::common::split(_absoluteName, sdf::kSdfScopeDelimiter); + if (names.empty()) + { + return nullptr; + } + + auto currentModel = _world->GetModel(names.front()); + for (std::size_t i = 1; i < names.size(); ++i) + { + if (nullptr == currentModel) + return nullptr; + currentModel = currentModel->GetNestedModel(names[i]); + } + return currentModel; +} + +TEST(FreeGroupFeatures, NestedFreeGroup) +{ + WorldPtr world = LoadWorld(TEST_WORLD_DIR "world_with_nested_model.sdf"); + ASSERT_NE(nullptr, world); + + auto checkFreeGroupForModel = [&](const std::string &_modelName) + { + auto model = GetModelFromAbsoluteName(world, _modelName); + if (nullptr == model) + return testing::AssertionFailure() << "Model could not be found"; + auto freeGroup = model->FindFreeGroup(); + if (nullptr == freeGroup) + return testing::AssertionFailure() << "Freegroup could not be found"; + if (nullptr == freeGroup) + return testing::AssertionFailure() << "CanonicalLink could not be found"; + return testing::AssertionSuccess(); + }; + + EXPECT_TRUE(checkFreeGroupForModel("parent_model")); + // Expect false because the link in nested_model is referenced by a joint. + EXPECT_FALSE(checkFreeGroupForModel("parent_model::nested_model")); + EXPECT_TRUE(checkFreeGroupForModel("parent_model::nested_model2")); + EXPECT_TRUE(checkFreeGroupForModel("parent_model::nested_model3")); +} + +TEST(FreeGroupFeatures, NestedFreeGroupSetWorldPose) +{ + WorldPtr world = LoadWorld(TEST_WORLD_DIR "world_with_nested_model.sdf"); + ASSERT_NE(nullptr, world); + + ignition::math::Pose3d parentModelNewPose(0, 0, 2, 0, 0, 0); + { + auto parentModel = world->GetModel("parent_model"); + ASSERT_NE(nullptr, parentModel); + auto freeGroup = parentModel->FindFreeGroup(); + ASSERT_NE(nullptr, freeGroup); + + freeGroup->SetWorldPose( + ignition::math::eigen3::convert(parentModelNewPose)); + + auto link1 = parentModel->GetLink("link1"); + ASSERT_NE(nullptr, link1); + auto frameData = link1->FrameDataRelativeToWorld(); + EXPECT_EQ(parentModelNewPose, + ignition::math::eigen3::convert(frameData.pose)); + } + { + auto nestedModel = + GetModelFromAbsoluteName(world, "parent_model::nested_model"); + ASSERT_NE(nullptr, nestedModel); + auto nestedLink1 = nestedModel->GetLink("nested_link1"); + ASSERT_NE(nullptr, nestedLink1); + auto frameData = nestedLink1->FrameDataRelativeToWorld(); + // Poses from SDF + ignition::math::Pose3d nestedModelPose(1, 2, 2, 0, 0, 0); + ignition::math::Pose3d nestedLinkPose(3, 1, 1, 0, 0, IGN_PI_2); + + ignition::math::Pose3d nestedLinkExpectedPose = + parentModelNewPose * nestedModelPose * nestedLinkPose; + + EXPECT_EQ(nestedLinkExpectedPose, + ignition::math::eigen3::convert(frameData.pose)); + } + { + auto parentModel = world->GetModel("parent_model2"); + ASSERT_NE(nullptr, parentModel); + auto freeGroup = parentModel->FindFreeGroup(); + ASSERT_NE(nullptr, freeGroup); + + freeGroup->SetWorldPose( + ignition::math::eigen3::convert(parentModelNewPose)); + + auto grandChild = GetModelFromAbsoluteName( + world, "parent_model2::child_model::grand_child_model"); + ASSERT_NE(nullptr, grandChild); + auto link1 = grandChild->GetLink("link1"); + ASSERT_NE(nullptr, link1); + auto frameData = link1->FrameDataRelativeToWorld(); + EXPECT_EQ(parentModelNewPose, + ignition::math::eigen3::convert(frameData.pose)); + } +} diff --git a/dartsim/src/SDFFeatures_TEST.cc b/dartsim/src/SDFFeatures_TEST.cc index 320bbed87..a195782cb 100644 --- a/dartsim/src/SDFFeatures_TEST.cc +++ b/dartsim/src/SDFFeatures_TEST.cc @@ -540,7 +540,7 @@ TEST_P(SDFFeatures_TEST, WorldWithNestedModel) WorldPtr world = this->LoadWorld(TEST_WORLD_DIR "/world_with_nested_model.sdf"); ASSERT_NE(nullptr, world); - EXPECT_EQ(4u, world->GetModelCount()); + EXPECT_EQ(2u, world->GetModelCount()); dart::simulation::WorldPtr dartWorld = world->GetDartsimWorld(); ASSERT_NE(nullptr, dartWorld); @@ -599,7 +599,7 @@ TEST_P(SDFFeatures_TEST, WorldWithNestedModelJointToWorld) WorldPtr world = this->LoadWorld( TEST_WORLD_DIR "/world_with_nested_model_joint_to_world.sdf"); ASSERT_NE(nullptr, world); - EXPECT_EQ(2u, world->GetModelCount()); + EXPECT_EQ(1u, world->GetModelCount()); dart::simulation::WorldPtr dartWorld = world->GetDartsimWorld(); ASSERT_NE(nullptr, dartWorld); diff --git a/dartsim/worlds/world_with_nested_model.sdf b/dartsim/worlds/world_with_nested_model.sdf index 6ba6c4723..0b71ad0d7 100644 --- a/dartsim/worlds/world_with_nested_model.sdf +++ b/dartsim/worlds/world_with_nested_model.sdf @@ -63,5 +63,13 @@ + + + + + + + + diff --git a/tpe/lib/src/Model.cc b/tpe/lib/src/Model.cc index 4adc5b3d6..f14c4a694 100644 --- a/tpe/lib/src/Model.cc +++ b/tpe/lib/src/Model.cc @@ -70,7 +70,7 @@ Entity &Model::AddLink() { std::size_t linkId = Entity::GetNextId(); - if (this->GetChildren().empty()) + if (this->GetLinkCount() == 0) { this->dataPtr->firstLinkId = linkId; this->dataPtr->canonicalLinkId = linkId; diff --git a/tpe/plugin/src/FreeGroupFeatures.cc b/tpe/plugin/src/FreeGroupFeatures.cc index d62562bb7..86fb30929 100644 --- a/tpe/plugin/src/FreeGroupFeatures.cc +++ b/tpe/plugin/src/FreeGroupFeatures.cc @@ -36,8 +36,8 @@ Identity FreeGroupFeatures::FindFreeGroupForModel( auto it = this->models.find(_modelID.id); if (it == this->models.end() || it->second == nullptr) return this->GenerateInvalidId(); - // if there are no links in this model, then the FreeGroup functions - // will not work properly; need to reject this case. + // if there are no links or nested modelsin this model, then the FreeGroup + // functions will not work properly; need to reject this case. if (it->second->model->GetChildCount() == 0) return this->GenerateInvalidId(); return this->GenerateIdentity(_modelID.id, it->second); @@ -54,24 +54,62 @@ Identity FreeGroupFeatures::FindFreeGroupForLink( } ///////////////////////////////////////////////// -Identity FreeGroupFeatures::GetFreeGroupRootLink(const Identity &_groupID) const +/// Helper function to find a model's root link by recursively searching for it +/// in nested models if the model has no links. +static tpelib::Link *FindModelRootLink(tpelib::Model *_model) { + if (nullptr == _model) + return nullptr; + // assume no canonical link for now - // assume groupID ~= modelID - const auto modelIt = this->models.find(_groupID.id); - if (modelIt != this->models.end() && modelIt->second != nullptr) + if (_model->GetLinkCount() > 0) { // assume canonical link is the first link in model // note the canonical link of a free group is renamed to root link in // ign-physics4. The canonical link / root link of a free group can be // different from the canonical link of a model. // Here we treat them the same and return the model's canonical link - tpelib::Entity &link = modelIt->second->model->GetCanonicalLink(); - auto linkPtr = std::make_shared(); - linkPtr->link = static_cast(&link); - return this->GenerateIdentity(link.GetId(), linkPtr); + return static_cast(&_model->GetCanonicalLink()); + } + else + { + // If the model doesn't have any links, we recursively search for the root + // link in the nested models. + for (size_t i = 0; i < _model->GetChildCount(); ++i) + { + auto *rootLink = FindModelRootLink( + static_cast(&_model->GetChildByIndex(i))); + if (nullptr != rootLink) + return rootLink; + } + } + return nullptr; +} + +///////////////////////////////////////////////// +Identity FreeGroupFeatures::GetFreeGroupRootLink(const Identity &_groupID) const +{ + // assume no canonical link for now + // assume groupID ~= modelID + const auto modelIt = this->models.find(_groupID.id); + if (modelIt != this->models.end() && modelIt->second != nullptr) + { + auto *rootLink = FindModelRootLink(modelIt->second->model); + if (nullptr != rootLink) + { + auto linkPtr = std::make_shared(); + linkPtr->link = rootLink; + return this->GenerateIdentity(rootLink->GetId(), linkPtr); + } + else { + return this->GenerateInvalidId(); + } } - return this->GenerateIdentity(_groupID.id, this->links.at(_groupID.id)); + auto linkIt = this->links.find(_groupID.id); + if (linkIt != this->links.end()) + return this->GenerateIdentity(_groupID.id, linkIt->second); + else + return this->GenerateInvalidId(); } ///////////////////////////////////////////////// @@ -88,8 +126,7 @@ void FreeGroupFeatures::SetFreeGroupWorldPose( { if (modelIt->second != nullptr) { - tpelib::Entity &linkEnt = modelIt->second->model->GetCanonicalLink(); - link = dynamic_cast(&linkEnt); + link = FindModelRootLink(modelIt->second->model); } } else diff --git a/tpe/plugin/src/FreeGroupFeatures_TEST.cc b/tpe/plugin/src/FreeGroupFeatures_TEST.cc new file mode 100644 index 000000000..5cae6b2a5 --- /dev/null +++ b/tpe/plugin/src/FreeGroupFeatures_TEST.cc @@ -0,0 +1,194 @@ +/* + * Copyright (C) 2021 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * +*/ + +#include + +#include + +#include +#include +#include +#include +#include +#include + +#include "ignition/physics/FrameSemantics.hh" +#include "ignition/physics/FreeGroup.hh" +#include "ignition/physics/GetEntities.hh" +#include "ignition/physics/RequestEngine.hh" +#include "ignition/physics/sdf/ConstructModel.hh" +#include "ignition/physics/sdf/ConstructNestedModel.hh" +#include "ignition/physics/sdf/ConstructWorld.hh" +#include "test/Utils.hh" + +struct TestFeatureList : ignition::physics::FeatureList< + ignition::physics::sdf::ConstructSdfWorld, + ignition::physics::sdf::ConstructSdfModel, + ignition::physics::sdf::ConstructSdfNestedModel, + ignition::physics::GetWorldFromEngine, + ignition::physics::GetModelFromWorld, + ignition::physics::GetNestedModelFromModel, + ignition::physics::GetLinkFromModel, + ignition::physics::LinkFrameSemantics, + ignition::physics::FindFreeGroupFeature, + ignition::physics::SetFreeGroupWorldPose > { }; + +using World = ignition::physics::World3d; +using WorldPtr = ignition::physics::World3dPtr; +using ModelPtr = ignition::physics::Model3dPtr; +using LinkPtr = ignition::physics::Link3dPtr; + +///////////////////////////////////////////////// +auto LoadEngine() +{ + ignition::plugin::Loader loader; + loader.LoadLib(tpe_plugin_LIB); + + ignition::plugin::PluginPtr tpe = + loader.Instantiate("ignition::physics::tpeplugin::Plugin"); + + auto engine = + ignition::physics::RequestEngine3d::From(tpe); + + return engine; +} + +///////////////////////////////////////////////// +WorldPtr LoadWorld(const std::string &_world) +{ + auto engine = LoadEngine(); + EXPECT_NE(nullptr, engine); + if (nullptr == engine) + { + return nullptr; + } + + sdf::Root root; + const sdf::Errors &errors = root.Load(_world); + EXPECT_EQ(0u, errors.size()) << errors; + + EXPECT_EQ(1u, root.WorldCount()); + const sdf::World *sdfWorld = root.WorldByIndex(0); + EXPECT_NE(nullptr, sdfWorld); + + auto world = engine->ConstructWorld(*sdfWorld); + EXPECT_NE(nullptr, world); + + return world; +} + +ModelPtr GetModelFromAbsoluteName(const WorldPtr &_world, + const std::string &_absoluteName) +{ + std::vector names = + ignition::common::split(_absoluteName, sdf::kSdfScopeDelimiter); + if (names.empty()) + { + return nullptr; + } + + auto currentModel = _world->GetModel(names.front()); + for (std::size_t i = 1; i < names.size(); ++i) + { + if (nullptr == currentModel) + return nullptr; + currentModel = currentModel->GetNestedModel(names[i]); + } + return currentModel; +} + +TEST(FreeGroupFeatures, NestedFreeGroup) +{ + WorldPtr world = LoadWorld(TEST_WORLD_DIR "world_with_nested_model.sdf"); + ASSERT_NE(nullptr, world); + + auto checkFreeGroupForModel = [&](const std::string &_modelName) + { + auto model = GetModelFromAbsoluteName(world, _modelName); + if (nullptr == model) + return testing::AssertionFailure() << "Model could not be found"; + auto freeGroup = model->FindFreeGroup(); + if (nullptr == freeGroup) + return testing::AssertionFailure() << "Freegroup could not be found"; + if (nullptr == freeGroup) + return testing::AssertionFailure() << "CanonicalLink could not be found"; + return testing::AssertionSuccess(); + }; + + EXPECT_TRUE(checkFreeGroupForModel("parent_model")); + EXPECT_TRUE(checkFreeGroupForModel("parent_model::nested_model")); + EXPECT_TRUE(checkFreeGroupForModel("parent_model::nested_model2")); + EXPECT_TRUE(checkFreeGroupForModel("parent_model::nested_model3")); +} + +TEST(FreeGroupFeatures, NestedFreeGroupSetWorldPose) +{ + WorldPtr world = LoadWorld(TEST_WORLD_DIR "world_with_nested_model.sdf"); + ASSERT_NE(nullptr, world); + + ignition::math::Pose3d parentModelNewPose(0, 0, 2, 0, 0, 0); + { + auto parentModel = world->GetModel("parent_model"); + ASSERT_NE(nullptr, parentModel); + auto freeGroup = parentModel->FindFreeGroup(); + ASSERT_NE(nullptr, freeGroup); + + freeGroup->SetWorldPose( + ignition::math::eigen3::convert(parentModelNewPose)); + + auto link1 = parentModel->GetLink("link1"); + ASSERT_NE(nullptr, link1); + auto frameData = link1->FrameDataRelativeToWorld(); + EXPECT_EQ(parentModelNewPose, + ignition::math::eigen3::convert(frameData.pose)); + } + { + auto nestedModel = + GetModelFromAbsoluteName(world, "parent_model::nested_model"); + ASSERT_NE(nullptr, nestedModel); + auto nestedLink1 = nestedModel->GetLink("nested_link1"); + ASSERT_NE(nullptr, nestedLink1); + auto frameData = nestedLink1->FrameDataRelativeToWorld(); + // Poses from SDF + ignition::math::Pose3d nestedModelPose(1, 2, 2, 0, 0, 0); + ignition::math::Pose3d nestedLinkPose(3, 1, 1, 0, 0, IGN_PI_2); + + ignition::math::Pose3d nestedLinkExpectedPose = + parentModelNewPose * nestedModelPose * nestedLinkPose; + + EXPECT_EQ(nestedLinkExpectedPose, + ignition::math::eigen3::convert(frameData.pose)); + } + { + auto parentModel = world->GetModel("parent_model2"); + ASSERT_NE(nullptr, parentModel); + auto freeGroup = parentModel->FindFreeGroup(); + ASSERT_NE(nullptr, freeGroup); + + freeGroup->SetWorldPose( + ignition::math::eigen3::convert(parentModelNewPose)); + + auto grandChild = GetModelFromAbsoluteName( + world, "parent_model2::child_model::grand_child_model"); + ASSERT_NE(nullptr, grandChild); + auto link1 = grandChild->GetLink("link1"); + ASSERT_NE(nullptr, link1); + auto frameData = link1->FrameDataRelativeToWorld(); + EXPECT_EQ(parentModelNewPose, + ignition::math::eigen3::convert(frameData.pose)); + } +} diff --git a/tpe/plugin/src/SDFFeatures_TEST.cc b/tpe/plugin/src/SDFFeatures_TEST.cc index 78cd97249..649d57118 100644 --- a/tpe/plugin/src/SDFFeatures_TEST.cc +++ b/tpe/plugin/src/SDFFeatures_TEST.cc @@ -366,6 +366,7 @@ TEST(SDFFeatures_TEST, NestedModel) ASSERT_NE(nullptr, tpeWorld); ASSERT_EQ(1u, tpeWorld->GetChildCount()); + EXPECT_EQ(1u, world.GetModelCount()); // check top level model ignition::physics::tpelib::Entity &model = diff --git a/tpe/plugin/src/SimulationFeatures_TEST.cc b/tpe/plugin/src/SimulationFeatures_TEST.cc index 9901d09b1..04df53442 100644 --- a/tpe/plugin/src/SimulationFeatures_TEST.cc +++ b/tpe/plugin/src/SimulationFeatures_TEST.cc @@ -30,9 +30,15 @@ #include #include #include +#include +#include +#include +#include #include #include +#include +#include #include #include @@ -41,19 +47,28 @@ #include "FreeGroupFeatures.hh" #include "ShapeFeatures.hh" #include "SimulationFeatures.hh" +#include "World.hh" struct TestFeatureList : ignition::physics::FeatureList< ignition::physics::tpeplugin::SimulationFeatureList, ignition::physics::tpeplugin::ShapeFeatureList, ignition::physics::tpeplugin::EntityManagementFeatureList, ignition::physics::tpeplugin::FreeGroupFeatureList, + ignition::physics::tpeplugin::RetrieveWorld, ignition::physics::GetContactsFromLastStepFeature, ignition::physics::LinkFrameSemantics, ignition::physics::GetModelBoundingBox, - ignition::physics::sdf::ConstructSdfWorld + ignition::physics::sdf::ConstructSdfWorld, + ignition::physics::sdf::ConstructSdfModel, + ignition::physics::sdf::ConstructSdfNestedModel, + ignition::physics::sdf::ConstructSdfLink, + ignition::physics::sdf::ConstructSdfCollision > { }; +using TestEnginePtr = ignition::physics::Engine3dPtr; using TestWorldPtr = ignition::physics::World3dPtr; +using TestModelPtr = ignition::physics::Model3dPtr; +using TestLinkPtr = ignition::physics::Link3dPtr; using TestShapePtr = ignition::physics::Shape3dPtr; using ContactPoint = ignition::physics::World3d::ContactPoint; @@ -91,6 +106,125 @@ std::unordered_set LoadWorlds( return worlds; } +///////////////////////////////////////////////// +static ignition::math::Pose3d ResolveSdfPose( + const ::sdf::SemanticPose &_semPose) +{ + ignition::math::Pose3d pose; + ::sdf::Errors errors = _semPose.Resolve(pose); + EXPECT_TRUE(errors.empty()) << errors; + return pose; +} + +std::unordered_set LoadWorldsPiecemeal( + const std::string &_library, const std::string &_world) +{ + ignition::plugin::Loader loader; + loader.LoadLib(_library); + + const std::set pluginNames = + ignition::physics::FindFeatures3d::From(loader); + + EXPECT_EQ(1u, pluginNames.size()); + + std::unordered_set worlds; + for (const std::string &name : pluginNames) + { + ignition::plugin::PluginPtr plugin = loader.Instantiate(name); + + igndbg << " -- Plugin name: " << name << std::endl; + + auto engine = + ignition::physics::RequestEngine3d::From(plugin); + EXPECT_NE(nullptr, engine); + + sdf::Root root; + const sdf::Errors &errors = root.Load(_world); + EXPECT_EQ(0u, errors.size()) << errors; + + EXPECT_EQ(1u, root.WorldCount()); + const sdf::World *sdfWorld = root.WorldByIndex(0); + EXPECT_NE(nullptr, sdfWorld); + + sdf::World newWorld; + newWorld.SetName(sdfWorld->Name()); + newWorld.SetGravity(sdfWorld->Gravity()); + auto world = engine->ConstructWorld(newWorld); + if (nullptr == world) + continue; + + std::unordered_map modelMap; + std::unordered_map linkMap; + + auto createModel = [&](const sdf::Model *_model, + const sdf::Model *_parentModel = nullptr) { + sdf::Model newSdfModel; + newSdfModel.SetName(_model->Name()); + newSdfModel.SetRawPose(ResolveSdfPose(_model->SemanticPose())); + newSdfModel.SetStatic(_model->Static()); + newSdfModel.SetSelfCollide(_model->SelfCollide()); + + TestModelPtr newModel; + if (nullptr != _parentModel) + { + auto it = modelMap.find(_parentModel); + ASSERT_TRUE(it != modelMap.end()); + newModel = it->second->ConstructNestedModel(newSdfModel); + } + else + { + newModel = world->ConstructModel(newSdfModel); + } + + EXPECT_NE(nullptr, newModel); + if (nullptr != newModel) + { + modelMap[_model] = newModel; + } + }; + + for (uint64_t i = 0; i < sdfWorld->ModelCount(); ++i) + { + const auto *model = sdfWorld->ModelByIndex(i); + createModel(model); + for (uint64_t nestedInd = 0; nestedInd < model->ModelCount(); ++nestedInd) + { + createModel(model->ModelByIndex(nestedInd), model); + } + } + + for (auto [sdfModel, physModel] : modelMap) + { + for (uint64_t li = 0; li < sdfModel->LinkCount(); ++li) + { + const auto *link = sdfModel->LinkByIndex(li); + sdf::Link newSdfLink; + newSdfLink.SetName(link->Name()); + newSdfLink.SetRawPose(ResolveSdfPose(link->SemanticPose())); + newSdfLink.SetInertial(link->Inertial()); + + auto newLink = physModel->ConstructLink(newSdfLink); + EXPECT_NE(nullptr, newLink); + if (nullptr == newLink) + return worlds; + + linkMap[link] = newLink; + } + } + + for (auto [sdfLink, physLink] : linkMap) + { + for (uint64_t ci = 0; ci < sdfLink->CollisionCount(); ++ci) + { + physLink->ConstructCollision(*sdfLink->CollisionByIndex(ci)); + } + } + worlds.insert(world); + } + + return worlds; +} + /// \brief Step forward in a world /// \param[in] _world The world to step in /// \param[in] _firstTime Whether this is the very first time this world is @@ -281,6 +415,57 @@ TEST_P(SimulationFeatures_TEST, FreeGroup) } } +TEST_P(SimulationFeatures_TEST, NestedFreeGroup) +{ + const std::string library = GetParam(); + if (library.empty()) + return; + + auto worlds = + LoadWorldsPiecemeal(library, TEST_WORLD_DIR "/nested_model.world"); + + for (const auto &world : worlds) + { + auto tpeWorld = world->GetTpeLibWorld(); + ASSERT_NE(nullptr, tpeWorld); + + // model free group test + auto model = world->GetModel("model"); + ASSERT_NE(nullptr, model); + auto freeGroup = model->FindFreeGroup(); + ASSERT_NE(nullptr, freeGroup); + ASSERT_NE(nullptr, freeGroup->RootLink()); + + ignition::math::Pose3d newPose(1, 1, 0, 0, 0, 0); + freeGroup->SetWorldPose(ignition::math::eigen3::convert(newPose)); + + { + auto link = model->GetLink("link"); + ASSERT_NE(nullptr, link); + auto frameData = link->FrameDataRelativeToWorld(); + EXPECT_EQ(newPose, + ignition::math::eigen3::convert(frameData.pose)); + } + { + auto nestedModel = model->GetNestedModel("nested_model"); + ASSERT_NE(nullptr, nestedModel); + auto nestedLink = nestedModel->GetLink("nested_link"); + ASSERT_NE(nullptr, nestedLink); + + // Poses from SDF + ignition::math::Pose3d nestedModelPose(1, 2, 2, 0, 0, 0); + ignition::math::Pose3d nestedLinkPose(3, 1, 1, 0, 0, IGN_PI_2); + + ignition::math::Pose3d nestedLinkExpectedPose = + newPose * nestedModelPose * nestedLinkPose; + + EXPECT_EQ(nestedLinkExpectedPose, + ignition::math::eigen3::convert( + nestedLink->FrameDataRelativeToWorld().pose)); + } + } +} + TEST_P(SimulationFeatures_TEST, CollideBitmasks) { const std::string library = GetParam(); diff --git a/tpe/plugin/worlds/world_with_nested_model.sdf b/tpe/plugin/worlds/world_with_nested_model.sdf new file mode 100644 index 000000000..57653b44e --- /dev/null +++ b/tpe/plugin/worlds/world_with_nested_model.sdf @@ -0,0 +1,48 @@ + + + + + + 1 2 2 0 0 0 + + 3 1 1 0 0 1.5707 + + + + 2 + + + + + + 0 1 0 1.5707 0 0 + + + + 2 + + + + + + + + + + + + + + + + + + + + + + + + +