From 9472ecb157902df98046fc7af87d0434f511c857 Mon Sep 17 00:00:00 2001
From: claireyywang <22240514+claireyywang@users.noreply.github.com>
Date: Tue, 20 Oct 2020 23:00:15 -0700
Subject: [PATCH 01/10] add SetCanonicalLink()

Signed-off-by: claireyywang <22240514+claireyywang@users.noreply.github.com>
---
 tpe/lib/src/Model.cc          | 10 ++++++----
 tpe/lib/src/Model.hh          |  3 +++
 tpe/plugin/src/SDFFeatures.cc |  7 +++++++
 3 files changed, 16 insertions(+), 4 deletions(-)

diff --git a/tpe/lib/src/Model.cc b/tpe/lib/src/Model.cc
index faeb5c7e5..a9f008cd7 100644
--- a/tpe/lib/src/Model.cc
+++ b/tpe/lib/src/Model.cc
@@ -61,10 +61,6 @@ Entity &Model::AddLink()
 {
   std::size_t linkId = Entity::GetNextId();
 
-  // first link added is the canonical link
-  if (this->GetChildren().empty())
-    this->dataPtr->canonicalLinkId = linkId;
-
   const auto[it, success]  = this->GetChildren().insert(
       {linkId, std::make_shared<Link>(linkId)});
 
@@ -85,6 +81,12 @@ Entity &Model::AddModel()
   return *it->second.get();
 }
 
+//////////////////////////////////////////////////
+void Model::SetCanonicalLink(const std::size_t linkId)
+{
+  this->dataPtr->canonicalLinkId = link.GetId();
+}
+
 //////////////////////////////////////////////////
 Entity &Model::GetCanonicalLink()
 {
diff --git a/tpe/lib/src/Model.hh b/tpe/lib/src/Model.hh
index 3e7e9a132..2cac8a7bc 100644
--- a/tpe/lib/src/Model.hh
+++ b/tpe/lib/src/Model.hh
@@ -52,6 +52,9 @@ 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(const std::size_t linkId);
+
   /// \brief Get the canonical link of model
   /// \return Entity the canonical (first) link
   public: Entity &GetCanonicalLink();
diff --git a/tpe/plugin/src/SDFFeatures.cc b/tpe/plugin/src/SDFFeatures.cc
index 08890c832..b432e3382 100644
--- a/tpe/plugin/src/SDFFeatures.cc
+++ b/tpe/plugin/src/SDFFeatures.cc
@@ -112,6 +112,13 @@ Identity SDFFeatures::ConstructSdfModel(
     this->ConstructSdfLink(modelIdentity, *_sdfModel.LinkByIndex(i));
   }
 
+  // set canonical link id
+  if (_sdfModel.CanonicalLink != nullptr)
+  {
+    Entity &canonicalLink = model->GetChildByName(
+                              _sdfModel.CanonicalLinkName());
+    model->SetCanonicalLink(canonicalLink.GetId());
+  }
   return modelIdentity;
 }
 

From 56369b102f3e6b8435fc6e873fbbfda34ccc4068 Mon Sep 17 00:00:00 2001
From: claireyywang <22240514+claireyywang@users.noreply.github.com>
Date: Tue, 20 Oct 2020 23:03:06 -0700
Subject: [PATCH 02/10] remove first link as canonical

Signed-off-by: claireyywang <22240514+claireyywang@users.noreply.github.com>
---
 tpe/lib/src/Model.cc | 14 +++-----------
 1 file changed, 3 insertions(+), 11 deletions(-)

diff --git a/tpe/lib/src/Model.cc b/tpe/lib/src/Model.cc
index a9f008cd7..90f20dbf9 100644
--- a/tpe/lib/src/Model.cc
+++ b/tpe/lib/src/Model.cc
@@ -102,20 +102,12 @@ Entity &Model::GetCanonicalLink()
   std::set<Model *> models;
   for (auto &it : this->GetChildren())
   {
-    // 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
+    Model *model = dynamic_cast<Model *>(it.second.get());
+    if (model)
     {
-      Model *model = dynamic_cast<Model *>(it.second.get());
-      if (model)
-      {
-        models.insert(model);
-      }
+      models.insert(model);
     }
   }
 

