From 65a18cb860a1c75df6a5625b09819cc0435fe884 Mon Sep 17 00:00:00 2001
From: Nate Koenig <nkoenig@users.noreply.github.com>
Date: Thu, 25 Feb 2021 17:59:47 -0800
Subject: [PATCH 1/7] Limit scene broadcast publications when paused (#497)

Signed-off-by: Nate Koenig <nate@openrobotics.org>
Signed-off-by: Louise Poubel <louise@openrobotics.org>

Co-authored-by: Nate Koenig <nate@openrobotics.org>
Co-authored-by: Louise Poubel <louise@openrobotics.org>
---
 include/ignition/gazebo/gui/GuiRunner.hh      |  4 ++
 src/gui/GuiRunner.cc                          | 45 +++++++++++++++++--
 src/systems/physics/Physics.cc                | 34 +++++++-------
 .../scene_broadcaster/SceneBroadcaster.cc     | 10 ++---
 .../scene_broadcaster/SceneBroadcaster.hh     |  1 +
 5 files changed, 68 insertions(+), 26 deletions(-)

diff --git a/include/ignition/gazebo/gui/GuiRunner.hh b/include/ignition/gazebo/gui/GuiRunner.hh
index 43a33a795e..4ea88aa0be 100644
--- a/include/ignition/gazebo/gui/GuiRunner.hh
+++ b/include/ignition/gazebo/gui/GuiRunner.hh
@@ -63,6 +63,10 @@ class IGNITION_GAZEBO_VISIBLE GuiRunner : public QObject
   /// \param[in] _msg New state message.
   private: void OnState(const msgs::SerializedStepMap &_msg);
 
+  /// \brief Update the plugins.
+  /// \todo(anyone) Move to GuiRunner::Implementation when porting to v5
+  private: void UpdatePlugins();
+
   /// \brief Entity-component manager.
   private: gazebo::EntityComponentManager ecm;
 
diff --git a/src/gui/GuiRunner.cc b/src/gui/GuiRunner.cc
index 88c7f4d46a..9d7ebeb16a 100644
--- a/src/gui/GuiRunner.cc
+++ b/src/gui/GuiRunner.cc
@@ -30,6 +30,16 @@
 using namespace ignition;
 using namespace gazebo;
 
+/// \todo(anyone) Move to GuiRunner::Implementation when porting to v5
+/// \brief Flag used to end the gUpdateThread.
+static bool gRunning = false;
+
+/// \brief Mutex to protect the plugin update.
+static std::mutex gUpdateMutex;
+
+/// \brief The plugin update thread..
+static std::thread gUpdateThread;
+
 /////////////////////////////////////////////////
 GuiRunner::GuiRunner(const std::string &_worldName)
 {
@@ -51,10 +61,31 @@ GuiRunner::GuiRunner(const std::string &_worldName)
          << std::endl;
 
   this->RequestState();
+
+  // Periodically update the plugins
+  // \todo(anyone) Move the global variables to GuiRunner::Implementation on v5
+  gRunning = true;
+  gUpdateThread = std::thread([&]()
+  {
+    while (gRunning)
+    {
+      {
+        std::lock_guard<std::mutex> lock(gUpdateMutex);
+        this->UpdatePlugins();
+      }
+      // This is roughly a 30Hz update rate.
+      std::this_thread::sleep_for(std::chrono::milliseconds(33));
+    }
+  });
 }
 
 /////////////////////////////////////////////////
-GuiRunner::~GuiRunner() = default;
+GuiRunner::~GuiRunner()
+{
+  gRunning = false;
+  if (gUpdateThread.joinable())
+    gUpdateThread.join();
+}
 
 /////////////////////////////////////////////////
 void GuiRunner::RequestState()
@@ -108,16 +139,22 @@ void GuiRunner::OnState(const msgs::SerializedStepMap &_msg)
   IGN_PROFILE_THREAD_NAME("GuiRunner::OnState");
   IGN_PROFILE("GuiRunner::Update");
 
+  std::lock_guard<std::mutex> lock(gUpdateMutex);
   this->ecm.SetState(_msg.state());
 
   // Update all plugins
   this->updateInfo = convert<UpdateInfo>(_msg.stats());
+  this->UpdatePlugins();
+  this->ecm.ClearNewlyCreatedEntities();
+  this->ecm.ProcessRemoveEntityRequests();
+}
+
+/////////////////////////////////////////////////
+void GuiRunner::UpdatePlugins()
+{
   auto plugins = gui::App()->findChildren<GuiSystem *>();
   for (auto plugin : plugins)
   {
     plugin->Update(this->updateInfo, this->ecm);
   }
-  this->ecm.ClearNewlyCreatedEntities();
-  this->ecm.ProcessRemoveEntityRequests();
 }
-
diff --git a/src/systems/physics/Physics.cc b/src/systems/physics/Physics.cc
index 84b5276e31..86ee7162a7 100644
--- a/src/systems/physics/Physics.cc
+++ b/src/systems/physics/Physics.cc
@@ -27,6 +27,7 @@
 #include <deque>
 #include <string>
 #include <unordered_map>
+#include <unordered_set>
 #include <vector>
 
 #include <ignition/common/MeshManager.hh>
@@ -200,6 +201,10 @@ class ignition::gazebo::systems::PhysicsPrivate
   /// has drained.
   public: std::unordered_map<Entity, bool> entityOffMap;
 
+  /// \brief Entities whose pose commands have been processed and should be
+  /// deleted the following iteration.
+  public: std::unordered_set<Entity> worldPoseCmdsToRemove;
+
   /// \brief used to store whether physics objects have been created.
   public: bool initialized = false;
 
@@ -1450,10 +1455,15 @@ void PhysicsPrivate::UpdatePhysics(EntityComponentManager &_ecm)
       });
 
   // Update model pose
+  auto olderWorldPoseCmdsToRemove = std::move(this->worldPoseCmdsToRemove);
+  this->worldPoseCmdsToRemove.clear();
+
   _ecm.Each<components::Model, components::WorldPoseCmd>(
       [&](const Entity &_entity, const components::Model *,
           const components::WorldPoseCmd *_poseCmd)
       {
+        this->worldPoseCmdsToRemove.insert(_entity);
+
         auto modelIt = this->entityModelMap.find(_entity);
         if (modelIt == this->entityModelMap.end())
           return true;
@@ -1509,6 +1519,13 @@ void PhysicsPrivate::UpdatePhysics(EntityComponentManager &_ecm)
         return true;
       });
 
+  // Remove world commands from previous iteration. We let them rotate one
+  // iteration so other systems have a chance to react to them too.
+  for (const Entity &entity : olderWorldPoseCmdsToRemove)
+  {
+    _ecm.RemoveComponent<components::WorldPoseCmd>(entity);
+  }
+
   // Slip compliance on Collisions
   _ecm.Each<components::SlipComplianceCmd>(
       [&](const Entity &_entity,
@@ -1641,23 +1658,6 @@ void PhysicsPrivate::UpdatePhysics(EntityComponentManager &_ecm)
         return true;
       });
 
-  // Clear pending commands
-  // Note: Removing components from inside an Each call can be dangerous.
-  // Instead, we collect all the entities that have the desired components and
-  // remove the component from them afterward.
-  std::vector<Entity> entitiesWorldCmd;
-  _ecm.Each<components::WorldPoseCmd>(
-      [&](const Entity &_entity, components::WorldPoseCmd*) -> bool
-      {
-        entitiesWorldCmd.push_back(_entity);
-        return true;
-      });
-
-  for (const Entity &entity : entitiesWorldCmd)
-  {
-    _ecm.RemoveComponent<components::WorldPoseCmd>(entity);
-  }
-
   // Populate bounding box info
   // Only compute bounding box if component exists to avoid unnecessary
   // computations
diff --git a/src/systems/scene_broadcaster/SceneBroadcaster.cc b/src/systems/scene_broadcaster/SceneBroadcaster.cc
index 722d8f58ac..ac93da86b6 100644
--- a/src/systems/scene_broadcaster/SceneBroadcaster.cc
+++ b/src/systems/scene_broadcaster/SceneBroadcaster.cc
@@ -272,12 +272,12 @@ void SceneBroadcaster::PostUpdate(const UpdateInfo &_info,
   // Throttle here instead of using transport::AdvertiseMessageOptions so that
   // we can skip the ECM serialization
   bool jumpBackInTime = _info.dt < std::chrono::steady_clock::duration::zero();
-  auto now = std::chrono::system_clock::now();
   bool changeEvent = _manager.HasEntitiesMarkedForRemoval() ||
-        _manager.HasNewEntities() || _manager.HasOneTimeComponentChanges() ||
-        jumpBackInTime;
-  bool itsPubTime = now - this->dataPtr->lastStatePubTime >
-       this->dataPtr->statePublishPeriod;
+    _manager.HasNewEntities() || _manager.HasOneTimeComponentChanges() ||
+    jumpBackInTime;
+  auto now = std::chrono::system_clock::now();
+  bool itsPubTime = !_info.paused && (now - this->dataPtr->lastStatePubTime >
+       this->dataPtr->statePublishPeriod);
   auto shouldPublish = this->dataPtr->statePub.HasConnections() &&
        (changeEvent || itsPubTime);
 
diff --git a/src/systems/scene_broadcaster/SceneBroadcaster.hh b/src/systems/scene_broadcaster/SceneBroadcaster.hh
index 0f19e23fe0..9506d080ba 100644
--- a/src/systems/scene_broadcaster/SceneBroadcaster.hh
+++ b/src/systems/scene_broadcaster/SceneBroadcaster.hh
@@ -55,6 +55,7 @@ namespace systems
                            EntityComponentManager &_ecm,
                            EventManager &_eventMgr) final;
 
+    // Documentation inherited
     public: void PostUpdate(const UpdateInfo &_info,
                 const EntityComponentManager &_ecm) final;
 

From 1a160849eb5fdd7f11d22025808a87178630f935 Mon Sep 17 00:00:00 2001
From: Nate Koenig <nkoenig@users.noreply.github.com>
Date: Mon, 1 Mar 2021 12:36:24 -0800
Subject: [PATCH 2/7] Merge pull request #651 from
 ignitionrobotics/particle_modification

Support particle modification
---
 CMakeLists.txt                                |  2 +-
 src/rendering/RenderUtil.cc                   | 15 ++++++--
 src/rendering/SceneManager.cc                 | 36 +++++++++++++------
 .../particle_emitter/ParticleEmitter.cc       | 21 ++++++-----
 test/integration/components.cc                | 36 +++++++++----------
 test/integration/particle_emitter.cc          | 34 +++++++++---------
 6 files changed, 88 insertions(+), 56 deletions(-)

diff --git a/CMakeLists.txt b/CMakeLists.txt
index abc409f202..378c5ef4bc 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -53,7 +53,7 @@ set(IGN_TRANSPORT_VER ${ignition-transport9_VERSION_MAJOR})
 
 #--------------------------------------
 # Find ignition-msgs
-ign_find_package(ignition-msgs6 REQUIRED VERSION 6.3)
+ign_find_package(ignition-msgs6 REQUIRED VERSION 6.4)
 set(IGN_MSGS_VER ${ignition-msgs6_VERSION_MAJOR})
 
 #--------------------------------------
diff --git a/src/rendering/RenderUtil.cc b/src/rendering/RenderUtil.cc
index bcc0c03429..204a1ad1e6 100644
--- a/src/rendering/RenderUtil.cc
+++ b/src/rendering/RenderUtil.cc
@@ -179,6 +179,9 @@ class ignition::gazebo::RenderUtilPrivate
   public: std::unordered_map<Entity, msgs::ParticleEmitter>
       newParticleEmittersCmds;
 
+  /// \brief A list of entities with particle emitter cmds to remove
+  public: std::vector<Entity> particleCmdsToRemove;
+
   /// \brief Map of ids of entites to be removed and sim iteration when the
   /// remove request is received
   public: std::unordered_map<Entity, uint64_t> removeEntities;
