From 5b973687ab645b37756a4e2564239978cd215997 Mon Sep 17 00:00:00 2001
From: claireyywang <22240514+claireyywang@users.noreply.github.com>
Date: Wed, 16 Dec 2020 16:40:54 -0800
Subject: [PATCH 01/15] add link updatepose

Signed-off-by: claireyywang <22240514+claireyywang@users.noreply.github.com>
---
 tpe/lib/src/Link.cc | 17 +++++++++++++++++
 tpe/lib/src/Link.hh |  9 +++++++++
 2 files changed, 26 insertions(+)

diff --git a/tpe/lib/src/Link.cc b/tpe/lib/src/Link.cc
index d464141c1..4aae19170 100644
--- a/tpe/lib/src/Link.cc
+++ b/tpe/lib/src/Link.cc
@@ -42,3 +42,20 @@ Entity &Link::AddCollision()
   this->ChildrenChanged();
   return *it->second.get();
 }
+
+//////////////////////////////////////////////////
+void Link::UpdatePose(
+  const double _timeStep,
+  const math::Vector3d _linearVelocity,
+  const math::Vector3d _angularVelocity)
+{
+  if (_linearVelocity == math::Vector3d::Zero &&
+      _angularVelocity == math::Vector3d::Zero)
+    return;
+
+  math::Pose3d currentPose = this->GetPose();
+  math::Pose3d nextPose(
+    currentPose.Pos() + _linearVelocity * _timeStep,
+    currentPose.Rot().Integrate(_angularVelocity, _timeStep));
+  this->SetPose(nextPose);
+}
diff --git a/tpe/lib/src/Link.hh b/tpe/lib/src/Link.hh
index 6cf68f55b..4fdf46c7d 100644
--- a/tpe/lib/src/Link.hh
+++ b/tpe/lib/src/Link.hh
@@ -42,6 +42,15 @@ class IGNITION_PHYSICS_TPELIB_VISIBLE Link : public Entity
   /// \brief Add a collision
   /// \return Newly created Collision
   public: Entity &AddCollision();
+
+  /// \brief Update pose relative to parent
+  /// \param[in] _timeStep current world timestep
+  /// \param[in] _linearVelocity linear velocity
+  /// \param[in] _angularVelocity angular velocity
+  public: void UpdatePose(
+    const double _timeStep,
+    const math::Vector3d _linearVelocity,
+    const math::Vector3d _angularVelocity);
 };
 
 }

From 35aaa183fa46141585395824e517f93185e2720e Mon Sep 17 00:00:00 2001
From: claireyywang <22240514+claireyywang@users.noreply.github.com>
Date: Wed, 16 Dec 2020 22:12:19 -0800
Subject: [PATCH 02/15] move updatePose and set velocity

Signed-off-by: claireyywang <22240514+claireyywang@users.noreply.github.com>
---
 tpe/lib/src/Entity.cc | 41 ++++++++++++++++++++
 tpe/lib/src/Entity.hh | 34 +++++++++++++++++
 tpe/lib/src/Link.cc   | 30 +++++++--------
 tpe/lib/src/Link.hh   |  9 -----
 tpe/lib/src/Model.cc  | 88 +++++++++++++++++++++----------------------
 tpe/lib/src/Model.hh  | 66 ++++++++++++++++----------------
 6 files changed, 166 insertions(+), 102 deletions(-)

diff --git a/tpe/lib/src/Entity.cc b/tpe/lib/src/Entity.cc
index f3158c7d1..2f1e2dbf8 100644
--- a/tpe/lib/src/Entity.cc
+++ b/tpe/lib/src/Entity.cc
@@ -148,6 +148,23 @@ math::Pose3d Entity::GetWorldPose() const
   return this->dataPtr->pose;
 }
 
+//////////////////////////////////////////////////
+void Entity::UpdatePose(
+  const double _timeStep,
+  const math::Vector3d _linearVelocity,
+  const math::Vector3d _angularVelocity)
+{
+  if (_linearVelocity == math::Vector3d::Zero &&
+      _angularVelocity == math::Vector3d::Zero)
+    return;
+
+  math::Pose3d currentPose = this->GetPose();
+  math::Pose3d nextPose(
+    currentPose.Pos() + _linearVelocity * _timeStep,
+    currentPose.Rot().Integrate(_angularVelocity, _timeStep));
+  this->SetPose(nextPose);
+}
+
 //////////////////////////////////////////////////
 void Entity::SetId(std::size_t _id)
 {
@@ -160,6 +177,30 @@ std::size_t Entity::GetId() const
   return this->dataPtr->id;
 }
 
+//////////////////////////////////////////////////
+void Entity::SetLinearVelocity(const math::Vector3d _velocity)
+{
+  this->linearVelocity = _velocity;
+}
+
+//////////////////////////////////////////////////
+math::Vector3d Entity::GetLinearVelocity() const
+{
+  return this->linearVelocity;
+}
+
+//////////////////////////////////////////////////
+void Entity::SetAngularVelocity(const math::Vector3d _velocity)
+{
+  this->angularVelocity = _velocity;
+}
+
+//////////////////////////////////////////////////
+math::Vector3d Entity::GetAngularVelocity() const
+{
+  return this->angularVelocity;
+}
+
 //////////////////////////////////////////////////
 Entity &Entity::GetChildById(std::size_t _id) const
 {
diff --git a/tpe/lib/src/Entity.hh b/tpe/lib/src/Entity.hh
index 98352cd4f..2e7946fda 100644
--- a/tpe/lib/src/Entity.hh
+++ b/tpe/lib/src/Entity.hh
@@ -23,6 +23,7 @@
 #include <memory>
 #include <string>
 
+#include <ignition/utilities/SuppressWarning.hh>
 #include <ignition/math/AxisAlignedBox.hh>
 #include <ignition/math/Pose3.hh>
 #include "ignition/physics/tpelib/Export.hh"
@@ -90,6 +91,15 @@ class IGNITION_PHYSICS_TPELIB_VISIBLE Entity
   /// \param[in] _pose Pose of entity to set to
   public: virtual void SetPose(const math::Pose3d &_pose);
 
+  /// \brief Update 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);
+
   /// \brief Get the pose of the entity
   /// \return Pose of entity
   public: virtual math::Pose3d GetPose() const;
@@ -98,6 +108,22 @@ class IGNITION_PHYSICS_TPELIB_VISIBLE Entity
   /// \return World pose of entity
   public: virtual math::Pose3d GetWorldPose() const;
 
+  /// \brief Set the linear velocity of entity
+  /// \param[in] _velocity linear velocity
+  public: virtual void SetLinearVelocity(const math::Vector3d _velocity);
+
+  /// \brief Get the linear velocity of entity
+  /// \return linear velocity of entity
+  public: virtual math::Vector3d GetLinearVelocity() const;
+
+  /// \brief Set the angular velocity of entity
+  /// \param[in] _velocity angular velocity of entity
+  public: virtual void SetAngularVelocity(const math::Vector3d _velocity);
+
+  /// \brief Get the angular velocity of entity
+  /// \return angular velocity
+  public: virtual math::Vector3d GetAngularVelocity() const;
+
   /// \brief Get a child entity by id
   /// \param[in] _id Id of child entity
   /// \return Child entity
@@ -179,6 +205,14 @@ class IGNITION_PHYSICS_TPELIB_VISIBLE Entity
   /// \return size_t id of next entity
   protected: static std::size_t GetNextId();
 
+  IGN_UTILS_WARN_IGNORE__DLL_INTERFACE_MISSING
+  /// \brief linear velocity of entity
+  protected: math::Vector3d linearVelocity;
+
+  /// \brief angular velocity of entity
+  protected: math::Vector3d angularVelocity;
+  IGN_UTILS_WARN_RESUME__DLL_INTERFACE_MISSING
+
   /// \brief Entity id counter
   private: static std::size_t nextId;
 
diff --git a/tpe/lib/src/Link.cc b/tpe/lib/src/Link.cc
index 4aae19170..75110bbd2 100644
--- a/tpe/lib/src/Link.cc
+++ b/tpe/lib/src/Link.cc
@@ -43,19 +43,19 @@ Entity &Link::AddCollision()
   return *it->second.get();
 }
 
