[TPE] Update link pose

tpe/lib/src/Entity.hh

@@ -173,7 +173,7 @@ class IGNITION_PHYSICS_TPELIB_VISIBLE Entity
 
   /// \brief Get number of children
   /// \return Map of child id's to child entities
-  protected: std::map<std::size_t, std::shared_ptr<Entity>> &GetChildren()
+  public: std::map<std::size_t, std::shared_ptr<Entity>> &GetChildren()
       const;
 
   /// \brief Update the entity bounding box

tpe/lib/src/Link.cc

@@ -42,3 +42,41 @@ Entity &Link::AddCollision()
   this->ChildrenChanged();
   return *it->second.get();
 }
+
+//////////////////////////////////////////////////
+void Link::SetLinearVelocity(const math::Vector3d &_velocity)
+{
+  this->linearVelocity = _velocity;
+}
+
+//////////////////////////////////////////////////
+math::Vector3d Link::GetLinearVelocity() const
+{
+  return this->linearVelocity;
+}
+
+//////////////////////////////////////////////////
+void Link::SetAngularVelocity(const math::Vector3d &_velocity)
+{
+  this->angularVelocity = _velocity;
+}
+
+//////////////////////////////////////////////////
+math::Vector3d Link::GetAngularVelocity() const
+{
+  return this->angularVelocity;
+}
+
+//////////////////////////////////////////////////
+void Link::UpdatePose(double _timeStep)
+{
+  if (this->linearVelocity == math::Vector3d::Zero &&
+      this->angularVelocity == math::Vector3d::Zero)
+    return;
+
+  math::Pose3d currentPose = this->GetPose();
+  math::Pose3d nextPose(
+    currentPose.Pos() + this->linearVelocity * _timeStep,
+    currentPose.Rot().Integrate(this->angularVelocity, _timeStep));
+  this->SetPose(nextPose);
+}

tpe/lib/src/Link.hh

@@ -18,6 +18,8 @@
 #ifndef IGNITION_PHYSICS_TPE_LIB_SRC_LINK_HH_
 #define IGNITION_PHYSICS_TPE_LIB_SRC_LINK_HH_
 
+#include <ignition/utilities/SuppressWarning.hh>
+
 #include "ignition/physics/tpelib/Export.hh"
 
 #include "Entity.hh"
@@ -42,6 +44,34 @@ class IGNITION_PHYSICS_TPELIB_VISIBLE Link : public Entity
   /// \brief Add a collision
   /// \return Newly created Collision
   public: Entity &AddCollision();
+
+  /// \brief Set the linear velocity of link relative to parent
+  /// \param[in] _velocity linear velocity in meters per second
+  public: void SetLinearVelocity(const math::Vector3d &_velocity);
+
+  /// \brief Get the linear velocity of link relative to parent
+  /// \return linear velocity of link in meters per second
+  public: math::Vector3d GetLinearVelocity() const;
+
+  /// \brief Set the angular velocity of link relative to parent
+  /// \param[in] _velocity angular velocity in radians per second
+  public: void SetAngularVelocity(const math::Vector3d &_velocity);
+
+  /// \brief Get the angular velocity of link relative to parent
+  /// \return angular velocity in radians per second
+  public: math::Vector3d GetAngularVelocity() const;
+
+  /// \brief Update the pose of the entity
+  /// \param[in] _timeStep current world timestep in seconds
+  public: virtual void UpdatePose(double _timeStep);
+
+  IGN_UTILS_WARN_IGNORE__DLL_INTERFACE_MISSING
+  /// \brief linear velocity of link
+  protected: math::Vector3d linearVelocity;
+
+  /// \brief angular velocity of link
+  protected: math::Vector3d angularVelocity;
+  IGN_UTILS_WARN_RESUME__DLL_INTERFACE_MISSING
 };
 
 }

tpe/lib/src/Link_TEST.cc

@@ -35,19 +35,36 @@ TEST(Link, BasicAPI)
   link.SetName("link_1");
   EXPECT_EQ("link_1", link.GetName());
 
-  link.SetPose(math::Pose3d(1, 2, 3, 0.1, 0.2, 0.3));
-  EXPECT_EQ(math::Pose3d(1, 2, 3, 0.1, 0.2, 0.3), link.GetPose());
+  math::Pose3d link1Pose(1, 2, 3, 0.1, 0.2, 0.3);
+  link.SetPose(link1Pose);
+  EXPECT_EQ(link1Pose, link.GetPose());
 
   Model model;
   auto modelPose = math::Pose3d(10, 0, 2, 1, 0, 0);
   model.SetPose(modelPose);
   Entity &linkEnt = model.AddLink();
   ASSERT_NE(nullptr, linkEnt.GetParent());
 
-  linkEnt.SetPose(math::Pose3d(0, 0.2, 0.5, 0, 1, 0));
+  math::Pose3d linkEntPose(0, 0.2, 0.5, 0, 1, 0);
+  linkEnt.SetPose(linkEntPose);
   EXPECT_EQ(math::Pose3d(10, -0.312675, 2.43845, 1.23686, 0.471978, 0.918989),
             linkEnt.GetWorldPose());
 
