diff --git a/tpe/lib/src/Collision.cc b/tpe/lib/src/Collision.cc
index cd1a6dadc..184b9510b 100644
--- a/tpe/lib/src/Collision.cc
+++ b/tpe/lib/src/Collision.cc
@@ -75,12 +75,24 @@ void Collision::SetShape(const Shape &_shape)
     const BoxShape *typedShape = static_cast<const BoxShape *>(&_shape);
     this->dataPtr->shape.reset(new BoxShape(*typedShape));
   }
+  else if (_shape.GetType() == ShapeType::CAPSULE)
+  {
+    const CapsuleShape *typedShape =
+      static_cast<const CapsuleShape *>(&_shape);
+    this->dataPtr->shape.reset(new CapsuleShape(*typedShape));
+  }
   else if (_shape.GetType() == ShapeType::CYLINDER)
   {
     const CylinderShape *typedShape =
       static_cast<const CylinderShape *>(&_shape);
     this->dataPtr->shape.reset(new CylinderShape(*typedShape));
   }
+  else if (_shape.GetType() == ShapeType::ELLIPSOID)
+  {
+    const EllipsoidShape *typedShape =
+      static_cast<const EllipsoidShape *>(&_shape);
+    this->dataPtr->shape.reset(new EllipsoidShape(*typedShape));
+  }
   else if (_shape.GetType() == ShapeType::SPHERE)
   {
     const SphereShape *typedShape = dynamic_cast<const SphereShape *>(&_shape);
diff --git a/tpe/lib/src/CollisionDetector_TEST.cc b/tpe/lib/src/CollisionDetector_TEST.cc
index 6305a6c03..9d2e99acf 100644
--- a/tpe/lib/src/CollisionDetector_TEST.cc
+++ b/tpe/lib/src/CollisionDetector_TEST.cc
@@ -113,6 +113,26 @@ TEST(CollisionDetector, CheckCollisions)
   cylinderShapeC.SetLength(4);
   collisionC->SetShape(cylinderShapeC);
 
+  // model D
+  std::shared_ptr<Model> modelD(new Model);
+  Entity &linkDEnt = modelD->AddLink();
+  Link *linkD = static_cast<Link *>(&linkDEnt);
+  Entity &collisionDEnt = linkD->AddCollision();
+  Collision *collisionD = static_cast<Collision *>(&collisionDEnt);
+  CapsuleShape capsuleShapeD;
+  capsuleShapeD.SetRadius(0.2);
+  capsuleShapeD.SetLength(0.6);
+  collisionD->SetShape(capsuleShapeD);
+
+  // model E
+  std::shared_ptr<Model> modelE(new Model);
+  Entity &linkEEnt = modelE->AddLink();
+  Link *linkE = static_cast<Link *>(&linkEEnt);
+  Entity &collisionEEnt = linkE->AddCollision();
+  Collision *collisionE = static_cast<Collision *>(&collisionEEnt);
+  EllipsoidShape ellipsoidShapeE;
+  ellipsoidShapeE.SetRadii({2, 2, 0.5});
+  collisionE->SetShape(ellipsoidShapeE);
 
   // check collisions
   CollisionDetector cd;
@@ -121,14 +141,18 @@ TEST(CollisionDetector, CheckCollisions)
   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));
+  modelD->SetPose(math::Pose3d(-200, 0, 0, 0, 0, 0));
+  modelE->SetPose(math::Pose3d(200, 0, 0, 0, 0, 0));
   entities[modelA->GetId()] = modelA;
   entities[modelB->GetId()] = modelB;
   entities[modelC->GetId()] = modelC;
+  entities[modelD->GetId()] = modelD;
+  entities[modelE->GetId()] = modelE;
 
   std::vector<Contact> contacts = cd.CheckCollisions(entities);
   EXPECT_TRUE(contacts.empty());
 
-  // collision between model A and B but not model C
+  // collision between model A and B but not model C, D and E
   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));
@@ -140,6 +164,8 @@ TEST(CollisionDetector, CheckCollisions)
     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_TRUE(c.entity1 != modelD->GetId() && c.entity2 != modelD->GetId());
+    EXPECT_TRUE(c.entity1 != modelE->GetId() && c.entity2 != modelE->GetId());
     EXPECT_NE(c.entity1, c.entity2);
   }
   // check single contact point
@@ -151,8 +177,12 @@ TEST(CollisionDetector, CheckCollisions)
       (contacts[0].entity2 == modelB->GetId()));
   EXPECT_TRUE((contacts[0].entity1 != modelC->GetId()) &&
       (contacts[0].entity2 != modelC->GetId()));
+  EXPECT_TRUE((contacts[0].entity1 != modelD->GetId()) &&
+      (contacts[0].entity2 != modelD->GetId()));
+  EXPECT_TRUE((contacts[0].entity1 != modelE->GetId()) &&
+      (contacts[0].entity2 != modelE->GetId()));
 
-  // collision between model A and C but not model B
+  // collision between model A and C but not model B, D and E
   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));
@@ -164,6 +194,8 @@ TEST(CollisionDetector, CheckCollisions)
     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_TRUE(c.entity1 != modelD->GetId() && c.entity2 != modelD->GetId());
+    EXPECT_TRUE(c.entity1 != modelE->GetId() && c.entity2 != modelE->GetId());
     EXPECT_NE(c.entity1, c.entity2);
   }
   // check single contact point
@@ -175,6 +207,10 @@ TEST(CollisionDetector, CheckCollisions)
       (contacts[0].entity2 != modelB->GetId()));
   EXPECT_TRUE((contacts[0].entity1 == modelC->GetId()) ||
       (contacts[0].entity2 == modelC->GetId()));
+  EXPECT_TRUE((contacts[0].entity1 != modelD->GetId()) &&
+      (contacts[0].entity2 != modelD->GetId()));
+  EXPECT_TRUE((contacts[0].entity1 != modelE->GetId()) &&
+      (contacts[0].entity2 != modelE->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));
@@ -196,6 +232,27 @@ TEST(CollisionDetector, CheckCollisions)
     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";