From 6961f82082b49fad19714dc08173e955c216f44d Mon Sep 17 00:00:00 2001
From: claireyywang <22240514+claireyywang@users.noreply.github.com>
Date: Wed, 21 Oct 2020 14:23:28 -0700
Subject: [PATCH 03/10] add test for non nested model

Signed-off-by: claireyywang <22240514+claireyywang@users.noreply.github.com>
---
 tpe/lib/src/Model.cc          | 2 +-
 tpe/lib/src/Model_TEST.cc     | 5 +++++
 tpe/plugin/src/SDFFeatures.cc | 4 ++--
 3 files changed, 8 insertions(+), 3 deletions(-)

diff --git a/tpe/lib/src/Model.cc b/tpe/lib/src/Model.cc
index 90f20dbf9..bc928fad6 100644
--- a/tpe/lib/src/Model.cc
+++ b/tpe/lib/src/Model.cc
@@ -84,7 +84,7 @@ Entity &Model::AddModel()
 //////////////////////////////////////////////////
 void Model::SetCanonicalLink(const std::size_t linkId)
 {
-  this->dataPtr->canonicalLinkId = link.GetId();
+  this->dataPtr->canonicalLinkId = linkId;
 }
 
 //////////////////////////////////////////////////
diff --git a/tpe/lib/src/Model_TEST.cc b/tpe/lib/src/Model_TEST.cc
index 520df4942..ff2c2cf63 100644
--- a/tpe/lib/src/Model_TEST.cc
+++ b/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());
diff --git a/tpe/plugin/src/SDFFeatures.cc b/tpe/plugin/src/SDFFeatures.cc
index b432e3382..a9027c6d8 100644
--- a/tpe/plugin/src/SDFFeatures.cc
+++ b/tpe/plugin/src/SDFFeatures.cc
@@ -113,9 +113,9 @@ Identity SDFFeatures::ConstructSdfModel(
   }
 
   // set canonical link id
-  if (_sdfModel.CanonicalLink != nullptr)
+  if (_sdfModel.CanonicalLink() != nullptr)
   {
-    Entity &canonicalLink = model->GetChildByName(
+    tpelib::Entity &canonicalLink = model->GetChildByName(
                               _sdfModel.CanonicalLinkName());
     model->SetCanonicalLink(canonicalLink.GetId());
   }

From e90c4d1fd41b4e459e57372b010a94516acd3848 Mon Sep 17 00:00:00 2001
From: claireyywang <22240514+claireyywang@users.noreply.github.com>
Date: Wed, 21 Oct 2020 21:55:28 -0700
Subject: [PATCH 04/10] set first link as canonical if undefined

Signed-off-by: claireyywang <22240514+claireyywang@users.noreply.github.com>
---
 tpe/plugin/src/SDFFeatures.cc | 13 ++++++++++---
 1 file changed, 10 insertions(+), 3 deletions(-)

diff --git a/tpe/plugin/src/SDFFeatures.cc b/tpe/plugin/src/SDFFeatures.cc
index a9027c6d8..7a915fe14 100644
--- a/tpe/plugin/src/SDFFeatures.cc
+++ b/tpe/plugin/src/SDFFeatures.cc
@@ -113,12 +113,19 @@ Identity SDFFeatures::ConstructSdfModel(
   }
 
   // set canonical link id
+  std::string canonicalLinkName = "";
   if (_sdfModel.CanonicalLink() != nullptr)
   {
-    tpelib::Entity &canonicalLink = model->GetChildByName(
-                              _sdfModel.CanonicalLinkName());
-    model->SetCanonicalLink(canonicalLink.GetId());
+    canonicalLinkName = _sdfModel.CanonicalLinkName();
   }
+  else
+  {
+    // set first link as canonical link
+    canonicalLinkName = *_sdfModel.LinkByIndex(0).Name();
+  }
+  tpelib::Entity &canonicalLink = model->GetChildByName(canonicalLinkName);
+  model->SetCanonicalLink(canonicalLink.GetId());
+
   return modelIdentity;
 }
 