-//////////////////////////////////////////////////
-void Link::UpdatePose(
-  const double _timeStep,
-  const math::Vector3d _linearVelocity,
-  const math::Vector3d _angularVelocity)
-{
-  if (_linearVelocity == math::Vector3d::Zero &&
-      _angularVelocity == math::Vector3d::Zero)
-    return;
+// //////////////////////////////////////////////////
+// void Link::UpdatePose(
+//   const double _timeStep,
+//   const math::Vector3d _linearVelocity,
+//   const math::Vector3d _angularVelocity)
+// {
+//   if (_linearVelocity == math::Vector3d::Zero &&
+//       _angularVelocity == math::Vector3d::Zero)
+//     return;
 
-  math::Pose3d currentPose = this->GetPose();
-  math::Pose3d nextPose(
-    currentPose.Pos() + _linearVelocity * _timeStep,
-    currentPose.Rot().Integrate(_angularVelocity, _timeStep));
-  this->SetPose(nextPose);
-}
+//   math::Pose3d currentPose = this->GetPose();
+//   math::Pose3d nextPose(
+//     currentPose.Pos() + _linearVelocity * _timeStep,
+//     currentPose.Rot().Integrate(_angularVelocity, _timeStep));
+//   this->SetPose(nextPose);
+// }
diff --git a/tpe/lib/src/Link.hh b/tpe/lib/src/Link.hh
index 4fdf46c7d..6cf68f55b 100644
--- a/tpe/lib/src/Link.hh
+++ b/tpe/lib/src/Link.hh
@@ -42,15 +42,6 @@ class IGNITION_PHYSICS_TPELIB_VISIBLE Link : public Entity
   /// \brief Add a collision
   /// \return Newly created Collision
   public: Entity &AddCollision();
-
-  /// \brief Update pose relative to parent
-  /// \param[in] _timeStep current world timestep
-  /// \param[in] _linearVelocity linear velocity
-  /// \param[in] _angularVelocity angular velocity
-  public: void UpdatePose(
-    const double _timeStep,
-    const math::Vector3d _linearVelocity,
-    const math::Vector3d _angularVelocity);
 };
 
 }
diff --git a/tpe/lib/src/Model.cc b/tpe/lib/src/Model.cc
index eaec716b8..0d455b7a4 100644
--- a/tpe/lib/src/Model.cc
+++ b/tpe/lib/src/Model.cc
@@ -126,47 +126,47 @@ Entity &Model::GetCanonicalLink()
   return kNullEntity;
 }
 
-//////////////////////////////////////////////////
-void Model::SetLinearVelocity(const math::Vector3d _velocity)
-{
-  this->linearVelocity = _velocity;
-}
-
-//////////////////////////////////////////////////
-math::Vector3d Model::GetLinearVelocity() const
-{
-  IGN_PROFILE("tpelib::Model::GetLinearVelocity");
-  return this->linearVelocity;
-}
-
-//////////////////////////////////////////////////
-void Model::SetAngularVelocity(const math::Vector3d _velocity)
-{
-  this->angularVelocity = _velocity;
-}
-
-//////////////////////////////////////////////////
-math::Vector3d Model::GetAngularVelocity() const
-{
-  IGN_PROFILE("tpelib::Model::GetAngularVelocity");
-  return this->angularVelocity;
-}
-
-//////////////////////////////////////////////////
-void Model::UpdatePose(
-  const double _timeStep,
-  const math::Vector3d _linearVelocity,
-  const math::Vector3d _angularVelocity)
-{
-  IGN_PROFILE("tpelib::Model::UpdatePose");
-
-  if (_linearVelocity == math::Vector3d::Zero &&
-      _angularVelocity == math::Vector3d::Zero)
-    return;
-
-  math::Pose3d currentPose = this->GetPose();
-  math::Pose3d nextPose(
-    currentPose.Pos() + _linearVelocity * _timeStep,
-    currentPose.Rot().Integrate(_angularVelocity, _timeStep));
-  this->SetPose(nextPose);
-}
+// //////////////////////////////////////////////////
+// void Model::SetLinearVelocity(const math::Vector3d _velocity)
+// {
+//   this->linearVelocity = _velocity;
+// }
+
+// //////////////////////////////////////////////////
+// math::Vector3d Model::GetLinearVelocity() const
+// {
+//   IGN_PROFILE("tpelib::Model::GetLinearVelocity");
+//   return this->linearVelocity;
+// }
+
+// //////////////////////////////////////////////////
+// void Model::SetAngularVelocity(const math::Vector3d _velocity)
+// {
+//   this->angularVelocity = _velocity;
+// }
+
+// //////////////////////////////////////////////////
+// math::Vector3d Model::GetAngularVelocity() const
+// {
+//   IGN_PROFILE("tpelib::Model::GetAngularVelocity");
+//   return this->angularVelocity;
+// }
+
+// //////////////////////////////////////////////////
+// void Model::UpdatePose(
+//   const double _timeStep,
+//   const math::Vector3d _linearVelocity,
+//   const math::Vector3d _angularVelocity)
+// {
+//   IGN_PROFILE("tpelib::Model::UpdatePose");
+
+//   if (_linearVelocity == math::Vector3d::Zero &&
+//       _angularVelocity == math::Vector3d::Zero)
+//     return;
+
+//   math::Pose3d currentPose = this->GetPose();
+//   math::Pose3d nextPose(
+//     currentPose.Pos() + _linearVelocity * _timeStep,
+//     currentPose.Rot().Integrate(_angularVelocity, _timeStep));
+//   this->SetPose(nextPose);
+// }
diff --git a/tpe/lib/src/Model.hh b/tpe/lib/src/Model.hh
index 8e910d45b..d78db3005 100644
--- a/tpe/lib/src/Model.hh
+++ b/tpe/lib/src/Model.hh
@@ -18,8 +18,6 @@
 #ifndef IGNITION_PHYSICS_TPE_LIB_SRC_MODEL_HH_
 #define IGNITION_PHYSICS_TPE_LIB_SRC_MODEL_HH_
 
-#include <ignition/utilities/SuppressWarning.hh>
-
 #include "ignition/physics/tpelib/Export.hh"
 
 #include "Entity.hh"
