diff --git a/tpe/lib/CMakeLists.txt b/tpe/lib/CMakeLists.txt
index d7dbb7fd0..ba370a5a3 100644
--- a/tpe/lib/CMakeLists.txt
+++ b/tpe/lib/CMakeLists.txt
@@ -9,6 +9,7 @@ target_link_libraries(${tpelib_target}
   PUBLIC
   PRIVATE
     ignition-common${IGN_COMMON_VER}::requested
+    ignition-math${IGN_MATH_VER}::eigen3
 )
  ign_build_tests(
   TYPE UNIT_tpelib
diff --git a/tpe/lib/src/Collision_TEST.cc b/tpe/lib/src/Collision_TEST.cc
index a2a66efb7..e81285ade 100644
--- a/tpe/lib/src/Collision_TEST.cc
+++ b/tpe/lib/src/Collision_TEST.cc
@@ -37,6 +37,16 @@ TEST(Collision, BasicAPI)
   collision.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), collision.GetPose());
 
+  Link link;
+  auto linkPose = math::Pose3d(1, 2, 3, 0.1, 0.2, 0.3);
+  link.SetPose(linkPose);
+  Entity &collisionEnt = link.AddCollision();
+  ASSERT_NE(nullptr, collisionEnt.GetParent());
+
+  collisionEnt.SetPose(math::Pose3d(0, 0.2, 0.5, 0, 1, 0));
+  EXPECT_EQ(math::Pose3d(3.06472, 2.2, 1.27944, 0.691965, 1.10889, 0.707449),
+            collisionEnt.GetWorldPose());
+
   EXPECT_EQ(0xFF, collision.GetCollideBitmask());
   collision.SetCollideBitmask(0x03);
   EXPECT_EQ(0x03, collision.GetCollideBitmask());
diff --git a/tpe/lib/src/Entity.cc b/tpe/lib/src/Entity.cc
index f2610f06a..ce99e2ddf 100644
--- a/tpe/lib/src/Entity.cc
+++ b/tpe/lib/src/Entity.cc
@@ -135,6 +135,15 @@ math::Pose3d Entity::GetPose() const
   return this->dataPtr->pose;
 }
 
+//////////////////////////////////////////////////
+math::Pose3d Entity::GetWorldPose() const
+{
+  if (this->dataPtr->parent)
+    return this->dataPtr->pose * this->dataPtr->parent->GetWorldPose();
+
+  return this->dataPtr->pose;
+}
+
 //////////////////////////////////////////////////
 void Entity::SetId(std::size_t _id)
 {
diff --git a/tpe/lib/src/Entity.hh b/tpe/lib/src/Entity.hh
index 8bdcfbc78..7112c1af5 100644
--- a/tpe/lib/src/Entity.hh
+++ b/tpe/lib/src/Entity.hh
@@ -91,9 +91,13 @@ class IGNITION_PHYSICS_TPELIB_VISIBLE Entity
   public: virtual void SetPose(const math::Pose3d &_pose);
 
   /// \brief Get the pose of the entity
-  /// \return Pose of entity to set to
+  /// \return Pose of entity
   public: virtual math::Pose3d GetPose() const;
 
+  /// \brief Get the world pose of the entity
+  /// \return World pose of entity
+  public: virtual math::Pose3d GetWorldPose() const;
+
   /// \brief Get a child entity by id
   /// \param[in] _id Id of child entity
   /// \return Child entity
diff --git a/tpe/lib/src/Link_TEST.cc b/tpe/lib/src/Link_TEST.cc
index 4fefc360f..351650d89 100644
--- a/tpe/lib/src/Link_TEST.cc
+++ b/tpe/lib/src/Link_TEST.cc
@@ -19,6 +19,7 @@
 
 #include "Collision.hh"
 #include "Link.hh"
+#include "Model.hh"
 
 using namespace ignition;
 using namespace physics;
@@ -37,6 +38,16 @@ TEST(Link, BasicAPI)
   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());
 
+  Model model;
+  auto modelPose = math::Pose3d(10, 0, 2, 1, 0, 0);
+  model.SetPose(modelPose);
+  Entity &linkEnt = model.AddLink();
+  ASSERT_NE(nullptr, linkEnt.GetParent());
+
+  linkEnt.SetPose(math::Pose3d(0, 0.2, 0.5, 0, 1, 0));
+  EXPECT_EQ(math::Pose3d(7.08596, 0.2, -6.83411, 1, 1, 0),
+            linkEnt.GetWorldPose());
+
   Link link2;
   EXPECT_NE(link.GetId(), link2.GetId());
 }
diff --git a/tpe/lib/src/Model_TEST.cc b/tpe/lib/src/Model_TEST.cc
index cd692e3bb..83f414c75 100644
--- a/tpe/lib/src/Model_TEST.cc
+++ b/tpe/lib/src/Model_TEST.cc
@@ -39,6 +39,7 @@ TEST(Model, BasicAPI)
 
   model.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), model.GetPose());
+  EXPECT_EQ(math::Pose3d(1, 2, 3, 0.1, 0.2, 0.3), model.GetWorldPose());
 
   model.SetLinearVelocity(math::Vector3d(.5, .5, .5));
   EXPECT_EQ(math::Vector3d(.5, .5, .5), model.GetLinearVelocity());
@@ -46,6 +47,9 @@ 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());
+  EXPECT_EQ(model.GetPose(), model.GetWorldPose());
+
   Model model2;
   EXPECT_NE(model.GetId(), model2.GetId());