From e0e2b84247fd0c1c518c185dc35a414551432935 Mon Sep 17 00:00:00 2001
From: claireyywang <22240514+claireyywang@users.noreply.github.com>
Date: Wed, 21 Oct 2020 23:12:46 -0700
Subject: [PATCH 05/10] add first inserted link as canonical

Signed-off-by: claireyywang <22240514+claireyywang@users.noreply.github.com>
---
 tpe/lib/src/Model_TEST.cc     | 15 ---------------
 tpe/plugin/src/SDFFeatures.cc | 18 +++++++++++++++++-
 2 files changed, 17 insertions(+), 16 deletions(-)

diff --git a/tpe/lib/src/Model_TEST.cc b/tpe/lib/src/Model_TEST.cc
index ff2c2cf63..4578cadfe 100644
--- a/tpe/lib/src/Model_TEST.cc
+++ b/tpe/lib/src/Model_TEST.cc
@@ -312,19 +312,4 @@ 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());
 }
diff --git a/tpe/plugin/src/SDFFeatures.cc b/tpe/plugin/src/SDFFeatures.cc
index 7a915fe14..6261385fb 100644
--- a/tpe/plugin/src/SDFFeatures.cc
+++ b/tpe/plugin/src/SDFFeatures.cc
@@ -121,7 +121,8 @@ Identity SDFFeatures::ConstructSdfModel(
   else
   {
     // set first link as canonical link
-    canonicalLinkName = *_sdfModel.LinkByIndex(0).Name();
+    if (_sdfModel.LinkByIndex(0))
+      canonicalLinkName = _sdfModel.LinkByIndex(0)->Name();
   }
   tpelib::Entity &canonicalLink = model->GetChildByName(canonicalLinkName);
   model->SetCanonicalLink(canonicalLink.GetId());
@@ -185,6 +186,21 @@ Identity SDFFeatures::ConstructSdfNestedModel(
     this->ConstructSdfLink(modelIdentity, *_sdfModel.LinkByIndex(i));
   }
 
+  // set canonical link id
+  std::string canonicalLinkName = "";
+  if (_sdfModel.CanonicalLink() != nullptr)
+  {
+    canonicalLinkName = _sdfModel.CanonicalLinkName();
+  }
+  else
+  {
+    // set first link as canonical link
+    if (_sdfModel.LinkByIndex(0))
+      canonicalLinkName = _sdfModel.LinkByIndex(0)->Name();
+  }
+  tpelib::Entity &canonicalLink = model->GetChildByName(canonicalLinkName);
+  model->SetCanonicalLink(canonicalLink.GetId());
+
   // construct nested models
   for (std::size_t i = 0; i < _sdfModel.ModelCount(); ++i)
   {

From 939853893cb03a20263e879748039ab84d98b86f Mon Sep 17 00:00:00 2001
From: claireyywang <22240514+claireyywang@users.noreply.github.com>
Date: Thu, 22 Oct 2020 22:03:35 -0700
Subject: [PATCH 06/10] track firstlink, set canonical link in sdfconstruct,
 roll back freegroup no canonical assumption

Signed-off-by: claireyywang <22240514+claireyywang@users.noreply.github.com>
---
 tpe/lib/src/Model.cc                | 17 ++++++++++++++++-
 tpe/lib/src/Model.hh                |  3 ++-
 tpe/plugin/src/FreeGroupFeatures.cc |  7 +++----
 tpe/plugin/src/SDFFeatures.cc       | 24 +++++++++---------------
 4 files changed, 30 insertions(+), 21 deletions(-)

diff --git a/tpe/lib/src/Model.cc b/tpe/lib/src/Model.cc
index bc928fad6..38b8869b8 100644
--- a/tpe/lib/src/Model.cc
+++ b/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,6 +64,9 @@ Entity &Model::AddLink()
 {
   std::size_t linkId = Entity::GetNextId();
 
+  if (this->GetChildren().empty())
+    this->dataPtr->firstLinkId = linkId;
+
   const auto[it, success]  = this->GetChildren().insert(
       {linkId, std::make_shared<Link>(linkId)});
 
@@ -84,7 +90,14 @@ Entity &Model::AddModel()
 //////////////////////////////////////////////////
 void Model::SetCanonicalLink(const std::size_t linkId)
 {
-  this->dataPtr->canonicalLinkId = linkId;
+  if (linkId != kNullEntityId)
+  {
+    this->dataPtr->canonicalLinkId = linkId;
+  }
+  else if (this->dataPtr->firstLinkId != kNullEntityId)
+  {
+    this->dataPtr->canonicalLinkId = this->dataPtr->firstLinkId;
+  }
 }
 
 //////////////////////////////////////////////////
@@ -94,7 +107,9 @@ Entity &Model::GetCanonicalLink()
   // 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
diff --git a/tpe/lib/src/Model.hh b/tpe/lib/src/Model.hh
index 2cac8a7bc..f53834126 100644
--- a/tpe/lib/src/Model.hh
+++ b/tpe/lib/src/Model.hh
@@ -53,7 +53,8 @@ class IGNITION_PHYSICS_TPELIB_VISIBLE Model : public Entity
   public: Entity &AddModel();
 
   /// \brief Set the canonical link of model
-  public: void SetCanonicalLink(const std::size_t linkId);
+  public: void SetCanonicalLink(
+    const std::size_t linkId = kNullEntityId);
 
   /// \brief Get the canonical link of model
   /// \return Entity the canonical (first) link
diff --git a/tpe/plugin/src/FreeGroupFeatures.cc b/tpe/plugin/src/FreeGroupFeatures.cc
index f982afbc7..2d306c2a1 100644
--- a/tpe/plugin/src/FreeGroupFeatures.cc
+++ b/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));
 }
 
 /////////////////////////////////////////////////