@@ -60,38 +58,38 @@ 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 Get the linear velocity of model
-  /// \return linear velocity of model
-  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 Get the angular velocity of model
-  /// \return angular velocity
-  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);
-
-  IGN_UTILS_WARN_IGNORE__DLL_INTERFACE_MISSING
-  /// \brief linear velocity of model
-  protected: math::Vector3d linearVelocity;
-
-  /// \brief angular velocity of model
-  protected: math::Vector3d angularVelocity;
-  IGN_UTILS_WARN_RESUME__DLL_INTERFACE_MISSING
+  // /// \brief Set the linear velocity of model
+  // /// \param[in] _velocity linear velocity
+  // public: void SetLinearVelocity(const math::Vector3d _velocity);
+
+  // /// \brief Get the linear velocity of model
+  // /// \return linear velocity of model
+  // 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 Get the angular velocity of model
+  // /// \return angular velocity
+  // 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);
+
+  // IGN_UTILS_WARN_IGNORE__DLL_INTERFACE_MISSING
+  // /// \brief linear velocity of model
+  // protected: math::Vector3d linearVelocity;
+
+  // /// \brief angular velocity of model
+  // protected: math::Vector3d angularVelocity;
+  // IGN_UTILS_WARN_RESUME__DLL_INTERFACE_MISSING
 
   /// \brief Pointer to private data class
   private: ModelPrivate *dataPtr = nullptr;

From 5078a305f2e1c8f2f9ace0e3ad89077271918b0f Mon Sep 17 00:00:00 2001
From: claireyywang <22240514+claireyywang@users.noreply.github.com>
Date: Wed, 16 Dec 2020 22:47:03 -0800
Subject: [PATCH 03/15] add updatepose and vels to link

Signed-off-by: claireyywang <22240514+claireyywang@users.noreply.github.com>
---
 tpe/lib/src/Entity.cc | 53 --------------------------
 tpe/lib/src/Entity.hh | 34 -----------------
 tpe/lib/src/Link.cc   | 56 +++++++++++++++++++--------
 tpe/lib/src/Link.hh   | 35 +++++++++++++++++
 tpe/lib/src/Model.cc  | 88 +++++++++++++++++++++----------------------
 tpe/lib/src/Model.hh  | 66 ++++++++++++++++----------------
 6 files changed, 153 insertions(+), 179 deletions(-)

diff --git a/tpe/lib/src/Entity.cc b/tpe/lib/src/Entity.cc
index 2f1e2dbf8..4a17cf948 100644
--- a/tpe/lib/src/Entity.cc
+++ b/tpe/lib/src/Entity.cc
@@ -148,59 +148,6 @@ math::Pose3d Entity::GetWorldPose() const
   return this->dataPtr->pose;
 }
 
-//////////////////////////////////////////////////
-void Entity::UpdatePose(
-  const double _timeStep,
-  const math::Vector3d _linearVelocity,
-  const math::Vector3d _angularVelocity)
-{
-  if (_linearVelocity == math::Vector3d::Zero &&
-      _angularVelocity == math::Vector3d::Zero)
-    return;
-
-  math::Pose3d currentPose = this->GetPose();
-  math::Pose3d nextPose(
-    currentPose.Pos() + _linearVelocity * _timeStep,
-    currentPose.Rot().Integrate(_angularVelocity, _timeStep));
-  this->SetPose(nextPose);
-}
-
-//////////////////////////////////////////////////
-void Entity::SetId(std::size_t _id)
-{
-  this->dataPtr->id = _id;
-}
-
-//////////////////////////////////////////////////
-std::size_t Entity::GetId() const
-{
-  return this->dataPtr->id;
-}
-
-//////////////////////////////////////////////////
-void Entity::SetLinearVelocity(const math::Vector3d _velocity)
-{
-  this->linearVelocity = _velocity;
-}
-
-//////////////////////////////////////////////////
-math::Vector3d Entity::GetLinearVelocity() const
-{
-  return this->linearVelocity;
-}
-
-//////////////////////////////////////////////////
-void Entity::SetAngularVelocity(const math::Vector3d _velocity)
-{
-  this->angularVelocity = _velocity;
-}
-
-//////////////////////////////////////////////////
-math::Vector3d Entity::GetAngularVelocity() const
-{
-  return this->angularVelocity;
-}
-
 //////////////////////////////////////////////////
 Entity &Entity::GetChildById(std::size_t _id) const
 {
diff --git a/tpe/lib/src/Entity.hh b/tpe/lib/src/Entity.hh
index 2e7946fda..98352cd4f 100644
--- a/tpe/lib/src/Entity.hh
+++ b/tpe/lib/src/Entity.hh
@@ -23,7 +23,6 @@
 #include <memory>
 #include <string>
 
-#include <ignition/utilities/SuppressWarning.hh>
 #include <ignition/math/AxisAlignedBox.hh>
 #include <ignition/math/Pose3.hh>
 #include "ignition/physics/tpelib/Export.hh"
@@ -91,15 +90,6 @@ class IGNITION_PHYSICS_TPELIB_VISIBLE Entity
   /// \param[in] _pose Pose of entity to set to
   public: virtual void SetPose(const math::Pose3d &_pose);
 
-  /// \brief Update 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);
-
   /// \brief Get the pose of the entity
   /// \return Pose of entity
   public: virtual math::Pose3d GetPose() const;
@@ -108,22 +98,6 @@ class IGNITION_PHYSICS_TPELIB_VISIBLE Entity
   /// \return World pose of entity
   public: virtual math::Pose3d GetWorldPose() const;
 
-  /// \brief Set the linear velocity of entity
-  /// \param[in] _velocity linear velocity
-  public: virtual void SetLinearVelocity(const math::Vector3d _velocity);
-
-  /// \brief Get the linear velocity of entity
-  /// \return linear velocity of entity
-  public: virtual math::Vector3d GetLinearVelocity() const;
-
-  /// \brief Set the angular velocity of entity
-  /// \param[in] _velocity angular velocity of entity
-  public: virtual void SetAngularVelocity(const math::Vector3d _velocity);
-
-  /// \brief Get the angular velocity of entity
-  /// \return angular velocity
-  public: virtual math::Vector3d GetAngularVelocity() const;
-
   /// \brief Get a child entity by id
   /// \param[in] _id Id of child entity
   /// \return Child entity
@@ -205,14 +179,6 @@ class IGNITION_PHYSICS_TPELIB_VISIBLE Entity
   /// \return size_t id of next entity
   protected: static std::size_t GetNextId();
 
-  IGN_UTILS_WARN_IGNORE__DLL_INTERFACE_MISSING
-  /// \brief linear velocity of entity
-  protected: math::Vector3d linearVelocity;
-
-  /// \brief angular velocity of entity
-  protected: math::Vector3d angularVelocity;
-  IGN_UTILS_WARN_RESUME__DLL_INTERFACE_MISSING
-
   /// \brief Entity id counter
   private: static std::size_t nextId;
 
diff --git a/tpe/lib/src/Link.cc b/tpe/lib/src/Link.cc
index 75110bbd2..bd035dec2 100644
--- a/tpe/lib/src/Link.cc
+++ b/tpe/lib/src/Link.cc
@@ -43,19 +43,43 @@ Entity &Link::AddCollision()
   return *it->second.get();
 }
 