+  math::Vector3d linVel(0, 0.1, 0);
+  link.SetLinearVelocity(linVel);
+  EXPECT_EQ(linVel, link.GetLinearVelocity());
+
+  math::Vector3d angVel(0.2, 0, 1);
+  link.SetAngularVelocity(angVel);
+  EXPECT_EQ(angVel, link.GetAngularVelocity());
+
+  double timeStep = 0.1;
+  math::Pose3d expectedPose(
+    link1Pose.Pos() + linVel * timeStep,
+    link1Pose.Rot().Integrate(angVel, timeStep));
+  link.UpdatePose(timeStep);
+  EXPECT_EQ(expectedPose, link.GetPose());
+
   Link link2;
   EXPECT_NE(link.GetId(), link2.GetId());
 }

tpe/lib/src/Model.cc

@@ -65,7 +65,10 @@ Entity &Model::AddLink()
   std::size_t linkId = Entity::GetNextId();
 
   if (this->GetChildren().empty())
+  {
     this->dataPtr->firstLinkId = linkId;
+    this->dataPtr->canonicalLinkId = linkId;
+  }
 
   const auto[it, success]  = this->GetChildren().insert(
       {linkId, std::make_shared<Link>(linkId)});
@@ -127,7 +130,7 @@ Entity &Model::GetCanonicalLink()
 }
 
 //////////////////////////////////////////////////
-void Model::SetLinearVelocity(const math::Vector3d _velocity)
+void Model::SetLinearVelocity(const math::Vector3d &_velocity)
 {
   this->linearVelocity = _velocity;
 }
@@ -140,7 +143,7 @@ math::Vector3d Model::GetLinearVelocity() const
 }
 
 //////////////////////////////////////////////////
-void Model::SetAngularVelocity(const math::Vector3d _velocity)
+void Model::SetAngularVelocity(const math::Vector3d &_velocity)
 {
   this->angularVelocity = _velocity;
 }
@@ -153,20 +156,17 @@ math::Vector3d Model::GetAngularVelocity() const
 }
 
 //////////////////////////////////////////////////
-void Model::UpdatePose(
-  const double _timeStep,
-  const math::Vector3d _linearVelocity,
-  const math::Vector3d _angularVelocity)
+void Model::UpdatePose(double _timeStep)
 {
   IGN_PROFILE("tpelib::Model::UpdatePose");
 
-  if (_linearVelocity == math::Vector3d::Zero &&
-      _angularVelocity == math::Vector3d::Zero)
+  if (this->linearVelocity == math::Vector3d::Zero &&
+      this->angularVelocity == math::Vector3d::Zero)
     return;
 
   math::Pose3d currentPose = this->GetPose();
   math::Pose3d nextPose(
-    currentPose.Pos() + _linearVelocity * _timeStep,
-    currentPose.Rot().Integrate(_angularVelocity, _timeStep));
+    currentPose.Pos() + this->linearVelocity * _timeStep,
+    currentPose.Rot().Integrate(this->angularVelocity, _timeStep));
   this->SetPose(nextPose);
 }

tpe/lib/src/Model.hh

@@ -60,30 +60,25 @@ class IGNITION_PHYSICS_TPELIB_VISIBLE Model : public Entity
   /// \return Entity the canonical (first) link
   public: Entity &GetCanonicalLink();
 
-  /// \brief Set the linear velocity of model
-  /// \param[in] _velocity linear velocity
-  public: void SetLinearVelocity(const math::Vector3d _velocity);
+  /// \brief Set the linear velocity of model relative to parent
+  /// \param[in] _velocity linear velocity in meters per second
+  public: void SetLinearVelocity(const math::Vector3d &_velocity);
 
-  /// \brief Get the linear velocity of model
-  /// \return linear velocity of model
+  /// \brief Get the linear velocity of model relative to parent
+  /// \return linear velocity of model in meters per second
   public: math::Vector3d GetLinearVelocity() const;
 
-  /// \brief Set the angular velocity of model
-  /// \param[in] _velocity angular velocity from world
-  public: void SetAngularVelocity(const math::Vector3d _velocity);
+  /// \brief Set the angular velocity of model relative to parent
+  /// \param[in] _velocity angular velocity from world in radians per second
+  public: void SetAngularVelocity(const math::Vector3d &_velocity);
 
-  /// \brief Get the angular velocity of model
-  /// \return angular velocity
+  /// \brief Get the angular velocity of model relative to parent
+  /// \return angular velocity in radians per second
   public: math::Vector3d GetAngularVelocity() const;
 
   /// \brief Update the pose of the entity
-  /// \param[in] _timeStep current world timestep
-  /// \param[in] _linearVelocity linear velocity
-  /// \param[in] _angularVelocity angular velocity
-  public: virtual void UpdatePose(
-    const double _timeStep,
-    const math::Vector3d _linearVelocity,
-    const math::Vector3d _angularVelocity);
+  /// \param[in] _timeStep current world timestep in seconds
+  public: virtual void UpdatePose(double _timeStep);
 
   IGN_UTILS_WARN_IGNORE__DLL_INTERFACE_MISSING
   /// \brief linear velocity of model