+    else if ((c.entity1 == modelA->GetId() || c.entity2 == modelA->GetId()) &&
+        (c.entity1 == modelD->GetId() || c.entity2 == modelD->GetId()))
+      FAIL() << "There should be no contacts between model A and D";
+    else if ((c.entity1 == modelA->GetId() || c.entity2 == modelA->GetId()) &&
+        (c.entity1 == modelE->GetId() || c.entity2 == modelE->GetId()))
+      FAIL() << "There should be no contacts between model A and E";
+    else if ((c.entity1 == modelB->GetId() || c.entity2 == modelB->GetId()) &&
+        (c.entity1 == modelD->GetId() || c.entity2 == modelD->GetId()))
+      FAIL() << "There should be no contacts between model B and D";
+    else if ((c.entity1 == modelB->GetId() || c.entity2 == modelB->GetId()) &&
+        (c.entity1 == modelE->GetId() || c.entity2 == modelE->GetId()))
+      FAIL() << "There should be no contacts between model B and E";
+    else if ((c.entity1 == modelC->GetId() || c.entity2 == modelC->GetId()) &&
+        (c.entity1 == modelD->GetId() || c.entity2 == modelD->GetId()))
+      FAIL() << "There should be no contacts between model C and D";
+    else if ((c.entity1 == modelC->GetId() || c.entity2 == modelC->GetId()) &&
+        (c.entity1 == modelE->GetId() || c.entity2 == modelE->GetId()))
+      FAIL() << "There should be no contacts between model C and E";
+    else if ((c.entity1 == modelD->GetId() || c.entity2 == modelD->GetId()) &&
+        (c.entity1 == modelE->GetId() || c.entity2 == modelE->GetId()))
+      FAIL() << "There should be no contacts between model D and E";
   }
   EXPECT_EQ(8u, contactAB);
   EXPECT_EQ(8u, contactBC);
@@ -240,6 +297,71 @@ TEST(CollisionDetector, CheckCollisions)
   EXPECT_EQ(8u, contacts.size());
   contacts = cd.CheckCollisions(entities, true);
   EXPECT_EQ(1u, contacts.size());
+
+  entities[modelC->GetId()] = modelC;
+  // collision between model E and B but not model A, C, D
+  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));
+  modelD->SetPose(math::Pose3d(-200, 0, 0, 0, 0, 0));
+  modelE->SetPose(math::Pose3d(5, 0, 0, 0, 0, 0));
+  contacts = cd.CheckCollisions(entities);
+  EXPECT_EQ(8u, contacts.size());
+
+  for (const auto &c : contacts)
+  {
+    EXPECT_TRUE(c.entity1 == modelE->GetId() || c.entity2 == modelE->GetId());
+    EXPECT_TRUE(c.entity1 == modelB->GetId() || c.entity2 == modelB->GetId());
+    EXPECT_TRUE(c.entity1 != modelA->GetId() && c.entity2 != modelA->GetId());
+    EXPECT_TRUE(c.entity1 != modelC->GetId() && c.entity2 != modelC->GetId());
+    EXPECT_TRUE(c.entity1 != modelD->GetId() && c.entity2 != modelD->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()));
+  EXPECT_TRUE((contacts[0].entity1 != modelD->GetId()) &&
+      (contacts[0].entity2 != modelD->GetId()));
+  EXPECT_TRUE((contacts[0].entity1 == modelE->GetId()) ||
+      (contacts[0].entity2 == modelE->GetId()));
+
+  // collision between model D and B but not model A, C, E
+  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));
+  modelD->SetPose(math::Pose3d(5, 0, 0, 0, 0, 0));
+  modelE->SetPose(math::Pose3d(200, 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_TRUE(c.entity1 == modelD->GetId() || c.entity2 == modelD->GetId());
+    EXPECT_TRUE(c.entity1 != modelE->GetId() && c.entity2 != modelE->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()));
+  EXPECT_TRUE((contacts[0].entity1 == modelD->GetId()) ||
+      (contacts[0].entity2 == modelD->GetId()));
+  EXPECT_TRUE((contacts[0].entity1 != modelE->GetId()) &&
+      (contacts[0].entity2 != modelE->GetId()));
 }
 
 /////////////////////////////////////////////////
diff --git a/tpe/lib/src/Collision_TEST.cc b/tpe/lib/src/Collision_TEST.cc
index d1bc6d1fa..80f104010 100644
--- a/tpe/lib/src/Collision_TEST.cc
+++ b/tpe/lib/src/Collision_TEST.cc
@@ -68,6 +68,20 @@ TEST(Collision, BoxShape)
       result->GetBoundingBox().Max());
 }
 
+/////////////////////////////////////////////////
+TEST(Collision, CapsuleShape)
+{
+  Collision collision;
+  CapsuleShape CapsuleShape;
+  CapsuleShape.SetRadius(2.0);
+  CapsuleShape.SetLength(3.0);
+  collision.SetShape(CapsuleShape);
+  auto result = collision.GetShape();
+  ASSERT_NE(nullptr, result);
+  EXPECT_EQ(ignition::math::Vector3d(2.0, 2.0, 3.5),
+      result->GetBoundingBox().Max());
+}
+
 /////////////////////////////////////////////////
 TEST(Collision, CylinderShape)
 {
@@ -82,6 +96,18 @@ TEST(Collision, CylinderShape)
       result->GetBoundingBox().Max());
 }
 
+/////////////////////////////////////////////////
+TEST(Collision, EllipsoidShape)
+{
+  Collision collision;
+  EllipsoidShape EllipsoidShape;
+  EllipsoidShape.SetRadii({1.0, 2.0, 1.3});
+  collision.SetShape(EllipsoidShape);
+  auto result = collision.GetShape();
+  ASSERT_NE(nullptr, result);
+  EXPECT_EQ(ignition::math::Vector3d(1.0, 2.0, 1.3),
+      result->GetBoundingBox().Max());}
+
 /////////////////////////////////////////////////
 TEST(Collision, SphereShape)
 {
diff --git a/tpe/lib/src/Shape.cc b/tpe/lib/src/Shape.cc
index 6337aebd3..abd2dbff8 100644
--- a/tpe/lib/src/Shape.cc
+++ b/tpe/lib/src/Shape.cc
@@ -97,6 +97,61 @@ void BoxShape::UpdateBoundingBox()
   this->bbox = math::AxisAlignedBox(-halfSize, halfSize);
 }
 
