Fix TPE Link velocity not being updated and Model velocity not having…

… any effect. (#289)

* Calculate link velocities, fix updates for models

Signed-off-by: Luca Della Vedova <luca@openrobotics.org>

* Add test for velocity loopback

Signed-off-by: Luca Della Vedova <luca@openrobotics.org>

* Avoid pushing duplicated links, change variable name

Signed-off-by: Luca Della Vedova <luca@openrobotics.org>

tpe/plugin/src/KinematicsFeatures.cc

@@ -56,6 +56,13 @@ FrameData3d KinematicsFeatures::FrameDataRelativeToWorld(
     {
       auto link = linkIt->second->link;
       data.pose = math::eigen3::convert(link->GetWorldPose());
+      auto modelId = link->GetParent()->GetId();
+      auto modelPtr = this->models.find(modelId)->second->model;
+      math::Pose3d parentWorldPose = modelPtr->GetWorldPose();
+      data.linearVelocity = math::eigen3::convert(parentWorldPose.Rot().Inverse() *
+          link->GetLinearVelocity() + modelPtr->GetLinearVelocity());
+      data.angularVelocity = math::eigen3::convert(parentWorldPose.Rot().Inverse() *
+          link->GetAngularVelocity() + modelPtr->GetAngularVelocity());
     }
     else
     {

tpe/plugin/src/SimulationFeatures.cc

@@ -72,36 +72,79 @@ void SimulationFeatures::Write(ChangedWorldPoses &_changedPoses) const
   _changedPoses.entries.reserve(this->links.size());
 
   std::unordered_map<std::size_t, math::Pose3d> newPoses;
+  // Store the updated links to avoid duplicated entries in _changedPoses
+  std::unordered_set<std::size_t> updatedLinkIds;
 
   for (const auto &[id, info] : this->links)
   {
     // make sure the link exists
     if (info)
     {
       const auto nextPose = info->link->GetPose();
-      auto iter = this->prevLinkPoses.find(id);
+      auto iter = this->prevEntityPoses.find(id);
 
       // 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
-      if ((iter == this->prevLinkPoses.end()) ||
+      if ((iter == this->prevEntityPoses.end()) ||
           !iter->second.Pos().Equal(nextPose.Pos(), 1e-6) ||
           !iter->second.Rot().Equal(nextPose.Rot(), 1e-6))
       {
         WorldPose wp;
         wp.pose = nextPose;
         wp.body = id;
         _changedPoses.entries.push_back(wp);
+        updatedLinkIds.insert(id);
         newPoses[id] = nextPose;
       }
       else
         newPoses[id] = iter->second;
     }
   }
 
+  // Iterate over models to make sure link velocities for moving models
+  // are calculated and sent
+  for (const auto &[id, info] : this->models)
+  {
+    // make sure the model exists
+    if (info)
+    {
+      if (info->model->GetStatic())
+        continue;
+      const auto nextPose = info->model->GetPose();
+      // Note, now prevEntityPoses also contains prevModelPoses
+      // Data structure has been kept to avoid breaking ABI
+      auto iter = this->prevEntityPoses.find(id);
+
+      // If the models's pose is new or has changed, calculate and add all
+      // the children links' poses to the output poses
+      if ((iter == this->prevEntityPoses.end()) ||
+          !iter->second.Pos().Equal(nextPose.Pos(), 1e-6) ||
+          !iter->second.Rot().Equal(nextPose.Rot(), 1e-6))
+      {
+        for (const auto &linkEnt : info->model->GetChildren())
+        {
+          // Avoid pushing if the link was already updated in the previous loop
+          auto linkId = linkEnt.second->GetId();
+          if (updatedLinkIds.find(linkId) == updatedLinkIds.end())
+          {
+            WorldPose wp;
+            wp.pose = linkEnt.second->GetPose();
+            wp.body = linkId;
+            _changedPoses.entries.push_back(wp);
+            updatedLinkIds.insert(linkId);
+          }
+          newPoses[id] = linkEnt.second->GetPose();
+        }
+      }
+      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
+  // next iteration. Re-setting this->prevEntityPoses with the contents of
   // newPoses ensures that we aren't caching data for links that were removed
-  this->prevLinkPoses = std::move(newPoses);
+  this->prevEntityPoses = std::move(newPoses);
 }
 
 std::vector<SimulationFeatures::ContactInternal>

tpe/plugin/src/SimulationFeatures.hh

@@ -61,9 +61,10 @@ class SimulationFeatures :
   /// \return Collision entity
   private: tpelib::Entity &GetModelCollision(std::size_t _id) const;
 
-  /// \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;
+  /// \brief entity poses from the most recent pose change/update.
+  /// The key is the entity's ID, and the value is the entity's pose
+  private: mutable std::unordered_map<std::size_t, math::Pose3d>
+     prevEntityPoses;
 };
 
 }

tpe/plugin/src/SimulationFeatures_TEST.cc

@@ -450,17 +450,28 @@ TEST_P(SimulationFeatures_TEST, FreeGroup)
     auto freeGroupLink = link->FindFreeGroup();
     ASSERT_NE(nullptr, freeGroupLink);
 
+    StepWorld(world, true);
+
     freeGroup->SetWorldPose(
       ignition::math::eigen3::convert(
         ignition::math::Pose3d(0, 0, 2, 0, 0, 0)));
     freeGroup->SetWorldLinearVelocity(
-      ignition::math::eigen3::convert(ignition::math::Vector3d(0.5, 0, 0.1)));
+      ignition::math::eigen3::convert(ignition::math::Vector3d(0.1, 0.2, 0.3)));
     freeGroup->SetWorldAngularVelocity(
-      ignition::math::eigen3::convert(ignition::math::Vector3d(0.1, 0.2, 0)));
+      ignition::math::eigen3::convert(ignition::math::Vector3d(0.4, 0.5, 0.6)));
 
     auto frameData = model->GetLink(0)->FrameDataRelativeToWorld();
     EXPECT_EQ(ignition::math::Pose3d(0, 0, 2, 0, 0, 0),
               ignition::math::eigen3::convert(frameData.pose));
+
+    // Step the world
+    StepWorld(world, false);
+    // Check that the first link's velocities are updated
+    frameData = model->GetLink(0)->FrameDataRelativeToWorld();
+    EXPECT_EQ(ignition::math::Vector3d(0.1, 0.2, 0.3),
+              ignition::math::eigen3::convert(frameData.linearVelocity));
+    EXPECT_EQ(ignition::math::Vector3d(0.4, 0.5, 0.6),
+              ignition::math::eigen3::convert(frameData.angularVelocity));
   }
 }