@@ -342,6 +345,13 @@ void RenderUtil::UpdateECM(const UpdateInfo &/*_info*/,
 {
   std::lock_guard<std::mutex> lock(this->dataPtr->updateMutex);
 
+  // Remove the commands from the entity
+  // these are commands from the last iteration. We want to make sure all
+  // systems have a chance to process them first before they are removed.
+  for (const auto &entity : this->dataPtr->particleCmdsToRemove)
+    _ecm.RemoveComponent<components::ParticleEmitterCmd>(entity);
+  this->dataPtr->particleCmdsToRemove.clear();
+
   // particle emitters commands
   _ecm.Each<components::ParticleEmitterCmd>(
       [&](const Entity &_entity,
@@ -349,7 +359,7 @@ void RenderUtil::UpdateECM(const UpdateInfo &/*_info*/,
       {
         // store emitter properties and update them in rendering thread
         this->dataPtr->newParticleEmittersCmds[_entity] =
-            _emitterCmd->Data();
+        _emitterCmd->Data();
 
         // update pose comp here
         if (_emitterCmd->Data().has_pose())
@@ -358,7 +368,8 @@ void RenderUtil::UpdateECM(const UpdateInfo &/*_info*/,
           if (poseComp)
             poseComp->Data() = msgs::Convert(_emitterCmd->Data().pose());
         }
-        _ecm.RemoveComponent<components::ParticleEmitterCmd>(_entity);
+        // Store the entity ids to clear outside of the `Each` loop.
+        this->dataPtr->particleCmdsToRemove.push_back(_entity);
 
         return true;
       });
diff --git a/src/rendering/SceneManager.cc b/src/rendering/SceneManager.cc
index 48b0317b9b..763f21b59f 100644
--- a/src/rendering/SceneManager.cc
+++ b/src/rendering/SceneManager.cc
@@ -1059,7 +1059,7 @@ rendering::ParticleEmitterPtr SceneManager::UpdateParticleEmitter(Entity _id,
     }
     default:
     {
-      emitter->SetType(ignition::rendering::EmitterType::EM_POINT);
+      // Do nothing if type is not set.
     }
   }
 
@@ -1068,20 +1068,28 @@ rendering::ParticleEmitterPtr SceneManager::UpdateParticleEmitter(Entity _id,
     emitter->SetEmitterSize(ignition::msgs::Convert(_emitter.size()));
 
   // Rate.
-  emitter->SetRate(_emitter.rate());
+  if (_emitter.has_rate())
+    emitter->SetRate(_emitter.rate().data());
 
   // Duration.
-  emitter->SetDuration(_emitter.duration());
+  if (_emitter.has_duration())
+    emitter->SetDuration(_emitter.duration().data());
 
   // Emitting.
-  emitter->SetEmitting(_emitter.emitting());
+  if (_emitter.has_emitting()) {
+    emitter->SetEmitting(_emitter.emitting().data());
+  }
 
   // Particle size.
-  emitter->SetParticleSize(
-    ignition::msgs::Convert(_emitter.particle_size()));
+  if (_emitter.has_particle_size())
+  {
+    emitter->SetParticleSize(
+        ignition::msgs::Convert(_emitter.particle_size()));
+  }
 
   // Lifetime.
-  emitter->SetLifetime(_emitter.lifetime());
+  if (_emitter.has_lifetime())
+    emitter->SetLifetime(_emitter.lifetime().data());
 
   // Material.
   if (_emitter.has_material())
@@ -1092,12 +1100,17 @@ rendering::ParticleEmitterPtr SceneManager::UpdateParticleEmitter(Entity _id,
   }
 
   // Velocity range.
-  emitter->SetVelocityRange(_emitter.min_velocity(), _emitter.max_velocity());
+  if (_emitter.has_min_velocity() && _emitter.has_max_velocity())
+  {
+    emitter->SetVelocityRange(_emitter.min_velocity().data(),
+        _emitter.max_velocity().data());
+  }
 
   // Color range image.
-  if (!_emitter.color_range_image().empty())
+  if (_emitter.has_color_range_image() &&
+      !_emitter.color_range_image().data().empty())
   {
-    emitter->SetColorRangeImage(_emitter.color_range_image());
+    emitter->SetColorRangeImage(_emitter.color_range_image().data());
   }
   // Color range.
   else if (_emitter.has_color_start() && _emitter.has_color_end())
@@ -1108,7 +1121,8 @@ rendering::ParticleEmitterPtr SceneManager::UpdateParticleEmitter(Entity _id,
   }
 
   // Scale rate.
-  emitter->SetScaleRate(_emitter.scale_rate());
+  if (_emitter.has_scale_rate())
+    emitter->SetScaleRate(_emitter.scale_rate().data());
 
   // pose
   if (_emitter.has_pose())
diff --git a/src/systems/particle_emitter/ParticleEmitter.cc b/src/systems/particle_emitter/ParticleEmitter.cc
index 697b260544..6c2de2a435 100644
--- a/src/systems/particle_emitter/ParticleEmitter.cc
+++ b/src/systems/particle_emitter/ParticleEmitter.cc
@@ -188,13 +188,16 @@ void ParticleEmitter::Configure(const Entity &_entity,
   ignition::msgs::Set(this->dataPtr->emitter.mutable_size(), size);
 
   // Rate.
-  this->dataPtr->emitter.set_rate(_sdf->Get<double>("rate", 10).first);
+  this->dataPtr->emitter.mutable_rate()->set_data(
+      _sdf->Get<double>("rate", 10).first);
 
   // Duration.
-  this->dataPtr->emitter.set_duration(_sdf->Get<double>("duration", 0).first);
+  this->dataPtr->emitter.mutable_duration()->set_data(
+      _sdf->Get<double>("duration", 0).first);
 
   // Emitting.
-  this->dataPtr->emitter.set_emitting(_sdf->Get<bool>("emitting", false).first);
+  this->dataPtr->emitter.mutable_emitting()->set_data(
+      _sdf->Get<bool>("emitting", false).first);
 
   // Particle size.
   size = ignition::math::Vector3d::One;
@@ -203,7 +206,8 @@ void ParticleEmitter::Configure(const Entity &_entity,
   ignition::msgs::Set(this->dataPtr->emitter.mutable_particle_size(), size);
 
   // Lifetime.
-  this->dataPtr->emitter.set_lifetime(_sdf->Get<double>("lifetime", 5).first);
+  this->dataPtr->emitter.mutable_lifetime()->set_data(
+      _sdf->Get<double>("lifetime", 5).first);
 
   // Material.
   if (_sdf->HasElement("material"))
@@ -216,11 +220,11 @@ void ParticleEmitter::Configure(const Entity &_entity,
   }
 
   // Min velocity.
-  this->dataPtr->emitter.set_min_velocity(
+  this->dataPtr->emitter.mutable_min_velocity()->set_data(
     _sdf->Get<double>("min_velocity", 1).first);
 
   // Max velocity.
-  this->dataPtr->emitter.set_max_velocity(
+  this->dataPtr->emitter.mutable_max_velocity()->set_data(
     _sdf->Get<double>("max_velocity", 1).first);
 
   // Color start.
@@ -236,7 +240,7 @@ void ParticleEmitter::Configure(const Entity &_entity,
   ignition::msgs::Set(this->dataPtr->emitter.mutable_color_end(), color);
 
   // Scale rate.
-  this->dataPtr->emitter.set_scale_rate(
+  this->dataPtr->emitter.mutable_scale_rate()->set_data(
     _sdf->Get<double>("scale_rate", 1).first);
 
   // Color range image.
@@ -250,7 +254,8 @@ void ParticleEmitter::Configure(const Entity &_entity,
     systemPaths.SetFilePathEnv(kResourcePathEnv);
     auto absolutePath = systemPaths.FindFile(path);
 
-    this->dataPtr->emitter.set_color_range_image(absolutePath);
+    this->dataPtr->emitter.mutable_color_range_image()->set_data(
+        absolutePath);
   }
 
   igndbg << "Loading particle emitter:" << std::endl
diff --git a/test/integration/components.cc b/test/integration/components.cc
index 28b1053329..c341dfcc0d 100644
--- a/test/integration/components.cc
+++ b/test/integration/components.cc
@@ -1520,15 +1520,15 @@ TEST_F(ComponentsTest, ParticleEmitter)
   emitter1.mutable_size()->set_x(1);
   emitter1.mutable_size()->set_y(2);
   emitter1.mutable_size()->set_z(3);
-  emitter1.set_rate(4.0);
-  emitter1.set_duration(5.0);
-  emitter1.set_emitting(false);
+  emitter1.mutable_rate()->set_data(4.0);
+  emitter1.mutable_duration()->set_data(5.0);
+  emitter1.mutable_emitting()->set_data(false);
   emitter1.mutable_particle_size()->set_x(0.1);
   emitter1.mutable_particle_size()->set_y(0.2);
   emitter1.mutable_particle_size()->set_z(0.3);
-  emitter1.set_lifetime(6.0);
-  emitter1.set_min_velocity(7.0);
-  emitter1.set_max_velocity(8.0);
+  emitter1.mutable_lifetime()->set_data(6.0);
+  emitter1.mutable_min_velocity()->set_data(7.0);
+  emitter1.mutable_max_velocity()->set_data(8.0);
   emitter1.mutable_color_start()->set_r(1.0);
   emitter1.mutable_color_start()->set_g(0);
   emitter1.mutable_color_start()->set_b(0);
@@ -1537,8 +1537,8 @@ TEST_F(ComponentsTest, ParticleEmitter)
   emitter1.mutable_color_end()->set_g(1.0);
   emitter1.mutable_color_end()->set_b(1.0);
   emitter1.mutable_color_end()->set_a(0);
-  emitter1.set_scale_rate(9.0);
-  emitter1.set_color_range_image("path_to_texture");
+  emitter1.mutable_scale_rate()->set_data(9.0);
+  emitter1.mutable_color_range_image()->set_data("path_to_texture");
 
   msgs::ParticleEmitter emitter2;
   emitter2.set_name("emitter2");
@@ -1547,15 +1547,15 @@ TEST_F(ComponentsTest, ParticleEmitter)
   emitter2.mutable_size()->set_x(1);
   emitter2.mutable_size()->set_y(2);
   emitter2.mutable_size()->set_z(3);
-  emitter2.set_rate(4.0);
-  emitter2.set_duration(5.0);
-  emitter2.set_emitting(false);
+  emitter2.mutable_rate()->set_data(4.0);
+  emitter2.mutable_duration()->set_data(5.0);
+  emitter2.mutable_emitting()->set_data(false);
   emitter2.mutable_particle_size()->set_x(0.1);
   emitter2.mutable_particle_size()->set_y(0.2);
   emitter2.mutable_particle_size()->set_z(0.3);
-  emitter2.set_lifetime(6.0);
-  emitter2.set_min_velocity(7.0);
-  emitter2.set_max_velocity(8.0);
+  emitter2.mutable_lifetime()->set_data(6.0);
+  emitter2.mutable_min_velocity()->set_data(7.0);
+  emitter2.mutable_max_velocity()->set_data(8.0);
   emitter2.mutable_color_start()->set_r(1.0);
   emitter2.mutable_color_start()->set_g(0);
   emitter2.mutable_color_start()->set_b(0);
@@ -1564,8 +1564,8 @@ TEST_F(ComponentsTest, ParticleEmitter)
   emitter2.mutable_color_end()->set_g(1.0);
   emitter2.mutable_color_end()->set_b(1.0);
   emitter2.mutable_color_end()->set_a(0);
-  emitter2.set_scale_rate(9.0);
-  emitter2.set_color_range_image("path_to_texture");
+  emitter2.mutable_scale_rate()->set_data(9.0);
+  emitter2.mutable_color_range_image()->set_data("path_to_texture");
 
   // Create components.
   auto comp1 = components::ParticleEmitter(emitter1);
@@ -1586,7 +1586,7 @@ TEST_F(ComponentsTest, ParticleEmitterCmd)
 {
   msgs::ParticleEmitter emitter1;
   emitter1.set_name("emitter1");
-  emitter1.set_emitting(true);
+  emitter1.mutable_emitting()->set_data(true);
 
   // Create components.
   auto comp1 = components::ParticleEmitterCmd(emitter1);
@@ -1598,6 +1598,6 @@ TEST_F(ComponentsTest, ParticleEmitterCmd)
   std::istringstream istr(ostr.str());
   components::ParticleEmitter comp3;
   comp3.Deserialize(istr);
-  EXPECT_EQ(comp1.Data().emitting(), comp3.Data().emitting());
+  EXPECT_EQ(comp1.Data().emitting().data(), comp3.Data().emitting().data());
   EXPECT_EQ(comp1.Data().name(), comp3.Data().name());
 }
diff --git a/test/integration/particle_emitter.cc b/test/integration/particle_emitter.cc
index 29dd17fa0c..c7ef738175 100644
--- a/test/integration/particle_emitter.cc
+++ b/test/integration/particle_emitter.cc
@@ -97,27 +97,29 @@ TEST_F(ParticleEmitterTest, SDFLoad)
                     msgs::Convert(_emitter->Data().pose()));
                 EXPECT_EQ(math::Vector3d(2, 2, 2),
                     msgs::Convert(_emitter->Data().size()));
-                EXPECT_DOUBLE_EQ(5.0, _emitter->Data().rate());
-                EXPECT_DOUBLE_EQ(1.0, _emitter->Data().duration());
-                EXPECT_TRUE(_emitter->Data().emitting());
+                EXPECT_DOUBLE_EQ(5.0, _emitter->Data().rate().data());
+                EXPECT_DOUBLE_EQ(1.0, _emitter->Data().duration().data());
+                EXPECT_TRUE(_emitter->Data().emitting().data());
                 EXPECT_EQ(math::Vector3d(3, 3, 3),
                     msgs::Convert(_emitter->Data().particle_size()));
-                EXPECT_DOUBLE_EQ(2.0, _emitter->Data().lifetime());
+                EXPECT_DOUBLE_EQ(2.0, _emitter->Data().lifetime().data());
                 // TODO(anyone) add material check here
-                EXPECT_DOUBLE_EQ(10.0, _emitter->Data().min_velocity());
-                EXPECT_DOUBLE_EQ(20.0, _emitter->Data().max_velocity());
+                EXPECT_DOUBLE_EQ(10.0,
+                    _emitter->Data().min_velocity().data());
+                EXPECT_DOUBLE_EQ(20.0,
+                    _emitter->Data().max_velocity().data());
                 EXPECT_EQ(math::Color::Blue,
                     msgs::Convert(_emitter->Data().color_start()));
                 EXPECT_EQ(math::Color::Green,
                     msgs::Convert(_emitter->Data().color_end()));
-                EXPECT_DOUBLE_EQ(10.0, _emitter->Data().scale_rate());
+                EXPECT_DOUBLE_EQ(10.0, _emitter->Data().scale_rate().data());
 
                 // color range image is empty because the emitter system
                 // will not be able to find a file that does not exist
                 // TODO(anyone) this should return  "/path/to/dummy_image.png"
                 // and let rendering do the findFile instead
                 EXPECT_EQ(std::string(),
-                    _emitter->Data().color_range_image());
+                    _emitter->Data().color_range_image().data());
               }
               else
               {
@@ -133,21 +135,21 @@ TEST_F(ParticleEmitterTest, SDFLoad)
                     msgs::Convert(_emitter->Data().pose()));
                 EXPECT_EQ(math::Vector3d(1, 1, 1),
                     msgs::Convert(_emitter->Data().size()));
-                EXPECT_DOUBLE_EQ(10.0, _emitter->Data().rate());
-                EXPECT_DOUBLE_EQ(0.0, _emitter->Data().duration());
-                EXPECT_FALSE(_emitter->Data().emitting());
+                EXPECT_DOUBLE_EQ(10.0, _emitter->Data().rate().data());
+                EXPECT_DOUBLE_EQ(0.0, _emitter->Data().duration().data());
+                EXPECT_FALSE(_emitter->Data().emitting().data());
                 EXPECT_EQ(math::Vector3d(1, 1, 1),
                     msgs::Convert(_emitter->Data().particle_size()));
-                EXPECT_DOUBLE_EQ(5.0, _emitter->Data().lifetime());
+                EXPECT_DOUBLE_EQ(5.0, _emitter->Data().lifetime().data());
                 // TODO(anyone) add material check here
-                EXPECT_DOUBLE_EQ(1.0, _emitter->Data().min_velocity());
-                EXPECT_DOUBLE_EQ(1.0, _emitter->Data().max_velocity());
+                EXPECT_DOUBLE_EQ(1.0, _emitter->Data().min_velocity().data());
+                EXPECT_DOUBLE_EQ(1.0, _emitter->Data().max_velocity().data());
                 EXPECT_EQ(math::Color::White,
                     msgs::Convert(_emitter->Data().color_start()));
                 EXPECT_EQ(math::Color::White,
                     msgs::Convert(_emitter->Data().color_end()));
-                EXPECT_DOUBLE_EQ(1.0, _emitter->Data().scale_rate());
-                EXPECT_EQ("", _emitter->Data().color_range_image());
+                EXPECT_DOUBLE_EQ(1.0, _emitter->Data().scale_rate().data());
+                EXPECT_EQ("", _emitter->Data().color_range_image().data());
               }
 
               return true;

From 2a73431ab4f0361ef37c59f4af5a29982d3af523 Mon Sep 17 00:00:00 2001
From: Louise Poubel <louise@openrobotics.org>
Date: Mon, 1 Mar 2021 13:28:17 -0800
Subject: [PATCH 3/7] Cache top level and static to speed up physics system
 (#656)

Signed-off-by: Louise Poubel <louise@openrobotics.org>
---
 src/systems/physics/Physics.cc | 407 ++++++++++++++++++---------------
 1 file changed, 226 insertions(+), 181 deletions(-)

diff --git a/src/systems/physics/Physics.cc b/src/systems/physics/Physics.cc
index 2a4a05cbbd..281cf7aeeb 100644
--- a/src/systems/physics/Physics.cc
+++ b/src/systems/physics/Physics.cc
@@ -197,6 +197,13 @@ class ignition::gazebo::systems::PhysicsPrivate
   /// associated with a physics Link
   public: std::unordered_map<LinkPtrType, Entity> linkEntityMap;
 
+  /// \brief Cache the top-level model for each entity.
+  /// The key is an entity and the value is its top level model.
+  public: std::unordered_map<Entity, Entity> topLevelModelMap;
+
+  /// \brief Keep track of what entities are static (models and links).
+  public: std::unordered_set<Entity> staticEntities;
+
   /// \brief A map between model entity ids in the ECM to whether its battery
   /// has drained.
   public: std::unordered_map<Entity, bool> entityOffMap;
@@ -713,6 +720,7 @@ void PhysicsPrivate::CreatePhysicsEntities(const EntityComponentManager &_ecm)
         if (staticComp && staticComp->Data())
         {
           model.SetStatic(staticComp->Data());
+          this->staticEntities.insert(_entity);
         }
         auto selfCollideComp = _ecm.Component<components::SelfCollide>(_entity);
         if (selfCollideComp && selfCollideComp ->Data())
@@ -747,11 +755,15 @@ void PhysicsPrivate::CreatePhysicsEntities(const EntityComponentManager &_ecm)
             }
             auto modelPtrPhys = nestedModelFeature->ConstructNestedModel(model);
             this->entityModelMap.insert(std::make_pair(_entity, modelPtrPhys));
+            this->topLevelModelMap.insert(std::make_pair(_entity,
+                topLevelModel(_entity, _ecm)));
           }
           else
           {
             auto modelPtrPhys = worldPtrPhys->ConstructModel(model);
             this->entityModelMap.insert(std::make_pair(_entity, modelPtrPhys));
+            this->topLevelModelMap.insert(std::make_pair(_entity,
+                topLevelModel(_entity, _ecm)));
           }
         }
         // check if parent is a model (nested model)
@@ -783,14 +795,18 @@ void PhysicsPrivate::CreatePhysicsEntities(const EntityComponentManager &_ecm)
             auto parentStaticComp =
               _ecm.Component<components::Static>(_parent->Data());
             if (parentStaticComp && parentStaticComp->Data())
+            {
               model.SetStatic(true);
-
+              this->staticEntities.insert(_entity);
+            }
 
             auto modelPtrPhys = nestedModelFeature->ConstructNestedModel(model);
             if (modelPtrPhys)
             {
               this->entityModelMap.insert(
                   std::make_pair(_entity, modelPtrPhys));
+              this->topLevelModelMap.insert(std::make_pair(_entity,
+                  topLevelModel(_entity, _ecm)));
             }
             else
             {
@@ -843,6 +859,12 @@ void PhysicsPrivate::CreatePhysicsEntities(const EntityComponentManager &_ecm)
         link.SetName(_name->Data());
         link.SetRawPose(_pose->Data());
 
+        if (this->staticEntities.find(_parent->Data()) !=
+            this->staticEntities.end())
+        {
+          this->staticEntities.insert(_entity);
+        }
+
         // get link inertial
         auto inertial = _ecm.Component<components::Inertial>(_entity);
         if (inertial)
@@ -853,6 +875,8 @@ void PhysicsPrivate::CreatePhysicsEntities(const EntityComponentManager &_ecm)
         auto linkPtrPhys = modelPtrPhys->ConstructLink(link);
         this->entityLinkMap.insert(std::make_pair(_entity, linkPtrPhys));
         this->linkEntityMap.insert(std::make_pair(linkPtrPhys, _entity));
+        this->topLevelModelMap.insert(std::make_pair(_entity,
+            topLevelModel(_entity, _ecm)));
 
         return true;
       });
@@ -985,6 +1009,8 @@ void PhysicsPrivate::CreatePhysicsEntities(const EntityComponentManager &_ecm)
             std::make_pair(_entity, collisionPtrPhys));
         this->collisionEntityMap.insert(
             std::make_pair(collisionPtrPhys, _entity));
+        this->topLevelModelMap.insert(std::make_pair(_entity,
+            topLevelModel(_entity, _ecm)));
         return true;
       });
 
@@ -1067,6 +1093,8 @@ void PhysicsPrivate::CreatePhysicsEntities(const EntityComponentManager &_ecm)
           // Some joints may not be supported, so only add them to the map if
           // the physics entity is valid
           this->entityJointMap.insert(std::make_pair(_entity, jointPtrPhys));
+          this->topLevelModelMap.insert(std::make_pair(_entity,
+              topLevelModel(_entity, _ecm)));
         }
         return true;
       });
@@ -1157,6 +1185,8 @@ void PhysicsPrivate::CreatePhysicsEntities(const EntityComponentManager &_ecm)
           igndbg << "Creating detachable joint [" << _entity << "]"
                  << std::endl;
           this->entityJointMap.insert(std::make_pair(_entity, jointPtrPhys));
+          this->topLevelModelMap.insert(std::make_pair(_entity,
+              topLevelModel(_entity, _ecm)));
         }
         else
         {
@@ -1196,6 +1226,7 @@ void PhysicsPrivate::RemovePhysicsEntities(const EntityComponentManager &_ecm)
               {
                 this->collisionEntityMap.erase(collIt->second);
                 this->entityCollisionMap.erase(collIt);
+                this->topLevelModelMap.erase(childCollision);
               }
             }
             // First erase the entry associated with this link from the