+//////////////////////////////////////////////////
+CapsuleShape::CapsuleShape() : Shape()
+{
+  this->type = ShapeType::CAPSULE;
+}
+
+//////////////////////////////////////////////////
+CapsuleShape::CapsuleShape(const CapsuleShape &_other)
+  : Shape()
+{
+  *this = _other;
+}
+
+//////////////////////////////////////////////////
+Shape &CapsuleShape::operator=(const Shape &_other)
+{
+  auto other = static_cast<const CapsuleShape *>(&_other);
+  this->radius = other->radius;
+  this->length = other->length;
+  return *this;
+}
+
+//////////////////////////////////////////////////
+double CapsuleShape::GetRadius() const
+{
+  return this->radius;
+}
+
+//////////////////////////////////////////////////
+void CapsuleShape::SetRadius(double _radius)
+{
+  this->radius = _radius;
+  this->dirty = true;
+}
+
+//////////////////////////////////////////////////
+double CapsuleShape::GetLength() const
+{
+  return this->length;
+}
+
+//////////////////////////////////////////////////
+void CapsuleShape::SetLength(double _length)
+{
+  this->length = _length;
+  this->dirty = true;
+}
+
+//////////////////////////////////////////////////
+void CapsuleShape::UpdateBoundingBox()
+{
+  math::Vector3d halfSize(this->radius, this->radius, this->length*0.5 + this->radius);
+  this->bbox = math::AxisAlignedBox(-halfSize, halfSize);
+}
+
 //////////////////////////////////////////////////
 CylinderShape::CylinderShape() : Shape()
 {
@@ -152,6 +207,47 @@ void CylinderShape::UpdateBoundingBox()
   this->bbox = math::AxisAlignedBox(-halfSize, halfSize);
 }
 
+//////////////////////////////////////////////////
+EllipsoidShape::EllipsoidShape() : Shape()
+{
+  this->type = ShapeType::ELLIPSOID;
+}
+
+//////////////////////////////////////////////////
+EllipsoidShape::EllipsoidShape(const EllipsoidShape &_other)
+  : Shape()
+{
+  *this = _other;
+}
+
+//////////////////////////////////////////////////
+Shape &EllipsoidShape::operator=(const Shape &_other)
+{
+  auto other = static_cast<const EllipsoidShape *>(&_other);
+  this->radii = other->radii;
+  return *this;
+}
+
+//////////////////////////////////////////////////
+math::Vector3d EllipsoidShape::GetRadii() const
+{
+  return this->radii;
+}
+
+//////////////////////////////////////////////////
+void EllipsoidShape::SetRadii(const math::Vector3d &_radii)
+{
+  this->radii = _radii;
+  this->dirty = true;
+}
+
+//////////////////////////////////////////////////
+void EllipsoidShape::UpdateBoundingBox()
+{
+  math::Vector3d halfSize(this->radii.X(), this->radii.Y(), this->radii.Z());
+  this->bbox = math::AxisAlignedBox(-halfSize, halfSize);
+}
+
 //////////////////////////////////////////////////
 SphereShape::SphereShape() : Shape()
 {
@@ -245,5 +341,3 @@ void MeshShape::UpdateBoundingBox()
   this->bbox = math::AxisAlignedBox(
       this->scale * this->meshAABB.Min(), this->scale * this->meshAABB.Max());
 }
-
-
diff --git a/tpe/lib/src/Shape.hh b/tpe/lib/src/Shape.hh
index a79042ea8..406f6f35c 100644
--- a/tpe/lib/src/Shape.hh
+++ b/tpe/lib/src/Shape.hh
@@ -53,6 +53,12 @@ enum class IGNITION_PHYSICS_TPELIB_VISIBLE ShapeType
 
   /// \brief A mesh shape.
   MESH = 5,
+
+  /// \brief A capsule shape.
+  CAPSULE = 6,
+
+  /// \brief A ellipsoid shape.
+  ELLIPSOID = 7,
 };
 
 
@@ -120,6 +126,49 @@ class IGNITION_PHYSICS_TPELIB_VISIBLE BoxShape : public Shape
   IGN_UTILS_WARN_RESUME__DLL_INTERFACE_MISSING
 };
 
+/// \brief Capsule geometry
+class IGNITION_PHYSICS_TPELIB_VISIBLE CapsuleShape : public Shape
+{
+  /// \brief Constructor
+  public: CapsuleShape();
+
+  /// \brief Copy Constructor
+  /// \param[in] _other shape to copy from
+  public: CapsuleShape(const CapsuleShape &_other);
+
+  /// \brief Destructor
+  public: ~CapsuleShape() = default;
+
+  /// \brief Assignment operator
+  /// \param[in] _other shape to copy from
+  public: Shape &operator=(const Shape &_other);
+
+  /// \brief Get capsule radius
+  /// \return capsule radius
+  public: double GetRadius() const;
+
+  /// \brief Set capsule radius
+  /// \param[in] _radius Cylinder radius
+  public: void SetRadius(double _radius);
+
+  /// \brief Get capsule length
+  /// \return Capsule length
+  public: double GetLength() const;
+
+  /// \brief Set capsule length
+  /// \param[in] _length Cylinder length
+  public: void SetLength(double _length);
+
+  // Documentation inherited
+  protected: virtual void UpdateBoundingBox() override;
+
+  /// \brief Capsule radius
+  private: double radius = 0.0;
+
+  /// \brief Capsule length
+  private: double length = 0.0;
+};
+
 /// \brief Cylinder geometry
 class IGNITION_PHYSICS_TPELIB_VISIBLE CylinderShape : public Shape
 {
@@ -163,6 +212,40 @@ class IGNITION_PHYSICS_TPELIB_VISIBLE CylinderShape : public Shape
   private: double length = 0.0;
 };
 
