From 48acd0e474ba9b6a8fadec79ef7d7f8b5f730598 Mon Sep 17 00:00:00 2001 From: Ashton Larkin Date: Thu, 1 Apr 2021 17:04:47 -0400 Subject: [PATCH] Map canonical links to their models (#736) Signed-off-by: Ashton Larkin --- .../physics/CanonicalLinkModelTracker.hh | 116 ++++++++++++ src/systems/physics/Physics.cc | 165 ++++++++++-------- test/integration/physics_system.cc | 48 ++++- test/worlds/nested_model_canonical_link.sdf | 34 ++++ 4 files changed, 286 insertions(+), 77 deletions(-) create mode 100644 src/systems/physics/CanonicalLinkModelTracker.hh diff --git a/src/systems/physics/CanonicalLinkModelTracker.hh b/src/systems/physics/CanonicalLinkModelTracker.hh new file mode 100644 index 0000000000..4f6f01443e --- /dev/null +++ b/src/systems/physics/CanonicalLinkModelTracker.hh @@ -0,0 +1,116 @@ +/* + * 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. + * +*/ +#ifndef IGNITION_GAZEBO_SYSTEMS_PHYSICS_CANONICAL_LINK_MODEL_TRACKER_HH_ +#define IGNITION_GAZEBO_SYSTEMS_PHYSICS_CANONICAL_LINK_MODEL_TRACKER_HH_ + +#include +#include + +#include "ignition/gazebo/Entity.hh" +#include "ignition/gazebo/EntityComponentManager.hh" +#include "ignition/gazebo/components/CanonicalLink.hh" +#include "ignition/gazebo/components/Model.hh" +#include "ignition/gazebo/config.hh" + +namespace ignition::gazebo +{ +inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { +namespace systems::physics_system +{ + /// \brief Helper class that keeps track of which models have a particular + /// canonical link. This is useful in the physics system for updating model + /// poses - if a canonical link moved in the most recent physics step, then + /// all of the models that have this canonical link should be updated. It's + /// important to preserve topological ordering of the models in case there's + /// a nested model that shares the same canonical link (in a case like this, + /// the parent model pose needs to be updated before updating the child model + /// pose - see the documentation that explains how model pose updates are + /// calculated in PhysicsPrivate::UpdateSim to understand why nested model + /// poses need to be updated in topological order). + /// + /// It's possible to loop through all of the models and to update poses if the + /// model moved using something like EntityComponentManager::Each, but the + /// performance of this approach is worse than using just the moved canonical + /// links to determine which model poses should be updated (consider the case + /// where there are a lot of non-static models in a world, but only a few move + /// frequently - if using EntityComponentManager::Each, we still need to check + /// every single non-static model after a physics update to make sure that the + /// model did not move. If we instead use the updated canonical link + /// information, then we can skip iterating over/checking the models that + /// don't need to be updated). + class CanonicalLinkModelTracker + { + /// \brief Save mappings for new models and their canonical links + /// \param[in] _ecm EntityComponentManager + public: void AddNewModels(const EntityComponentManager &_ecm); + + /// \brief Get a topological ordering of models that have a particular + /// canonical link + /// \param[in] _canonicalLink The canonical link + /// \return The models that have this link as their canonical link, in + /// topological order + public: const std::set &CanonicalLinkModels( + const Entity _canonicalLink) const; + + /// \brief Remove a link from the mapping. This method should be called when + /// a link is removed from simulation + /// \param[in] _link The link to remove + public: void RemoveLink(const Entity &_link); + + /// \brief A mapping of canonical links to the models that have this + /// canonical link. The key is the canonical link entity, and the value is + /// the model entities that have this canonical link. The models in the + /// value are in topological order + private: std::unordered_map> linkModelMap; + + /// \brief An empty set of models that is returned from the + /// CanonicalLinkModels method for links that map to no models + private: const std::set emptyModelOrdering{}; + }; + + void CanonicalLinkModelTracker::AddNewModels( + const EntityComponentManager &_ecm) + { + _ecm.EachNew( + [this](const Entity &_model, const components::Model *, + const components::ModelCanonicalLink *_canonicalLinkComp) + { + this->linkModelMap[_canonicalLinkComp->Data()].insert(_model); + return true; + }); + } + + const std::set &CanonicalLinkModelTracker::CanonicalLinkModels( + const Entity _canonicalLink) const + { + auto it = this->linkModelMap.find(_canonicalLink); + if (it != this->linkModelMap.end()) + return it->second; + + // if an invalid entity was given, it maps to no models + return this->emptyModelOrdering; + } + + void CanonicalLinkModelTracker::RemoveLink(const Entity &_link) + { + this->linkModelMap.erase(_link); + } +} +} +} + +#endif diff --git a/src/systems/physics/Physics.cc b/src/systems/physics/Physics.cc index 2aaea50da8..c290b547f3 100644 --- a/src/systems/physics/Physics.cc +++ b/src/systems/physics/Physics.cc @@ -25,6 +25,7 @@ #include #include #include +#include #include #include #include @@ -119,6 +120,7 @@ #include "ignition/gazebo/components/ThreadPitch.hh" #include "ignition/gazebo/components/World.hh" +#include "CanonicalLinkModelTracker.hh" #include "EntityFeatureMap.hh" using namespace ignition; @@ -193,7 +195,10 @@ class ignition::gazebo::systems::PhysicsPrivate /// not write this data to ForwardStep::Output. If not, _ecm is used to get /// this updated link pose data). /// \return A map of gazebo link entities to their updated pose data. - public: std::unordered_map ChangedLinks( + /// std::map is used because canonical links must be in topological order + /// to ensure that nested models with multiple canonical links are updated + /// properly (models must be updated in topological order). + public: std::map ChangedLinks( EntityComponentManager &_ecm, const ignition::physics::ForwardStep::Output &_updatedLinks); @@ -203,7 +208,7 @@ class ignition::gazebo::systems::PhysicsPrivate /// most recent physics step. The key is the entity of the link, and the /// value is the updated frame data corresponding to that entity. public: void UpdateSim(EntityComponentManager &_ecm, - const std::unordered_map< + const std::map< Entity, physics::FrameData3d> &_linkFrameData); /// \brief Update collision components from physics simulation @@ -237,6 +242,11 @@ class ignition::gazebo::systems::PhysicsPrivate /// after a physics step. public: std::unordered_map linkWorldPoses; + /// \brief Keep a mapping of canonical links to models that have this + /// canonical link. Useful for updating model poses efficiently after a + /// physics step + public: CanonicalLinkModelTracker canonicalLinkModelTracker; + /// \brief Keep track of non-static model world poses. Since non-static /// models may not move on a given iteration, we want to keep track of the /// most recent model world pose change that took place. @@ -1178,6 +1188,7 @@ void PhysicsPrivate::RemovePhysicsEntities(const EntityComponentManager &_ecm) this->topLevelModelMap.erase(childLink); this->staticEntities.erase(childLink); this->linkWorldPoses.erase(childLink); + this->canonicalLinkModelTracker.RemoveLink(childLink); } for (const auto &childJoint : @@ -1835,13 +1846,13 @@ ignition::math::Pose3d PhysicsPrivate::RelativePose(const Entity &_from, } ////////////////////////////////////////////////// -std::unordered_map PhysicsPrivate::ChangedLinks( +std::map PhysicsPrivate::ChangedLinks( EntityComponentManager &_ecm, const ignition::physics::ForwardStep::Output &_updatedLinks) { IGN_PROFILE("Links Frame Data"); - std::unordered_map linkFrameData; + std::map linkFrameData; // Check to see if the physics engine gave a list of changed poses. If not, we // will iterate through all of the links via the ECM to see which ones changed @@ -1912,83 +1923,91 @@ std::unordered_map PhysicsPrivate::ChangedLinks( ////////////////////////////////////////////////// void PhysicsPrivate::UpdateSim(EntityComponentManager &_ecm, - const std::unordered_map &_linkFrameData) + const std::map &_linkFrameData) { IGN_PROFILE("PhysicsPrivate::UpdateSim"); IGN_PROFILE_BEGIN("Models"); - _ecm.Each( - [&](const Entity &_entity, components::Model *, - components::ModelCanonicalLink *_canonicalLink) -> bool - { - // If the model's canonical link did not move, we don't need to update - // the model's pose - auto linkFrameIt = _linkFrameData.find(_canonicalLink->Data()); - if (linkFrameIt == _linkFrameData.end()) - return true; - - std::optional parentWorldPose; + // make sure we have an up-to-date mapping of canonical links to their models + this->canonicalLinkModelTracker.AddNewModels(_ecm); - // If this model is nested, we assume the pose of the parent model has - // already been updated. We expect to find the updated pose in - // this->modelWorldPoses. If not found, this must not be nested, so - // this model's pose component would reflect it's absolute pose. - auto parentModelPoseIt = - this->modelWorldPoses.find( - _ecm.Component(_entity)->Data()); - if (parentModelPoseIt != this->modelWorldPoses.end()) - { - parentWorldPose = parentModelPoseIt->second; - } + for (const auto &[linkEntity, frameData] : _linkFrameData) + { + // get a topological ordering of the models that have linkEntity as the + // model's canonical link. If linkEntity isn't a canonical link for any + // models, canonicalLinkModels will be empty + auto canonicalLinkModels = + this->canonicalLinkModelTracker.CanonicalLinkModels(linkEntity); + + // Update poses for all of the models that have this changed canonical link + // (linkEntity). Since we have the models in topological order and + // _linkFrameData stores links in topological order thanks to the ordering + // of std::map (entity IDs are created in ascending order), this should + // properly handle pose updates for nested models + for (auto &model : canonicalLinkModels) + { + std::optional parentWorldPose; + + // If this model is nested, the pose of the parent model has already + // been updated since we iterate through the modified links in + // topological order. We expect to find the updated pose in + // this->modelWorldPoses. If not found, this must not be nested, so this + // model's pose component would reflect it's absolute pose. + auto parentModelPoseIt = + this->modelWorldPoses.find( + _ecm.Component(model)->Data()); + if (parentModelPoseIt != this->modelWorldPoses.end()) + { + parentWorldPose = parentModelPoseIt->second; + } - // Given the following frame names: - // W: World/inertial frame - // P: Parent frame (this could be a parent model or the World frame) - // M: This model's frame - // L: The frame of this model's canonical link - // - // And the following quantities: - // (See http://sdformat.org/tutorials?tut=specify_pose for pose - // convention) - // parentWorldPose (X_WP): Pose of the parent frame w.r.t the world - // linkPoseFromModel (X_ML): Pose of the canonical link frame w.r.t the - // model frame - // linkWorldPose (X_WL): Pose of the canonical link w.r.t the world - // modelWorldPose (X_WM): Pose of this model w.r.t the world - // - // The Pose component of this model entity stores the pose of M w.r.t P - // (X_PM) and is calculated as - // X_PM = (X_WP)^-1 * X_WM - // - // And X_WM is calculated from X_WL, which is obtained from physics as: - // X_WM = X_WL * (X_ML)^-1 - auto linkPoseFromModel = - this->RelativePose(_entity, linkFrameIt->first, _ecm); - const auto &linkWorldPose = linkFrameIt->second.pose; - const auto &modelWorldPose = - math::eigen3::convert(linkWorldPose) * linkPoseFromModel.Inverse(); - - this->modelWorldPoses[_entity] = modelWorldPose; - - // update model's pose - auto modelPose = _ecm.Component(_entity); - if (parentWorldPose) - { - *modelPose = - components::Pose(parentWorldPose->Inverse() * modelWorldPose); - } - else - { - // This is a non-nested model and parentWorldPose would be identity - // because it would be the pose of the parent (world) w.r.t the world. - *modelPose = components::Pose(modelWorldPose); - } + // Given the following frame names: + // W: World/inertial frame + // P: Parent frame (this could be a parent model or the World frame) + // M: This model's frame + // L: The frame of this model's canonical link + // + // And the following quantities: + // (See http://sdformat.org/tutorials?tut=specify_pose for pose + // convention) + // parentWorldPose (X_WP): Pose of the parent frame w.r.t the world + // linkPoseFromModel (X_ML): Pose of the canonical link frame w.r.t the + // model frame + // linkWorldPose (X_WL): Pose of the canonical link w.r.t the world + // modelWorldPose (X_WM): Pose of this model w.r.t the world + // + // The Pose component of this model entity stores the pose of M w.r.t P + // (X_PM) and is calculated as + // X_PM = (X_WP)^-1 * X_WM + // + // And X_WM is calculated from X_WL, which is obtained from physics as: + // X_WM = X_WL * (X_ML)^-1 + auto linkPoseFromModel = this->RelativePose(model, linkEntity, _ecm); + const auto &linkWorldPose = frameData.pose; + const auto &modelWorldPose = + math::eigen3::convert(linkWorldPose) * linkPoseFromModel.Inverse(); + + this->modelWorldPoses[model] = modelWorldPose; + + // update model's pose + auto modelPose = _ecm.Component(model); + if (parentWorldPose) + { + *modelPose = + components::Pose(parentWorldPose->Inverse() * modelWorldPose); + } + else + { + // This is a non-nested model and parentWorldPose would be identity + // because it would be the pose of the parent (world) w.r.t the world. + *modelPose = components::Pose(modelWorldPose); + } - _ecm.SetChanged(_entity, components::Pose::typeId, - ComponentState::PeriodicChange); - return true; - }); + _ecm.SetChanged(model, components::Pose::typeId, + ComponentState::PeriodicChange); + } + } IGN_PROFILE_END(); // Link poses, velocities... diff --git a/test/integration/physics_system.cc b/test/integration/physics_system.cc index a08269dc3d..e1ac1762ec 100644 --- a/test/integration/physics_system.cc +++ b/test/integration/physics_system.cc @@ -31,8 +31,10 @@ #include #include +#include "ignition/gazebo/Entity.hh" #include "ignition/gazebo/Server.hh" #include "ignition/gazebo/SystemLoader.hh" +#include "ignition/gazebo/Util.hh" #include "ignition/gazebo/test_config.hh" // NOLINT(build/include) #include "ignition/gazebo/components/AxisAlignedBox.hh" @@ -1293,11 +1295,17 @@ TEST_F(PhysicsSystemFixture, NestedModelIndividualCanonicalLinks) // Create a system that records the poses of the links after physics test::Relay testSystem; + // Store a pointer to the ECM used in testSystem so that it can be used to + // help verify poses at the end of this test + const gazebo::EntityComponentManager *ecm; + std::unordered_map postUpModelPoses; testSystem.OnPostUpdate( - [&postUpModelPoses](const gazebo::UpdateInfo &, + [&postUpModelPoses, &ecm](const gazebo::UpdateInfo &, const gazebo::EntityComponentManager &_ecm) { + ecm = &_ecm; + _ecm.Each( [&](const ignition::gazebo::Entity &, const components::Model *, const components::Name *_name, const components::Pose *_pose)->bool @@ -1313,10 +1321,10 @@ TEST_F(PhysicsSystemFixture, NestedModelIndividualCanonicalLinks) server.AddSystem(testSystem.systemPtr); const size_t iters = 500; server.Run(true, iters, false); - EXPECT_EQ(3u, postUpModelPoses.size()); + EXPECT_EQ(6u, postUpModelPoses.size()); auto modelIt = postUpModelPoses.find("model_00"); - EXPECT_NE(postUpModelPoses.end(), modelIt); + ASSERT_NE(postUpModelPoses.end(), modelIt); // link_00 is resting on the ground_box, so it remains stationary. And since // it is the canonical link of the parent model, model_00, the model frame @@ -1330,6 +1338,38 @@ TEST_F(PhysicsSystemFixture, NestedModelIndividualCanonicalLinks) const double dt = 0.001; const double zExpected = 0.5 * world->Gravity().Z() * pow(dt * iters, 2); modelIt = postUpModelPoses.find("model_01"); - EXPECT_NE(postUpModelPoses.end(), modelIt); + ASSERT_NE(postUpModelPoses.end(), modelIt); EXPECT_NEAR(zExpected, modelIt->second.Z(), 1e-2); + + // model_10, model_11 and model_12 share the same canonical link (link_12), + // which is a floating link. So, link_12 should fall due to gravity, which + // also means that the frames for model_10, model_11 and model_12 should fall + auto model10It = postUpModelPoses.find("model_10"); + ASSERT_NE(postUpModelPoses.end(), model10It); + EXPECT_NEAR(zExpected, model10It->second.Z(), 1e-2); + auto model11It = postUpModelPoses.find("model_11"); + ASSERT_NE(postUpModelPoses.end(), model11It); + auto model12It = postUpModelPoses.find("model_12"); + ASSERT_NE(postUpModelPoses.end(), model12It); + // (since model_11 and model_12 are nested, their poses are computed w.r.t. + // model_10) + EXPECT_DOUBLE_EQ(0.0, model11It->second.Z()); + EXPECT_DOUBLE_EQ(0.0, model12It->second.Z()); + // check the world pose of model_11 and model_12 to make sure the z components + // of these world poses match zExpected + ASSERT_NE(nullptr, ecm); + const auto model11Entity = + ecm->EntityByComponents(components::Name(model11It->first)); + ASSERT_NE(kNullEntity, model11Entity); + const auto model11WorldPose = gazebo::worldPose(model11Entity, *ecm); + EXPECT_NEAR(zExpected, model11WorldPose.Z(), 1e-2); + const auto model12Entity = + ecm->EntityByComponents(components::Name(model12It->first)); + ASSERT_NE(kNullEntity, model12Entity); + const auto model12WorldPose = gazebo::worldPose(model12Entity, *ecm); + EXPECT_NEAR(zExpected, model12WorldPose.Z(), 1e-2); + EXPECT_DOUBLE_EQ(model11WorldPose.Z(), model12WorldPose.Z()); + // zExpected is also the z component of the world pose for model_10 + EXPECT_DOUBLE_EQ(model11WorldPose.Z(), model10It->second.Z()); + EXPECT_DOUBLE_EQ(model12WorldPose.Z(), model10It->second.Z()); } diff --git a/test/worlds/nested_model_canonical_link.sdf b/test/worlds/nested_model_canonical_link.sdf index d246d3d6a9..9a5664ca22 100644 --- a/test/worlds/nested_model_canonical_link.sdf +++ b/test/worlds/nested_model_canonical_link.sdf @@ -84,5 +84,39 @@ + + + 0 5 0 0 0 0 + + + 4 5 0 0 0 0 + + + 10 5 0 0 0 0 + + + + + 1 1 1 + + + + + + + 1 1 1 + + + + + + + + + +