-// //////////////////////////////////////////////////
-// void Link::UpdatePose(
-//   const double _timeStep,
-//   const math::Vector3d _linearVelocity,
-//   const math::Vector3d _angularVelocity)
-// {
-//   if (_linearVelocity == math::Vector3d::Zero &&
-//       _angularVelocity == math::Vector3d::Zero)
-//     return;
-
-//   math::Pose3d currentPose = this->GetPose();
-//   math::Pose3d nextPose(
-//     currentPose.Pos() + _linearVelocity * _timeStep,
-//     currentPose.Rot().Integrate(_angularVelocity, _timeStep));
-//   this->SetPose(nextPose);
-// }
+//////////////////////////////////////////////////
+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(
+  const double _timeStep,
+  const math::Vector3d _linearVelocity,
+  const math::Vector3d _angularVelocity)
+{
+  if (_linearVelocity == math::Vector3d::Zero &&
+      _angularVelocity == math::Vector3d::Zero)
+    return;
+
+  math::Pose3d currentPose = this->GetPose();
+  math::Pose3d nextPose(
+    currentPose.Pos() + _linearVelocity * _timeStep,
+    currentPose.Rot().Integrate(_angularVelocity, _timeStep));
+  this->SetPose(nextPose);
+}
diff --git a/tpe/lib/src/Link.hh b/tpe/lib/src/Link.hh
index 6cf68f55b..ee1216a7c 100644
--- a/tpe/lib/src/Link.hh
+++ b/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,39 @@ 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
+  /// \param[in] _velocity linear velocity
+  public: void SetLinearVelocity(const math::Vector3d _velocity);
+
+  /// \brief Get the linear velocity of link
+  /// \return linear velocity of link
+  public: math::Vector3d GetLinearVelocity() const;
+
+  /// \brief Set the angular velocity of link
+  /// \param[in] _velocity angular velocity
+  public: void SetAngularVelocity(const math::Vector3d _velocity);
+
+  /// \brief Get the angular velocity of link
+  /// \return angular velocity
+  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);
+
+  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
 };
 
 }
diff --git a/tpe/lib/src/Model.cc b/tpe/lib/src/Model.cc
index 0d455b7a4..eaec716b8 100644
--- a/tpe/lib/src/Model.cc
+++ b/tpe/lib/src/Model.cc
@@ -126,47 +126,47 @@ Entity &Model::GetCanonicalLink()
   return kNullEntity;
 }
 
-// //////////////////////////////////////////////////
-// void Model::SetLinearVelocity(const math::Vector3d _velocity)
-// {
-//   this->linearVelocity = _velocity;
-// }
-
-// //////////////////////////////////////////////////
-// math::Vector3d Model::GetLinearVelocity() const
-// {
-//   IGN_PROFILE("tpelib::Model::GetLinearVelocity");
-//   return this->linearVelocity;
-// }
-
-// //////////////////////////////////////////////////
-// void Model::SetAngularVelocity(const math::Vector3d _velocity)
-// {
-//   this->angularVelocity = _velocity;
-// }
-
-// //////////////////////////////////////////////////
-// math::Vector3d Model::GetAngularVelocity() const
-// {
-//   IGN_PROFILE("tpelib::Model::GetAngularVelocity");
-//   return this->angularVelocity;
-// }
-
-// //////////////////////////////////////////////////
-// void Model::UpdatePose(
-//   const double _timeStep,
-//   const math::Vector3d _linearVelocity,
-//   const math::Vector3d _angularVelocity)
-// {
-//   IGN_PROFILE("tpelib::Model::UpdatePose");
-
-//   if (_linearVelocity == math::Vector3d::Zero &&
-//       _angularVelocity == math::Vector3d::Zero)
-//     return;
-
-//   math::Pose3d currentPose = this->GetPose();
-//   math::Pose3d nextPose(
-//     currentPose.Pos() + _linearVelocity * _timeStep,
-//     currentPose.Rot().Integrate(_angularVelocity, _timeStep));
-//   this->SetPose(nextPose);
-// }
+//////////////////////////////////////////////////
+void Model::SetLinearVelocity(const math::Vector3d _velocity)
+{
+  this->linearVelocity = _velocity;
+}
+
+//////////////////////////////////////////////////
+math::Vector3d Model::GetLinearVelocity() const
+{
+  IGN_PROFILE("tpelib::Model::GetLinearVelocity");
+  return this->linearVelocity;
+}
+
+//////////////////////////////////////////////////
+void Model::SetAngularVelocity(const math::Vector3d _velocity)
+{
+  this->angularVelocity = _velocity;
+}
+
+//////////////////////////////////////////////////
+math::Vector3d Model::GetAngularVelocity() const
+{
+  IGN_PROFILE("tpelib::Model::GetAngularVelocity");
+  return this->angularVelocity;
+}
+
+//////////////////////////////////////////////////
+void Model::UpdatePose(
+  const double _timeStep,
+  const math::Vector3d _linearVelocity,
+  const math::Vector3d _angularVelocity)
+{
+  IGN_PROFILE("tpelib::Model::UpdatePose");
+
+  if (_linearVelocity == math::Vector3d::Zero &&
+      _angularVelocity == math::Vector3d::Zero)
+    return;
+
+  math::Pose3d currentPose = this->GetPose();
+  math::Pose3d nextPose(
+    currentPose.Pos() + _linearVelocity * _timeStep,
+    currentPose.Rot().Integrate(_angularVelocity, _timeStep));
+  this->SetPose(nextPose);
+}
diff --git a/tpe/lib/src/Model.hh b/tpe/lib/src/Model.hh
index d78db3005..8e910d45b 100644
--- a/tpe/lib/src/Model.hh
+++ b/tpe/lib/src/Model.hh
@@ -18,6 +18,8 @@
 #ifndef IGNITION_PHYSICS_TPE_LIB_SRC_MODEL_HH_
 #define IGNITION_PHYSICS_TPE_LIB_SRC_MODEL_HH_
 
+#include <ignition/utilities/SuppressWarning.hh>
+
 #include "ignition/physics/tpelib/Export.hh"
 
 #include "Entity.hh"
@@ -58,38 +60,38 @@ 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 Get the linear velocity of model
-  // /// \return linear velocity of model
-  // 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 Get the angular velocity of model
-  // /// \return angular velocity
-  // 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);
-
-  // IGN_UTILS_WARN_IGNORE__DLL_INTERFACE_MISSING
-  // /// \brief linear velocity of model
-  // protected: math::Vector3d linearVelocity;
-
-  // /// \brief angular velocity of model
-  // protected: math::Vector3d angularVelocity;
-  // IGN_UTILS_WARN_RESUME__DLL_INTERFACE_MISSING
+  /// \brief Set the linear velocity of model
+  /// \param[in] _velocity linear velocity
+  public: void SetLinearVelocity(const math::Vector3d _velocity);
+
+  /// \brief Get the linear velocity of model
+  /// \return linear velocity of model
+  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 Get the angular velocity of model
+  /// \return angular velocity
+  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);
+
+  IGN_UTILS_WARN_IGNORE__DLL_INTERFACE_MISSING
+  /// \brief linear velocity of model
+  protected: math::Vector3d linearVelocity;
+
+  /// \brief angular velocity of model
+  protected: math::Vector3d angularVelocity;
+  IGN_UTILS_WARN_RESUME__DLL_INTERFACE_MISSING
 
   /// \brief Pointer to private data class
   private: ModelPrivate *dataPtr = nullptr;

From a87b9a4964427dfc4a5f8189e2c8a9925a824577 Mon Sep 17 00:00:00 2001
From: claireyywang <22240514+claireyywang@users.noreply.github.com>
Date: Wed, 16 Dec 2020 22:59:40 -0800
Subject: [PATCH 04/15] update link pose in world