+/// \brief Ellipsoid geometry
+class IGNITION_PHYSICS_TPELIB_VISIBLE EllipsoidShape : public Shape
+{
+  /// \brief Constructor
+  public: EllipsoidShape();
+
+  /// \brief Copy Constructor
+  /// \param[in] _other shape to copy from
+  public: EllipsoidShape(const EllipsoidShape &_other);
+
+  /// \brief Destructor
+  public: ~EllipsoidShape() = default;
+
+  /// \brief Assignment operator
+  /// \param[in] _other shape to copy from
+  public: Shape &operator=(const Shape &_other);
+
+  /// \brief Get ellipsoid radius
+  /// \return ellipsoid radius
+  public: math::Vector3d GetRadii() const;
+
+  /// \brief Set ellipsoid radius
+  /// \param[in] _radius ellipsoid radius
+  public: void SetRadii(const math::Vector3d &_radii);
+
+  // Documentation inherited
+  protected: virtual void UpdateBoundingBox() override;
+
+  IGN_UTILS_WARN_IGNORE__DLL_INTERFACE_MISSING
+  /// \brief ellipsoid radius
+  private: math::Vector3d radii = math::Vector3d::Zero;
+  IGN_UTILS_WARN_RESUME__DLL_INTERFACE_MISSING
+};
+
 /// \brief Sphere geometry
 class IGNITION_PHYSICS_TPELIB_VISIBLE SphereShape : public Shape
 {
diff --git a/tpe/lib/src/Shape_TEST.cc b/tpe/lib/src/Shape_TEST.cc
index 767402b6f..251cf52d4 100644
--- a/tpe/lib/src/Shape_TEST.cc
+++ b/tpe/lib/src/Shape_TEST.cc
@@ -45,6 +45,28 @@ TEST(Shape, BoxShape)
   EXPECT_EQ(math::Vector3d(0.6, 1.8, 2.9), bbox.Max());
 }
 
+/////////////////////////////////////////////////
+TEST(Shape, CapsuleShape)
+{
+  CapsuleShape shape;
+  EXPECT_EQ(ShapeType::CAPSULE, shape.GetType());
+  math::AxisAlignedBox empty = shape.GetBoundingBox();
+  EXPECT_EQ(math::Vector3d::Zero, empty.Center());
+  EXPECT_EQ(math::Vector3d::Zero, empty.Size());
+
+  double radius = 0.6;
+  double length = 2.8;
+  shape.SetRadius(radius);
+  EXPECT_NEAR(radius, shape.GetRadius(), 1e-6);
+  shape.SetLength(length);
+  EXPECT_NEAR(length, shape.GetLength(), 1e-6);
+  math::AxisAlignedBox bbox = shape.GetBoundingBox();
+  EXPECT_EQ(math::Vector3d::Zero, bbox.Center());
+  EXPECT_EQ(math::Vector3d(1.2, 1.2, 4.0), bbox.Size());
+  EXPECT_EQ(math::Vector3d(-0.6, -0.6, -2.0), bbox.Min());
+  EXPECT_EQ(math::Vector3d(0.6, 0.6, 2.0), bbox.Max());
+}
+
 /////////////////////////////////////////////////
 TEST(Shape, CylinderShape)
 {
@@ -67,6 +89,25 @@ TEST(Shape, CylinderShape)
   EXPECT_EQ(math::Vector3d(0.6, 0.6, 1.4), bbox.Max());
 }
 
