From 0b61cb1e43d87b7d1f769b3aae2b57b5254ed9d2 Mon Sep 17 00:00:00 2001 From: Ashton Larkin Date: Tue, 9 Mar 2021 22:28:34 -0500 Subject: [PATCH] Iterate through changed links only in UpdateSim (#678) Signed-off-by: Ashton Larkin --- src/systems/physics/EntityFeatureMap.hh | 31 ++- src/systems/physics/EntityFeatureMap_TEST.cc | 16 +- src/systems/physics/Physics.cc | 200 ++++++++++++------- test/integration/wind_effects.cc | 2 +- test/worlds/wind_effects.sdf | 2 +- 5 files changed, 166 insertions(+), 85 deletions(-) diff --git a/src/systems/physics/EntityFeatureMap.hh b/src/systems/physics/EntityFeatureMap.hh index f9d8fe52c02..8008f478d7e 100644 --- a/src/systems/physics/EntityFeatureMap.hh +++ b/src/systems/physics/EntityFeatureMap.hh @@ -137,6 +137,7 @@ namespace systems::physics_system return castEntity; } } + /// \brief Helper function to cast from an entity type with minimum features /// to an entity with a different set of features. This overload takes a /// physics entity as input @@ -187,6 +188,21 @@ namespace systems::physics_system return kNullEntity; } + /// \brief Get the physics entity with required features that has a + /// particular ID + /// \param[in] _id The ID of the desired physics entity + /// \return If found, returns the corresponding physics entity. Otherwise, + /// nullptr + public: RequiredEntityPtr GetPhysicsEntityPtr(std::size_t _id) const + { + auto it = this->physEntityById.find(_id); + if (it != this->physEntityById.end()) + { + return it->second; + } + return nullptr; + } + /// \brief Check whether there is a physics entity associated with the given /// Gazebo entity /// \param[in] _entity Gazebo entity. @@ -215,6 +231,7 @@ namespace systems::physics_system { this->entityMap[_entity] = _physicsEntity; this->reverseMap[_physicsEntity] = _entity; + this->physEntityById[_physicsEntity->EntityID()] = _physicsEntity; } /// \brief Remove entity from all associated maps @@ -226,8 +243,9 @@ namespace systems::physics_system if (it != this->entityMap.end()) { this->reverseMap.erase(it->second); - this->entityMap.erase(it); + this->physEntityById.erase(it->second->EntityID()); this->castCache.erase(_entity); + this->entityMap.erase(it); return true; } return false; @@ -242,8 +260,9 @@ namespace systems::physics_system if (it != this->reverseMap.end()) { this->entityMap.erase(it->second); - this->reverseMap.erase(it); + this->physEntityById.erase(it->first->EntityID()); this->castCache.erase(it->second); + this->reverseMap.erase(it); return true; } return false; @@ -257,13 +276,13 @@ namespace systems::physics_system return this->entityMap; } - /// \brief Get the total number of entries in the three maps. Only used for + /// \brief Get the total number of entries in the maps. Only used for /// testing. /// \return Number of entries in all the maps. public: std::size_t TotalMapEntryCount() const { return this->entityMap.size() + this->reverseMap.size() + - this->castCache.size(); + this->castCache.size() + this->physEntityById.size(); } /// \brief Map from Gazebo entity to physics entities with required features @@ -272,6 +291,10 @@ namespace systems::physics_system /// \brief Reverse map of entityMap private: std::unordered_map reverseMap; + /// \brief Map of physics entity IDs to the corresponding physics entity + /// with required features + private: std::unordered_map physEntityById; + /// \brief Cache map from Gazebo entity to physics entities with optional /// features private: mutable std::unordered_map castCache; diff --git a/src/systems/physics/EntityFeatureMap_TEST.cc b/src/systems/physics/EntityFeatureMap_TEST.cc index 9b842031b51..e315735cd16 100644 --- a/src/systems/physics/EntityFeatureMap_TEST.cc +++ b/src/systems/physics/EntityFeatureMap_TEST.cc @@ -118,8 +118,8 @@ TEST_F(EntityFeatureMapFixture, AddCastRemoveEntity) testMap.AddEntity(gazeboWorld1Entity, testWorld1); - // After adding the entity, there should be one entry each in two maps - EXPECT_EQ(2u, testMap.TotalMapEntryCount()); + // After adding the entity, there should be one entry each in three maps + EXPECT_EQ(3u, testMap.TotalMapEntryCount()); EXPECT_EQ(testWorld1, testMap.Get(gazeboWorld1Entity)); EXPECT_EQ(gazeboWorld1Entity, testMap.Get(testWorld1)); @@ -128,7 +128,7 @@ TEST_F(EntityFeatureMapFixture, AddCastRemoveEntity) testMap.EntityCast(gazeboWorld1Entity); ASSERT_NE(nullptr, testWorld1Feature1); // After the cast, there should be one more entry in the cache map. - EXPECT_EQ(3u, testMap.TotalMapEntryCount()); + EXPECT_EQ(4u, testMap.TotalMapEntryCount()); // Cast to optional feature2 auto testWorld1Feature2 = @@ -136,12 +136,12 @@ TEST_F(EntityFeatureMapFixture, AddCastRemoveEntity) ASSERT_NE(nullptr, testWorld1Feature2); // After the cast, the number of entries should remain the same because we // have not added an entity. - EXPECT_EQ(3u, testMap.TotalMapEntryCount()); + EXPECT_EQ(4u, testMap.TotalMapEntryCount()); // Add another entity WorldPtrType testWorld2 = this->engine->ConstructEmptyWorld("world2"); testMap.AddEntity(gazeboWorld2Entity, testWorld2); - EXPECT_EQ(5u, testMap.TotalMapEntryCount()); + EXPECT_EQ(7u, testMap.TotalMapEntryCount()); EXPECT_EQ(testWorld2, testMap.Get(gazeboWorld2Entity)); EXPECT_EQ(gazeboWorld2Entity, testMap.Get(testWorld2)); @@ -149,21 +149,21 @@ TEST_F(EntityFeatureMapFixture, AddCastRemoveEntity) testMap.EntityCast(testWorld2); ASSERT_NE(nullptr, testWorld2Feature1); // After the cast, there should be one more entry in the cache map. - EXPECT_EQ(6u, testMap.TotalMapEntryCount()); + EXPECT_EQ(8u, testMap.TotalMapEntryCount()); auto testWorld2Feature2 = testMap.EntityCast(testWorld2); ASSERT_NE(nullptr, testWorld2Feature2); // After the cast, the number of entries should remain the same because we // have not added an entity. - EXPECT_EQ(6u, testMap.TotalMapEntryCount()); + EXPECT_EQ(8u, testMap.TotalMapEntryCount()); // Remove entitites testMap.Remove(gazeboWorld1Entity); EXPECT_FALSE(testMap.HasEntity(gazeboWorld1Entity)); EXPECT_EQ(nullptr, testMap.Get(gazeboWorld1Entity)); EXPECT_EQ(gazebo::kNullEntity, testMap.Get(testWorld1)); - EXPECT_EQ(3u, testMap.TotalMapEntryCount()); + EXPECT_EQ(4u, testMap.TotalMapEntryCount()); testMap.Remove(testWorld2); EXPECT_FALSE(testMap.HasEntity(gazeboWorld2Entity)); diff --git a/src/systems/physics/Physics.cc b/src/systems/physics/Physics.cc index 97c1bf9d910..5aeac72faef 100644 --- a/src/systems/physics/Physics.cc +++ b/src/systems/physics/Physics.cc @@ -181,11 +181,30 @@ class ignition::gazebo::systems::PhysicsPrivate /// \brief Step the simulation for each world /// \param[in] _dt Duration - public: void Step(const std::chrono::steady_clock::duration &_dt); + /// \returns Output data from the physics engine (this currently contains + /// data for links that experienced a pose change in the physics step) + public: ignition::physics::ForwardStep::Output Step( + const std::chrono::steady_clock::duration &_dt); + + /// \brief Get data of links that were updated in the latest physics step. + /// \param[in] _ecm Mutable reference to ECM. + /// \param[in] _updatedLinks Updated link poses from the latest physics step + /// that were written to by the physics engine (some physics engines may + /// 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( + EntityComponentManager &_ecm, + const ignition::physics::ForwardStep::Output &_updatedLinks); /// \brief Update components from physics simulation /// \param[in] _ecm Mutable reference to ECM. - public: void UpdateSim(EntityComponentManager &_ecm); + /// \param[in] _linkFrameData Links that experienced a pose change in the + /// 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< + Entity, physics::FrameData3d> &_linkFrameData); /// \brief Update collision components from physics simulation /// \param[in] _ecm Mutable reference to ECM. @@ -218,6 +237,11 @@ class ignition::gazebo::systems::PhysicsPrivate /// after a physics step. public: std::unordered_map linkWorldPoses; + /// \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. + public: std::unordered_map modelWorldPoses; + /// \brief A map between model entity ids in the ECM to whether its battery /// has drained. public: std::unordered_map entityOffMap; @@ -244,10 +268,7 @@ class ignition::gazebo::systems::PhysicsPrivate pose3Eql { [](const math::Pose3d &_a, const math::Pose3d &_b) { return _a.Pos().Equal(_b.Pos(), 1e-6) && - math::equal(_a.Rot().X(), _b.Rot().X(), 1e-6) && - math::equal(_a.Rot().Y(), _b.Rot().Y(), 1e-6) && - math::equal(_a.Rot().Z(), _b.Rot().Z(), 1e-6) && - math::equal(_a.Rot().W(), _b.Rot().W(), 1e-6); + _a.Rot().Equal(_b.Rot(), 1e-6); }}; /// \brief AxisAlignedBox equality comparison function. @@ -579,12 +600,14 @@ void Physics::Update(const UpdateInfo &_info, EntityComponentManager &_ecm) { this->dataPtr->CreatePhysicsEntities(_ecm); this->dataPtr->UpdatePhysics(_ecm); + ignition::physics::ForwardStep::Output stepOutput; // Only step if not paused. if (!_info.paused) { - this->dataPtr->Step(_info.dt); + stepOutput = this->dataPtr->Step(_info.dt); } - this->dataPtr->UpdateSim(_ecm); + auto changedLinks = this->dataPtr->ChangedLinks(_ecm, stepOutput); + this->dataPtr->UpdateSim(_ecm, changedLinks); // Entities scheduled to be removed should be removed from physics after the // simulation step. Otherwise, since the to-be-removed entity still shows up @@ -1164,6 +1187,7 @@ void PhysicsPrivate::RemovePhysicsEntities(const EntityComponentManager &_ecm) this->entityModelMap.Remove(_entity); this->topLevelModelMap.erase(_entity); this->staticEntities.erase(_entity); + this->modelWorldPoses.erase(_entity); } return true; }); @@ -1645,7 +1669,8 @@ void PhysicsPrivate::UpdatePhysics(EntityComponentManager &_ecm) } ////////////////////////////////////////////////// -void PhysicsPrivate::Step(const std::chrono::steady_clock::duration &_dt) +ignition::physics::ForwardStep::Output PhysicsPrivate::Step( + const std::chrono::steady_clock::duration &_dt) { IGN_PROFILE("PhysicsPrivate::Step"); ignition::physics::ForwardStep::Input input; @@ -1658,6 +1683,8 @@ void PhysicsPrivate::Step(const std::chrono::steady_clock::duration &_dt) { world.second->Step(output, state, input); } + + return output; } ////////////////////////////////////////////////// @@ -1697,16 +1724,44 @@ ignition::math::Pose3d PhysicsPrivate::RelativePose(const Entity &_from, } ////////////////////////////////////////////////// -void PhysicsPrivate::UpdateSim(EntityComponentManager &_ecm) +std::unordered_map PhysicsPrivate::ChangedLinks( + EntityComponentManager &_ecm, + const ignition::physics::ForwardStep::Output &_updatedLinks) { - IGN_PROFILE("PhysicsPrivate::UpdateSim"); + IGN_PROFILE("Links Frame Data"); - // Link entities and their latest frame data. std::unordered_map linkFrameData; - // Go through and retrieve frame data for each link in a non-static model - IGN_PROFILE_BEGIN("Links Frame Data"); - _ecm.Each( + // 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 + if (_updatedLinks.Has()) + { + for (const auto &link : + _updatedLinks.Query()->entries) + { + // get the gazebo entity that matches the updated physics link entity + const auto linkPhys = this->entityLinkMap.GetPhysicsEntityPtr(link.body); + if (nullptr == linkPhys) + { + ignerr << "Internal error: a physics entity ptr with an ID of [" + << link.body << "] does not exist." << std::endl; + continue; + } + auto entity = this->entityLinkMap.Get(linkPhys); + if (entity == kNullEntity) + { + ignerr << "Internal error: no gazebo entity matches the physics entity " + << "with ID [" << link.body << "]." << std::endl; + continue; + } + + auto frameData = linkPhys->FrameDataRelativeToWorld(); + linkFrameData[entity] = frameData; + } + } + else + { + _ecm.Each( [&](const Entity &_entity, components::Link *) -> bool { if (this->staticEntities.find(_entity) != this->staticEntities.end()) @@ -1721,41 +1776,57 @@ void PhysicsPrivate::UpdateSim(EntityComponentManager &_ecm) } auto frameData = linkPhys->FrameDataRelativeToWorld(); - linkFrameData[_entity] = frameData; + + // update the link pose if this is the first update, + // or if the link pose has changed since the last update + // (if the link pose hasn't changed, there's no need for a pose update) + const auto worldPoseMath3d = ignition::math::eigen3::convert( + frameData.pose); + if ((this->linkWorldPoses.find(_entity) == this->linkWorldPoses.end()) + || !this->pose3Eql(this->linkWorldPoses[_entity], worldPoseMath3d)) + { + // cache the updated link pose to check if the link pose has changed + // during the next iteration + this->linkWorldPoses[_entity] = worldPoseMath3d; + + linkFrameData[_entity] = frameData; + } + return true; }); - IGN_PROFILE_END(); + } + + return linkFrameData; +} + +////////////////////////////////////////////////// +void PhysicsPrivate::UpdateSim(EntityComponentManager &_ecm, + const std::unordered_map &_linkFrameData) +{ + IGN_PROFILE("PhysicsPrivate::UpdateSim"); IGN_PROFILE_BEGIN("Models"); - // Model entities and their world poses. - std::unordered_map modelWorldPoses; - _ecm.Each( - [&](const Entity &_entity, components::Model *, components::Pose *_pose, - components::ParentEntity *_parentEntity, components::Static *_static, + _ecm.Each( + [&](const Entity &_entity, components::Model *, components::ModelCanonicalLink *_canonicalLink) -> bool { - if (_static->Data()) - { + // 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; - } - auto linkFrameIt = linkFrameData.find(_canonicalLink->Data()); - if (linkFrameIt == linkFrameData.end()) - { - ignerr << "Internal error: Frame data for link [" << _entity - << "] not found" << std::endl; - return true; - } std::optional parentWorldPose; // 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 - // modelWorldPoses. If not found, this must not be nested, so this - // model's pose component would reflect it's absolute pose. - auto parentModelPoseIt = modelWorldPoses.find(_parentEntity->Data()); - if (parentModelPoseIt != modelWorldPoses.end()) + // 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; } @@ -1787,71 +1858,58 @@ void PhysicsPrivate::UpdateSim(EntityComponentManager &_ecm) const auto &modelWorldPose = math::eigen3::convert(linkWorldPose) * linkPoseFromModel.Inverse(); - modelWorldPoses[_entity] = modelWorldPose; + this->modelWorldPoses[_entity] = modelWorldPose; // update model's pose + auto modelPose = _ecm.Component(_entity); if (parentWorldPose) { - *_pose = + *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. - *_pose = components::Pose(modelWorldPose); + *modelPose = components::Pose(modelWorldPose); } _ecm.SetChanged(_entity, components::Pose::typeId, ComponentState::PeriodicChange); return true; }); - IGN_PROFILE_END(); // Link poses, velocities... IGN_PROFILE_BEGIN("Links"); - for (const auto &[entity, frameData] : linkFrameData) + for (const auto &[entity, frameData] : _linkFrameData) { IGN_PROFILE_BEGIN("Local pose"); - auto canonicalLink = _ecm.Component(entity); const auto &worldPose = frameData.pose; const auto parentEntity = _ecm.ParentEntity(entity); - // update the link or top level model pose if this is the first update, - // or if the link pose has changed since the last update - // (if the link pose hasn't changed, there's no need for a pose update) - const auto worldPoseMath3d = ignition::math::eigen3::convert(worldPose); - if ((this->linkWorldPoses.find(entity) == this->linkWorldPoses.end()) - || !this->pose3Eql(this->linkWorldPoses[entity], worldPoseMath3d)) + if (!canonicalLink) { - // cache the updated link pose to check if the link pose has changed - // during the next iteration - this->linkWorldPoses[entity] = worldPoseMath3d; - - if (!canonicalLink) + // Compute the relative pose of this link from the parent model + auto parentModelPoseIt = this->modelWorldPoses.find(parentEntity); + if (parentModelPoseIt == this->modelWorldPoses.end()) { - // Compute the relative pose of this link from the parent model - auto parentModelPoseIt = modelWorldPoses.find(parentEntity); - if (parentModelPoseIt == modelWorldPoses.end()) - { - ignerr << "Internal error: parent model [" << parentEntity - << "] does not have a world pose available" << std::endl; - continue; - } - const math::Pose3d &parentWorldPose = parentModelPoseIt->second; - - // Unlike canonical links, pose of regular links can move relative. - // to the parent. Same for links inside nested models. - auto pose = _ecm.Component(entity); - *pose = components::Pose(parentWorldPose.Inverse() * - math::eigen3::convert(worldPose)); - _ecm.SetChanged(entity, components::Pose::typeId, - ComponentState::PeriodicChange); + ignerr << "Internal error: parent model [" << parentEntity + << "] does not have a world pose available" << std::endl; + continue; } + const math::Pose3d &parentWorldPose = parentModelPoseIt->second; + + // Unlike canonical links, pose of regular links can move relative. + // to the parent. Same for links inside nested models. + auto pose = _ecm.Component(entity); + *pose = components::Pose(parentWorldPose.Inverse() * + math::eigen3::convert(worldPose)); + _ecm.SetChanged(entity, components::Pose::typeId, + ComponentState::PeriodicChange); } IGN_PROFILE_END(); diff --git a/test/integration/wind_effects.cc b/test/integration/wind_effects.cc index a441740542e..d0bd9716695 100644 --- a/test/integration/wind_effects.cc +++ b/test/integration/wind_effects.cc @@ -270,7 +270,7 @@ TEST_F(WindEffectsTest , TopicsAndServices) this->server->Run(true, 10, false); // As specified in SDF - math::Vector3d windVelSeed{1.0, 0.0, 1.0}; + math::Vector3d windVelSeed{10.0, 0.0, 10.0}; transport::Node node; std::size_t timeout{5000}; diff --git a/test/worlds/wind_effects.sdf b/test/worlds/wind_effects.sdf index 1d8b2516400..290b80ca5e6 100644 --- a/test/worlds/wind_effects.sdf +++ b/test/worlds/wind_effects.sdf @@ -31,7 +31,7 @@ - 1 0 1 + 10 0 10