From 1ec2665c33a632b26d1c8f84832efbd3c40422e0 Mon Sep 17 00:00:00 2001
From: Diego Ferigo <diego.ferigo@iit.it>
Date: Thu, 1 Apr 2021 11:59:40 +0200
Subject: [PATCH 1/6] Vendor Physics system

https://github.com/ignitionrobotics/ign-gazebo/commit/1924de9b2286a83424e96ca0d9f99903b4864070
---
 cpp/scenario/plugins/Physics/CMakeLists.txt   |    4 +-
 .../plugins/Physics/EntityFeatureMap.hh       |  313 ++
 cpp/scenario/plugins/Physics/Physics.cc       | 2376 +++++++++++++++
 cpp/scenario/plugins/Physics/Physics.cpp      | 2556 -----------------
 cpp/scenario/plugins/Physics/Physics.h        |  143 -
 cpp/scenario/plugins/Physics/Physics.hh       |   94 +
 6 files changed, 2785 insertions(+), 2701 deletions(-)
 create mode 100644 cpp/scenario/plugins/Physics/EntityFeatureMap.hh
 create mode 100644 cpp/scenario/plugins/Physics/Physics.cc
 delete mode 100644 cpp/scenario/plugins/Physics/Physics.cpp
 delete mode 100644 cpp/scenario/plugins/Physics/Physics.h
 create mode 100644 cpp/scenario/plugins/Physics/Physics.hh