+/////////////////////////////////////////////////
+TEST(Shape, EllipsoidShape)
+{
+  EllipsoidShape shape;
+  EXPECT_EQ(ShapeType::ELLIPSOID, shape.GetType());
+  math::AxisAlignedBox empty = shape.GetBoundingBox();
+  EXPECT_EQ(math::Vector3d::Zero, empty.Center());
+  EXPECT_EQ(math::Vector3d::Zero, empty.Size());
+
+  math::Vector3d radii(0.5, 0.6, 0.7);
+  shape.SetRadii(radii);
+  EXPECT_EQ(radii, shape.GetRadii());
+  math::AxisAlignedBox bbox = shape.GetBoundingBox();
+  EXPECT_EQ(math::Vector3d::Zero, bbox.Center());
+  EXPECT_EQ(math::Vector3d(1.0, 1.2, 1.4), bbox.Size());
+  EXPECT_EQ(math::Vector3d(-0.5, -0.6, -0.7), bbox.Min());
+  EXPECT_EQ(math::Vector3d(0.5, 0.6, 0.7), bbox.Max());
+}
+
 /////////////////////////////////////////////////
 TEST(Shape, SphereShape)
 {
diff --git a/tpe/plugin/src/SDFFeatures.cc b/tpe/plugin/src/SDFFeatures.cc
index 10fba3159..3fc92482f 100644
--- a/tpe/plugin/src/SDFFeatures.cc
+++ b/tpe/plugin/src/SDFFeatures.cc
@@ -18,7 +18,9 @@
 #include "SDFFeatures.hh"
 
 #include <sdf/Box.hh>
+#include <sdf/Capsule.hh>
 #include <sdf/Cylinder.hh>
+#include <sdf/Ellipsoid.hh>
 #include <sdf/Sphere.hh>
 #include <sdf/Geometry.hh>
 #include <sdf/World.hh>
@@ -282,6 +284,14 @@ Identity SDFFeatures::ConstructSdfCollision(
     shape.SetSize(boxSdf->Size());
     collision->SetShape(shape);
   }
+  else if (geom->Type() == ::sdf::GeometryType::CAPSULE)
+  {
+    const auto capsuleSdf = geom->CapsuleShape();
+    tpelib::CapsuleShape shape;
+    shape.SetRadius(capsuleSdf->Radius());
+    shape.SetLength(capsuleSdf->Length());
+    collision->SetShape(shape);
+  }
   else if (geom->Type() == ::sdf::GeometryType::CYLINDER)
   {
     const auto cylinderSdf = geom->CylinderShape();
@@ -290,6 +300,13 @@ Identity SDFFeatures::ConstructSdfCollision(
     shape.SetLength(cylinderSdf->Length());
     collision->SetShape(shape);
   }
+  else if (geom->Type() == ::sdf::GeometryType::ELLIPSOID)
+  {
+    const auto ellipsoidSdf = geom->EllipsoidShape();
+    tpelib::EllipsoidShape shape;
+    shape.SetRadii(ellipsoidSdf->Radii());
+    collision->SetShape(shape);
+  }
   else if (geom->Type() == ::sdf::GeometryType::SPHERE)
   {
     const auto sphereSdf = geom->SphereShape();
diff --git a/tpe/plugin/src/ShapeFeatures.cc b/tpe/plugin/src/ShapeFeatures.cc
index 2fd585ad1..e26fc7f68 100644
--- a/tpe/plugin/src/ShapeFeatures.cc
+++ b/tpe/plugin/src/ShapeFeatures.cc
@@ -95,6 +95,83 @@ Identity ShapeFeatures::CastToCylinderShape(const Identity &_shapeID) const
   return this->GenerateInvalidId();
 }
 
+/////////////////////////////////////////////////
+Identity ShapeFeatures::CastToCapsuleShape(const Identity &_shapeID) const
+{
+  auto it = this->collisions.find(_shapeID);
+  if (it != this->collisions.end() && it->second != nullptr)
+  {
+    auto *shape = it->second->collision->GetShape();
+    if (shape != nullptr && dynamic_cast<tpelib::CapsuleShape*>(shape))
+      return this->GenerateIdentity(_shapeID, it->second);
+  }
+  return this->GenerateInvalidId();
+}
+
+/////////////////////////////////////////////////
+double ShapeFeatures::GetCapsuleShapeRadius(
+  const Identity &_capsuleID) const
+{
+  // assume _capsuleID ~= _collisionID
+  auto it = this->collisions.find(_capsuleID);
+  if (it != this->collisions.end() && it->second != nullptr)
+  {
+    auto *shape = it->second->collision->GetShape();
+    if (shape != nullptr)
+    {
+      auto *capsule = static_cast<tpelib::CapsuleShape*>(shape);
+      return capsule->GetRadius();
+    }
+  }
+  // return invalid radius if no collision found
+  return -1.0;
+}
+
+/////////////////////////////////////////////////
+double ShapeFeatures::GetCapsuleShapeLength(
+  const Identity &_capsuleID) const
+{
+  // assume _capsuleID ~= _collisionID
+  auto it = this->collisions.find(_capsuleID);
+  if (it != this->collisions.end() && it->second != nullptr)
+  {
+    auto *shape = it->second->collision->GetShape();
+    if (shape != nullptr)
+    {
+      auto *capsule = static_cast<tpelib::CapsuleShape*>(shape);
+      return capsule->GetLength();
+    }
+  }
+  // return invalid height if no collision found
+  return -1.0;
+}
+
+/////////////////////////////////////////////////
+Identity ShapeFeatures::AttachCapsuleShape(
+  const Identity &_linkID,
+  const std::string &_name,
+  const double _radius,
+  const double _length,
+  const Pose3d &_pose)
+{
+  auto it = this->links.find(_linkID);
+  if (it != this->links.end() && it->second != nullptr)
+  {
+    auto &collision = static_cast<tpelib::Collision&>(
+      it->second->link->AddCollision());
+    collision.SetName(_name);
+    collision.SetPose(math::eigen3::convert(_pose));
+
+    tpelib::CapsuleShape capsuleshape;
+    capsuleshape.SetRadius(_radius);
+    capsuleshape.SetLength(_length);
+    collision.SetShape(capsuleshape);
+
+    return this->AddCollision(_linkID, collision);
+  }
+  return this->GenerateInvalidId();
+}
+
 /////////////////////////////////////////////////
 double ShapeFeatures::GetCylinderShapeRadius(
   const Identity &_cylinderID) const
@@ -159,6 +236,62 @@ Identity ShapeFeatures::AttachCylinderShape(
   return this->GenerateInvalidId();
 }
 
+/////////////////////////////////////////////////
+Identity ShapeFeatures::CastToEllipsoidShape(const Identity &_shapeID) const
+{
+  auto it = this->collisions.find(_shapeID);
+  if (it != this->collisions.end() && it->second != nullptr)
+  {
+    auto *shape = it->second->collision->GetShape();
+    if (shape != nullptr && dynamic_cast<tpelib::EllipsoidShape*>(shape))
+      return this->GenerateIdentity(_shapeID, it->second);
+  }
+  return this->GenerateInvalidId();
+}
+
+/////////////////////////////////////////////////
+Vector3d ShapeFeatures::GetEllipsoidShapeRadii(
+  const Identity &_capsuleID) const
+{
+  // assume _capsuleID ~= _collisionID
+  auto it = this->collisions.find(_capsuleID);
+  if (it != this->collisions.end() && it->second != nullptr)
+  {
+    auto *shape = it->second->collision->GetShape();
+    if (shape != nullptr)
+    {
+      auto *capsule = static_cast<tpelib::EllipsoidShape*>(shape);
+      return math::eigen3::convert(capsule->GetRadii());
+    }
+  }
+  // return invalid radius if no collision found
+  return math::eigen3::convert(math::Vector3d(-1.0, -1.0, -1.0));
+}
+
+/////////////////////////////////////////////////
+Identity ShapeFeatures::AttachEllipsoidShape(
+  const Identity &_linkID,
+  const std::string &_name,
+  Vector3d _radii,
+  const Pose3d &_pose)
+{
+  auto it = this->links.find(_linkID);
+  if (it != this->links.end() && it->second != nullptr)
+  {
+    auto &collision = static_cast<tpelib::Collision&>(
+      it->second->link->AddCollision());
+    collision.SetName(_name);
+    collision.SetPose(math::eigen3::convert(_pose));
+
+    tpelib::EllipsoidShape capsuleshape;
+    capsuleshape.SetRadii(math::eigen3::convert(_radii));
+    collision.SetShape(capsuleshape);
+
+    return this->AddCollision(_linkID, collision);
+  }
+  return this->GenerateInvalidId();
+}
+
 /////////////////////////////////////////////////
 Identity ShapeFeatures::CastToSphereShape(
   const Identity &_shapeID) const
diff --git a/tpe/plugin/src/ShapeFeatures.hh b/tpe/plugin/src/ShapeFeatures.hh
index 5fca7de61..8cce3f138 100644
--- a/tpe/plugin/src/ShapeFeatures.hh
+++ b/tpe/plugin/src/ShapeFeatures.hh
@@ -22,7 +22,9 @@
 
 #include <ignition/physics/Shape.hh>
 #include <ignition/physics/BoxShape.hh>
+#include <ignition/physics/CapsuleShape.hh>
 #include <ignition/physics/CylinderShape.hh>
+#include <ignition/physics/EllipsoidShape.hh>
 #include <ignition/physics/mesh/MeshShape.hh>
 #include <ignition/physics/SphereShape.hh>
 
@@ -37,9 +39,15 @@ struct ShapeFeatureList : FeatureList<
   AttachBoxShapeFeature,
   GetShapeBoundingBox,
 
+  GetCapsuleShapeProperties,
+  AttachCapsuleShapeFeature,
+
   GetCylinderShapeProperties,
   AttachCylinderShapeFeature,
 
+  GetEllipsoidShapeProperties,
+  AttachEllipsoidShapeFeature,
+
   GetSphereShapeProperties,
   AttachSphereShapeFeature,
 
@@ -64,6 +72,22 @@ class ShapeFeatures :
     const LinearVector3d &_size,
     const Pose3d &_pose) override;
 
+  // ----- Capsule Features -----
+  public: Identity CastToCapsuleShape(
+    const Identity &_shapeID) const override;
+
+  public: double GetCapsuleShapeRadius(
+    const Identity &_capsuleID) const override;
+
+  public: double GetCapsuleShapeLength(
+    const Identity &_capsuleID) const override;
+
+  public: Identity AttachCapsuleShape(
+    const Identity &_linkID,
+    const std::string &_name,
+    double _radius,
+    double _height,
+    const Pose3d &_pose) override;
 
   // ----- Cylinder Features -----
   public: Identity CastToCylinderShape(
@@ -82,6 +106,18 @@ class ShapeFeatures :
     double _height,
     const Pose3d &_pose) override;
 
+  // ----- Capsule Features -----
+  public: Identity CastToEllipsoidShape(
+    const Identity &_shapeID) const override;
+
+  public: Vector3d GetEllipsoidShapeRadii(
+    const Identity &_capsuleID) const override;
+
+  public: Identity AttachEllipsoidShape(
+    const Identity &_linkID,
+    const std::string &_name,
+    Vector3d _radii,
+    const Pose3d &_pose) override;
 
   // ----- Sphere Features -----
   public: Identity CastToSphereShape(
diff --git a/tpe/plugin/src/SimulationFeatures_TEST.cc b/tpe/plugin/src/SimulationFeatures_TEST.cc
index 9901d09b1..85befd8c8 100644
--- a/tpe/plugin/src/SimulationFeatures_TEST.cc
+++ b/tpe/plugin/src/SimulationFeatures_TEST.cc
@@ -204,6 +204,33 @@ TEST_P(SimulationFeatures_TEST, ShapeFeatures)
     EXPECT_EQ(2u, cylinderLink->GetShapeCount());
     EXPECT_EQ(cylinder2, cylinderLink->GetShape(1));
 
+    auto ellipsoid = world->GetModel("ellipsoid");
+    auto ellipsoidLink = ellipsoid->GetLink(0);
+    auto ellipsoidCollision = ellipsoidLink->GetShape(0);
+    auto ellipsoidShape = ellipsoidCollision->CastToEllipsoidShape();
+    EXPECT_EQ(
+      ignition::math::Vector3d(0.2, 0.3, 0.5),
+      ignition::math::eigen3::convert(ellipsoidShape->GetRadii()));
+
+    auto ellipsoid2 = ellipsoidLink->AttachEllipsoidShape(
+      "ellipsoid2",
+      ignition::math::eigen3::convert(ignition::math::Vector3d(0.2, 0.3, 0.5)),
+      Eigen::Isometry3d::Identity());
+    EXPECT_EQ(2u, ellipsoidLink->GetShapeCount());
+    EXPECT_EQ(ellipsoid2, ellipsoidLink->GetShape(1));
+
+    auto capsule = world->GetModel("capsule");
+    auto capsuleLink = capsule->GetLink(0);
+    auto capsuleCollision = capsuleLink->GetShape(0);
+    auto capsuleShape = capsuleCollision->CastToCapsuleShape();
+    EXPECT_NEAR(0.2, capsuleShape->GetRadius(), 1e-6);
+    EXPECT_NEAR(0.6, capsuleShape->GetLength(), 1e-6);
+
+    auto capsule2 = capsuleLink->AttachCapsuleShape(
+      "capsule2", 0.2, 0.6, Eigen::Isometry3d::Identity());
+    EXPECT_EQ(2u, capsuleLink->GetShapeCount());
+    EXPECT_EQ(capsule2, capsuleLink->GetShape(1));
+
     // Test the bounding boxes in the local frames
     auto sphereAABB =
       sphereCollision->GetAxisAlignedBoundingBox(*sphereCollision);
@@ -211,6 +238,10 @@ TEST_P(SimulationFeatures_TEST, ShapeFeatures)
       boxCollision->GetAxisAlignedBoundingBox(*boxCollision);
     auto cylinderAABB =
       cylinderCollision->GetAxisAlignedBoundingBox(*cylinderCollision);
+    auto ellipsoidAABB =
+      ellipsoidCollision->GetAxisAlignedBoundingBox(*ellipsoidCollision);
+    auto capsuleAABB =
+      capsuleCollision->GetAxisAlignedBoundingBox(*capsuleCollision);
 
     EXPECT_EQ(ignition::math::Vector3d(-1, -1, -1),
               ignition::math::eigen3::convert(sphereAABB).Min());
@@ -224,11 +255,21 @@ TEST_P(SimulationFeatures_TEST, ShapeFeatures)
               ignition::math::eigen3::convert(cylinderAABB).Min());
     EXPECT_EQ(ignition::math::Vector3d(0.5, 0.5, 0.55),
               ignition::math::eigen3::convert(cylinderAABB).Max());
+    EXPECT_EQ(ignition::math::Vector3d(-0.2, -0.3, -0.5),
+              ignition::math::eigen3::convert(ellipsoidAABB).Min());
+    EXPECT_EQ(ignition::math::Vector3d(0.2, 0.3, 0.5),
+              ignition::math::eigen3::convert(ellipsoidAABB).Max());
+    EXPECT_EQ(ignition::math::Vector3d(-0.2, -0.2, -0.5),
+              ignition::math::eigen3::convert(capsuleAABB).Min());
+    EXPECT_EQ(ignition::math::Vector3d(0.2, 0.2, 0.5),
+              ignition::math::eigen3::convert(capsuleAABB).Max());
 
     // check model AABB. By default, the AABBs are in world frame
     auto sphereModelAABB = sphere->GetAxisAlignedBoundingBox();
     auto boxModelAABB = box->GetAxisAlignedBoundingBox();
     auto cylinderModelAABB = cylinder->GetAxisAlignedBoundingBox();
+    auto ellipsoidModelAABB = ellipsoid->GetAxisAlignedBoundingBox();
+    auto capsuleModelAABB = capsule->GetAxisAlignedBoundingBox();
     EXPECT_EQ(ignition::math::Vector3d(-1, 0.5, -0.5),
               ignition::math::eigen3::convert(sphereModelAABB).Min());
     EXPECT_EQ(ignition::math::Vector3d(1, 2.5, 1.5),
@@ -241,6 +282,14 @@ TEST_P(SimulationFeatures_TEST, ShapeFeatures)
               ignition::math::eigen3::convert(cylinderModelAABB).Min());
     EXPECT_EQ(ignition::math::Vector3d(3, 1.5, 2.5),
               ignition::math::eigen3::convert(cylinderModelAABB).Max());
+    EXPECT_EQ(ignition::math::Vector3d(-0.2, -5.3, 0.2),
+              ignition::math::eigen3::convert(ellipsoidModelAABB).Min());
+    EXPECT_EQ(ignition::math::Vector3d(0.2, -4.7, 1.2),
+              ignition::math::eigen3::convert(ellipsoidModelAABB).Max());
+    EXPECT_EQ(ignition::math::Vector3d(-0.2, -3.2, 0),
+              ignition::math::eigen3::convert(capsuleModelAABB).Min());
+    EXPECT_EQ(ignition::math::Vector3d(0.2, -2.8, 1),
+              ignition::math::eigen3::convert(capsuleModelAABB).Max());
   }
 }
 