tpe/lib/src/Model_TEST.cc

@@ -47,7 +47,7 @@ TEST(Model, BasicAPI)
   model.SetAngularVelocity(math::Vector3d(1.0, 1.0, 1.0));
   EXPECT_EQ(math::Vector3d(1.0, 1.0, 1.0), model.GetAngularVelocity());
 
-  model.UpdatePose(0.1, model.GetLinearVelocity(), model.GetAngularVelocity());
+  model.UpdatePose(0.1);
   EXPECT_EQ(model.GetPose(), model.GetWorldPose());
 
   EXPECT_FALSE(model.GetStatic());
@@ -66,8 +66,7 @@ TEST(Model, BasicAPI)
   math::Pose3d expectedPose(
     originalPose.Pos() + math::Vector3d(0.1, 0.1, 0.1) * timeStep,
     originalPose.Rot().Integrate(math::Vector3d(1.0, 0, 0), timeStep));
-  model2.UpdatePose(
-    timeStep, model2.GetLinearVelocity(), model2.GetAngularVelocity());
+  model2.UpdatePose(timeStep);
   EXPECT_EQ(expectedPose, model2.GetPose());
 }
 

tpe/lib/src/World.cc

@@ -23,6 +23,7 @@
 #include <ignition/math/Pose3.hh>
 #include "World.hh"
 #include "Model.hh"
+#include "Link.hh"
 
 using namespace ignition;
 using namespace physics;
@@ -66,10 +67,17 @@ void World::Step()
   for (auto it = children.begin(); it != children.end(); ++it)
   {
     auto model = std::dynamic_pointer_cast<Model>(it->second);
-    model->UpdatePose(
-      this->timeStep,
-      model->GetLinearVelocity(),
-      model->GetAngularVelocity());
+    model->UpdatePose(this->timeStep);
+    auto &ents = model->GetChildren();
+    for (auto linkIt = ents.begin(); linkIt != ents.end(); ++linkIt)
+    {
+      // if child of model is link
+      auto link = std::dynamic_pointer_cast<Link>(linkIt->second);
+      if (link)
+      {
+        link->UpdatePose(this->timeStep);
+      }
+    }
   }
 
   // check colliisions

tpe/plugin/src/FreeGroupFeatures.cc

@@ -63,6 +63,10 @@ Identity FreeGroupFeatures::GetFreeGroupCanonicalLink(
   if (modelIt != this->models.end() && modelIt->second != nullptr)
   {
     // assume canonical link is the first link in model
+    // note the canonical link of a free group is renamed to root link in
+    // ign-physics4. The canonical link / root link of a free group can be
+    // different from the canonical link of a model.
+    // Here we treat them the same and return the model's canonical link
     tpelib::Entity &link = modelIt->second->model->GetCanonicalLink();
     auto linkPtr = std::make_shared<LinkInfo>();
     linkPtr->link = static_cast<tpelib::Link *>(&link);
@@ -141,24 +145,46 @@ void FreeGroupFeatures::SetFreeGroupWorldLinearVelocity(
   const Identity &_groupID,
   const LinearVelocity &_linearVelocity)
 {
-  // assume no canonical link for now
-  // assume groupID ~= modelID
   auto it = this->models.find(_groupID.id);
   // set model linear velocity
   if (it != this->models.end() && it->second != nullptr)
+  {
     it->second->model->SetLinearVelocity(
       math::eigen3::convert(_linearVelocity));
+  }
+  else
+  {
+    auto linkIt = this->links.find(_groupID.id);
+    if (linkIt != this->links.end() && linkIt->second != nullptr)
+    {
+      math::Pose3d linkWorldPose = linkIt->second->link->GetWorldPose();
+      linkIt->second->link->SetLinearVelocity(
+        linkWorldPose.Rot().Inverse() *
+        math::eigen3::convert( _linearVelocity));
+    }
+  }
 }
 
 /////////////////////////////////////////////////
 void FreeGroupFeatures::SetFreeGroupWorldAngularVelocity(
   const Identity &_groupID, const AngularVelocity &_angularVelocity)
 {
-  // assume no canonical link for now
-  // assume groupID ~= modelID
   auto it = this->models.find(_groupID.id);
   // set model angular velocity
   if (it != this->models.end() && it->second != nullptr)
+  {
     it->second->model->SetAngularVelocity(
       math::eigen3::convert(_angularVelocity));
+  }
+  else
+  {
+    auto linkIt = this->links.find(_groupID.id);
+    if (linkIt != this->links.end() && linkIt->second != nullptr)
+    {
+      math::Pose3d linkWorldPose = linkIt->second->link->GetWorldPose();
+      linkIt->second->link->SetAngularVelocity(
+        linkWorldPose.Rot().Inverse() *
+        math::eigen3::convert(_angularVelocity));
+    }
+  }
 }