diff --git a/cpp/scenario/plugins/Physics/CMakeLists.txt b/cpp/scenario/plugins/Physics/CMakeLists.txt
index 80fca4065..ce817beb3 100644
--- a/cpp/scenario/plugins/Physics/CMakeLists.txt
+++ b/cpp/scenario/plugins/Physics/CMakeLists.txt
@@ -27,8 +27,8 @@
 # =============
 
 add_library(PhysicsSystem SHARED
-    Physics.h
-    Physics.cpp)
+    Physics.hh
+    Physics.cc)
 
 target_link_libraries(PhysicsSystem
     PUBLIC
diff --git a/cpp/scenario/plugins/Physics/EntityFeatureMap.hh b/cpp/scenario/plugins/Physics/EntityFeatureMap.hh
new file mode 100644
index 000000000..8008f478d
--- /dev/null
+++ b/cpp/scenario/plugins/Physics/EntityFeatureMap.hh
@@ -0,0 +1,313 @@
+/*
+ * 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 Get the physics entity with required features that has a
+    /// particular ID
+    /// \param[in] _id The ID of the desired physics entity
+    /// \return If found, returns the corresponding physics entity. Otherwise,
+    /// nullptr
+    public: RequiredEntityPtr GetPhysicsEntityPtr(std::size_t _id) const
+    {
+      auto it = this->physEntityById.find(_id);
+      if (it != this->physEntityById.end())
+      {
+        return it->second;
+      }
+      return nullptr;
+    }
+
+    /// \brief Check whether there is a physics entity associated with the given
+    /// Gazebo entity
+    /// \param[in] _entity Gazebo entity.
+    /// \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;
+      this->physEntityById[_physicsEntity->EntityID()] = _physicsEntity;
+    }
+
+    /// \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->physEntityById.erase(it->second->EntityID());
+        this->castCache.erase(_entity);
+        this->entityMap.erase(it);
+        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->physEntityById.erase(it->first->EntityID());
+        this->castCache.erase(it->second);
+        this->reverseMap.erase(it);
+        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 maps. Only used for
+    /// testing.
+    /// \return Number of entries in all the maps.
+    public: std::size_t TotalMapEntryCount() const
+    {
+      return this->entityMap.size() + this->reverseMap.size() +
+             this->castCache.size() + this->physEntityById.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 Map of physics entity IDs to the corresponding physics entity
+    /// with required features
+    private: std::unordered_map<std::size_t, RequiredEntityPtr> physEntityById;
+
+    /// \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/cpp/scenario/plugins/Physics/Physics.cc b/cpp/scenario/plugins/Physics/Physics.cc
new file mode 100644
index 000000000..5aeac72fa
--- /dev/null
+++ b/cpp/scenario/plugins/Physics/Physics.cc
@@ -0,0 +1,2376 @@
+/*
+ * Copyright (C) 2018 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 "Physics.hh"
+
+#include <ignition/msgs/contact.pb.h>
+#include <ignition/msgs/contacts.pb.h>
+#include <ignition/msgs/entity.pb.h>
+#include <ignition/msgs/Utility.hh>
+
+#include <algorithm>
+#include <iostream>
+#include <deque>
+#include <string>
+#include <unordered_map>
+#include <unordered_set>
+#include <vector>
+
+#include <ignition/common/MeshManager.hh>
+#include <ignition/common/Profiler.hh>
+#include <ignition/common/SystemPaths.hh>
+#include <ignition/math/AxisAlignedBox.hh>
+#include <ignition/math/eigen3/Conversions.hh>
+#include <ignition/math/Vector3.hh>
+#include <ignition/physics/config.hh>
+#include <ignition/physics/FeatureList.hh>
+#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>
+
+// SDF
+#include <sdf/Collision.hh>
+#include <sdf/Joint.hh>
+#include <sdf/Link.hh>
+#include <sdf/Mesh.hh>
+#include <sdf/Model.hh>
+#include <sdf/Surface.hh>
+#include <sdf/World.hh>
+
+#include "ignition/gazebo/EntityComponentManager.hh"
+#include "ignition/gazebo/Util.hh"
+
+// Components
+#include "ignition/gazebo/components/AngularAcceleration.hh"
+#include "ignition/gazebo/components/AngularVelocity.hh"
+#include "ignition/gazebo/components/AngularVelocityCmd.hh"
+#include "ignition/gazebo/components/AxisAlignedBox.hh"
+#include "ignition/gazebo/components/BatterySoC.hh"
+#include "ignition/gazebo/components/CanonicalLink.hh"
+#include "ignition/gazebo/components/ChildLinkName.hh"
+#include "ignition/gazebo/components/Collision.hh"
+#include "ignition/gazebo/components/ContactSensorData.hh"
+#include "ignition/gazebo/components/Geometry.hh"
+#include "ignition/gazebo/components/Gravity.hh"
+#include "ignition/gazebo/components/Inertial.hh"
+#include "ignition/gazebo/components/DetachableJoint.hh"
+#include "ignition/gazebo/components/Joint.hh"
+#include "ignition/gazebo/components/JointAxis.hh"
+#include "ignition/gazebo/components/JointPosition.hh"
+#include "ignition/gazebo/components/JointPositionReset.hh"
+#include "ignition/gazebo/components/JointType.hh"
+#include "ignition/gazebo/components/JointVelocity.hh"
+#include "ignition/gazebo/components/JointVelocityCmd.hh"
+#include "ignition/gazebo/components/JointVelocityReset.hh"
+#include "ignition/gazebo/components/LinearAcceleration.hh"
+#include "ignition/gazebo/components/LinearVelocity.hh"
+#include "ignition/gazebo/components/LinearVelocityCmd.hh"
+#include "ignition/gazebo/components/Link.hh"
+#include "ignition/gazebo/components/Model.hh"
+#include "ignition/gazebo/components/Name.hh"
+#include "ignition/gazebo/components/ParentEntity.hh"
+#include "ignition/gazebo/components/ParentLinkName.hh"
+#include "ignition/gazebo/components/ExternalWorldWrenchCmd.hh"
+#include "ignition/gazebo/components/JointForceCmd.hh"
+#include "ignition/gazebo/components/PhysicsEnginePlugin.hh"
+#include "ignition/gazebo/components/Pose.hh"
+#include "ignition/gazebo/components/PoseCmd.hh"
+#include "ignition/gazebo/components/SelfCollide.hh"
+#include "ignition/gazebo/components/SlipComplianceCmd.hh"
+#include "ignition/gazebo/components/Static.hh"
+#include "ignition/gazebo/components/ThreadPitch.hh"
+#include "ignition/gazebo/components/World.hh"
+
+#include "EntityFeatureMap.hh"
+
+using namespace ignition;
+using namespace ignition::gazebo;
+using namespace ignition::gazebo::systems;
+using namespace ignition::gazebo::systems::physics_system;
+namespace components = ignition::gazebo::components;
+
+
+// Private data class.
+class ignition::gazebo::systems::PhysicsPrivate
+{
+  /// \brief This is the minimum set of features that any physics engine must
+  /// implement to be supported by this system.
+  /// New features can't be added to this list in minor / patch releases, in
+  /// order to maintain backwards compatibility with downstream physics plugins.
+  public: struct MinimumFeatureList : ignition::physics::FeatureList<
+          ignition::physics::FindFreeGroupFeature,
+          ignition::physics::SetFreeGroupWorldPose,
+          ignition::physics::FreeGroupFrameSemantics,
+          ignition::physics::LinkFrameSemantics,
+          ignition::physics::ForwardStep,
+          ignition::physics::RemoveEntities,
+          ignition::physics::sdf::ConstructSdfLink,
+          ignition::physics::sdf::ConstructSdfModel,
+          ignition::physics::sdf::ConstructSdfWorld
+          >{};
+
+  /// \brief Engine type with just the minimum features.
+  public: using EnginePtrType = ignition::physics::EnginePtr<
+            ignition::physics::FeaturePolicy3d, MinimumFeatureList>;
+
+  /// \brief World type with just the minimum features.
+  public: using WorldPtrType = ignition::physics::WorldPtr<
+            ignition::physics::FeaturePolicy3d, MinimumFeatureList>;
+
+  /// \brief Model type with just the minimum features.
+  public: using ModelPtrType = ignition::physics::ModelPtr<
+            ignition::physics::FeaturePolicy3d, MinimumFeatureList>;
+
+  /// \brief Link type with just the minimum features.
+  public: using LinkPtrType = ignition::physics::LinkPtr<
+            ignition::physics::FeaturePolicy3d, MinimumFeatureList>;
+
+  /// \brief Free group type with just the minimum features.
+  public: using FreeGroupPtrType = ignition::physics::FreeGroupPtr<
+            ignition::physics::FeaturePolicy3d, MinimumFeatureList>;
+
+  /// \brief Create physics entities
+  /// \param[in] _ecm Constant reference to ECM.
+  public: void CreatePhysicsEntities(const EntityComponentManager &_ecm);
+
+  /// \brief Remove physics entities if they are removed from the ECM
+  /// \param[in] _ecm Constant reference to ECM.
+  public: void RemovePhysicsEntities(const EntityComponentManager &_ecm);
+
+  /// \brief Update physics from components
+  /// \param[in] _ecm Mutable reference to ECM.
+  public: void UpdatePhysics(EntityComponentManager &_ecm);
+
+  /// \brief Step the simulation for each world
+  /// \param[in] _dt Duration
+  /// \returns Output data from the physics engine (this currently contains
+  /// data for links that experienced a pose change in the physics step)
+  public: ignition::physics::ForwardStep::Output Step(
+              const std::chrono::steady_clock::duration &_dt);
+
+  /// \brief Get data of links that were updated in the latest physics step.
+  /// \param[in] _ecm Mutable reference to ECM.
+  /// \param[in] _updatedLinks Updated link poses from the latest physics step
+  /// that were written to by the physics engine (some physics engines may
+  /// not write this data to ForwardStep::Output. If not, _ecm is used to get
+  /// this updated link pose data).
+  /// \return A map of gazebo link entities to their updated pose data.
+  public: std::unordered_map<Entity, physics::FrameData3d> ChangedLinks(
+              EntityComponentManager &_ecm,
+              const ignition::physics::ForwardStep::Output &_updatedLinks);
+
+  /// \brief Update components from physics simulation
+  /// \param[in] _ecm Mutable reference to ECM.
+  /// \param[in] _linkFrameData Links that experienced a pose change in the
+  /// most recent physics step. The key is the entity of the link, and the
+  /// value is the updated frame data corresponding to that entity.
+  public: void UpdateSim(EntityComponentManager &_ecm,
+              const std::unordered_map<
+                Entity, physics::FrameData3d> &_linkFrameData);
+
+  /// \brief Update collision components from physics simulation
+  /// \param[in] _ecm Mutable reference to ECM.
+  public: void UpdateCollisions(EntityComponentManager &_ecm);
+
+  /// \brief FrameData relative to world at a given offset pose
+  /// \param[in] _link ign-physics link
+  /// \param[in] _pose Offset pose in which to compute the frame data
+  /// \returns FrameData at the given offset pose
+  public: physics::FrameData3d LinkFrameDataAtOffset(
+      const LinkPtrType &_link, const math::Pose3d &_pose) const;
+
+  /// \brief Get transform from one ancestor entity to a descendant entity
+  /// that are in the same model.
+  /// \param[in] _from An ancestor of the _to entity.
+  /// \param[in] _to A descendant of the _from entity.
+  /// \return Pose transform between the two entities
+  public: ignition::math::Pose3d RelativePose(const Entity &_from,
+      const Entity &_to, const EntityComponentManager &_ecm) const;
+
+  /// \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 Keep track of poses for links attached to non-static models.
+  /// This allows for skipping pose updates if a link's pose didn't change
+  /// after a physics step.
+  public: std::unordered_map<Entity, ignition::math::Pose3d> linkWorldPoses;
+
+  /// \brief Keep track of non-static model world poses. Since non-static
+  /// models may not move on a given iteration, we want to keep track of the
+  /// most recent model world pose change that took place.
+  public: std::unordered_map<Entity, math::Pose3d> modelWorldPoses;
+
+  /// \brief A map between model entity ids in the ECM to whether its battery
+  /// 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;
+
+  /// \brief Pointer to the underlying ign-physics Engine entity.
+  public: EnginePtrType engine = nullptr;
+
+  /// \brief Vector3d equality comparison function.
+  public: std::function<bool(const math::Vector3d &, const math::Vector3d &)>
+          vec3Eql { [](const math::Vector3d &_a, const math::Vector3d &_b)
+                    {
+                      return _a.Equal(_b, 1e-6);
+                    }};
+
+  /// \brief Pose3d equality comparison function.
+  public: std::function<bool(const math::Pose3d &, const math::Pose3d &)>
+          pose3Eql { [](const math::Pose3d &_a, const math::Pose3d &_b)
+                     {
+                       return _a.Pos().Equal(_b.Pos(), 1e-6) &&
+                         _a.Rot().Equal(_b.Rot(), 1e-6);
+                     }};
+
+  /// \brief AxisAlignedBox equality comparison function.
+  public: std::function<bool(const math::AxisAlignedBox &,
+          const math::AxisAlignedBox&)>
+          axisAlignedBoxEql { [](const math::AxisAlignedBox &_a,
+                                 const math::AxisAlignedBox &_b)
+                     {
+                       return _a == _b;
+                     }};
+
+  /// \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: struct FrictionPyramidSlipComplianceFeatureList
+      : physics::FeatureList<
+            MinimumFeatureList,
+            ignition::physics::GetShapeFrictionPyramidSlipCompliance,
+            ignition::physics::SetShapeFrictionPyramidSlipCompliance>{};
+  //////////////////////////////////////////////////
+  // Joints
+
+  /// \brief Feature list to handle joints.
+  public: struct JointFeatureList : ignition::physics::FeatureList<
+            MinimumFeatureList,
+            ignition::physics::GetBasicJointProperties,
+            ignition::physics::GetBasicJointState,
+            ignition::physics::SetBasicJointState,
+            ignition::physics::sdf::ConstructSdfJoint>{};
+
+
+  //////////////////////////////////////////////////
+  // Detachable joints
+
+  /// \brief Feature list to process `DetachableJoint` components.
+  public: struct DetachableJointFeatureList : physics::FeatureList<
+            JointFeatureList,
+            physics::AttachFixedJointFeature,
+            physics::DetachJointFeature,
+            physics::SetJointTransformFromParentFeature>{};
+
+  //////////////////////////////////////////////////
+  // Collisions
+
+  /// \brief Feature list to handle collisions.
+  public: struct CollisionFeatureList : ignition::physics::FeatureList<
+            MinimumFeatureList,
+            ignition::physics::GetContactsFromLastStepFeature,
+            ignition::physics::sdf::ConstructSdfCollision>{};
+
+  /// \brief Collision type with collision features.
+  public: using ShapePtrType = ignition::physics::ShapePtr<
+            ignition::physics::FeaturePolicy3d, CollisionFeatureList>;
+
+  /// \brief World type with just the minimum features. Non-pointer.
+  public: using WorldShapeType = ignition::physics::World<
+            ignition::physics::FeaturePolicy3d, CollisionFeatureList>;
+
+  //////////////////////////////////////////////////
+  // Collision filtering with bitmasks
+
+  /// \brief Feature list to filter collisions with bitmasks.
+  public: struct CollisionMaskFeatureList : ignition::physics::FeatureList<
+          CollisionFeatureList,
+          ignition::physics::CollisionFilterMaskFeature>{};
+
+  //////////////////////////////////////////////////
+  // Link force
+  /// \brief Feature list for applying forces to links.
+  public: struct LinkForceFeatureList : ignition::physics::FeatureList<
+            ignition::physics::AddLinkExternalForceTorque>{};
+
+
+  //////////////////////////////////////////////////
+  // Bounding box
+  /// \brief Feature list for model bounding box.
+  public: struct BoundingBoxFeatureList : ignition::physics::FeatureList<
+            MinimumFeatureList,
+            ignition::physics::GetModelBoundingBox>{};
+
+
+  //////////////////////////////////////////////////
+  // Joint velocity command
+  /// \brief Feature list for set joint velocity command.
+  public: struct JointVelocityCommandFeatureList : physics::FeatureList<
+            physics::SetJointVelocityCommandFeature>{};
+
+  //////////////////////////////////////////////////
+  // World velocity command
+  public: struct WorldVelocityCommandFeatureList :
+            ignition::physics::FeatureList<
+              ignition::physics::SetFreeGroupWorldVelocity>{};
+
+
+  //////////////////////////////////////////////////
+  // Meshes
+
+  /// \brief Feature list for meshes.
+  /// Include MinimumFeatureList so created collision can be automatically
+  /// up-cast.
+  public: struct MeshFeatureList : physics::FeatureList<
+            CollisionFeatureList,
+            physics::mesh::AttachMeshShapeFeature>{};
+
+  //////////////////////////////////////////////////
+  // Nested Models
+
+  /// \brief Feature list to construct nested models
+  public: struct NestedModelFeatureList : ignition::physics::FeatureList<
+            MinimumFeatureList,
+            ignition::physics::sdf::ConstructSdfNestedModel>{};
+
+  //////////////////////////////////////////////////
+  /// \brief World EntityFeatureMap
+  public: using WorldEntityMap = EntityFeatureMap3d<
+          physics::World,
+          MinimumFeatureList,
+          CollisionFeatureList,
+          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.
+  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;
+};
+
+//////////////////////////////////////////////////
+Physics::Physics() : System(), dataPtr(std::make_unique<PhysicsPrivate>())
+{
+}
+
+//////////////////////////////////////////////////
+void Physics::Configure(const Entity &_entity,
+    const std::shared_ptr<const sdf::Element> &_sdf,
+    EntityComponentManager &_ecm,
+    EventManager &/*_eventMgr*/)
+{
+  std::string pluginLib;
+
+  // 1. Engine from component (from command line / ServerConfig)
+  auto engineComp = _ecm.Component<components::PhysicsEnginePlugin>(_entity);
+  if (engineComp && !engineComp->Data().empty())
+  {
+    pluginLib = engineComp->Data();
+  }
+  // 2. Engine from SDF
+  else if (_sdf->HasElement("engine"))
+  {
+    auto sdfClone = _sdf->Clone();
+    auto engineElem = sdfClone->GetElement("engine");
+    pluginLib = engineElem->Get<std::string>("filename", pluginLib).first;
+  }
+
+  // 3. Use DART by default
+  if (pluginLib.empty())
+  {
+    pluginLib = "libignition-physics-dartsim-plugin.so";
+  }
+
+  // Update component
+  if (!engineComp)
+  {
+    _ecm.CreateComponent(_entity, components::PhysicsEnginePlugin(pluginLib));
+  }
+  else
+  {
+    engineComp->SetData(pluginLib,
+        [](const std::string &_a, const std::string &_b){return _a == _b;});
+  }
+
+  // Find engine shared library
+  // Look in:
+  // * Paths from environment variable
+  // * Engines installed with ign-physics
+  common::SystemPaths systemPaths;
+  systemPaths.SetPluginPathEnv(this->dataPtr->pluginPathEnv);
+  systemPaths.AddPluginPaths({IGNITION_PHYSICS_ENGINE_INSTALL_DIR});
+
+  auto pathToLib = systemPaths.FindSharedLibrary(pluginLib);
+  if (pathToLib.empty())
+  {
+    ignerr << "Failed to find plugin [" << pluginLib
+           << "]. Have you checked the " << this->dataPtr->pluginPathEnv
+           << " environment variable?" << std::endl;
+    return;
+  }
+
+  // Load engine plugin
+  ignition::plugin::Loader pluginLoader;
+  auto plugins = pluginLoader.LoadLib(pathToLib);
+  if (plugins.empty())
+  {
+    ignerr << "Unable to load the [" << pathToLib << "] library.\n";
+    return;
+  }
+
+  auto classNames = pluginLoader.AllPlugins();
+  if (classNames.empty())
+  {
+    ignerr << "No plugins found in library [" << pathToLib << "]." << std::endl;
+    return;
+  }
+
+  // Get the first plugin that works
+  for (auto className : classNames)
+  {
+    auto plugin = pluginLoader.Instantiate(className);
+
+    if (!plugin)
+      continue;
+
+    this->dataPtr->engine = ignition::physics::RequestEngine<
+      ignition::physics::FeaturePolicy3d,
+      PhysicsPrivate::MinimumFeatureList>::From(plugin);
+
+    if (nullptr != this->dataPtr->engine)
+    {
+      igndbg << "Loaded [" << className << "] from library ["
+             << pathToLib << "]" << std::endl;
+      break;
+    }
+
+    auto missingFeatures = ignition::physics::RequestEngine<
+        ignition::physics::FeaturePolicy3d,
+        PhysicsPrivate::MinimumFeatureList>::MissingFeatureNames(plugin);
+
+    std::stringstream msg;
+    msg << "Plugin [" << className << "] misses required features:"
+        << std::endl;
+    for (auto feature : missingFeatures)
+    {
+      msg << "- " << feature << std::endl;
+    }
+    ignwarn << msg.str();
+  }
+
+  if (nullptr == this->dataPtr->engine)
+  {
+    ignerr << "Failed to load a valid physics engine from [" << pathToLib
+           << "]."
+           << std::endl;
+  }
+}
+
+//////////////////////////////////////////////////
+Physics::~Physics() = default;
+
+//////////////////////////////////////////////////
+void Physics::Update(const UpdateInfo &_info, EntityComponentManager &_ecm)
+{
+  IGN_PROFILE("Physics::Update");
+
+  // \TODO(anyone) Support rewind
+  if (_info.dt < std::chrono::steady_clock::duration::zero())
+  {
+    ignwarn << "Detected jump back in time ["
+        << std::chrono::duration_cast<std::chrono::seconds>(_info.dt).count()
+        << "s]. System may not work properly." << std::endl;
+  }
+
+  if (this->dataPtr->engine)
+  {
+    this->dataPtr->CreatePhysicsEntities(_ecm);
+    this->dataPtr->UpdatePhysics(_ecm);
+    ignition::physics::ForwardStep::Output stepOutput;
+    // Only step if not paused.
+    if (!_info.paused)
+    {
+      stepOutput = this->dataPtr->Step(_info.dt);
+    }
+    auto changedLinks = this->dataPtr->ChangedLinks(_ecm, stepOutput);
+    this->dataPtr->UpdateSim(_ecm, changedLinks);
+
+    // Entities scheduled to be removed should be removed from physics after the
+    // simulation step. Otherwise, since the to-be-removed entity still shows up
+    // in the ECM::Each the UpdatePhysics and UpdateSim calls will have an error
+    this->dataPtr->RemovePhysicsEntities(_ecm);
+  }
+}
+
+//////////////////////////////////////////////////
+void PhysicsPrivate::CreatePhysicsEntities(const EntityComponentManager &_ecm)
+{
+  // Get all the new worlds
+  _ecm.EachNew<components::World, components::Name, components::Gravity>(
+      [&](const Entity &_entity,
+        const components::World * /* _world */,
+        const components::Name *_name,
+        const components::Gravity *_gravity)->bool
+      {
+        // Check if world already exists
+        if (this->entityWorldMap.HasEntity(_entity))
+        {
+          ignwarn << "World entity [" << _entity
+                  << "] marked as new, but it's already on the map."
+                  << std::endl;
+          return true;
+        }
+
+        sdf::World world;
+        world.SetName(_name->Data());
+        world.SetGravity(_gravity->Data());
+        auto worldPtrPhys = this->engine->ConstructWorld(world);
+        this->entityWorldMap.AddEntity(_entity, worldPtrPhys);
+
+        return true;
+      });
+
+  _ecm.EachNew<components::Model, components::Name, components::Pose,
+            components::ParentEntity>(
+      [&](const Entity &_entity,
+          const components::Model *,
+          const components::Name *_name,
+          const components::Pose *_pose,
+          const components::ParentEntity *_parent)->bool
+      {
+        // Check if model already exists
+        if (this->entityModelMap.HasEntity(_entity))
+        {
+          ignwarn << "Model entity [" << _entity
+                  << "] marked as new, but it's already on the map."
+                  << std::endl;
+          return true;
+        }
+        // TODO(anyone) Don't load models unless they have collisions
+
+        // Check if parent world / model exists
+        sdf::Model model;
+        model.SetName(_name->Data());
+        model.SetRawPose(_pose->Data());
+        auto staticComp = _ecm.Component<components::Static>(_entity);
+        if (staticComp && staticComp->Data())
+        {
+          model.SetStatic(staticComp->Data());
+          this->staticEntities.insert(_entity);
+        }
+        auto selfCollideComp = _ecm.Component<components::SelfCollide>(_entity);
+        if (selfCollideComp && selfCollideComp ->Data())
+        {
+          model.SetSelfCollide(selfCollideComp->Data());
+        }
+
+        // check if parent is a world
+        if (auto worldPtrPhys =
+                this->entityWorldMap.Get(_parent->Data()))
+        {
+          // Use the ConstructNestedModel feature for nested models
+          if (model.ModelCount() > 0)
+          {
+            auto nestedModelFeature =
+                this->entityWorldMap.EntityCast<NestedModelFeatureList>(
+                    _parent->Data());
+            if (!nestedModelFeature)
+            {
+              static bool informed{false};
+              if (!informed)
+              {
+                igndbg << "Attempting to construct nested models, but the "
+                       << "phyiscs engine doesn't support feature "
+                       << "[ConstructSdfNestedModelFeature]. "
+                       << "Nested model will be ignored."
+                       << std::endl;
+                informed = true;
+              }
+              return true;
+            }
+            auto modelPtrPhys = nestedModelFeature->ConstructNestedModel(model);
+            this->entityModelMap.AddEntity(_entity, modelPtrPhys);
+            this->topLevelModelMap.insert(std::make_pair(_entity,
+                topLevelModel(_entity, _ecm)));
+          }
+          else
+          {
+            auto modelPtrPhys = worldPtrPhys->ConstructModel(model);
+            this->entityModelMap.AddEntity(_entity, modelPtrPhys);
+            this->topLevelModelMap.insert(std::make_pair(_entity,
+                topLevelModel(_entity, _ecm)));
+          }
+        }
+        // check if parent is a model (nested model)
+        else
+        {
+          if (auto parentPtrPhys = this->entityModelMap.Get(_parent->Data()))
+          {
+            auto nestedModelFeature =
+                this->entityModelMap.EntityCast<NestedModelFeatureList>(
+                    _parent->Data());
+            if (!nestedModelFeature)
+            {
+              static bool informed{false};
+              if (!informed)
+              {
+                igndbg << "Attempting to construct nested models, but the "
+                       << "physics engine doesn't support feature "
+                       << "[ConstructSdfNestedModelFeature]. "
+                       << "Nested model will be ignored."
+                       << std::endl;
+                informed = true;
+              }
+              return true;
+            }
+
+            // override static property only if parent is static.
+            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.AddEntity(_entity, modelPtrPhys);
+              this->topLevelModelMap.insert(std::make_pair(_entity,
+                  topLevelModel(_entity, _ecm)));
+            }
+            else
+            {
+              ignerr << "Model: '" << _name->Data() << "' not loaded. "
+                     << "Failed to create nested model."
+                     << std::endl;
+            }
+          }
+          else
+          {
+            ignwarn << "Model's parent entity [" << _parent->Data()
+                    << "] not found on world / model map." << std::endl;
+            return true;
+          }
+        }
+
+        return true;
+      });
+
+  _ecm.EachNew<components::Link, components::Name, components::Pose,
+            components::ParentEntity>(
+      [&](const Entity &_entity,
+        const components::Link * /* _link */,
+        const components::Name *_name,
+        const components::Pose *_pose,
+        const components::ParentEntity *_parent)->bool
+      {
+        // Check if link already exists
+        if (this->entityLinkMap.HasEntity(_entity))
+        {
+          ignwarn << "Link entity [" << _entity
+                  << "] marked as new, but it's already on the map."
+                  << std::endl;
+          return true;
+        }
+
+        // TODO(anyone) Don't load links unless they have collisions
+
+        // Check if parent model exists
+        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.Get(_parent->Data());
+
+        sdf::Link link;
+        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)
+        {
+          link.SetInertial(inertial->Data());
+        }
+
+        auto linkPtrPhys = modelPtrPhys->ConstructLink(link);
+        this->entityLinkMap.AddEntity(_entity, linkPtrPhys);
+        this->topLevelModelMap.insert(std::make_pair(_entity,
+            topLevelModel(_entity, _ecm)));
+
+        return true;
+      });
+
+  // We don't need to add visuals to the physics engine.
+
+  // collisions
+  _ecm.EachNew<components::Collision, components::Name, components::Pose,
+            components::Geometry, components::CollisionElement,
+            components::ParentEntity>(
+      [&](const Entity &_entity,
+          const components::Collision *,
+          const components::Name *_name,
+          const components::Pose *_pose,
+          const components::Geometry *_geom,
+          const components::CollisionElement *_collElement,
+          const components::ParentEntity *_parent) -> bool
+      {
+        if (this->entityCollisionMap.HasEntity(_entity))
+        {
+           ignwarn << "Collision entity [" << _entity
+                   << "] marked as new, but it's already on the map."
+                   << std::endl;
+          return true;
+        }
+
+        // Check if parent link exists
+        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.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
+        // collision.
+        sdf::Collision collision = _collElement->Data();
+        collision.SetRawPose(_pose->Data());
+        collision.SetPoseRelativeTo("");
+        auto collideBitmask = collision.Surface()->Contact()->CollideBitmask();
+
+        ShapePtrType collisionPtrPhys;
+        if (_geom->Data().Type() == sdf::GeometryType::MESH)
+        {
+          const sdf::Mesh *meshSdf = _geom->Data().MeshShape();
+          if (nullptr == meshSdf)
+          {
+            ignwarn << "Mesh geometry for collision [" << _name->Data()
+                    << "] missing mesh shape." << std::endl;
+            return true;
+          }
+
+          auto &meshManager = *ignition::common::MeshManager::Instance();
+          auto fullPath = asFullPath(meshSdf->Uri(), meshSdf->FilePath());
+          auto *mesh = meshManager.Load(fullPath);
+          if (nullptr == mesh)
+          {
+            ignwarn << "Failed to load mesh from [" << fullPath
+                    << "]." << std::endl;
+            return true;
+          }
+
+          auto linkMeshFeature =
+              this->entityLinkMap.EntityCast<MeshFeatureList>(_parent->Data());
+          if (!linkMeshFeature)
+          {
+            static bool informed{false};
+            if (!informed)
+            {
+              igndbg << "Attempting to process mesh geometries, but the physics"
+                     << " engine doesn't support feature "
+                     << "[AttachMeshShapeFeature]. Meshes will be ignored."
+                     << std::endl;
+              informed = true;
+            }
+            return true;
+          }
+
+          collisionPtrPhys = linkMeshFeature->AttachMeshShape(_name->Data(),
+              *mesh,
+              math::eigen3::convert(_pose->Data()),
+              math::eigen3::convert(meshSdf->Scale()));
+        }
+        else
+        {
+          auto linkCollisionFeature =
+              this->entityLinkMap.EntityCast<CollisionFeatureList>(
+                  _parent->Data());
+          if (!linkCollisionFeature)
+          {
+            static bool informed{false};
+            if (!informed)
+            {
+              igndbg << "Attempting to process collisions, but the physics "
+                     << "engine doesn't support feature "
+                     << "[ConstructSdfCollision]. Collisions will be ignored."
+                     << std::endl;
+              informed = true;
+            }
+            return true;
+          }
+
+          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 =
+            this->entityCollisionMap.EntityCast<CollisionMaskFeatureList>(
+                _entity);
+        if (filterMaskFeature)
+        {
+          filterMaskFeature->SetCollisionFilterMask(collideBitmask);
+        }
+        else
+        {
+          static bool informed{false};
+          if (!informed)
+          {
+            igndbg << "Attempting to set collision bitmasks, but the physics "
+                   << "engine doesn't support feature [CollisionFilterMask]. "
+                   << "Collision bitmasks will be ignored." << std::endl;
+            informed = true;
+          }
+        }
+
+        this->topLevelModelMap.insert(std::make_pair(_entity,
+            topLevelModel(_entity, _ecm)));
+        return true;
+      });
+
+  // joints
+  _ecm.EachNew<components::Joint, components::Name, components::JointType,
+               components::Pose, components::ThreadPitch,
+               components::ParentEntity, components::ParentLinkName,
+               components::ChildLinkName>(
+      [&](const Entity &_entity,
+          const components::Joint * /* _joint */,
+          const components::Name *_name,
+          const components::JointType *_jointType,
+          const components::Pose *_pose,
+          const components::ThreadPitch *_threadPitch,
+          const components::ParentEntity *_parentModel,
+          const components::ParentLinkName *_parentLinkName,
+          const components::ChildLinkName *_childLinkName) -> bool
+      {
+        // Check if joint already exists
+        if (this->entityJointMap.HasEntity(_entity))
+        {
+          ignwarn << "Joint entity [" << _entity
+                  << "] marked as new, but it's already on the map."
+                  << std::endl;
+          return true;
+        }
+
+        // Check if parent model exists
+        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.Get(_parentModel->Data());
+
+        auto modelJointFeature =
+            this->entityModelMap.EntityCast<JointFeatureList>(
+                _parentModel->Data());
+        if (!modelJointFeature)
+        {
+          static bool informed{false};
+          if (!informed)
+          {
+            igndbg << "Attempting to process joints, but the physics "
+                   << "engine doesn't support joint features. "
+                   << "Joints will be ignored." << std::endl;
+            informed = true;
+          }
+
+          // Break Each call since no joints can be processed
+          return false;
+        }
+
+        sdf::Joint joint;
+        joint.SetName(_name->Data());
+        joint.SetType(_jointType->Data());
+        joint.SetRawPose(_pose->Data());
+        joint.SetThreadPitch(_threadPitch->Data());
+
+        joint.SetParentLinkName(_parentLinkName->Data());
+        joint.SetChildLinkName(_childLinkName->Data());
+
+        auto jointAxis = _ecm.Component<components::JointAxis>(_entity);
+        auto jointAxis2 = _ecm.Component<components::JointAxis2>(_entity);
+
+        // Since we're making copies of the joint axes that were created using
+        // `Model::Load`, frame semantics should work for resolving their xyz
+        // axis
+        if (jointAxis)
+          joint.SetAxis(0, jointAxis->Data());
+        if (jointAxis2)
+          joint.SetAxis(1, jointAxis2->Data());
+
+        // Use the parent link's parent model as the model of this joint
+        auto jointPtrPhys = modelJointFeature->ConstructJoint(joint);
+
+        if (jointPtrPhys.Valid())
+        {
+          // Some joints may not be supported, so only add them to the map if
+          // the physics entity is valid
+          this->entityJointMap.AddEntity(_entity, jointPtrPhys);
+          this->topLevelModelMap.insert(std::make_pair(_entity,
+              topLevelModel(_entity, _ecm)));
+        }
+        return true;
+      });
+
+  _ecm.EachNew<components::BatterySoC>(
+      [&](const Entity & _entity, const components::BatterySoC *)->bool
+      {
+        // Parent entity of battery is model entity
+        this->entityOffMap.insert(std::make_pair(
+          _ecm.ParentEntity(_entity), false));
+        return true;
+      });
+
+  // Detachable joints
+  _ecm.EachNew<components::DetachableJoint>(
+      [&](const Entity &_entity,
+          const components::DetachableJoint *_jointInfo) -> bool
+      {
+        if (_jointInfo->Data().jointType != "fixed")
+        {
+          ignerr << "Detachable joint type [" << _jointInfo->Data().jointType
+                 << "] is currently not supported" << std::endl;
+          return true;
+        }
+        // Check if joint already exists
+        if (this->entityJointMap.HasEntity(_entity))
+        {
+          ignwarn << "Joint entity [" << _entity
+                  << "] marked as new, but it's already on the map."
+                  << std::endl;
+          return true;
+        }
+
+        // Check if the link entities exist in the physics engine
+        auto parentLinkPhys =
+            this->entityLinkMap.Get(_jointInfo->Data().parentLink);
+        if (!parentLinkPhys)
+        {
+          ignwarn << "DetachableJoint's parent link entity ["
+                  << _jointInfo->Data().parentLink << "] not found in link map."
+                  << std::endl;
+          return true;
+        }
+
+        auto childLinkEntity = _jointInfo->Data().childLink;
+
+        // Get child link
+        auto childLinkPhys = this->entityLinkMap.Get(childLinkEntity);
+        if (!childLinkPhys)
+        {
+          ignwarn << "Failed to find joint's child link [" << childLinkEntity
+                  << "]." << std::endl;
+          return true;
+        }
+
+        auto childLinkDetachableJointFeature =
+            this->entityLinkMap.EntityCast<DetachableJointFeatureList>(
+                childLinkEntity);
+        if (!childLinkDetachableJointFeature)
+        {
+          static bool informed{false};
+          if (!informed)
+          {
+            igndbg << "Attempting to create a detachable joint, but the physics"
+                   << " engine doesn't support feature "
+                   << "[AttachFixedJointFeature]. Detachable joints will be "
+                   << "ignored." << std::endl;
+            informed = true;
+          }
+
+          // Break Each call since no DetachableJoints can be processed
+          return false;
+        }
+
+        const auto poseParent =
+            parentLinkPhys->FrameDataRelativeToWorld().pose;
+        const auto poseChild =
+            childLinkDetachableJointFeature->FrameDataRelativeToWorld().pose;
+
+        // Pose of child relative to parent
+        auto poseParentChild = poseParent.inverse() * poseChild;
+        auto jointPtrPhys =
+            childLinkDetachableJointFeature->AttachFixedJoint(parentLinkPhys);
+        if (jointPtrPhys.Valid())
+        {
+          // We let the joint be at the origin of the child link.
+          jointPtrPhys->SetTransformFromParent(poseParentChild);
+
+          igndbg << "Creating detachable joint [" << _entity << "]"
+                 << std::endl;
+          this->entityJointMap.AddEntity(_entity, jointPtrPhys);
+          this->topLevelModelMap.insert(std::make_pair(_entity,
+              topLevelModel(_entity, _ecm)));
+        }
+        else
+        {
+          ignwarn << "DetachableJoint could not be created." << std::endl;
+        }
+        return true;
+      });
+}
+
+//////////////////////////////////////////////////
+void PhysicsPrivate::RemovePhysicsEntities(const EntityComponentManager &_ecm)
+{
+  // Assume the world will not be erased
+  // Only removing models is supported by ign-physics right now so we only
+  // remove links, joints and collisions if they are children of the removed
+  // model.
+  // We assume the links, joints and collisions will be removed from the
+  // physics engine when the containing model gets removed so, here, we only
+  // remove the entities from the gazebo entity->physics entity map.
+  _ecm.EachRemoved<components::Model>(
+      [&](const Entity &_entity, const components::Model *
+          /* _model */) -> bool
+      {
+        // Remove model if found
+        if (auto modelPtrPhys = this->entityModelMap.Get(_entity))
+        {
+          // Remove child links, collisions and joints first
+          for (const auto &childLink :
+               _ecm.ChildrenByComponents(_entity, components::Link()))
+          {
+            for (const auto &childCollision :
+                 _ecm.ChildrenByComponents(childLink, components::Collision()))
+            {
+              this->entityCollisionMap.Remove(childCollision);
+              this->topLevelModelMap.erase(childCollision);
+            }
+            this->entityLinkMap.Remove(childLink);
+            this->topLevelModelMap.erase(childLink);
+            this->staticEntities.erase(childLink);
+            this->linkWorldPoses.erase(childLink);
+          }
+
+          for (const auto &childJoint :
+               _ecm.ChildrenByComponents(_entity, components::Joint()))
+          {
+            this->entityJointMap.Remove(childJoint);
+            this->topLevelModelMap.erase(childJoint);
+          }
+
+          this->entityFreeGroupMap.Remove(_entity);
+          // Remove the model from the physics engine
+          modelPtrPhys->Remove();
+          this->entityModelMap.Remove(_entity);
+          this->topLevelModelMap.erase(_entity);
+          this->staticEntities.erase(_entity);
+          this->modelWorldPoses.erase(_entity);
+        }
+        return true;
+      });
+
+  _ecm.EachRemoved<components::DetachableJoint>(
+      [&](const Entity &_entity, const components::DetachableJoint *) -> bool
+      {
+        if (!this->entityJointMap.HasEntity(_entity))
+        {
+          ignwarn << "Failed to find joint [" << _entity
+                  << "]." << std::endl;
+          return true;
+        }
+
+        auto castEntity =
+            this->entityJointMap.EntityCast<DetachableJointFeatureList>(
+                _entity);
+        if (!castEntity)
+        {
+          static bool informed{false};
+          if (!informed)
+          {
+            igndbg << "Attempting to detach a joint, but the physics "
+                   << "engine doesn't support feature "
+                   << "[DetachJointFeature]. Joint won't be detached."
+                   << std::endl;
+            informed = true;
+          }
+
+          // Break Each call since no DetachableJoints can be processed
+          return false;
+        }
+
+        igndbg << "Detaching joint [" << _entity << "]" << std::endl;
+        castEntity->Detach();
+        return true;
+      });
+}
+
+//////////////////////////////////////////////////
+void PhysicsPrivate::UpdatePhysics(EntityComponentManager &_ecm)
+{
+  IGN_PROFILE("PhysicsPrivate::UpdatePhysics");
+  // Battery state
+  _ecm.Each<components::BatterySoC>(
+      [&](const Entity & _entity, const components::BatterySoC *_bat)
+      {
+        if (_bat->Data() <= 0)
+          entityOffMap[_ecm.ParentEntity(_entity)] = true;
+        else
+          entityOffMap[_ecm.ParentEntity(_entity)] = false;
+        return true;
+      });
+
+  // Handle joint state
+  _ecm.Each<components::Joint, components::Name>(
+      [&](const Entity &_entity, const components::Joint *,
+          const components::Name *_name)
+      {
+        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 = jointPhys->GetDegreesOfFreedom();
+          for (std::size_t i = 0; i < nDofs; ++i)
+          {
+            jointPhys->SetForce(i, 0);
+          }
+          return true;
+        }
+
+        auto posReset = _ecm.Component<components::JointPositionReset>(
+            _entity);
+        auto velReset = _ecm.Component<components::JointVelocityReset>(
+            _entity);
+
+        // Reset the velocity
+        if (velReset)
+        {
+          auto& jointVelocity = velReset->Data();
+
+          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 "
+                    << jointPhys->GetDegreesOfFreedom()
+                    << " while the component has "
+                    << jointVelocity.size() << ".\n";
+            }
+
+            std::size_t nDofs = std::min(
+                jointVelocity.size(), jointPhys->GetDegreesOfFreedom());
+
+            for (std::size_t i = 0; i < nDofs; ++i)
+            {
+              jointPhys->SetVelocity(i, jointVelocity[i]);
+            }
+        }
+
+        // Reset the position
+        if (posReset)
+        {
+          auto &jointPosition = posReset->Data();
+
+          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 "
+                    << jointPhys->GetDegreesOfFreedom()
+                    << " while the component has "
+                    << jointPosition.size() << ".\n";
+            }
+            std::size_t nDofs = std::min(
+                jointPosition.size(), jointPhys->GetDegreesOfFreedom());
+            for (std::size_t i = 0; i < nDofs; ++i)
+            {
+              jointPhys->SetPosition(i, jointPosition[i]);
+            }
+        }
+
+        auto force = _ecm.Component<components::JointForceCmd>(_entity);
+        auto velCmd = _ecm.Component<components::JointVelocityCmd>(_entity);
+
+        if (force)
+        {
+          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 "
+                    << jointPhys->GetDegreesOfFreedom() << " while the "
+                    << " component has " << force->Data().size() << ".\n";
+          }
+          std::size_t nDofs = std::min(force->Data().size(),
+                                       jointPhys->GetDegreesOfFreedom());
+          for (std::size_t i = 0; i < nDofs; ++i)
+          {
+            jointPhys->SetForce(i, force->Data()[i]);
+          }
+        }
+        // Only set joint velocity if joint force is not set.
+        // If both the cmd and reset components are found, cmd is ignored.
+        else if (velCmd)
+        {
+          auto velocityCmd = velCmd->Data();
+
+          if (velReset)
+          {
+            ignwarn << "Found both JointVelocityReset and "
+                    << "JointVelocityCmd components for Joint ["
+                    << _name->Data() << "(Entity=" << _entity
+                    << "]). Ignoring JointVelocityCmd component."
+                    << std::endl;
+            return true;
+          }
+
+          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 "
+                    << jointPhys->GetDegreesOfFreedom()
+                    << " while the component has "
+                    << velocityCmd.size() << ".\n";
+          }
+
+          auto jointVelFeature =
+              this->entityJointMap.EntityCast<JointVelocityCommandFeatureList>(
+                  _entity);
+          if (!jointVelFeature)
+          {
+            return true;
+          }
+
+          std::size_t nDofs = std::min(
+            velocityCmd.size(),
+            jointPhys->GetDegreesOfFreedom());
+
+          for (std::size_t i = 0; i < nDofs; ++i)
+          {
+            jointVelFeature->SetVelocityCommand(i, velocityCmd[i]);
+          }
+        }
+
+        return true;
+      });
+
+  // Link wrenches
+  _ecm.Each<components::ExternalWorldWrenchCmd>(
+      [&](const Entity &_entity,
+          const components::ExternalWorldWrenchCmd *_wrenchComp)
+      {
+        if (!this->entityLinkMap.HasEntity(_entity))
+        {
+          ignwarn << "Failed to find link [" << _entity
+                  << "]." << std::endl;
+          return true;
+        }
+
+        auto linkForceFeature =
+            this->entityLinkMap.EntityCast<LinkForceFeatureList>(_entity);
+        if (!linkForceFeature)
+        {
+          static bool informed{false};
+          if (!informed)
+          {
+            igndbg << "Attempting to apply a wrench, but the physics "
+                   << "engine doesn't support feature "
+                   << "[AddLinkExternalForceTorque]. Wrench will be ignored."
+                   << std::endl;
+            informed = true;
+          }
+
+          // Break Each call since no ExternalWorldWrenchCmd's can be processed
+          return false;
+        }
+
+        math::Vector3 force = msgs::Convert(_wrenchComp->Data().force());
+        math::Vector3 torque = msgs::Convert(_wrenchComp->Data().torque());
+        linkForceFeature->AddExternalForce(math::eigen3::convert(force));
+        linkForceFeature->AddExternalTorque(math::eigen3::convert(torque));
+
+        return true;
+      });
+
+  // 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 modelPtrPhys = this->entityModelMap.Get(_entity);
+        if (nullptr == modelPtrPhys)
+          return true;
+
+        // world pose cmd currently not supported for nested models
+        if (_entity != this->topLevelModelMap[_entity])
+        {
+          ignerr << "Unable to set world pose for nested models."
+                 << std::endl;
+          return true;
+        }
+
+        // TODO(addisu) Store the free group instead of searching for it at
+        // every iteration
+        auto freeGroup = modelPtrPhys->FindFreeGroup();
+        if (!freeGroup)
+          return true;
+
+        // Get root link offset
+        const auto linkEntity =
+            this->entityLinkMap.Get(freeGroup->RootLink());
+        if (linkEntity == kNullEntity)
+          return true;
+
+        // set world pose of root link in freegroup
+        // root link might be in a nested model so use RelativePose to get
+        // its pose relative to this model
+        math::Pose3d linkPose =
+            this->RelativePose(_entity, linkEntity, _ecm);
+
+        freeGroup->SetWorldPose(math::eigen3::convert(_poseCmd->Data() *
+                                linkPose));
+
+        // Process pose commands for static models here, as one-time changes
+        if (this->staticEntities.find(_entity) != this->staticEntities.end())
+        {
+          auto worldPoseComp = _ecm.Component<components::Pose>(_entity);
+          if (worldPoseComp)
+          {
+            auto state = worldPoseComp->SetData(_poseCmd->Data(),
+                this->pose3Eql) ?
+                ComponentState::OneTimeChange :
+                ComponentState::NoChange;
+            _ecm.SetChanged(_entity, components::Pose::typeId, state);
+          }
+        }
+
+        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,
+          const components::SlipComplianceCmd *_slipCmdComp)
+      {
+        if (!this->entityCollisionMap.HasEntity(_entity))
+        {
+          ignwarn << "Failed to find shape [" << _entity << "]." << std::endl;
+          return true;
+        }
+
+        auto slipComplianceShape =
+            this->entityCollisionMap
+                .EntityCast<FrictionPyramidSlipComplianceFeatureList>(_entity);
+
+        if (!slipComplianceShape)
+        {
+          ignwarn << "Can't process Wheel Slip component, physics engine "
+                  << "missing SetShapeFrictionPyramidSlipCompliance"
+                  << std::endl;
+
+          // Break Each call since no SlipCompliances can be processed
+          return false;
+        }
+
+        if (_slipCmdComp->Data().size() == 2)
+        {
+          slipComplianceShape->SetPrimarySlipCompliance(
+              _slipCmdComp->Data()[0]);
+          slipComplianceShape->SetSecondarySlipCompliance(
+              _slipCmdComp->Data()[1]);
+        }
+
+        return true;
+      });
+
+  // Update model angular velocity
+  _ecm.Each<components::Model, components::AngularVelocityCmd>(
+      [&](const Entity &_entity, const components::Model *,
+          const components::AngularVelocityCmd *_angularVelocityCmd)
+      {
+        auto modelPtrPhys = this->entityModelMap.Get(_entity);
+        if (nullptr == modelPtrPhys)
+          return true;
+
+        // angular vel cmd currently not supported for nested models
+        if (_entity != this->topLevelModelMap[_entity])
+        {
+          ignerr << "Unable to set angular velocity for nested models."
+                 << std::endl;
+          return true;
+        }
+
+        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 =
+            this->entityFreeGroupMap
+                .EntityCast<WorldVelocityCommandFeatureList>(_entity);
+        if (!worldAngularVelFeature)
+        {
+          static bool informed{false};
+          if (!informed)
+          {
+            igndbg << "Attempting to set model angular velocity, but the "
+                   << "physics engine doesn't support velocity commands. "
+                   << "Velocity won't be set."
+                   << std::endl;
+            informed = true;
+          }
+          return true;
+        }
+
+        worldAngularVelFeature->SetWorldAngularVelocity(
+            math::eigen3::convert(worldAngularVel));
+
+        return true;
+      });
+
+  // Update model linear velocity
+  _ecm.Each<components::Model, components::LinearVelocityCmd>(
+      [&](const Entity &_entity, const components::Model *,
+          const components::LinearVelocityCmd *_linearVelocityCmd)
+      {
+        auto modelPtrPhys = this->entityModelMap.Get(_entity);
+        if (nullptr == modelPtrPhys)
+          return true;
+
+        // linear vel cmd currently not supported for nested models
+        if (_entity != this->topLevelModelMap[_entity])
+        {
+          ignerr << "Unable to set linear velocity for nested models."
+                 << std::endl;
+          return true;
+        }
+
+        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 =
+            this->entityFreeGroupMap
+                .EntityCast<WorldVelocityCommandFeatureList>(_entity);
+        if (!worldLinearVelFeature)
+        {
+          static bool informed{false};
+          if (!informed)
+          {
+            igndbg << "Attempting to set model linear velocity, but the "
+                   << "physics engine doesn't support velocity commands. "
+                   << "Velocity won't be set."
+                   << std::endl;
+            informed = true;
+          }
+          return true;
+        }
+
+        worldLinearVelFeature->SetWorldLinearVelocity(
+            math::eigen3::convert(worldLinearVel));
+
+        return true;
+      });
+
+  // Populate bounding box info
+  // Only compute bounding box if component exists to avoid unnecessary
+  // computations
+  _ecm.Each<components::Model, components::AxisAlignedBox>(
+      [&](const Entity &_entity, const components::Model *,
+          components::AxisAlignedBox *_bbox)
+      {
+        if (!this->entityModelMap.HasEntity(_entity))
+        {
+          ignwarn << "Failed to find model [" << _entity << "]." << std::endl;
+          return true;
+        }
+
+        auto bbModel =
+            this->entityModelMap.EntityCast<BoundingBoxFeatureList>(_entity);
+
+        if (!bbModel)
+        {
+          static bool informed{false};
+          if (!informed)
+          {
+            igndbg << "Attempting to get a bounding box, but the physics "
+                   << "engine doesn't support feature "
+                   << "[GetModelBoundingBox]. Bounding box won't be populated."
+                   << std::endl;
+            informed = true;
+          }
+
+          // Break Each call since no AxisAlignedBox'es can be processed
+          return false;
+        }
+
+        math::AxisAlignedBox bbox =
+            math::eigen3::convert(bbModel->GetAxisAlignedBoundingBox());
+        auto state = _bbox->SetData(bbox, this->axisAlignedBoxEql) ?
+            ComponentState::OneTimeChange :
+            ComponentState::NoChange;
+        _ecm.SetChanged(_entity, components::AxisAlignedBox::typeId, state);
+
+        return true;
+      });
+}
+
+//////////////////////////////////////////////////
+ignition::physics::ForwardStep::Output PhysicsPrivate::Step(
+    const std::chrono::steady_clock::duration &_dt)
+{
+  IGN_PROFILE("PhysicsPrivate::Step");
+  ignition::physics::ForwardStep::Input input;
+  ignition::physics::ForwardStep::State state;
+  ignition::physics::ForwardStep::Output output;
+
+  input.Get<std::chrono::steady_clock::duration>() = _dt;
+
+  for (const auto &world : this->entityWorldMap.Map())
+  {
+    world.second->Step(output, state, input);
+  }
+
+  return output;
+}
+
+//////////////////////////////////////////////////
+ignition::math::Pose3d PhysicsPrivate::RelativePose(const Entity &_from,
+  const Entity &_to, const EntityComponentManager &_ecm) const
+{
+  math::Pose3d transform;
+
+  if (_from == _to)
+    return transform;
+
+  auto currentEntity = _to;
+  auto parentComp = _ecm.Component<components::ParentEntity>(_to);
+  while (parentComp)
+  {
+    auto parentEntity = parentComp->Data();
+
+    // get the entity pose
+    auto entityPoseComp =
+      _ecm.Component<components::Pose>(currentEntity);
+
+    // update transform
+    transform = entityPoseComp->Data() * transform;
+
+    if (parentEntity == _from)
+      break;
+
+    // set current entity to parent
+    currentEntity = parentEntity;
+
+    // get entity's parent
+    parentComp = _ecm.Component<components::ParentEntity>(
+      parentEntity);
+  }
+
+  return transform;
+}
+
+//////////////////////////////////////////////////
+std::unordered_map<Entity, physics::FrameData3d> PhysicsPrivate::ChangedLinks(
+    EntityComponentManager &_ecm,
+    const ignition::physics::ForwardStep::Output &_updatedLinks)
+{
+  IGN_PROFILE("Links Frame Data");
+
+  std::unordered_map<Entity, physics::FrameData3d> linkFrameData;
+
+  // Check to see if the physics engine gave a list of changed poses. If not, we
+  // will iterate through all of the links via the ECM to see which ones changed
+  if (_updatedLinks.Has<ignition::physics::ChangedWorldPoses>())
+  {
+    for (const auto &link :
+        _updatedLinks.Query<ignition::physics::ChangedWorldPoses>()->entries)
+    {
+      // get the gazebo entity that matches the updated physics link entity
+      const auto linkPhys = this->entityLinkMap.GetPhysicsEntityPtr(link.body);
+      if (nullptr == linkPhys)
+      {
+        ignerr << "Internal error: a physics entity ptr with an ID of ["
+          << link.body << "] does not exist." << std::endl;
+        continue;
+      }
+      auto entity = this->entityLinkMap.Get(linkPhys);
+      if (entity == kNullEntity)
+      {
+        ignerr << "Internal error: no gazebo entity matches the physics entity "
+          << "with ID [" << link.body << "]." << std::endl;
+        continue;
+      }
+
+      auto frameData = linkPhys->FrameDataRelativeToWorld();
+      linkFrameData[entity] = frameData;
+    }
+  }
+  else
+  {
+    _ecm.Each<components::Link>(
+      [&](const Entity &_entity, components::Link *) -> bool
+      {
+        if (this->staticEntities.find(_entity) != this->staticEntities.end())
+          return true;
+
+        auto linkPhys = this->entityLinkMap.Get(_entity);
+        if (nullptr == linkPhys)
+        {
+          ignerr << "Internal error: link [" << _entity
+                 << "] not in entity map" << std::endl;
+          return true;
+        }
+
+        auto frameData = linkPhys->FrameDataRelativeToWorld();
+
+        // update the link pose if this is the first update,
+        // or if the link pose has changed since the last update
+        // (if the link pose hasn't changed, there's no need for a pose update)
+        const auto worldPoseMath3d = ignition::math::eigen3::convert(
+            frameData.pose);
+        if ((this->linkWorldPoses.find(_entity) == this->linkWorldPoses.end())
+            || !this->pose3Eql(this->linkWorldPoses[_entity], worldPoseMath3d))
+        {
+          // cache the updated link pose to check if the link pose has changed
+          // during the next iteration
+          this->linkWorldPoses[_entity] = worldPoseMath3d;
+
+          linkFrameData[_entity] = frameData;
+        }
+
+        return true;
+      });
+  }
+
+  return linkFrameData;
+}
+
+//////////////////////////////////////////////////
+void PhysicsPrivate::UpdateSim(EntityComponentManager &_ecm,
+    const std::unordered_map<Entity, physics::FrameData3d> &_linkFrameData)
+{
+  IGN_PROFILE("PhysicsPrivate::UpdateSim");
+
+  IGN_PROFILE_BEGIN("Models");
+
+  _ecm.Each<components::Model, components::ModelCanonicalLink>(
+      [&](const Entity &_entity, components::Model *,
+          components::ModelCanonicalLink *_canonicalLink) -> bool
+      {
+        // If the model's canonical link did not move, we don't need to update
+        // the model's pose
+        auto linkFrameIt = _linkFrameData.find(_canonicalLink->Data());
+        if (linkFrameIt == _linkFrameData.end())
+          return true;
+
+        std::optional<math::Pose3d> parentWorldPose;
+
+        // If this model is nested, we assume the pose of the parent model has
+        // already been updated. We expect to find the updated pose in
+        // this->modelWorldPoses. If not found, this must not be nested, so
+        // this model's pose component would reflect it's absolute pose.
+        auto parentModelPoseIt =
+          this->modelWorldPoses.find(
+              _ecm.Component<components::ParentEntity>(_entity)->Data());
+        if (parentModelPoseIt != this->modelWorldPoses.end())
+        {
+          parentWorldPose = parentModelPoseIt->second;
+        }
+
+        // Given the following frame names:
+        // W: World/inertial frame
+        // P: Parent frame (this could be a parent model or the World frame)
+        // M: This model's frame
+        // L: The frame of this model's canonical link
+        //
+        // And the following quantities:
+        // (See http://sdformat.org/tutorials?tut=specify_pose for pose
+        // convention)
+        // parentWorldPose (X_WP): Pose of the parent frame w.r.t the world
+        // linkPoseFromModel (X_ML): Pose of the canonical link frame w.r.t the
+        // model frame
+        // linkWorldPose (X_WL): Pose of the canonical link w.r.t the world
+        // modelWorldPose (X_WM): Pose of this model w.r.t the world
+        //
+        // The Pose component of this model entity stores the pose of M w.r.t P
+        // (X_PM) and is calculated as
+        //   X_PM = (X_WP)^-1 * X_WM
+        //
+        // And X_WM is calculated from X_WL, which is obtained from physics as:
+        //   X_WM = X_WL * (X_ML)^-1
+        auto linkPoseFromModel =
+            this->RelativePose(_entity, linkFrameIt->first, _ecm);
+        const auto &linkWorldPose = linkFrameIt->second.pose;
+        const auto &modelWorldPose =
+            math::eigen3::convert(linkWorldPose) * linkPoseFromModel.Inverse();
+
+        this->modelWorldPoses[_entity] = modelWorldPose;
+
+        // update model's pose
+        auto modelPose = _ecm.Component<components::Pose>(_entity);
+        if (parentWorldPose)
+        {
+          *modelPose =
+              components::Pose(parentWorldPose->Inverse() * modelWorldPose);
+        }
+        else
+        {
+          // This is a non-nested model and parentWorldPose would be identity
+          // because it would be the pose of the parent (world) w.r.t the world.
+          *modelPose = components::Pose(modelWorldPose);
+        }
+
+        _ecm.SetChanged(_entity, components::Pose::typeId,
+                        ComponentState::PeriodicChange);
+        return true;
+      });
+  IGN_PROFILE_END();
+
+  // Link poses, velocities...
+  IGN_PROFILE_BEGIN("Links");
+  for (const auto &[entity, frameData] : _linkFrameData)
+  {
+    IGN_PROFILE_BEGIN("Local pose");
+    auto canonicalLink =
+        _ecm.Component<components::CanonicalLink>(entity);
+
+    const auto &worldPose = frameData.pose;
+    const auto parentEntity = _ecm.ParentEntity(entity);
+
+    if (!canonicalLink)
+    {
+      // Compute the relative pose of this link from the parent model
+      auto parentModelPoseIt = this->modelWorldPoses.find(parentEntity);
+      if (parentModelPoseIt == this->modelWorldPoses.end())
+      {
+        ignerr << "Internal error: parent model [" << parentEntity
+              << "] does not have a world pose available" << std::endl;
+        continue;
+      }
+      const math::Pose3d &parentWorldPose = parentModelPoseIt->second;
+
+      // Unlike canonical links, pose of regular links can move relative.
+      // to the parent. Same for links inside nested models.
+      auto pose = _ecm.Component<components::Pose>(entity);
+      *pose = components::Pose(parentWorldPose.Inverse() *
+                                math::eigen3::convert(worldPose));
+      _ecm.SetChanged(entity, components::Pose::typeId,
+          ComponentState::PeriodicChange);
+    }
+    IGN_PROFILE_END();
+
+    // 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);
+    }
+
+    // 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);
+    }
+
+    // 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);
+    }
+
+    // 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 acceleration in world frame coordinates
+    auto worldAngAccelComp =
+        _ecm.Component<components::WorldAngularAcceleration>(entity);
+
+    if (worldAngAccelComp)
+    {
+      auto state = worldAngAccelComp->SetData(
+          math::eigen3::convert(frameData.angularAcceleration),
+          this->vec3Eql) ?
+          ComponentState::PeriodicChange :
+          ComponentState::NoChange;
+      _ecm.SetChanged(entity,
+          components::WorldAngularAcceleration::typeId, state);
+    }
+
+    const Eigen::Matrix3d R_bs = worldPose.linear().transpose(); // NOLINT
+
+    // 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);
+    }
+
+    // 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);
+    }
+
+    // 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);
+    }
+  }
+  IGN_PROFILE_END();
+
+  // pose/velocity/acceleration of non-link entities such as sensors /
+  // collisions. These get updated only if another system has created a
+  // components::WorldPose component for the entity.
+  // Populated components:
+  // * WorldPose
+  // * WorldLinearVelocity
+  // * AngularVelocity
+  // * LinearAcceleration
+
+  IGN_PROFILE_BEGIN("Sensors / collisions");
+  // world pose
+  _ecm.Each<components::Pose, components::WorldPose,
+            components::ParentEntity>(
+      [&](const Entity &,
+          const components::Pose *_pose, components::WorldPose *_worldPose,
+          const components::ParentEntity *_parent)->bool
+      {
+        // check if parent entity is a link, e.g. entity is sensor / collision
+        if (auto linkPhys = this->entityLinkMap.Get(_parent->Data()))
+        {
+          const auto entityFrameData =
+              this->LinkFrameDataAtOffset(linkPhys, _pose->Data());
+
+          *_worldPose = components::WorldPose(
+              math::eigen3::convert(entityFrameData.pose));
+        }
+
+        return true;
+      });
+
+  // world linear velocity
+  _ecm.Each<components::Pose, components::WorldLinearVelocity,
+            components::ParentEntity>(
+      [&](const Entity &,
+          const components::Pose *_pose,
+          components::WorldLinearVelocity *_worldLinearVel,
+          const components::ParentEntity *_parent)->bool
+      {
+        // check if parent entity is a link, e.g. entity is sensor / collision
+        if (auto linkPhys = this->entityLinkMap.Get(_parent->Data()))
+        {
+          const auto entityFrameData =
+              this->LinkFrameDataAtOffset(linkPhys, _pose->Data());
+
+          // set entity world linear velocity
+          *_worldLinearVel = components::WorldLinearVelocity(
+              math::eigen3::convert(entityFrameData.linearVelocity));
+        }
+
+        return true;
+      });
+
+  // body angular velocity
+  _ecm.Each<components::Pose, components::AngularVelocity,
+            components::ParentEntity>(
+      [&](const Entity &,
+          const components::Pose *_pose,
+          components::AngularVelocity *_angularVel,
+          const components::ParentEntity *_parent)->bool
+      {
+        // check if parent entity is a link, e.g. entity is sensor / collision
+        if (auto linkPhys = this->entityLinkMap.Get(_parent->Data()))
+        {
+          const auto entityFrameData =
+              this->LinkFrameDataAtOffset(linkPhys, _pose->Data());
+
+          auto entityWorldPose = math::eigen3::convert(entityFrameData.pose);
+          ignition::math::Vector3d entityWorldAngularVel =
+              math::eigen3::convert(entityFrameData.angularVelocity);
+
+          auto entityBodyAngularVel =
+              entityWorldPose.Rot().RotateVectorReverse(entityWorldAngularVel);
+          *_angularVel = components::AngularVelocity(entityBodyAngularVel);
+        }
+
+        return true;
+      });
+
+  // body linear acceleration
+  _ecm.Each<components::Pose, components::LinearAcceleration,
+            components::ParentEntity>(
+      [&](const Entity &,
+          const components::Pose *_pose,
+          components::LinearAcceleration *_linearAcc,
+          const components::ParentEntity *_parent)->bool
+      {
+        if (auto linkPhys = this->entityLinkMap.Get(_parent->Data()))
+        {
+          const auto entityFrameData =
+              this->LinkFrameDataAtOffset(linkPhys, _pose->Data());
+
+          auto entityWorldPose = math::eigen3::convert(entityFrameData.pose);
+          ignition::math::Vector3d entityWorldLinearAcc =
+              math::eigen3::convert(entityFrameData.linearAcceleration);
+
+          auto entityBodyLinearAcc =
+              entityWorldPose.Rot().RotateVectorReverse(entityWorldLinearAcc);
+          *_linearAcc = components::LinearAcceleration(entityBodyLinearAcc);
+        }
+
+        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
+      {
+        entitiesPositionReset.push_back(_entity);
+        return true;
+      });
+
+  for (const auto entity : entitiesPositionReset)
+  {
+    _ecm.RemoveComponent<components::JointPositionReset>(entity);
+  }
+
+  std::vector<Entity> entitiesVelocityReset;
+  _ecm.Each<components::JointVelocityReset>(
+      [&](const Entity &_entity, components::JointVelocityReset *) -> bool
+      {
+        entitiesVelocityReset.push_back(_entity);
+        return true;
+      });
+
+  for (const auto entity : entitiesVelocityReset)
+  {
+    _ecm.RemoveComponent<components::JointVelocityReset>(entity);
+  }
+
+  // Clear pending commands
+  _ecm.Each<components::JointForceCmd>(
+      [&](const Entity &, components::JointForceCmd *_force) -> bool
+      {
+        std::fill(_force->Data().begin(), _force->Data().end(), 0.0);
+        return true;
+      });
+
+  _ecm.Each<components::ExternalWorldWrenchCmd >(
+      [&](const Entity &, components::ExternalWorldWrenchCmd *_wrench) -> bool
+      {
+        _wrench->Data().Clear();
+        return true;
+      });
+
+  _ecm.Each<components::JointVelocityCmd>(
+      [&](const Entity &, components::JointVelocityCmd *_vel) -> bool
+      {
+        std::fill(_vel->Data().begin(), _vel->Data().end(), 0.0);
+        return true;
+      });
+
+  _ecm.Each<components::SlipComplianceCmd>(
+      [&](const Entity &, components::SlipComplianceCmd *_slip) -> bool
+      {
+        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
+      {
+        if (auto jointPhys = this->entityJointMap.Get(_entity))
+        {
+          _jointPos->Data().resize(jointPhys->GetDegreesOfFreedom());
+          for (std::size_t i = 0; i < jointPhys->GetDegreesOfFreedom();
+               ++i)
+          {
+            _jointPos->Data()[i] = jointPhys->GetPosition(i);
+          }
+          _ecm.SetChanged(_entity, components::JointPosition::typeId,
+              ComponentState::PeriodicChange);
+        }
+        return true;
+      });
+
+  // Update joint Velocities
+  _ecm.Each<components::Joint, components::JointVelocity>(
+      [&](const Entity &_entity, components::Joint *,
+          components::JointVelocity *_jointVel) -> bool
+      {
+        if (auto jointPhys = this->entityJointMap.Get(_entity))
+        {
+          _jointVel->Data().resize(jointPhys->GetDegreesOfFreedom());
+          for (std::size_t i = 0; i < jointPhys->GetDegreesOfFreedom();
+               ++i)
+          {
+            _jointVel->Data()[i] = jointPhys->GetVelocity(i);
+          }
+        }
+        return true;
+      });
+  IGN_PROFILE_END();
+
+  // TODO(louise) Skip this if there are no collision features
+  this->UpdateCollisions(_ecm);
+}
+
+//////////////////////////////////////////////////
+void PhysicsPrivate::UpdateCollisions(EntityComponentManager &_ecm)
+{
+  IGN_PROFILE("PhysicsPrivate::UpdateCollisions");
+  // Quit early if the ContactData component hasn't been created. This means
+  // there are no systems that need contact information
+  if (!_ecm.HasComponentType(components::ContactSensorData::typeId))
+    return;
+
+  // TODO(addisu) If systems are assumed to only have one world, we should
+  // capture the world Entity in a Configure call
+  Entity worldEntity = _ecm.EntityByComponents(components::World());
+
+  if (worldEntity == kNullEntity)
+  {
+    ignerr << "Missing world entity.\n";
+    return;
+  }
+
+  if (!this->entityWorldMap.HasEntity(worldEntity))
+  {
+    ignwarn << "Failed to find world [" << worldEntity << "]." << std::endl;
+    return;
+  }
+
+  auto worldCollisionFeature =
+      this->entityWorldMap.EntityCast<CollisionFeatureList>(worldEntity);
+  if (!worldCollisionFeature)
+  {
+    static bool informed{false};
+    if (!informed)
+    {
+      igndbg << "Attempting process contacts, but the physics "
+             << "engine doesn't support collision features. "
+             << "Contacts won't be computed."
+             << std::endl;
+      informed = true;
+    }
+    return;
+  }
+
+  // Each contact object we get from ign-physics contains the EntityPtrs of the
+  // two colliding entities and other data about the contact such as the
+  // position. This map groups contacts so that it is easy to query all the
+  // contacts of one entity.
+  using EntityContactMap = std::unordered_map<Entity,
+      std::deque<const WorldShapeType::ContactPoint *>>;
+
+  // This data structure is essentially a mapping between a pair of entities and
+  // a list of pointers to their contact object. We use a map inside a map to
+  // create msgs::Contact objects conveniently later on.
+  std::unordered_map<Entity, EntityContactMap> entityContactMap;
+
+  // Note that we are temporarily storing pointers to elements in this
+  // ("allContacts") container. Thus, we must make sure it doesn't get destroyed
+  // until the end of this function.
+  auto allContacts = worldCollisionFeature->GetContactsFromLastStep();
+  for (const auto &contactComposite : allContacts)
+  {
+    const auto &contact = contactComposite.Get<WorldShapeType::ContactPoint>();
+    auto coll1Entity = this->entityCollisionMap.Get(contact.collision1);
+    auto coll2Entity = this->entityCollisionMap.Get(contact.collision2);
+
+
+    if (coll1Entity != kNullEntity && coll2Entity != kNullEntity)
+    {
+      entityContactMap[coll1Entity][coll2Entity].push_back(&contact);
+      entityContactMap[coll2Entity][coll1Entity].push_back(&contact);
+    }
+  }
+
+  // Go through each collision entity that has a ContactData component and
+  // set the component value to the list of contacts that correspond to
+  // the collision entity
+  _ecm.Each<components::Collision, components::ContactSensorData>(
+      [&](const Entity &_collEntity1, components::Collision *,
+          components::ContactSensorData *_contacts) -> bool
+      {
+        if (entityContactMap.find(_collEntity1) == entityContactMap.end())
+        {
+          // Clear the last contact data
+          *_contacts = components::ContactSensorData();
+          return true;
+        }
+
+        const auto &contactMap = entityContactMap[_collEntity1];
+
+        msgs::Contacts contactsComp;
+
+        for (const auto &[collEntity2, contactData] : contactMap)
+        {
+          msgs::Contact *contactMsg = contactsComp.add_contact();
+          contactMsg->mutable_collision1()->set_id(_collEntity1);
+          contactMsg->mutable_collision2()->set_id(collEntity2);
+          for (const auto &contact : contactData)
+          {
+            auto *position = contactMsg->add_position();
+            position->set_x(contact->point.x());
+            position->set_y(contact->point.y());
+            position->set_z(contact->point.z());
+          }
+        }
+        *_contacts = components::ContactSensorData(contactsComp);
+
+        return true;
+      });
+}
+
+physics::FrameData3d PhysicsPrivate::LinkFrameDataAtOffset(
+      const LinkPtrType &_link, const math::Pose3d &_pose) const
+{
+  physics::FrameData3d parent;
+  parent.pose = math::eigen3::convert(_pose);
+  physics::RelativeFrameData3d relFrameData(_link->GetFrameID(), parent);
+  return this->engine->Resolve(relFrameData, physics::FrameID::World());
+}
+
+IGNITION_ADD_PLUGIN(Physics,
+                    ignition::gazebo::System,
+                    Physics::ISystemConfigure,
+                    Physics::ISystemUpdate)
+
+IGNITION_ADD_PLUGIN_ALIAS(Physics, "ignition::gazebo::systems::Physics")
diff --git a/cpp/scenario/plugins/Physics/Physics.cpp b/cpp/scenario/plugins/Physics/Physics.cpp
deleted file mode 100644
index efdd5181f..000000000
--- a/cpp/scenario/plugins/Physics/Physics.cpp
+++ /dev/null
@@ -1,2556 +0,0 @@
-/*
- * Copyright (C) 2020 Open Source Robotics Foundation
- * All rights reserved.
- *
- * 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 "Physics.h"
-#include "scenario/gazebo/components/ExternalWorldWrenchCmdWithDuration.h"
-#include "scenario/gazebo/components/HistoryOfAppliedJointForces.h"
-#include "scenario/gazebo/components/JointAcceleration.h"
-#include "scenario/gazebo/components/SimulatedTime.h"
-#include "scenario/gazebo/components/WorldVelocityCmd.h"
-#include "scenario/gazebo/helpers.h"
-
-#include <ignition/common/MeshManager.hh>
-#include <ignition/common/SystemPaths.hh>
-#include <ignition/gazebo/EntityComponentManager.hh>
-#include <ignition/gazebo/Util.hh>
-#include <ignition/gazebo/components/AngularAcceleration.hh>
-#include <ignition/gazebo/components/AngularVelocity.hh>
-#include <ignition/gazebo/components/AngularVelocityCmd.hh>
-#include <ignition/gazebo/components/AxisAlignedBox.hh>
-#include <ignition/gazebo/components/BatterySoC.hh>
-#include <ignition/gazebo/components/CanonicalLink.hh>
-#include <ignition/gazebo/components/ChildLinkName.hh>
-#include <ignition/gazebo/components/Collision.hh>
-#include <ignition/gazebo/components/ContactSensorData.hh>
-#include <ignition/gazebo/components/DetachableJoint.hh>
-#include <ignition/gazebo/components/ExternalWorldWrenchCmd.hh>
-#include <ignition/gazebo/components/Geometry.hh>
-#include <ignition/gazebo/components/Gravity.hh>
-#include <ignition/gazebo/components/Inertial.hh>
-#include <ignition/gazebo/components/Joint.hh>
-#include <ignition/gazebo/components/JointAxis.hh>
-#include <ignition/gazebo/components/JointForce.hh>
-#include <ignition/gazebo/components/JointForceCmd.hh>
-#include <ignition/gazebo/components/JointPosition.hh>
-#include <ignition/gazebo/components/JointPositionReset.hh>
-#include <ignition/gazebo/components/JointType.hh>
-#include <ignition/gazebo/components/JointVelocity.hh>
-#include <ignition/gazebo/components/JointVelocityCmd.hh>
-#include <ignition/gazebo/components/JointVelocityReset.hh>
-#include <ignition/gazebo/components/LinearAcceleration.hh>
-#include <ignition/gazebo/components/LinearVelocity.hh>
-#include <ignition/gazebo/components/LinearVelocityCmd.hh>
-#include <ignition/gazebo/components/Link.hh>
-#include <ignition/gazebo/components/Model.hh>
-#include <ignition/gazebo/components/Name.hh>
-#include <ignition/gazebo/components/ParentEntity.hh>
-#include <ignition/gazebo/components/ParentLinkName.hh>
-#include <ignition/gazebo/components/PhysicsEnginePlugin.hh>
-#include <ignition/gazebo/components/Pose.hh>
-#include <ignition/gazebo/components/PoseCmd.hh>
-#include <ignition/gazebo/components/SelfCollide.hh>
-#include <ignition/gazebo/components/SlipComplianceCmd.hh>
-#include <ignition/gazebo/components/Static.hh>
-#include <ignition/gazebo/components/ThreadPitch.hh>
-#include <ignition/gazebo/components/World.hh>
-#include <ignition/math/AxisAlignedBox.hh>
-#include <ignition/math/Vector3.hh>
-#include <ignition/math/eigen3/Conversions.hh>
-#include <ignition/msgs/Utility.hh>
-#include <ignition/msgs/contact.pb.h>
-#include <ignition/msgs/contacts.pb.h>
-#include <ignition/msgs/entity.pb.h>
-#include <ignition/physics/RelativeQuantity.hh>
-#include <ignition/physics/RequestEngine.hh>
-#include <ignition/physics/config.hh>
-#include <ignition/physics/mesh/MeshShape.hh>
-#include <ignition/plugin/Loader.hh>
-#include <ignition/plugin/PluginPtr.hh>
-#include <ignition/plugin/Register.hh>
-#include <sdf/Collision.hh>
-#include <sdf/Joint.hh>
-#include <sdf/Link.hh>
-#include <sdf/Mesh.hh>
-#include <sdf/Model.hh>
-#include <sdf/Surface.hh>
-#include <sdf/World.hh>
-
-#include <algorithm>
-#include <deque>
-#include <iostream>
-#include <string>
-#include <unordered_map>
-#include <vector>
-
-using namespace ignition;
-using namespace scenario::plugins::gazebo;
-using namespace ignition::gazebo;
-using namespace ignition::gazebo::systems;
-using namespace ignition::gazebo::components;
-
-class Physics::Impl
-{
-public:
-    /// \brief This is the minimum set of features that any physics engine must
-    /// implement to be supported by this system.
-    /// New features can't be added to this list in minor / patch releases, in
-    /// order to maintain backwards compatibility with downstream physics
-    /// plugins.
-    struct MinimumFeatureList
-        : ignition::physics::FeatureList< //
-              ignition::physics::FindFreeGroupFeature,
-              ignition::physics::SetFreeGroupWorldPose,
-              ignition::physics::FreeGroupFrameSemantics,
-              ignition::physics::LinkFrameSemantics,
-              ignition::physics::SetFreeGroupWorldVelocity,
-              ignition::physics::ForwardStep,
-              ignition::physics::RemoveEntities,
-              ignition::physics::sdf::ConstructSdfLink,
-              ignition::physics::sdf::ConstructSdfModel,
-              ignition::physics::sdf::ConstructSdfWorld>
-    {};
-
-    /// \brief Engine type with just the minimum features.
-    using EnginePtrType = ignition::physics::EnginePtr< //
-        ignition::physics::FeaturePolicy3d,
-        MinimumFeatureList>;
-
-    /// \brief World type with just the minimum features.
-    using WorldPtrType = ignition::physics::WorldPtr< //
-        ignition::physics::FeaturePolicy3d,
-        MinimumFeatureList>;
-
-    /// \brief Model type with just the minimum features.
-    using ModelPtrType = ignition::physics::ModelPtr< //
-        ignition::physics::FeaturePolicy3d,
-        MinimumFeatureList>;
-
-    /// \brief Link type with just the minimum features.
-    using LinkPtrType = ignition::physics::LinkPtr< //
-        ignition::physics::FeaturePolicy3d,
-        MinimumFeatureList>;
-
-    /// \brief Free group type with just the minimum features.
-    using FreeGroupPtrType = ignition::physics::FreeGroupPtr< //
-        ignition::physics::FeaturePolicy3d,
-        MinimumFeatureList>;
-
-    /// \brief Create physics entities
-    /// \param[in] _ecm Constant reference to ECM.
-    void CreatePhysicsEntities(const EntityComponentManager& _ecm);
-
-    /// \brief Remove physics entities if they are removed from the ECM
-    /// \param[in] _ecm Constant reference to ECM.
-    void RemovePhysicsEntities(const EntityComponentManager& _ecm);
-
-    /// \brief Update physics from components
-    /// \param[in] _ecm Constant reference to ECM.
-    void UpdatePhysics(const ignition::gazebo::UpdateInfo& _info,
-                       EntityComponentManager& _ecm);
-
-    /// \brief Step the simulationrfor each world
-    /// \param[in] _dt Duration
-    void Step(const std::chrono::steady_clock::duration& _dt);
-
-    /// \brief Update components from physics simulation
-    /// \param[in] _ecm Mutable reference to ECM.
-    void UpdateSim(const ignition::gazebo::UpdateInfo& _info,
-                   EntityComponentManager& _ecm);
-
-    /// \brief Update collision components from physics simulation
-    /// \param[in] _ecm Mutable reference to ECM.
-    void UpdateCollisions(EntityComponentManager& _ecm);
-
-    /// \brief FrameData relative to world at a given offset pose
-    /// \param[in] _link ign-physics link
-    /// \param[in] _pose Offset pose in which to compute the frame data
-    /// \returns FrameData at the given offset pose
-    ignition::physics::FrameData3d
-    LinkFrameDataAtOffset(const LinkPtrType& _link,
-                          const math::Pose3d& _pose) const;
-
-    /// \brief Get transform from one ancestor entity to a descendant entity
-    /// that are in the same model.
-    /// \param[in] _from An ancestor of the _to entity.
-    /// \param[in] _to A descendant of the _from entity.
-    /// \return Pose transform between the two entities
-    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.
-    std::unordered_map<Entity, WorldPtrType> entityWorldMap;
-
-    /// \brief A map between model entity ids in the ECM to Model Entities in
-    /// ign-physics.
-    std::unordered_map<Entity, ModelPtrType> entityModelMap;
-
-    /// \brief A map between link entity ids in the ECM to Link Entities in
-    /// ign-physics.
-    std::unordered_map<Entity, LinkPtrType> entityLinkMap;
-
-    /// \brief Reverse of entityLinkMap. This is used for finding the Entity
-    /// associated with a physics Link
-    std::unordered_map<LinkPtrType, Entity> linkEntityMap;
-
-    /// \brief A map between model entity ids in the ECM to whether its battery
-    /// has drained.
-    std::unordered_map<Entity, bool> entityOffMap;
-
-    /// \brief used to store whether physics objects have been created.
-    bool initialized = false;
-
-    /// \brief Pointer to the underlying ign-physics Engine entity.
-    EnginePtrType engine = nullptr;
-
-    /// \brief Vector3d equality comparison function.
-    std::function<bool(const math::Vector3d&, const math::Vector3d&)> vec3Eql{
-        [](const math::Vector3d& _a, const math::Vector3d& _b) {
-            return _a.Equal(_b, 1e-6);
-        }};
-
-    /// \brief Pose3d equality comparison function.
-    std::function<bool(const math::Pose3d&, const math::Pose3d&)> pose3Eql{
-        [](const math::Pose3d& _a, const math::Pose3d& _b) {
-            return _a.Pos().Equal(_b.Pos(), 1e-6)
-                   && math::equal(_a.Rot().X(), _b.Rot().X(), 1e-6)
-                   && math::equal(_a.Rot().Y(), _b.Rot().Y(), 1e-6)
-                   && math::equal(_a.Rot().Z(), _b.Rot().Z(), 1e-6)
-                   && math::equal(_a.Rot().W(), _b.Rot().W(), 1e-6);
-        }};
-
-    /// \brief AxisAlignedBox equality comparison function.
-    std::function<bool(const math::AxisAlignedBox&,
-                       const math::AxisAlignedBox&)>
-        axisAlignedBoxEql{
-            [](const math::AxisAlignedBox& _a, const math::AxisAlignedBox& _b) {
-                return _a == _b;
-            }};
-
-    /// \brief Environment variable which holds paths to look for engine plugins
-    std::string pluginPathEnv = "IGN_GAZEBO_PHYSICS_ENGINE_PATH";
-
-    //////////////////////////////////////////////////
-    // Slip Compliance
-
-    /// \brief Feature list to process `FrictionPyramidSlipCompliance`
-    /// components.
-    using FrictionPyramidSlipComplianceFeatureList = physics::FeatureList<
-        MinimumFeatureList,
-        ignition::physics::GetShapeFrictionPyramidSlipCompliance,
-        ignition::physics::SetShapeFrictionPyramidSlipCompliance>;
-
-    /// \brief Shape type with slip compliance features.
-    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`.
-    std::unordered_map<Entity, ShapeSlipParamPtrType> entityShapeSlipParamMap;
-
-    //////////////////////////////////////////////////
-    // Joints
-
-    /// \brief Feature list to handle joints.
-    using JointFeatureList = ignition::physics::FeatureList<
-        MinimumFeatureList,
-        ignition::physics::GetBasicJointProperties,
-        ignition::physics::GetBasicJointState,
-        ignition::physics::SetBasicJointState,
-        ignition::physics::sdf::ConstructSdfJoint>;
-
-    /// \brief Joint type with joint features.
-    using JointPtrType = ignition::physics::JointPtr< //
-        ignition::physics::FeaturePolicy3d,
-        JointFeatureList>;
-
-    /// \brief Model type with joint features (models to attach to).
-    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
-    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`.
-    std::unordered_map<Entity, ModelJointPtrType> entityModelJointMap;
-
-    //////////////////////////////////////////////////
-    // Detachable joints
-
-    /// \brief Feature list to process `DetachableJoint` components.
-    using DetachableJointFeatureList = ignition::physics::FeatureList< //
-        JointFeatureList,
-        ignition::physics::AttachFixedJointFeature,
-        ignition::physics::DetachJointFeature,
-        ignition::physics::SetJointTransformFromParentFeature>;
-
-    /// \brief Joint type with detachable joint features.
-    using JointDetachableJointPtrType = ignition::physics::JointPtr< //
-        ignition::physics::FeaturePolicy3d,
-        DetachableJointFeatureList>;
-
-    /// \brief Link type with detachable joint features (links to attach to).
-    using LinkDetachableJointPtrType = ignition::physics::LinkPtr< //
-        ignition::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`.
-    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`.
-    std::unordered_map<Entity, LinkDetachableJointPtrType>
-        entityLinkDetachableJointMap;
-
-    //////////////////////////////////////////////////
-    // Collisions
-
-    /// \brief Feature list to handle collisions.
-    using CollisionFeatureList = ignition::physics::FeatureList< //
-        MinimumFeatureList,
-        ignition::physics::GetContactsFromLastStepFeature,
-        ignition::physics::sdf::ConstructSdfCollision>;
-
-    /// \brief Collision type with collision features.
-    using ShapePtrType = ignition::physics::ShapePtr< //
-        ignition::physics::FeaturePolicy3d,
-        CollisionFeatureList>;
-
-    /// \brief Link type with collision features.
-    using LinkShapePtrType = ignition::physics::LinkPtr< //
-        ignition::physics::FeaturePolicy3d,
-        CollisionFeatureList>;
-
-    /// \brief World type with collision features.
-    using WorldShapePtrType = ignition::physics::WorldPtr< //
-        ignition::physics::FeaturePolicy3d,
-        CollisionFeatureList>;
-
-    /// \brief World type with just the minimum features. Non-pointer.
-    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.
-    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.
-    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`.
-    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`.
-    std::unordered_map<Entity, WorldShapePtrType> entityWorldCollisionMap;
-
-    //////////////////////////////////////////////////
-    // Collision filtering with bitmasks
-
-    /// \brief Feature list to filter collisions with bitmasks.
-    using CollisionMaskFeatureList = ignition::physics::FeatureList<
-        CollisionFeatureList,
-        ignition::physics::CollisionFilterMaskFeature>;
-
-    /// \brief Collision type with collision filtering features.
-    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`.
-    std::unordered_map<Entity, ShapeFilterMaskPtrType> entityShapeMaskMap;
-
-    //////////////////////////////////////////////////
-    // Link force
-
-    /// \brief Feature list for applying forces to links.
-    using LinkForceFeatureList = ignition::physics::FeatureList< //
-        ignition::physics::AddLinkExternalForceTorque>;
-
-    /// \brief Link type with bounding box feature.
-    using LinkForcePtrType = ignition::physics::LinkPtr< //
-        ignition::physics::FeaturePolicy3d,
-        LinkForceFeatureList>;
-
-    /// \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`.
-    std::unordered_map<Entity, LinkForcePtrType> entityLinkForceMap;
-
-    //////////////////////////////////////////////////
-    // Bounding box
-
-    /// \brief Feature list for model bounding box.
-    using BoundingBoxFeatureList = ignition::physics::FeatureList< //
-        MinimumFeatureList,
-        ignition::physics::GetModelBoundingBox>;
-
-    /// \brief Model type with bounding box feature.
-    using ModelBoundingBoxPtrType = ignition::physics::ModelPtr< //
-        ignition::physics::FeaturePolicy3d,
-        BoundingBoxFeatureList>;
-
-    /// \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`.
-    std::unordered_map<Entity, ModelBoundingBoxPtrType>
-        entityModelBoundingBoxMap;
-
-    //////////////////////////////////////////////////
-    // Joint velocity command
-
-    /// \brief Feature list for set joint velocity command.
-    using JointVelocityCommandFeatureList = ignition::physics::FeatureList< //
-        physics::SetJointVelocityCommandFeature>;
-
-    /// \brief Joint type with set joint velocity command.
-    using JointVelocityCommandPtrType = ignition::physics::JointPtr< //
-        ignition::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`.
-    std::unordered_map<Entity, JointVelocityCommandPtrType>
-        entityJointVelocityCommandMap;
-
-    //////////////////////////////////////////////////
-    // World velocity command
-    using WorldVelocityCommandFeatureList = ignition::physics::FeatureList<
-        ignition::physics::SetFreeGroupWorldVelocity>;
-
-    /// \brief Free group type with world velocity command.
-    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`.
-    std::unordered_map<Entity, FreeGroupVelocityCmdPtrType>
-        entityWorldVelocityCommandMap;
-
-    //////////////////////////////////////////////////
-    // Meshes
-
-    /// \brief Feature list for meshes.
-    /// Include MinimumFeatureList so created collision can be automatically
-    /// up-cast.
-    using MeshFeatureList = ignition::physics::FeatureList< //
-        CollisionFeatureList,
-        ignition::physics::mesh::AttachMeshShapeFeature>;
-
-    /// \brief Link type with meshes.
-    using LinkMeshPtrType = physics::LinkPtr< //
-        ignition::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`.
-    std::unordered_map<Entity, LinkMeshPtrType> entityLinkMeshMap;
-
-    //////////////////////////////////////////////////
-    // Nested Models
-
-    /// \brief Feature list to construct nested models
-    using NestedModelFeatureList = ignition::physics::FeatureList<
-        MinimumFeatureList,
-        ignition::physics::sdf::ConstructSdfNestedModel>;
-
-    /// \brief Model type with nested model feature.
-    using ModelNestedModelPtrType =
-        physics::ModelPtr<physics::FeaturePolicy3d, NestedModelFeatureList>;
-
-    /// \brief World type with nested model feature.
-    using WorldNestedModelPtrType =
-        physics::WorldPtr<physics::FeaturePolicy3d, 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`.
-    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`.
-    std::unordered_map<Entity, WorldNestedModelPtrType>
-        entityWorldNestedModelMap;
-
-    /// \brief Boolean value that is true only the first call of Configure and
-    /// PreUpdate.
-    bool firstRun = true;
-};
-
-Physics::Physics()
-    : System()
-    , pImpl(std::make_unique<Impl>())
-{}
-
-void Physics::Configure(const Entity& _entity,
-                        const std::shared_ptr<const sdf::Element>& _sdf,
-                        EntityComponentManager& _ecm,
-                        EventManager& /*_eventMgr*/)
-{
-    std::string pluginLib;
-
-    // 1. Engine from component (from command line / ServerConfig)
-    auto engineComp =
-        _ecm.Component<ignition::gazebo::components::PhysicsEnginePlugin>(
-            _entity);
-    if (engineComp && !engineComp->Data().empty()) {
-        pluginLib = engineComp->Data();
-    }
-    // 2. Engine from SDF
-    else if (_sdf->HasElement("engine")) {
-        auto sdfClone = _sdf->Clone();
-        auto engineElem = sdfClone->GetElement("engine");
-        pluginLib = engineElem->Get<std::string>("filename", pluginLib).first;
-    }
-
-    // 3. Use DART by default
-    if (pluginLib.empty()) {
-        pluginLib = "ignition-physics-dartsim-plugin";
-    }
-
-    // Update component
-    if (!engineComp) {
-        _ecm.CreateComponent(_entity,
-                             components::PhysicsEnginePlugin(pluginLib));
-    }
-    else {
-        engineComp->SetData(pluginLib,
-                            [](const std::string& _a, const std::string& _b) {
-                                return _a == _b;
-                            });
-    }
-
-    // Find engine shared library
-    // Look in:
-    // * Paths from environment variable
-    // * Engines installed with ign-physics
-    common::SystemPaths systemPaths;
-    systemPaths.SetPluginPathEnv(pImpl->pluginPathEnv);
-    systemPaths.AddPluginPaths({IGNITION_PHYSICS_ENGINE_INSTALL_DIR});
-
-    auto pathToLib = systemPaths.FindSharedLibrary(pluginLib);
-    if (pathToLib.empty()) {
-        ignerr << "Failed to find plugin [" << pluginLib
-               << "]. Have you checked the " << pImpl->pluginPathEnv
-               << " environment variable?" << std::endl;
-        return;
-    }
-
-    // Load engine plugin
-    ignition::plugin::Loader pluginLoader;
-    auto plugins = pluginLoader.LoadLib(pathToLib);
-    if (plugins.empty()) {
-        ignerr << "Unable to load the [" << pathToLib << "] library.\n";
-        return;
-    }
-
-    auto classNames = pluginLoader.AllPlugins();
-    if (classNames.empty()) {
-        ignerr << "No plugins found in library [" << pathToLib << "]."
-               << std::endl;
-        return;
-    }
-
-    // Get the first plugin that works
-    for (auto className : classNames) {
-        auto plugin = pluginLoader.Instantiate(className);
-
-        if (!plugin)
-            continue;
-
-        pImpl->engine = ignition::physics::RequestEngine<
-            ignition::physics::FeaturePolicy3d,
-            Impl::MinimumFeatureList>::From(plugin);
-
-        if (nullptr != pImpl->engine) {
-            igndbg << "Loaded [" << className << "] from library [" << pathToLib
-                   << "]" << std::endl;
-            break;
-        }
-
-        auto missingFeatures = ignition::physics::RequestEngine<
-            ignition::physics::FeaturePolicy3d,
-            Impl::MinimumFeatureList>::MissingFeatureNames(plugin);
-
-        std::stringstream msg;
-        msg << "Plugin [" << className
-            << "] misses required features:" << std::endl;
-        for (auto feature : missingFeatures) {
-            msg << "- " << feature << std::endl;
-        }
-        ignwarn << msg.str();
-    }
-
-    if (nullptr == pImpl->engine) {
-        ignerr << "Failed to load a valid physics engine from [" << pathToLib
-               << "]." << std::endl;
-    }
-}
-
-Physics::~Physics() = default;
-
-void Physics::Update(const UpdateInfo& _info, EntityComponentManager& _ecm)
-{
-    // \TODO(anyone) Support rewind
-    if (_info.dt < std::chrono::steady_clock::duration::zero()) {
-        ignwarn << "Detected jump back in time ["
-                << std::chrono::duration_cast<std::chrono::seconds>(_info.dt)
-                       .count()
-                << "s]. System may not work properly." << std::endl;
-    }
-
-    // Update the component with the time in seconds that  the simulation
-    // will have after the step
-    _ecm.Each<components::World, components::SimulatedTime>(
-        [&](const Entity& worldEntity,
-            const components::World*,
-            components::SimulatedTime*) {
-            scenario::gazebo::utils::setExistingComponentData<
-                ignition::gazebo::components::SimulatedTime>(
-                &_ecm, worldEntity, _info.simTime);
-            return true;
-        });
-
-    if (this->pImpl->engine) {
-        this->pImpl->CreatePhysicsEntities(_ecm);
-        this->pImpl->UpdatePhysics(_info, _ecm);
-
-        // Only step if not paused.
-        if (!_info.paused) {
-            this->pImpl->Step(_info.dt);
-        }
-
-        this->pImpl->UpdateSim(_info, _ecm);
-
-        // Entities scheduled to be removed should be removed from physics
-        // after the simulation step. Otherwise, since the to-be-removed
-        // entity still shows up in the ECM::Each the UpdatePhysics and
-        // UpdateSim calls will have an error
-        this->pImpl->RemovePhysicsEntities(_ecm);
-    }
-}
-
-void Physics::Impl::CreatePhysicsEntities(const EntityComponentManager& _ecm)
-{
-    auto processWorld = [&](const Entity& _entity,
-                            const components::World* /* _world */,
-                            const components::Name* _name,
-                            const components::Gravity* _gravity) -> bool {
-        if (this->entityWorldMap.find(_entity) != this->entityWorldMap.end()) {
-            ignwarn << "World entity [" << _entity
-                    << "] marked as new, but it's already on the map."
-                    << std::endl;
-            return true;
-        }
-
-        sdf::World world;
-        world.SetName(_name->Data());
-        world.SetGravity(_gravity->Data());
-        auto worldPtrPhys = this->engine->ConstructWorld(world);
-        this->entityWorldMap.insert(std::make_pair(_entity, worldPtrPhys));
-
-        return true;
-    };
-
-    auto processModel = [&](const Entity& _entity,
-                            const components::Model*,
-                            const components::Name* _name,
-                            const components::Pose* _pose,
-                            const components::ParentEntity* _parent) -> bool {
-        //        ignerr << "model " << _name->Data() << std::endl;
-        // Check if model already exists
-        if (this->entityModelMap.find(_entity) != this->entityModelMap.end()) {
-            ignwarn << "Model entity [" << _entity
-                    << "] marked as new, but it's already on the map."
-                    << std::endl;
-            return true;
-        }
-
-        // TODO(anyone) Don't load models unless they have collisions
-
-        // Check if parent world / model exists
-        sdf::Model model;
-        model.SetName(_name->Data());
-        model.SetRawPose(_pose->Data());
-
-        auto staticComp = _ecm.Component<components::Static>(_entity);
-        if (staticComp && staticComp->Data()) {
-            model.SetStatic(staticComp->Data());
-        }
-
-        auto selfCollideComp = _ecm.Component<components::SelfCollide>(_entity);
-        if (selfCollideComp && selfCollideComp->Data()) {
-            model.SetSelfCollide(selfCollideComp->Data());
-        }
-
-        // check if parent is a world
-        auto worldIt = this->entityWorldMap.find(_parent->Data());
-        if (worldIt != this->entityWorldMap.end()) {
-            auto worldPtrPhys = worldIt->second;
-
-            // Use the ConstructNestedModel feature for nested models
-            if (model.ModelCount() > 0) {
-                auto nestedModelFeature =
-                    entityCast(_parent->Data(),
-                               worldPtrPhys,
-                               this->entityWorldNestedModelMap);
-                if (!nestedModelFeature) {
-                    static bool informed{false};
-                    if (!informed) {
-                        igndbg
-                            << "Attempting to construct nested models, but the "
-                            << "phyiscs engine doesn't support feature "
-                            << "[ConstructSdfNestedModelFeature]. "
-                            << "Nested model will be ignored." << std::endl;
-                        informed = true;
-                    }
-                    return true;
-                }
-                auto modelPtrPhys =
-                    nestedModelFeature->ConstructNestedModel(model);
-                this->entityModelMap.insert(
-                    std::make_pair(_entity, modelPtrPhys));
-            }
-            else {
-                auto modelPtrPhys = worldPtrPhys->ConstructModel(model);
-                this->entityModelMap.insert(
-                    std::make_pair(_entity, modelPtrPhys));
-            }
-        }
-        // check if parent is a model (nested model)
-        else {
-            auto parentIt = this->entityModelMap.find(_parent->Data());
-            if (parentIt != this->entityModelMap.end()) {
-                auto parentPtrPhys = parentIt->second;
-
-                auto nestedModelFeature =
-                    entityCast(_parent->Data(),
-                               parentPtrPhys,
-                               this->entityModelNestedModelMap);
-                if (!nestedModelFeature) {
-                    static bool informed{false};
-                    if (!informed) {
-                        igndbg
-                            << "Attempting to construct nested models, but the "
-                            << "physics engine doesn't support feature "
-                            << "[ConstructSdfNestedModelFeature]. "
-                            << "Nested model will be ignored." << std::endl;
-                        informed = true;
-                    }
-                    return true;
-                }
-
-                // override static property only if parent is static.
-                auto parentStaticComp =
-                    _ecm.Component<components::Static>(_parent->Data());
-                if (parentStaticComp && parentStaticComp->Data())
-                    model.SetStatic(true);
-
-                auto modelPtrPhys =
-                    nestedModelFeature->ConstructNestedModel(model);
-                if (modelPtrPhys) {
-                    this->entityModelMap.insert(
-                        std::make_pair(_entity, modelPtrPhys));
-                }
-                else {
-                    ignerr << "Model: '" << _name->Data() << "' not loaded. "
-                           << "Failed to create nested model." << std::endl;
-                }
-            }
-            else {
-                ignwarn << "Model's parent entity [" << _parent->Data()
-                        << "] not found on world / model map." << std::endl;
-                return true;
-            }
-        }
-
-        return true;
-    };
-
-    auto processLink = [&](const Entity& _entity,
-                           const components::Link* /* _link */,
-                           const components::Name* _name,
-                           const components::Pose* _pose,
-                           const components::ParentEntity* _parent) -> bool {
-        // Check if link already exists
-        if (this->entityLinkMap.find(_entity) != this->entityLinkMap.end()) {
-            ignwarn << "Link entity [" << _entity
-                    << "] marked as new, but it's already on the map."
-                    << std::endl;
-            return true;
-        }
-
-        // TODO(anyone) Don't load links unless they have collisions
-
-        // Check if parent model exists
-        if (this->entityModelMap.find(_parent->Data())
-            == this->entityModelMap.end()) {
-            ignwarn << "Link's parent entity [" << _parent->Data()
-                    << "] not found on model map." << std::endl;
-            return true;
-        }
-        auto modelPtrPhys = this->entityModelMap.at(_parent->Data());
-
-        sdf::Link link;
-        link.SetName(_name->Data());
-        link.SetRawPose(_pose->Data());
-
-        // get link inertial
-        auto inertial = _ecm.Component<components::Inertial>(_entity);
-        if (inertial) {
-            link.SetInertial(inertial->Data());
-        }
-
-        auto linkPtrPhys = modelPtrPhys->ConstructLink(link);
-        this->entityLinkMap.insert(std::make_pair(_entity, linkPtrPhys));
-        this->linkEntityMap.insert(std::make_pair(linkPtrPhys, _entity));
-
-        return true;
-    };
-
-    auto processCollision =
-        [&](const Entity& _entity,
-            const components::Collision*,
-            const components::Name* _name,
-            const components::Pose* _pose,
-            const components::Geometry* _geom,
-            const components::CollisionElement* _collElement,
-            const components::ParentEntity* _parent) -> bool {
-        if (this->entityCollisionMap.find(_entity)
-            != this->entityCollisionMap.end()) {
-            ignwarn << "Collision entity [" << _entity
-                    << "] marked as new, but it's already on the map."
-                    << std::endl;
-            return true;
-        }
-
-        // Check if parent link exists
-        if (this->entityLinkMap.find(_parent->Data())
-            == this->entityLinkMap.end()) {
-            ignwarn << "Collision's parent entity [" << _parent->Data()
-                    << "] not found on link map." << std::endl;
-            return true;
-        }
-        auto linkPtrPhys = this->entityLinkMap.at(_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
-        // collision.
-        sdf::Collision collision = _collElement->Data();
-        collision.SetRawPose(_pose->Data());
-        collision.SetPoseRelativeTo("");
-        auto collideBitmask = collision.Surface()->Contact()->CollideBitmask();
-
-        ShapePtrType collisionPtrPhys;
-        if (_geom->Data().Type() == sdf::GeometryType::MESH) {
-            const sdf::Mesh* meshSdf = _geom->Data().MeshShape();
-            if (nullptr == meshSdf) {
-                ignwarn << "Mesh geometry for collision [" << _name->Data()
-                        << "] missing mesh shape." << std::endl;
-                return true;
-            }
-
-            auto& meshManager = *ignition::common::MeshManager::Instance();
-            auto fullPath = asFullPath(meshSdf->Uri(), meshSdf->FilePath());
-            auto* mesh = meshManager.Load(fullPath);
-            if (nullptr == mesh) {
-                ignwarn << "Failed to load mesh from [" << fullPath << "]."
-                        << std::endl;
-                return true;
-            }
-
-            auto linkMeshFeature = entityCast(
-                _parent->Data(), linkPtrPhys, this->entityLinkMeshMap);
-            if (!linkMeshFeature) {
-                static bool informed{false};
-                if (!informed) {
-                    igndbg
-                        << "Attempting to process mesh geometries, but the "
-                        << "physics engine doesn't support feature "
-                        << "[AttachMeshShapeFeature]. Meshes will be ignored."
-                        << std::endl;
-                    informed = true;
-                }
-                return true;
-            }
-
-            collisionPtrPhys = linkMeshFeature->AttachMeshShape(
-                _name->Data(),
-                *mesh,
-                math::eigen3::convert(_pose->Data()),
-                math::eigen3::convert(meshSdf->Scale()));
-        }
-        else {
-            auto linkCollisionFeature = entityCast(
-                _parent->Data(), linkPtrPhys, this->entityLinkCollisionMap);
-            if (!linkCollisionFeature) {
-                static bool informed{false};
-                if (!informed) {
-                    igndbg
-                        << "Attempting to process collisions, but the physics "
-                        << "engine doesn't support feature "
-                        << "[ConstructSdfCollision]. Collisions will be "
-                        << "ignored." << std::endl;
-                    informed = true;
-                }
-                return true;
-            }
-
-            collisionPtrPhys =
-                linkCollisionFeature->ConstructCollision(collision);
-        }
-        // Check that the physics engine has a filter mask feature
-        // Set the collide_bitmask if it does
-        auto filterMaskFeature =
-            entityCast(_parent->Data(), collisionPtrPhys, entityShapeMaskMap);
-        if (filterMaskFeature) {
-            filterMaskFeature->SetCollisionFilterMask(collideBitmask);
-        }
-        else {
-            static bool informed{false};
-            if (!informed) {
-                igndbg
-                    << "Attempting to set collision bitmasks, but the physics "
-                    << "engine doesn't support feature [CollisionFilterMask]. "
-                    << "Collision bitmasks will be ignored." << std::endl;
-                informed = true;
-            }
-        }
-
-        this->entityCollisionMap.insert(
-            std::make_pair(_entity, collisionPtrPhys));
-        this->collisionEntityMap.insert(
-            std::make_pair(collisionPtrPhys, _entity));
-        return true;
-    };
-
-    auto processJoint =
-        [&](const Entity& _entity,
-            const components::Joint* /* _joint */,
-            const components::Name* _name,
-            const components::JointType* _jointType,
-            const components::Pose* _pose,
-            const components::ThreadPitch* _threadPitch,
-            const components::ParentEntity* _parentModel,
-            const components::ParentLinkName* _parentLinkName,
-            const components::ChildLinkName* _childLinkName) -> bool {
-        // Check if joint already exists
-        if (this->entityJointMap.find(_entity) != this->entityJointMap.end()) {
-            ignwarn << "Joint entity [" << _entity
-                    << "] marked as new, but it's already on the map."
-                    << std::endl;
-            return true;
-        }
-
-        // Check if parent model exists
-        if (this->entityModelMap.find(_parentModel->Data())
-            == this->entityModelMap.end()) {
-            ignwarn << "Joint's parent entity [" << _parentModel->Data()
-                    << "] not found on model map." << std::endl;
-            return true;
-        }
-        auto modelPtrPhys = this->entityModelMap.at(_parentModel->Data());
-
-        auto modelJointFeature = entityCast(
-            _parentModel->Data(), modelPtrPhys, this->entityModelJointMap);
-        if (!modelJointFeature) {
-            static bool informed{false};
-            if (!informed) {
-                igndbg << "Attempting to process joints, but the physics "
-                       << "engine doesn't support joint features. "
-                       << "Joints will be ignored." << std::endl;
-                informed = true;
-            }
-
-            // Break Each call since no joints can be processed
-            return false;
-        }
-
-        sdf::Joint joint;
-        joint.SetName(_name->Data());
-        joint.SetType(_jointType->Data());
-        joint.SetRawPose(_pose->Data());
-        joint.SetThreadPitch(_threadPitch->Data());
-
-        joint.SetParentLinkName(_parentLinkName->Data());
-        joint.SetChildLinkName(_childLinkName->Data());
-
-        auto jointAxis = _ecm.Component<components::JointAxis>(_entity);
-        auto jointAxis2 = _ecm.Component<components::JointAxis2>(_entity);
-
-        // Since we're making copies of the joint axes that were created
-        // using `Model::Load`, frame semantics should work for resolving
-        // their xyz axis
-        if (jointAxis)
-            joint.SetAxis(0, jointAxis->Data());
-        if (jointAxis2)
-            joint.SetAxis(1, jointAxis2->Data());
-
-        // Use the parent link's parent model as the model of this joint
-        auto jointPtrPhys = modelJointFeature->ConstructJoint(joint);
-
-        if (jointPtrPhys.Valid()) {
-            // 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));
-        }
-        return true;
-    };
-
-    auto processDetachableJoint =
-        [&](const Entity& _entity,
-            const components::DetachableJoint* _jointInfo) -> bool {
-        if (_jointInfo->Data().jointType != "fixed") {
-            ignerr << "Detachable joint type [" << _jointInfo->Data().jointType
-                   << "] is currently not supported" << std::endl;
-            return true;
-        }
-        // Check if joint already exists
-        if (this->entityJointMap.find(_entity) != this->entityJointMap.end()) {
-            ignwarn << "Joint entity [" << _entity
-                    << "] marked as new, but it's already on the map."
-                    << std::endl;
-            return true;
-        }
-
-        // Check if the link entities exist in the physics engine
-        auto parentLinkPhysIt =
-            this->entityLinkMap.find(_jointInfo->Data().parentLink);
-        if (parentLinkPhysIt == this->entityLinkMap.end()) {
-            ignwarn << "DetachableJoint's parent link entity ["
-                    << _jointInfo->Data().parentLink
-                    << "] not found in link map." << std::endl;
-            return true;
-        }
-
-        auto childLinkEntity = _jointInfo->Data().childLink;
-
-        // Get child link
-        auto childLinkIt = this->entityLinkMap.find(childLinkEntity);
-        if (childLinkIt == this->entityLinkMap.end()) {
-            ignwarn << "Failed to find joint's child link [" << childLinkEntity
-                    << "]." << std::endl;
-            return true;
-        }
-
-        auto childLinkDetachableJointFeature =
-            entityCast(childLinkEntity,
-                       childLinkIt->second,
-                       this->entityLinkDetachableJointMap);
-        if (!childLinkDetachableJointFeature) {
-            static bool informed{false};
-            if (!informed) {
-                igndbg
-                    << "Attempting to create a detachable joint, but the "
-                       "physics"
-                    << " engine doesn't support feature "
-                    << "[AttachFixedJointFeature]. Detachable joints will be "
-                    << "ignored." << std::endl;
-                informed = true;
-            }
-
-            // Break Each call since no DetachableJoints can be processed
-            return false;
-        }
-
-        const auto poseParent =
-            parentLinkPhysIt->second->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);
-        if (jointPtrPhys.Valid()) {
-            // We let the joint be at the origin of the child link.
-            jointPtrPhys->SetTransformFromParent(poseParentChild);
-
-            igndbg << "Creating detachable joint [" << _entity << "]"
-                   << std::endl;
-            this->entityJointMap.insert(std::make_pair(_entity, jointPtrPhys));
-        }
-        else {
-            ignwarn << "DetachableJoint could not be created." << std::endl;
-        }
-        return true;
-    };
-
-    if (this->firstRun) {
-        this->firstRun = false;
-
-        _ecm.Each<components::World, components::Name, components::Gravity>(
-            processWorld);
-
-        _ecm.Each<components::Model,
-                  components::Name,
-                  components::Pose,
-                  components::ParentEntity>(processModel);
-
-        _ecm.Each<components::Link,
-                  components::Name,
-                  components::Pose,
-                  components::ParentEntity>(processLink);
-
-        // We don't need to add visuals to the physics engine.
-
-        _ecm.Each<components::Collision,
-                  components::Name,
-                  components::Pose,
-                  components::Geometry,
-                  components::CollisionElement,
-                  components::ParentEntity>(processCollision);
-
-        _ecm.Each<components::Joint,
-                  components::Name,
-                  components::JointType,
-                  components::Pose,
-                  components::ThreadPitch,
-                  components::ParentEntity,
-                  components::ParentLinkName,
-                  components::ChildLinkName>(processJoint);
-
-        _ecm.Each<components::BatterySoC>(
-            [&](const Entity& _entity, const components::BatterySoC*) -> bool {
-                // Parent entity of battery is model entity
-                this->entityOffMap.insert(
-                    std::make_pair(_ecm.ParentEntity(_entity), false));
-                return true;
-            });
-
-        _ecm.Each<components::DetachableJoint>(processDetachableJoint);
-    }
-    else {
-        _ecm.EachNew<components::World, components::Name, components::Gravity>(
-            processWorld);
-
-        _ecm.EachNew<components::Model,
-                     components::Name,
-                     components::Pose,
-                     components::ParentEntity>(processModel);
-
-        _ecm.EachNew<components::Link,
-                     components::Name,
-                     components::Pose,
-                     components::ParentEntity>(processLink);
-
-        // We don't need to add visuals to the physics engine.
-
-        _ecm.EachNew<components::Collision,
-                     components::Name,
-                     components::Pose,
-                     components::Geometry,
-                     components::CollisionElement,
-                     components::ParentEntity>(processCollision);
-
-        _ecm.EachNew<components::Joint,
-                     components::Name,
-                     components::JointType,
-                     components::Pose,
-                     components::ThreadPitch,
-                     components::ParentEntity,
-                     components::ParentLinkName,
-                     components::ChildLinkName>(processJoint);
-
-        _ecm.EachNew<components::BatterySoC>(
-            [&](const Entity& _entity, const components::BatterySoC*) -> bool {
-                // Parent entity of battery is model entity
-                this->entityOffMap.insert(
-                    std::make_pair(_ecm.ParentEntity(_entity), false));
-                return true;
-            });
-
-        _ecm.EachNew<components::DetachableJoint>(processDetachableJoint);
-    }
-}
-
-void Physics::Impl::RemovePhysicsEntities(const EntityComponentManager& _ecm)
-{
-    // Assume the world will not be erased
-    // Only removing models is supported by ign-physics right now so we only
-    // remove links, joints and collisions if they are children of the
-    // removed model. We assume the links, joints and collisions will be
-    // removed from the physics engine when the containing model gets
-    // removed so, here, we only remove the entities from the gazebo
-    // entity->physics entity map.
-    _ecm.EachRemoved<components::Model>([&](const Entity& _entity,
-                                            const components::Model*
-                                            /* _model */) -> bool {
-        // Remove model if found
-        auto modelIt = this->entityModelMap.find(_entity);
-        if (modelIt != this->entityModelMap.end()) {
-            // Remove child links, collisions and joints first
-            for (const auto& childLink :
-                 _ecm.ChildrenByComponents(_entity, components::Link())) {
-                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->entityLinkMap.erase(childLink);
-            }
-
-            for (const auto& childJoint :
-                 _ecm.ChildrenByComponents(_entity, components::Joint())) {
-                this->entityJointMap.erase(childJoint);
-            }
-
-            // Remove the model from the physics engine
-            modelIt->second->Remove();
-            this->entityModelMap.erase(_entity);
-        }
-        return true;
-    });
-
-    _ecm.EachRemoved<components::DetachableJoint>(
-        [&](const Entity& _entity, const components::DetachableJoint*) -> bool {
-            auto jointIt = this->entityJointMap.find(_entity);
-            if (jointIt == this->entityJointMap.end()) {
-                ignwarn << "Failed to find joint [" << _entity << "]."
-                        << std::endl;
-                return true;
-            }
-
-            auto castEntity = entityCast(
-                _entity, jointIt->second, this->entityJointDetachableJointMap);
-            if (!castEntity) {
-                static bool informed{false};
-                if (!informed) {
-                    igndbg << "Attempting to detach a joint, but the physics "
-                           << "engine doesn't support feature "
-                           << "[DetachJointFeature]. Joint won't be detached."
-                           << std::endl;
-                    informed = true;
-                }
-
-                // Break Each call since no DetachableJoints can be processed
-                return false;
-            }
-
-            igndbg << "Detaching joint [" << _entity << "]" << std::endl;
-            castEntity->Detach();
-            return true;
-        });
-}
-
-void Physics::Impl::UpdatePhysics(const ignition::gazebo::UpdateInfo& _info,
-                                  EntityComponentManager& _ecm)
-{
-    // Battery state
-    _ecm.Each<components::BatterySoC>(
-        [&](const Entity& _entity, const components::BatterySoC* _bat) {
-            if (_bat->Data() <= 0)
-                entityOffMap[_ecm.ParentEntity(_entity)] = true;
-            else
-                entityOffMap[_ecm.ParentEntity(_entity)] = false;
-            return true;
-        });
-
-    // Handle joint state
-    _ecm.Each<components::Joint, components::Name>([&](const Entity& _entity,
-                                                       const components::Joint*,
-                                                       const components::Name*
-                                                           _name) {
-        auto jointIt = this->entityJointMap.find(_entity);
-        if (jointIt == this->entityJointMap.end())
-            return true;
-
-        // Model is out of battery
-        if (this->entityOffMap[_ecm.ParentEntity(_entity)]) {
-            std::size_t nDofs = jointIt->second->GetDegreesOfFreedom();
-            for (std::size_t i = 0; i < nDofs; ++i) {
-                jointIt->second->SetForce(i, 0);
-            }
-            return true;
-        }
-
-        auto posReset = _ecm.Component<components::JointPositionReset>(_entity);
-        auto velReset = _ecm.Component<components::JointVelocityReset>(_entity);
-
-        // Reset the velocity
-        if (velReset) {
-            auto& jointVelocity = velReset->Data();
-
-            if (jointVelocity.size()
-                != jointIt->second->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() << " while the "
-                    << " component has " << jointVelocity.size() << ".\n";
-            }
-
-            std::size_t nDofs = std::min(
-                jointVelocity.size(), jointIt->second->GetDegreesOfFreedom());
-
-            for (std::size_t i = 0; i < nDofs; ++i) {
-                jointIt->second->SetVelocity(i, jointVelocity[i]);
-            }
-        }
-
-        // Reset the position
-        if (posReset) {
-            auto& jointPosition = posReset->Data();
-
-            if (jointPosition.size()
-                != jointIt->second->GetDegreesOfFreedom()) {
-                ignwarn
-                    << "There is a mismatch in the degrees of freedom "
-                       "between "
-                    << "Joint [" << _name->Data() << "(Entity=" << _entity
-                    << ")] and its JointPositionReset component. The joint has "
-                    << jointIt->second->GetDegreesOfFreedom() << " while the "
-                    << " component has " << jointPosition.size() << ".\n";
-            }
-            std::size_t nDofs = std::min(
-                jointPosition.size(), jointIt->second->GetDegreesOfFreedom());
-            for (std::size_t i = 0; i < nDofs; ++i) {
-                jointIt->second->SetPosition(i, jointPosition[i]);
-            }
-        }
-
-        auto force = _ecm.Component<components::JointForceCmd>(_entity);
-        auto velCmd = _ecm.Component<components::JointVelocityCmd>(_entity);
-
-        if (force) {
-            if (force->Data().size()
-                != jointIt->second->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 "
-                        << " component has " << force->Data().size() << ".\n";
-            }
-            std::size_t nDofs = std::min(
-                force->Data().size(), jointIt->second->GetDegreesOfFreedom());
-            for (std::size_t i = 0; i < nDofs; ++i) {
-                jointIt->second->SetForce(i, force->Data()[i]);
-            }
-        }
-        else {
-            // Only set joint velocity if joint force is not set.
-            // If both the cmd and reset components are found, cmd is
-            // ignored.
-            if (velCmd) {
-                auto velocityCmd = velCmd->Data();
-
-                if (velReset) {
-                    ignwarn << "Found both JointVelocityReset and "
-                            << "JointVelocityCmd components "
-                            << "for Joint [" << _name->Data()
-                            << "(Entity=" << _entity
-                            << "]). Ignoring JointVelocityReset component."
-                            << std::endl;
-                    return true;
-                }
-
-                if (velocityCmd.size()
-                    != jointIt->second->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()
-                            << " while the component has " << velocityCmd.size()
-                            << ".\n";
-                }
-
-                auto jointVelFeature =
-                    entityCast(_entity,
-                               jointIt->second,
-                               this->entityJointVelocityCommandMap);
-                if (!jointVelFeature) {
-                    return true;
-                }
-
-                std::size_t nDofs = std::min(
-                    velocityCmd.size(), jointIt->second->GetDegreesOfFreedom());
-
-                for (std::size_t i = 0; i < nDofs; ++i) {
-                    jointVelFeature->SetVelocityCommand(i, velocityCmd[i]);
-                }
-            }
-        }
-        return true;
-    });
-
-    // Link wrenches
-    _ecm.Each<components::ExternalWorldWrenchCmd>(
-        [&](const Entity& _entity,
-            const components::ExternalWorldWrenchCmd* _wrenchComp) {
-            auto linkIt = this->entityLinkMap.find(_entity);
-            if (linkIt == this->entityLinkMap.end()) {
-                ignwarn << "Failed to find link [" << _entity << "]."
-                        << std::endl;
-                return true;
-            }
-
-            auto linkForceFeature =
-                entityCast(_entity, linkIt->second, this->entityLinkForceMap);
-            if (!linkForceFeature) {
-                static bool informed{false};
-                if (!informed) {
-                    igndbg << "Attempting to apply a wrench, but the physics "
-                           << "engine doesn't support feature "
-                           << "[AddLinkExternalForceTorque]. Wrench will be "
-                              "ignored."
-                           << std::endl;
-                    informed = true;
-                }
-
-                // Break Each call since no ExternalWorldWrenchCmd's can be
-                // processed
-                return false;
-            }
-
-            math::Vector3 force = msgs::Convert(_wrenchComp->Data().force());
-            math::Vector3 torque = msgs::Convert(_wrenchComp->Data().torque());
-            linkForceFeature->AddExternalForce(math::eigen3::convert(force));
-            linkForceFeature->AddExternalTorque(math::eigen3::convert(torque));
-
-            return true;
-        });
-
-    // Link wrenches with duration
-    if (!_info.paused) {
-        _ecm.Each<components::ExternalWorldWrenchCmdWithDuration>(
-            [&](const Entity& _entity,
-                components::ExternalWorldWrenchCmdWithDuration*
-                    _wrenchWithDurComp) {
-                auto linkIt = this->entityLinkMap.find(_entity);
-                if (linkIt == this->entityLinkMap.end()) {
-                    ignwarn << "Failed to find link [" << _entity << "]."
-                            << std::endl;
-                    return true;
-                }
-
-                auto linkForceFeature = entityCast(
-                    _entity, linkIt->second, this->entityLinkForceMap);
-                if (!linkForceFeature) {
-                    static bool informed{false};
-                    if (!informed) {
-                        igndbg
-                            << "Attempting to apply a wrench, but the physics "
-                            << "engine doesn't support feature "
-                            << "[AddLinkExternalForceTorque]. Wrench will be "
-                               "ignored."
-                            << std::endl;
-                        informed = true;
-                    }
-
-                    // Break Each call since no ExternalWorldWrenchCmd's can be
-                    // processed
-                    return false;
-                }
-
-                const auto& totalWrench =
-                    _wrenchWithDurComp->Data().totalWrench();
-                const math::Vector3 force = msgs::Convert(totalWrench.force());
-                const math::Vector3 torque =
-                    msgs::Convert(totalWrench.torque());
-
-                linkForceFeature->AddExternalForce(
-                    math::eigen3::convert(force));
-                linkForceFeature->AddExternalTorque(
-                    math::eigen3::convert(torque));
-
-                // NOTE: Cleaning could be moved to UpdateSim, but let's
-                //       keep things all together for now
-                auto simTimeAfterStep = _info.simTime;
-                _wrenchWithDurComp->Data().cleanExpired(simTimeAfterStep);
-
-                return true;
-            });
-    }
-
-    // Update model pose
-    _ecm.Each<components::Model, components::WorldPoseCmd>(
-        [&](const Entity& _entity,
-            const components::Model*,
-            const components::WorldPoseCmd* _poseCmd) {
-            auto modelIt = this->entityModelMap.find(_entity);
-            if (modelIt == this->entityModelMap.end())
-                return true;
-
-            // world pose cmd currently not supported for nested models
-            if (_entity != topLevelModel(_entity, _ecm)) {
-                ignerr << "Unable to set world pose for nested models."
-                       << std::endl;
-                return true;
-            }
-
-            // The canonical link as specified by sdformat is different from
-            // the canonical link of the FreeGroup object
-
-            // TODO(addisu) Store the free group instead of searching for it
-            // at every iteration
-            auto freeGroup = modelIt->second->FindFreeGroup();
-            if (!freeGroup)
-                return true;
-
-            // Get canonical link offset
-            auto linkEntityIt =
-                this->linkEntityMap.find(freeGroup->CanonicalLink());
-            if (linkEntityIt == this->linkEntityMap.end())
-                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);
-
-            freeGroup->SetWorldPose(
-                math::eigen3::convert(_poseCmd->Data() * 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()) {
-                auto worldPoseComp = _ecm.Component<components::Pose>(_entity);
-                if (worldPoseComp) {
-                    auto state =
-                        worldPoseComp->SetData(_poseCmd->Data(), this->pose3Eql)
-                            ? ComponentState::OneTimeChange
-                            : ComponentState::NoChange;
-                    _ecm.SetChanged(_entity, components::Pose::typeId, state);
-                }
-            }
-
-            return true;
-        });
-
-    // Slip compliance on Collisions
-    _ecm.Each<components::SlipComplianceCmd>(
-        [&](const Entity& _entity,
-            const components::SlipComplianceCmd* _slipCmdComp) {
-            auto shapeIt = this->entityCollisionMap.find(_entity);
-            if (shapeIt == this->entityCollisionMap.end()) {
-                ignwarn << "Failed to find shape [" << _entity << "]."
-                        << std::endl;
-                return true;
-            }
-
-            auto slipComplianceShape = entityCast(
-                _entity, shapeIt->second, this->entityShapeSlipParamMap);
-
-            if (!slipComplianceShape) {
-                ignwarn << "Can't process Wheel Slip component, physics engine "
-                        << "missing SetShapeFrictionPyramidSlipCompliance"
-                        << std::endl;
-
-                // Break Each call since no SlipCompliances can be processed
-                return false;
-            }
-
-            if (_slipCmdComp->Data().size() == 2) {
-                slipComplianceShape->SetPrimarySlipCompliance(
-                    _slipCmdComp->Data()[0]);
-                slipComplianceShape->SetSecondarySlipCompliance(
-                    _slipCmdComp->Data()[1]);
-            }
-
-            return true;
-        });
-
-    // Update model angular velocity
-    _ecm.Each<components::Model, components::AngularVelocityCmd>(
-        [&](const Entity& _entity,
-            const components::Model*,
-            const components::AngularVelocityCmd* _angularVelocityCmd) {
-            auto modelIt = this->entityModelMap.find(_entity);
-            if (modelIt == this->entityModelMap.end())
-                return true;
-
-            // angular vel cmd currently not supported for nested models
-            if (_entity != topLevelModel(_entity, _ecm)) {
-                ignerr << "Unable to set angular velocity for nested models."
-                       << std::endl;
-                return true;
-            }
-
-            auto freeGroup = modelIt->second->FindFreeGroup();
-            if (!freeGroup)
-                return true;
-
-            const components::Pose* poseComp =
-                _ecm.Component<components::Pose>(_entity);
-            math::Vector3d worldAngularVel =
-                poseComp->Data().Rot() * _angularVelocityCmd->Data();
-
-            auto worldAngularVelFeature = entityCast(
-                _entity, freeGroup, this->entityWorldVelocityCommandMap);
-            if (!worldAngularVelFeature) {
-                static bool informed{false};
-                if (!informed) {
-                    igndbg
-                        << "Attempting to set model angular velocity, but the "
-                        << "physics engine doesn't support velocity commands. "
-                        << "Velocity won't be set." << std::endl;
-                    informed = true;
-                }
-                return true;
-            }
-
-            worldAngularVelFeature->SetWorldAngularVelocity(
-                math::eigen3::convert(worldAngularVel));
-
-            return true;
-        });
-
-    // Update model linear velocity
-    _ecm.Each<components::Model, components::LinearVelocityCmd>(
-        [&](const Entity& _entity,
-            const components::Model*,
-            const components::LinearVelocityCmd* _linearVelocityCmd) {
-            auto modelIt = this->entityModelMap.find(_entity);
-            if (modelIt == this->entityModelMap.end())
-                return true;
-
-            // linear vel cmd currently not supported for nested models
-            if (_entity != topLevelModel(_entity, _ecm)) {
-                ignerr << "Unable to set linear velocity for nested models."
-                       << std::endl;
-                return true;
-            }
-
-            auto freeGroup = modelIt->second->FindFreeGroup();
-            if (!freeGroup)
-                return true;
-
-            const components::Pose* poseComp =
-                _ecm.Component<components::Pose>(_entity);
-            math::Vector3d worldLinearVel =
-                poseComp->Data().Rot() * _linearVelocityCmd->Data();
-
-            auto worldLinearVelFeature = entityCast(
-                _entity, freeGroup, this->entityWorldVelocityCommandMap);
-            if (!worldLinearVelFeature) {
-                static bool informed{false};
-                if (!informed) {
-                    igndbg
-                        << "Attempting to set model linear velocity, but the "
-                        << "physics engine doesn't support velocity commands. "
-                        << "Velocity won't be set." << std::endl;
-                    informed = true;
-                }
-                return true;
-            }
-
-            worldLinearVelFeature->SetWorldLinearVelocity(
-                math::eigen3::convert(worldLinearVel));
-
-            return true;
-        });
-
-    // Process WorldVelocityCmd
-    _ecm.Each<components::Model, components::WorldVelocityCmd>(
-        [&](const Entity& _entity,
-            const components::Model*,
-            components::WorldVelocityCmd* _modelWorldVelocityCmd) {
-            auto modelIt = this->entityModelMap.find(_entity);
-            if (modelIt == this->entityModelMap.end())
-                return true;
-
-            // The canonical link as specified by sdformat is different from
-            // the canonical link of the FreeGroup object
-
-            // TODO(addisu) Store the free group instead of searching for it
-            // at every iteration
-
-            // The FreeGroup is created only for floating-base object that
-            // do not have any defined joint between the world and their
-            // base
-            auto freeGroup = modelIt->second->FindFreeGroup();
-            if (!freeGroup) {
-                ignwarn << "Failed to find FreeGroup. Linear and angular "
-                           "velocities commands ignored."
-                        << std::endl;
-                return true;
-            }
-
-            ignition::math::Vector3d& modelWorldLinearVelocity =
-                _modelWorldVelocityCmd->Data().linear;
-            ignition::math::Vector3d& modelWorldAngularVelocity =
-                _modelWorldVelocityCmd->Data().angular;
-
-            freeGroup->SetWorldLinearVelocity(
-                math::eigen3::convert(modelWorldLinearVelocity));
-            freeGroup->SetWorldAngularVelocity(
-                math::eigen3::convert(modelWorldAngularVelocity));
-
-            // TODO(diego): static models from above
-            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
-    _ecm.Each<components::Model,
-              components::AxisAlignedBox>([&](const Entity& _entity,
-                                              const components::Model*,
-                                              components::AxisAlignedBox*
-                                                  _bbox) {
-        auto modelIt = this->entityModelMap.find(_entity);
-        if (modelIt == this->entityModelMap.end()) {
-            ignwarn << "Failed to find model [" << _entity << "]." << std::endl;
-            return true;
-        }
-
-        auto bbModel = entityCast(
-            _entity, modelIt->second, this->entityModelBoundingBoxMap);
-        if (!bbModel) {
-            static bool informed{false};
-            if (!informed) {
-                igndbg
-                    << "Attempting to get a bounding box, but the physics "
-                    << "engine doesn't support feature "
-                    << "[GetModelBoundingBox]. Bounding box won't be populated."
-                    << std::endl;
-                informed = true;
-            }
-
-            // Break Each call since no AxisAlignedBox'es can be processed
-            return false;
-        }
-
-        math::AxisAlignedBox bbox =
-            math::eigen3::convert(bbModel->GetAxisAlignedBoundingBox());
-        auto state = _bbox->SetData(bbox, this->axisAlignedBoxEql)
-                         ? ComponentState::OneTimeChange
-                         : ComponentState::NoChange;
-        _ecm.SetChanged(_entity, components::AxisAlignedBox::typeId, state);
-
-        return true;
-    });
-
-    // Clear WorldVelocityCmd
-    entitiesWorldCmd.clear();
-    _ecm.Each<components::WorldVelocityCmd>(
-        [&](const Entity& _entity, components::WorldVelocityCmd*) -> bool {
-            entitiesWorldCmd.push_back(_entity);
-            return true;
-        });
-
-    for (const Entity& entity : entitiesWorldCmd) {
-        _ecm.RemoveComponent<components::WorldVelocityCmd>(entity);
-    }
-}
-
-void Physics::Impl::Step(const std::chrono::steady_clock::duration& _dt)
-{
-    ignition::physics::ForwardStep::Input input;
-    ignition::physics::ForwardStep::State state;
-    ignition::physics::ForwardStep::Output output;
-
-    input.Get<std::chrono::steady_clock::duration>() = _dt;
-
-    for (auto& world : this->entityWorldMap) {
-        world.second->Step(output, state, input);
-    }
-}
-
-ignition::math::Pose3d
-Physics::Impl::RelativePose(const Entity& _from,
-                            const Entity& _to,
-                            const EntityComponentManager& _ecm) const
-{
-    math::Pose3d transform;
-
-    if (_from == _to)
-        return transform;
-
-    auto currentEntity = _to;
-    auto parentComp = _ecm.Component<components::ParentEntity>(_to);
-    while (parentComp) {
-        auto parentEntity = parentComp->Data();
-
-        // get the entity pose
-        auto entityPoseComp = _ecm.Component<components::Pose>(currentEntity);
-
-        // update transform
-        transform = entityPoseComp->Data() * transform;
-
-        if (parentEntity == _from)
-            break;
-
-        // set current entity to parent
-        currentEntity = parentEntity;
-
-        // get entity's parent
-        parentComp = _ecm.Component<components::ParentEntity>(parentEntity);
-    }
-
-    return transform;
-}
-
-void Physics::Impl::UpdateSim(const ignition::gazebo::UpdateInfo& _info,
-                              EntityComponentManager& _ecm)
-{
-    // local pose
-    _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())
-            return true;
-
-        auto linkIt = this->entityLinkMap.find(_entity);
-        if (linkIt != this->entityLinkMap.end()) {
-            // get top level model of this link
-            auto topLevelModelEnt = topLevelModel(_parent->Data(), _ecm);
-
-            auto canonicalLink =
-                _ecm.Component<components::CanonicalLink>(_entity);
-
-            auto frameData = linkIt->second->FrameDataRelativeToWorld();
-            const auto& worldPose = frameData.pose;
-
-            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);
-            }
-
-            // 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);
-            }
-
-            // 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);
-            }
-
-            // 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);
-            }
-
-            // 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 acceleration in world frame coordinates
-            auto worldAngAccelComp =
-                _ecm.Component<components::WorldAngularAcceleration>(_entity);
-
-            if (worldAngAccelComp) {
-                auto state =
-                    worldAngAccelComp->SetData(
-                        math::eigen3::convert(frameData.angularAcceleration),
-                        this->vec3Eql)
-                        ? ComponentState::PeriodicChange
-                        : ComponentState::NoChange;
-                _ecm.SetChanged(_entity,
-                                components::WorldAngularAcceleration::typeId,
-                                state);
-            }
-
-            const Eigen::Matrix3d R_bs =
-                worldPose.linear().transpose(); // NOLINT
-
-            // 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);
-            }
-
-            // 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);
-            }
-
-            // 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;
-    });
-
-    // history of applied joint forces (commands)
-    _ecm.Each<components::Joint,
-              components::Name,
-              components::JointForceCmd>([&](const Entity& _entity,
-                                             components::Joint* /*_joint*/,
-                                             components::Name* /*_name*/,
-                                             components::JointForceCmd*
-                                                 _forceCmd) -> bool {
-        // Get the data from the components
-        auto jointForceCmdData = _forceCmd->Data();
-
-        // If the history is enabled, append the force command also there
-        auto historyComponent = _ecm.Component<
-            ignition::gazebo::components::HistoryOfAppliedJointForces>(_entity);
-
-        // Since the operation is an append, we have to perform it only when
-        // the physics step is actually performed
-        if (!_info.paused && historyComponent) {
-            auto& history = scenario::gazebo::utils::getExistingComponentData<
-                ignition::gazebo::components::HistoryOfAppliedJointForces>(
-                &_ecm, _entity);
-
-            for (const auto& jointForceCmd : jointForceCmdData) {
-                history.push(jointForceCmd);
-            }
-        }
-
-        return true;
-    });
-
-    // pose/velocity/acceleration of non-link entities such as sensors /
-    // collisions. These get updated only if another system has created a
-    // components::WorldPose component for the entity.
-    // Populated components:
-    // * WorldPose
-    // * WorldLinearVelocity
-    // * AngularVelocity
-    // * LinearAcceleration
-
-    // world pose
-    _ecm.Each<components::Pose,
-              components::WorldPose,
-              components::ParentEntity>(
-        [&](const Entity&,
-            const components::Pose* _pose,
-            components::WorldPose* _worldPose,
-            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()) {
-                const auto entityFrameData =
-                    this->LinkFrameDataAtOffset(linkIt->second, _pose->Data());
-
-                *_worldPose = components::WorldPose(
-                    math::eigen3::convert(entityFrameData.pose));
-            }
-
-            return true;
-        });
-
-    // world linear velocity
-    _ecm.Each<components::Pose,
-              components::WorldLinearVelocity,
-              components::ParentEntity>(
-        [&](const Entity&,
-            const components::Pose* _pose,
-            components::WorldLinearVelocity* _worldLinearVel,
-            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()) {
-                const auto entityFrameData =
-                    this->LinkFrameDataAtOffset(linkIt->second, _pose->Data());
-
-                // set entity world linear velocity
-                *_worldLinearVel = components::WorldLinearVelocity(
-                    math::eigen3::convert(entityFrameData.linearVelocity));
-            }
-
-            return true;
-        });
-
-    // body angular velocity
-    _ecm.Each<components::Pose,
-              components::AngularVelocity,
-              components::ParentEntity>(
-        [&](const Entity&,
-            const components::Pose* _pose,
-            components::AngularVelocity* _angularVel,
-            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()) {
-                const auto entityFrameData =
-                    this->LinkFrameDataAtOffset(linkIt->second, _pose->Data());
-
-                auto entityWorldPose =
-                    math::eigen3::convert(entityFrameData.pose);
-                ignition::math::Vector3d entityWorldAngularVel =
-                    math::eigen3::convert(entityFrameData.angularVelocity);
-
-                auto entityBodyAngularVel =
-                    entityWorldPose.Rot().RotateVectorReverse(
-                        entityWorldAngularVel);
-                *_angularVel =
-                    components::AngularVelocity(entityBodyAngularVel);
-            }
-
-            return true;
-        });
-
-    // body linear acceleration
-    _ecm.Each<components::Pose,
-              components::LinearAcceleration,
-              components::ParentEntity>(
-        [&](const Entity&,
-            const components::Pose* _pose,
-            components::LinearAcceleration* _linearAcc,
-            const components::ParentEntity* _parent) -> bool {
-            auto linkIt = this->entityLinkMap.find(_parent->Data());
-            if (linkIt != this->entityLinkMap.end()) {
-                const auto entityFrameData =
-                    this->LinkFrameDataAtOffset(linkIt->second, _pose->Data());
-
-                auto entityWorldPose =
-                    math::eigen3::convert(entityFrameData.pose);
-                ignition::math::Vector3d entityWorldLinearAcc =
-                    math::eigen3::convert(entityFrameData.linearAcceleration);
-
-                auto entityBodyLinearAcc =
-                    entityWorldPose.Rot().RotateVectorReverse(
-                        entityWorldLinearAcc);
-                *_linearAcc =
-                    components::LinearAcceleration(entityBodyLinearAcc);
-            }
-
-            return true;
-        });
-
-    // Clear reset components
-    std::vector<Entity> entitiesPositionReset;
-    _ecm.Each<components::JointPositionReset>(
-        [&](const Entity& _entity, components::JointPositionReset*) -> bool {
-            entitiesPositionReset.push_back(_entity);
-            return true;
-        });
-
-    for (const auto entity : entitiesPositionReset) {
-        _ecm.RemoveComponent<components::JointPositionReset>(entity);
-    }
-
-    std::vector<Entity> entitiesVelocityReset;
-    _ecm.Each<components::JointVelocityReset>(
-        [&](const Entity& _entity, components::JointVelocityReset*) -> bool {
-            entitiesVelocityReset.push_back(_entity);
-            return true;
-        });
-
-    for (const auto entity : entitiesVelocityReset) {
-        _ecm.RemoveComponent<components::JointVelocityReset>(entity);
-    }
-
-    // Clear pending commands
-    _ecm.Each<components::JointForceCmd>(
-        [&](const Entity&, components::JointForceCmd* _force) -> bool {
-            std::fill(_force->Data().begin(), _force->Data().end(), 0.0);
-            return true;
-        });
-
-    _ecm.Each<components::ExternalWorldWrenchCmd>(
-        [&](const Entity&,
-            components::ExternalWorldWrenchCmd* _wrench) -> bool {
-            _wrench->Data().Clear();
-            return true;
-        });
-
-    _ecm.Each<components::JointVelocityCmd>(
-        [&](const Entity&, components::JointVelocityCmd* _vel) -> bool {
-            std::fill(_vel->Data().begin(), _vel->Data().end(), 0.0);
-            return true;
-        });
-
-    _ecm.Each<components::SlipComplianceCmd>(
-        [&](const Entity&, components::SlipComplianceCmd* _slip) -> bool {
-            std::fill(_slip->Data().begin(), _slip->Data().end(), 0.0);
-            return true;
-        });
-
-    // Update joint positions
-    _ecm.Each<components::Joint, components::JointPosition>(
-        [&](const Entity& _entity,
-            components::Joint*,
-            components::JointPosition* _jointPos) -> bool {
-            auto jointIt = this->entityJointMap.find(_entity);
-            if (jointIt != this->entityJointMap.end()) {
-                _jointPos->Data().resize(
-                    jointIt->second->GetDegreesOfFreedom());
-                for (std::size_t i = 0;
-                     i < jointIt->second->GetDegreesOfFreedom();
-                     ++i) {
-                    _jointPos->Data()[i] = jointIt->second->GetPosition(i);
-                }
-            }
-            return true;
-        });
-
-    // Update joint Velocities
-    _ecm.Each<components::Joint, components::JointVelocity>(
-        [&](const Entity& _entity,
-            components::Joint*,
-            components::JointVelocity* _jointVel) -> bool {
-            auto jointIt = this->entityJointMap.find(_entity);
-            if (jointIt != this->entityJointMap.end()) {
-                _jointVel->Data().resize(
-                    jointIt->second->GetDegreesOfFreedom());
-                for (std::size_t i = 0;
-                     i < jointIt->second->GetDegreesOfFreedom();
-                     ++i) {
-                    _jointVel->Data()[i] = jointIt->second->GetVelocity(i);
-                }
-            }
-            return true;
-        });
-
-    // Update joint acceleration
-    _ecm.Each<components::Joint, components::JointAcceleration>(
-        [&](const Entity& _entity,
-            components::Joint*,
-            components::JointAcceleration* _jointAcc) -> bool {
-            auto jointIt = this->entityJointMap.find(_entity);
-            if (jointIt != this->entityJointMap.end()) {
-                _jointAcc->Data().resize(
-                    jointIt->second->GetDegreesOfFreedom());
-                for (std::size_t i = 0;
-                     i < jointIt->second->GetDegreesOfFreedom();
-                     ++i) {
-                    _jointAcc->Data()[i] = jointIt->second->GetAcceleration(i);
-                }
-            }
-            return true;
-        });
-
-    // Update joint forces
-    _ecm.Each<components::Joint, components::JointForce>(
-        [&](const Entity& _entity,
-            components::Joint*,
-            components::JointForce* _jointForce) -> bool {
-            auto jointIt = this->entityJointMap.find(_entity);
-            if (jointIt != this->entityJointMap.end()) {
-                _jointForce->Data().resize(
-                    jointIt->second->GetDegreesOfFreedom());
-                for (std::size_t i = 0;
-                     i < jointIt->second->GetDegreesOfFreedom();
-                     ++i) {
-                    _jointForce->Data()[i] = jointIt->second->GetForce(i);
-                }
-            }
-            return true;
-        });
-
-    // TODO(louise) Skip this if there are no collision features
-    this->UpdateCollisions(_ecm);
-}
-
-void Physics::Impl::UpdateCollisions(EntityComponentManager& _ecm)
-{
-    // Quit early if the ContactData component hasn't been created. This
-    // means there are no systems that need contact information
-    if (!_ecm.HasComponentType(components::ContactSensorData::typeId))
-        return;
-
-    // TODO(addisu) If systems are assumed to only have one world, we should
-    // capture the world Entity in a Configure call
-    Entity worldEntity = _ecm.EntityByComponents(components::World());
-
-    if (worldEntity == kNullEntity) {
-        ignerr << "Missing world entity.\n";
-        return;
-    }
-
-    auto worldIt = this->entityWorldMap.find(worldEntity);
-    if (worldIt == this->entityWorldMap.end()) {
-        ignwarn << "Failed to find world [" << worldEntity << "]." << std::endl;
-        return;
-    }
-
-    auto worldCollisionFeature =
-        entityCast(worldEntity, worldIt->second, this->entityWorldCollisionMap);
-    if (!worldCollisionFeature) {
-        static bool informed{false};
-        if (!informed) {
-            igndbg << "Attempting process contacts, but the physics "
-                   << "engine doesn't support collision features. "
-                   << "Contacts won't be computed." << std::endl;
-            informed = true;
-        }
-        return;
-    }
-
-    // Struct containing pointer of contact data
-    struct AllContactData
-    {
-        const WorldShapeType::ContactPoint* point;
-        const WorldShapeType::ExtraContactData* extra;
-    };
-
-    // Each contact object we get from ign-physics contains the EntityPtrs
-    // of the two colliding entities and other data about the contact such
-    // as the position. This map groups contacts so that it is easy to query
-    // all the contacts of one entity.
-    using EntityContactMap =
-        std::unordered_map<Entity, std::deque<AllContactData>>;
-
-    // This data structure is essentially a mapping between a pair of
-    // entities and a list of pointers to their contact object. We use a map
-    // inside a map to create msgs::Contact objects conveniently later on.
-    std::unordered_map<Entity, EntityContactMap> entityContactMap;
-
-    // Note that we are temporarily storing pointers to elements in this
-    // ("allContacts") container. Thus, we must make sure it doesn't get
-    // destroyed until the end of this function.
-    auto allContacts = worldCollisionFeature->GetContactsFromLastStep();
-    for (const auto& contactComposite : allContacts) {
-        // Get the RequireData
-        const auto& contact =
-            contactComposite.Get<WorldShapeType::ContactPoint>();
-        auto coll1It = this->collisionEntityMap.find(contact.collision1);
-        auto coll2It = this->collisionEntityMap.find(contact.collision2);
-
-        // Check the ExpectData
-        const auto* extraContactData =
-            contactComposite.Query<WorldShapeType::ExtraContactData>();
-
-        if ((coll1It != this->collisionEntityMap.end())
-            && (coll2It != this->collisionEntityMap.end())) {
-
-            AllContactData allContactData;
-            allContactData.point = &contact;
-
-            if (extraContactData) {
-                allContactData.extra = extraContactData;
-            }
-
-            // Note that the ExtraContactData is valid only when the first
-            // collision is the first body. Quantities like the force and
-            // the normal must be flipped in the second case.
-            entityContactMap[coll1It->second][coll2It->second].push_back(
-                allContactData);
-            entityContactMap[coll2It->second][coll1It->second].push_back(
-                allContactData);
-        }
-    }
-
-    // Go through each collision entity that has a ContactData component and
-    // set the component value to the list of contacts that correspond to
-    // the collision entity
-    _ecm.Each<components::Collision, components::ContactSensorData>(
-        [&](const Entity& _collEntity1,
-            components::Collision*,
-            components::ContactSensorData* _contacts) -> bool {
-            if (entityContactMap.find(_collEntity1) == entityContactMap.end()) {
-                // Clear the last contact data
-                *_contacts = components::ContactSensorData();
-                return true;
-            }
-
-            const auto& contactMap = entityContactMap[_collEntity1];
-
-            msgs::Contacts contactsComp;
-
-            for (const auto& [collEntity2, contactData] : contactMap) {
-
-                msgs::Contact* contactMsg = contactsComp.add_contact();
-                contactMsg->mutable_collision1()->set_id(_collEntity1);
-                contactMsg->mutable_collision2()->set_id(collEntity2);
-
-                for (const auto& contact : contactData) {
-                    auto* position = contactMsg->add_position();
-                    position->set_x(contact.point->point.x());
-                    position->set_y(contact.point->point.y());
-                    position->set_z(contact.point->point.z());
-
-                    if (contact.extra) {
-                        // Add the penetration depth
-                        contactMsg->add_depth(contact.extra->depth);
-
-                        // Get the name of the collisions
-                        auto collisionName1 =
-                            _ecm.Component<components::Name>(_collEntity1)
-                                ->Data();
-                        auto collisionName2 =
-                            _ecm.Component<components::Name>(collEntity2)
-                                ->Data();
-
-                        // Add the wrench (only the force component)
-                        auto* wrench = contactMsg->add_wrench();
-                        wrench->set_body_1_name(collisionName1);
-                        wrench->set_body_2_name(collisionName2);
-                        auto* wrench1 = wrench->mutable_body_1_wrench();
-                        auto* wrench2 = wrench->mutable_body_2_wrench();
-
-                        auto* force1 = wrench1->mutable_force();
-                        auto* force2 = wrench2->mutable_force();
-                        auto* torque1 = wrench1->mutable_torque();
-                        auto* torque2 = wrench2->mutable_torque();
-
-                        // The same ContactPoint and ExtraContactData are
-                        // used for the contact between collision1 and
-                        // collision2. In those structures there is some
-                        // data, like the force and normal, that cannot
-                        // commute.
-                        if (_collEntity1
-                            == this->collisionEntityMap.at(
-                                contact.point->collision1)) {
-                            assert(collEntity2
-                                   == this->collisionEntityMap.at(
-                                       contact.point->collision2));
-                            // Use the data as it is
-                            *force1 = msgs::Convert(
-                                math::eigen3::convert(contact.extra->force));
-                            *force2 = msgs::Convert(
-                                -math::eigen3::convert(contact.extra->force));
-                            // Add the wrench normal
-                            auto* normal = contactMsg->add_normal();
-                            normal->set_x(contact.extra->normal.x());
-                            normal->set_y(contact.extra->normal.y());
-                            normal->set_z(contact.extra->normal.z());
-                        }
-                        else {
-                            assert(collEntity2
-                                   == this->collisionEntityMap.at(
-                                       contact.point->collision1));
-                            // Flip the force
-                            *force1 = msgs::Convert(
-                                -math::eigen3::convert(contact.extra->force));
-                            *force2 = msgs::Convert(
-                                math::eigen3::convert(contact.extra->force));
-                            // Flip the normal
-                            auto* normal = contactMsg->add_normal();
-                            normal->set_x(-contact.extra->normal.x());
-                            normal->set_y(-contact.extra->normal.y());
-                            normal->set_z(-contact.extra->normal.z());
-                        }
-
-                        *torque1 = msgs::Convert(math::Vector3d::Zero);
-                        *torque2 = msgs::Convert(math::Vector3d::Zero);
-                    }
-                }
-            }
-            *_contacts = components::ContactSensorData(contactsComp);
-
-            return true;
-        });
-}
-
-physics::FrameData3d
-Physics::Impl::LinkFrameDataAtOffset(const LinkPtrType& _link,
-                                     const math::Pose3d& _pose) const
-{
-    physics::FrameData3d parent;
-    parent.pose = math::eigen3::convert(_pose);
-    physics::RelativeFrameData3d relFrameData(_link->GetFrameID(), parent);
-    return this->engine->Resolve(relFrameData, physics::FrameID::World());
-}
-
-IGNITION_ADD_PLUGIN(Physics,
-                    ignition::gazebo::System,
-                    Physics::ISystemConfigure,
-                    Physics::ISystemUpdate)
-IGNITION_ADD_PLUGIN_ALIAS(Physics, "ignition::gazebo::systems::Physics")
diff --git a/cpp/scenario/plugins/Physics/Physics.h b/cpp/scenario/plugins/Physics/Physics.h
deleted file mode 100644
index 39d8612aa..000000000
--- a/cpp/scenario/plugins/Physics/Physics.h
+++ /dev/null
@@ -1,143 +0,0 @@
-/*
- * Copyright (C) 2020 Open Source Robotics Foundation
- * All rights reserved.
- *
- * 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 SCENARIO_PLUGINS_GAZEBO_PHYSICS
-#define SCENARIO_PLUGINS_GAZEBO_PHYSICS
-
-#include <memory>
-#include <unordered_map>
-#include <utility>
-
-#include <ignition/gazebo/System.hh>
-#include <ignition/physics/FeatureList.hh>
-#include <ignition/physics/FindFeatures.hh>
-#include <ignition/physics/RequestFeatures.hh>
-
-// Features need to be defined ahead of entityCast
-#include <ignition/physics/BoxShape.hh>
-#include <ignition/physics/CylinderShape.hh>
-#include <ignition/physics/FixedJoint.hh>
-#include <ignition/physics/ForwardStep.hh>
-#include <ignition/physics/FrameSemantics.hh>
-#include <ignition/physics/FreeGroup.hh>
-#include <ignition/physics/GetBoundingBox.hh>
-#include <ignition/physics/GetContacts.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>
-
-namespace scenario::plugins::gazebo {
-    class Physics;
-    template <typename PolicyT,
-              typename ToFeatureList,
-              typename MinimumFeatureList,
-              template <typename, typename>
-              class ToEntity,
-              template <typename, typename>
-              class MinimumEntity>
-    ignition::physics::EntityPtr<ToEntity<PolicyT, ToFeatureList>> entityCast(
-        ignition::gazebo::Entity _entity,
-        const ignition::physics::EntityPtr<
-            MinimumEntity<PolicyT, MinimumFeatureList>>& _minimumEntity,
-        std::unordered_map<
-            ignition::gazebo::Entity,
-            ignition::physics::EntityPtr<ToEntity<PolicyT, ToFeatureList>>>&
-            _castMap);
-} // namespace scenario::plugins::gazebo
-
-class scenario::plugins::gazebo::Physics final
-    : public ignition::gazebo::System
-    , public ignition::gazebo::ISystemConfigure
-    , public ignition::gazebo::ISystemUpdate
-{
-public:
-    explicit Physics();
-    ~Physics() override;
-
-    // Documentation inherited
-    void Configure(const ignition::gazebo::Entity& entity,
-                   const std::shared_ptr<const sdf::Element>& sdf,
-                   ignition::gazebo::EntityComponentManager& ecm,
-                   ignition::gazebo::EventManager& eventMgr) final;
-
-    void Update(const ignition::gazebo::UpdateInfo& info,
-                ignition::gazebo::EntityComponentManager& ecm) override;
-
-private:
-    class Impl;
-    std::unique_ptr<Impl> pImpl;
-};
-
-/// \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>
-ignition::physics::EntityPtr<ToEntity<PolicyT, ToFeatureList>>
-scenario::plugins::gazebo::entityCast(
-    ignition::gazebo::Entity _entity,
-    const ignition::physics::EntityPtr<
-        MinimumEntity<PolicyT, MinimumFeatureList>>& _minimumEntity,
-    std::unordered_map<
-        ignition::gazebo::Entity,
-        ignition::physics::EntityPtr<ToEntity<PolicyT, ToFeatureList>>>&
-        _castMap)
-{
-    // Has already been cast
-    auto castIt = _castMap.find(_entity);
-    if (castIt != _castMap.end()) {
-        return castIt->second;
-    }
-
-    ignition::physics::EntityPtr<ToEntity<PolicyT, ToFeatureList>> castEntity;
-
-    // Cast
-    castEntity =
-        ignition::physics::RequestFeatures<ToFeatureList>::From(_minimumEntity);
-
-    if (castEntity) {
-        _castMap.insert(std::make_pair(_entity, castEntity));
-    }
-
-    return castEntity;
-}
-
-#endif // SCENARIO_PLUGINS_GAZEBO_PHYSICS
diff --git a/cpp/scenario/plugins/Physics/Physics.hh b/cpp/scenario/plugins/Physics/Physics.hh
new file mode 100644
index 000000000..d1c523601
--- /dev/null
+++ b/cpp/scenario/plugins/Physics/Physics.hh
@@ -0,0 +1,94 @@
+/*
+ * Copyright (C) 2018 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_HH_
+#define IGNITION_GAZEBO_SYSTEMS_PHYSICS_HH_
+
+#include <memory>
+#include <unordered_map>
+#include <utility>
+#include <ignition/physics/FindFeatures.hh>
+#include <ignition/physics/RequestFeatures.hh>
+
+// Features need to be defined ahead of entityCast
+#include <ignition/physics/BoxShape.hh>
+#include <ignition/physics/CapsuleShape.hh>
+#include <ignition/physics/CylinderShape.hh>
+#include <ignition/physics/EllipsoidShape.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/gazebo/config.hh>
+#include <ignition/gazebo/System.hh>
+
+namespace ignition
+{
+namespace gazebo
+{
+// Inline bracket to help doxygen filtering.
+inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE {
+namespace systems
+{
+  // Forward declarations.
+  class PhysicsPrivate;
+
+  /// \class Physics Physics.hh ignition/gazebo/systems/Physics.hh
+  /// \brief Base class for a System.
+  class Physics:
+    public System,
+    public ISystemConfigure,
+    public ISystemUpdate
+  {
+    /// \brief Constructor
+    public: explicit Physics();
+
+    /// \brief Destructor
+    public: ~Physics() override;
+
+    // Documentation inherited
+    public: void Configure(const Entity &_entity,
+                           const std::shared_ptr<const sdf::Element> &_sdf,
+                           EntityComponentManager &_ecm,
+                           EventManager &_eventMgr) final;
+
+    /// Documentation inherited
+    public: void Update(const UpdateInfo &_info,
+                EntityComponentManager &_ecm) final;
+
+    /// \brief Private data pointer.
+    private: std::unique_ptr<PhysicsPrivate> dataPtr;
+  };
+}
+}
+}
+}
+#endif

From 21c92337a9285e463c5d309c9123f6159930829f Mon Sep 17 00:00:00 2001
From: Diego Ferigo <diego.ferigo@iit.it>
Date: Tue, 27 Apr 2021 19:36:42 +0200
Subject: [PATCH 2/6] Custom Physics features

---
 cpp/scenario/plugins/Physics/Physics.cc | 386 +++++++++++++++++++++---
 cpp/scenario/plugins/Physics/Physics.hh |   3 +-
 2 files changed, 351 insertions(+), 38 deletions(-)

diff --git a/cpp/scenario/plugins/Physics/Physics.cc b/cpp/scenario/plugins/Physics/Physics.cc
index 5aeac72fa..e13f8b239 100644
--- a/cpp/scenario/plugins/Physics/Physics.cc
+++ b/cpp/scenario/plugins/Physics/Physics.cc
@@ -15,6 +15,7 @@
  *
  */
 
