From bebb17576c0b8d9e7a48cfca5e6578fea41d22f1 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 Signed-off-by: Ashton Larkin --- src/systems/physics/EntityFeatureMap.hh | 31 +- src/systems/physics/EntityFeatureMap_TEST.cc | 16 +- src/systems/physics/Physics.cc | 431 ++++++++++--------- 3 files changed, 254 insertions(+), 224 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 484145d4b78..c014eba4811 100644 --- a/src/systems/physics/Physics.cc +++ b/src/systems/physics/Physics.cc @@ -180,11 +180,17 @@ 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 Update components from physics simulation /// \param[in] _ecm Mutable reference to ECM. - public: void UpdateSim(EntityComponentManager &_ecm); + /// \param[in] _changedLinks Links that experienced a pose change in the + /// most recent physics step + public: void UpdateSim(EntityComponentManager &_ecm, + const ignition::physics::ForwardStep::Output &_changedLinks); /// \brief Update collision components from physics simulation /// \param[in] _ecm Mutable reference to ECM. @@ -578,12 +584,13 @@ 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); + this->dataPtr->UpdateSim(_ecm, stepOutput); // Entities scheduled to be removed should be removed from physics after the // simulation step. Otherwise, since the to-be-removed entity still shows up @@ -1647,7 +1654,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; @@ -1660,6 +1668,8 @@ void PhysicsPrivate::Step(const std::chrono::steady_clock::duration &_dt) { world.second->Step(output, state, input); } + + return output; } ////////////////////////////////////////////////// @@ -1699,232 +1709,229 @@ ignition::math::Pose3d PhysicsPrivate::RelativePose(const Entity &_from, } ////////////////////////////////////////////////// -void PhysicsPrivate::UpdateSim(EntityComponentManager &_ecm) +void PhysicsPrivate::UpdateSim(EntityComponentManager &_ecm, + const ignition::physics::ForwardStep::Output &_changedLinks) { IGN_PROFILE("PhysicsPrivate::UpdateSim"); // Link poses, velocities... IGN_PROFILE_BEGIN("Links"); - _ecm.Each( - [&](const Entity &_entity, components::Link * /*_link*/, - components::Pose *_pose, - const components::ParentEntity *_parent)->bool - { - // If parent is static, don't process pose changes as periodic - if (this->staticEntities.find(_entity) != this->staticEntities.end()) - return true; - - auto linkPhys = this->entityLinkMap.Get(_entity); - if (nullptr == linkPhys) - { - ignerr << "Internal error: link [" << _entity - << "] not in entity map" << std::endl; - return true; - } - - IGN_PROFILE_BEGIN("Local pose"); - // get top level model of this link - auto topLevelModelEnt = this->topLevelModelMap[_parent->Data()]; - - auto canonicalLink = - _ecm.Component(_entity); + for (const auto &link : + _changedLinks.Get().entries) + { + // get the gazebo entity that corresponds to 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 parent = _ecm.ParentEntity(entity); - auto frameData = linkPhys->FrameDataRelativeToWorld(); - const auto &worldPose = frameData.pose; + IGN_PROFILE_BEGIN("Local pose"); + // get top level model of this link + auto topLevelModelEnt = this->topLevelModelMap[parent]; - // 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)) - { - // cache the updated link pose to check if the link pose has changed - // during the next iteration - this->linkWorldPoses[_entity] = worldPoseMath3d; + auto canonicalLink = + _ecm.Component(entity); - if (canonicalLink) - { - // This is the canonical link, update the top level model. - // The pose of this link w.r.t its top level model never changes - // because it's "fixed" to the model. Instead, we change - // the top level model's pose here. The physics engine gives us the - // pose of this link relative to world so to set the top level - // model's pose, we have to post-multiply it by the inverse of the - // transform of the link w.r.t to its top level model. - math::Pose3d linkPoseFromTopLevelModel; - linkPoseFromTopLevelModel = - this->RelativePose(topLevelModelEnt, _entity, _ecm); - - // update top level model's pose - auto mutableModelPose = - _ecm.Component(topLevelModelEnt); - *(mutableModelPose) = components::Pose( - math::eigen3::convert(worldPose) * - linkPoseFromTopLevelModel.Inverse()); - - _ecm.SetChanged(topLevelModelEnt, components::Pose::typeId, - ComponentState::PeriodicChange); - } - else - { - // Compute the relative pose of this link from the top level model - // first get the world pose of the top level model - auto worldComp = - _ecm.Component(topLevelModelEnt); - // if the worldComp is a nullptr, something is wrong with ECS - if (!worldComp) - { - ignerr << "The parent component of " << topLevelModelEnt - << " could not be found. This should never happen!\n"; - return true; - } - math::Pose3d parentWorldPose = - this->RelativePose(worldComp->Data(), _parent->Data(), _ecm); - - // Unlike canonical links, pose of regular links can move relative - // to the parent. Same for links inside nested models. - *_pose = components::Pose(math::eigen3::convert(worldPose) + - parentWorldPose.Inverse()); - _ecm.SetChanged(_entity, components::Pose::typeId, - ComponentState::PeriodicChange); - } - } - IGN_PROFILE_END(); + auto frameData = linkPhys->FrameDataRelativeToWorld(); + const auto &worldPose = frameData.pose; - // Populate world poses, velocities and accelerations of the link. For - // now these components are updated only if another system has created - // the corresponding component on the entity. - auto worldPoseComp = _ecm.Component(_entity); - if (worldPoseComp) - { - auto state = - worldPoseComp->SetData(math::eigen3::convert(frameData.pose), - this->pose3Eql) ? - ComponentState::PeriodicChange : - ComponentState::NoChange; - _ecm.SetChanged(_entity, components::WorldPose::typeId, state); - } + if (canonicalLink) + { + // This is the canonical link, update the top level model. + // The pose of this link w.r.t its top level model never changes + // because it's "fixed" to the model. Instead, we change + // the top level model's pose here. The physics engine gives us the + // pose of this link relative to world so to set the top level + // model's pose, we have to post-multiply it by the inverse of the + // transform of the link w.r.t to its top level model. + math::Pose3d linkPoseFromTopLevelModel; + linkPoseFromTopLevelModel = + this->RelativePose(topLevelModelEnt, entity, _ecm); + + // update top level model's pose + auto mutableModelPose = + _ecm.Component(topLevelModelEnt); + *(mutableModelPose) = components::Pose( + math::eigen3::convert(worldPose) * + linkPoseFromTopLevelModel.Inverse()); + + _ecm.SetChanged(topLevelModelEnt, components::Pose::typeId, + ComponentState::PeriodicChange); + } + else + { + // Compute the relative pose of this link from the top level model + // first get the world pose of the top level model + auto worldComp = + _ecm.Component(topLevelModelEnt); + // if the worldComp is a nullptr, something is wrong with ECS + if (!worldComp) + { + ignerr << "The parent component of " << topLevelModelEnt + << " could not be found. This should never happen!\n"; + continue; + } + math::Pose3d parentWorldPose = + this->RelativePose(worldComp->Data(), parent, _ecm); + + // Unlike canonical links, pose of regular links can move relative + // to the parent. Same for links inside nested models. + auto pose = _ecm.Component(entity); + if (!pose) + { + ignerr << "Link [" << entity << "] has no pose. " + << "This should never happen!\n"; + continue; + } + *pose = components::Pose(math::eigen3::convert(worldPose) + + parentWorldPose.Inverse()); + _ecm.SetChanged(entity, components::Pose::typeId, + ComponentState::PeriodicChange); + } + IGN_PROFILE_END(); - // Velocity in world coordinates - auto worldLinVelComp = - _ecm.Component(_entity); - if (worldLinVelComp) - { - auto state = worldLinVelComp->SetData( - math::eigen3::convert(frameData.linearVelocity), - this->vec3Eql) ? - ComponentState::PeriodicChange : - ComponentState::NoChange; - _ecm.SetChanged(_entity, - components::WorldLinearVelocity::typeId, state); - } + // Populate world poses, velocities and accelerations of the link. For + // now these components are updated only if another system has created + // the corresponding component on the entity. + auto worldPoseComp = _ecm.Component(entity); + if (worldPoseComp) + { + auto state = + worldPoseComp->SetData(math::eigen3::convert(frameData.pose), + this->pose3Eql) ? + ComponentState::PeriodicChange : + ComponentState::NoChange; + _ecm.SetChanged(entity, components::WorldPose::typeId, state); + } - // Angular velocity in world frame coordinates - auto worldAngVelComp = - _ecm.Component(_entity); - if (worldAngVelComp) - { - auto state = worldAngVelComp->SetData( - math::eigen3::convert(frameData.angularVelocity), - this->vec3Eql) ? - ComponentState::PeriodicChange : - ComponentState::NoChange; - _ecm.SetChanged(_entity, - components::WorldAngularVelocity::typeId, state); - } + // Velocity in world coordinates + auto worldLinVelComp = + _ecm.Component(entity); + if (worldLinVelComp) + { + auto state = worldLinVelComp->SetData( + math::eigen3::convert(frameData.linearVelocity), + this->vec3Eql) ? + ComponentState::PeriodicChange : + ComponentState::NoChange; + _ecm.SetChanged(entity, + components::WorldLinearVelocity::typeId, state); + } - // Acceleration in world frame coordinates - auto worldLinAccelComp = - _ecm.Component(_entity); - if (worldLinAccelComp) - { - auto state = worldLinAccelComp->SetData( - math::eigen3::convert(frameData.linearAcceleration), - this->vec3Eql) ? - ComponentState::PeriodicChange : - ComponentState::NoChange; - _ecm.SetChanged(_entity, - components::WorldLinearAcceleration::typeId, state); - } + // Angular velocity in world frame coordinates + auto worldAngVelComp = + _ecm.Component(entity); + if (worldAngVelComp) + { + auto state = worldAngVelComp->SetData( + math::eigen3::convert(frameData.angularVelocity), + this->vec3Eql) ? + ComponentState::PeriodicChange : + ComponentState::NoChange; + _ecm.SetChanged(entity, + components::WorldAngularVelocity::typeId, state); + } - // Angular acceleration in world frame coordinates - auto worldAngAccelComp = - _ecm.Component(_entity); + // Acceleration in world frame coordinates + auto worldLinAccelComp = + _ecm.Component(entity); + if (worldLinAccelComp) + { + auto state = worldLinAccelComp->SetData( + math::eigen3::convert(frameData.linearAcceleration), + this->vec3Eql) ? + ComponentState::PeriodicChange : + ComponentState::NoChange; + _ecm.SetChanged(entity, + components::WorldLinearAcceleration::typeId, state); + } - if (worldAngAccelComp) - { - auto state = worldAngAccelComp->SetData( - math::eigen3::convert(frameData.angularAcceleration), - this->vec3Eql) ? - ComponentState::PeriodicChange : - ComponentState::NoChange; - _ecm.SetChanged(_entity, - components::WorldAngularAcceleration::typeId, state); - } + // Angular acceleration in world frame coordinates + auto worldAngAccelComp = + _ecm.Component(entity); + if (worldAngAccelComp) + { + auto state = worldAngAccelComp->SetData( + math::eigen3::convert(frameData.angularAcceleration), + this->vec3Eql) ? + ComponentState::PeriodicChange : + ComponentState::NoChange; + _ecm.SetChanged(entity, + components::WorldAngularAcceleration::typeId, state); + } - const Eigen::Matrix3d R_bs = worldPose.linear().transpose(); // NOLINT + const Eigen::Matrix3d R_bs = worldPose.linear().transpose(); // NOLINT - // Velocity in body-fixed frame coordinates - auto bodyLinVelComp = - _ecm.Component(_entity); - if (bodyLinVelComp) - { - Eigen::Vector3d bodyLinVel = R_bs * frameData.linearVelocity; - auto state = - bodyLinVelComp->SetData(math::eigen3::convert(bodyLinVel), - this->vec3Eql) ? - ComponentState::PeriodicChange : - ComponentState::NoChange; - _ecm.SetChanged(_entity, components::LinearVelocity::typeId, state); - } + // Velocity in body-fixed frame coordinates + auto bodyLinVelComp = + _ecm.Component(entity); + if (bodyLinVelComp) + { + Eigen::Vector3d bodyLinVel = R_bs * frameData.linearVelocity; + auto state = + bodyLinVelComp->SetData(math::eigen3::convert(bodyLinVel), + this->vec3Eql) ? + ComponentState::PeriodicChange : + ComponentState::NoChange; + _ecm.SetChanged(entity, components::LinearVelocity::typeId, state); + } - // Angular velocity in body-fixed frame coordinates - auto bodyAngVelComp = - _ecm.Component(_entity); - if (bodyAngVelComp) - { - Eigen::Vector3d bodyAngVel = R_bs * frameData.angularVelocity; - auto state = - bodyAngVelComp->SetData(math::eigen3::convert(bodyAngVel), - this->vec3Eql) ? - ComponentState::PeriodicChange : - ComponentState::NoChange; - _ecm.SetChanged(_entity, components::AngularVelocity::typeId, - state); - } + // Angular velocity in body-fixed frame coordinates + auto bodyAngVelComp = + _ecm.Component(entity); + if (bodyAngVelComp) + { + Eigen::Vector3d bodyAngVel = R_bs * frameData.angularVelocity; + auto state = + bodyAngVelComp->SetData(math::eigen3::convert(bodyAngVel), + this->vec3Eql) ? + ComponentState::PeriodicChange : + ComponentState::NoChange; + _ecm.SetChanged(entity, components::AngularVelocity::typeId, + state); + } - // Acceleration in body-fixed frame coordinates - auto bodyLinAccelComp = - _ecm.Component(_entity); - if (bodyLinAccelComp) - { - Eigen::Vector3d bodyLinAccel = R_bs * frameData.linearAcceleration; - auto state = - bodyLinAccelComp->SetData(math::eigen3::convert(bodyLinAccel), - this->vec3Eql)? - ComponentState::PeriodicChange : - ComponentState::NoChange; - _ecm.SetChanged(_entity, components::LinearAcceleration::typeId, - state); - } + // Acceleration in body-fixed frame coordinates + auto bodyLinAccelComp = + _ecm.Component(entity); + if (bodyLinAccelComp) + { + Eigen::Vector3d bodyLinAccel = R_bs * frameData.linearAcceleration; + auto state = + bodyLinAccelComp->SetData(math::eigen3::convert(bodyLinAccel), + this->vec3Eql)? + ComponentState::PeriodicChange : + ComponentState::NoChange; + _ecm.SetChanged(entity, components::LinearAcceleration::typeId, + state); + } - // Angular acceleration in world frame coordinates - auto bodyAngAccelComp = - _ecm.Component(_entity); - if (bodyAngAccelComp) - { - Eigen::Vector3d bodyAngAccel = R_bs * frameData.angularAcceleration; - auto state = - bodyAngAccelComp->SetData(math::eigen3::convert(bodyAngAccel), - this->vec3Eql) ? - ComponentState::PeriodicChange : - ComponentState::NoChange; - _ecm.SetChanged(_entity, components::AngularAcceleration::typeId, - state); - } - return true; - }); + // Angular acceleration in world frame coordinates + auto bodyAngAccelComp = + _ecm.Component(entity); + if (bodyAngAccelComp) + { + Eigen::Vector3d bodyAngAccel = R_bs * frameData.angularAcceleration; + auto state = + bodyAngAccelComp->SetData(math::eigen3::convert(bodyAngAccel), + this->vec3Eql) ? + ComponentState::PeriodicChange : + ComponentState::NoChange; + _ecm.SetChanged(entity, components::AngularAcceleration::typeId, + state); + } + } IGN_PROFILE_END(); // pose/velocity/acceleration of non-link entities such as sensors /