Remove hash map allocation on every update step (#1005)

Signed-off-by: Luca Della Vedova <lucadv@intrinsic.ai>

Author: luca-della-vedova

bullet-featherstone/src/Base.hh

@@ -39,6 +39,7 @@
 #include <algorithm>
 #include <limits>
 #include <memory>
+#include <optional>
 #include <string>
 #include <unordered_map>
 #include <unordered_set>
@@ -48,6 +49,7 @@
 
 #include <gz/common/Console.hh>
 #include <gz/math/eigen3/Conversions.hh>
+#include <gz/math/Pose3.hh>
 #include <gz/math/SemanticVersion.hh>
 #include <gz/physics/Implements.hh>
 
@@ -236,6 +238,8 @@ struct LinkInfo
   std::unordered_map<std::string, std::size_t> collisionNameToEntityId = {};
   // Link is either static, fixed to world, or has zero dofs
   bool isStaticOrFixed = false;
+  // Cached pose from the previous physics step for performance optimization
+  mutable std::optional<math::Pose3d> prevPose = std::nullopt;
 };
 
 struct CollisionInfo

bullet-featherstone/src/SimulationFeatures.cc

@@ -370,31 +370,21 @@ void SimulationFeatures::Write(ChangedWorldPoses &_changedPoses) const
   _changedPoses.entries.clear();
   _changedPoses.entries.reserve(this->links.size());
 
-  std::unordered_map<std::size_t, math::Pose3d> newPoses;
-
   for (const auto &[id, info] : this->links)
   {
     const auto &model = this->ReferenceInterface<ModelInfo>(info->model);
     WorldPose wp;
     wp.pose = gz::math::eigen3::convert(GetWorldTransformOfLink(*model, *info));
     wp.body = id;
 
-    auto iter = this->prevLinkPoses.find(id);
-    if ((iter == this->prevLinkPoses.end()) ||
-        !iter->second.Pos().Equal(wp.pose.Pos(), 1e-6) ||
-        !iter->second.Rot().Equal(wp.pose.Rot(), 1e-6))
+    if (!info->prevPose.has_value() ||
+        !info->prevPose->Pos().Equal(wp.pose.Pos(), 1e-6) ||
+        !info->prevPose->Rot().Equal(wp.pose.Rot(), 1e-6))
     {
       _changedPoses.entries.push_back(wp);
-      newPoses[id] = wp.pose;
+      info->prevPose = wp.pose;
     }
-    else
-      newPoses[id] = iter->second;
   }
-
-  // Save the new poses so that they can be used to check for updates in the
-  // next iteration. Re-setting this->prevLinkPoses with the contents of
-  // newPoses ensures that we aren't caching data for links that were removed
-  this->prevLinkPoses = std::move(newPoses);
 }
 }  // namespace bullet_featherstone
 }  // namespace physics

bullet-featherstone/src/SimulationFeatures.hh

@@ -57,10 +57,6 @@ class SimulationFeatures :
 
   public: std::vector<ContactInternal> GetContactsFromLastStep(
       const Identity &_worldID) const override;
-
-  /// \brief link poses from the most recent pose change/update.
-  /// The key is the link's ID, and the value is the link's pose
-  private: mutable std::unordered_map<std::size_t, math::Pose3d> prevLinkPoses;
 };
 
 }  // namespace bullet_featherstone

dartsim/src/Base.hh

@@ -37,6 +37,7 @@
 #include <gz/common/Console.hh>
 #include <gz/math/eigen3/Conversions.hh>
 #include <gz/math/Inertial.hh>
+#include <gz/math/Pose3.hh>
 #include <gz/math/SemanticVersion.hh>
 #include <gz/physics/detail/EntityStorage.hh>
 #include <gz/physics/Implements.hh>
@@ -82,6 +83,9 @@ struct LinkInfo
   /// \brief The total link inertia, which may be split between the `link` and
   /// `weldedNodes` body nodes.
   std::optional<math::Inertiald> inertial;
+  /// \brief Cached pose from the previous physics step, used for performance
+  /// optimization.
+  std::optional<math::Pose3d> prevPose;
 };
 
 struct JointInfo

dartsim/src/SimulationFeatures.cc

@@ -169,8 +169,6 @@ void SimulationFeatures::Write(ChangedWorldPoses &_changedPoses) const
   _changedPoses.entries.clear();
   _changedPoses.entries.reserve(this->links.size());
 
-  std::unordered_map<std::size_t, math::Pose3d> newPoses;
-
   for (const auto &[id, info] : this->links.idToObject)
   {
     // make sure the link exists
@@ -183,23 +181,15 @@ void SimulationFeatures::Write(ChangedWorldPoses &_changedPoses) const
 
       // If the link's pose is new or has changed, save this new pose and
       // add it to the output poses. Otherwise, keep the existing link pose
-      auto iter = this->prevLinkPoses.find(id);
-      if ((iter == this->prevLinkPoses.end()) ||
-          !iter->second.Pos().Equal(wp.pose.Pos(), 1e-6) ||
-          !iter->second.Rot().Equal(wp.pose.Rot(), 1e-6))
+      if (!info->prevPose.has_value() ||
+          !info->prevPose->Pos().Equal(wp.pose.Pos(), 1e-6) ||
+          !info->prevPose->Rot().Equal(wp.pose.Rot(), 1e-6))
       {
         _changedPoses.entries.push_back(wp);
-        newPoses[id] = wp.pose;
+        info->prevPose = wp.pose;
       }
-      else
-        newPoses[id] = iter->second;
     }
   }
-
-  // Save the new poses so that they can be used to check for updates in the
-  // next iteration. Re-setting this->prevLinkPoses with the contents of
-  // newPoses ensures that we aren't caching data for links that were removed
-  this->prevLinkPoses = std::move(newPoses);
 }
 
 SimulationFeatures::RayIntersectionInternal

dartsim/src/SimulationFeatures.hh

@@ -142,10 +142,6 @@ class SimulationFeatures :
       const std::vector<BatchRayQuery> &_rays,
       BatchedRayIntersectionData &_output) const override;
 
-  /// \brief link poses from the most recent pose change/update.
-  /// The key is the link's ID, and the value is the link's pose
-  private: mutable std::unordered_map<std::size_t, math::Pose3d> prevLinkPoses;
-
   private: std::optional<ContactInternal> convertContact(
     const dart::collision::Contact& _contact) const;