@@ -1206,17 +1237,22 @@ void PhysicsPrivate::RemovePhysicsEntities(const EntityComponentManager &_ecm)
               this->linkEntityMap.erase(linkPhysIt->second);
             }
             this->entityLinkMap.erase(childLink);
+            this->topLevelModelMap.erase(childLink);
+            this->staticEntities.erase(childLink);
           }
 
           for (const auto &childJoint :
                _ecm.ChildrenByComponents(_entity, components::Joint()))
           {
             this->entityJointMap.erase(childJoint);
+            this->topLevelModelMap.erase(childJoint);
           }
 
           // Remove the model from the physics engine
           modelIt->second->Remove();
           this->entityModelMap.erase(_entity);
+          this->topLevelModelMap.erase(_entity);
+          this->staticEntities.erase(_entity);
         }
         return true;
       });
@@ -1460,7 +1496,7 @@ void PhysicsPrivate::UpdatePhysics(EntityComponentManager &_ecm)
           return true;
 
         // world pose cmd currently not supported for nested models
-        if (_entity != topLevelModel(_entity, _ecm))
+        if (_entity != this->topLevelModelMap[_entity])
         {
           ignerr << "Unable to set world pose for nested models."
                  << std::endl;
@@ -1492,9 +1528,7 @@ void PhysicsPrivate::UpdatePhysics(EntityComponentManager &_ecm)
                                 linkPose));
 
         // Process pose commands for static models here, as one-time changes