+// clang-format off
 #include "Physics.hh"
 
 #include <ignition/msgs/contact.pb.h>
@@ -121,6 +122,13 @@
 
 #include "EntityFeatureMap.hh"
 
+// Extra components
+#include "scenario/gazebo/components/ExternalWorldWrenchCmdWithDuration.h"
+#include "scenario/gazebo/components/HistoryOfAppliedJointForces.h"
+#include "scenario/gazebo/components/JointAcceleration.h"
+#include <ignition/gazebo/components/JointForce.hh>
+#include "scenario/gazebo/components/SimulatedTime.h"
+
 using namespace ignition;
 using namespace ignition::gazebo;
 using namespace ignition::gazebo::systems;
@@ -177,7 +185,8 @@ class ignition::gazebo::systems::PhysicsPrivate
 
   /// \brief Update physics from components
   /// \param[in] _ecm Mutable reference to ECM.
-  public: void UpdatePhysics(EntityComponentManager &_ecm);
+  public: void UpdatePhysics(EntityComponentManager &_ecm,
+                             const ignition::gazebo::UpdateInfo& _info);
 
   /// \brief Step the simulation for each world
   /// \param[in] _dt Duration
@@ -204,7 +213,8 @@ class ignition::gazebo::systems::PhysicsPrivate
   /// value is the updated frame data corresponding to that entity.
   public: void UpdateSim(EntityComponentManager &_ecm,
               const std::unordered_map<
-                Entity, physics::FrameData3d> &_linkFrameData);
+                Entity, physics::FrameData3d> &_linkFrameData,
+              const ignition::gazebo::UpdateInfo &_info);
 
   /// \brief Update collision components from physics simulation
   /// \param[in] _ecm Mutable reference to ECM.
