diff --git a/tpe/lib/src/CollisionDetector.cc b/tpe/lib/src/CollisionDetector.cc
new file mode 100644
index 000000000..ef3f1f5ca
--- /dev/null
+++ b/tpe/lib/src/CollisionDetector.cc
@@ -0,0 +1,159 @@
+/*
+ * 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 "CollisionDetector.hh"
+#include "Utils.hh"
+
+using namespace ignition;
+using namespace physics;
+using namespace tpelib;
+
+
+//////////////////////////////////////////////////
+std::vector<Contact> CollisionDetector::CheckCollisions(
+    const std::map<std::size_t, std::shared_ptr<Entity>> &_entities,
+    bool _singleContact)
+{
+  // contacts to be filled and returned
+  std::vector<Contact> contacts;
+
+  // cache of axis aligned box in world frame
+  std::map<std::size_t, math::AxisAlignedBox> worldAabb;
+
+  for (auto it = _entities.begin(); it != _entities.end(); ++it)
+  {
+    // Get world axis aligned box for entity 1
+    math::AxisAlignedBox wb1;
+    std::shared_ptr<Entity> e1 = it->second;
+    auto wb1It = worldAabb.find(e1->GetId());
+    if (wb1It == worldAabb.end())
+    {
+      // get bbox in local frame
+      math::AxisAlignedBox b1 = e1->GetBoundingBox();
+      // convert to world aabb
+      math::Pose3d p1 = e1->GetPose();
+      wb1 = transformAxisAlignedBox(b1, p1);
+      worldAabb[e1->GetId()] = wb1;
+    }
+    else
+    {
+      wb1 = wb1It->second;
+    }
+
+    for (auto it2 = std::next(it, 1); it2 != _entities.end(); ++it2)
+    {
+      // Get world axis aligned box for entity 2
+      math::AxisAlignedBox wb2;
+      std::shared_ptr<Entity> e2 = it2->second;
+      auto wb2It = worldAabb.find(e2->GetId());
+      if (wb2It == worldAabb.end())
+      {
+        // get bbox in local frame
+        math::AxisAlignedBox b2 = e2->GetBoundingBox();
+        // convert to world aabb
+        math::Pose3d p2 = e2->GetPose();
+        wb2 = transformAxisAlignedBox(b2, p2);
+        worldAabb[e2->GetId()] = wb2;
+      }
+      else
+      {
+        wb2 = wb2It->second;
+      }
+
+      // Check intersection
+      std::vector<math::Vector3d> points;
+      if (this->GetIntersectionPoints(wb1, wb2, points, _singleContact))
+      {
+        Contact c;
+        // TPE checks collisions in the model level so contacts are associated
+        // with models and not collisions!
+        c.entity1 = e1->GetId();
+        c.entity2 = e2->GetId();
+        for (const auto &p : points)
+        {
+          c.point = p;
+          contacts.push_back(c);
+        }
+      }
+    }
+  }
+  return contacts;
+}
+
+//////////////////////////////////////////////////
+bool CollisionDetector::GetIntersectionPoints(const math::AxisAlignedBox &_b1,
+    const math::AxisAlignedBox &_b2,
+    std::vector<math::Vector3d> &_points, bool _singleContact)
+{
+  // fast intersection check
+  if (_b1.Intersects(_b2))
+  {
+    // when two boxes intersect, the overlapping region is a small box
+    // get all corners of this intersection box
+    math::Vector3d min;
+    math::Vector3d max;
+    min.X() = std::max(_b1.Min().X(), _b2.Min().X());
+    min.Y() = std::max(_b1.Min().Y(), _b2.Min().Y());
+    min.Z() = std::max(_b1.Min().Z(), _b2.Min().Z());
+
+    max.X() = std::min(_b1.Max().X(), _b2.Max().X());
+    max.Y() = std::min(_b1.Max().Y(), _b2.Max().Y());
+    max.Z() = std::min(_b1.Max().Z(), _b2.Max().Z());
+
+    if (_singleContact)
+    {
+      // return center of intersecting region
+      _points.push_back(min + 0.5*(max-min));
+      return true;
+    }
+
+    // min min min
+    math::Vector3d corner = min;
+    _points.push_back(corner);
+
+    // min min max
+    corner.Z() = max.Z();
+    _points.push_back(corner);
+
+    // min max max
+    corner.Y() = max.Y();
+    _points.push_back(corner);
+
+    // min max min
+    corner.Z() = min.Z();
+    _points.push_back(corner);
+
+    // max max min
+    corner.X() = max.X();
+    _points.push_back(corner);
+
+    // max max max
+    corner.Z() = max.Z();
+    _points.push_back(corner);
+
+    // max min max
+    corner.Y() = min.Y();
+    _points.push_back(corner);
+
+    // max min min
+    corner.Z() = min.Z();
+    _points.push_back(corner);
+
+    return true;
+  }
+  return false;
+}
diff --git a/tpe/lib/src/CollisionDetector.hh b/tpe/lib/src/CollisionDetector.hh
new file mode 100644
index 000000000..dd69c11a9
--- /dev/null
+++ b/tpe/lib/src/CollisionDetector.hh
@@ -0,0 +1,87 @@
+/*
+ * 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.
+ *
+*/
+
+#ifndef IGNITION_PHYSICS_TPE_LIB_SRC_COLLISIONDETECTOR_HH_
+#define IGNITION_PHYSICS_TPE_LIB_SRC_COLLISIONDETECTOR_HH_
+
+#include <map>
+#include <memory>
+#include <string>
+#include <vector>
+
+#include <ignition/math/Pose3.hh>
+#include <ignition/utilities/SuppressWarning.hh>
+
+#include "ignition/physics/tpelib/Export.hh"
+
+#include "Entity.hh"
+
+namespace ignition {
+namespace physics {
+namespace tpelib {
+
+/// \brief A data structure to store contact properties
+class IGNITION_PHYSICS_TPELIB_VISIBLE Contact
+{
+  /// \brief Id of frst collision entity
+  public: std::size_t entity1 = kNullEntityId;
+
+  /// \brief Id of second collision entity
+  public: std::size_t entity2 = kNullEntityId;
+
+  IGN_UTILS_WARN_IGNORE__DLL_INTERFACE_MISSING
+  /// \brief Point of contact in world frame;
+  public: math::Vector3d point;
+  IGN_UTILS_WARN_RESUME__DLL_INTERFACE_MISSING
+};
+
+/// \brief Collision Detector that checks collisions between a list of entities
+class IGNITION_PHYSICS_TPELIB_VISIBLE CollisionDetector
+{
+  /// \brief Constructor
+  public: CollisionDetector() = default;
+
+  /// \brief Destructor
+  public: ~CollisionDetector() = default;
+
+  /// \brief Check collisions between a list entities and get all contact points
+  /// \param[in] _entities List of entities
+  /// \param[in] _singleContact Get only 1 contact point for each pair of
+  /// collisions.
+  /// The contact point will be at the center of all points
+  /// \return A list of contact points
+  public: std::vector<Contact> CheckCollisions(
+      const std::map<std::size_t, std::shared_ptr<Entity>> &_entities,
+      bool _singleContact = false);
+
+  /// \brief Get a vector of intersection points between two axis aligned boxes
+  /// \param[in] _b1 Axis aligned box 1
+  /// \param[in] _b2 Axis aligned box 2
+  /// \param[out] _points Intersection points to be filled
+  /// \param[in] _singleContact Get only 1 intersection point at center of
+  /// all points
+  public: bool GetIntersectionPoints(const math::AxisAlignedBox &_b1,
+      const math::AxisAlignedBox &_b2,
+      std::vector<math::Vector3d> &_points,
+      bool _singleContact = false);
+};
+
+}
+}
+}
+
+#endif
diff --git a/tpe/lib/src/CollisionDetector_TEST.cc b/tpe/lib/src/CollisionDetector_TEST.cc
new file mode 100644
index 000000000..4b8439138
--- /dev/null
+++ b/tpe/lib/src/CollisionDetector_TEST.cc
@@ -0,0 +1,236 @@
+/*
+ * 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 <ignition/math/AxisAlignedBox.hh>
+
+#include "Collision.hh"
+#include "CollisionDetector.hh"
+#include "Model.hh"
+#include "Link.hh"
+#include "Shape.hh"
+
+using namespace ignition;
+using namespace physics;
+using namespace tpelib;
+
+/////////////////////////////////////////////////
+TEST(CollisionDetector, GetIntersectionPoints)
+{
+  CollisionDetector cd;
+
+  // get intersection points between two invalid boxes
+  math::AxisAlignedBox box1;
+  math::AxisAlignedBox box2;
+  std::vector<math::Vector3d> points;
+  EXPECT_FALSE(cd.GetIntersectionPoints(box1, box2, points));
+  EXPECT_TRUE(points.empty());
+
+  // get intersection points between two valid boxes
+  // there should be 8 intersection points representing the region of
+  // intersection
+  box1 = math::AxisAlignedBox(
+      math::Vector3d(-2, -2, -2), math::Vector3d(2, 2, 2));
+  box2 = math::AxisAlignedBox(
+      math::Vector3d(-1, -1, -1), math::Vector3d(3, 3, 3));
+  EXPECT_TRUE(cd.GetIntersectionPoints(box1, box2, points));
+  EXPECT_EQ(8u, points.size());
+
+  std::vector<math::Vector3d> expectedPoints;
+  expectedPoints.push_back(math::Vector3d(-1, -1, -1));
+  expectedPoints.push_back(math::Vector3d(-1, -1, 2));
+  expectedPoints.push_back(math::Vector3d(-1, 2, -1));
+  expectedPoints.push_back(math::Vector3d(2, -1, -1));
+  expectedPoints.push_back(math::Vector3d(2, 2, 2));
+  expectedPoints.push_back(math::Vector3d(2, 2, -1));
+  expectedPoints.push_back(math::Vector3d(2, -1, 2));
+  expectedPoints.push_back(math::Vector3d(-1, 2, 2));
+
+  // check against expected points and remove if found
+  for (const auto &p : points)
+  {
+    expectedPoints.erase(
+      std::remove(expectedPoints.begin(), expectedPoints.end(), p),
+      expectedPoints.end());
+  }
+  // all contact points should match expected points
+  EXPECT_TRUE(expectedPoints.empty());
+
+  // Test single contact point
+  points.clear();
+  EXPECT_TRUE(cd.GetIntersectionPoints(box1, box2, points, true));
+  EXPECT_EQ(1u, points.size());
+  EXPECT_EQ(math::Vector3d(0.5, 0.5, 0.5), points[0]);
+}
+
+/////////////////////////////////////////////////
+TEST(CollisionDetector, CheckCollisions)
+{
+  // set up entities for testing collision detection
+
+  // model A
+  std::shared_ptr<Model> modelA(new Model);
+  Entity &linkAEnt = modelA->AddLink();
+  Link *linkA = static_cast<Link *>(&linkAEnt);
+  Entity &collisionAEnt = linkA->AddCollision();
+  Collision *collisionA = static_cast<Collision *>(&collisionAEnt);
+  BoxShape boxShapeA;
+  boxShapeA.SetSize(ignition::math::Vector3d(4, 4, 4));
+  collisionA->SetShape(boxShapeA);
+
+  // model B
+  std::shared_ptr<Model> modelB(new Model);
+  Entity &linkBEnt = modelB->AddLink();
+  Link *linkB = static_cast<Link *>(&linkBEnt);
+  Entity &collisionBEnt = linkB->AddCollision();
+  Collision *collisionB = static_cast<Collision *>(&collisionBEnt);
+  SphereShape sphereShapeB;
+  sphereShapeB.SetRadius(5);
+  collisionB->SetShape(sphereShapeB);
+
+  // model C
+  std::shared_ptr<Model> modelC(new Model);
+  Entity &linkCEnt = modelC->AddLink();
+  Link *linkC = static_cast<Link *>(&linkCEnt);
+  Entity &collisionCEnt = linkC->AddCollision();
+  Collision *collisionC = static_cast<Collision *>(&collisionCEnt);
+  CylinderShape cylinderShapeC;
+  cylinderShapeC.SetRadius(2);
+  cylinderShapeC.SetLength(4);
+  collisionC->SetShape(cylinderShapeC);
+
+
+  // check collisions
+  CollisionDetector cd;
+  std::map<std::size_t, std::shared_ptr<Entity>> entities;
+  // verify no contacts if models are far apart
+  modelA->SetPose(math::Pose3d(100, 0, 0, 0, 0, 0));
+  modelB->SetPose(math::Pose3d(0, 0, 0, 0, 0, 0));
+  modelC->SetPose(math::Pose3d(-100, 0, 0, 0, 0, 0));
+  entities[modelA->GetId()] = modelA;
+  entities[modelB->GetId()] = modelB;
+  entities[modelC->GetId()] = modelC;
+
+  std::vector<Contact> contacts = cd.CheckCollisions(entities);
+  EXPECT_TRUE(contacts.empty());
+
+  // collision between model A and B but not model C
+  modelA->SetPose(math::Pose3d(2, 2, 2, 0, 0, 0));
+  modelB->SetPose(math::Pose3d(0, 0, 0, 0, 0, 0));
+  modelC->SetPose(math::Pose3d(100, 0, 0, 0, 0, 0));
+  contacts = cd.CheckCollisions(entities);
+  EXPECT_EQ(8u, contacts.size());
+
+  for (const auto &c : contacts)
+  {
+    EXPECT_TRUE(c.entity1 == modelA->GetId() || c.entity2 == modelA->GetId());
+    EXPECT_TRUE(c.entity1 == modelB->GetId() || c.entity2 == modelB->GetId());
+    EXPECT_TRUE(c.entity1 != modelC->GetId() && c.entity2 != modelC->GetId());
+    EXPECT_NE(c.entity1, c.entity2);
+  }
+  // check single contact point
+  contacts = cd.CheckCollisions(entities, true);
+  EXPECT_EQ(1u, contacts.size());
+  EXPECT_TRUE((contacts[0].entity1 == modelA->GetId()) ||
+      (contacts[0].entity2 == modelA->GetId()));
+  EXPECT_TRUE((contacts[0].entity1 == modelB->GetId()) ||
+      (contacts[0].entity2 == modelB->GetId()));
+  EXPECT_TRUE((contacts[0].entity1 != modelC->GetId()) &&
+      (contacts[0].entity2 != modelC->GetId()));
+
+  // collision between model A and C but not model B
+  modelA->SetPose(math::Pose3d(2, 1, 2, 0, 0, 0));
+  modelB->SetPose(math::Pose3d(100, 0, 0, 0, 0, 0));
+  modelC->SetPose(math::Pose3d(2, 3, 4, 0, 0, 0));
+  contacts = cd.CheckCollisions(entities);
+  EXPECT_EQ(8u, contacts.size());
+
+  for (const auto &c : contacts)
+  {
+    EXPECT_TRUE(c.entity1 == modelA->GetId() || c.entity2 == modelA->GetId());
+    EXPECT_TRUE(c.entity1 != modelB->GetId() && c.entity2 != modelB->GetId());
+    EXPECT_TRUE(c.entity1 == modelC->GetId() || c.entity2 == modelC->GetId());
+    EXPECT_NE(c.entity1, c.entity2);
+  }
+  // check single contact point
+  contacts = cd.CheckCollisions(entities, true);
+  EXPECT_EQ(1u, contacts.size());
+  EXPECT_TRUE((contacts[0].entity1 == modelA->GetId()) ||
+      (contacts[0].entity2 == modelA->GetId()));
+  EXPECT_TRUE((contacts[0].entity1 != modelB->GetId()) &&
+      (contacts[0].entity2 != modelB->GetId()));
+  EXPECT_TRUE((contacts[0].entity1 == modelC->GetId()) ||
+      (contacts[0].entity2 == modelC->GetId()));
+
+  // collision between model A and B, and B and C, but not A and C
+  modelA->SetPose(math::Pose3d(-2, -2, -2, 0, 0, 0));
+  modelB->SetPose(math::Pose3d(0, 0, 0, 0, 0, 0));
+  modelC->SetPose(math::Pose3d(3, 3, 3, 0, 0, 0));
+  contacts = cd.CheckCollisions(entities);
+  EXPECT_EQ(16u, contacts.size());
+
+  unsigned int contactAB = 0u;
+  unsigned int contactBC = 0u;
+  for (const auto &c : contacts)
+  {
+    if ((c.entity1 == modelA->GetId() || c.entity2 == modelA->GetId()) &&
+        (c.entity1 == modelB->GetId() || c.entity2 == modelB->GetId()))
+      contactAB++;
+    else if ((c.entity1 == modelB->GetId() || c.entity2 == modelB->GetId()) &&
+        (c.entity1 == modelC->GetId() || c.entity2 == modelC->GetId()))
+      contactBC++;
+    else if ((c.entity1 == modelA->GetId() || c.entity2 == modelA->GetId()) &&
+        (c.entity1 == modelC->GetId() || c.entity2 == modelC->GetId()))
+      FAIL() << "There should be no contacts between model A and C";
+  }
+  EXPECT_EQ(8u, contactAB);
+  EXPECT_EQ(8u, contactBC);
+
+  // check single contact point
+  contacts = cd.CheckCollisions(entities, true);
+  EXPECT_EQ(2u, contacts.size());
+
+  // collision between model A and B, B and C, and A and C
+  modelA->SetPose(math::Pose3d(-1, -1, -1, 0, 0, 0));
+  modelB->SetPose(math::Pose3d(0, 0, 0, 0, 0, 0));
+  modelC->SetPose(math::Pose3d(1, 1, 1, 0, 0, 0));
+  contacts = cd.CheckCollisions(entities);
+  EXPECT_EQ(24u, contacts.size());
+
+  contactAB = 0u;
+  contactBC = 0u;
+  unsigned int contactAC = 0u;
+  for (const auto &c : contacts)
+  {
+    if ((c.entity1 == modelA->GetId() || c.entity2 == modelA->GetId()) &&
+        (c.entity1 == modelB->GetId() || c.entity2 == modelB->GetId()))
+      contactAB++;
+    else if ((c.entity1 == modelB->GetId() || c.entity2 == modelB->GetId()) &&
+        (c.entity1 == modelC->GetId() || c.entity2 == modelC->GetId()))
+      contactBC++;
+    else if ((c.entity1 == modelA->GetId() || c.entity2 == modelA->GetId()) &&
+        (c.entity1 == modelC->GetId() || c.entity2 == modelC->GetId()))
+      contactAC++;
+  }
+  EXPECT_EQ(8u, contactAB);
+  EXPECT_EQ(8u, contactBC);
+  EXPECT_EQ(8u, contactAC);
+
+  // check single contact point
+  contacts = cd.CheckCollisions(entities, true);
+  EXPECT_EQ(3u, contacts.size());
+}
diff --git a/tpe/lib/src/World.cc b/tpe/lib/src/World.cc
index d83b62d7a..89ec520a1 100644
--- a/tpe/lib/src/World.cc
+++ b/tpe/lib/src/World.cc
@@ -72,6 +72,12 @@ void World::Step()
       model->GetAngularVelocity());
   }
 
+  // check colliisions
+  // the last bool arg tells the collision checker to return one single contact
+  // point for each pair of collisions
+  this->contacts = std::move(
+      this->collisionDetector.CheckCollisions(children, true));
+
   // increment world time by step size
   this->time += this->timeStep;
 }
@@ -84,3 +90,9 @@ Entity &World::AddModel()
     {modelId, std::make_shared<Model>(modelId)});
   return *it->second.get();
 }
+
+/////////////////////////////////////////////////
+std::vector<Contact> World::GetContacts() const
+{
+  return this->contacts;
+}
diff --git a/tpe/lib/src/World.hh b/tpe/lib/src/World.hh
index 498e6bf39..e8a5d9a44 100644
--- a/tpe/lib/src/World.hh
+++ b/tpe/lib/src/World.hh
@@ -18,8 +18,12 @@
 #ifndef IGNITION_PHYSICS_TPE_LIB_SRC_WORLD_HH_
 #define IGNITION_PHYSICS_TPE_LIB_SRC_WORLD_HH_
 
+#include <vector>
+#include <ignition/utilities/SuppressWarning.hh>
+
 #include "ignition/physics/tpelib/Export.hh"
 
+#include "CollisionDetector.hh"
 #include "Entity.hh"
 
 namespace ignition {
@@ -42,9 +46,11 @@ class IGNITION_PHYSICS_TPELIB_VISIBLE World : public Entity
   public: void SetTime(double _time);
 
   /// \brief Get the time of the world
-  /// \return double current time of the world
+  /// \return current time of the world
   public: double GetTime() const;
 
+  /// \brief Set the time step of the world
+  /// \param[in] _timestep time step to set to.
   public: void SetTimeStep(double _timeStep);
 
   /// \brief Get the timestep
@@ -58,11 +64,23 @@ class IGNITION_PHYSICS_TPELIB_VISIBLE World : public Entity
   /// \return Model added to the world
   public: Entity &AddModel();
 
+  /// \brief Get contacts from last step
+  /// \return Contacts from last step
+  public: std::vector<Contact> GetContacts() const;
+
   /// \brief World time
   protected: double time{0.0};
 
   /// \brief Time step size
   protected: double timeStep{0.1};
+
+  /// \brief Collision detector
+  protected: CollisionDetector collisionDetector;
+
+  IGN_UTILS_WARN_IGNORE__DLL_INTERFACE_MISSING
+  /// \brief list of contacts
+  protected: std::vector<Contact> contacts;
+  IGN_UTILS_WARN_RESUME__DLL_INTERFACE_MISSING
 };
 
 }  // namespace tpelib