Signed-off-by: claireyywang <22240514+claireyywang@users.noreply.github.com>
---
 tpe/lib/src/Entity.hh |  2 +-
 tpe/lib/src/World.cc  | 10 ++++++++++
 2 files changed, 11 insertions(+), 1 deletion(-)

diff --git a/tpe/lib/src/Entity.hh b/tpe/lib/src/Entity.hh
index 98352cd4f..9e9f28db9 100644
--- a/tpe/lib/src/Entity.hh
+++ b/tpe/lib/src/Entity.hh
@@ -165,7 +165,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
diff --git a/tpe/lib/src/World.cc b/tpe/lib/src/World.cc
index 33dfc313d..21ddee627 100644
--- a/tpe/lib/src/World.cc
+++ b/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;
@@ -70,6 +71,15 @@ void World::Step()
       this->timeStep,
       model->GetLinearVelocity(),
       model->GetAngularVelocity());
+    auto &ents = model->GetChildren();
+    for (auto linkIt = ents.begin(); linkIt != ents.end(); ++linkIt)
+    {
+      auto link = std::dynamic_pointer_cast<Link>(linkIt->second);
+      link->UpdatePose(
+        this->timeStep,
+        link->GetLinearVelocity(),
+        link->GetAngularVelocity());
+    }
   }
 
   // check colliisions

From 3f81365493b22557b9cfed9f97f138fcd9ee0174 Mon Sep 17 00:00:00 2001
From: claireyywang <22240514+claireyywang@users.noreply.github.com>
Date: Thu, 17 Dec 2020 17:31:45 -0800
Subject: [PATCH 05/15] add link updatepose and link to freegroup set velocity

Signed-off-by: claireyywang <22240514+claireyywang@users.noreply.github.com>
---
 tpe/lib/src/World.cc                |  1 +
 tpe/plugin/src/FreeGroupFeatures.cc | 27 +++++++++++++++++++++++----
 2 files changed, 24 insertions(+), 4 deletions(-)

diff --git a/tpe/lib/src/World.cc b/tpe/lib/src/World.cc
index 21ddee627..d182c0bcb 100644
--- a/tpe/lib/src/World.cc
+++ b/tpe/lib/src/World.cc
@@ -75,6 +75,7 @@ void World::Step()
     for (auto linkIt = ents.begin(); linkIt != ents.end(); ++linkIt)
     {
       auto link = std::dynamic_pointer_cast<Link>(linkIt->second);
+      std::cout << link->GetAngularVelocity()[2] << std::endl;
       link->UpdatePose(
         this->timeStep,
         link->GetLinearVelocity(),
diff --git a/tpe/plugin/src/FreeGroupFeatures.cc b/tpe/plugin/src/FreeGroupFeatures.cc
index 2d306c2a1..35ce2d07d 100644
--- a/tpe/plugin/src/FreeGroupFeatures.cc
+++ b/tpe/plugin/src/FreeGroupFeatures.cc
@@ -141,24 +141,43 @@ 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)
+    {
+      linkIt->second->link->SetLinearVelocity(
+        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)
+    {
+      std::cout << "found link to set angular velocity" << std::endl;
+      linkIt->second->link->SetAngularVelocity(
+        math::eigen3::convert(_angularVelocity));
+    }
+  }
 }

From 8580f7a16151716fa50ba793eec410ca8a2020a6 Mon Sep 17 00:00:00 2001
From: claireyywang <22240514+claireyywang@users.noreply.github.com>
Date: Thu, 17 Dec 2020 17:33:02 -0800
Subject: [PATCH 06/15] fix Entity SetId GetId

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

diff --git a/tpe/lib/src/Entity.cc b/tpe/lib/src/Entity.cc
index 4a17cf948..f3158c7d1 100644
--- a/tpe/lib/src/Entity.cc
+++ b/tpe/lib/src/Entity.cc
@@ -148,6 +148,18 @@ math::Pose3d Entity::GetWorldPose() const
   return this->dataPtr->pose;
 }
 
+//////////////////////////////////////////////////
+void Entity::SetId(std::size_t _id)
+{
+  this->dataPtr->id = _id;
+}
+
+//////////////////////////////////////////////////
+std::size_t Entity::GetId() const
+{
+  return this->dataPtr->id;
+}
+
 //////////////////////////////////////////////////
 Entity &Entity::GetChildById(std::size_t _id) const
 {

From aca27e00d642018e4b62ace69db2c9d1b84cad41 Mon Sep 17 00:00:00 2001
From: claireyywang <22240514+claireyywang@users.noreply.github.com>
Date: Thu, 17 Dec 2020 18:16:23 -0800
Subject: [PATCH 07/15] remove std::cout

Signed-off-by: claireyywang <22240514+claireyywang@users.noreply.github.com>
---
 tpe/lib/src/World.cc                | 1 -
 tpe/plugin/src/FreeGroupFeatures.cc | 1 -
 2 files changed, 2 deletions(-)

diff --git a/tpe/lib/src/World.cc b/tpe/lib/src/World.cc
index d182c0bcb..21ddee627 100644
--- a/tpe/lib/src/World.cc
+++ b/tpe/lib/src/World.cc
@@ -75,7 +75,6 @@ void World::Step()
     for (auto linkIt = ents.begin(); linkIt != ents.end(); ++linkIt)
     {
       auto link = std::dynamic_pointer_cast<Link>(linkIt->second);
-      std::cout << link->GetAngularVelocity()[2] << std::endl;
       link->UpdatePose(
         this->timeStep,
         link->GetLinearVelocity(),
diff --git a/tpe/plugin/src/FreeGroupFeatures.cc b/tpe/plugin/src/FreeGroupFeatures.cc
index 35ce2d07d..d67672710 100644
--- a/tpe/plugin/src/FreeGroupFeatures.cc
+++ b/tpe/plugin/src/FreeGroupFeatures.cc
@@ -175,7 +175,6 @@ void FreeGroupFeatures::SetFreeGroupWorldAngularVelocity(
     auto linkIt = this->links.find(_groupID.id);
     if (linkIt != this->links.end() && linkIt->second != nullptr)
     {
-      std::cout << "found link to set angular velocity" << std::endl;
       linkIt->second->link->SetAngularVelocity(
         math::eigen3::convert(_angularVelocity));
     }

From f933a0f7dc284a92e9b76d76b60efbea284c0959 Mon Sep 17 00:00:00 2001
From: claireyywang <22240514+claireyywang@users.noreply.github.com>
Date: Mon, 21 Dec 2020 23:06:18 -0800
Subject: [PATCH 08/15] pass by & and add unit tests

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

diff --git a/tpe/lib/src/Link.cc b/tpe/lib/src/Link.cc
index bd035dec2..a16438623 100644
--- a/tpe/lib/src/Link.cc
+++ b/tpe/lib/src/Link.cc
@@ -44,7 +44,7 @@ Entity &Link::AddCollision()
 }
 
 //////////////////////////////////////////////////
-void Link::SetLinearVelocity(const math::Vector3d _velocity)
+void Link::SetLinearVelocity(const &math::Vector3d _velocity)
 {
   this->linearVelocity = _velocity;
 }
@@ -56,7 +56,7 @@ math::Vector3d Link::GetLinearVelocity() const
 }
 
 //////////////////////////////////////////////////
-void Link::SetAngularVelocity(const math::Vector3d _velocity)
+void Link::SetAngularVelocity(const &math::Vector3d _velocity)
 {
   this->angularVelocity = _velocity;
 }
diff --git a/tpe/lib/src/Link.hh b/tpe/lib/src/Link.hh
index ee1216a7c..411b69bbc 100644
--- a/tpe/lib/src/Link.hh
+++ b/tpe/lib/src/Link.hh
@@ -47,7 +47,7 @@ class IGNITION_PHYSICS_TPELIB_VISIBLE Link : public Entity
 
   /// \brief Set the linear velocity of link
   /// \param[in] _velocity linear velocity
-  public: void SetLinearVelocity(const math::Vector3d _velocity);
+  public: void SetLinearVelocity(const &math::Vector3d _velocity);
 
   /// \brief Get the linear velocity of link
   /// \return linear velocity of link
@@ -55,7 +55,7 @@ class IGNITION_PHYSICS_TPELIB_VISIBLE Link : public Entity
 
   /// \brief Set the angular velocity of link
   /// \param[in] _velocity angular velocity
-  public: void SetAngularVelocity(const math::Vector3d _velocity);
+  public: void SetAngularVelocity(const &math::Vector3d _velocity);
 
   /// \brief Get the angular velocity of link
   /// \return angular velocity
diff --git a/tpe/lib/src/Link_TEST.cc b/tpe/lib/src/Link_TEST.cc
index 6481b47c6..e67206cdb 100644
--- a/tpe/lib/src/Link_TEST.cc
+++ b/tpe/lib/src/Link_TEST.cc
@@ -48,6 +48,12 @@ TEST(Link, BasicAPI)
   EXPECT_EQ(math::Pose3d(10, -0.312675, 2.43845, 1.23686, 0.471978, 0.918989),
             linkEnt.GetWorldPose());
 
+  linkEnt.SetLinearVelocity(math::Vector3d(0, 0.1, 0));
+  EXPECT_EQ(math::Vector3d(0, 0.1, 0), linkEnt.GetLinearVelocity());
+
+  linkEnt.SetAngularVelocity(math::Vector3d(0.2, 0, 1));
+  EXPECT_EQ(math::Vector3d(0.2, 0, 1), linkEnt.GetAngularVelocity());
+
   Link link2;
   EXPECT_NE(link.GetId(), link2.GetId());
 }

From 78899fc255abf9f554d4fe3adc754771fa26c19e Mon Sep 17 00:00:00 2001
From: claireyywang <22240514+claireyywang@users.noreply.github.com>
Date: Tue, 22 Dec 2020 20:50:47 -0800
Subject: [PATCH 09/15] correct &

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

diff --git a/tpe/lib/src/Link.cc b/tpe/lib/src/Link.cc
index a16438623..c43b7dca3 100644
--- a/tpe/lib/src/Link.cc
+++ b/tpe/lib/src/Link.cc
@@ -44,7 +44,7 @@ Entity &Link::AddCollision()
 }
 
 //////////////////////////////////////////////////