diff --git a/tpe/plugin/src/SDFFeatures.cc b/tpe/plugin/src/SDFFeatures.cc
index 6261385fb..04d702d13 100644
--- a/tpe/plugin/src/SDFFeatures.cc
+++ b/tpe/plugin/src/SDFFeatures.cc
@@ -113,19 +113,16 @@ Identity SDFFeatures::ConstructSdfModel(
   }
 
   // set canonical link id
-  std::string canonicalLinkName = "";
   if (_sdfModel.CanonicalLink() != nullptr)
   {
-    canonicalLinkName = _sdfModel.CanonicalLinkName();
+    std::string canonicalLinkName = _sdfModel.CanonicalLinkName();
+    tpelib::Entity &canonicalLink = model->GetChildByName(canonicalLinkName);
+    model->SetCanonicalLink(canonicalLink.GetId());
   }
   else
   {
-    // set first link as canonical link
-    if (_sdfModel.LinkByIndex(0))
-      canonicalLinkName = _sdfModel.LinkByIndex(0)->Name();
+    model->SetCanonicalLink();
   }
-  tpelib::Entity &canonicalLink = model->GetChildByName(canonicalLinkName);
-  model->SetCanonicalLink(canonicalLink.GetId());
 
   return modelIdentity;
 }
@@ -187,20 +184,17 @@ Identity SDFFeatures::ConstructSdfNestedModel(
   }
 
   // set canonical link id
-  std::string canonicalLinkName = "";
   if (_sdfModel.CanonicalLink() != nullptr)
   {
-    canonicalLinkName = _sdfModel.CanonicalLinkName();
+    std::string canonicalLinkName = _sdfModel.CanonicalLinkName();
+    tpelib::Entity &canonicalLink = model->GetChildByName(canonicalLinkName);
+    model->SetCanonicalLink(canonicalLink.GetId());
   }
   else
   {
-    // set first link as canonical link
-    if (_sdfModel.LinkByIndex(0))
-      canonicalLinkName = _sdfModel.LinkByIndex(0)->Name();
+    model->SetCanonicalLink();
   }