@@ -460,6 +470,10 @@ class ignition::gazebo::systems::PhysicsPrivate
   /// \brief A map between collision entity ids in the ECM to FreeGroup Entities
   /// in ign-physics.
   public: EntityFreeGroupMap entityFreeGroupMap;
+
+  /// \brief Boolean value that is true only the first call of Configure and
+  /// PreUpdate.
+  bool firstRun = true;
 };
 
 //////////////////////////////////////////////////
@@ -596,10 +610,22 @@ void Physics::Update(const UpdateInfo &_info, EntityComponentManager &_ecm)
         << "s]. System may not work properly." << std::endl;
   }
 
+  // Update the component with the time in seconds that the simulation
+  // will have after the step
+  _ecm.Each<components::World, components::SimulatedTime>(
+              [&](const Entity& worldEntity,
+              const components::World*,
+              components::SimulatedTime*) {
+
+      scenario::gazebo::utils::setExistingComponentData<
+              components::SimulatedTime>(&_ecm, worldEntity, _info.simTime);
+      return true;
+  });
+
   if (this->dataPtr->engine)
   {
     this->dataPtr->CreatePhysicsEntities(_ecm);
-    this->dataPtr->UpdatePhysics(_ecm);
+    this->dataPtr->UpdatePhysics(_ecm, _info);
     ignition::physics::ForwardStep::Output stepOutput;
     // Only step if not paused.
     if (!_info.paused)
@@ -607,7 +633,7 @@ void Physics::Update(const UpdateInfo &_info, EntityComponentManager &_ecm)
       stepOutput = this->dataPtr->Step(_info.dt);
     }
     auto changedLinks = this->dataPtr->ChangedLinks(_ecm, stepOutput);
