Physics system: update link poses if the canonical link pose has been updated

src/systems/physics/Physics.cc

@@ -1868,90 +1868,154 @@ void PhysicsPrivate::UpdateSim(EntityComponentManager &_ecm)
 
   // Link poses, velocities...
   IGN_PROFILE_BEGIN("Links");
-  _ecm.Each<components::Link, components::Pose, components::ParentEntity>(
-      [&](const Entity &_entity, components::Link * /*_link*/,
-          components::Pose *_pose,
+  // Process pose updates of canonical links that moved first, so that the
+  // corresponding model pose is up-to-date. This is necessary because the poses
+  // of non-canonical links are saved w.r.t. the top level model they're
+  // attached to. If a canonical link's pose changed, then all other links that
+  // belong to this model will need to have their pose updated. We keep track of
+  // which models had a canonical link update so that we update all link poses
+  // in this model later.
+  //
+  // We also keep track of each canonical link's FrameData because calling
+  // FrameDataRelativeToWorld can be expensive, so we want to make sure that we
+  // avoid calling this again on canonical links when doing the remaining link
+  // update work later.
+  std::unordered_set<Entity> modelPoseChanged;
+  std::unordered_map<Entity, physics::FrameData3d> canonicalLinkWorldFrameData;
+  _ecm.Each<components::CanonicalLink, components::ParentEntity>(
+      [&](const Entity &_entity, components::CanonicalLink * /*_link*/,
           const components::ParentEntity *_parent)->bool
       {
         // If parent is static, don't process pose changes as periodic
         if (this->staticEntities.find(_entity) != this->staticEntities.end())
           return true;
 
+        IGN_PROFILE_BEGIN("Local pose");
+
         auto linkPhys = this->entityLinkMap.Get(_entity);
         if (nullptr == linkPhys)
         {
           ignerr << "Internal error: link [" << _entity
                  << "] not in entity map" << std::endl;
           return true;
         }
-
-        IGN_PROFILE_BEGIN("Local pose");
-        // get top level model of this link
-        auto topLevelModelEnt = this->topLevelModelMap[_parent->Data()];
-
-        auto canonicalLink =
-            _ecm.Component<components::CanonicalLink>(_entity);
-
         auto frameData = linkPhys->FrameDataRelativeToWorld();
-        const auto &worldPose = frameData.pose;
+        canonicalLinkWorldFrameData[_entity] = frameData;
+        const auto worldPoseMath3d = math::eigen3::convert(frameData.pose);
 
-        // update the link or top level model pose if this is the first update,
-        // or if the link pose has changed since the last update
-        // (if the link pose hasn't changed, there's no need for a pose update)
-        const auto worldPoseMath3d = ignition::math::eigen3::convert(worldPose);
+        // update the top level model pose if this is the first update,
+        // or if the canonical link pose has changed since the last update
         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;
 
-          if (canonicalLink)
+          auto topLevelModelEnt = this->topLevelModelMap[_parent->Data()];
+          modelPoseChanged.insert(topLevelModelEnt);
+
+          // Since 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(
+              worldPoseMath3d * linkPoseFromTopLevelModel.Inverse());
+
+          _ecm.SetChanged(topLevelModelEnt, components::Pose::typeId,
+              ComponentState::PeriodicChange);
+        }
+
+        return true;
+      });
+
+  // Now that all canonical link pose updates have been processed and all model
+  // poses are up-to-date, we perform all other link updates.
+  _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
+        if (this->staticEntities.find(_entity) != this->staticEntities.end())
+          return true;
+
+        IGN_PROFILE_BEGIN("Local pose");
+
+        // if we're processing a canonical link, we can access the cached
+        // FrameData instead of calling FrameDataRelativeToWorld again (this can
+        // be an expensive call)
+        physics::FrameData3d frameData;
+        auto frameDataIter = canonicalLinkWorldFrameData.find(_entity);
+        bool isCanonicalLink =
+          frameDataIter != canonicalLinkWorldFrameData.end();
+        if (isCanonicalLink)
+        {
+          frameData = frameDataIter->second;
+        }
+        else
+        {
+          auto linkPhys = this->entityLinkMap.Get(_entity);
+          if (nullptr == linkPhys)
           {
-            // 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);
+            ignerr << "Internal error: link [" << _entity
+                   << "] not in entity map" << std::endl;
+            return true;
           }
-          else
+          frameData = linkPhys->FrameDataRelativeToWorld();
+        }
+        const auto &worldPose = frameData.pose;
+
+        auto topLevelModelEnt = this->topLevelModelMap[_parent->Data()];
+
+        // Update the link pose if this is the first update, or if the link pose
+        // has changed since the last update. If this link is a canonical link,
+        // we can skip it since we just updated all of the canonical link poses.
+        // We should also update the link pose if the link's top level model
+        // pose has been updated because non-canonical links are saved w.r.t.
+        // the top level model (if the top level model pose changes, but we
+        // don't update the pose of the model's non-canonical links, then it
+        // seems like the link(s) have changed pose, which may not be true).
+        const auto worldPoseMath3d = ignition::math::eigen3::convert(worldPose);
+        if (!isCanonicalLink &&
+            ((this->linkWorldPoses.find(_entity) == this->linkWorldPoses.end())
+            || modelPoseChanged.find(topLevelModelEnt) != modelPoseChanged.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;
+
+          // 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)
           {
-            // 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);
+            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(parentWorldPose.Inverse() *
+                                    worldPoseMath3d);
+          _ecm.SetChanged(_entity, components::Pose::typeId,
+              ComponentState::PeriodicChange);
         }
         IGN_PROFILE_END();
 
@@ -1962,7 +2026,7 @@ void PhysicsPrivate::UpdateSim(EntityComponentManager &_ecm)
         if (worldPoseComp)
         {
           auto state =
-              worldPoseComp->SetData(math::eigen3::convert(frameData.pose),
+              worldPoseComp->SetData(worldPoseMath3d,
               this->pose3Eql) ?
               ComponentState::PeriodicChange :
               ComponentState::NoChange;

test/integration/physics_system.cc

@@ -18,6 +18,7 @@
 #include <gtest/gtest.h>
 
 #include <algorithm>
+#include <string>
 #include <vector>
 
 #include <ignition/common/Console.hh>
@@ -33,6 +34,7 @@
 
 #include "ignition/gazebo/Server.hh"
 #include "ignition/gazebo/SystemLoader.hh"
+#include "ignition/gazebo/Util.hh"
 #include "ignition/gazebo/test_config.hh"  // NOLINT(build/include)
 
 #include "ignition/gazebo/components/AxisAlignedBox.hh"
@@ -987,3 +989,99 @@ TEST_F(PhysicsSystemFixture, NestedModel)
   EXPECT_NE(parents.end(), parentIt);
   EXPECT_EQ("model_01", parentIt->second);
 }
+
+/////////////////////////////////////////////////
+// This tests whether pose updates are correct for a model whose canonical link
+// changes, but other links do not
+TEST_F(PhysicsSystemFixture, MovingCanonicalLinkOnly)
+{
+  ignition::gazebo::ServerConfig serverConfig;
+
+  const auto sdfFile = common::joinPaths(PROJECT_SOURCE_PATH, "test", "worlds",
+    "only_canonical_link_moves.sdf");
+
+  sdf::Root root;
+  root.Load(sdfFile);
+  const sdf::World *world = root.WorldByIndex(0);
+  ASSERT_TRUE(nullptr != world);
+
+  serverConfig.SetSdfFile(sdfFile);
+
+  gazebo::Server server(serverConfig);
+
+  server.SetUpdatePeriod(1us);
+
+  // Create a system that records the poses of the links after physics
+  test::Relay testSystem;
+
+  size_t numBaseLinkChecks = 0;
+  size_t numOuterLinkChecks = 0;
+  size_t numBaseLinkCustomChecks = 0;
+  size_t numOuterLinkCustomChecks = 0;
+
+  size_t currIter = 0;
+  testSystem.OnPostUpdate(
+    [&world, &numBaseLinkChecks, &numOuterLinkChecks, &numBaseLinkCustomChecks,
+    &numOuterLinkCustomChecks, &currIter](
+      const gazebo::UpdateInfo &,
+      const gazebo::EntityComponentManager &_ecm)
+    {
+      currIter++;
+      _ecm.Each<components::Link, components::Name>(
+          [&](const ignition::gazebo::Entity &_entity,
+          const components::Link *, const components::Name *_name)->bool
+          {
+            // ignore the link for the ground plane
+            if (_name->Data() != "surface")
+            {
+              const double dt = 0.001;
+              const double zExpected =
+                0.5 * world->Gravity().Z() * pow(dt * currIter, 2);
+
+              if (_name->Data() == "base_link")
+              {
+                // link "base_link" falls due to gravity, starting from rest
+                EXPECT_NEAR(zExpected,
+                    ignition::gazebo::worldPose(_entity, _ecm).Z(), 1e-2);
+                numBaseLinkChecks++;
+              }
+              else if (_name->Data() == "link0_outer")
+              {
+                // link "link0_outer" is resting on the ground and does not
+                // move, so it should always have a pose of (1 0 0 0 0 0)
+                EXPECT_EQ(ignition::math::Pose3d(1, 0, 0, 0, 0, 0),
+                    ignition::gazebo::worldPose(_entity, _ecm));
+                numOuterLinkChecks++;
+              }
+              else if (_name->Data() == "base_link_custom")
+              {
+                // same as link "base_link"
+                EXPECT_NEAR(zExpected,
+                    ignition::gazebo::worldPose(_entity, _ecm).Z(), 1e-2);
+                numBaseLinkCustomChecks++;
+              }
+              else if (_name->Data() == "link0_outer_custom")
+              {
+                // same as "link0_outer", but with an offset pose
+                EXPECT_EQ(ignition::math::Pose3d(1, 2, 0, 0, 0, 0),
+                    ignition::gazebo::worldPose(_entity, _ecm));
+                numOuterLinkCustomChecks++;
+              }
+            }
+
+            return true;
+          });
+
+      return true;
+    });
+
+  server.AddSystem(testSystem.systemPtr);
+  const size_t iters = 500;
+  server.Run(true, iters, false);
+
+  EXPECT_EQ(iters, currIter);
+  EXPECT_EQ(iters, numBaseLinkChecks);
+  EXPECT_EQ(iters, numOuterLinkChecks);
+  EXPECT_EQ(iters, numBaseLinkCustomChecks);
+  EXPECT_EQ(iters, numOuterLinkCustomChecks);
+}