-  tpelib::Entity &canonicalLink = model->GetChildByName(canonicalLinkName);
-  model->SetCanonicalLink(canonicalLink.GetId());
-
+  
   // construct nested models
   for (std::size_t i = 0; i < _sdfModel.ModelCount(); ++i)
   {

From c3fcf60aa863167bb48b74c8285621069f4da641 Mon Sep 17 00:00:00 2001
From: claireyywang <22240514+claireyywang@users.noreply.github.com>
Date: Mon, 9 Nov 2020 11:34:26 -0800
Subject: [PATCH 07/10] remove models in Model

---
 tpe/lib/src/Model.cc | 17 +----------------
 tpe/lib/src/Model.hh |  2 +-
 2 files changed, 2 insertions(+), 17 deletions(-)

diff --git a/tpe/lib/src/Model.cc b/tpe/lib/src/Model.cc
index 38b8869b8..86509e848 100644
--- a/tpe/lib/src/Model.cc
+++ b/tpe/lib/src/Model.cc
@@ -88,7 +88,7 @@ Entity &Model::AddModel()
 }
 
 //////////////////////////////////////////////////
-void Model::SetCanonicalLink(const std::size_t linkId)
+void Model::SetCanonicalLink(std::size_t linkId)
 {
   if (linkId != kNullEntityId)
   {
@@ -111,21 +111,6 @@ Entity &Model::GetCanonicalLink()
     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())
-  {
-    // if child is nested model, store it first and only return nested
-    // links if there are no links in this model
-    Model *model = dynamic_cast<Model *>(it.second.get());
-    if (model)
-    {
-      models.insert(model);
-    }
-  }
-
   for (auto m : models)
   {
     auto &link = m->GetCanonicalLink();
diff --git a/tpe/lib/src/Model.hh b/tpe/lib/src/Model.hh
index f53834126..8e910d45b 100644
--- a/tpe/lib/src/Model.hh
+++ b/tpe/lib/src/Model.hh
@@ -54,7 +54,7 @@ class IGNITION_PHYSICS_TPELIB_VISIBLE Model : public Entity
 
   /// \brief Set the canonical link of model
   public: void SetCanonicalLink(
-    const std::size_t linkId = kNullEntityId);
+    std::size_t linkId = kNullEntityId);
 
   /// \brief Get the canonical link of model
   /// \return Entity the canonical (first) link

From 58bf8d8481e2f1d3eecde8c43c2e2750060fa1cb Mon Sep 17 00:00:00 2001
From: claireyywang <22240514+claireyywang@users.noreply.github.com>
Date: Mon, 9 Nov 2020 20:20:22 -0800
Subject: [PATCH 08/10] add canonical link test

Signed-off-by: claireyywang <22240514+claireyywang@users.noreply.github.com>
---
 tpe/lib/src/Model.cc      | 27 ++++++++++++++++-----------
 tpe/lib/src/Model.hh      |  2 +-
 tpe/lib/src/Model_TEST.cc | 28 ++++++++++++++++++++++++++++
 3 files changed, 45 insertions(+), 12 deletions(-)

diff --git a/tpe/lib/src/Model.cc b/tpe/lib/src/Model.cc
index 86509e848..43d508e05 100644
--- a/tpe/lib/src/Model.cc
+++ b/tpe/lib/src/Model.cc
@@ -90,11 +90,8 @@ Entity &Model::AddModel()
 //////////////////////////////////////////////////
 void Model::SetCanonicalLink(std::size_t linkId)
 {
-  if (linkId != kNullEntityId)
-  {
-    this->dataPtr->canonicalLinkId = linkId;
-  }
-  else if (this->dataPtr->firstLinkId != kNullEntityId)
+  this->dataPtr->canonicalLinkId = linkId;
+  if (this->dataPtr->canonicalLinkId == kNullEntityId)
   {
     this->dataPtr->canonicalLinkId = this->dataPtr->firstLinkId;
   }
@@ -110,14 +107,22 @@ Entity &Model::GetCanonicalLink()
   {
     return linkEnt;
   }
-
-  for (auto m : models)
+  else
   {
-    auto &link = m->GetCanonicalLink();
-    if (link.GetId() != kNullEntity.GetId())
-      return link;
+    for (auto &child : this->GetChildren())
+    {
+      Entity childEnt = *(child.second);
+      Model *nestedModel = static_cast<Model *>(&childEnt);
+      if (nestedModel != nullptr)
+      {
+        Entity &ent = nestedModel->GetChildById(this->dataPtr->canonicalLinkId);
+        if (ent.GetId() != kNullEntityId)
+        {        
+          return ent;
+        }
+      }
+    }
   }
-
   return kNullEntity;
 }
 
diff --git a/tpe/lib/src/Model.hh b/tpe/lib/src/Model.hh
index 8e910d45b..7e8bc27fe 100644
--- a/tpe/lib/src/Model.hh
+++ b/tpe/lib/src/Model.hh
@@ -54,7 +54,7 @@ class IGNITION_PHYSICS_TPELIB_VISIBLE Model : public Entity
 
   /// \brief Set the canonical link of model
   public: void SetCanonicalLink(
-    std::size_t linkId = kNullEntityId);
+    std::size_t linkId=kNullEntityId);
 
   /// \brief Get the canonical link of model
   /// \return Entity the canonical (first) link