-    this->dataPtr->UpdateSim(_ecm, changedLinks);
+    this->dataPtr->UpdateSim(_ecm, changedLinks, _info);
 
     // Entities scheduled to be removed should be removed from physics after the
     // simulation step. Otherwise, since the to-be-removed entity still shows up
@@ -620,7 +646,7 @@ void Physics::Update(const UpdateInfo &_info, EntityComponentManager &_ecm)
 void PhysicsPrivate::CreatePhysicsEntities(const EntityComponentManager &_ecm)
 {
   // Get all the new worlds
-  _ecm.EachNew<components::World, components::Name, components::Gravity>(
+  auto processWorld =
       [&](const Entity &_entity,
         const components::World * /* _world */,
         const components::Name *_name,
@@ -642,10 +668,9 @@ void PhysicsPrivate::CreatePhysicsEntities(const EntityComponentManager &_ecm)
         this->entityWorldMap.AddEntity(_entity, worldPtrPhys);
 
         return true;
-      });
+      };
 
-  _ecm.EachNew<components::Model, components::Name, components::Pose,
-            components::ParentEntity>(
+  auto processModel =
       [&](const Entity &_entity,
           const components::Model *,
           const components::Name *_name,
@@ -770,10 +795,9 @@ void PhysicsPrivate::CreatePhysicsEntities(const EntityComponentManager &_ecm)
         }
 
         return true;
-      });
+      };
 