-        const components::Static *staticComp =
-          _ecm.Component<components::Static>(_entity);
-        if (staticComp && staticComp->Data())
+        if (this->staticEntities.find(_entity) != this->staticEntities.end())
         {
           auto worldPoseComp = _ecm.Component<components::Pose>(_entity);
           if (worldPoseComp)
@@ -1556,7 +1590,7 @@ void PhysicsPrivate::UpdatePhysics(EntityComponentManager &_ecm)
           return true;
 
         // angular vel cmd currently not supported for nested models
-        if (_entity != topLevelModel(_entity, _ecm))
+        if (_entity != this->topLevelModelMap[_entity])
         {
           ignerr << "Unable to set angular velocity for nested models."
                  << std::endl;
@@ -1604,7 +1638,7 @@ void PhysicsPrivate::UpdatePhysics(EntityComponentManager &_ecm)
           return true;
 
         // linear vel cmd currently not supported for nested models
-        if (_entity != topLevelModel(_entity, _ecm))
+        if (_entity != this->topLevelModelMap[_entity])
         {
           ignerr << "Unable to set linear velocity for nested models."
                  << std::endl;
@@ -1759,212 +1793,217 @@ void PhysicsPrivate::UpdateSim(EntityComponentManager &_ecm)
 {
   IGN_PROFILE("PhysicsPrivate::UpdateSim");
 
-  // local pose
+  // Link poses, velocities...
+  IGN_PROFILE_BEGIN("Links");
   _ecm.Each<components::Link, components::Pose, components::ParentEntity>(
       [&](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
-        const auto *staticComp =
-          _ecm.Component<components::Static>(_parent->Data());
-
-        if (staticComp && staticComp->Data())
+        if (this->staticEntities.find(_entity) != this->staticEntities.end())
           return true;
 
         auto linkIt = this->entityLinkMap.find(_entity);
-        if (linkIt != this->entityLinkMap.end())
+        if (linkIt == this->entityLinkMap.end())
         {
-          // get top level model of this link
-          auto topLevelModelEnt = topLevelModel(_parent->Data(), _ecm);
+          ignerr << "Internal error: link [" << _entity
+                 << "] not in entity map" << std::endl;
+          return true;
+        }
 
-          auto canonicalLink =
-              _ecm.Component<components::CanonicalLink>(_entity);
+        IGN_PROFILE_BEGIN("Local pose");
+        // get top level model of this link
+        auto topLevelModelEnt = this->topLevelModelMap[_parent->Data()];
 
-          auto frameData = linkIt->second->FrameDataRelativeToWorld();
-          const auto &worldPose = frameData.pose;
+        auto canonicalLink =
+            _ecm.Component<components::CanonicalLink>(_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<components::Pose>(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<components::ParentEntity>(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);
-          }
+        auto frameData = linkIt->second->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<components::WorldPose>(_entity);
-          if (worldPoseComp)
+        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<components::Pose>(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<components::ParentEntity>(topLevelModelEnt);
+          // if the worldComp is a nullptr, something is wrong with ECS
+          if (!worldComp)
           {
-            auto state =
-                worldPoseComp->SetData(math::eigen3::convert(frameData.pose),
-                this->pose3Eql) ?
-                ComponentState::PeriodicChange :
-                ComponentState::NoChange;
-            _ecm.SetChanged(_entity, components::WorldPose::typeId, state);
+            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();
 
-          // Velocity in world coordinates
-          auto worldLinVelComp =
-              _ecm.Component<components::WorldLinearVelocity>(_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<components::WorldPose>(_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<components::WorldAngularVelocity>(_entity);
-          if (worldAngVelComp)
-          {
-            auto state = worldAngVelComp->SetData(
-                math::eigen3::convert(frameData.angularVelocity),
+        // Velocity in world coordinates
+        auto worldLinVelComp =
+            _ecm.Component<components::WorldLinearVelocity>(_entity);
+        if (worldLinVelComp)
+        {
+          auto state = worldLinVelComp->SetData(
+                math::eigen3::convert(frameData.linearVelocity),
                 this->vec3Eql) ?
                 ComponentState::PeriodicChange :
                 ComponentState::NoChange;
-            _ecm.SetChanged(_entity,
-                components::WorldAngularVelocity::typeId, state);
-          }
+          _ecm.SetChanged(_entity,
+              components::WorldLinearVelocity::typeId, state);
+        }
 
-          // Acceleration in world frame coordinates
-          auto worldLinAccelComp =
-              _ecm.Component<components::WorldLinearAcceleration>(_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<components::WorldAngularVelocity>(_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<components::WorldAngularAcceleration>(_entity);
+        // Acceleration in world frame coordinates
+        auto worldLinAccelComp =
+            _ecm.Component<components::WorldLinearAcceleration>(_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<components::WorldAngularAcceleration>(_entity);
 
-          const Eigen::Matrix3d R_bs = worldPose.linear().transpose(); // NOLINT
+        if (worldAngAccelComp)
+        {
+          auto state = worldAngAccelComp->SetData(
+              math::eigen3::convert(frameData.angularAcceleration),
+              this->vec3Eql) ?
+              ComponentState::PeriodicChange :
+              ComponentState::NoChange;
+          _ecm.SetChanged(_entity,
+              components::WorldAngularAcceleration::typeId, state);
+        }
 
-          // Velocity in body-fixed frame coordinates
-          auto bodyLinVelComp =
-              _ecm.Component<components::LinearVelocity>(_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);
-          }
+        const Eigen::Matrix3d R_bs = worldPose.linear().transpose(); // NOLINT
 
-          // Angular velocity in body-fixed frame coordinates
-          auto bodyAngVelComp =
-              _ecm.Component<components::AngularVelocity>(_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);
-          }
+        // Velocity in body-fixed frame coordinates
+        auto bodyLinVelComp =
+            _ecm.Component<components::LinearVelocity>(_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);
+        }
 
-          // Acceleration in body-fixed frame coordinates
-          auto bodyLinAccelComp =
-              _ecm.Component<components::LinearAcceleration>(_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 velocity in body-fixed frame coordinates
+        auto bodyAngVelComp =
+            _ecm.Component<components::AngularVelocity>(_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 acceleration in world frame coordinates
-          auto bodyAngAccelComp =
-              _ecm.Component<components::AngularAcceleration>(_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);
-          }
+        // Acceleration in body-fixed frame coordinates
+        auto bodyLinAccelComp =
+            _ecm.Component<components::LinearAcceleration>(_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<components::AngularAcceleration>(_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;
       });
+  IGN_PROFILE_END();
 
   // pose/velocity/acceleration of non-link entities such as sensors /
   // collisions. These get updated only if another system has created a
@@ -1975,6 +2014,7 @@ void PhysicsPrivate::UpdateSim(EntityComponentManager &_ecm)
   // * AngularVelocity
   // * LinearAcceleration
 
+  IGN_PROFILE_BEGIN("Sensors / collisions");
   // world pose
   _ecm.Each<components::Pose, components::WorldPose,
             components::ParentEntity>(
@@ -2071,8 +2111,10 @@ void PhysicsPrivate::UpdateSim(EntityComponentManager &_ecm)
 
         return true;
       });
+  IGN_PROFILE_END();
 
   // Clear reset components
+  IGN_PROFILE_BEGIN("Clear / reset components");
   std::vector<Entity> entitiesPositionReset;
   _ecm.Each<components::JointPositionReset>(
       [&](const Entity &_entity, components::JointPositionReset *) -> bool
@@ -2127,8 +2169,10 @@ void PhysicsPrivate::UpdateSim(EntityComponentManager &_ecm)
         std::fill(_slip->Data().begin(), _slip->Data().end(), 0.0);
         return true;
       });
+  IGN_PROFILE_END();
 
   // Update joint positions
+  IGN_PROFILE_BEGIN("Joints");
   _ecm.Each<components::Joint, components::JointPosition>(
       [&](const Entity &_entity, components::Joint *,
           components::JointPosition *_jointPos) -> bool
@@ -2163,6 +2207,7 @@ void PhysicsPrivate::UpdateSim(EntityComponentManager &_ecm)
         }
         return true;
       });
+  IGN_PROFILE_END();
 
   // TODO(louise) Skip this if there are no collision features
   this->UpdateCollisions(_ecm);

From 6b124f8e74fee56e0e13685a960493ed3c2e33b7 Mon Sep 17 00:00:00 2001
From: Nate Koenig <nkoenig@users.noreply.github.com>
Date: Mon, 1 Mar 2021 13:44:15 -0800
Subject: [PATCH 4/7] Performer count (#652)

* Support particle modification

Signed-off-by: Nate Koenig <nate@openrobotics.org>

* delay removal of particle emitter cmd component

Signed-off-by: Ian Chen <ichen@osrfoundation.org>

* clear cmds

Signed-off-by: Ian Chen <ichen@osrfoundation.org>

* Add entity count to performer detector events

Signed-off-by: Nate Koenig <nate@openrobotics.org>

* udpate documentation and test

Signed-off-by: Nate Koenig <nate@openrobotics.org>

* fix typo

Signed-off-by: Ian Chen <ichen@osrfoundation.org>

* Cache top level and static to speed up physics system

Signed-off-by: Louise Poubel <louise@openrobotics.org>

Co-authored-by: Nate Koenig <nate@openrobotics.org>
Co-authored-by: Ian Chen <ichen@osrfoundation.org>
Co-authored-by: Louise Poubel <louise@openrobotics.org>
---
 .../performer_detector/PerformerDetector.cc     |  5 +++++
 .../performer_detector/PerformerDetector.hh     |  4 +++-
 test/integration/performer_detector.cc          | 17 ++++++++++++++---
 3 files changed, 22 insertions(+), 4 deletions(-)

diff --git a/src/systems/performer_detector/PerformerDetector.cc b/src/systems/performer_detector/PerformerDetector.cc
index 9aa1dc3a45..7da45abd73 100644
--- a/src/systems/performer_detector/PerformerDetector.cc
+++ b/src/systems/performer_detector/PerformerDetector.cc
@@ -242,6 +242,11 @@ void PerformerDetector::Publish(
     headerData->set_key("state");
     headerData->add_value(std::to_string(_state));
   }
+  {
+    auto *headerData = msg.mutable_header()->add_data();
+    headerData->set_key("count");
+    headerData->add_value(std::to_string(this->detectedEntities.size()));
+  }
 
   // Include the optional extra header data.
   for (const auto &data : this->extraHeaderData)
diff --git a/src/systems/performer_detector/PerformerDetector.hh b/src/systems/performer_detector/PerformerDetector.hh
index 58b6fdc326..24df42a580 100644
--- a/src/systems/performer_detector/PerformerDetector.hh
+++ b/src/systems/performer_detector/PerformerDetector.hh
@@ -51,7 +51,9 @@ namespace systems
   /// header will contain the key "frame_id" with a value set to the name of
   /// the model containing the PerformerDetector system and the key "state" with
   /// a value set to "1" if the performer is entering the detector's region and
-  /// "0" if the performer is leaving the region.
+  /// "0" if the performer is leaving the region. The `data` field of the
+  /// header will also contain the key "count" with a value set to the
+  /// number of performers currently in the region.
   ///
   /// The PerformerDetector has to be attached to a <model> and it's region is
   /// centered on the containing model's origin.
diff --git a/test/integration/performer_detector.cc b/test/integration/performer_detector.cc
index 58f7be3af4..ff851cf0af 100644
--- a/test/integration/performer_detector.cc
+++ b/test/integration/performer_detector.cc
@@ -74,8 +74,9 @@ TEST_F(PerformerDetectorTest, MovingPerformer)
   transport::Node node;
   auto cmdVelPub = node.Advertise<msgs::Twist>("/model/vehicle_blue/cmd_vel");
 
+  std::string expectedCount = "1";
   auto detectorCb = std::function<void(const msgs::Pose &)>(
-      [this](const auto &_msg)
+      [this, &expectedCount](const auto &_msg)
       {
         std::lock_guard<std::mutex> lock(this->poseMsgsMutex);
         this->poseMsgs.push_back(_msg);
@@ -89,6 +90,7 @@ TEST_F(PerformerDetectorTest, MovingPerformer)
 
         bool hasUniqueKey = false;
         bool hasDuplicateKey = false;
+        bool hasCount = false;
         for (int i = 0; i < _msg.header().data_size(); ++i)
         {
           EXPECT_NE(_msg.header().data(i).key(), "no_value");
@@ -104,18 +106,27 @@ TEST_F(PerformerDetectorTest, MovingPerformer)
             EXPECT_EQ(_msg.header().data(i).value(0), "second_value");
             hasDuplicateKey  = true;
           }
+          else if (_msg.header().data(i).key() == "count")
+          {
+            EXPECT_EQ(_msg.header().data(i).value(0), expectedCount);
+            hasCount = true;
+          }
         }
         if (detectorName == "detector1")
         {
-          EXPECT_EQ(4, _msg.header().data_size());
+          EXPECT_EQ(5, _msg.header().data_size());
           EXPECT_TRUE(hasDuplicateKey);
           EXPECT_TRUE(hasUniqueKey);
+          EXPECT_TRUE(hasCount);
         }
         else
         {
-          EXPECT_EQ(2, _msg.header().data_size());
+          EXPECT_EQ(3, _msg.header().data_size());
           EXPECT_FALSE(hasDuplicateKey);
           EXPECT_FALSE(hasUniqueKey);
+          EXPECT_TRUE(hasCount);
+          // Change the expected count after 'detector2' is triggered.
+          expectedCount = "0";
         }
       });
 

From e592cae8ce81a91114a3dc4469bfc5e820bf4bb8 Mon Sep 17 00:00:00 2001
From: "Addisu Z. Taddese" <addisu@openrobotics.org>
Date: Mon, 1 Mar 2021 17:28:36 -0600
Subject: [PATCH 5/7] Use a custom data structure to manage entity feature maps
 (#586)

* Use a custom data structure to manage entity feature maps

This ensures that when a model is removed, all the related physics
entities are removed as well causing the physics engine to clean up the
memory used by the model and its children.

This also fixes a segfault that occurs when ign-gazebo is terminated
after a model is unloaded by the level manager.  The segfault seems to
be due to a bug in DART, but the bug is exposed due to the way children
physics entity pointers outlived their parent models because they
weren't removed from their entity map when the parent model was unloaded.

Signed-off-by: Addisu Z. Taddese <addisu@openrobotics.org>

* Fix collide_bitmask by calling EntityCast after AddEntity

This also uses the Entity of the collision instead of the parent

Signed-off-by: Addisu Z. Taddese <addisu@openrobotics.org>

* Change FeatureLists to structs to improve compile time

Signed-off-by: Addisu Z. Taddese <addisu@openrobotics.org>

* Cleanup and documentation

Signed-off-by: Addisu Z. Taddese <addisu@openrobotics.org>

* Add test for EntityFeatureMap

Signed-off-by: Addisu Z. Taddese <addisu@openrobotics.org>

* Fix documentation

Signed-off-by: Addisu Z. Taddese <addisu@openrobotics.org>
---
 src/systems/physics/CMakeLists.txt           |  10 +
 src/systems/physics/EntityFeatureMap.hh      | 290 +++++++++
 src/systems/physics/EntityFeatureMap_TEST.cc | 173 +++++
 src/systems/physics/Physics.cc               | 637 ++++++++-----------
 src/systems/physics/Physics.hh               |  47 --
 5 files changed, 740 insertions(+), 417 deletions(-)
 create mode 100644 src/systems/physics/EntityFeatureMap.hh
 create mode 100644 src/systems/physics/EntityFeatureMap_TEST.cc

diff --git a/src/systems/physics/CMakeLists.txt b/src/systems/physics/CMakeLists.txt
index 1a118c287c..8e4cfb072e 100644
--- a/src/systems/physics/CMakeLists.txt
+++ b/src/systems/physics/CMakeLists.txt
@@ -7,3 +7,13 @@ gz_add_system(physics
     ignition-physics${IGN_PHYSICS_VER}::ignition-physics${IGN_PHYSICS_VER}
 )
 
+set (gtest_sources
+  EntityFeatureMap_TEST.cc
+)
+
+ign_build_tests(TYPE UNIT
+  SOURCES
+  ${gtest_sources}
+  LIB_DEPS
+  ignition-physics${IGN_PHYSICS_VER}::core
+)
diff --git a/src/systems/physics/EntityFeatureMap.hh b/src/systems/physics/EntityFeatureMap.hh
new file mode 100644
index 0000000000..f9d8fe52c0
--- /dev/null
+++ b/src/systems/physics/EntityFeatureMap.hh
@@ -0,0 +1,290 @@
+/*
+ * 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_ENTITY_FEATURE_MAP_HH_
+#define IGNITION_GAZEBO_SYSTEMS_PHYSICS_ENTITY_FEATURE_MAP_HH_
+
+#include <tuple>
+#include <type_traits>
+#include <unordered_map>
+
+#include <ignition/physics/Entity.hh>
+#include <ignition/physics/FindFeatures.hh>
+#include <ignition/physics/FeatureList.hh>
+#include <ignition/physics/RequestFeatures.hh>
+
+#include "ignition/gazebo/Entity.hh"
+
+namespace ignition::gazebo
+{
+inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE {
+namespace systems::physics_system
+{
+  // \brief Helper class that associates Gazebo entities with Physics entities
+  // with required and optional features. It can be used to cast a physics
+  // entity with the required features to another physics entity with one of
+  // the optional features. This class was created to keep all physics entities
+  // in one place so that when a gazebo entity is removed, all the mapped
+  // physics entitities can be removed at the same time. This ensures that
+  // reference counts are properly zeroed out in the underlying physics engines
+  // and the memory associated with the physics entities can be freed.
+  //
+  // DEV WARNING: There is an implicit conversion between physics EntityPtr and
+  // std::size_t in ign-physics. This seems also implicitly convert between
+  // EntityPtr and gazebo Entity. Therefore, any member function that takes a
+  // gazebo Entity can accidentally take an EntityPtr. To prevent this, for
+  // every function that takes a gazebo Entity, we should always have an
+  // overload that also takes an EntityPtr with required features. We can do
+  // this because there's a 1:1 mapping between the two in maps contained in
+  // this class.
+  //
+  // \tparam PhysicsEntityT Type of entity, such as World, Model, or Link
+  // \tparam PolicyT Policy of the physics engine (2D, 3D)
+  // \tparam RequiredFeatureList Required features of the physics entity
+  // \tparam OptionalFeatureLists One or more optional feature lists of the
+  // physics entity.
+  template <template <typename, typename> class PhysicsEntityT,
+            typename PolicyT, typename RequiredFeatureList,
+            typename... OptionalFeatureLists>
+  class EntityFeatureMap
+  {
+    /// \brief Template that's preset with the class's Policy type
+    /// \tparam T A FeatureList that is used in creating an EntityPtr
+    public: template <typename T>
+    using PhysicsEntityPtr = physics::EntityPtr<PhysicsEntityT<PolicyT, T>>;
+
+    /// \brief A EntityPtr with made from RequiredFeatureList
+    public: using RequiredEntityPtr = PhysicsEntityPtr<RequiredFeatureList>;
+
+    /// \brief Checks whether type T is in the OptionalFeatureLists
+    /// \tparam T A FeatureList to search for in OptionalFeatureLists
+    public: template <typename T>
+            using HasFeatureList =
+                std::disjunction<std::is_same<T, OptionalFeatureLists>...>;
+
+    /// \brief A tuple where the first element is an EntityPtr with made from
+    /// RequiredFeatureList and the remaining elements are made from
+    /// types in OptionalFeatureLists
+    private: using ValueType =
+                 std::tuple<RequiredEntityPtr,
+                            PhysicsEntityPtr<OptionalFeatureLists>...>;
+
+    /// \brief Helper function to cast from an entity type with minimum features
+    /// to an entity with a different set of features. When the entity is cast
+    /// successfully, it is added to an internal cache so that subsequent casts
+    /// will use the entity from the cache.
+    /// \tparam ToFeatureList The list of features of the resulting entity.
+    /// \param[in] _entity Gazebo entity.
+    /// \return Physics entity with features in ToFeatureList. nullptr if the
+    /// entity can't be found or the physics engine doesn't support the
+    /// requested feature.
+    public: template <typename ToFeatureList>
+            PhysicsEntityPtr<ToFeatureList>
+            EntityCast(gazebo::Entity _entity) const
+    {
+      // Using constexpr to limit compiler error message to the static_assert
+      // cppcheck-suppress syntaxError
+      if constexpr (!HasFeatureList<ToFeatureList>::value) // NOLINT
+      {
+        static_assert(HasFeatureList<ToFeatureList>::value,
+            "Trying to cast to a FeatureList not included in the "
+            "optional FeatureLists of this map.");
+        return nullptr;
+      }
+      else
+      {
+        using ToEntityPtr = PhysicsEntityPtr<ToFeatureList>;
+        // Has already been cast
+        auto castIt = this->castCache.find(_entity);
+        if (castIt != this->castCache.end())
+        {
+          auto castEntity = std::get<ToEntityPtr>(castIt->second);
+          if (nullptr != castEntity)
+          {
+            return castEntity;
+          }
+        }
+
+        auto reqEntity = this->Get(_entity);
+        if (nullptr == reqEntity)
+        {
+          return nullptr;
+        }
+
+        // Cast
+        auto castEntity = physics::RequestFeatures<ToFeatureList>::From(
+            this->Get(_entity));
+
+        if (castEntity)
+        {
+          std::get<ToEntityPtr>(this->castCache[_entity]) = castEntity;
+        }
+
+        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
+    /// \tparam ToFeatureList The list of features of the resulting entity.
+    /// \param[in] _entity Physics entity with required features.
+    /// \return Physics entity with features in ToFeatureList. nullptr if the
+    /// entity can't be found or the physics engine doesn't support the
+    /// requested feature.
+    public: template <typename ToFeatureList>
+            PhysicsEntityPtr<ToFeatureList>
+            EntityCast(const RequiredEntityPtr &_physicsEntity) const
+    {
+      auto gzEntity = this->Get(_physicsEntity);
+      if (kNullEntity == gzEntity)
+      {
+        return nullptr;
+      }
+      return this->EntityCast<ToFeatureList>(gzEntity);
+    }
+
+    /// \brief Get the physics entity with required features that corresponds to
+    /// the input Gazebo entity
+    /// \param[in] _entity Gazebo entity.
+    /// \return If found, returns the corresponding physics entity. Otherwise,
+    /// nullptr
+    public: RequiredEntityPtr Get(const Entity &_entity) const
+    {
+      auto it = this->entityMap.find(_entity);
+      if (it != this->entityMap.end())
+      {
+        return it->second;
+      }
+      return nullptr;
+    }
+
+    /// \brief Get Gazebo entity that corresponds to the physics entity with
+    /// required features
+    /// \param[in] _physEntity Physics entity with required features
+    /// \return If found, returns the corresponding Gazebo entity. Otherwise,
+    /// kNullEntity
+    public: Entity Get(const RequiredEntityPtr &_physEntity) const
+    {
+      auto it = this->reverseMap.find(_physEntity);
+      if (it != this->reverseMap.end())
+      {
+        return it->second;
+      }
+      return kNullEntity;
+    }
+
+    /// \brief Check whether there is a physics entity associated with the given
+    /// Gazebo entity
+    /// \param[in] _entity Gazebo entity.
+    /// \return True if the there is a physics entity associated with the given
+    /// Gazebo entity
+    public: bool HasEntity(const Entity &_entity) const
+    {
+      return this->entityMap.find(_entity) != this->entityMap.end();
+    }
+
+    /// \brief Check whether there is a gazebo entity associated with the given
+    /// physics entity
+    /// \param[in] _physicsEntity physics entity with required features.
+    /// \return True if the there is a gazebo entity associated with the given
+    /// physics entity
+    public: bool HasEntity(const RequiredEntityPtr &_physicsEntity) const
+    {
+      return this->reverseMap.find(_physicsEntity) != this->reverseMap.end();
+    }
+
+    /// \brief Add a mapping between gazebo and physics entities
+    /// \param[in] _entity Gazebo entity.
+    /// \param[in] _physicsEntity Physics entity with required feature
+    public: void AddEntity(const Entity &_entity,
+                           const RequiredEntityPtr &_physicsEntity)
+    {
+      this->entityMap[_entity] = _physicsEntity;
+      this->reverseMap[_physicsEntity] = _entity;
+    }
+
+    /// \brief Remove entity from all associated maps
+    /// \param[in] _entity Gazebo entity.
+    /// \return True if the entity was found and removed.
+    public: bool Remove(Entity _entity)
+    {
+      auto it = this->entityMap.find(_entity);
+      if (it != this->entityMap.end())
+      {
+        this->reverseMap.erase(it->second);
+        this->entityMap.erase(it);
+        this->castCache.erase(_entity);
+        return true;
+      }
+      return false;
+    }
+
+    /// \brief Remove physics entity from all associated maps
+    /// \param[in] _entity Gazebo entity.
+    /// \return True if the entity was found and removed.
+    public: bool Remove(const RequiredEntityPtr &_physicsEntity)
+    {
+      auto it = this->reverseMap.find(_physicsEntity);
+      if (it != this->reverseMap.end())
+      {
+        this->entityMap.erase(it->second);
+        this->reverseMap.erase(it);
+        this->castCache.erase(it->second);
+        return true;
+      }
+      return false;
+    }
+
+    /// \brief Get the map from Gazebo entity to physics entities with required
+    /// features
+    /// \return Immumtable entity map
+    public: const std::unordered_map<Entity, RequiredEntityPtr> &Map() const
+    {
+      return this->entityMap;
+    }
+
+    /// \brief Get the total number of entries in the three 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();
+    }
+
+    /// \brief Map from Gazebo entity to physics entities with required features
+    private: std::unordered_map<Entity, RequiredEntityPtr> entityMap;
+
+    /// \brief Reverse map of entityMap
+    private: std::unordered_map<RequiredEntityPtr, Entity> reverseMap;
+
+    /// \brief Cache map from Gazebo entity to physics entities with optional
+    /// features
+    private: mutable std::unordered_map<Entity, ValueType> castCache;
+  };
+
+  /// \brief Convenience template that presets EntityFeatureMap with
+  /// FeaturePolicy3d
+  template <template <typename, typename> class PhysicsEntityT,
+            typename RequiredFeatureList, typename... OptionalFeatureLists>
+  using EntityFeatureMap3d =
+      EntityFeatureMap<PhysicsEntityT, physics::FeaturePolicy3d,
+                       RequiredFeatureList, OptionalFeatureLists...>;
+}
+}
+}
+#endif
diff --git a/src/systems/physics/EntityFeatureMap_TEST.cc b/src/systems/physics/EntityFeatureMap_TEST.cc
new file mode 100644
index 0000000000..9b842031b5
--- /dev/null
+++ b/src/systems/physics/EntityFeatureMap_TEST.cc
@@ -0,0 +1,173 @@
+/*
+ * 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 "EntityFeatureMap.hh"
+
+#include <gtest/gtest.h>
+
+#include <ignition/physics/BoxShape.hh>
+#include <ignition/physics/CylinderShape.hh>
+#include <ignition/physics/ConstructEmpty.hh>
+#include <ignition/physics/Entity.hh>
+#include <ignition/physics/ForwardStep.hh>
+#include <ignition/physics/Implements.hh>
+#include <ignition/physics/Link.hh>
+#include <ignition/physics/RemoveEntities.hh>
+#include <ignition/physics/config.hh>
+#include <ignition/plugin/Loader.hh>
+
+#include "ignition/gazebo/EntityComponentManager.hh"
+
+using namespace ignition;
+using namespace ignition::gazebo::systems::physics_system;
+
+struct MinimumFeatureList
+    : physics::FeatureList<physics::ConstructEmptyWorldFeature,
+                           physics::ConstructEmptyModelFeature,
+                           physics::ConstructEmptyLinkFeature>
+{
+};
+
+using EnginePtrType =
+    physics::EnginePtr<physics::FeaturePolicy3d, MinimumFeatureList>;
+
+
+class EntityFeatureMapFixture: public ::testing::Test
+{
+  protected: void SetUp() override
+  {
+    const std::string pluginLib = "libignition-physics-dartsim-plugin.so";
+
+    common::SystemPaths systemPaths;
+    systemPaths.AddPluginPaths({IGNITION_PHYSICS_ENGINE_INSTALL_DIR});
+
+    auto pathToLib = systemPaths.FindSharedLibrary(pluginLib);
+    ASSERT_FALSE(pathToLib.empty())
+        << "Failed to find plugin [" << pluginLib << "]";
+
+    // Load engine plugin
+    ignition::plugin::Loader pluginLoader;
+    auto plugins = pluginLoader.LoadLib(pathToLib);
+    ASSERT_FALSE(plugins.empty())
+        << "Unable to load the [" << pathToLib << "] library.";
+
+    auto classNames = pluginLoader.AllPlugins();
+    ASSERT_FALSE(classNames.empty())
+        << "No plugins found in library [" << pathToLib << "].";
+
+    for (const auto &className : classNames)
+    {
+      // Get the first plugin that works
+      auto plugin = pluginLoader.Instantiate(className);
+
+      ASSERT_TRUE(plugin) << "Class " << className << " not found in library ["
+                          << pathToLib << "]";
+
+      this->engine =
+          ignition::physics::RequestEngine<physics::FeaturePolicy3d,
+                                           MinimumFeatureList>::From(plugin);
+
+      ASSERT_NE(nullptr, this->engine);
+      break;
+    }
+  }
+
+  public: EnginePtrType engine;
+};
+
+TEST_F(EntityFeatureMapFixture, AddCastRemoveEntity)
+{
+  struct TestOptionalFeatures1
+      : physics::FeatureList<physics::LinkFrameSemantics>
+  {
+  };
+  using TestOptionalFeatures2 = physics::FeatureList<physics::RemoveEntities>;
+
+  using WorldEntityMap =
+      EntityFeatureMap3d<physics::World, MinimumFeatureList,
+                         TestOptionalFeatures1, TestOptionalFeatures2>;
+
+  using WorldPtrType = physics::EntityPtr<
+      physics::World<physics::FeaturePolicy3d, MinimumFeatureList>>;
+
+  // Making these entities different from 1 and 2 ensures that the implicit
+  // conversion in ign-physics between EntityPtr and std::size_t doesn't cause
+  // false positive tests
+  gazebo::Entity gazeboWorld1Entity = 123;
+  gazebo::Entity gazeboWorld2Entity = 456;
+  WorldPtrType testWorld1 = this->engine->ConstructEmptyWorld("world1");
+  WorldEntityMap testMap;
+  EXPECT_FALSE(testMap.HasEntity(gazeboWorld1Entity));
+  EXPECT_EQ(nullptr, testMap.Get(gazeboWorld1Entity));
+  EXPECT_EQ(gazebo::kNullEntity, testMap.Get(testWorld1));
+  EXPECT_EQ(0u, testMap.TotalMapEntryCount());
+
+  testMap.AddEntity(gazeboWorld1Entity, testWorld1);
+
+  // After adding the entity, there should be one entry each in two maps
+  EXPECT_EQ(2u, testMap.TotalMapEntryCount());
+  EXPECT_EQ(testWorld1, testMap.Get(gazeboWorld1Entity));
+  EXPECT_EQ(gazeboWorld1Entity, testMap.Get(testWorld1));
+
+  // Cast to optional feature1
+  auto testWorld1Feature1 =
+      testMap.EntityCast<TestOptionalFeatures1>(gazeboWorld1Entity);
+  ASSERT_NE(nullptr, testWorld1Feature1);
+  // After the cast, there should be one more entry in the cache map.
+  EXPECT_EQ(3u, testMap.TotalMapEntryCount());
+
+  // Cast to optional feature2
+  auto testWorld1Feature2 =
+      testMap.EntityCast<TestOptionalFeatures2>(gazeboWorld1Entity);
+  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());
+
+  // Add another entity
+  WorldPtrType testWorld2 = this->engine->ConstructEmptyWorld("world2");
+  testMap.AddEntity(gazeboWorld2Entity, testWorld2);
+  EXPECT_EQ(5u, testMap.TotalMapEntryCount());
+  EXPECT_EQ(testWorld2, testMap.Get(gazeboWorld2Entity));
+  EXPECT_EQ(gazeboWorld2Entity, testMap.Get(testWorld2));
+
+  auto testWorld2Feature1 =
+      testMap.EntityCast<TestOptionalFeatures1>(testWorld2);
+  ASSERT_NE(nullptr, testWorld2Feature1);
+  // After the cast, there should be one more entry in the cache map.
+  EXPECT_EQ(6u, testMap.TotalMapEntryCount());
+
+  auto testWorld2Feature2 =
+      testMap.EntityCast<TestOptionalFeatures2>(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());
+
+  // 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());
+
+  testMap.Remove(testWorld2);
+  EXPECT_FALSE(testMap.HasEntity(gazeboWorld2Entity));
+  EXPECT_EQ(nullptr, testMap.Get(gazeboWorld2Entity));
+  EXPECT_EQ(gazebo::kNullEntity, testMap.Get(testWorld2));
+  EXPECT_EQ(0u, testMap.TotalMapEntryCount());
+}
diff --git a/src/systems/physics/Physics.cc b/src/systems/physics/Physics.cc
index 86ee7162a7..853dc4d4df 100644
--- a/src/systems/physics/Physics.cc
+++ b/src/systems/physics/Physics.cc
@@ -41,6 +41,27 @@
 #include <ignition/physics/FeaturePolicy.hh>
 #include <ignition/physics/RelativeQuantity.hh>
 #include <ignition/physics/RequestEngine.hh>
+
+#include <ignition/physics/BoxShape.hh>
+#include <ignition/physics/CylinderShape.hh>
+#include <ignition/physics/ForwardStep.hh>
+#include <ignition/physics/FrameSemantics.hh>
+#include <ignition/physics/FreeGroup.hh>
+#include <ignition/physics/FixedJoint.hh>
+#include <ignition/physics/GetContacts.hh>
+#include <ignition/physics/GetBoundingBox.hh>
+#include <ignition/physics/Joint.hh>
+#include <ignition/physics/Link.hh>
+#include <ignition/physics/RemoveEntities.hh>
+#include <ignition/physics/Shape.hh>
+#include <ignition/physics/SphereShape.hh>
+#include <ignition/physics/mesh/MeshShape.hh>
+#include <ignition/physics/sdf/ConstructCollision.hh>
+#include <ignition/physics/sdf/ConstructJoint.hh>
+#include <ignition/physics/sdf/ConstructLink.hh>
+#include <ignition/physics/sdf/ConstructModel.hh>
+#include <ignition/physics/sdf/ConstructNestedModel.hh>
+#include <ignition/physics/sdf/ConstructWorld.hh>
 #include <ignition/plugin/Loader.hh>
 #include <ignition/plugin/PluginPtr.hh>
 #include <ignition/plugin/Register.hh>
@@ -98,8 +119,11 @@
 #include "ignition/gazebo/components/ThreadPitch.hh"
 #include "ignition/gazebo/components/World.hh"
 
+#include "EntityFeatureMap.hh"
+
 using namespace ignition;
 using namespace ignition::gazebo::systems;
+using namespace ignition::gazebo::systems::physics_system;
 namespace components = ignition::gazebo::components;
 
 
@@ -181,22 +205,6 @@ class ignition::gazebo::systems::PhysicsPrivate
   public: ignition::math::Pose3d RelativePose(const Entity &_from,
       const Entity &_to, const EntityComponentManager &_ecm) const;
 
-  /// \brief A map between world entity ids in the ECM to World Entities in
-  /// ign-physics.
-  public: std::unordered_map<Entity, WorldPtrType> entityWorldMap;
-
-  /// \brief A map between model entity ids in the ECM to Model Entities in
-  /// ign-physics.
-  public: std::unordered_map<Entity, ModelPtrType> entityModelMap;
-
-  /// \brief A map between link entity ids in the ECM to Link Entities in
-  /// ign-physics.
-  public: std::unordered_map<Entity, LinkPtrType> entityLinkMap;
-
-  /// \brief Reverse of entityLinkMap. This is used for finding the Entity
-  /// associated with a physics Link
-  public: std::unordered_map<LinkPtrType, Entity> linkEntityMap;
-
   /// \brief A map between model entity ids in the ECM to whether its battery
   /// has drained.
   public: std::unordered_map<Entity, bool> entityOffMap;
@@ -241,223 +249,93 @@ class ignition::gazebo::systems::PhysicsPrivate
   /// \brief Environment variable which holds paths to look for engine plugins
   public: std::string pluginPathEnv = "IGN_GAZEBO_PHYSICS_ENGINE_PATH";
 
+  //////////////////////////////////////////////////
+  ////////////// Optional Features /////////////////
+  //////////////////////////////////////////////////
+
   //////////////////////////////////////////////////
   // Slip Compliance
 
   /// \brief Feature list to process `FrictionPyramidSlipCompliance` components.
-  public: using FrictionPyramidSlipComplianceFeatureList = physics::FeatureList<
+  public: struct FrictionPyramidSlipComplianceFeatureList
+      : physics::FeatureList<
             MinimumFeatureList,
             ignition::physics::GetShapeFrictionPyramidSlipCompliance,
-            ignition::physics::SetShapeFrictionPyramidSlipCompliance>;
-
-  /// \brief Shape type with slip compliance features.
-  public: using ShapeSlipParamPtrType = physics::ShapePtr<
-            physics::FeaturePolicy3d, FrictionPyramidSlipComplianceFeatureList>;
-
-  /// \brief A map between shape entity ids in the ECM to Shape Entities in
-  /// ign-physics
-  /// All shapes on this map are also in `entityCollisionMap`. The difference
-  /// is that here they've been casted for
-  /// `FrictionPyramidSlipComplianceFeatureList`.
-  public: std::unordered_map<Entity, ShapeSlipParamPtrType>
-      entityShapeSlipParamMap;
-
+            ignition::physics::SetShapeFrictionPyramidSlipCompliance>{};
   //////////////////////////////////////////////////
   // Joints
 
   /// \brief Feature list to handle joints.
-  public: using JointFeatureList = ignition::physics::FeatureList<
+  public: struct JointFeatureList : ignition::physics::FeatureList<
             MinimumFeatureList,
             ignition::physics::GetBasicJointProperties,
             ignition::physics::GetBasicJointState,
             ignition::physics::SetBasicJointState,
-            ignition::physics::sdf::ConstructSdfJoint>;
+            ignition::physics::sdf::ConstructSdfJoint>{};
 
-  /// \brief Joint type with joint features.
-  public: using JointPtrType = ignition::physics::JointPtr<
-            ignition::physics::FeaturePolicy3d, JointFeatureList>;
-
-  /// \brief Model type with joint features (models to attach to).
-  public: using ModelJointPtrType = ignition::physics::ModelPtr<
-            ignition::physics::FeaturePolicy3d, JointFeatureList>;
-
-  /// \brief A map between joint entity ids in the ECM to Joint Entities in
-  /// ign-physics
-  public: std::unordered_map<Entity, JointPtrType> entityJointMap;
-
-  /// \brief A map between model entity ids in the ECM to Model Entities in
-  /// ign-physics, with attach feature.
-  /// All models on this map are also in `entityModelMap`. The difference is
-  /// that here they've been casted for `JointFeatureList`.
-  public: std::unordered_map<Entity, ModelJointPtrType> entityModelJointMap;
 
   //////////////////////////////////////////////////
   // Detachable joints
 
   /// \brief Feature list to process `DetachableJoint` components.
-  public: using DetachableJointFeatureList = physics::FeatureList<
+  public: struct DetachableJointFeatureList : physics::FeatureList<
             JointFeatureList,
             physics::AttachFixedJointFeature,
             physics::DetachJointFeature,
-            physics::SetJointTransformFromParentFeature>;
-
-  /// \brief Joint type with detachable joint features.
-  public: using JointDetachableJointPtrType = physics::JointPtr<
-            physics::FeaturePolicy3d, DetachableJointFeatureList>;
-
-  /// \brief Link type with detachable joint features (links to attach to).
-  public: using LinkDetachableJointPtrType = physics::LinkPtr<
-            physics::FeaturePolicy3d, DetachableJointFeatureList>;
-
-  /// \brief A map between joint entity ids in the ECM to Joint Entities in
-  /// ign-physics, with detach feature.
-  /// All joints on this map are also in `entityJointMap`. The difference is
-  /// that here they've been casted for `physics::DetachJointFeature`.
-  public: std::unordered_map<Entity, JointDetachableJointPtrType>
-      entityJointDetachableJointMap;
-
-  /// \brief A map between link entity ids in the ECM to Link Entities in
-  /// ign-physics, with attach feature.
-  /// All links on this map are also in `entityLinkMap`. The difference is
-  /// that here they've been casted for `DetachableJointFeatureList`.
-  public: std::unordered_map<Entity, LinkDetachableJointPtrType>
-      entityLinkDetachableJointMap;
+            physics::SetJointTransformFromParentFeature>{};
 
   //////////////////////////////////////////////////
   // Collisions
 
   /// \brief Feature list to handle collisions.
-  public: using CollisionFeatureList = ignition::physics::FeatureList<
+  public: struct CollisionFeatureList : ignition::physics::FeatureList<
             MinimumFeatureList,
             ignition::physics::GetContactsFromLastStepFeature,
-            ignition::physics::sdf::ConstructSdfCollision>;
+            ignition::physics::sdf::ConstructSdfCollision>{};
 
   /// \brief Collision type with collision features.
   public: using ShapePtrType = ignition::physics::ShapePtr<
             ignition::physics::FeaturePolicy3d, CollisionFeatureList>;
 
-  /// \brief Link type with collision features.
-  public: using LinkShapePtrType = ignition::physics::LinkPtr<
-            ignition::physics::FeaturePolicy3d, CollisionFeatureList>;
-
-  /// \brief World type with collision features.
-  public: using WorldShapePtrType = ignition::physics::WorldPtr<
-            ignition::physics::FeaturePolicy3d, CollisionFeatureList>;
-
   /// \brief World type with just the minimum features. Non-pointer.
   public: using WorldShapeType = ignition::physics::World<
             ignition::physics::FeaturePolicy3d, CollisionFeatureList>;
 
-  /// \brief A map between collision entity ids in the ECM to Shape Entities in
-  /// ign-physics.
-  public: std::unordered_map<Entity, ShapePtrType> entityCollisionMap;
-
-  /// \brief A map between shape entities in ign-physics to collision entities
-  /// in the ECM. This is the reverse map of entityCollisionMap.
-  public: std::unordered_map<ShapePtrType, Entity> collisionEntityMap;
-
-  /// \brief A map between link entity ids in the ECM to Link Entities in
-  /// ign-physics, with attach feature.
-  /// All links on this map are also in `entityLinkMap`. The difference is
-  /// that here they've been casted for `CollisionFeatureList`.
-  public: std::unordered_map<Entity, LinkShapePtrType> entityLinkCollisionMap;
-
-  /// \brief A map between world entity ids in the ECM to World Entities in
-  /// ign-physics, with attach feature.
-  /// All worlds on this map are also in `entityWorldMap`. The difference is
-  /// that here they've been casted for `CollisionFeatureList`.
-  public: std::unordered_map<Entity, WorldShapePtrType> entityWorldCollisionMap;
-
   //////////////////////////////////////////////////
   // Collision filtering with bitmasks
 
   /// \brief Feature list to filter collisions with bitmasks.
-  public: using CollisionMaskFeatureList = ignition::physics::FeatureList<
+  public: struct CollisionMaskFeatureList : ignition::physics::FeatureList<
           CollisionFeatureList,
-          ignition::physics::CollisionFilterMaskFeature>;
-
-  /// \brief Collision type with collision filtering features.
-  public: using ShapeFilterMaskPtrType = ignition::physics::ShapePtr<
-            ignition::physics::FeaturePolicy3d, CollisionMaskFeatureList>;
-
-  /// \brief A map between collision entity ids in the ECM to Shape Entities in
-  /// ign-physics, with collision filtering feature.
-  /// All links on this map are also in `entityCollisionMap`. The difference is
-  /// that here they've been casted for `CollisionMaskFeatureList`.
-  public: std::unordered_map<Entity, ShapeFilterMaskPtrType> entityShapeMaskMap;
+          ignition::physics::CollisionFilterMaskFeature>{};
 
   //////////////////////////////////////////////////
   // Link force
-
   /// \brief Feature list for applying forces to links.
-  public: using LinkForceFeatureList = ignition::physics::FeatureList<
-            ignition::physics::AddLinkExternalForceTorque>;
-
-  /// \brief Link type with bounding box feature.
-  public: using LinkForcePtrType = ignition::physics::LinkPtr<
-            ignition::physics::FeaturePolicy3d, LinkForceFeatureList>;
+  public: struct LinkForceFeatureList : ignition::physics::FeatureList<
+            ignition::physics::AddLinkExternalForceTorque>{};
 
-  /// \brief A map between link entity ids in the ECM to Link Entities in
-  /// ign-physics, with force feature.
-  /// All links on this map are also in `entityLinkMap`. The difference is
-  /// that here they've been casted for `LinkForceFeatureList`.
-  public: std::unordered_map<Entity, LinkForcePtrType> entityLinkForceMap;
 
   //////////////////////////////////////////////////
   // Bounding box
-
   /// \brief Feature list for model bounding box.
-  public: using BoundingBoxFeatureList = ignition::physics::FeatureList<
+  public: struct BoundingBoxFeatureList : ignition::physics::FeatureList<
             MinimumFeatureList,
-            ignition::physics::GetModelBoundingBox>;
-
-  /// \brief Model type with bounding box feature.
-  public: using ModelBoundingBoxPtrType = ignition::physics::ModelPtr<
-            ignition::physics::FeaturePolicy3d, BoundingBoxFeatureList>;
+            ignition::physics::GetModelBoundingBox>{};
 
-  /// \brief A map between model entity ids in the ECM to Model Entities in
-  /// ign-physics, with bounding box feature.
-  /// All models on this map are also in `entityModelMap`. The difference is
-  /// that here they've been casted for `BoundingBoxFeatureList`.
-  public: std::unordered_map<Entity, ModelBoundingBoxPtrType>
-      entityModelBoundingBoxMap;
 
   //////////////////////////////////////////////////
   // Joint velocity command
-
   /// \brief Feature list for set joint velocity command.
-  public: using JointVelocityCommandFeatureList = physics::FeatureList<
-            physics::SetJointVelocityCommandFeature>;
-
-  /// \brief Joint type with set joint velocity command.
-  public: using JointVelocityCommandPtrType = physics::JointPtr<
-            physics::FeaturePolicy3d, JointVelocityCommandFeatureList>;
-
-  /// \brief A map between joint entity ids in the ECM to Joint Entities in
-  /// ign-physics, with velocity command feature.
-  /// All joints on this map are also in `entityJointMap`. The difference is
-  /// that here they've been casted for `JointVelocityCommandFeatureList`.
-  public: std::unordered_map<Entity, JointVelocityCommandPtrType>
-      entityJointVelocityCommandMap;
+  public: struct JointVelocityCommandFeatureList : physics::FeatureList<
+            physics::SetJointVelocityCommandFeature>{};
 
   //////////////////////////////////////////////////
   // World velocity command
-  public: using WorldVelocityCommandFeatureList =
+  public: struct WorldVelocityCommandFeatureList :
             ignition::physics::FeatureList<
-              ignition::physics::SetFreeGroupWorldVelocity>;
+              ignition::physics::SetFreeGroupWorldVelocity>{};
 
-  /// \brief Free group type with world velocity command.
-  public: using FreeGroupVelocityCmdPtrType =
-            ignition::physics::FreeGroupPtr<
-              ignition::physics::FeaturePolicy3d,
-                WorldVelocityCommandFeatureList>;
-
-  /// \brief A map between free group entity ids in the ECM
-  /// to FreeGroup Entities in ign-physics, with velocity command feature.
-  /// All FreeGroup on this map are casted for
-  /// `WorldVelocityCommandFeatureList`.
-  public: std::unordered_map<Entity, FreeGroupVelocityCmdPtrType>
-      entityWorldVelocityCommandMap;
 
   //////////////////////////////////////////////////
   // Meshes
@@ -465,50 +343,89 @@ class ignition::gazebo::systems::PhysicsPrivate
   /// \brief Feature list for meshes.
   /// Include MinimumFeatureList so created collision can be automatically
   /// up-cast.
-  public: using MeshFeatureList = physics::FeatureList<
+  public: struct MeshFeatureList : physics::FeatureList<
             CollisionFeatureList,
-            physics::mesh::AttachMeshShapeFeature>;
-
-  /// \brief Link type with meshes.
-  public: using LinkMeshPtrType = physics::LinkPtr<
-            physics::FeaturePolicy3d, MeshFeatureList>;
-
-  /// \brief A map between link entity ids in the ECM to Link Entities in
-  /// ign-physics, with mesh feature.
-  /// All links on this map are also in `entityLinkMap`. The difference is
-  /// that here they've been casted for `MeshFeatureList`.
-  public: std::unordered_map<Entity, LinkMeshPtrType>
-      entityLinkMeshMap;
+            physics::mesh::AttachMeshShapeFeature>{};
 
   //////////////////////////////////////////////////
   // Nested Models
 
   /// \brief Feature list to construct nested models
-  public: using NestedModelFeatureList = ignition::physics::FeatureList<
+  public: struct NestedModelFeatureList : ignition::physics::FeatureList<
             MinimumFeatureList,
-            ignition::physics::sdf::ConstructSdfNestedModel>;
+            ignition::physics::sdf::ConstructSdfNestedModel>{};
 
-  /// \brief Model type with nested model feature.
-  public: using ModelNestedModelPtrType = physics::ModelPtr<
-            physics::FeaturePolicy3d, NestedModelFeatureList>;
+  //////////////////////////////////////////////////
+  /// \brief World EntityFeatureMap
+  public: using WorldEntityMap = EntityFeatureMap3d<
+          physics::World,
+          MinimumFeatureList,
+          CollisionFeatureList,
+          NestedModelFeatureList>;
 
-  /// \brief World type with nested model feature.
-  public: using WorldNestedModelPtrType = physics::WorldPtr<
-            physics::FeaturePolicy3d, NestedModelFeatureList>;
+  /// \brief A map between world entity ids in the ECM to World Entities in
+  /// ign-physics.
+  public: WorldEntityMap entityWorldMap;
+
+  /// \brief Model EntityFeatureMap
+  public: using ModelEntityMap = EntityFeatureMap3d<
+            physics::Model,
+            MinimumFeatureList,
+            JointFeatureList,
+            BoundingBoxFeatureList,
+            NestedModelFeatureList>;
 
   /// \brief A map between model entity ids in the ECM to Model Entities in
-  /// ign-physics, with Nested Model feature.
-  /// All models on this map are also in `entityModelMap`. The difference is
-  /// that here they've been casted for `ConstructedSdfNestedModel`.
-  public: std::unordered_map<Entity, ModelNestedModelPtrType>
-      entityModelNestedModelMap;
-
-  /// \brief A map between model entity ids in the ECM to World Entities in
-  /// ign-physics, with Nested Model feature.
-  /// All models on this map are also in `entityWorldMap`. The difference is
-  /// that here they've been casted for `ConstructedSdfNestedModel`.
-  public: std::unordered_map<Entity, WorldNestedModelPtrType>
-      entityWorldNestedModelMap;
+  /// ign-physics.
+  public: ModelEntityMap entityModelMap;
+
+  /// \brief Link EntityFeatureMap
+  public: using EntityLinkMap = EntityFeatureMap3d<
+            physics::Link,
+            MinimumFeatureList,
+            DetachableJointFeatureList,
+            CollisionFeatureList,
+            LinkForceFeatureList,
+            MeshFeatureList>;
+
+  /// \brief A map between link entity ids in the ECM to Link Entities in
+  /// ign-physics.
+  public: EntityLinkMap entityLinkMap;
+
+  /// \brief Joint EntityFeatureMap
+  public: using EntityJointMap = EntityFeatureMap3d<
+            physics::Joint,
+            JointFeatureList,
+            DetachableJointFeatureList,
+            JointVelocityCommandFeatureList
+            >;
+
+  /// \brief A map between joint entity ids in the ECM to Joint Entities in
+  /// ign-physics
+  public: EntityJointMap entityJointMap;
+
+  /// \brief Collision EntityFeatureMap
+  public: using EntityCollisionMap = EntityFeatureMap3d<
+            physics::Shape,
+            CollisionFeatureList,
+            CollisionMaskFeatureList,
+            FrictionPyramidSlipComplianceFeatureList
+            >;
+
+  /// \brief A map between collision entity ids in the ECM to Shape Entities in
+  /// ign-physics.
+  public: EntityCollisionMap entityCollisionMap;
+
+  /// \brief FreeGroup EntityFeatureMap
+  public: using EntityFreeGroupMap = EntityFeatureMap3d<
+            physics::FreeGroup,
+            MinimumFeatureList,
+            WorldVelocityCommandFeatureList
+            >;
+
+  /// \brief A map between collision entity ids in the ECM to FreeGroup Entities
+  /// in ign-physics.
+  public: EntityFreeGroupMap entityFreeGroupMap;
 };
 
 //////////////////////////////////////////////////
@@ -674,7 +591,7 @@ void PhysicsPrivate::CreatePhysicsEntities(const EntityComponentManager &_ecm)
         const components::Gravity *_gravity)->bool
       {
         // Check if world already exists
-        if (this->entityWorldMap.find(_entity) != this->entityWorldMap.end())
+        if (this->entityWorldMap.HasEntity(_entity))
         {
           ignwarn << "World entity [" << _entity
                   << "] marked as new, but it's already on the map."
@@ -686,7 +603,7 @@ void PhysicsPrivate::CreatePhysicsEntities(const EntityComponentManager &_ecm)
         world.SetName(_name->Data());
         world.SetGravity(_gravity->Data());
         auto worldPtrPhys = this->engine->ConstructWorld(world);
-        this->entityWorldMap.insert(std::make_pair(_entity, worldPtrPhys));
+        this->entityWorldMap.AddEntity(_entity, worldPtrPhys);
 
         return true;
       });
@@ -700,7 +617,7 @@ void PhysicsPrivate::CreatePhysicsEntities(const EntityComponentManager &_ecm)
           const components::ParentEntity *_parent)->bool
       {
         // Check if model already exists
-        if (this->entityModelMap.find(_entity) != this->entityModelMap.end())
+        if (this->entityModelMap.HasEntity(_entity))
         {
           ignwarn << "Model entity [" << _entity
                   << "] marked as new, but it's already on the map."
@@ -725,16 +642,15 @@ void PhysicsPrivate::CreatePhysicsEntities(const EntityComponentManager &_ecm)
         }
 
         // check if parent is a world
-        auto worldIt = this->entityWorldMap.find(_parent->Data());
-        if (worldIt != this->entityWorldMap.end())
+        if (auto worldPtrPhys =
+                this->entityWorldMap.Get(_parent->Data()))
         {
-          auto worldPtrPhys = worldIt->second;
-
           // Use the ConstructNestedModel feature for nested models
           if (model.ModelCount() > 0)
           {
-            auto nestedModelFeature = entityCast(_parent->Data(), worldPtrPhys,
-                this->entityWorldNestedModelMap);
+            auto nestedModelFeature =
+                this->entityWorldMap.EntityCast<NestedModelFeatureList>(
+                    _parent->Data());
             if (!nestedModelFeature)
             {
               static bool informed{false};
@@ -750,24 +666,22 @@ void PhysicsPrivate::CreatePhysicsEntities(const EntityComponentManager &_ecm)
               return true;
             }
             auto modelPtrPhys = nestedModelFeature->ConstructNestedModel(model);
-            this->entityModelMap.insert(std::make_pair(_entity, modelPtrPhys));
+            this->entityModelMap.AddEntity(_entity, modelPtrPhys);
           }
           else
           {
             auto modelPtrPhys = worldPtrPhys->ConstructModel(model);
-            this->entityModelMap.insert(std::make_pair(_entity, modelPtrPhys));
+            this->entityModelMap.AddEntity(_entity, modelPtrPhys);
           }
         }
         // check if parent is a model (nested model)
         else
         {
-          auto parentIt = this->entityModelMap.find(_parent->Data());
-          if (parentIt != this->entityModelMap.end())
+          if (auto parentPtrPhys = this->entityModelMap.Get(_parent->Data()))
           {
-            auto parentPtrPhys = parentIt->second;
-
-            auto nestedModelFeature = entityCast(_parent->Data(), parentPtrPhys,
-                this->entityModelNestedModelMap);
+            auto nestedModelFeature =
+                this->entityModelMap.EntityCast<NestedModelFeatureList>(
+                    _parent->Data());
             if (!nestedModelFeature)
             {
               static bool informed{false};
@@ -793,8 +707,7 @@ void PhysicsPrivate::CreatePhysicsEntities(const EntityComponentManager &_ecm)
             auto modelPtrPhys = nestedModelFeature->ConstructNestedModel(model);
             if (modelPtrPhys)
             {
-              this->entityModelMap.insert(
-                  std::make_pair(_entity, modelPtrPhys));
+              this->entityModelMap.AddEntity(_entity, modelPtrPhys);
             }
             else
             {
@@ -823,7 +736,7 @@ void PhysicsPrivate::CreatePhysicsEntities(const EntityComponentManager &_ecm)
         const components::ParentEntity *_parent)->bool
       {
         // Check if link already exists
-        if (this->entityLinkMap.find(_entity) != this->entityLinkMap.end())
+        if (this->entityLinkMap.HasEntity(_entity))
         {
           ignwarn << "Link entity [" << _entity
                   << "] marked as new, but it's already on the map."
@@ -834,14 +747,14 @@ void PhysicsPrivate::CreatePhysicsEntities(const EntityComponentManager &_ecm)
         // TODO(anyone) Don't load links unless they have collisions
 
         // Check if parent model exists
-        if (this->entityModelMap.find(_parent->Data())
-            == this->entityModelMap.end())
+        if (!this->entityModelMap.HasEntity(_parent->Data()))
         {
           ignwarn << "Link's parent entity [" << _parent->Data()
                   << "] not found on model map." << std::endl;
           return true;
         }
-        auto modelPtrPhys = this->entityModelMap.at(_parent->Data());
+        auto modelPtrPhys =
+            this->entityModelMap.Get(_parent->Data());
 
         sdf::Link link;
         link.SetName(_name->Data());
@@ -855,8 +768,7 @@ void PhysicsPrivate::CreatePhysicsEntities(const EntityComponentManager &_ecm)
         }
 
         auto linkPtrPhys = modelPtrPhys->ConstructLink(link);
-        this->entityLinkMap.insert(std::make_pair(_entity, linkPtrPhys));
-        this->linkEntityMap.insert(std::make_pair(linkPtrPhys, _entity));
+        this->entityLinkMap.AddEntity(_entity, linkPtrPhys);
 
         return true;
       });
@@ -875,8 +787,7 @@ void PhysicsPrivate::CreatePhysicsEntities(const EntityComponentManager &_ecm)
           const components::CollisionElement *_collElement,
           const components::ParentEntity *_parent) -> bool
       {
-        if (this->entityCollisionMap.find(_entity) !=
-            this->entityCollisionMap.end())
+        if (this->entityCollisionMap.HasEntity(_entity))
         {
            ignwarn << "Collision entity [" << _entity
                    << "] marked as new, but it's already on the map."
@@ -885,14 +796,13 @@ void PhysicsPrivate::CreatePhysicsEntities(const EntityComponentManager &_ecm)
         }
 
         // Check if parent link exists
-        if (this->entityLinkMap.find(_parent->Data())
-            == this->entityLinkMap.end())
+        if (!this->entityLinkMap.HasEntity(_parent->Data()))
         {
           ignwarn << "Collision's parent entity [" << _parent->Data()
                   << "] not found on link map." << std::endl;
           return true;
         }
-        auto linkPtrPhys = this->entityLinkMap.at(_parent->Data());
+        auto linkPtrPhys = this->entityLinkMap.Get(_parent->Data());
 
         // Make a copy of the collision DOM so we can set its pose which has
         // been resolved and is now expressed w.r.t the parent link of the
@@ -923,8 +833,8 @@ void PhysicsPrivate::CreatePhysicsEntities(const EntityComponentManager &_ecm)
             return true;
           }
 
-          auto linkMeshFeature = entityCast(_parent->Data(), linkPtrPhys,
-              this->entityLinkMeshMap);
+          auto linkMeshFeature =
+              this->entityLinkMap.EntityCast<MeshFeatureList>(_parent->Data());
           if (!linkMeshFeature)
           {
             static bool informed{false};
@@ -946,8 +856,9 @@ void PhysicsPrivate::CreatePhysicsEntities(const EntityComponentManager &_ecm)
         }
         else
         {
-          auto linkCollisionFeature = entityCast(_parent->Data(), linkPtrPhys,
-              this->entityLinkCollisionMap);
+          auto linkCollisionFeature =
+              this->entityLinkMap.EntityCast<CollisionFeatureList>(
+                  _parent->Data());
           if (!linkCollisionFeature)
           {
             static bool informed{false};
@@ -965,10 +876,14 @@ void PhysicsPrivate::CreatePhysicsEntities(const EntityComponentManager &_ecm)
           collisionPtrPhys =
               linkCollisionFeature->ConstructCollision(collision);
         }
+
+        this->entityCollisionMap.AddEntity(_entity, collisionPtrPhys);
+
         // Check that the physics engine has a filter mask feature
         // Set the collide_bitmask if it does
-        auto filterMaskFeature = entityCast(_parent->Data(), collisionPtrPhys,
-            entityShapeMaskMap);
+        auto filterMaskFeature =
+            this->entityCollisionMap.EntityCast<CollisionMaskFeatureList>(
+                _entity);
         if (filterMaskFeature)
         {
           filterMaskFeature->SetCollisionFilterMask(collideBitmask);
@@ -985,10 +900,6 @@ void PhysicsPrivate::CreatePhysicsEntities(const EntityComponentManager &_ecm)
           }
         }
 
-        this->entityCollisionMap.insert(
-            std::make_pair(_entity, collisionPtrPhys));
-        this->collisionEntityMap.insert(
-            std::make_pair(collisionPtrPhys, _entity));
         return true;
       });
 
@@ -1008,7 +919,7 @@ void PhysicsPrivate::CreatePhysicsEntities(const EntityComponentManager &_ecm)
           const components::ChildLinkName *_childLinkName) -> bool
       {
         // Check if joint already exists
-        if (this->entityJointMap.find(_entity) != this->entityJointMap.end())
+        if (this->entityJointMap.HasEntity(_entity))
         {
           ignwarn << "Joint entity [" << _entity
                   << "] marked as new, but it's already on the map."
@@ -1017,17 +928,17 @@ void PhysicsPrivate::CreatePhysicsEntities(const EntityComponentManager &_ecm)
         }
 
         // Check if parent model exists
-        if (this->entityModelMap.find(_parentModel->Data())
-            == this->entityModelMap.end())
+        if (!this->entityModelMap.HasEntity(_parentModel->Data()))
         {
           ignwarn << "Joint's parent entity [" << _parentModel->Data()
                   << "] not found on model map." << std::endl;
           return true;
         }
-        auto modelPtrPhys = this->entityModelMap.at(_parentModel->Data());
+        auto modelPtrPhys = this->entityModelMap.Get(_parentModel->Data());
 
-        auto modelJointFeature = entityCast(_parentModel->Data(), modelPtrPhys,
-            this->entityModelJointMap);
+        auto modelJointFeature =
+            this->entityModelMap.EntityCast<JointFeatureList>(
+                _parentModel->Data());
         if (!modelJointFeature)
         {
           static bool informed{false};
@@ -1070,7 +981,7 @@ void PhysicsPrivate::CreatePhysicsEntities(const EntityComponentManager &_ecm)
         {
           // Some joints may not be supported, so only add them to the map if
           // the physics entity is valid
-          this->entityJointMap.insert(std::make_pair(_entity, jointPtrPhys));
+          this->entityJointMap.AddEntity(_entity, jointPtrPhys);
         }
         return true;
       });
@@ -1096,7 +1007,7 @@ void PhysicsPrivate::CreatePhysicsEntities(const EntityComponentManager &_ecm)
           return true;
         }
         // Check if joint already exists
-        if (this->entityJointMap.find(_entity) != this->entityJointMap.end())
+        if (this->entityJointMap.HasEntity(_entity))
         {
           ignwarn << "Joint entity [" << _entity
                   << "] marked as new, but it's already on the map."
@@ -1105,9 +1016,9 @@ void PhysicsPrivate::CreatePhysicsEntities(const EntityComponentManager &_ecm)
         }
 
         // Check if the link entities exist in the physics engine
-        auto parentLinkPhysIt =
-            this->entityLinkMap.find(_jointInfo->Data().parentLink);
-        if (parentLinkPhysIt == this->entityLinkMap.end())
+        auto parentLinkPhys =
+            this->entityLinkMap.Get(_jointInfo->Data().parentLink);
+        if (!parentLinkPhys)
         {
           ignwarn << "DetachableJoint's parent link entity ["
                   << _jointInfo->Data().parentLink << "] not found in link map."
@@ -1118,16 +1029,17 @@ void PhysicsPrivate::CreatePhysicsEntities(const EntityComponentManager &_ecm)
         auto childLinkEntity = _jointInfo->Data().childLink;
 
         // Get child link
-        auto childLinkIt = this->entityLinkMap.find(childLinkEntity);
-        if (childLinkIt == this->entityLinkMap.end())
+        auto childLinkPhys = this->entityLinkMap.Get(childLinkEntity);
+        if (!childLinkPhys)
         {
           ignwarn << "Failed to find joint's child link [" << childLinkEntity
                   << "]." << std::endl;
           return true;
         }
 
-        auto childLinkDetachableJointFeature = entityCast(childLinkEntity,
-            childLinkIt->second, this->entityLinkDetachableJointMap);
+        auto childLinkDetachableJointFeature =
+            this->entityLinkMap.EntityCast<DetachableJointFeatureList>(
+                childLinkEntity);
         if (!childLinkDetachableJointFeature)
         {
           static bool informed{false};
@@ -1145,14 +1057,14 @@ void PhysicsPrivate::CreatePhysicsEntities(const EntityComponentManager &_ecm)
         }
 
         const auto poseParent =
-            parentLinkPhysIt->second->FrameDataRelativeToWorld().pose;
+            parentLinkPhys->FrameDataRelativeToWorld().pose;
         const auto poseChild =
             childLinkDetachableJointFeature->FrameDataRelativeToWorld().pose;
 
         // Pose of child relative to parent
         auto poseParentChild = poseParent.inverse() * poseChild;
-        auto jointPtrPhys = childLinkDetachableJointFeature->AttachFixedJoint(
-            parentLinkPhysIt->second);
+        auto jointPtrPhys =
+            childLinkDetachableJointFeature->AttachFixedJoint(parentLinkPhys);
         if (jointPtrPhys.Valid())
         {
           // We let the joint be at the origin of the child link.
@@ -1160,7 +1072,7 @@ void PhysicsPrivate::CreatePhysicsEntities(const EntityComponentManager &_ecm)
 
           igndbg << "Creating detachable joint [" << _entity << "]"
                  << std::endl;
-          this->entityJointMap.insert(std::make_pair(_entity, jointPtrPhys));
+          this->entityJointMap.AddEntity(_entity, jointPtrPhys);
         }
         else
         {
@@ -1185,8 +1097,7 @@ void PhysicsPrivate::RemovePhysicsEntities(const EntityComponentManager &_ecm)
           /* _model */) -> bool
       {
         // Remove model if found
-        auto modelIt = this->entityModelMap.find(_entity);
-        if (modelIt != this->entityModelMap.end())
+        if (auto modelPtrPhys = this->entityModelMap.Get(_entity))
         {
           // Remove child links, collisions and joints first
           for (const auto &childLink :
@@ -1195,32 +1106,21 @@ void PhysicsPrivate::RemovePhysicsEntities(const EntityComponentManager &_ecm)
             for (const auto &childCollision :
                  _ecm.ChildrenByComponents(childLink, components::Collision()))
             {
-              auto collIt = this->entityCollisionMap.find(childCollision);
-              if (collIt != this->entityCollisionMap.end())
-              {
-                this->collisionEntityMap.erase(collIt->second);
-                this->entityCollisionMap.erase(collIt);
-              }
-            }
-            // First erase the entry associated with this link from the
-            // linkEntityMap which is the reverse of entityLinkMap
-            auto linkPhysIt = this->entityLinkMap.find(childLink);
-            if (linkPhysIt != this->entityLinkMap.end())
-            {
-              this->linkEntityMap.erase(linkPhysIt->second);
+              this->entityCollisionMap.Remove(childCollision);
             }
-            this->entityLinkMap.erase(childLink);
+            this->entityLinkMap.Remove(childLink);
           }
 
           for (const auto &childJoint :
                _ecm.ChildrenByComponents(_entity, components::Joint()))
           {
-            this->entityJointMap.erase(childJoint);
+            this->entityJointMap.Remove(childJoint);
           }
 
+          this->entityFreeGroupMap.Remove(_entity);
           // Remove the model from the physics engine
-          modelIt->second->Remove();
-          this->entityModelMap.erase(_entity);
+          modelPtrPhys->Remove();
+          this->entityModelMap.Remove(_entity);
         }
         return true;
       });
@@ -1228,16 +1128,16 @@ void PhysicsPrivate::RemovePhysicsEntities(const EntityComponentManager &_ecm)
   _ecm.EachRemoved<components::DetachableJoint>(
       [&](const Entity &_entity, const components::DetachableJoint *) -> bool
       {
-        auto jointIt = this->entityJointMap.find(_entity);
-        if (jointIt == this->entityJointMap.end())
+        if (!this->entityJointMap.HasEntity(_entity))
         {
           ignwarn << "Failed to find joint [" << _entity
                   << "]." << std::endl;
           return true;
         }
 
-        auto castEntity = entityCast(_entity, jointIt->second,
-            this->entityJointDetachableJointMap);
+        auto castEntity =
+            this->entityJointMap.EntityCast<DetachableJointFeatureList>(
+                _entity);
         if (!castEntity)
         {
           static bool informed{false};
@@ -1280,17 +1180,17 @@ void PhysicsPrivate::UpdatePhysics(EntityComponentManager &_ecm)
       [&](const Entity &_entity, const components::Joint *,
           const components::Name *_name)
       {
-        auto jointIt = this->entityJointMap.find(_entity);
-        if (jointIt == this->entityJointMap.end())
+        auto jointPhys = this->entityJointMap.Get(_entity);
+        if (nullptr == jointPhys)
           return true;
 
         // Model is out of battery
         if (this->entityOffMap[_ecm.ParentEntity(_entity)])
         {
-          std::size_t nDofs = jointIt->second->GetDegreesOfFreedom();
+          std::size_t nDofs = jointPhys->GetDegreesOfFreedom();
           for (std::size_t i = 0; i < nDofs; ++i)
           {
-            jointIt->second->SetForce(i, 0);
+            jointPhys->SetForce(i, 0);
           }
           return true;
         }
@@ -1305,23 +1205,23 @@ void PhysicsPrivate::UpdatePhysics(EntityComponentManager &_ecm)
         {
           auto& jointVelocity = velReset->Data();
 
-          if (jointVelocity.size() != jointIt->second->GetDegreesOfFreedom())
+          if (jointVelocity.size() != jointPhys->GetDegreesOfFreedom())
           {
             ignwarn << "There is a mismatch in the degrees of freedom "
                     << "between Joint [" << _name->Data() << "(Entity="
                     << _entity << ")] and its JointVelocityReset "
                     << "component. The joint has "
-                    << jointIt->second->GetDegreesOfFreedom()
+                    << jointPhys->GetDegreesOfFreedom()
                     << " while the component has "
                     << jointVelocity.size() << ".\n";
             }
 
             std::size_t nDofs = std::min(
-                jointVelocity.size(), jointIt->second->GetDegreesOfFreedom());
+                jointVelocity.size(), jointPhys->GetDegreesOfFreedom());
 
             for (std::size_t i = 0; i < nDofs; ++i)
             {
-              jointIt->second->SetVelocity(i, jointVelocity[i]);
+              jointPhys->SetVelocity(i, jointVelocity[i]);
             }
         }
 
@@ -1330,21 +1230,21 @@ void PhysicsPrivate::UpdatePhysics(EntityComponentManager &_ecm)
         {
           auto &jointPosition = posReset->Data();
 
-          if (jointPosition.size() != jointIt->second->GetDegreesOfFreedom())
+          if (jointPosition.size() != jointPhys->GetDegreesOfFreedom())
           {
             ignwarn << "There is a mismatch in the degrees of freedom "
                     << "between Joint [" << _name->Data() << "(Entity="
                     << _entity << ")] and its JointPositionyReset "
                     << "component. The joint has "
-                    << jointIt->second->GetDegreesOfFreedom()
+                    << jointPhys->GetDegreesOfFreedom()
                     << " while the component has "
                     << jointPosition.size() << ".\n";
             }
             std::size_t nDofs = std::min(
-                jointPosition.size(), jointIt->second->GetDegreesOfFreedom());
+                jointPosition.size(), jointPhys->GetDegreesOfFreedom());
             for (std::size_t i = 0; i < nDofs; ++i)
             {
-              jointIt->second->SetPosition(i, jointPosition[i]);
+              jointPhys->SetPosition(i, jointPosition[i]);
             }
         }
 
@@ -1353,19 +1253,19 @@ void PhysicsPrivate::UpdatePhysics(EntityComponentManager &_ecm)
 
         if (force)
         {
-          if (force->Data().size() != jointIt->second->GetDegreesOfFreedom())
+          if (force->Data().size() != jointPhys->GetDegreesOfFreedom())
           {
             ignwarn << "There is a mismatch in the degrees of freedom between "
                     << "Joint [" << _name->Data() << "(Entity=" << _entity
                     << ")] and its JointForceCmd component. The joint has "
-                    << jointIt->second->GetDegreesOfFreedom() << " while the "
+                    << jointPhys->GetDegreesOfFreedom() << " while the "
                     << " component has " << force->Data().size() << ".\n";
           }
           std::size_t nDofs = std::min(force->Data().size(),
-                                       jointIt->second->GetDegreesOfFreedom());
+                                       jointPhys->GetDegreesOfFreedom());
           for (std::size_t i = 0; i < nDofs; ++i)
           {
-            jointIt->second->SetForce(i, force->Data()[i]);
+            jointPhys->SetForce(i, force->Data()[i]);
           }
         }
         // Only set joint velocity if joint force is not set.
@@ -1384,19 +1284,20 @@ void PhysicsPrivate::UpdatePhysics(EntityComponentManager &_ecm)
             return true;
           }
 
-          if (velocityCmd.size() != jointIt->second->GetDegreesOfFreedom())
+          if (velocityCmd.size() != jointPhys->GetDegreesOfFreedom())
           {
             ignwarn << "There is a mismatch in the degrees of freedom"
                     << " between Joint [" << _name->Data()
                     << "(Entity=" << _entity<< ")] and its "
                     << "JointVelocityCmd component. The joint has "
-                    << jointIt->second->GetDegreesOfFreedom()
+                    << jointPhys->GetDegreesOfFreedom()
                     << " while the component has "
                     << velocityCmd.size() << ".\n";
           }
 
-          auto jointVelFeature = entityCast(_entity, jointIt->second,
-              this->entityJointVelocityCommandMap);
+          auto jointVelFeature =
+              this->entityJointMap.EntityCast<JointVelocityCommandFeatureList>(
+                  _entity);
           if (!jointVelFeature)
           {
             return true;
@@ -1404,7 +1305,7 @@ void PhysicsPrivate::UpdatePhysics(EntityComponentManager &_ecm)
 
           std::size_t nDofs = std::min(
             velocityCmd.size(),
-            jointIt->second->GetDegreesOfFreedom());
+            jointPhys->GetDegreesOfFreedom());
 
           for (std::size_t i = 0; i < nDofs; ++i)
           {
@@ -1420,16 +1321,15 @@ void PhysicsPrivate::UpdatePhysics(EntityComponentManager &_ecm)
       [&](const Entity &_entity,
           const components::ExternalWorldWrenchCmd *_wrenchComp)
       {
-        auto linkIt = this->entityLinkMap.find(_entity);
-        if (linkIt == this->entityLinkMap.end())
+        if (!this->entityLinkMap.HasEntity(_entity))
         {
           ignwarn << "Failed to find link [" << _entity
                   << "]." << std::endl;
           return true;
         }
 
-        auto linkForceFeature = entityCast(_entity, linkIt->second,
-            this->entityLinkForceMap);
+        auto linkForceFeature =
+            this->entityLinkMap.EntityCast<LinkForceFeatureList>(_entity);
         if (!linkForceFeature)
         {
           static bool informed{false};
@@ -1464,8 +1364,8 @@ void PhysicsPrivate::UpdatePhysics(EntityComponentManager &_ecm)
       {
         this->worldPoseCmdsToRemove.insert(_entity);
 
-        auto modelIt = this->entityModelMap.find(_entity);
-        if (modelIt == this->entityModelMap.end())
+        auto modelPtrPhys = this->entityModelMap.Get(_entity);
+        if (nullptr == modelPtrPhys)
           return true;
 
         // world pose cmd currently not supported for nested models
@@ -1481,21 +1381,21 @@ void PhysicsPrivate::UpdatePhysics(EntityComponentManager &_ecm)
 
         // TODO(addisu) Store the free group instead of searching for it at
         // every iteration
-        auto freeGroup = modelIt->second->FindFreeGroup();
+        auto freeGroup = modelPtrPhys->FindFreeGroup();
         if (!freeGroup)
           return true;
 
         // Get canonical link offset
-        auto linkEntityIt =
-            this->linkEntityMap.find(freeGroup->CanonicalLink());
-        if (linkEntityIt == this->linkEntityMap.end())
+        const auto linkEntity =
+            this->entityLinkMap.Get(freeGroup->CanonicalLink());
+        if (linkEntity == kNullEntity)
           return true;
 
         // set world pose of canonical link in freegroup
         // canonical link might be in a nested model so use RelativePose to get
         // its pose relative to this model
         math::Pose3d linkPose =
-            this->RelativePose(_entity, linkEntityIt->second, _ecm);
+            this->RelativePose(_entity, linkEntity, _ecm);
 
         freeGroup->SetWorldPose(math::eigen3::convert(_poseCmd->Data() *
                                 linkPose));
@@ -1531,15 +1431,15 @@ void PhysicsPrivate::UpdatePhysics(EntityComponentManager &_ecm)
       [&](const Entity &_entity,
           const components::SlipComplianceCmd *_slipCmdComp)
       {
-        auto shapeIt = this->entityCollisionMap.find(_entity);
-        if (shapeIt == this->entityCollisionMap.end())
+        if (!this->entityCollisionMap.HasEntity(_entity))
         {
           ignwarn << "Failed to find shape [" << _entity << "]." << std::endl;
           return true;
         }
 
-        auto slipComplianceShape = entityCast(_entity, shapeIt->second,
-            this->entityShapeSlipParamMap);
+        auto slipComplianceShape =
+            this->entityCollisionMap
+                .EntityCast<FrictionPyramidSlipComplianceFeatureList>(_entity);
 
         if (!slipComplianceShape)
         {
@@ -1567,8 +1467,8 @@ void PhysicsPrivate::UpdatePhysics(EntityComponentManager &_ecm)
       [&](const Entity &_entity, const components::Model *,
           const components::AngularVelocityCmd *_angularVelocityCmd)
       {
-        auto modelIt = this->entityModelMap.find(_entity);
-        if (modelIt == this->entityModelMap.end())
+        auto modelPtrPhys = this->entityModelMap.Get(_entity);
+        if (nullptr == modelPtrPhys)
           return true;
 
         // angular vel cmd currently not supported for nested models
@@ -1579,17 +1479,19 @@ void PhysicsPrivate::UpdatePhysics(EntityComponentManager &_ecm)
           return true;
         }
 
-        auto freeGroup = modelIt->second->FindFreeGroup();
+        auto freeGroup = modelPtrPhys->FindFreeGroup();
         if (!freeGroup)
           return true;
+        this->entityFreeGroupMap.AddEntity(_entity, freeGroup);
 
         const components::Pose *poseComp =
             _ecm.Component<components::Pose>(_entity);
         math::Vector3d worldAngularVel = poseComp->Data().Rot() *
             _angularVelocityCmd->Data();
 
-        auto worldAngularVelFeature = entityCast(_entity, freeGroup,
-            this->entityWorldVelocityCommandMap);
+        auto worldAngularVelFeature =
+            this->entityFreeGroupMap
+                .EntityCast<WorldVelocityCommandFeatureList>(_entity);
         if (!worldAngularVelFeature)
         {
           static bool informed{false};
@@ -1615,8 +1517,8 @@ void PhysicsPrivate::UpdatePhysics(EntityComponentManager &_ecm)
       [&](const Entity &_entity, const components::Model *,
           const components::LinearVelocityCmd *_linearVelocityCmd)
       {
-        auto modelIt = this->entityModelMap.find(_entity);
-        if (modelIt == this->entityModelMap.end())
+        auto modelPtrPhys = this->entityModelMap.Get(_entity);
+        if (nullptr == modelPtrPhys)
           return true;
 
         // linear vel cmd currently not supported for nested models
@@ -1627,17 +1529,20 @@ void PhysicsPrivate::UpdatePhysics(EntityComponentManager &_ecm)
           return true;
         }
 
-        auto freeGroup = modelIt->second->FindFreeGroup();
+        auto freeGroup = modelPtrPhys->FindFreeGroup();
         if (!freeGroup)
           return true;
 
+        this->entityFreeGroupMap.AddEntity(_entity, freeGroup);
+
         const components::Pose *poseComp =
             _ecm.Component<components::Pose>(_entity);
         math::Vector3d worldLinearVel = poseComp->Data().Rot() *
             _linearVelocityCmd->Data();
 
-        auto worldLinearVelFeature = entityCast(_entity, freeGroup,
-              this->entityWorldVelocityCommandMap);
+        auto worldLinearVelFeature =
+            this->entityFreeGroupMap
+                .EntityCast<WorldVelocityCommandFeatureList>(_entity);
         if (!worldLinearVelFeature)
         {
           static bool informed{false};
@@ -1665,15 +1570,15 @@ void PhysicsPrivate::UpdatePhysics(EntityComponentManager &_ecm)
       [&](const Entity &_entity, const components::Model *,
           components::AxisAlignedBox *_bbox)
       {
-        auto modelIt = this->entityModelMap.find(_entity);
-        if (modelIt == this->entityModelMap.end())
+        if (!this->entityModelMap.HasEntity(_entity))
         {
           ignwarn << "Failed to find model [" << _entity << "]." << std::endl;
           return true;
         }
 
-        auto bbModel = entityCast(_entity, modelIt->second,
-            this->entityModelBoundingBoxMap);
+        auto bbModel =
+            this->entityModelMap.EntityCast<BoundingBoxFeatureList>(_entity);
+
         if (!bbModel)
         {
           static bool informed{false};
@@ -1711,7 +1616,7 @@ void PhysicsPrivate::Step(const std::chrono::steady_clock::duration &_dt)
 
   input.Get<std::chrono::steady_clock::duration>() = _dt;
 
-  for (auto &world : this->entityWorldMap)
+  for (const auto &world : this->entityWorldMap.Map())
   {
     world.second->Step(output, state, input);
   }
@@ -1771,8 +1676,7 @@ void PhysicsPrivate::UpdateSim(EntityComponentManager &_ecm)
         if (staticComp && staticComp->Data())
           return true;
 
-        auto linkIt = this->entityLinkMap.find(_entity);
-        if (linkIt != this->entityLinkMap.end())
+        if (auto linkPhys = this->entityLinkMap.Get(_entity))
         {
           // get top level model of this link
           auto topLevelModelEnt = topLevelModel(_parent->Data(), _ecm);
@@ -1780,7 +1684,7 @@ void PhysicsPrivate::UpdateSim(EntityComponentManager &_ecm)
           auto canonicalLink =
               _ecm.Component<components::CanonicalLink>(_entity);
 
-          auto frameData = linkIt->second->FrameDataRelativeToWorld();
+          auto frameData = linkPhys->FrameDataRelativeToWorld();
           const auto &worldPose = frameData.pose;
 
           if (canonicalLink)
@@ -1982,11 +1886,10 @@ void PhysicsPrivate::UpdateSim(EntityComponentManager &_ecm)
           const components::ParentEntity *_parent)->bool
       {
         // check if parent entity is a link, e.g. entity is sensor / collision
-        auto linkIt = this->entityLinkMap.find(_parent->Data());
-        if (linkIt != this->entityLinkMap.end())
+        if (auto linkPhys = this->entityLinkMap.Get(_parent->Data()))
         {
           const auto entityFrameData =
-              this->LinkFrameDataAtOffset(linkIt->second, _pose->Data());
+              this->LinkFrameDataAtOffset(linkPhys, _pose->Data());
 
           *_worldPose = components::WorldPose(
               math::eigen3::convert(entityFrameData.pose));
@@ -2004,11 +1907,10 @@ void PhysicsPrivate::UpdateSim(EntityComponentManager &_ecm)
           const components::ParentEntity *_parent)->bool
       {
         // check if parent entity is a link, e.g. entity is sensor / collision
-        auto linkIt = this->entityLinkMap.find(_parent->Data());
-        if (linkIt != this->entityLinkMap.end())
+        if (auto linkPhys = this->entityLinkMap.Get(_parent->Data()))
         {
           const auto entityFrameData =
-              this->LinkFrameDataAtOffset(linkIt->second, _pose->Data());
+              this->LinkFrameDataAtOffset(linkPhys, _pose->Data());
 
           // set entity world linear velocity
           *_worldLinearVel = components::WorldLinearVelocity(
@@ -2027,11 +1929,10 @@ void PhysicsPrivate::UpdateSim(EntityComponentManager &_ecm)
           const components::ParentEntity *_parent)->bool
       {
         // check if parent entity is a link, e.g. entity is sensor / collision
-        auto linkIt = this->entityLinkMap.find(_parent->Data());
-        if (linkIt != this->entityLinkMap.end())
+        if (auto linkPhys = this->entityLinkMap.Get(_parent->Data()))
         {
           const auto entityFrameData =
-              this->LinkFrameDataAtOffset(linkIt->second, _pose->Data());
+              this->LinkFrameDataAtOffset(linkPhys, _pose->Data());
 
           auto entityWorldPose = math::eigen3::convert(entityFrameData.pose);
           ignition::math::Vector3d entityWorldAngularVel =
@@ -2053,11 +1954,10 @@ void PhysicsPrivate::UpdateSim(EntityComponentManager &_ecm)
           components::LinearAcceleration *_linearAcc,
           const components::ParentEntity *_parent)->bool
       {
-        auto linkIt = this->entityLinkMap.find(_parent->Data());
-        if (linkIt != this->entityLinkMap.end())
+        if (auto linkPhys = this->entityLinkMap.Get(_parent->Data()))
         {
           const auto entityFrameData =
-              this->LinkFrameDataAtOffset(linkIt->second, _pose->Data());
+              this->LinkFrameDataAtOffset(linkPhys, _pose->Data());
 
           auto entityWorldPose = math::eigen3::convert(entityFrameData.pose);
           ignition::math::Vector3d entityWorldLinearAcc =
@@ -2132,14 +2032,13 @@ void PhysicsPrivate::UpdateSim(EntityComponentManager &_ecm)
       [&](const Entity &_entity, components::Joint *,
           components::JointPosition *_jointPos) -> bool
       {
-        auto jointIt = this->entityJointMap.find(_entity);
-        if (jointIt != this->entityJointMap.end())
+        if (auto jointPhys = this->entityJointMap.Get(_entity))
         {
-          _jointPos->Data().resize(jointIt->second->GetDegreesOfFreedom());
-          for (std::size_t i = 0; i < jointIt->second->GetDegreesOfFreedom();
+          _jointPos->Data().resize(jointPhys->GetDegreesOfFreedom());
+          for (std::size_t i = 0; i < jointPhys->GetDegreesOfFreedom();
                ++i)
           {
-            _jointPos->Data()[i] = jointIt->second->GetPosition(i);
+            _jointPos->Data()[i] = jointPhys->GetPosition(i);
           }
         }
         return true;
@@ -2150,14 +2049,13 @@ void PhysicsPrivate::UpdateSim(EntityComponentManager &_ecm)
       [&](const Entity &_entity, components::Joint *,
           components::JointVelocity *_jointVel) -> bool
       {
-        auto jointIt = this->entityJointMap.find(_entity);
-        if (jointIt != this->entityJointMap.end())
+        if (auto jointPhys = this->entityJointMap.Get(_entity))
         {
-          _jointVel->Data().resize(jointIt->second->GetDegreesOfFreedom());
-          for (std::size_t i = 0; i < jointIt->second->GetDegreesOfFreedom();
+          _jointVel->Data().resize(jointPhys->GetDegreesOfFreedom());
+          for (std::size_t i = 0; i < jointPhys->GetDegreesOfFreedom();
                ++i)
           {
-            _jointVel->Data()[i] = jointIt->second->GetVelocity(i);
+            _jointVel->Data()[i] = jointPhys->GetVelocity(i);
           }
         }
         return true;
@@ -2186,15 +2084,14 @@ void PhysicsPrivate::UpdateCollisions(EntityComponentManager &_ecm)
     return;
   }
 
-  auto worldIt = this->entityWorldMap.find(worldEntity);
-  if (worldIt == this->entityWorldMap.end())
+  if (!this->entityWorldMap.HasEntity(worldEntity))
   {
     ignwarn << "Failed to find world [" << worldEntity << "]." << std::endl;
     return;
   }
 
-  auto worldCollisionFeature = entityCast(worldEntity, worldIt->second,
-      this->entityWorldCollisionMap);
+  auto worldCollisionFeature =
+      this->entityWorldMap.EntityCast<CollisionFeatureList>(worldEntity);
   if (!worldCollisionFeature)
   {
     static bool informed{false};
@@ -2228,14 +2125,14 @@ void PhysicsPrivate::UpdateCollisions(EntityComponentManager &_ecm)
   for (const auto &contactComposite : allContacts)
   {
     const auto &contact = contactComposite.Get<WorldShapeType::ContactPoint>();
-    auto coll1It = this->collisionEntityMap.find(contact.collision1);
-    auto coll2It = this->collisionEntityMap.find(contact.collision2);
+    auto coll1Entity = this->entityCollisionMap.Get(contact.collision1);
+    auto coll2Entity = this->entityCollisionMap.Get(contact.collision2);
+
 
-    if ((coll1It != this->collisionEntityMap.end()) &&
-        (coll2It != this->collisionEntityMap.end()))
+    if (coll1Entity != kNullEntity && coll2Entity != kNullEntity)
     {
-      entityContactMap[coll1It->second][coll2It->second].push_back(&contact);
-      entityContactMap[coll2It->second][coll1It->second].push_back(&contact);
+      entityContactMap[coll1Entity][coll2Entity].push_back(&contact);
+      entityContactMap[coll2Entity][coll1Entity].push_back(&contact);
     }
   }
 
diff --git a/src/systems/physics/Physics.hh b/src/systems/physics/Physics.hh
index 255017c4fc..77d6ac8ff5 100644
--- a/src/systems/physics/Physics.hh
+++ b/src/systems/physics/Physics.hh
@@ -86,53 +86,6 @@ namespace systems
     /// \brief Private data pointer.
     private: std::unique_ptr<PhysicsPrivate> dataPtr;
   };
-
-  /// \brief Helper function to cast from an entity type with minimum features
-  /// to an entity with a different set of features. When the entity is cast
-  /// successfully, it is added to _castMap so that subsequent casts will
-  /// use the entity from the map.
-  /// \tparam PolicyT The feature policy, such as
-  /// `ignition::physics::FeaturePolicy3d`.
-  /// \tparam ToFeatureList The list of features of the resulting entity.
-  /// \tparam MinimumFeatureList The minimum list of features.
-  /// \tparam ToEntity Type of entities with ToFeatureList
-  /// \tparam MinimumEntity Type of entities with MinimumFeatureList
-  /// \param[in] _entity Entity ID.
-  /// \param[in] _minimumEntity Entity pointer with minimum features.
-  /// \param[in] _castMap Map to store entities that have already been cast.
-  template <
-      typename PolicyT,
-      typename ToFeatureList,
-      typename MinimumFeatureList,
-      template <typename, typename> class ToEntity,
-      template <typename, typename> class MinimumEntity>
-  physics::EntityPtr<ToEntity<PolicyT, ToFeatureList>> entityCast(
-      Entity _entity,
-      const physics::EntityPtr<MinimumEntity<PolicyT, MinimumFeatureList>>
-        &_minimumEntity,
-      std::unordered_map<Entity, physics::EntityPtr<
-        ToEntity<PolicyT, ToFeatureList>>> &_castMap)
-  {
-    // Has already been cast
-    auto castIt = _castMap.find(_entity);
-    if (castIt != _castMap.end())
-    {
-      return castIt->second;
-    }
-
-    physics::EntityPtr<ToEntity<PolicyT, ToFeatureList>> castEntity;
-
-    // Cast
-    castEntity =
-        physics::RequestFeatures<ToFeatureList>::From(_minimumEntity);
-
-    if (castEntity)
-    {
-      _castMap.insert(std::make_pair(_entity, castEntity));
-    }
-
-    return castEntity;
-  }
 }
 }
 }

From 4e4b3a43bcd5e93b16b08936687a7c0a7fad9052 Mon Sep 17 00:00:00 2001
From: Nate Koenig <nkoenig@users.noreply.github.com>
Date: Mon, 1 Mar 2021 21:46:21 -0800
Subject: [PATCH 6/7] 4.6.0 prep (#660)

Signed-off-by: Nate Koenig <nate@openrobotics.org>

Co-authored-by: Nate Koenig <nate@openrobotics.org>
---
 CMakeLists.txt |  2 +-
 Changelog.md   | 48 ++++++++++++++++++++++++++++++++++++++++++++++++
 2 files changed, 49 insertions(+), 1 deletion(-)

diff --git a/CMakeLists.txt b/CMakeLists.txt
index 378c5ef4bc..fa01df0e0e 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -3,7 +3,7 @@ cmake_minimum_required(VERSION 3.5.1 FATAL_ERROR)
 #============================================================================
 # Initialize the project
 #============================================================================
-project(ignition-gazebo4 VERSION 4.5.0)
+project(ignition-gazebo4 VERSION 4.6.0)
 
 #============================================================================
 # Find ignition-cmake
diff --git a/Changelog.md b/Changelog.md
index 6e030ad5e1..9785e4f911 100644
--- a/Changelog.md
+++ b/Changelog.md
@@ -1,5 +1,53 @@
 ## Ignition Gazebo 4.x
 
+### Ignition Gazebo 4.6.0 (2021-03-01)
+
+1. Use a custom data structure to manage entity feature maps.
+    * [Pull Request 586](https://github.com/ignitionrobotics/ign-gazebo/pull/586)
+
+1. Limit scene broadcast publications when paused.
+    * [Pull Request 497](https://github.com/ignitionrobotics/ign-gazebo/pull/497)
+
+1. Report performer count in PerformerDetector plugin.
+    * [Pull Request 652](https://github.com/ignitionrobotics/ign-gazebo/pull/652)
+
+1. Cache top level and static to speed up physics system.
+    * [Pull Request 656](https://github.com/ignitionrobotics/ign-gazebo/pull/656)
+
+1. Support particle emitter modification using partial message.
+    * [Pull Request 651](https://github.com/ignitionrobotics/ign-gazebo/pull/651)
+
+1. Set LD_LIBRARY_PATH on Actions CI.
+    * [Pull Request 650](https://github.com/ignitionrobotics/ign-gazebo/pull/650)
+
+1. Fix flaky SceneBroadcaster test.
+    * [Pull Request 641](https://github.com/ignitionrobotics/ign-gazebo/pull/641)
+
+1. Add a convenience function for getting possibly non-existing components.
+    * [Pull Request 629](https://github.com/ignitionrobotics/ign-gazebo/pull/629)
+
+1. Add msg to show the computed temperature range computed from temperature
+   gradient.
+    * [Pull Request 643](https://github.com/ignitionrobotics/ign-gazebo/pull/643)
+
+1. Add TF/Pose_V pub in DiffDrive.
+    * [Pull Request 548](https://github.com/ignitionrobotics/ign-gazebo/pull/548)
+
+1. Relax flaky performance test.
+    * [Pull Request 640](https://github.com/ignitionrobotics/ign-gazebo/pull/640)
+
+1. Improve velocity control test.
+    * [Pull Request 642](https://github.com/ignitionrobotics/ign-gazebo/pull/642)
+
+1. Validity check for user defined topics in JointPositionController.
+    * [Pull Request 639](https://github.com/ignitionrobotics/ign-gazebo/pull/639)
+
+1. Add laser_retro support.
+    * [Pull Request 603](https://github.com/ignitionrobotics/ign-gazebo/pull/603)
+
+1. Fix pose of plane visual with non-default normal vector.
+    * [Pull Request 574](https://github.com/ignitionrobotics/ign-gazebo/pull/574)
+
 ### Ignition Gazebo 4.5.0 (2020-02-17)
 
 1. Added particle system.

From 86e6ad93581362eba4e89a58dc1fef058806a081 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Sebastian=20Wallk=C3=B6tter?= <sebastian@wallkoetter.net>
Date: Wed, 3 Mar 2021 09:39:10 -0800
Subject: [PATCH 7/7] Update links v3 -> v4 (#664)
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

* Update links v3 -> v4

Closes: https://github.com/ignitionrobotics/ign-gazebo/issues/663

I've also checked the other links. Some GH ones were pointing to `master`; the branch doesn't exist and I assume it should be `gazebo4`. Also, I've updated all the other links to point to `4.x` instead of `3.x`.

server: 4.0 -> 4.6
gui: 4.0 -> 4.2

Signed-off-by: Sebastian Wallkötter <sebastian@wallkoetter.net>
---
 tutorials/resources.md | 18 +++++++++---------
 1 file changed, 9 insertions(+), 9 deletions(-)

diff --git a/tutorials/resources.md b/tutorials/resources.md
index 28a3c89e9f..84a16b7676 100644
--- a/tutorials/resources.md
+++ b/tutorials/resources.md
@@ -31,16 +31,16 @@ System plugins may be loaded through:
     * Attached to a **model**: `<model><plugin>`
     * Attached to a **sensor**: `<sensor><plugin>`
 * Passing the shared library and class to be loaded through
-  [PluginInfo](https://ignitionrobotics.org/api/gazebo/3.0/classignition_1_1gazebo_1_1ServerConfig_1_1PluginInfo.html)
-  (within [ServerConfig](https://ignitionrobotics.org/api/gazebo/3.0/classignition_1_1gazebo_1_1ServerConfig.html))
+  [PluginInfo](https://ignitionrobotics.org/api/gazebo/4.6/classignition_1_1gazebo_1_1ServerConfig_1_1PluginInfo.html)
+  (within [ServerConfig](https://ignitionrobotics.org/api/gazebo/4.6/classignition_1_1gazebo_1_1ServerConfig.html))
   when instantiating the
-  [Server](https://ignitionrobotics.org/api/gazebo/3.0/classignition_1_1gazebo_1_1Server.html#a084ef7616f5af42061a7aeded5651ab0).
+  [Server](https://ignitionrobotics.org/api/gazebo/4.6/classignition_1_1gazebo_1_1Server.html#a084ef7616f5af42061a7aeded5651ab0).
 
 Ignition will look for system plugins on the following paths, in order:
 
 1. All paths on the `IGN_GAZEBO_SYSTEM_PLUGIN_PATH` environment variable
 2. `$HOME/.ignition/gazebo/plugins`
-3. [Systems that are installed with Ignition Gazebo](https://github.com/ignitionrobotics/ign-gazebo/blob/master/src/systems/)
+3. [Systems that are installed with Ignition Gazebo](https://github.com/ignitionrobotics/ign-gazebo/blob/ign-gazebo4/src/systems/)
 
 ### Ignition GUI plugins
 
@@ -51,7 +51,7 @@ GUI plugins may be loaded through:
 
 * Tags in SDF world files, where `filename` is the shared library:
     * `<world><gui><plugin>`
-* Tags in [GUI config files](https://ignitionrobotics.org/api/gui/3.0/config.html),
+* Tags in [GUI config files](https://ignitionrobotics.org/api/gui/4.2/config.html),
   where `filename` is the shared library:
     * `<plugin>`
 * The plugin menu on the top-right of the screen.
@@ -59,10 +59,10 @@ GUI plugins may be loaded through:
 Ignition will look for GUI plugins on the following paths, in order:
 
 1. All paths set on the `IGN_GUI_PLUGIN_PATH` environment variable
-2. [GUI plugins that are installed with Ignition Gazebo](https://github.com/ignitionrobotics/ign-gazebo/tree/master/src/gui/plugins)
+2. [GUI plugins that are installed with Ignition Gazebo](https://github.com/ignitionrobotics/ign-gazebo/tree/ign-gazebo4/src/gui/plugins)
 3. Other paths added by calling `ignition::gui::App()->AddPluginPath`
 4. `~/.ignition/gui/plugins`
-5. [Plugins which are installed with Ignition GUI](https://ignitionrobotics.org/api/gui/3.0/namespaceignition_1_1gui_1_1plugins.html)
+5. [Plugins which are installed with Ignition GUI](https://ignitionrobotics.org/api/gui/4.2/namespaceignition_1_1gui_1_1plugins.html)
 
 ### Physics engines
 
@@ -111,7 +111,7 @@ Top-level entities such as models, lights and actors may be loaded through:
     * Path / URL to SDF file
     * (TODO) `ignition::msgs::Model`, `ignition::msgs::Light`
 * Within a system, using
-  [SdfEntityCreator](https://ignitionrobotics.org/api/gazebo/3.0/classignition_1_1gazebo_1_1SdfEntityCreator.html)
+  [SdfEntityCreator](https://ignitionrobotics.org/api/gazebo/4.6/classignition_1_1gazebo_1_1SdfEntityCreator.html)
   or directly creating components and entities.
 
 Ignition will look for URIs (path / URL) in the following, in order:
@@ -147,7 +147,7 @@ Ignition will look for URIs (path / URL) in the following, in order:
 ### GUI configuration
 
 Ignition Gazebo's
-[GUI configuration](https://ignitionrobotics.org/api/gui/3.0/config.html)
+[GUI configuration](https://ignitionrobotics.org/api/gui/4.2/config.html)
 can come from the following, in order:
 
 1. The command line option `--gui-config <file path>`