@@ -343,6 +392,14 @@ TEST_P(SimulationFeatures_TEST, RetrieveContacts)
     auto cylinderFreeGroup = cylinder->FindFreeGroup();
     EXPECT_NE(nullptr, cylinderFreeGroup);
 
+    auto capsule = world->GetModel("capsule");
+    auto capsuleFreeGroup = capsule->FindFreeGroup();
+    EXPECT_NE(nullptr, capsuleFreeGroup);
+
+    auto ellipsoid = world->GetModel("ellipsoid");
+    auto ellipsoidFreeGroup = ellipsoid->FindFreeGroup();
+    EXPECT_NE(nullptr, ellipsoidFreeGroup);
+
     auto box = world->GetModel("box");
 
     // step and get contacts
@@ -350,10 +407,13 @@ TEST_P(SimulationFeatures_TEST, RetrieveContacts)
     EXPECT_TRUE(checkedOutput);
     auto contacts = world->GetContactsFromLastStep();
 
-    // large box in the middle should be intersecting with sphere and cylinder
-    EXPECT_EQ(2u, contacts.size());
+    // large box in the middle should be intersecting with sphere, cylinder,
+    // capsule and ellipsoid
+    EXPECT_EQ(4u, contacts.size());
     unsigned int contactBoxSphere = 0u;
     unsigned int contactBoxCylinder = 0u;