-  _ecm.EachNew<components::Link, components::Name, components::Pose,
-            components::ParentEntity>(
+  auto processLink =
       [&](const Entity &_entity,
         const components::Link * /* _link */,
         const components::Name *_name,
@@ -824,14 +848,12 @@ void PhysicsPrivate::CreatePhysicsEntities(const EntityComponentManager &_ecm)
             topLevelModel(_entity, _ecm)));
 
         return true;
-      });
+      };
 
   // We don't need to add visuals to the physics engine.
 
   // collisions
-  _ecm.EachNew<components::Collision, components::Name, components::Pose,
-            components::Geometry, components::CollisionElement,
-            components::ParentEntity>(
+  auto processCollision =
       [&](const Entity &_entity,
           const components::Collision *,
           const components::Name *_name,
@@ -956,13 +978,10 @@ void PhysicsPrivate::CreatePhysicsEntities(const EntityComponentManager &_ecm)
         this->topLevelModelMap.insert(std::make_pair(_entity,
             topLevelModel(_entity, _ecm)));
         return true;
-      });
+      };
 
   // joints
-  _ecm.EachNew<components::Joint, components::Name, components::JointType,
-               components::Pose, components::ThreadPitch,
-               components::ParentEntity, components::ParentLinkName,
-               components::ChildLinkName>(
+  auto processJoint =
       [&](const Entity &_entity,
           const components::Joint * /* _joint */,
           const components::Name *_name,
@@ -1041,19 +1060,19 @@ void PhysicsPrivate::CreatePhysicsEntities(const EntityComponentManager &_ecm)
               topLevelModel(_entity, _ecm)));
         }
         return true;