-void Link::SetLinearVelocity(const &math::Vector3d _velocity)
+void Link::SetLinearVelocity(const math::Vector3d &_velocity)
 {
   this->linearVelocity = _velocity;
 }
@@ -56,7 +56,7 @@ math::Vector3d Link::GetLinearVelocity() const
 }
 
 //////////////////////////////////////////////////
-void Link::SetAngularVelocity(const &math::Vector3d _velocity)
+void Link::SetAngularVelocity(const math::Vector3d &_velocity)
 {
   this->angularVelocity = _velocity;
 }
@@ -70,8 +70,8 @@ math::Vector3d Link::GetAngularVelocity() const
 //////////////////////////////////////////////////
 void Link::UpdatePose(
   const double _timeStep,
-  const math::Vector3d _linearVelocity,
-  const math::Vector3d _angularVelocity)
+  const math::Vector3d &_linearVelocity,
+  const math::Vector3d &_angularVelocity)
 {
   if (_linearVelocity == math::Vector3d::Zero &&
       _angularVelocity == math::Vector3d::Zero)
diff --git a/tpe/lib/src/Link.hh b/tpe/lib/src/Link.hh
index 411b69bbc..5ebf31700 100644
--- a/tpe/lib/src/Link.hh
+++ b/tpe/lib/src/Link.hh
@@ -47,7 +47,7 @@ class IGNITION_PHYSICS_TPELIB_VISIBLE Link : public Entity
 
   /// \brief Set the linear velocity of link
   /// \param[in] _velocity linear velocity
-  public: void SetLinearVelocity(const &math::Vector3d _velocity);
+  public: void SetLinearVelocity(const math::Vector3d &_velocity);
 
   /// \brief Get the linear velocity of link
   /// \return linear velocity of link
@@ -55,7 +55,7 @@ class IGNITION_PHYSICS_TPELIB_VISIBLE Link : public Entity
 
   /// \brief Set the angular velocity of link
   /// \param[in] _velocity angular velocity
-  public: void SetAngularVelocity(const &math::Vector3d _velocity);
+  public: void SetAngularVelocity(const math::Vector3d &_velocity);
 
   /// \brief Get the angular velocity of link
   /// \return angular velocity
@@ -67,8 +67,8 @@ class IGNITION_PHYSICS_TPELIB_VISIBLE Link : public Entity
   /// \param[in] _angularVelocity angular velocity
   public: virtual void UpdatePose(
     const double _timeStep,
-    const math::Vector3d _linearVelocity,
-    const math::Vector3d _angularVelocity);
+    const math::Vector3d &_linearVelocity,
+    const math::Vector3d &_angularVelocity);
 
   IGN_UTILS_WARN_IGNORE__DLL_INTERFACE_MISSING
   /// \brief linear velocity of link
diff --git a/tpe/lib/src/Model.cc b/tpe/lib/src/Model.cc
index eaec716b8..6fe7b71b3 100644
--- a/tpe/lib/src/Model.cc
+++ b/tpe/lib/src/Model.cc
@@ -127,7 +127,7 @@ Entity &Model::GetCanonicalLink()
 }
 
 //////////////////////////////////////////////////
-void Model::SetLinearVelocity(const math::Vector3d _velocity)
+void Model::SetLinearVelocity(const math::Vector3d &_velocity)
 {
   this->linearVelocity = _velocity;
 }
@@ -140,7 +140,7 @@ math::Vector3d Model::GetLinearVelocity() const
 }
 
 //////////////////////////////////////////////////
-void Model::SetAngularVelocity(const math::Vector3d _velocity)
+void Model::SetAngularVelocity(const math::Vector3d &_velocity)
 {
   this->angularVelocity = _velocity;
 }