diff --git a/tpe/lib/src/Model_TEST.cc b/tpe/lib/src/Model_TEST.cc
index 4578cadfe..fa8ce9952 100644
--- a/tpe/lib/src/Model_TEST.cc
+++ b/tpe/lib/src/Model_TEST.cc
@@ -312,4 +312,32 @@ TEST(Model, NestedModel)
 
   Entity nullEnt = model.GetChildById(modelId);
   EXPECT_EQ(Entity::kNullEntity.GetId(), nullEnt.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());
 }

From a82bb9b3202c632ff71606ec2b34ab1757309c28 Mon Sep 17 00:00:00 2001
From: claireyywang <22240514+claireyywang@users.noreply.github.com>
Date: Mon, 9 Nov 2020 20:20:22 -0800
Subject: [PATCH 09/10] add canonical link test

Signed-off-by: claireyywang <22240514+claireyywang@users.noreply.github.com>
---
 tpe/lib/src/Model.cc      | 27 ++++++++++++++++-----------
 tpe/lib/src/Model.hh      |  2 +-
 tpe/lib/src/Model_TEST.cc | 28 ++++++++++++++++++++++++++++
 3 files changed, 45 insertions(+), 12 deletions(-)

diff --git a/tpe/lib/src/Model.cc b/tpe/lib/src/Model.cc
index 86509e848..43d508e05 100644
--- a/tpe/lib/src/Model.cc
+++ b/tpe/lib/src/Model.cc
@@ -90,11 +90,8 @@ Entity &Model::AddModel()
 //////////////////////////////////////////////////
 void Model::SetCanonicalLink(std::size_t linkId)
 {
-  if (linkId != kNullEntityId)
-  {
-    this->dataPtr->canonicalLinkId = linkId;
-  }
-  else if (this->dataPtr->firstLinkId != kNullEntityId)
+  this->dataPtr->canonicalLinkId = linkId;
+  if (this->dataPtr->canonicalLinkId == kNullEntityId)
   {
     this->dataPtr->canonicalLinkId = this->dataPtr->firstLinkId;
   }
@@ -110,14 +107,22 @@ Entity &Model::GetCanonicalLink()
   {
     return linkEnt;
   }
-
-  for (auto m : models)
+  else
   {
-    auto &link = m->GetCanonicalLink();
-    if (link.GetId() != kNullEntity.GetId())
-      return link;
+    for (auto &child : this->GetChildren())
+    {
+      Entity childEnt = *(child.second);
+      Model *nestedModel = static_cast<Model *>(&childEnt);
+      if (nestedModel != nullptr)
+      {
+        Entity &ent = nestedModel->GetChildById(this->dataPtr->canonicalLinkId);
+        if (ent.GetId() != kNullEntityId)
+        {        
+          return ent;
+        }
+      }
+    }
   }
-
   return kNullEntity;
 }
 