-      });
+      };
 
-  _ecm.EachNew<components::BatterySoC>(
+  auto processBatterySoC =
       [&](const Entity & _entity, const components::BatterySoC *)->bool
       {
         // Parent entity of battery is model entity
         this->entityOffMap.insert(std::make_pair(
           _ecm.ParentEntity(_entity), false));
         return true;
-      });
+      };
 
   // Detachable joints
-  _ecm.EachNew<components::DetachableJoint>(
+  auto processDetachableJoint =
       [&](const Entity &_entity,
           const components::DetachableJoint *_jointInfo) -> bool
       {
@@ -1138,7 +1157,80 @@ void PhysicsPrivate::CreatePhysicsEntities(const EntityComponentManager &_ecm)
           ignwarn << "DetachableJoint could not be created." << std::endl;
         }
         return true;
-      });
+      };
+
+  if (this->firstRun)
+  {
+    this->firstRun = false;
+
+    _ecm.Each<components::World, components::Name, components::Gravity>(
+                processWorld);
+
+    _ecm.Each<components::Model,
+            components::Name,
+            components::Pose,
+            components::ParentEntity>(processModel);
+
+    _ecm.Each<components::Link,
+            components::Name,
+            components::Pose,
+            components::ParentEntity>(processLink);
+
+    _ecm.Each<components::Collision,
+            components::Name,
+            components::Pose,
+            components::Geometry,
+            components::CollisionElement,
+            components::ParentEntity>(processCollision);
+
+    _ecm.Each<components::Joint,
+            components::Name,
+            components::JointType,
+            components::Pose,
+            components::ThreadPitch,
+            components::ParentEntity,
+            components::ParentLinkName,
+            components::ChildLinkName>(processJoint);
+
+    _ecm.Each<components::BatterySoC>(processBatterySoC);
+
+    _ecm.Each<components::DetachableJoint>(processDetachableJoint);
+  }
+  else
+  {
+    _ecm.EachNew<components::World, components::Name, components::Gravity>(
+                processWorld);
+
+    _ecm.EachNew<components::Model,
+            components::Name,
+            components::Pose,
+            components::ParentEntity>(processModel);
+
+    _ecm.EachNew<components::Link,
+            components::Name,
+            components::Pose,
+            components::ParentEntity>(processLink);
+
+    _ecm.EachNew<components::Collision,
+            components::Name,
+            components::Pose,
+            components::Geometry,
+            components::CollisionElement,
+            components::ParentEntity>(processCollision);
+
+    _ecm.EachNew<components::Joint,
+            components::Name,
+            components::JointType,
+            components::Pose,
+            components::ThreadPitch,
+            components::ParentEntity,
+            components::ParentLinkName,
+            components::ChildLinkName>(processJoint);
+
+    _ecm.EachNew<components::BatterySoC>(processBatterySoC);
+
+    _ecm.EachNew<components::DetachableJoint>(processDetachableJoint);
+  }
 }
 
 //////////////////////////////////////////////////
@@ -1228,7 +1320,8 @@ void PhysicsPrivate::RemovePhysicsEntities(const EntityComponentManager &_ecm)
 }
 
 //////////////////////////////////////////////////
-void PhysicsPrivate::UpdatePhysics(EntityComponentManager &_ecm)
+void PhysicsPrivate::UpdatePhysics(EntityComponentManager &_ecm,
+                                   const ignition::gazebo::UpdateInfo &_info)
 {
   IGN_PROFILE("PhysicsPrivate::UpdatePhysics");
   // Battery state
@@ -1421,6 +1514,54 @@ void PhysicsPrivate::UpdatePhysics(EntityComponentManager &_ecm)
         return true;
       });
 
+  // Link wrenches with duration
+  if (!_info.paused)
+  {
+    _ecm.Each<components::ExternalWorldWrenchCmdWithDuration>(
+        [&](const Entity& _entity,
+            components::ExternalWorldWrenchCmdWithDuration*
+                _wrenchWithDurComp)
+    {
+      if (!this->entityLinkMap.HasEntity(_entity))
+      {
+        ignwarn << "Failed to find link [" << _entity << "]."
+                << std::endl;
+        return true;
+      }
+
+      auto linkForceFeature =
+           this->entityLinkMap.EntityCast<LinkForceFeatureList>(_entity);
+      if (!linkForceFeature) {
+          static bool informed{false};
+          if (!informed)
+          {
+            igndbg << "Attempting to apply a wrench, but the physics "
+                   << "engine doesn't support feature "
+                   << "[AddLinkExternalForceTorque]. Wrench will be ignored."
+                   << std::endl;
+            informed = true;
+          }
+
+          // Break Each call since no ExternalWorldWrenchCmd's can be processed
+          return false;
+      }
+
+      const auto& totalWrench =  _wrenchWithDurComp->Data().totalWrench();
+      const math::Vector3 force = msgs::Convert(totalWrench.force());
+      const math::Vector3 torque = msgs::Convert(totalWrench.torque());
+
+      linkForceFeature->AddExternalForce(math::eigen3::convert(force));
+      linkForceFeature->AddExternalTorque(math::eigen3::convert(torque));
+
+      // NOTE: Cleaning could be moved to UpdateSim, but let's
+      //       keep things all together for now
+      auto simTimeAfterStep = _info.simTime;
+      _wrenchWithDurComp->Data().cleanExpired(simTimeAfterStep);
+
+      return true;
+    });
+  }
+
   // Update model pose
   auto olderWorldPoseCmdsToRemove = std::move(this->worldPoseCmdsToRemove);
   this->worldPoseCmdsToRemove.clear();
@@ -1782,15 +1923,15 @@ std::unordered_map<Entity, physics::FrameData3d> PhysicsPrivate::ChangedLinks(
         // (if the link pose hasn't changed, there's no need for a pose update)
         const auto worldPoseMath3d = ignition::math::eigen3::convert(
             frameData.pose);
-        if ((this->linkWorldPoses.find(_entity) == this->linkWorldPoses.end())
-            || !this->pose3Eql(this->linkWorldPoses[_entity], worldPoseMath3d))
-        {
+        // if ((this->linkWorldPoses.find(_entity) == this->linkWorldPoses.end())
+        //     || !this->pose3Eql(this->linkWorldPoses[_entity], worldPoseMath3d))
+        // {
           // cache the updated link pose to check if the link pose has changed
           // during the next iteration
           this->linkWorldPoses[_entity] = worldPoseMath3d;
 
           linkFrameData[_entity] = frameData;
-        }
+        // }
 
         return true;
       });
@@ -1801,7 +1942,8 @@ std::unordered_map<Entity, physics::FrameData3d> PhysicsPrivate::ChangedLinks(
 
 //////////////////////////////////////////////////
 void PhysicsPrivate::UpdateSim(EntityComponentManager &_ecm,
-    const std::unordered_map<Entity, physics::FrameData3d> &_linkFrameData)
+    const std::unordered_map<Entity, physics::FrameData3d> &_linkFrameData,
+    const ignition::gazebo::UpdateInfo &_info)
 {
   IGN_PROFILE("PhysicsPrivate::UpdateSim");
 
@@ -2047,6 +2189,37 @@ void PhysicsPrivate::UpdateSim(EntityComponentManager &_ecm,
   }
   IGN_PROFILE_END();
 
+  // history of applied joint forces (commands)
+  _ecm.Each<components::Joint,
+            components::JointForceCmd,
+            components::HistoryOfAppliedJointForces>(
+       [&](const Entity& /*_entity*/,
+           components::Joint* /*_joint*/,
+           components::JointForceCmd* _forceCmd,
+           components::HistoryOfAppliedJointForces* _history) -> bool
+  {
+      // Since the operation is an append, we have to perform it only when
+      // the physics step is actually performed
+      if (_info.paused)
+      {
+          return true;
+      }
+
+      // Get the history queue
+      auto& history = _history->Data();
+
+      // Get the data from the component
+      auto jointForceCmdData = _forceCmd->Data();
+
+      // Append values to the queue
+      for (const auto& jointForceCmd : jointForceCmdData)
+      {
+        history.push(jointForceCmd);
+      }
+
+      return true;
+  });
+
   // pose/velocity/acceleration of non-link entities such as sensors /
   // collisions. These get updated only if another system has created a
   // components::WorldPose component for the entity.
@@ -2245,6 +2418,41 @@ void PhysicsPrivate::UpdateSim(EntityComponentManager &_ecm,
         }
         return true;
       });
+
+  // Update joint accelerations
+  _ecm.Each<components::Joint, components::JointAcceleration>(
+      [&](const Entity& _entity,
+          components::Joint*,
+          components::JointAcceleration* _jointAcc) -> bool
+  {
+    if (auto jointPhys = this->entityJointMap.Get(_entity))
+    {
+      _jointAcc->Data().resize(jointPhys->GetDegreesOfFreedom());
+      for (std::size_t i = 0; i < jointPhys->GetDegreesOfFreedom(); ++i)
+      {
+        _jointAcc->Data()[i] = jointPhys->GetAcceleration(i);
+      }
+    }
+    return true;
+  });
+
+  // Update joint forces
+  _ecm.Each<components::Joint, components::JointForce>(
+      [&](const Entity& _entity,
+          components::Joint*,
+          components::JointForce* _jointForce) -> bool
+  {
+    if (auto jointPhys = this->entityJointMap.Get(_entity))
+    {
+      _jointForce->Data().resize(jointPhys->GetDegreesOfFreedom());
+      for (std::size_t i = 0; i < jointPhys->GetDegreesOfFreedom(); ++i)
+      {
+          _jointForce->Data()[i] = jointPhys->GetForce(i);
+      }
+    }
+    return true;
+  });
+
   IGN_PROFILE_END();
 
   // TODO(louise) Skip this if there are no collision features
@@ -2264,6 +2472,24 @@ void PhysicsPrivate::UpdateCollisions(EntityComponentManager &_ecm)
   // capture the world Entity in a Configure call
   Entity worldEntity = _ecm.EntityByComponents(components::World());
 
+  // Check if there is at least one collision that requires contact data.
+  // The previous check does not block the scenario where all the collisions
+  // start enabled and then get disabled.
+  bool hasContactSensorData = false;
+  _ecm.Each<components::ContactSensorData, components::Collision>(
+      [&](const Entity&,
+          components::ContactSensorData*,
+          components::Collision*) -> bool
+  {
+      hasContactSensorData = true;
+      return false;
+  });
+
+  // Quit early if the ContactData component was created and then removed.
+  // This means there are no systems that need contact information.
+  if (!hasContactSensorData)
+    return;
+
   if (worldEntity == kNullEntity)
   {
     ignerr << "Missing world entity.\n";
@@ -2292,12 +2518,19 @@ void PhysicsPrivate::UpdateCollisions(EntityComponentManager &_ecm)
     return;
   }
 
+  // Struct containing pointer of contact data
+  struct AllContactData
+  {
+      const WorldShapeType::ContactPoint* point;
+      const WorldShapeType::ExtraContactData* extra;
+  };
+
   // Each contact object we get from ign-physics contains the EntityPtrs of the
   // two colliding entities and other data about the contact such as the
   // position. This map groups contacts so that it is easy to query all the
   // contacts of one entity.
   using EntityContactMap = std::unordered_map<Entity,
-      std::deque<const WorldShapeType::ContactPoint *>>;
+      std::deque<AllContactData>>;
 
   // This data structure is essentially a mapping between a pair of entities and
   // a list of pointers to their contact object. We use a map inside a map to
@@ -2310,15 +2543,32 @@ void PhysicsPrivate::UpdateCollisions(EntityComponentManager &_ecm)
   auto allContacts = worldCollisionFeature->GetContactsFromLastStep();
   for (const auto &contactComposite : allContacts)
   {
+    // Get the RequireData
     const auto &contact = contactComposite.Get<WorldShapeType::ContactPoint>();
     auto coll1Entity = this->entityCollisionMap.Get(contact.collision1);
     auto coll2Entity = this->entityCollisionMap.Get(contact.collision2);
 
+    // Check the ExpectData
+    const auto* extraContactData =
+        contactComposite.Query<WorldShapeType::ExtraContactData>();
 
     if (coll1Entity != kNullEntity && coll2Entity != kNullEntity)
     {
-      entityContactMap[coll1Entity][coll2Entity].push_back(&contact);
-      entityContactMap[coll2Entity][coll1Entity].push_back(&contact);
+      AllContactData allContactData;
+      allContactData.point = &contact;
+
+      if (extraContactData)
+      {
+        allContactData.extra = extraContactData;
+      }
+
+      // Note that the ExtraContactData is valid only when the first
+      // collision is the first body. Quantities like the force and
+      // the normal must be flipped in the second case.
+      entityContactMap[coll1Entity][coll2Entity].push_back(
+        allContactData);
+      entityContactMap[coll2Entity][coll1Entity].push_back(
+        allContactData);
     }
   }
 
