diff --git a/tpe/lib/src/Collision.cc b/tpe/lib/src/Collision.cc
index d17963882..6993474c3 100644
--- a/tpe/lib/src/Collision.cc
+++ b/tpe/lib/src/Collision.cc
@@ -99,3 +99,11 @@ Shape *Collision::GetShape() const
 {
   return this->dataPtr->shape.get();
 }
+
+//////////////////////////////////////////////////
+math::AxisAlignedBox Collision::GetBoundingBox(bool /*_force*/) // NOLINT
+{
+  if (this->dataPtr->shape)
+    return this->dataPtr->shape->GetBoundingBox();
+  return math::AxisAlignedBox();
+}
diff --git a/tpe/lib/src/Collision.hh b/tpe/lib/src/Collision.hh
index 30ad500ea..61b0786e0 100644
--- a/tpe/lib/src/Collision.hh
+++ b/tpe/lib/src/Collision.hh
@@ -18,6 +18,8 @@
 #ifndef IGNITION_PHYSICS_TPE_LIB_SRC_COLLISION_HH_
 #define IGNITION_PHYSICS_TPE_LIB_SRC_COLLISION_HH_
 
+#include <ignition/math/AxisAlignedBox.hh>
+
 #include "ignition/physics/tpelib/Export.hh"
 
 #include "Entity.hh"
@@ -60,6 +62,9 @@ class IGNITION_PHYSICS_TPELIB_VISIBLE Collision : public Entity
   /// \return shape of collision
   public: Shape *GetShape() const;
 
+  // Documentation inherited
+  public: math::AxisAlignedBox GetBoundingBox(bool _force) override;
+
   /// \brief Private data pointer class
   private: CollisionPrivate *dataPtr = nullptr;
 };
diff --git a/tpe/lib/src/Entity.cc b/tpe/lib/src/Entity.cc
index 4b170e451..b788cc361 100644
--- a/tpe/lib/src/Entity.cc
+++ b/tpe/lib/src/Entity.cc
@@ -16,6 +16,7 @@
 */
 
 #include "Entity.hh"
+#include "Utils.hh"
 
 /// \brief Private data class for entity
 class ignition::physics::tpelib::EntityPrivate
@@ -31,6 +32,12 @@ class ignition::physics::tpelib::EntityPrivate
 
   /// \brief Child entities
   public: std::map<std::size_t, std::shared_ptr<Entity>> children;
+
+  /// \brief Bounding Box
+  public: math::AxisAlignedBox bbox;
+
+  /// \brief Flag to indicate if bounding box changed
+  public: bool bboxDirty = true;
 };
 
 using namespace ignition;
@@ -55,6 +62,7 @@ Entity::Entity(const Entity &_other)
   this->dataPtr->name = _other.dataPtr->name;
   this->dataPtr->pose = _other.dataPtr->pose;
   this->dataPtr->children = _other.dataPtr->children;
+  this->dataPtr->bbox = _other.dataPtr->bbox;
 }
 
 //////////////////////////////////////////////////
@@ -184,6 +192,32 @@ size_t Entity::GetChildCount() const
   return this->dataPtr->children.size();
 }
 