@@ -155,8 +155,8 @@ math::Vector3d Model::GetAngularVelocity() const
 //////////////////////////////////////////////////
 void Model::UpdatePose(
   const double _timeStep,
-  const math::Vector3d _linearVelocity,
-  const math::Vector3d _angularVelocity)
+  const math::Vector3d &_linearVelocity,
+  const math::Vector3d &_angularVelocity)
 {
   IGN_PROFILE("tpelib::Model::UpdatePose");
 
diff --git a/tpe/lib/src/Model.hh b/tpe/lib/src/Model.hh
index 8e910d45b..9017a9a17 100644
--- a/tpe/lib/src/Model.hh
+++ b/tpe/lib/src/Model.hh
@@ -62,7 +62,7 @@ class IGNITION_PHYSICS_TPELIB_VISIBLE Model : public Entity
 
   /// \brief Set the linear velocity of model
   /// \param[in] _velocity linear velocity
-  public: void SetLinearVelocity(const math::Vector3d _velocity);
+  public: void SetLinearVelocity(const math::Vector3d &_velocity);
 
   /// \brief Get the linear velocity of model
   /// \return linear velocity of model
@@ -70,7 +70,7 @@ class IGNITION_PHYSICS_TPELIB_VISIBLE Model : public Entity
 
   /// \brief Set the angular velocity of model
   /// \param[in] _velocity angular velocity from world
-  public: void SetAngularVelocity(const math::Vector3d _velocity);
+  public: void SetAngularVelocity(const math::Vector3d &_velocity);
 
   /// \brief Get the angular velocity of model
   /// \return angular velocity
@@ -82,8 +82,8 @@ class IGNITION_PHYSICS_TPELIB_VISIBLE Model : public Entity
   /// \param[in] _angularVelocity angular velocity
   public: virtual void UpdatePose(
     const double _timeStep,
-    const math::Vector3d _linearVelocity,
-    const math::Vector3d _angularVelocity);
+    const math::Vector3d &_linearVelocity,
+    const math::Vector3d &_angularVelocity);
 
   IGN_UTILS_WARN_IGNORE__DLL_INTERFACE_MISSING
   /// \brief linear velocity of model

From 32e7b87402255aa1a858707b56df6266dffdcdd7 Mon Sep 17 00:00:00 2001
From: claireyywang <22240514+claireyywang@users.noreply.github.com>
Date: Tue, 22 Dec 2020 21:33:57 -0800
Subject: [PATCH 10/15] correct link tests

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

diff --git a/tpe/lib/src/Link_TEST.cc b/tpe/lib/src/Link_TEST.cc
index e67206cdb..87e68c7ed 100644
--- a/tpe/lib/src/Link_TEST.cc
+++ b/tpe/lib/src/Link_TEST.cc
@@ -48,11 +48,11 @@ TEST(Link, BasicAPI)
   EXPECT_EQ(math::Pose3d(10, -0.312675, 2.43845, 1.23686, 0.471978, 0.918989),
             linkEnt.GetWorldPose());
 
-  linkEnt.SetLinearVelocity(math::Vector3d(0, 0.1, 0));
-  EXPECT_EQ(math::Vector3d(0, 0.1, 0), linkEnt.GetLinearVelocity());
+  link.SetLinearVelocity(math::Vector3d(0, 0.1, 0));
+  EXPECT_EQ(math::Vector3d(0, 0.1, 0), link.GetLinearVelocity());
 
-  linkEnt.SetAngularVelocity(math::Vector3d(0.2, 0, 1));
-  EXPECT_EQ(math::Vector3d(0.2, 0, 1), linkEnt.GetAngularVelocity());
+  link.SetAngularVelocity(math::Vector3d(0.2, 0, 1));
+  EXPECT_EQ(math::Vector3d(0.2, 0, 1), link.GetAngularVelocity());
 
   Link link2;
   EXPECT_NE(link.GetId(), link2.GetId());

From ba45184d9f31813ccc33ea96ff158322e6dc8eda Mon Sep 17 00:00:00 2001
From: Ian Chen <ichen@osrfoundation.org>
Date: Fri, 19 Feb 2021 14:55:20 -0800
Subject: [PATCH 11/15] fix reference frame when setting link vel

Signed-off-by: Ian Chen <ichen@osrfoundation.org>
---
 tpe/plugin/src/FreeGroupFeatures.cc | 6 +++++-
 1 file changed, 5 insertions(+), 1 deletion(-)

diff --git a/tpe/plugin/src/FreeGroupFeatures.cc b/tpe/plugin/src/FreeGroupFeatures.cc
index d67672710..84f10b805 100644
--- a/tpe/plugin/src/FreeGroupFeatures.cc
+++ b/tpe/plugin/src/FreeGroupFeatures.cc
@@ -153,8 +153,10 @@ void FreeGroupFeatures::SetFreeGroupWorldLinearVelocity(
     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(
-        math::eigen3::convert(_linearVelocity));
+        linkWorldPose.Rot().Inverse() *
+        math::eigen3::convert( _linearVelocity));
     }
   }
 }
@@ -175,7 +177,9 @@ void FreeGroupFeatures::SetFreeGroupWorldAngularVelocity(
     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));
     }
   }

From 498d2c20958100db67547305710bf80d6c921add Mon Sep 17 00:00:00 2001
From: Ian Chen <ichen@osrfoundation.org>
Date: Mon, 22 Feb 2021 11:15:06 -0800
Subject: [PATCH 12/15] check child of mdoel is link

Signed-off-by: Ian Chen <ichen@osrfoundation.org>
---
 tpe/lib/src/World.cc | 12 ++++++++----
 1 file changed, 8 insertions(+), 4 deletions(-)

diff --git a/tpe/lib/src/World.cc b/tpe/lib/src/World.cc
index 21ddee627..c1eab195d 100644
--- a/tpe/lib/src/World.cc
+++ b/tpe/lib/src/World.cc
@@ -74,11 +74,15 @@ void World::Step()
     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);
-      link->UpdatePose(
-        this->timeStep,
-        link->GetLinearVelocity(),
-        link->GetAngularVelocity());
+      if (link)
+      {
+        link->UpdatePose(
+          this->timeStep,
+          link->GetLinearVelocity(),
+          link->GetAngularVelocity());
+      }
     }
   }
 

From 58f3e0bf8c800fef22a1418ab1c76f06571dcb56 Mon Sep 17 00:00:00 2001
From: Ian Chen <ichen@osrfoundation.org>
Date: Tue, 16 Mar 2021 18:27:53 -0700
Subject: [PATCH 13/15] address feedback

Signed-off-by: Ian Chen <ichen@osrfoundation.org>
---
 tpe/lib/src/Link.cc       | 13 +++++--------
 tpe/lib/src/Link.hh       |  9 ++-------
 tpe/lib/src/Link_TEST.cc  | 25 ++++++++++++++++++-------
 tpe/lib/src/Model.cc      | 13 +++++--------
 tpe/lib/src/Model.hh      | 17 ++++++-----------
 tpe/lib/src/Model_TEST.cc |  5 ++---
 tpe/lib/src/World.cc      | 10 ++--------
 7 files changed, 40 insertions(+), 52 deletions(-)

diff --git a/tpe/lib/src/Link.cc b/tpe/lib/src/Link.cc
index c43b7dca3..c14076b5f 100644
--- a/tpe/lib/src/Link.cc
+++ b/tpe/lib/src/Link.cc
@@ -68,18 +68,15 @@ math::Vector3d Link::GetAngularVelocity() const
 }
 
 //////////////////////////////////////////////////
-void Link::UpdatePose(
-  const double _timeStep,
-  const math::Vector3d &_linearVelocity,
-  const math::Vector3d &_angularVelocity)
+void Link::UpdatePose(double _timeStep)
 {
-  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);
 }
diff --git a/tpe/lib/src/Link.hh b/tpe/lib/src/Link.hh
index 5ebf31700..bdf3c4cf4 100644
--- a/tpe/lib/src/Link.hh
+++ b/tpe/lib/src/Link.hh
@@ -62,13 +62,8 @@ class IGNITION_PHYSICS_TPELIB_VISIBLE Link : public Entity
   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 link