@@ -2345,12 +2595,73 @@ void PhysicsPrivate::UpdateCollisions(EntityComponentManager &_ecm)
           msgs::Contact *contactMsg = contactsComp.add_contact();
           contactMsg->mutable_collision1()->set_id(_collEntity1);
           contactMsg->mutable_collision2()->set_id(collEntity2);
+
           for (const auto &contact : contactData)
           {
             auto *position = contactMsg->add_position();
-            position->set_x(contact->point.x());
-            position->set_y(contact->point.y());
-            position->set_z(contact->point.z());
+            position->set_x(contact.point->point.x());
+            position->set_y(contact.point->point.y());
+            position->set_z(contact.point->point.z());
+
+            if (contact.extra)
+            {
+              // Add the penetration depth
+              contactMsg->add_depth(contact.extra->depth);
+
+              // Get the name of the collisions
+              auto collisionName1 =
+                  _ecm.Component<components::Name>(_collEntity1)->Data();
+              auto collisionName2 =
+                  _ecm.Component<components::Name>(collEntity2)->Data();
+
+              // Add the wrench (only the force component)
+              auto* wrench = contactMsg->add_wrench();
+              wrench->set_body_1_name(collisionName1);
+              wrench->set_body_2_name(collisionName2);
+              auto* wrench1 = wrench->mutable_body_1_wrench();
+              auto* wrench2 = wrench->mutable_body_2_wrench();
+
+              auto* force1 = wrench1->mutable_force();
+              auto* force2 = wrench2->mutable_force();
+              auto* torque1 = wrench1->mutable_torque();
+              auto* torque2 = wrench2->mutable_torque();
+
+              // The same ContactPoint and ExtraContactData are
+              // used for the contact between collision1 and
+              // collision2. In those structures there is some
+              // data, like the force and normal, that cannot
+              // commute.
+              if (_collEntity1 == this->entityCollisionMap.Get(
+                          ShapePtrType(contact.point->collision1)))
+              {
+                assert(collEntity2 == this->entityCollisionMap.Get(
+                           ShapePtrType(contact.point->collision2)));
+                // Use the data as it is
+                *force1 = msgs::Convert(math::eigen3::convert(contact.extra->force));
+                *force2 = msgs::Convert(-math::eigen3::convert(contact.extra->force));
+                // Add the wrench normal
+                auto* normal = contactMsg->add_normal();
+                normal->set_x(contact.extra->normal.x());
+                normal->set_y(contact.extra->normal.y());
+                normal->set_z(contact.extra->normal.z());
+              }
+              else
+              {
+                assert(collEntity2 == this->entityCollisionMap.Get(
+                           ShapePtrType(contact.point->collision1)));
+                // Flip the force
+                *force1 = msgs::Convert(-math::eigen3::convert(contact.extra->force));
+                *force2 = msgs::Convert(math::eigen3::convert(contact.extra->force));
+                // Flip the normal
+                auto* normal = contactMsg->add_normal();
+                normal->set_x(-contact.extra->normal.x());
+                normal->set_y(-contact.extra->normal.y());
+                normal->set_z(-contact.extra->normal.z());
+              }
+
+              *torque1 = msgs::Convert(math::Vector3d::Zero);
+              *torque2 = msgs::Convert(math::Vector3d::Zero);
+            }
           }
         }
         *_contacts = components::ContactSensorData(contactsComp);
@@ -2374,3 +2685,4 @@ IGNITION_ADD_PLUGIN(Physics,
                     Physics::ISystemUpdate)
 
 IGNITION_ADD_PLUGIN_ALIAS(Physics, "ignition::gazebo::systems::Physics")
+IGNITION_ADD_PLUGIN_ALIAS(Physics, "scenario::plugins::gazebo::Physics")
diff --git a/cpp/scenario/plugins/Physics/Physics.hh b/cpp/scenario/plugins/Physics/Physics.hh
index d1c523601..4cdbd2a66 100644
--- a/cpp/scenario/plugins/Physics/Physics.hh
+++ b/cpp/scenario/plugins/Physics/Physics.hh
@@ -13,10 +13,11 @@
  * See the License for the specific language governing permissions and
  * limitations under the License.
  *
-*/
+ */
 #ifndef IGNITION_GAZEBO_SYSTEMS_PHYSICS_HH_
 #define IGNITION_GAZEBO_SYSTEMS_PHYSICS_HH_
 
+// clang-format off
 #include <memory>
 #include <unordered_map>
 #include <utility>

From f0bb297d5886acde6726f66c1cd530f5a5895d4c Mon Sep 17 00:00:00 2001
From: Diego Ferigo <diego.ferigo@iit.it>
Date: Thu, 1 Apr 2021 15:49:14 +0200
Subject: [PATCH 3/6] Disable using Ignition Dome in CI/CD

---
 .github/workflows/cicd.yml | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/.github/workflows/cicd.yml b/.github/workflows/cicd.yml
index e2ff819ec..11ec19dd4 100644
--- a/.github/workflows/cicd.yml
+++ b/.github/workflows/cicd.yml
@@ -20,9 +20,9 @@ jobs:
           - User
           - Developer
         ignition:
-          - edifice
-          - dome
           # - citadel
+          # - dome
+          - edifice
 
     env:
       CCACHE_DIR: ${{ github.workspace }}/.ccache

From 52ca07f3384e7cbb028e0e4aa86f27dfb2ae21b7 Mon Sep 17 00:00:00 2001
From: Diego Ferigo <diego.ferigo@iit.it>
Date: Mon, 5 Apr 2021 20:08:53 +0200
Subject: [PATCH 4/6] Enable Physics system profiling

---
 cpp/scenario/CMakeLists.txt                 | 3 +++
 cpp/scenario/plugins/Physics/CMakeLists.txt | 4 ++++
 2 files changed, 7 insertions(+)

diff --git a/cpp/scenario/CMakeLists.txt b/cpp/scenario/CMakeLists.txt
index 1f1cffe53..7721c9f77 100644
--- a/cpp/scenario/CMakeLists.txt
+++ b/cpp/scenario/CMakeLists.txt
@@ -9,6 +9,9 @@ set(SCENARIO_PRIVATE_DEPENDENCIES "")
 
 if(SCENARIO_USE_IGNITION)
 
+    option(ENABLE_PROFILER "Enable Ignition Profiler" OFF)
+    mark_as_advanced(ENABLE_PROFILER)
+
     add_subdirectory(gazebo)
     add_subdirectory(plugins)
     add_subdirectory(controllers)
diff --git a/cpp/scenario/plugins/Physics/CMakeLists.txt b/cpp/scenario/plugins/Physics/CMakeLists.txt
index ce817beb3..0ca0be299 100644
--- a/cpp/scenario/plugins/Physics/CMakeLists.txt
+++ b/cpp/scenario/plugins/Physics/CMakeLists.txt
@@ -41,6 +41,10 @@ target_link_libraries(PhysicsSystem
 target_include_directories(PhysicsSystem PRIVATE
     $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}>)
 
+if(ENABLE_PROFILER)
+    target_compile_definitions(PhysicsSystem PRIVATE "IGN_PROFILER_ENABLE=1")
+endif()
+
 # ===================
 # Install the targets
 # ===================

From 22437a06d2fa1721425a8c1de6b6cc6a71dee475 Mon Sep 17 00:00:00 2001
From: Diego Ferigo <diego.ferigo@iit.it>
Date: Tue, 27 Apr 2021 16:52:36 +0200
Subject: [PATCH 5/6] Use upstream components to reset the base velocity

---
 cpp/scenario/gazebo/CMakeLists.txt            |  1 -
 .../gazebo/components/WorldVelocityCmd.h      | 65 -------------
 .../gazebo/include/scenario/gazebo/helpers.h  | 22 ++---
 cpp/scenario/gazebo/src/Model.cpp             | 92 ++++++++-----------
 cpp/scenario/gazebo/src/helpers.cpp           | 39 ++++----
 5 files changed, 65 insertions(+), 154 deletions(-)
 delete mode 100644 cpp/scenario/gazebo/include/scenario/gazebo/components/WorldVelocityCmd.h

diff --git a/cpp/scenario/gazebo/CMakeLists.txt b/cpp/scenario/gazebo/CMakeLists.txt
index 06a84761a..545f6f1a7 100644
--- a/cpp/scenario/gazebo/CMakeLists.txt
+++ b/cpp/scenario/gazebo/CMakeLists.txt
@@ -34,7 +34,6 @@ set(EXTRA_COMPONENTS_PUBLIC_HEADERS
     include/scenario/gazebo/components/BaseWorldAccelerationTarget.h
     include/scenario/gazebo/components/JointControlMode.h
     include/scenario/gazebo/components/JointController.h
-    include/scenario/gazebo/components/WorldVelocityCmd.h
     include/scenario/gazebo/components/JointPositionTarget.h
     include/scenario/gazebo/components/JointVelocityTarget.h
     include/scenario/gazebo/components/JointAccelerationTarget.h
diff --git a/cpp/scenario/gazebo/include/scenario/gazebo/components/WorldVelocityCmd.h b/cpp/scenario/gazebo/include/scenario/gazebo/components/WorldVelocityCmd.h
deleted file mode 100644
index 13a8d7490..000000000
--- a/cpp/scenario/gazebo/include/scenario/gazebo/components/WorldVelocityCmd.h
+++ /dev/null
@@ -1,65 +0,0 @@
-/*
- * Copyright (C) 2020 Istituto Italiano di Tecnologia (IIT)
- * All rights reserved.
- *
- * This project is dual licensed under LGPL v2.1+ or Apache License.
- *
- * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * *
- *
- * This software may be modified and distributed under the terms of the
- * GNU Lesser General Public License v2.1 or any later version.
- *
- * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * *
- *
- * 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_COMPONENTS_WORLDVELOCITYCMD_H
-#define IGNITION_GAZEBO_COMPONENTS_WORLDVELOCITYCMD_H
-
-#include <ignition/math/Vector3.hh>
-
-#include <ignition/gazebo/Export.hh>
-#include <ignition/gazebo/config.hh>
-
-#include "ignition/gazebo/components/Component.hh"
-#include <ignition/gazebo/components/Factory.hh>
-
-namespace ignition::gazebo {
-    // Inline bracket to help doxygen filtering.
-    inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE {
-        struct WorldVelocity
-        {
-            math::Vector3d linear;
-            math::Vector3d angular;
-
-            bool operator==(const WorldVelocity& other) const
-            {
-                return this->linear == other.linear
-                       && this->angular == other.angular;
-            }
-        };
-
-        namespace components {
-            /// \brief A component type that contains commanded velocity of
-            ///        an entity in the world frame represented by
-            ///        ignition::math::Vector3d.
-            using WorldVelocityCmd =
-                Component<WorldVelocity, class WorldVelocityCmdTag>;
-            IGN_GAZEBO_REGISTER_COMPONENT(
-                "ign_gazebo_components.WorldVelocityCmdTag",
-                WorldVelocityCmd)
-        } // namespace components
-    } // namespace IGNITION_GAZEBO_VERSION_NAMESPACE
-} // namespace ignition::gazebo
-#endif // IGNITION_GAZEBO_COMPONENTS_WORLDVELOCITYCMD_H
diff --git a/cpp/scenario/gazebo/include/scenario/gazebo/helpers.h b/cpp/scenario/gazebo/include/scenario/gazebo/helpers.h
index 138d63def..6c8958001 100644
--- a/cpp/scenario/gazebo/include/scenario/gazebo/helpers.h
+++ b/cpp/scenario/gazebo/include/scenario/gazebo/helpers.h
@@ -165,17 +165,17 @@ namespace scenario::gazebo::utils {
     sdf::JointType toSdf(const scenario::core::JointType type);
     scenario::core::JointType fromSdf(const sdf::JointType sdfType);
 
-    std::pair<ignition::math::Vector3d, ignition::math::Vector3d>
-    fromModelToBaseVelocity(const ignition::math::Vector3d& linModelVelocity,
-                            const ignition::math::Vector3d& angModelVelocity,
-                            const ignition::math::Pose3d& M_H_B,
-                            const ignition::math::Quaterniond& W_R_B);
-
-    std::pair<ignition::math::Vector3d, ignition::math::Vector3d>
-    fromBaseToModelVelocity(const ignition::math::Vector3d& linBaseVelocity,
-                            const ignition::math::Vector3d& angBaseVelocity,
-                            const ignition::math::Pose3d& M_H_B,
-                            const ignition::math::Quaterniond& W_R_B);
+    ignition::math::Vector3d fromModelToBaseLinearVelocity(
+        const ignition::math::Vector3d& linModelVelocity,
+        const ignition::math::Vector3d& angModelVelocity,
+        const ignition::math::Pose3d& M_H_B,
+        const ignition::math::Quaterniond& W_R_B);
+
+    ignition::math::Vector3d fromBaseToModelLinearVelocity(
+        const ignition::math::Vector3d& linBaseVelocity,
+        const ignition::math::Vector3d& angBaseVelocity,
+        const ignition::math::Pose3d& M_H_B,
+        const ignition::math::Quaterniond& W_R_B);
 
     std::shared_ptr<World> getParentWorld(const GazeboEntity& gazeboEntity);
 
diff --git a/cpp/scenario/gazebo/src/Model.cpp b/cpp/scenario/gazebo/src/Model.cpp
index 7f1101713..6d9c7b545 100644
--- a/cpp/scenario/gazebo/src/Model.cpp
+++ b/cpp/scenario/gazebo/src/Model.cpp
@@ -34,15 +34,16 @@
 #include "scenario/gazebo/components/BaseWorldVelocityTarget.h"
 #include "scenario/gazebo/components/JointControllerPeriod.h"
 #include "scenario/gazebo/components/Timestamp.h"
-#include "scenario/gazebo/components/WorldVelocityCmd.h"
 #include "scenario/gazebo/exceptions.h"
 #include "scenario/gazebo/helpers.h"
 
 #include <ignition/common/Event.hh>
 #include <ignition/gazebo/Events.hh>
 #include <ignition/gazebo/Model.hh>
+#include <ignition/gazebo/components/AngularVelocityCmd.hh>
 #include <ignition/gazebo/components/CanonicalLink.hh>
 #include <ignition/gazebo/components/Joint.hh>
+#include <ignition/gazebo/components/LinearVelocityCmd.hh>
 #include <ignition/gazebo/components/Link.hh>
 #include <ignition/gazebo/components/Name.hh>
 #include <ignition/gazebo/components/ParentEntity.hh>
@@ -300,49 +301,13 @@ bool Model::resetBaseOrientation(const std::array<double, 4>& orientation)
 
 bool Model::resetBaseWorldLinearVelocity(const std::array<double, 3>& linear)
 {
-    // Check if the velocity was not already reset in this simulation run,
-    // otherwise the previous target would get overridden
-    if (!this->m_ecm->EntityHasComponentType(
-            this->m_entity,
-            ignition::gazebo::components::WorldVelocityCmd::typeId)) {
+    // Note: there could be a rigid transformation between the base frame and
+    //       the canonical frame. The Physics system processes velocity commands
+    //       in the canonical frame, but this method receives velocity commands
+    //       of in the base frame (all expressed in world coordinates).
+    //       Therefore, we need to compute the base linear velocity from the
+    //       base frame to the canonical frame.
 
-        return this->resetBaseWorldVelocity(linear,
-                                            this->baseWorldAngularVelocity());
-    }
-
-    // Get the existing cmd
-    const auto& velocityCmd = utils::getExistingComponentData<
-        ignition::gazebo::components::WorldVelocityCmd>(m_ecm, m_entity);
-
-    // Override only the linear velocity
-    return this->resetBaseWorldVelocity(
-        linear, utils::fromIgnitionVector(velocityCmd.angular));
-}
-
-bool Model::resetBaseWorldAngularVelocity(const std::array<double, 3>& angular)
-{
-    // Check if the velocity was not already reset in this simulation run,
-    // otherwise the previous target would get overridden
-    if (!this->m_ecm->EntityHasComponentType(
-            this->m_entity,
-            ignition::gazebo::components::WorldVelocityCmd::typeId)) {
-
-        return this->resetBaseWorldVelocity(this->baseWorldLinearVelocity(),
-                                            angular);
-    }
-
-    // Get the existing cmd
-    const auto& velocityCmd = utils::getExistingComponentData<
-        ignition::gazebo::components::WorldVelocityCmd>(m_ecm, m_entity);
-
-    // Override only the angular velocity
-    return this->resetBaseWorldVelocity(
-        utils::fromIgnitionVector(velocityCmd.linear), angular);
-}
-
-bool Model::resetBaseWorldVelocity(const std::array<double, 3>& linear,
-                                   const std::array<double, 3>& angular)
-{
     // Get the entity of the canonical (base) link
     const auto canonicalLinkEntity = m_ecm->EntityByComponents(
         ignition::gazebo::components::Link(),
@@ -359,23 +324,40 @@ bool Model::resetBaseWorldVelocity(const std::array<double, 3>& linear,
     const auto& W_R_B = utils::toIgnitionQuaternion(
         this->getLink(this->baseFrame())->orientation());
 
-    // Create the new model velocity
-    ignition::gazebo::WorldVelocity baseWorldVelocity;
-
-    // Compute the mixed velocity of the base link
-    std::tie(baseWorldVelocity.linear, baseWorldVelocity.angular) =
-        utils::fromModelToBaseVelocity(utils::toIgnitionVector3(linear),
-                                       utils::toIgnitionVector3(angular),
-                                       M_H_B,
-                                       W_R_B);
+    // Compute the linear part of the base link mixed velocity
+    const ignition::math::Vector3d baseLinearWorldVelocity =
+        utils::fromModelToBaseLinearVelocity(
+            utils::toIgnitionVector3(linear),
+            utils::toIgnitionVector3(this->baseWorldAngularVelocity()),
+            M_H_B,
+            W_R_B);
 
     // Store the new velocity
-    utils::setComponentData<ignition::gazebo::components::WorldVelocityCmd>(
-        m_ecm, m_entity, baseWorldVelocity);
+    utils::setComponentData<ignition::gazebo::components::LinearVelocityCmd>(
+        m_ecm, m_entity, baseLinearWorldVelocity);
+
+    return true;
+}
+
+bool Model::resetBaseWorldAngularVelocity(const std::array<double, 3>& angular)
+{
+    // Note: the angular part of the velocity does not change between the base
+    //       link and the canonical link (as the linear part).
+    //       In fact, the angular velocity is invariant if there's a rigid
+    //       transformation between the two frames, like in this case.
+    utils::setComponentData<ignition::gazebo::components::AngularVelocityCmd>(
+        m_ecm, m_entity, utils::toIgnitionVector3(angular));
 
     return true;
 }
 
+bool Model::resetBaseWorldVelocity(const std::array<double, 3>& linear,
+                                   const std::array<double, 3>& angular)
+{
+    return this->resetBaseWorldLinearVelocity(linear)
+           && this->resetBaseWorldAngularVelocity(angular);
+}
+
 bool Model::valid() const
 {
     return this->validEntity() && pImpl->model.Valid(*m_ecm);
@@ -1050,7 +1032,7 @@ std::array<double, 3> Model::baseWorldLinearVelocity() const
             this->getLink(this->baseFrame())->worldAngularVelocity());
 
     // Convert the base velocity to the model mixed velocity
-    auto [modelLinearVelocity, _] = utils::fromBaseToModelVelocity( //
+    const auto& modelLinearVelocity = utils::fromBaseToModelLinearVelocity( //
         canonicalLinkLinearVelocity,
         canonicalLinkAngularVelocity,
         M_H_B,
diff --git a/cpp/scenario/gazebo/src/helpers.cpp b/cpp/scenario/gazebo/src/helpers.cpp
index 5ef22dea2..22ea23b27 100644
--- a/cpp/scenario/gazebo/src/helpers.cpp
+++ b/cpp/scenario/gazebo/src/helpers.cpp
@@ -504,46 +504,41 @@ scenario::core::JointType utils::fromSdf(const sdf::JointType sdfType)
     return type;
 }
 
-std::pair<ignition::math::Vector3d, ignition::math::Vector3d>
-utils::fromModelToBaseVelocity(const ignition::math::Vector3d& linModelVelocity,
-                               const ignition::math::Vector3d& angModelVelocity,
-                               const ignition::math::Pose3d& M_H_B,
-                               const ignition::math::Quaterniond& W_R_B)
+ignition::math::Vector3d utils::fromModelToBaseLinearVelocity(
+    const ignition::math::Vector3d& linModelVelocity,
+    const ignition::math::Vector3d& angModelVelocity,
+    const ignition::math::Pose3d& M_H_B,
+    const ignition::math::Quaterniond& W_R_B)
 {
-    ignition::math::Vector3d linBaseVelocity;
-    const ignition::math::Vector3d& angBaseVelocity = angModelVelocity;
-
     // Extract the rotation and the position of the model wrt to the base
     auto B_R_M = M_H_B.Rot().Inverse();
     auto M_o_B = M_H_B.Pos();
     auto B_o_M = -B_R_M * M_o_B;
 
     // Compute the base linear velocity
-    linBaseVelocity = linModelVelocity - angModelVelocity.Cross(W_R_B * B_o_M);
+    const ignition::math::Vector3d linBaseVelocity =
+        linModelVelocity - angModelVelocity.Cross(W_R_B * B_o_M);
 
-    // Return the mixed velocity of the base
-    return {linBaseVelocity, angBaseVelocity};
+    // Return the linear part of the mixed velocity of the base
+    return linBaseVelocity;
 }
 
-std::pair<ignition::math::Vector3d, ignition::math::Vector3d>
-utils::fromBaseToModelVelocity(const ignition::math::Vector3d& linBaseVelocity,
-                               const ignition::math::Vector3d& angBaseVelocity,
-                               const ignition::math::Pose3d& M_H_B,
-                               const ignition::math::Quaterniond& W_R_B)
+ignition::math::Vector3d utils::fromBaseToModelLinearVelocity(
+    const ignition::math::Vector3d& linBaseVelocity,
+    const ignition::math::Vector3d& angBaseVelocity,
+    const ignition::math::Pose3d& M_H_B,
+    const ignition::math::Quaterniond& W_R_B)
 {
-    ignition::math::Vector3d linModelVelocity;
-    const ignition::math::Vector3d& angModelVelocity = angBaseVelocity;
-
     // Extract the rotation and the position of the model wrt to the base
     auto B_R_M = M_H_B.Rot().Inverse();
     auto M_o_B = M_H_B.Pos();
 
     // Compute the model linear velocity
-    linModelVelocity =
+    const ignition::math::Vector3d linModelVelocity =
         linBaseVelocity - angBaseVelocity.Cross(W_R_B * B_R_M * M_o_B);
 
-    // Return the mixed velocity of the model
-    return {linModelVelocity, angModelVelocity};
+    // Return the linear part of the mixed velocity of the model
+    return linModelVelocity;
 }
 
 std::shared_ptr<World> utils::getParentWorld(const GazeboEntity& gazeboEntity)

From 4cab097bba4f06dce6d3b60005142a4ea171f3ab Mon Sep 17 00:00:00 2001
From: Diego Ferigo <diego.ferigo@iit.it>
Date: Tue, 27 Apr 2021 19:37:12 +0200
Subject: [PATCH 6/6] Fix updating the GUI state with paused run

---
 cpp/scenario/gazebo/src/GazeboSimulator.cpp | 36 ++++++++++++++++++++-
 1 file changed, 35 insertions(+), 1 deletion(-)

diff --git a/cpp/scenario/gazebo/src/GazeboSimulator.cpp b/cpp/scenario/gazebo/src/GazeboSimulator.cpp
index 05049143f..4b08d9d67 100644
--- a/cpp/scenario/gazebo/src/GazeboSimulator.cpp
+++ b/cpp/scenario/gazebo/src/GazeboSimulator.cpp
@@ -39,6 +39,7 @@
 #include <ignition/gazebo/Server.hh>
 #include <ignition/gazebo/ServerConfig.hh>
 #include <ignition/gazebo/components/Name.hh>
+#include <ignition/gazebo/components/Pose.hh>
 #include <ignition/gazebo/components/World.hh>
 #include <ignition/transport/Node.hh>
 #include <ignition/transport/Publisher.hh>
@@ -233,12 +234,45 @@ bool GazeboSimulator::run(const bool paused)
         iterations = 1;
     }
 
+    // Recent versions of Ignition Gazebo optimize the streaming of pose updates
+    // in order to reduce the bandwidth between server and client.
+    // However, it does not take into account paused steps.
+    // Here below we force all the Pose components to be streamed by manually
+    // setting them as changed.
+    if (paused) {
+        // Get the singleton
+        auto& ecmSingleton =
+            scenario::plugins::gazebo::ECMSingleton::Instance();
+
+        // Process all worlds
+        for (const auto& worldName : this->worldNames()) {
+            assert(ecmSingleton.hasWorld(worldName));
+            assert(ecmSingleton.valid(worldName));
+            assert(ecmSingleton.getECM(worldName));
+
+            // Get the ECM
+            auto* ecm = ecmSingleton.getECM(worldName);
+
+            // Mark all all entities with Pose component as Changed
+            ecm->Each<ignition::gazebo::components::Pose>(
+                [&](const ignition::gazebo::Entity& entity,
+                    ignition::gazebo::components::Pose*) -> bool {
+                    ecm->SetChanged(
+                        entity,
+                        ignition::gazebo::components::Pose::typeId,
+                        ignition::gazebo::ComponentState::OneTimeChange);
+                    return true;
+                });
+        }
+    }
+
+    // Paused simulation run
     if (paused && !server->RunOnce(/*paused=*/true)) {
         sError << "The server couldn't execute the paused step" << std::endl;
         return false;
     }
 
-    // Run the simulation
+    // Unpaused simulation run
     if (!paused
         && !server->Run(/*blocking=*/deterministic,
                         /*iterations=*/iterations,