+//////////////////////////////////////////////////
+math::AxisAlignedBox Entity::GetBoundingBox(bool _force)
+{
+  if (_force || this->dataPtr->bboxDirty)
+  {
+    this->UpdateBoundingBox(_force);
+    this->dataPtr->bboxDirty = false;
+  }
+  return this->dataPtr->bbox;
+}
+
+//////////////////////////////////////////////////
+void Entity::UpdateBoundingBox(bool _force)
+{
+  math::AxisAlignedBox box;
+  for (auto &it : this->dataPtr->children)
+  {
+    auto transformedBox =
+        transformAxisAlignedBox(it.second->GetBoundingBox(_force),
+        it.second->GetPose());
+    box.Merge(transformedBox);
+  }
+
+  this->dataPtr->bbox = box;
+}
+
 //////////////////////////////////////////////////
 std::map<std::size_t, std::shared_ptr<Entity>> &Entity::GetChildren() const
 {
diff --git a/tpe/lib/src/Entity.hh b/tpe/lib/src/Entity.hh
index 998903fbb..b14be285d 100644
--- a/tpe/lib/src/Entity.hh
+++ b/tpe/lib/src/Entity.hh
@@ -23,6 +23,7 @@
 #include <memory>
 #include <string>
 
+#include <ignition/math/AxisAlignedBox.hh>
 #include <ignition/math/Pose3.hh>
 #include "ignition/physics/tpelib/Export.hh"
 
@@ -116,11 +117,20 @@ class IGNITION_PHYSICS_TPELIB_VISIBLE Entity
   /// \return Number of children
   public: virtual size_t GetChildCount() const;
 
+  /// \brief Get bounding box of entity
+  /// \param[in] _force True to force update bounding box
+  /// \return Entity bounding box
+  public: virtual math::AxisAlignedBox GetBoundingBox(bool _force = false);
+
   /// \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()
       const;
 
+  /// \brief Update the entity bounding box
+  /// \param[in] _force True to force update children's bounding box
+  private: virtual void UpdateBoundingBox(bool _force = false);
+
   /// \brief An invalid vertex.
   public: static Entity kNullEntity;
 
diff --git a/tpe/lib/src/Model_TEST.cc b/tpe/lib/src/Model_TEST.cc
index 8ba0844f1..4e4e72bc0 100644
--- a/tpe/lib/src/Model_TEST.cc
+++ b/tpe/lib/src/Model_TEST.cc
@@ -17,8 +17,10 @@
 
 #include <gtest/gtest.h>
 
+#include "Collision.hh"
 #include "Link.hh"
 #include "Model.hh"
+#include "Shape.hh"
 
 using namespace ignition;
 using namespace physics;
@@ -102,3 +104,52 @@ TEST(Model, Link)
   EXPECT_EQ(Entity::kNullEntity.GetId(), nullEnt.GetId());
 }
 