diff --git a/tpe/lib/src/Link_TEST.cc b/tpe/lib/src/Link_TEST.cc
index 87e68c7ed..f086901d8 100644
--- a/tpe/lib/src/Link_TEST.cc
+++ b/tpe/lib/src/Link_TEST.cc
@@ -35,8 +35,9 @@ 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);
@@ -44,15 +45,25 @@ TEST(Link, BasicAPI)
   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());
 
-  link.SetLinearVelocity(math::Vector3d(0, 0.1, 0));
-  EXPECT_EQ(math::Vector3d(0, 0.1, 0), link.GetLinearVelocity());
+  math::Vector3d linVel(0, 0.1, 0);
+  link.SetLinearVelocity(linVel);
+  EXPECT_EQ(linVel, link.GetLinearVelocity());
 
-  link.SetAngularVelocity(math::Vector3d(0.2, 0, 1));
-  EXPECT_EQ(math::Vector3d(0.2, 0, 1), link.GetAngularVelocity());
+  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());
diff --git a/tpe/lib/src/Model.cc b/tpe/lib/src/Model.cc
index 6fe7b71b3..f70db0365 100644
--- a/tpe/lib/src/Model.cc
+++ b/tpe/lib/src/Model.cc
@@ -153,20 +153,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);
 }
diff --git a/tpe/lib/src/Model.hh b/tpe/lib/src/Model.hh
index 9017a9a17..bdf49af15 100644
--- a/tpe/lib/src/Model.hh
+++ b/tpe/lib/src/Model.hh
@@ -61,29 +61,24 @@ class IGNITION_PHYSICS_TPELIB_VISIBLE Model : public Entity
   public: Entity &GetCanonicalLink();
 
   /// \brief Set the linear velocity of model
-  /// \param[in] _velocity linear velocity
+  /// \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
+  /// \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
+  /// \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
+  /// \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
diff --git a/tpe/lib/src/Model_TEST.cc b/tpe/lib/src/Model_TEST.cc
index 310a95f89..930eaa11d 100644
--- a/tpe/lib/src/Model_TEST.cc
+++ b/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());
 }
 
diff --git a/tpe/lib/src/World.cc b/tpe/lib/src/World.cc
index c1eab195d..1b623e3ae 100644
--- a/tpe/lib/src/World.cc
+++ b/tpe/lib/src/World.cc
@@ -67,10 +67,7 @@ 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)
     {
@@ -78,10 +75,7 @@ void World::Step()
       auto link = std::dynamic_pointer_cast<Link>(linkIt->second);
       if (link)
       {
-        link->UpdatePose(
-          this->timeStep,
-          link->GetLinearVelocity(),
-          link->GetAngularVelocity());
+        link->UpdatePose(this->timeStep);
       }
     }
   }

From ab04c90a6c24f40bec929f74d8a3d637444e3bcf Mon Sep 17 00:00:00 2001
From: Ian Chen <ichen@osrfoundation.org>
Date: Tue, 16 Mar 2021 18:36:48 -0700
Subject: [PATCH 14/15] more units and frame

Signed-off-by: Ian Chen <ichen@osrfoundation.org>
---
 tpe/lib/src/Link.hh  | 16 ++++++++--------
 tpe/lib/src/Model.hh |  8 ++++----
 2 files changed, 12 insertions(+), 12 deletions(-)

diff --git a/tpe/lib/src/Link.hh b/tpe/lib/src/Link.hh
index bdf3c4cf4..05cfccf65 100644
--- a/tpe/lib/src/Link.hh
+++ b/tpe/lib/src/Link.hh
@@ -45,20 +45,20 @@ class IGNITION_PHYSICS_TPELIB_VISIBLE Link : public Entity
   /// \return Newly created Collision
   public: Entity &AddCollision();
 
-  /// \brief Set the linear velocity of link
-  /// \param[in] _velocity linear velocity
+  /// \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
-  /// \return linear velocity of link
+  /// \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
-  /// \param[in] _velocity angular velocity
+  /// \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
-  /// \return angular 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
diff --git a/tpe/lib/src/Model.hh b/tpe/lib/src/Model.hh
index bdf49af15..b62322179 100644
--- a/tpe/lib/src/Model.hh
+++ b/tpe/lib/src/Model.hh
@@ -60,19 +60,19 @@ class IGNITION_PHYSICS_TPELIB_VISIBLE Model : public Entity
   /// \return Entity the canonical (first) link
   public: Entity &GetCanonicalLink();
 
-  /// \brief Set the linear velocity of model
+  /// \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
+  /// \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
+  /// \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
+  /// \brief Get the angular velocity of model relative to parent
   /// \return angular velocity in radians per second
   public: math::Vector3d GetAngularVelocity() const;
 

From 1ee1e671ae41b4b6f0a32a6661652d538396f523 Mon Sep 17 00:00:00 2001
From: Ian Chen <ichen@osrfoundation.org>
Date: Thu, 1 Apr 2021 17:19:35 -0700
Subject: [PATCH 15/15] fix setting canonical link

Signed-off-by: Ian Chen <ichen@osrfoundation.org>
---
 tpe/lib/src/Model.cc                |  3 +++
 tpe/plugin/src/FreeGroupFeatures.cc |  4 +++
 tpe/plugin/src/SDFFeatures.cc       | 42 ++++++++++++++++-------------
 3 files changed, 31 insertions(+), 18 deletions(-)

diff --git a/tpe/lib/src/Model.cc b/tpe/lib/src/Model.cc
index f70db0365..3a95140cf 100644
--- a/tpe/lib/src/Model.cc
+++ b/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)});
diff --git a/tpe/plugin/src/FreeGroupFeatures.cc b/tpe/plugin/src/FreeGroupFeatures.cc
index 84f10b805..81c8d12e8 100644
--- a/tpe/plugin/src/FreeGroupFeatures.cc
+++ b/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);
diff --git a/tpe/plugin/src/SDFFeatures.cc b/tpe/plugin/src/SDFFeatures.cc
index bb250beb6..b07311b66 100644
--- a/tpe/plugin/src/SDFFeatures.cc
+++ b/tpe/plugin/src/SDFFeatures.cc
@@ -114,16 +114,19 @@ Identity SDFFeatures::ConstructSdfModel(
     this->ConstructSdfLink(modelIdentity, *_sdfModel.LinkByIndex(i));
   }
 
-  // set canonical link id
-  if (_sdfModel.CanonicalLink() != nullptr)
+  if (_sdfModel.LinkCount() > 0u)
   {
-    std::string canonicalLinkName = _sdfModel.CanonicalLinkName();
-    tpelib::Entity &canonicalLink = model->GetChildByName(canonicalLinkName);
-    model->SetCanonicalLink(canonicalLink.GetId());
-  }
-  else
-  {
-    model->SetCanonicalLink();
+    // 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;
@@ -185,16 +188,19 @@ Identity SDFFeatures::ConstructSdfNestedModel(
     this->ConstructSdfLink(modelIdentity, *_sdfModel.LinkByIndex(i));
   }
 
-  // set canonical link id
-  if (_sdfModel.CanonicalLink() != nullptr)
+  if (_sdfModel.LinkCount() > 0u)
   {
-    std::string canonicalLinkName = _sdfModel.CanonicalLinkName();
-    tpelib::Entity &canonicalLink = model->GetChildByName(canonicalLinkName);
-    model->SetCanonicalLink(canonicalLink.GetId());
-  }
-  else
-  {
-    model->SetCanonicalLink();
+    // 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