diff --git a/tpe/lib/src/Model.hh b/tpe/lib/src/Model.hh
index 8e910d45b..7e8bc27fe 100644
--- a/tpe/lib/src/Model.hh
+++ b/tpe/lib/src/Model.hh
@@ -54,7 +54,7 @@ class IGNITION_PHYSICS_TPELIB_VISIBLE Model : public Entity
 
   /// \brief Set the canonical link of model
   public: void SetCanonicalLink(
-    std::size_t linkId = kNullEntityId);
+    std::size_t linkId=kNullEntityId);
 
   /// \brief Get the canonical link of model
   /// \return Entity the canonical (first) link
diff --git a/tpe/lib/src/Model_TEST.cc b/tpe/lib/src/Model_TEST.cc
index 4578cadfe..fa8ce9952 100644
--- a/tpe/lib/src/Model_TEST.cc
+++ b/tpe/lib/src/Model_TEST.cc
@@ -312,4 +312,32 @@ TEST(Model, NestedModel)
 
   Entity nullEnt = model.GetChildById(modelId);
   EXPECT_EQ(Entity::kNullEntity.GetId(), nullEnt.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());
 }

From 888d1589950dabe461ee462dace3b244f6ed1cd7 Mon Sep 17 00:00:00 2001
From: claireyywang <22240514+claireyywang@users.noreply.github.com>
Date: Wed, 11 Nov 2020 11:34:04 -0800
Subject: [PATCH 10/10] add sdfeatures test

Signed-off-by: claireyywang <22240514+claireyywang@users.noreply.github.com>
---
 tpe/lib/src/Model.cc               | 2 +-
 tpe/lib/src/Model.hh               | 2 +-
 tpe/plugin/src/SDFFeatures.cc      | 2 +-
 tpe/plugin/src/SDFFeatures_TEST.cc | 7 +++++++
 4 files changed, 10 insertions(+), 3 deletions(-)

diff --git a/tpe/lib/src/Model.cc b/tpe/lib/src/Model.cc
index 43d508e05..eaec716b8 100644
--- a/tpe/lib/src/Model.cc
+++ b/tpe/lib/src/Model.cc
@@ -117,7 +117,7 @@ Entity &Model::GetCanonicalLink()
       {
         Entity &ent = nestedModel->GetChildById(this->dataPtr->canonicalLinkId);
         if (ent.GetId() != kNullEntityId)
-        {        
+        {
           return ent;
         }
       }
diff --git a/tpe/lib/src/Model.hh b/tpe/lib/src/Model.hh
index 7e8bc27fe..8e910d45b 100644
--- a/tpe/lib/src/Model.hh
+++ b/tpe/lib/src/Model.hh
@@ -54,7 +54,7 @@ class IGNITION_PHYSICS_TPELIB_VISIBLE Model : public Entity
 
   /// \brief Set the canonical link of model
   public: void SetCanonicalLink(
-    std::size_t linkId=kNullEntityId);
+    std::size_t linkId = kNullEntityId);
 
   /// \brief Get the canonical link of model
   /// \return Entity the canonical (first) link
diff --git a/tpe/plugin/src/SDFFeatures.cc b/tpe/plugin/src/SDFFeatures.cc
index 04d702d13..71f23bddf 100644
--- a/tpe/plugin/src/SDFFeatures.cc
+++ b/tpe/plugin/src/SDFFeatures.cc
@@ -194,7 +194,7 @@ Identity SDFFeatures::ConstructSdfNestedModel(
   {
     model->SetCanonicalLink();
   }
-  
+
   // construct nested models
   for (std::size_t i = 0; i < _sdfModel.ModelCount(); ++i)
   {
diff --git a/tpe/plugin/src/SDFFeatures_TEST.cc b/tpe/plugin/src/SDFFeatures_TEST.cc
index bbef33f84..78cd97249 100644
--- a/tpe/plugin/src/SDFFeatures_TEST.cc
+++ b/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.