Support setting canonical link

tpe/lib/src/Model.cc

@@ -31,6 +31,9 @@ class ignition::physics::tpelib::ModelPrivate
 {
   /// \brief Canonical link id;
   public: std::size_t canonicalLinkId = kNullEntityId;
+
+  /// \brief First inserted link id;
+  public: std::size_t firstLinkId = kNullEntityId;
 };
 
 using namespace ignition;
@@ -61,9 +64,8 @@ Entity &Model::AddLink()
 {
   std::size_t linkId = Entity::GetNextId();
 
-  // first link added is the canonical link
   if (this->GetChildren().empty())
-    this->dataPtr->canonicalLinkId = linkId;
+    this->dataPtr->firstLinkId = linkId;
 
   const auto[it, success]  = this->GetChildren().insert(
       {linkId, std::make_shared<Link>(linkId)});
@@ -85,45 +87,42 @@ Entity &Model::AddModel()
   return *it->second.get();
 }
 
+//////////////////////////////////////////////////
+void Model::SetCanonicalLink(std::size_t linkId)
+{
+  this->dataPtr->canonicalLinkId = linkId;
+  if (this->dataPtr->canonicalLinkId == kNullEntityId)
+  {
+    this->dataPtr->canonicalLinkId = this->dataPtr->firstLinkId;
+  }
+}
+
 //////////////////////////////////////////////////
 Entity &Model::GetCanonicalLink()
 {
   // return canonical link but make sure it exists
   // todo(anyone) Prevent removal of canonical link in a model?
   Entity &linkEnt = this->GetChildById(this->dataPtr->canonicalLinkId);
   if (linkEnt.GetId() != kNullEntityId)
+  {
     return linkEnt;
-
-  // todo(anyone) the code below does not guarantee that the link returned is
-  // the first link defined in SDF since GetChildren returns a std::map, and
-  // likewise the use of std::set, do not preserve order.
-  std::set<Model *> models;
-  for (auto &it : this->GetChildren())
+  }
+  else
   {
-    // return the first link found as canonical link
-    if (dynamic_cast<Link *>(it.second.get()))
-    {
-      return *it.second;
-    }
-    // if child is nested model, store it first and only return nested
-    // links if there are no links in this model
-    else
+    for (auto &child : this->GetChildren())
     {
-      Model *model = dynamic_cast<Model *>(it.second.get());
-      if (model)
+      Entity childEnt = *(child.second);
+      Model *nestedModel = static_cast<Model *>(&childEnt);
+      if (nestedModel != nullptr)
       {
-        models.insert(model);
+        Entity &ent = nestedModel->GetChildById(this->dataPtr->canonicalLinkId);
+        if (ent.GetId() != kNullEntityId)
+        {
+          return ent;
+        }
       }
     }
   }
-
-  for (auto m : models)
-  {
-    auto &link = m->GetCanonicalLink();
-    if (link.GetId() != kNullEntity.GetId())
-      return link;
-  }
-
   return kNullEntity;
 }
 

tpe/lib/src/Model.hh

@@ -52,6 +52,10 @@ class IGNITION_PHYSICS_TPELIB_VISIBLE Model : public Entity
   /// \return Newly created nested model
   public: Entity &AddModel();
 
+  /// \brief Set the canonical link of model
+  public: void SetCanonicalLink(
+    std::size_t linkId = kNullEntityId);
+
   /// \brief Get the canonical link of model
   /// \return Entity the canonical (first) link
   public: Entity &GetCanonicalLink();

tpe/lib/src/Model_TEST.cc

@@ -107,6 +107,11 @@ TEST(Model, Link)
   EXPECT_NE(nullptr, link2);
   EXPECT_EQ(linkEnt2.GetId(), link2->GetId());
 
+  // test canonical link
+  model.SetCanonicalLink(link->GetId());
+  EXPECT_NE(Entity::kNullEntity.GetId(), model.GetCanonicalLink().GetId());
+  EXPECT_EQ(link->GetId(), model.GetCanonicalLink().GetId());
+
   // test remove child by id
   model.RemoveChildById(linkId);
   EXPECT_EQ(1u, model.GetChildCount());
@@ -308,18 +313,31 @@ TEST(Model, NestedModel)
   Entity nullEnt = model.GetChildById(modelId);
   EXPECT_EQ(Entity::kNullEntity.GetId(), nullEnt.GetId());
 