+/////////////////////////////////////////////////
+TEST(Model, BoundingBox)
+{
+  Model model;
+  EXPECT_EQ(0u, model.GetChildCount());
+  EXPECT_EQ(math::AxisAlignedBox(), model.GetBoundingBox());
+
+  // add link with sphere collision shape
+  Entity &linkEnt = model.AddLink();
+  linkEnt.SetPose(math::Pose3d(0, 0, 1, 0, 0, 0));
+  EXPECT_EQ(math::AxisAlignedBox(), linkEnt.GetBoundingBox());
+  EXPECT_EQ(math::AxisAlignedBox(), model.GetBoundingBox());
+
+  Link *link = static_cast<Link *>(&linkEnt);
+  Entity &collisionEnt = link->AddCollision();
+  Collision *collision = static_cast<Collision *>(&collisionEnt);
+  SphereShape sphereShape;
+  sphereShape.SetRadius(2.0);
+  collision->SetShape(sphereShape);
+
+  math::AxisAlignedBox expectedBoxLinkFrame(
+      math::Vector3d(-2, -2, -2), math::Vector3d(2, 2, 2));
+  EXPECT_EQ(expectedBoxLinkFrame, linkEnt.GetBoundingBox(true));
+
+  math::AxisAlignedBox expectedBoxModelFrame(
+      math::Vector3d(-2, -2, -1), math::Vector3d(2, 2, 3));
+  EXPECT_EQ(expectedBoxModelFrame, model.GetBoundingBox(true));
+
+  // add another link with box collision shape
+  Entity &linkEnt2 = model.AddLink();
+  linkEnt2.SetPose(math::Pose3d(0, 1, 0, 0, 0, 0));
+  EXPECT_EQ(math::AxisAlignedBox(), linkEnt2.GetBoundingBox());
+  EXPECT_EQ(expectedBoxModelFrame, model.GetBoundingBox());
+
+  Link *link2 = static_cast<Link *>(&linkEnt2);
+  Entity &collisionEnt2 = link2->AddCollision();
+  Collision *collision2 = static_cast<Collision *>(&collisionEnt2);
+  BoxShape boxShape;
+  boxShape.SetSize(math::Vector3d(3, 4, 5));
+  collision2->SetShape(boxShape);
+
+  expectedBoxLinkFrame = math::AxisAlignedBox(
+      math::Vector3d(-1.5, -2, -2.5), math::Vector3d(1.5, 2, 2.5));
+  EXPECT_EQ(expectedBoxLinkFrame, linkEnt2.GetBoundingBox(true));
+
+  expectedBoxModelFrame = math::AxisAlignedBox(
+      math::Vector3d(-2, -2, -2.5), math::Vector3d(2, 3, 3));
+  EXPECT_EQ(expectedBoxModelFrame, model.GetBoundingBox(true));
+}
diff --git a/tpe/lib/src/Utils.cc b/tpe/lib/src/Utils.cc
new file mode 100644
index 000000000..cd1149faf
--- /dev/null
+++ b/tpe/lib/src/Utils.cc
@@ -0,0 +1,102 @@
+/*
+ * Copyright (C) 2020 Open Source Robotics Foundation
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ *
+*/
+
+#include "Utils.hh"
+
+namespace ignition {
+namespace physics {
+namespace tpelib {
+
+//////////////////////////////////////////////////
+math::AxisAlignedBox transformAxisAlignedBox(
+    const math::AxisAlignedBox &_box, const math::Pose3d &_pose)
+{
+  if (_box == math::AxisAlignedBox())
+    return _box;
+
+  // Transform the original axis aligned box by applying the pose
+  // transformation to all 8 corners. This results in a rotated box, which
+  // we need to fit a new axis aligned box to. We can achieve this by creating
+  // an empty axis aligned box and merging it with each corner of the rotated
+  // box. The result is a larger axis aligned box that surrounds the rotated box
+
+  // old min and max of original axis aligned box
+  math::Vector3d oldMin = _box.Min();
+  math::Vector3d oldMax = _box.Max();
+
+  // empty min max of new axis aligned box
+  // we will merge the empty min and max values with each of the
+  //  transformed corners
+  math::Vector3d newMin(math::MAX_D, math::MAX_D, math::MAX_D);
+  math::Vector3d newMax(math::LOW_D, math::LOW_D, math::LOW_D);
+
+  // min min min
+  // transform corner
+  math::Vector3d corner = oldMin;
+  auto v = _pose.Rot() * corner + _pose.Pos();
+  // merge min/max with transformed corner
+  newMin.Min(v);
+  newMax.Max(v);
+
+  // min min max
+  corner.Z() = oldMax.Z();
+  v = _pose.Rot() * corner + _pose.Pos();
+  newMin.Min(v);
+  newMax.Max(v);
+
+  // min max max
+  corner.Y() = oldMax.Y();
+  v = _pose.Rot() * corner + _pose.Pos();
+  newMin.Min(v);
+  newMax.Max(v);
+
+  // min max min
+  corner.Z() = oldMin.Z();
+  v = _pose.Rot() * corner + _pose.Pos();
+  newMin.Min(v);
+  newMax.Max(v);
+
+  // max max min
+  corner.X() = oldMax.X();
+  v = _pose.Rot() * corner + _pose.Pos();
+  newMin.Min(v);
+  newMax.Max(v);
+
+  // max max max
+  corner.Z() = oldMax.Z();
+  v = _pose.Rot() * corner + _pose.Pos();
+  newMin.Min(v);
+  newMax.Max(v);
+
+  // max min max
+  corner.Y() = oldMin.Y();
+  v = _pose.Rot() * corner + _pose.Pos();
+  newMin.Min(v);
+  newMax.Max(v);
+
+  // max min min
+  corner.Z() = oldMin.Z();
+  v = _pose.Rot() * corner + _pose.Pos();
+  newMin.Min(v);
+  newMax.Max(v);
+
+  return math::AxisAlignedBox(newMin, newMax);
+}
+
+}
+}
+}
diff --git a/tpe/lib/src/Utils.hh b/tpe/lib/src/Utils.hh
new file mode 100644
index 000000000..94416af77
--- /dev/null
+++ b/tpe/lib/src/Utils.hh
@@ -0,0 +1,40 @@
+/*
+ * Copyright (C) 2020 Open Source Robotics Foundation
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ *
+*/
+
+#include <ignition/math/AxisAlignedBox.hh>
+#include <ignition/math/Pose3.hh>
+
+#include "ignition/physics/tpelib/Export.hh"
+
+namespace ignition {
+namespace physics {
+namespace tpelib {
+
+  /// \brief Transform an axis aligned box by pose
+  /// \param[in] _box Axis aligned box to be transformed
+  /// \param[in] _pose Transform to be applied
+  /// \return New axis aligned box that surrounds the transformed version of
+  /// the old box
+  IGNITION_PHYSICS_TPELIB_VISIBLE
+  math::AxisAlignedBox transformAxisAlignedBox(
+      const math::AxisAlignedBox &_box, const math::Pose3d &_pose);
+}
+}
+}
+
+
+
diff --git a/tpe/lib/src/Utils_TEST.cc b/tpe/lib/src/Utils_TEST.cc
new file mode 100644
index 000000000..b332f5a39
--- /dev/null
+++ b/tpe/lib/src/Utils_TEST.cc
@@ -0,0 +1,55 @@
+/*
+ * Copyright (C) 2020 Open Source Robotics Foundation
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ *
+*/
+
+#include <gtest/gtest.h>
+
+#include "Utils.hh"
+
+using namespace ignition;
+using namespace physics;
+using namespace tpelib;
+
+/////////////////////////////////////////////////
+TEST(Utils, TransformAxisAlignedBox)
+{
+  math::Pose3d p(2, 3, 4, 0, IGN_PI * 0.5, 0);
+  math::AxisAlignedBox box;
+  math::AxisAlignedBox boxTransformed = transformAxisAlignedBox(box, p);
+  EXPECT_EQ(math::AxisAlignedBox(), boxTransformed);
+
+  // box2 centered at [0, 0, 0] with size [2, 4, 6]
+  math::AxisAlignedBox box2(
+      math::Vector3d(-1, -2, -3), math::Vector3d(1, 2, 3));
+
+  // translate box2
+  math::Pose3d poseNoRot(2, 2, 2, 0, 0, 0, 0);
+  math::AxisAlignedBox box2Transformed =
+      transformAxisAlignedBox(box2, poseNoRot);
+  EXPECT_EQ(math::Vector3d(2, 2, 2), box2Transformed.Center());
+  EXPECT_EQ(box2.Size(), box2Transformed.Size());
+  EXPECT_EQ(math::AxisAlignedBox(math::Vector3d(1, 0, -1),
+      math::Vector3d(3, 4, 5)), box2Transformed);
+
+  // translate and rotate box2
+  math::Pose3d pose(2, 2, 2, 0, IGN_PI * 0.5, 0);
+  math::AxisAlignedBox box2TransformedRot =
+      transformAxisAlignedBox(box2, pose);
+  EXPECT_EQ(math::Vector3d(2, 2, 2), box2TransformedRot.Center());
+  EXPECT_EQ((pose.Rot() * box2.Size()).Abs(), box2TransformedRot.Size());
+  EXPECT_EQ(math::AxisAlignedBox(math::Vector3d(-1, 0, 1),
+      math::Vector3d(5, 4, 3)), box2TransformedRot);
+}