+    unsigned int contactBoxCapsule = 0u;
+    unsigned int contactBoxEllipsoid = 0u;
 
     for (auto &contact : contacts)
     {
@@ -382,6 +442,22 @@ TEST_P(SimulationFeatures_TEST, RetrieveContacts)
         EXPECT_TRUE(ignition::physics::test::Equal(expectedContactPos,
             contactPoint.point, 1e-6));
       }
+      else if ((m1->GetName() == "box" && m2->GetName() == "capsule") ||
+          (m1->GetName() == "capsule" && m2->GetName() == "box"))
+      {
+        contactBoxCapsule++;
+        Eigen::Vector3d expectedContactPos = Eigen::Vector3d(0.0, -3, 0.5);
+        EXPECT_TRUE(ignition::physics::test::Equal(expectedContactPos,
+            contactPoint.point, 1e-6));
+      }
+      else if ((m1->GetName() == "box" && m2->GetName() == "ellipsoid") ||
+          (m1->GetName() == "ellipsoid" && m2->GetName() == "box"))
+      {
+        contactBoxEllipsoid++;
+        Eigen::Vector3d expectedContactPos = Eigen::Vector3d(0.0, -5, 0.6);
+        EXPECT_TRUE(ignition::physics::test::Equal(expectedContactPos,
+            contactPoint.point, 1e-6));
+      }
       else
       {
         FAIL() << "There should not be contacts between: "
@@ -390,6 +466,8 @@ TEST_P(SimulationFeatures_TEST, RetrieveContacts)
     }
     EXPECT_EQ(1u, contactBoxSphere);
     EXPECT_EQ(1u, contactBoxCylinder);