-  // add nested link and verify it is the canonical link in the model tree
-  Entity &nestedLink = nestedModel2->AddLink();
-  EXPECT_EQ(nestedLink.GetId(), model.GetCanonicalLink().GetId());
-
-  // add link in top level model and verify the new link becomes the canonical
-  // link in the tree as preference is given to links in higher level
-  Entity &link = model.AddLink();
-  EXPECT_EQ(link.GetId(), model.GetCanonicalLink().GetId());
-
-  // add a few more links in top level model and verify canonical link stays
-  // the same
-  model.AddLink();
-  model.AddLink();
-  EXPECT_EQ(link.GetId(), model.GetCanonicalLink().GetId());
+  // test canonical link within nested model
+  Model m0;
+  m0.SetName("m0");
+
+  // add nested models m1 and m2
+  Entity &nestedModelEntm1 = m0.AddModel();
+  nestedModelEntm1.SetName("m1");
+  Model *m1 = static_cast<Model *>(&nestedModelEntm1);
+  Entity &nestedModelEntm2 = m0.AddModel();
+  nestedModelEntm2.SetName("m2");
+  Model *m2 = static_cast<Model *>(&nestedModelEntm2);
+
+  // add links to nested models
+  Entity &linkEnt1 = m1->AddLink();
+  linkEnt1.SetName("x");
+  EXPECT_EQ(1u, m1->GetChildCount());
+  Entity &linkEnt2 = m2->AddLink();
+  linkEnt2.SetName("y");
+  EXPECT_EQ(1u, m2->GetChildCount());
+
+  // set link y to be the canonical link of m0
+  m0.SetCanonicalLink(linkEnt2.GetId());
+  EXPECT_EQ(linkEnt2.GetId(), m0.GetCanonicalLink().GetId());
+
+  // Set link y to be default canonical link of m2
+  m2->SetCanonicalLink();
+  EXPECT_EQ(linkEnt2.GetId(), m2->GetCanonicalLink().GetId());
 }

tpe/plugin/src/FreeGroupFeatures.cc

@@ -36,12 +36,11 @@ Identity FreeGroupFeatures::FindFreeGroupForModel(
   auto it = this->models.find(_modelID.id);
   if (it == this->models.end() || it->second == nullptr)
     return this->GenerateInvalidId();
-  auto modelPtr = it->second;
   // if there are no links in this model, then the FreeGroup functions
   // will not work properly; need to reject this case.
-  if (modelPtr->model->GetChildCount() == 0)
+  if (it->second->model->GetChildCount() == 0)
     return this->GenerateInvalidId();
-  return _modelID;
+  return this->GenerateIdentity(_modelID.id, it->second);
 }
 
 /////////////////////////////////////////////////
@@ -69,7 +68,7 @@ Identity FreeGroupFeatures::GetFreeGroupCanonicalLink(
     linkPtr->link = static_cast<tpelib::Link *>(&link);
     return this->GenerateIdentity(link.GetId(), linkPtr);
   }
-  return this->GenerateInvalidId();
+  return this->GenerateIdentity(_groupID.id, this->links.at(_groupID.id));
 }
 
 /////////////////////////////////////////////////

tpe/plugin/src/SDFFeatures.cc

@@ -112,6 +112,18 @@ Identity SDFFeatures::ConstructSdfModel(
     this->ConstructSdfLink(modelIdentity, *_sdfModel.LinkByIndex(i));
   }
 
+  // set canonical link id
+  if (_sdfModel.CanonicalLink() != nullptr)
+  {
+    std::string canonicalLinkName = _sdfModel.CanonicalLinkName();
+    tpelib::Entity &canonicalLink = model->GetChildByName(canonicalLinkName);
+    model->SetCanonicalLink(canonicalLink.GetId());
+  }
+  else
+  {
+    model->SetCanonicalLink();
+  }
+
   return modelIdentity;
 }
 
@@ -171,6 +183,18 @@ Identity SDFFeatures::ConstructSdfNestedModel(
     this->ConstructSdfLink(modelIdentity, *_sdfModel.LinkByIndex(i));
   }
 
+  // set canonical link id
+  if (_sdfModel.CanonicalLink() != nullptr)
+  {
+    std::string canonicalLinkName = _sdfModel.CanonicalLinkName();
+    tpelib::Entity &canonicalLink = model->GetChildByName(canonicalLinkName);
+    model->SetCanonicalLink(canonicalLink.GetId());
+  }
+  else
+  {
+    model->SetCanonicalLink();
+  }
+
   // construct nested models
   for (std::size_t i = 0; i < _sdfModel.ModelCount(); ++i)
   {

tpe/plugin/src/SDFFeatures_TEST.cc

@@ -37,6 +37,7 @@
 #include <ignition/physics/sdf/ConstructWorld.hh>
 
 #include "lib/src/Entity.hh"
+#include "lib/src/Model.hh"
 #include "lib/src/World.hh"
 #include "World.hh"
 
@@ -413,6 +414,12 @@ TEST(SDFFeatures_TEST, NestedModel)
       nestedCollision.GetId());
   EXPECT_EQ("nested_collision", nestedCollision.GetName());
   EXPECT_EQ(ignition::math::Pose3d::Zero, nestedCollision.GetPose());
+
+  // canonical link
+  ignition::physics::tpelib::Model *m =
+      static_cast<ignition::physics::tpelib::Model *>(&model);
+  ignition::physics::tpelib::Entity canLink = m->GetCanonicalLink();
+  EXPECT_EQ(link.GetId(), canLink.GetId());
 }
 
 // Test ConstructModel function.