+    EXPECT_EQ(1u, contactBoxCapsule);
+    EXPECT_EQ(1u, contactBoxEllipsoid);
 
     // move sphere away
     sphereFreeGroup->SetWorldPose(ignition::math::eigen3::convert(
@@ -400,10 +478,13 @@ TEST_P(SimulationFeatures_TEST, RetrieveContacts)
     EXPECT_FALSE(checkedOutput);
     contacts = world->GetContactsFromLastStep();
 
-    // large box in the middle should be intersecting with cylinder
-    EXPECT_EQ(1u, contacts.size());
+    // large box in the middle should be intersecting with cylinder, capsule,
+    // ellipsoid
+    EXPECT_EQ(3u, contacts.size());
 
     contactBoxCylinder = 0u;
+    contactBoxCapsule = 0u;
+    contactBoxEllipsoid = 0u;
     for (auto contact : contacts)
     {
       const auto &contactPoint = contact.Get<::ContactPoint>();
@@ -417,20 +498,49 @@ TEST_P(SimulationFeatures_TEST, RetrieveContacts)
       auto m2 = c2->GetLink()->GetModel();
       if ((m1->GetName() == "box" && m2->GetName() == "cylinder") ||
           (m1->GetName() == "cylinder" && m2->GetName() == "box"))
+      {
         contactBoxCylinder++;
+        Eigen::Vector3d expectedContactPos = Eigen::Vector3d(0, -1.5, 0.5);
+        EXPECT_TRUE(ignition::physics::test::Equal(expectedContactPos,
+            contactPoint.point, 1e-6));
+      }
+      else if ((m1->GetName() == "box" && m2->GetName() == "capsule") ||
+          (m1->GetName() == "capsule" && m2->GetName() == "box"))
+      {
+        contactBoxCapsule++;
+        Eigen::Vector3d expectedContactPos = Eigen::Vector3d(0.0, -3, 0.5);
+        EXPECT_TRUE(ignition::physics::test::Equal(expectedContactPos,
+            contactPoint.point, 1e-6));
+      }
+      else if ((m1->GetName() == "box" && m2->GetName() == "ellipsoid") ||
+          (m1->GetName() == "ellipsoid" && m2->GetName() == "box"))
+      {
+        contactBoxEllipsoid++;
+        Eigen::Vector3d expectedContactPos = Eigen::Vector3d(0.0, -5, 0.6);
+        EXPECT_TRUE(ignition::physics::test::Equal(expectedContactPos,
+            contactPoint.point, 1e-6));
+      }
       else
+      {
         FAIL() << "There should only be contacts between box and cylinder";
-
-      Eigen::Vector3d expectedContactPos = Eigen::Vector3d(0, -1.5, 0.5);
-      EXPECT_TRUE(ignition::physics::test::Equal(expectedContactPos,
-          contactPoint.point, 1e-6));
+      }
     }
     EXPECT_EQ(1u, contactBoxCylinder);
+    EXPECT_EQ(1u, contactBoxCapsule);
+    EXPECT_EQ(1u, contactBoxEllipsoid);
 
     // move cylinder away
     cylinderFreeGroup->SetWorldPose(ignition::math::eigen3::convert(
         ignition::math::Pose3d(0, -100, 0.5, 0, 0, 0)));
 
+    // move capsule away
+    capsuleFreeGroup->SetWorldPose(ignition::math::eigen3::convert(
+        ignition::math::Pose3d(0, -100, 100, 0, 0, 0)));
+
+    // move ellipsoid away
+    ellipsoidFreeGroup->SetWorldPose(ignition::math::eigen3::convert(
+        ignition::math::Pose3d(0, -100, -100, 0, 0, 0)));
+
     // step and get contacts
     checkedOutput = StepWorld(world, false);
     EXPECT_FALSE(checkedOutput);
diff --git a/tpe/plugin/worlds/shapes.world b/tpe/plugin/worlds/shapes.world
index bbddf19be..cb3bce5d9 100644
--- a/tpe/plugin/worlds/shapes.world
+++ b/tpe/plugin/worlds/shapes.world
@@ -96,5 +96,74 @@
         </visual>
       </link>
     </model>
+
+    <model name="capsule">
+      <pose>0 -3.0 0.5 0 0 0</pose>
+      <link name="capsule_link">
+        <inertial>
+          <inertia>
+            <ixx>0.074154</ixx>
+            <ixy>0</ixy>
+            <ixz>0</ixz>
+            <iyy>0.074154</iyy>
+            <iyz>0</iyz>
+            <izz>0.018769</izz>
+          </inertia>
+          <mass>1.0</mass>
+        </inertial>
+        <collision name="capsule_collision">
+          <geometry>
+            <capsule>
+              <radius>0.2</radius>
+              <length>0.6</length>
+            </capsule>
+          </geometry>
+        </collision>
+        <visual name="capsule_visual">
+          <geometry>
+            <capsule>
+              <radius>0.2</radius>
+              <length>0.6</length>
+            </capsule>
+          </geometry>
+          <material>
+            <ambient>1 1 0 1</ambient>
+            <diffuse>1 1 0 1</diffuse>
+            <specular>1 1 0 1</specular>
+          </material>
+        </visual>
+      </link>
+    </model>
+
+    <model name="ellipsoid">
+      <pose>0 -5 0.7 0 0 0</pose>
+      <link name="ellipsoid_link">
+        <inertial>
+          <inertia>
+            <ixx>0.068</ixx>
+            <ixy>0</ixy>
+            <ixz>0</ixz>
+            <iyy>0.058</iyy>
+            <iyz>0</iyz>
+            <izz>0.026</izz>
+          </inertia>
+          <mass>1.0</mass>
+        </inertial>
+        <collision name="ellipsoid_collision">
+          <geometry>
+            <ellipsoid>
+              <radii>0.2 0.3 0.5</radii>
+            </ellipsoid>
+          </geometry>
+        </collision>
+        <visual name="ellipsoid_visual">
+          <geometry>
+            <ellipsoid>
+              <radii>0.2 0.3 0.5</radii>
+            </ellipsoid>
+          </geometry>
+        </visual>
+      </link>
+    </model>
   </world>
 </sdf>