diff --git a/README.md b/README.md
index 3aa42a185..264ed1736 100644
--- a/README.md
+++ b/README.md
@@ -58,7 +58,7 @@ Gazebo Physics provides the following functionality:
   at runtime.
 * Features for common aspects of rigid body dynamic simulation
     - Construct model from [SDFormat](http://sdformat.org/) file.
-    - Collision shapes (such as box, sphere, cylinder, capsule, ellipsoid, mesh, heightmap).
+    - Collision shapes (such as box, sphere, cylinder, cone, capsule, ellipsoid, mesh, heightmap).
     - Joint types (such as revolute, prismatic, fixed, ball, screw, universal).
     - Step simulation, get/set state, apply inputs.
 * Reference implementation of physics plugin using
diff --git a/bullet-featherstone/README.md b/bullet-featherstone/README.md
index 5444cd089..932842b68 100644
--- a/bullet-featherstone/README.md
+++ b/bullet-featherstone/README.md
@@ -65,6 +65,8 @@ These are the specific physics API features implemented.
   * AttachBoxShapeFeature
   * GetCapsuleShapeProperties
   * AttachCapsuleShapeFeature
+  * GetConeShapeProperties
+  * AttachConeShapeFeature
   * GetCylinderShapeProperties
   * AttachCylinderShapeFeature
   * GetEllipsoidShapeProperties
diff --git a/bullet-featherstone/src/SDFFeatures.cc b/bullet-featherstone/src/SDFFeatures.cc
index 0679fb831..3ad3b3f5b 100644
--- a/bullet-featherstone/src/SDFFeatures.cc
+++ b/bullet-featherstone/src/SDFFeatures.cc
@@ -25,6 +25,7 @@
 
 #include <sdf/Box.hh>
 #include <sdf/Capsule.hh>
+#include <sdf/Cone.hh>
 #include <sdf/Cylinder.hh>
 #include <sdf/Ellipsoid.hh>
 #include <sdf/Geometry.hh>
@@ -963,6 +964,14 @@ bool SDFFeatures::AddSdfCollision(
     const auto radius = sphere->Radius();
     shape = std::make_unique<btSphereShape>(static_cast<btScalar>(radius));
   }
+  else if (const auto *cone = geom->ConeShape())
+  {
+    const auto radius = static_cast<btScalar>(cone->Radius());
+    const auto height = static_cast<btScalar>(cone->Length());
+    shape =
+      std::make_unique<btConeShapeZ>(radius, height);
+    shape->setMargin(0.0);
+  }
   else if (const auto *cylinder = geom->CylinderShape())
   {
     const auto radius = static_cast<btScalar>(cylinder->Radius());
diff --git a/bullet-featherstone/src/ShapeFeatures.cc b/bullet-featherstone/src/ShapeFeatures.cc
index 1f05f51fc..038ad8357 100644
--- a/bullet-featherstone/src/ShapeFeatures.cc
+++ b/bullet-featherstone/src/ShapeFeatures.cc
@@ -179,6 +179,86 @@ Identity ShapeFeatures::AttachCapsuleShape(
   return identity;
 }
 
+/////////////////////////////////////////////////
+Identity ShapeFeatures::CastToConeShape(const Identity &_shapeID) const
+{
+  const auto *shapeInfo = this->ReferenceInterface<CollisionInfo>(_shapeID);
+  if (shapeInfo != nullptr)
+  {
+    const auto &shape = shapeInfo->collider;
+    if (dynamic_cast<btConeShape*>(shape.get()))
+      return this->GenerateIdentity(_shapeID, this->Reference(_shapeID));
+  }
+
+  return this->GenerateInvalidId();
+}
+
+/////////////////////////////////////////////////
+double ShapeFeatures::GetConeShapeRadius(
+    const Identity &_coneID) const
+{
+  auto it = this->collisions.find(_coneID);
+  if (it != this->collisions.end() && it->second != nullptr)
+  {
+    if (it->second->collider != nullptr)
+    {
+      auto *cone = static_cast<btConeShape*>(
+        it->second->collider.get());
+      if (cone)
+      {
+        return cone->getRadius();
+      }
+    }
+  }
+
+  return -1;
+}
+
+/////////////////////////////////////////////////
+double ShapeFeatures::GetConeShapeHeight(
+    const Identity &_coneID) const
+{
+  auto it = this->collisions.find(_coneID);
+  if (it != this->collisions.end() && it->second != nullptr)
+  {
+    if (it->second->collider != nullptr)
+    {
+      auto *cone = static_cast<btConeShape*>(
+        it->second->collider.get());
+      if (cone)
+      {
+        return cone->getHeight();
+      }
+    }
+  }
+
+  return -1;
+}
+
+/////////////////////////////////////////////////
+Identity ShapeFeatures::AttachConeShape(
+    const Identity &_linkID,
+    const std::string &_name,
+    const double _radius,
+    const double _height,
+    const Pose3d &_pose)
+{
+  const auto radius = static_cast<btScalar>(_radius);
+  const auto height = static_cast<btScalar>(_height);
+  auto shape =
+    std::make_unique<btConeShapeZ>(radius, height);
+  shape->setMargin(0.0);
+
+  auto identity = this->AddCollision(
+    CollisionInfo{
+      _name,
+      std::move(shape),
+      _linkID,
+      _pose});
+
+  return identity;
+}
+
 /////////////////////////////////////////////////
 Identity ShapeFeatures::CastToCylinderShape(const Identity &_shapeID) const
 {
diff --git a/bullet-featherstone/src/ShapeFeatures.hh b/bullet-featherstone/src/ShapeFeatures.hh
index 998c97e66..4bcd86fac 100644
--- a/bullet-featherstone/src/ShapeFeatures.hh
+++ b/bullet-featherstone/src/ShapeFeatures.hh
@@ -21,6 +21,7 @@
 #include <gz/physics/Shape.hh>
 #include <gz/physics/BoxShape.hh>
 #include <gz/physics/CapsuleShape.hh>
+#include <gz/physics/ConeShape.hh>
 #include <gz/physics/CylinderShape.hh>
 #include <gz/physics/EllipsoidShape.hh>
 #include <gz/physics/SphereShape.hh>
@@ -42,6 +43,9 @@ struct ShapeFeatureList : FeatureList<
   GetCapsuleShapeProperties,
   AttachCapsuleShapeFeature,
 
+  GetConeShapeProperties,
+  AttachConeShapeFeature,
+
   GetCylinderShapeProperties,
   AttachCylinderShapeFeature,
 
@@ -90,6 +94,23 @@ class ShapeFeatures :
       double _length,
       const Pose3d &_pose) override;
 
+  // ----- Cone Features -----
+  public: Identity CastToConeShape(
+      const Identity &_shapeID) const override;
+
+  public: double GetConeShapeRadius(
+      const Identity &_coneID) const override;
+
+  public: double GetConeShapeHeight(
+      const Identity &_coneID) const override;
+
+  public: Identity AttachConeShape(
+      const Identity &_linkID,
+      const std::string &_name,
+      double _radius,
+      double _height,
+      const Pose3d &_pose) override;
+
   // ----- Cylinder Features -----
   public: Identity CastToCylinderShape(
       const Identity &_shapeID) const override;
diff --git a/bullet/src/SDFFeatures.cc b/bullet/src/SDFFeatures.cc
index efdb37b67..2d69cc61f 100644
--- a/bullet/src/SDFFeatures.cc
+++ b/bullet/src/SDFFeatures.cc
@@ -22,9 +22,10 @@
 #include <sdf/Geometry.hh>
 #include <sdf/Box.hh>
 #include <sdf/Capsule.hh>
+#include <sdf/Cone.hh>
+#include <sdf/Cylinder.hh>
 #include <sdf/Ellipsoid.hh>
 #include <sdf/Sphere.hh>
-#include <sdf/Cylinder.hh>
 #include <sdf/Plane.hh>
 #include <sdf/JointAxis.hh>
 
@@ -228,6 +229,14 @@ Identity SDFFeatures::ConstructSdfCollision(
     const auto radius = static_cast<btScalar>(sphere->Radius());
     shape = std::make_shared<btSphereShape>(radius);
   }
+  else if (geom->ConeShape())
+  {
+    const auto cone = geom->ConeShape();
+    const auto radius = static_cast<btScalar>(cone->Radius());
+    const auto height = static_cast<btScalar>(cone->Length());
+    shape =
+      std::make_shared<btConeShapeZ>(radius, height);
+  }
   else if (geom->CylinderShape())
   {
     const auto cylinder = geom->CylinderShape();
diff --git a/dartsim/src/CustomConeMeshShape.cc b/dartsim/src/CustomConeMeshShape.cc
new file mode 100644
index 000000000..a3f8bf7ed
--- /dev/null
+++ b/dartsim/src/CustomConeMeshShape.cc
@@ -0,0 +1,62 @@
+/*
+ * Copyright (C) 2018 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 "CustomConeMeshShape.hh"
+
+#include <memory>
+#include <string>
+
+#include <gz/common/Console.hh>
+#include <gz/common/MeshManager.hh>
+#include <gz/math/Cone.hh>
+
+namespace gz {
+namespace physics {
+namespace dartsim {
+
+/////////////////////////////////////////////////
+const gz::common::Mesh* MakeCustomConeMesh(
+    const gz::math::Coned &_cone,
+    int _meshRings,
+    int _meshSegments)
+{
+  common::MeshManager *meshMgr = common::MeshManager::Instance();
+  std::string coneMeshName = std::string("cone_mesh")
+    + "_" + std::to_string(_cone.Radius())
+    + "_" + std::to_string(_cone.Length());
+  meshMgr->CreateCone(
+    coneMeshName,
+    _cone.Radius(),
+    _cone.Length(),
+    _meshRings, _meshSegments);
+  return meshMgr->MeshByName(coneMeshName);
+}
+
+/////////////////////////////////////////////////
+CustomConeMeshShape::CustomConeMeshShape(
+    const gz::math::Coned &_cone,
+    int _meshRings,
+    int _meshSegments)
+  : CustomMeshShape(*MakeCustomConeMesh(_cone, _meshRings, _meshSegments),
+                    Eigen::Vector3d(1, 1, 1)),
+    cone(_cone)
+{
+}
+
+}
+}
+}
diff --git a/dartsim/src/CustomConeMeshShape.hh b/dartsim/src/CustomConeMeshShape.hh
new file mode 100644
index 000000000..64e0e9cec
--- /dev/null
+++ b/dartsim/src/CustomConeMeshShape.hh
@@ -0,0 +1,44 @@
+/*
+ * Copyright (C) 2024 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 GZ_PHYSICS_DARTSIM_SRC_CUSTOMCONEMESHSHAPE_HH_
+#define GZ_PHYSICS_DARTSIM_SRC_CUSTOMCONEMESHSHAPE_HH_
+
+#include <gz/math/Cone.hh>
+#include "CustomMeshShape.hh"
+
+namespace gz {
+namespace physics {
+namespace dartsim {
+
+/// \brief This class creates a custom derivative of the CustomMeshShape to
+/// allow casting to a Cone shape.
+class CustomConeMeshShape : public CustomMeshShape
+{
+  public: CustomConeMeshShape(
+      const gz::math::Coned &_cone,
+      int _meshRings = 1,
+      int _meshSegments = 36);
+
+  public: gz::math::Coned cone;
+};
+
+}
+}
+}
+
+#endif  // GZ_PHYSICS_DARTSIM_SRC_CUSTOMMESHSHAPE_HH_
diff --git a/dartsim/src/SDFFeatures.cc b/dartsim/src/SDFFeatures.cc
index 08a1183f9..3993dceb6 100644
--- a/dartsim/src/SDFFeatures.cc
+++ b/dartsim/src/SDFFeatures.cc
@@ -20,12 +20,14 @@
 #include <cmath>
 #include <limits>
 #include <memory>
+#include <string>
 #include <utility>
 
 #include <dart/constraint/ConstraintSolver.hpp>
 #include <dart/dynamics/BallJoint.hpp>
 #include <dart/dynamics/BoxShape.hpp>
 #include <dart/dynamics/CapsuleShape.hpp>
+#include <dart/dynamics/ConeShape.hpp>
 #include <dart/dynamics/CylinderShape.hpp>
 #include <dart/dynamics/EllipsoidShape.hpp>
 #include <dart/dynamics/FreeJoint.hpp>
@@ -49,6 +51,7 @@
 #include <sdf/Box.hh>
 #include <sdf/Collision.hh>
 #include <sdf/Capsule.hh>
+#include <sdf/Cone.hh>
 #include <sdf/Cylinder.hh>
 #include <sdf/Ellipsoid.hh>
 #include <sdf/Geometry.hh>
@@ -65,6 +68,7 @@
 #include <sdf/World.hh>
 
 #include "AddedMassFeatures.hh"
+#include "CustomConeMeshShape.hh"
 #include "CustomMeshShape.hh"
 
 namespace gz {
@@ -340,11 +344,27 @@ static ShapeAndTransform ConstructGeometry(
   {
     return ConstructCapsule(*_geometry.CapsuleShape());
   }
+  else if (_geometry.ConeShape())
+  {
+    // TODO(anyone): Replace this code when Cone is supported by DART
+    gzwarn << "DART: Cone is not a supported collision geomerty"
+           << " primitive, using generated mesh of a cone instead"
+           << std::endl;
+    auto mesh =
+      std::make_shared<CustomConeMeshShape>(_geometry.ConeShape()->Shape());
+    auto mesh2 = std::dynamic_pointer_cast<dart::dynamics::MeshShape>(mesh);
+    return {mesh2};
+  }
   else if (_geometry.CylinderShape())
+  {
     return ConstructCylinder(*_geometry.CylinderShape());
+  }
   else if (_geometry.EllipsoidShape())
   {
     // TODO(anyone): Replace this code when Ellipsoid is supported by DART
+    gzwarn << "DART: Ellipsoid is not a supported collision geomerty"
+           << " primitive, using generated mesh of an ellipsoid instead"
+           << std::endl;
     common::MeshManager *meshMgr = common::MeshManager::Instance();
     std::string ellipsoidMeshName = std::string("ellipsoid_mesh")
       + "_" + std::to_string(_geometry.EllipsoidShape()->Radii().X())
diff --git a/dartsim/src/SDFFeatures_TEST.cc b/dartsim/src/SDFFeatures_TEST.cc
index d3db87464..613bb8144 100644
--- a/dartsim/src/SDFFeatures_TEST.cc
+++ b/dartsim/src/SDFFeatures_TEST.cc
@@ -972,10 +972,11 @@ TEST_P(SDFFeatures_TEST, Shapes)
   auto dartWorld = world->GetDartsimWorld();
   ASSERT_NE(nullptr, dartWorld);
 
-  ASSERT_EQ(5u, dartWorld->getNumSkeletons());
+  ASSERT_EQ(6u, dartWorld->getNumSkeletons());
 
   int count{0};
-  for (auto name : {"sphere", "box", "cylinder", "capsule", "ellipsoid"})
+  for (auto name : {"sphere", "box", "cylinder", "capsule", "ellipsoid",
+                    "cone"})
   {
     const auto skeleton = dartWorld->getSkeleton(count++);
     ASSERT_NE(nullptr, skeleton);
diff --git a/dartsim/src/ShapeFeatures.cc b/dartsim/src/ShapeFeatures.cc
index 8b42a0dbb..5b19c17a0 100644
--- a/dartsim/src/ShapeFeatures.cc
+++ b/dartsim/src/ShapeFeatures.cc
@@ -17,7 +17,9 @@
 
 #include "ShapeFeatures.hh"
 
+#include <cstddef>
 #include <memory>
+#include <string>
 
 #include <dart/dynamics/BoxShape.hpp>
 #include <dart/dynamics/CapsuleShape.hpp>
@@ -32,6 +34,7 @@
 #include <gz/common/Mesh.hh>
 #include <gz/common/MeshManager.hh>
 
+#include "CustomConeMeshShape.hh"
 #include "CustomHeightmapShape.hh"
 #include "CustomMeshShape.hh"
 
@@ -161,6 +164,70 @@ Identity ShapeFeatures::AttachCapsuleShape(
   return this->GenerateIdentity(shapeID, this->shapes.at(shapeID));
 }
 
+/////////////////////////////////////////////////
+Identity ShapeFeatures::CastToConeShape(const Identity &_shapeID) const
+{
+  const auto *shapeInfo = this->ReferenceInterface<ShapeInfo>(_shapeID);
+
+  const dart::dynamics::ShapePtr &shape = shapeInfo->node->getShape();
+
+  if (dynamic_cast<CustomMeshShape *>(shape.get()))
+    return this->GenerateIdentity(_shapeID, this->Reference(_shapeID));
+
+  return this->GenerateInvalidId();
+}
+
+/////////////////////////////////////////////////
+double ShapeFeatures::GetConeShapeRadius(
+    const Identity &_coneID) const
+{
+  const auto *shapeInfo = this->ReferenceInterface<ShapeInfo>(_coneID);
+
+  auto *coneShape =
+      static_cast<CustomConeMeshShape *>(
+          shapeInfo->node->getShape().get());
+
+  return coneShape->cone.Radius();
+}
+
+/////////////////////////////////////////////////
+double ShapeFeatures::GetConeShapeHeight(
+    const Identity &_coneID) const
+{
+  const auto *shapeInfo = this->ReferenceInterface<ShapeInfo>(_coneID);
+
+  auto *coneShape =
+      static_cast<CustomConeMeshShape *>(
+          shapeInfo->node->getShape().get());
+
+  return coneShape->cone.Length();
+}
+
+/////////////////////////////////////////////////
+Identity ShapeFeatures::AttachConeShape(
+    const Identity &_linkID,
+    const std::string &_name,
+    const double _radius,
+    const double _height,
+    const Pose3d &_pose)
+{
+  gzwarn << "DART: Cone is not a supported collision geomerty"
+         << " primitive, using generated mesh of a cone instead"
+         << std::endl;
+  auto mesh =
+    std::make_shared<CustomConeMeshShape>(gz::math::Coned(_height, _radius));
+
+  DartBodyNode *bn = this->ReferenceInterface<LinkInfo>(_linkID)->link.get();
+  dart::dynamics::ShapeNode *sn =
+      bn->createShapeNodeWith<dart::dynamics::CollisionAspect,
+                              dart::dynamics::DynamicsAspect>(
+          mesh, bn->getName() + ":" + _name);
+
+  sn->setRelativeTransform(_pose);
+  const std::size_t shapeID = this->AddShape({sn, _name});
+  return this->GenerateIdentity(shapeID, this->shapes.at(shapeID));
+}
+
 /////////////////////////////////////////////////
 Identity ShapeFeatures::CastToCylinderShape(const Identity &_shapeID) const
 {
@@ -255,6 +322,9 @@ Identity ShapeFeatures::AttachEllipsoidShape(
     const Vector3d &_radii,
     const Pose3d &_pose)
 {
+  gzwarn << "DART: Ellipsoid is not a supported collision geomerty"
+         << " primitive, using generated mesh of an ellipsoid instead"
+         << std::endl;
   common::MeshManager *meshMgr = common::MeshManager::Instance();
   std::string ellipsoidMeshName = _name + "_ellipsoid_mesh"
     + "_" + std::to_string(_radii[0])
diff --git a/dartsim/src/ShapeFeatures.hh b/dartsim/src/ShapeFeatures.hh
index e83992419..b112856c2 100644
--- a/dartsim/src/ShapeFeatures.hh
+++ b/dartsim/src/ShapeFeatures.hh
@@ -23,6 +23,7 @@
 #include <gz/physics/Shape.hh>
 #include <gz/physics/BoxShape.hh>
 #include <gz/physics/CapsuleShape.hh>
+#include <gz/physics/ConeShape.hh>
 #include <gz/physics/CylinderShape.hh>
 #include <gz/physics/EllipsoidShape.hh>
 #include <gz/physics/heightmap/HeightmapShape.hh>
@@ -55,6 +56,10 @@ struct ShapeFeatureList : FeatureList<
   //  SetCapsulerShapeProperties,
   AttachCapsuleShapeFeature,
 
+  GetConeShapeProperties,
+//  SetConeShapeProperties,
+  AttachConeShapeFeature,
+
   GetCylinderShapeProperties,
 //  SetCylinderShapeProperties,
   AttachCylinderShapeFeature,
@@ -121,6 +126,23 @@ class ShapeFeatures :
       double _length,
       const Pose3d &_pose) override;
 
+  // ----- Cone Features -----
+  public: Identity CastToConeShape(
+      const Identity &_shapeID) const override;
+
+  public: double GetConeShapeRadius(
+      const Identity &_coneID) const override;
+
+  public: double GetConeShapeHeight(
+      const Identity &_coneID) const override;
+
+  public: Identity AttachConeShape(
+      const Identity &_linkID,
+      const std::string &_name,
+      double _radius,
+      double _height,
+      const Pose3d &_pose) override;
+
   // ----- Cylinder Features -----
   public: Identity CastToCylinderShape(
       const Identity &_shapeID) const override;
diff --git a/include/gz/physics/ConeShape.hh b/include/gz/physics/ConeShape.hh
new file mode 100644
index 000000000..143d3490a
--- /dev/null
+++ b/include/gz/physics/ConeShape.hh
@@ -0,0 +1,154 @@
+/*
+ * Copyright 2024 CogniPilot Foundation
+ * Copyright 2024 Open Source Robotics Foundation
+ * Copyright 2024 Rudis Laboratories
+ *
+ * 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 GZ_PHYSICS_CONESHAPE_HH_
+#define GZ_PHYSICS_CONESHAPE_HH_
+
+#include <string>
+
+#include <gz/physics/DeclareShapeType.hh>
+#include <gz/physics/Geometry.hh>
+
+namespace gz
+{
+  namespace physics
+  {
+    GZ_PHYSICS_DECLARE_SHAPE_TYPE(ConeShape)
+
+    class GZ_PHYSICS_VISIBLE GetConeShapeProperties
+        : public virtual FeatureWithRequirements<ConeShapeCast>
+    {
+      public: template <typename PolicyT, typename FeaturesT>
+      class ConeShape : public virtual Entity<PolicyT, FeaturesT>
+      {
+        public: using Scalar = typename PolicyT::Scalar;
+
+        /// \brief Get the radius of this ConeShape
+        /// \return the radius of this ConeShape
+        public: Scalar GetRadius() const;
+
+        /// \brief Get the height (length along the local z-axis) of this
+        /// ConeShape.
+        /// \return the height of this ConeShape
+        public: Scalar GetHeight() const;
+      };
+
+      public: template <typename PolicyT>
+      class Implementation : public virtual Feature::Implementation<PolicyT>
+      {
+        public: using Scalar = typename PolicyT::Scalar;
+
+        public: virtual Scalar GetConeShapeRadius(
+            const Identity &_coneID) const = 0;
+
+        public: virtual Scalar GetConeShapeHeight(
+            const Identity &_coneID) const = 0;
+      };
+    };
+
+    /////////////////////////////////////////////////
+    /// \brief This feature sets the ConeShape properties such as
+    /// the cone radius and height.
+    class GZ_PHYSICS_VISIBLE SetConeShapeProperties
+        : public virtual FeatureWithRequirements<ConeShapeCast>
+    {
+      public: template <typename PolicyT, typename FeaturesT>
+      class ConeShape : public virtual Entity<PolicyT, FeaturesT>
+      {
+        public: using Scalar = typename PolicyT::Scalar;
+
+        /// \brief Set the radius of this ConeShape
+        /// \param[in] _radius
+        ///   The desired radius of this ConeShape
+        public: void SetRadius(Scalar _radius);
+
+        /// \brief Set the height of this ConeShape
+        /// \param[in] _height
+        ///   The desired height of this ConeShape
+        public: void SetHeight(Scalar _height);
+      };
+
+      public: template <typename PolicyT>
+      class Implementation : public virtual Feature::Implementation<PolicyT>
+      {
+        public: using Scalar = typename PolicyT::Scalar;
+
+        public: virtual void SetConeShapeRadius(
+            const Identity &_coneID, Scalar _radius) = 0;
+
+        public: virtual void SetConeShapeHeight(
+            const Identity &_coneID, Scalar _height) = 0;
+      };
+    };
+
+    /////////////////////////////////////////////////
+    /// \brief This feature constructs a new cone shape and attaches the
+    /// desired pose in the link frame. The pose could be defined to be the
+    /// cone center point in actual implementation.
+    class GZ_PHYSICS_VISIBLE AttachConeShapeFeature
+        : public virtual FeatureWithRequirements<ConeShapeCast>
+    {
+      public: template <typename PolicyT, typename FeaturesT>
+      class Link : public virtual Feature::Link<PolicyT, FeaturesT>
+      {
+        public: using Scalar = typename PolicyT::Scalar;
+
+        public: using PoseType =
+            typename FromPolicy<PolicyT>::template Use<Pose>;
+
+        public: using ShapePtrType = ConeShapePtr<PolicyT, FeaturesT>;
+
+        /// \brief Rigidly attach a ConeShape to this link.
+        /// \param[in] _name
+        /// \param[in] _radius
+        ///   The radius of the cone.
+        /// \param[in] _height
+        ///   The height of the cone.
+        /// \param[in] _pose
+        ///   The desired pose of the ConeShape relative to the Link frame.
+        /// \returns a ShapePtrType to the newly constructed ConeShape
+        public: ShapePtrType AttachConeShape(
+            const std::string &_name = "cone",
+            Scalar _radius = 1.0,
+            Scalar _height = 1.0,
+            const PoseType &_pose = PoseType::Identity());
+      };
+
+      public: template <typename PolicyT>
+      class Implementation : public virtual Feature::Implementation<PolicyT>
+      {
+        public: using Scalar = typename PolicyT::Scalar;
+
+        public: using PoseType =
+            typename FromPolicy<PolicyT>::template Use<Pose>;
+
+        public: virtual Identity AttachConeShape(
+            const Identity &_linkID,
+            const std::string &_name,
+            Scalar _radius,
+            Scalar _height,
+            const PoseType &_pose) = 0;
+      };
+    };
+  }
+}
+
+#include <gz/physics/detail/ConeShape.hh>
+
+#endif
diff --git a/include/gz/physics/detail/ConeShape.hh b/include/gz/physics/detail/ConeShape.hh
new file mode 100644
index 000000000..0630b281a
--- /dev/null
+++ b/include/gz/physics/detail/ConeShape.hh
@@ -0,0 +1,84 @@
+/*
+ * Copyright 2024 CogniPilot Foundation
+ * Copyright 2024 Open Source Robotics Foundation
+ * Copyright 2024 Rudis Laboratories
+ *
+ * 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 GZ_PHYSICS_DETAIL_CONESHAPE_HH_
+#define GZ_PHYSICS_DETAIL_CONESHAPE_HH_
+
+#include <string>
+
+#include <gz/physics/ConeShape.hh>
+
+namespace gz
+{
+  namespace physics
+  {
+    /////////////////////////////////////////////////
+    template <typename PolicyT, typename FeaturesT>
+    auto GetConeShapeProperties::ConeShape<PolicyT, FeaturesT>
+    ::GetRadius() const -> Scalar
+    {
+      return this->template Interface<GetConeShapeProperties>()
+          ->GetConeShapeRadius(this->identity);
+    }
+
+    /////////////////////////////////////////////////
+    template <typename PolicyT, typename FeaturesT>
+    auto GetConeShapeProperties::ConeShape<PolicyT, FeaturesT>
+    ::GetHeight() const -> Scalar
+    {
+      return this->template Interface<GetConeShapeProperties>()
+          ->GetConeShapeHeight(this->identity);
+    }
+
+    /////////////////////////////////////////////////
+    template <typename PolicyT, typename FeaturesT>
+    void SetConeShapeProperties::ConeShape<PolicyT, FeaturesT>
+    ::SetRadius(Scalar _radius)
+    {
+      this->template Interface<SetConeShapeProperties>()
+          ->SetConeShapeRadius(this->identity, _radius);
+    }
+
+    /////////////////////////////////////////////////
+    template <typename PolicyT, typename FeaturesT>
+    void SetConeShapeProperties::ConeShape<PolicyT, FeaturesT>
+    ::SetHeight(Scalar _height)
+    {
+      this->template Interface<SetConeShapeProperties>()
+          ->SetConeShapeHeight(this->identity, _height);
+    }
+
+    /////////////////////////////////////////////////
+    template <typename PolicyT, typename FeaturesT>
+    auto AttachConeShapeFeature::Link<PolicyT, FeaturesT>
+    ::AttachConeShape(
+        const std::string &_name,
+        Scalar _radius,
+        Scalar _height,
+        const PoseType &_pose) -> ShapePtrType
+    {
+      return ShapePtrType(this->pimpl,
+            this->template Interface<AttachConeShapeFeature>()
+                ->AttachConeShape(
+                            this->identity, _name, _radius, _height, _pose));
+    }
+  }
+}
+
+#endif
diff --git a/test/common_test/simulation_features.cc b/test/common_test/simulation_features.cc
index d92e474f9..ec0c05072 100644
--- a/test/common_test/simulation_features.cc
+++ b/test/common_test/simulation_features.cc
@@ -36,13 +36,14 @@
 #include <gz/physics/sdf/ConstructWorld.hh>
 
 #include "gz/physics/BoxShape.hh"
-#include <gz/physics/GetContacts.hh>
 #include "gz/physics/ContactProperties.hh"
-#include "gz/physics/CylinderShape.hh"
 #include "gz/physics/CapsuleShape.hh"
+#include "gz/physics/ConeShape.hh"
+#include "gz/physics/CylinderShape.hh"
 #include "gz/physics/EllipsoidShape.hh"
 #include <gz/physics/FreeGroup.hh>
 #include <gz/physics/GetBoundingBox.hh>
+#include <gz/physics/GetContacts.hh>
 #include "gz/physics/SphereShape.hh"
 
 #include <gz/physics/ConstructEmpty.hh>
@@ -79,11 +80,13 @@ struct Features : gz::physics::FeatureList<
 
   gz::physics::AttachBoxShapeFeature,
   gz::physics::AttachSphereShapeFeature,
+  gz::physics::AttachConeShapeFeature,
   gz::physics::AttachCylinderShapeFeature,
   gz::physics::AttachEllipsoidShapeFeature,
   gz::physics::AttachCapsuleShapeFeature,
   gz::physics::GetSphereShapeProperties,
   gz::physics::GetBoxShapeProperties,
+  gz::physics::GetConeShapeProperties,
   gz::physics::GetCylinderShapeProperties,
   gz::physics::GetCapsuleShapeProperties,
   gz::physics::GetEllipsoidShapeProperties
@@ -264,7 +267,7 @@ TYPED_TEST(SimulationFeaturesCollisionPairMaxContactsTest,
     EXPECT_TRUE(checkedOutput);
 
     contacts = world->GetContactsFromLastStep();
-    EXPECT_EQ(4u, contacts.size());
+    EXPECT_EQ(5u, contacts.size());
 
     world->SetCollisionPairMaxContacts(0u);
     EXPECT_EQ(0u, world->GetCollisionPairMaxContacts());
@@ -372,11 +375,13 @@ struct FeaturesShapeFeatures : gz::physics::FeatureList<
 
   gz::physics::AttachBoxShapeFeature,
   gz::physics::AttachSphereShapeFeature,
+  gz::physics::AttachConeShapeFeature,
   gz::physics::AttachCylinderShapeFeature,
   gz::physics::AttachEllipsoidShapeFeature,
   gz::physics::AttachCapsuleShapeFeature,
   gz::physics::GetSphereShapeProperties,
   gz::physics::GetBoxShapeProperties,
+  gz::physics::GetConeShapeProperties,
   gz::physics::GetCylinderShapeProperties,
   gz::physics::GetCapsuleShapeProperties,
   gz::physics::GetEllipsoidShapeProperties
@@ -445,6 +450,7 @@ TYPED_TEST(SimulationFeaturesShapeFeaturesTest, ShapeFeatures)
     auto cylinderLink = cylinder->GetLink(0);
     auto cylinderCollision = cylinderLink->GetShape(0);
     auto cylinderShape = cylinderCollision->CastToCylinderShape();
+    ASSERT_NE(nullptr, cylinderShape);
     EXPECT_NEAR(0.5, cylinderShape->GetRadius(), 1e-6);
     EXPECT_NEAR(1.1, cylinderShape->GetHeight(), 1e-6);
 
@@ -452,6 +458,7 @@ TYPED_TEST(SimulationFeaturesShapeFeaturesTest, ShapeFeatures)
         "cylinder2", 3.0, 4.0, Eigen::Isometry3d::Identity());
     EXPECT_EQ(2u, cylinderLink->GetShapeCount());
     EXPECT_EQ(cylinder2, cylinderLink->GetShape(1));
+    ASSERT_NE(nullptr, cylinderLink->GetShape(1)->CastToCylinderShape());
     EXPECT_NEAR(3.0,
         cylinderLink->GetShape(1)->CastToCylinderShape()->GetRadius(),
         1e-6);
@@ -463,6 +470,7 @@ TYPED_TEST(SimulationFeaturesShapeFeaturesTest, ShapeFeatures)
     auto ellipsoidLink = ellipsoid->GetLink(0);
     auto ellipsoidCollision = ellipsoidLink->GetShape(0);
     auto ellipsoidShape = ellipsoidCollision->CastToEllipsoidShape();
+    ASSERT_NE(nullptr, ellipsoidShape);
     EXPECT_TRUE(
       gz::math::Vector3d(0.2, 0.3, 0.5).Equal(
       gz::math::eigen3::convert(ellipsoidShape->GetRadii()), 0.1));
@@ -478,6 +486,7 @@ TYPED_TEST(SimulationFeaturesShapeFeaturesTest, ShapeFeatures)
     auto capsuleLink = capsule->GetLink(0);
     auto capsuleCollision = capsuleLink->GetShape(0);
     auto capsuleShape = capsuleCollision->CastToCapsuleShape();
+    ASSERT_NE(nullptr, capsuleShape);
     EXPECT_NEAR(0.2, capsuleShape->GetRadius(), 1e-6);
     EXPECT_NEAR(0.6, capsuleShape->GetLength(), 1e-6);
 
@@ -486,6 +495,26 @@ TYPED_TEST(SimulationFeaturesShapeFeaturesTest, ShapeFeatures)
     EXPECT_EQ(2u, capsuleLink->GetShapeCount());
     EXPECT_EQ(capsule2, capsuleLink->GetShape(1));
 
+    auto cone = world->GetModel("cone");
+    auto coneLink = cone->GetLink(0);
+    auto coneCollision = coneLink->GetShape(0);
+    auto coneShape = coneCollision->CastToConeShape();
+    ASSERT_NE(nullptr, coneShape);
+    EXPECT_NEAR(0.5, coneShape->GetRadius(), 1e-6);
+    EXPECT_NEAR(1.1, coneShape->GetHeight(), 1e-6);
+
+    auto cone2 = coneLink->AttachConeShape(
+        "cone2", 3.0, 4.0, Eigen::Isometry3d::Identity());
+    EXPECT_EQ(2u, coneLink->GetShapeCount());
+    EXPECT_EQ(cone2, coneLink->GetShape(1));
+    ASSERT_NE(nullptr, coneLink->GetShape(1)->CastToConeShape());
+    EXPECT_NEAR(3.0,
+        coneLink->GetShape(1)->CastToConeShape()->GetRadius(),
+        1e-6);
+    EXPECT_NEAR(4.0,
+        coneLink->GetShape(1)->CastToConeShape()->GetHeight(),
+        1e-6);
+
     // Test the bounding boxes in the local frames
     auto sphereAABB =
       sphereCollision->GetAxisAlignedBoundingBox(*sphereCollision);
@@ -497,6 +526,8 @@ TYPED_TEST(SimulationFeaturesShapeFeaturesTest, ShapeFeatures)
       ellipsoidCollision->GetAxisAlignedBoundingBox(*ellipsoidCollision);
     auto capsuleAABB =
       capsuleCollision->GetAxisAlignedBoundingBox(*capsuleCollision);
+    auto coneAABB =
+      coneCollision->GetAxisAlignedBoundingBox(*coneCollision);
 
     EXPECT_TRUE(gz::math::Vector3d(-1, -1, -1).Equal(
                 gz::math::eigen3::convert(sphereAABB).Min(), 0.1));
@@ -518,6 +549,10 @@ TYPED_TEST(SimulationFeaturesShapeFeaturesTest, ShapeFeatures)
                 gz::math::eigen3::convert(capsuleAABB).Min(), 0.1));
     EXPECT_TRUE(gz::math::Vector3d(0.2, 0.2, 0.5).Equal(
                 gz::math::eigen3::convert(capsuleAABB).Max(), 0.1));
+    EXPECT_EQ(gz::math::Vector3d(-0.5, -0.5, -0.55),
+        gz::math::eigen3::convert(coneAABB).Min());
+    EXPECT_EQ(gz::math::Vector3d(0.5, 0.5, 0.55),
+        gz::math::eigen3::convert(coneAABB).Max());
 
     // check model AABB. By default, the AABBs are in world frame
     auto sphereModelAABB = sphere->GetAxisAlignedBoundingBox();
@@ -525,6 +560,7 @@ TYPED_TEST(SimulationFeaturesShapeFeaturesTest, ShapeFeatures)
     auto cylinderModelAABB = cylinder->GetAxisAlignedBoundingBox();
     auto ellipsoidModelAABB = ellipsoid->GetAxisAlignedBoundingBox();
     auto capsuleModelAABB = capsule->GetAxisAlignedBoundingBox();
+    auto coneModelAABB = cone->GetAxisAlignedBoundingBox();
 
     EXPECT_EQ(gz::math::Vector3d(-1, 0.5, -0.5),
         gz::math::eigen3::convert(sphereModelAABB).Min());
@@ -546,6 +582,10 @@ TYPED_TEST(SimulationFeaturesShapeFeaturesTest, ShapeFeatures)
         gz::math::eigen3::convert(capsuleModelAABB).Min());
     EXPECT_EQ(gz::math::Vector3d(0.2, -2.8, 1),
         gz::math::eigen3::convert(capsuleModelAABB).Max());
+    EXPECT_EQ(gz::math::Vector3d(-3, -9.5, -1.5),
+        gz::math::eigen3::convert(coneModelAABB).Min());
+    EXPECT_EQ(gz::math::Vector3d(3, -3.5, 2.5),
+        gz::math::eigen3::convert(coneModelAABB).Max());
   }
 }
 
@@ -780,6 +820,10 @@ TYPED_TEST(SimulationFeaturesTestBasic, RetrieveContacts)
     auto ellipsoidFreeGroup = ellipsoid->FindFreeGroup();
     EXPECT_NE(nullptr, ellipsoidFreeGroup);
 
+    auto cone = world->GetModel("cone");
+    auto coneFreeGroup = cone->FindFreeGroup();
+    EXPECT_NE(nullptr, coneFreeGroup);
+
     auto box = world->GetModel("box");
 
     // step and get contacts
@@ -794,6 +838,7 @@ TYPED_TEST(SimulationFeaturesTestBasic, RetrieveContacts)
     unsigned int contactBoxCylinder = 0u;
     unsigned int contactBoxCapsule = 0u;
     unsigned int contactBoxEllipsoid = 0u;
+    unsigned int contactBoxCone = 0u;
 
     for (auto &contact : contacts)
     {
@@ -827,6 +872,11 @@ TYPED_TEST(SimulationFeaturesTestBasic, RetrieveContacts)
       {
         contactBoxEllipsoid++;
       }
+      else if ((m1->GetName() == "box" && m2->GetName() == "cone") ||
+          (m1->GetName() == "cone" && m2->GetName() == "box"))
+      {
+        contactBoxCone++;
+      }
       else
       {
         FAIL() << "There should not be contacts between: "
@@ -837,6 +887,7 @@ TYPED_TEST(SimulationFeaturesTestBasic, RetrieveContacts)
     EXPECT_NE(0u, contactBoxCylinder);
     EXPECT_NE(0u, contactBoxCapsule);
     EXPECT_NE(0u, contactBoxEllipsoid);
+    EXPECT_NE(0u, contactBoxCone);
 
     // move sphere away
     sphereFreeGroup->SetWorldPose(gz::math::eigen3::convert(
@@ -848,12 +899,13 @@ TYPED_TEST(SimulationFeaturesTestBasic, RetrieveContacts)
     contacts = world->GetContactsFromLastStep();
 
     // large box in the middle should be intersecting with cylinder, capsule,
-    // ellipsoid
+    // ellipsoid, cone
     EXPECT_NE(0u, contacts.size());
 
     contactBoxCylinder = 0u;
     contactBoxCapsule = 0u;
     contactBoxEllipsoid = 0u;
+    contactBoxCone = 0u;
     for (auto contact : contacts)
     {
       const auto &contactPoint =
@@ -881,6 +933,11 @@ TYPED_TEST(SimulationFeaturesTestBasic, RetrieveContacts)
       {
         contactBoxEllipsoid++;
       }
+      else if ((m1->GetName() == "box" && m2->GetName() == "cone") ||
+          (m1->GetName() == "cone" && m2->GetName() == "box"))
+      {
+        contactBoxCone++;
+      }
       else
       {
         FAIL() << "There should only be contacts between box and cylinder";
@@ -889,6 +946,7 @@ TYPED_TEST(SimulationFeaturesTestBasic, RetrieveContacts)
     EXPECT_NE(0u, contactBoxCylinder);
     EXPECT_NE(0u, contactBoxCapsule);
     EXPECT_NE(0u, contactBoxEllipsoid);
+    EXPECT_NE(0u, contactBoxCone);
 
     // move cylinder away
     cylinderFreeGroup->SetWorldPose(gz::math::eigen3::convert(
@@ -902,6 +960,10 @@ TYPED_TEST(SimulationFeaturesTestBasic, RetrieveContacts)
     ellipsoidFreeGroup->SetWorldPose(gz::math::eigen3::convert(
         gz::math::Pose3d(0, -100, -100, 0, 0, 0)));
 
+    // move cone away
+    coneFreeGroup->SetWorldPose(gz::math::eigen3::convert(
+        gz::math::Pose3d(100, -100, 0.5, 0, 0, 0)));
+
     // step and get contacts
     checkedOutput = StepWorld<Features>(world, false).first;
     EXPECT_FALSE(checkedOutput);
diff --git a/test/common_test/worlds/shapes.world b/test/common_test/worlds/shapes.world
index cb3bce5d9..0b66579d6 100644
--- a/test/common_test/worlds/shapes.world
+++ b/test/common_test/worlds/shapes.world
@@ -165,5 +165,39 @@
         </visual>
       </link>
     </model>
+
+    <model name="cone">
+      <pose>0 -6.5 0.5 0 0 0</pose>
+      <link name="cone_link">
+        <inertial>
+          <inertia>
+            <ixx>2</ixx>
+            <ixy>0</ixy>
+            <ixz>0</ixz>
+            <iyy>2</iyy>
+            <iyz>0</iyz>
+            <izz>2</izz>
+          </inertia>
+          <mass>2.0</mass>
+          <pose>0 0 -0.275 0 0 0</pose>
+        </inertial>
+        <collision name="cone_collision">
+          <geometry>
+            <cone>
+              <radius>0.5</radius>
+              <length>1.1</length>
+            </cone>
+          </geometry>
+        </collision>
+        <visual name="cone_visual">
+          <geometry>
+            <cone>
+              <radius>0.5</radius>
+              <length>1.1</length>
+            </cone>
+          </geometry>
+        </visual>
+      </link>
+    </model>
   </world>
 </sdf>
diff --git a/tpe/lib/src/Collision.cc b/tpe/lib/src/Collision.cc
index 114851f1a..1a340e0a4 100644
--- a/tpe/lib/src/Collision.cc
+++ b/tpe/lib/src/Collision.cc
@@ -81,6 +81,12 @@ void Collision::SetShape(const Shape &_shape)
       static_cast<const CapsuleShape *>(&_shape);
     this->dataPtr->shape.reset(new CapsuleShape(*typedShape));
   }
+  else if (_shape.GetType() == ShapeType::CONE)
+  {
+    const ConeShape *typedShape =
+      static_cast<const ConeShape *>(&_shape);
+    this->dataPtr->shape.reset(new ConeShape(*typedShape));
+  }
   else if (_shape.GetType() == ShapeType::CYLINDER)
   {
     const CylinderShape *typedShape =
diff --git a/tpe/lib/src/Shape.cc b/tpe/lib/src/Shape.cc
index 44fbc0e1f..4f1d12126 100644
--- a/tpe/lib/src/Shape.cc
+++ b/tpe/lib/src/Shape.cc
@@ -115,6 +115,45 @@ void CapsuleShape::UpdateBoundingBox()
   this->bbox = math::AxisAlignedBox(-halfSize, halfSize);
 }
 
+//////////////////////////////////////////////////
+ConeShape::ConeShape() : Shape()
+{
+  this->type = ShapeType::CONE;
+}
+
+//////////////////////////////////////////////////
+double ConeShape::GetRadius() const
+{
+  return this->radius;
+}
+
+//////////////////////////////////////////////////
+void ConeShape::SetRadius(double _radius)
+{
+  this->radius = _radius;
+  this->dirty = true;
+}
+
+//////////////////////////////////////////////////
+double ConeShape::GetLength() const
+{
+  return this->length;
+}
+
+//////////////////////////////////////////////////
+void ConeShape::SetLength(double _length)
+{
+  this->length = _length;
+  this->dirty = true;
+}
+
+//////////////////////////////////////////////////
+void ConeShape::UpdateBoundingBox()
+{
+  math::Vector3d halfSize(this->radius, this->radius, this->length*0.5);
+  this->bbox = math::AxisAlignedBox(-halfSize, halfSize);
+}
+
 //////////////////////////////////////////////////
 CylinderShape::CylinderShape() : Shape()
 {
diff --git a/tpe/lib/src/Shape.hh b/tpe/lib/src/Shape.hh
index 1cdb15415..55b5374f4 100644
--- a/tpe/lib/src/Shape.hh
+++ b/tpe/lib/src/Shape.hh
@@ -59,6 +59,9 @@ enum class GZ_PHYSICS_TPELIB_VISIBLE ShapeType
 
   /// \brief A ellipsoid shape.
   ELLIPSOID = 7,
+
+  /// \brief A cone shape.
+  CONE = 10,
 };
 
 
@@ -153,6 +156,42 @@ class GZ_PHYSICS_TPELIB_VISIBLE CapsuleShape : public Shape
   private: double length = 0.0;
 };
 
+/// \brief Cone geometry
+class GZ_PHYSICS_TPELIB_VISIBLE ConeShape : public Shape
+{
+  /// \brief Constructor
+  public: ConeShape();
+
+  /// \brief Destructor
+  public: virtual ~ConeShape() = default;
+
+  /// \brief Get cone radius
+  /// \return cone radius
+  public: double GetRadius() const;
+
+  /// \brief Set cone radius
+  /// \param[in] _radius Cone radius
+  public: void SetRadius(double _radius);
+
+  /// \brief Get cone length
+  /// \return Cone length
+  public: double GetLength() const;
+
+  /// \brief Set cone length
+  /// \param[in] _length Cone length
+  public: void SetLength(double _length);
+
+  // Documentation inherited
+  protected: virtual void UpdateBoundingBox() override;
+
+  /// \brief Cone radius
+  private: double radius = 0.0;
+
+  /// \brief Cone length
+  private: double length = 0.0;
+};
+
+
 /// \brief Cylinder geometry
 class GZ_PHYSICS_TPELIB_VISIBLE CylinderShape : public Shape
 {
diff --git a/tpe/plugin/src/SDFFeatures.cc b/tpe/plugin/src/SDFFeatures.cc
index 409e222c7..4286fdb49 100644
--- a/tpe/plugin/src/SDFFeatures.cc
+++ b/tpe/plugin/src/SDFFeatures.cc
@@ -19,6 +19,7 @@
 
 #include <sdf/Box.hh>
 #include <sdf/Capsule.hh>
+#include <sdf/Cone.hh>
 #include <sdf/Cylinder.hh>
 #include <sdf/Ellipsoid.hh>
 #include <sdf/Sphere.hh>
@@ -292,6 +293,14 @@ Identity SDFFeatures::ConstructSdfCollision(
     shape.SetLength(capsuleSdf->Length());
     collision->SetShape(shape);
   }
+  else if (geom->Type() == ::sdf::GeometryType::CONE)
+  {
+    const auto coneSdf = geom->ConeShape();
+    tpelib::ConeShape shape;
+    shape.SetRadius(coneSdf->Radius());
+    shape.SetLength(coneSdf->Length());
+    collision->SetShape(shape);
+  }
   else if (geom->Type() == ::sdf::GeometryType::CYLINDER)
   {
     const auto cylinderSdf = geom->CylinderShape();
diff --git a/tpe/plugin/src/ShapeFeatures.cc b/tpe/plugin/src/ShapeFeatures.cc
index c01b0b9c4..c4f6b7f84 100644
--- a/tpe/plugin/src/ShapeFeatures.cc
+++ b/tpe/plugin/src/ShapeFeatures.cc
@@ -82,19 +82,6 @@ Identity ShapeFeatures::AttachBoxShape(
   return this->GenerateInvalidId();
 }
 
-/////////////////////////////////////////////////
-Identity ShapeFeatures::CastToCylinderShape(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::CylinderShape*>(shape))
-      return this->GenerateIdentity(_shapeID, it->second);
-  }
-  return this->GenerateInvalidId();
-}
-
 /////////////////////////////////////////////////
 Identity ShapeFeatures::CastToCapsuleShape(const Identity &_shapeID) const
 {
@@ -172,6 +159,96 @@ Identity ShapeFeatures::AttachCapsuleShape(
   return this->GenerateInvalidId();
 }
 
+/////////////////////////////////////////////////
+Identity ShapeFeatures::CastToConeShape(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::ConeShape*>(shape))
+      return this->GenerateIdentity(_shapeID, it->second);
+  }
+  return this->GenerateInvalidId();
+}
+
+/////////////////////////////////////////////////
+double ShapeFeatures::GetConeShapeRadius(
+  const Identity &_coneID) const
+{
+  // assume _coneID ~= _collisionID
+  auto it = this->collisions.find(_coneID);
+  if (it != this->collisions.end() && it->second != nullptr)
+  {
+    auto *shape = it->second->collision->GetShape();
+    if (shape != nullptr)
+    {
+      auto *cone = static_cast<tpelib::ConeShape*>(shape);
+      return cone->GetRadius();
+    }
+  }
+  // return invalid radius if no collision found
+  return -1.0;
+}
+
+/////////////////////////////////////////////////
+double ShapeFeatures::GetConeShapeHeight(
+  const Identity &_coneID) const
+{
+  // assume _coneID ~= _collisionID
+  auto it = this->collisions.find(_coneID);
+  if (it != this->collisions.end() && it->second != nullptr)
+  {
+    auto *shape = it->second->collision->GetShape();
+    if (shape != nullptr)
+    {
+      auto *cone = static_cast<tpelib::ConeShape*>(shape);
+      return cone->GetLength();
+    }
+  }
+  // return invalid height if no collision found
+  return -1.0;
+}
+
+/////////////////////////////////////////////////
+Identity ShapeFeatures::AttachConeShape(
+  const Identity &_linkID,
+  const std::string &_name,
+  const double _radius,
+  const double _height,
+  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::ConeShape coneshape;
+    coneshape.SetRadius(_radius);
+    coneshape.SetLength(_height);
+    collision.SetShape(coneshape);
+
+    return this->AddCollision(_linkID, collision);
+  }
+  return this->GenerateInvalidId();
+}
+
+/////////////////////////////////////////////////
+Identity ShapeFeatures::CastToCylinderShape(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::CylinderShape*>(shape))
+      return this->GenerateIdentity(_shapeID, it->second);
+  }
+  return this->GenerateInvalidId();
+}
+
 /////////////////////////////////////////////////
 double ShapeFeatures::GetCylinderShapeRadius(
   const Identity &_cylinderID) const
diff --git a/tpe/plugin/src/ShapeFeatures.hh b/tpe/plugin/src/ShapeFeatures.hh
index e09564e38..6b90dbc5c 100644
--- a/tpe/plugin/src/ShapeFeatures.hh
+++ b/tpe/plugin/src/ShapeFeatures.hh
@@ -23,6 +23,7 @@
 #include <gz/physics/Shape.hh>
 #include <gz/physics/BoxShape.hh>
 #include <gz/physics/CapsuleShape.hh>
+#include <gz/physics/ConeShape.hh>
 #include <gz/physics/CylinderShape.hh>
 #include <gz/physics/EllipsoidShape.hh>
 #include <gz/physics/mesh/MeshShape.hh>
@@ -42,6 +43,9 @@ struct ShapeFeatureList : FeatureList<
   GetCapsuleShapeProperties,
   AttachCapsuleShapeFeature,
 
+  GetConeShapeProperties,
+  AttachConeShapeFeature,
+
   GetCylinderShapeProperties,
   AttachCylinderShapeFeature,
 
@@ -89,6 +93,23 @@ class ShapeFeatures :
     double _length,
     const Pose3d &_pose) override;
 
+  // ----- Cone Features -----
+  public: Identity CastToConeShape(
+    const Identity &_shapeID) const override;
+
+  public: double GetConeShapeRadius(
+    const Identity &_coneID) const override;
+
+  public: double GetConeShapeHeight(
+    const Identity &_coneID) const override;
+
+  public: Identity AttachConeShape(
+    const Identity &_linkID,
+    const std::string &_name,
+    double _radius,
+    double _height,
+    const Pose3d &_pose) override;
+
   // ----- Cylinder Features -----
   public: Identity CastToCylinderShape(
     const Identity &_shapeID) const override;
@@ -106,7 +127,7 @@ class ShapeFeatures :
     double _height,
     const Pose3d &_pose) override;
 
-  // ----- Capsule Features -----
+  // ----- Ellipsoid Features -----
   public: Identity CastToEllipsoidShape(
     const Identity &_shapeID) const override;
 
diff --git a/tpe/plugin/src/SimulationFeatures_TEST.cc b/tpe/plugin/src/SimulationFeatures_TEST.cc
index ddecee5de..cf44d5485 100644
--- a/tpe/plugin/src/SimulationFeatures_TEST.cc
+++ b/tpe/plugin/src/SimulationFeatures_TEST.cc
@@ -603,6 +603,10 @@ TEST_P(SimulationFeatures_TEST, RetrieveContacts)
     auto ellipsoidFreeGroup = ellipsoid->FindFreeGroup();
     EXPECT_NE(nullptr, ellipsoidFreeGroup);
 
+    auto cone = world->GetModel("cone");
+    auto coneFreeGroup = cone->FindFreeGroup();
+    EXPECT_NE(nullptr, coneFreeGroup);
+
     auto box = world->GetModel("box");
 
     // step and get contacts
@@ -611,12 +615,13 @@ TEST_P(SimulationFeatures_TEST, RetrieveContacts)
     auto contacts = world->GetContactsFromLastStep();
 
     // large box in the middle should be intersecting with sphere, cylinder,
-    // capsule and ellipsoid
-    EXPECT_EQ(4u, contacts.size());
+    // capsule, ellipsoid, and cone
+    EXPECT_EQ(5u, contacts.size());
     unsigned int contactBoxSphere = 0u;
     unsigned int contactBoxCylinder = 0u;
     unsigned int contactBoxCapsule = 0u;
     unsigned int contactBoxEllipsoid = 0u;
+    unsigned int contactBoxCone = 0u;
 
     for (auto &contact : contacts)
     {
@@ -661,6 +666,16 @@ TEST_P(SimulationFeatures_TEST, RetrieveContacts)
         EXPECT_TRUE(physics::test::Equal(expectedContactPos,
             contactPoint.point, 1e-6));
       }
+      else if ((m1->GetName() == "box" && m2->GetName() == "cone") ||
+              (m1->GetName() == "cone" && m2->GetName() == "box"))
+      {
+        contactBoxCone++;
+        Eigen::Vector3d expectedContactPos = Eigen::Vector3d(0.0, -6.5, 0.5);
+        EXPECT_TRUE(physics::test::Equal(expectedContactPos,
+            contactPoint.point, 1e-6))
+            << "expected: " << expectedContactPos << "\n"
+            << "  actual: " << contactPoint.point;
+      }
       else
       {
         FAIL() << "There should not be contacts between: "
@@ -671,6 +686,7 @@ TEST_P(SimulationFeatures_TEST, RetrieveContacts)
     EXPECT_EQ(1u, contactBoxCylinder);
     EXPECT_EQ(1u, contactBoxCapsule);
     EXPECT_EQ(1u, contactBoxEllipsoid);
+    EXPECT_EQ(1u, contactBoxCone);
 
     // move sphere away
     sphereFreeGroup->SetWorldPose(math::eigen3::convert(
@@ -682,12 +698,13 @@ TEST_P(SimulationFeatures_TEST, RetrieveContacts)
     contacts = world->GetContactsFromLastStep();
 
     // large box in the middle should be intersecting with cylinder, capsule,
-    // ellipsoid
-    EXPECT_EQ(3u, contacts.size());
+    // ellipsoid, and cone
+    EXPECT_EQ(4u, contacts.size());
 
     contactBoxCylinder = 0u;
     contactBoxCapsule = 0u;
     contactBoxEllipsoid = 0u;
+    contactBoxCone = 0u;
     for (auto contact : contacts)
     {
       const auto &contactPoint = contact.Get<::TestContactPoint>();
@@ -723,6 +740,14 @@ TEST_P(SimulationFeatures_TEST, RetrieveContacts)
         EXPECT_TRUE(physics::test::Equal(expectedContactPos,
             contactPoint.point, 1e-6));
       }
+      else if ((m1->GetName() == "box" && m2->GetName() == "cone") ||
+              (m1->GetName() == "cone" && m2->GetName() == "box"))
+      {
+        contactBoxCone++;
+        Eigen::Vector3d expectedContactPos = Eigen::Vector3d(0.0, -6.5, 0.5);
+        EXPECT_TRUE(physics::test::Equal(expectedContactPos,
+            contactPoint.point, 1e-6));
+      }
       else
       {
         FAIL() << "There should only be contacts between box and cylinder";
@@ -731,6 +756,7 @@ TEST_P(SimulationFeatures_TEST, RetrieveContacts)
     EXPECT_EQ(1u, contactBoxCylinder);
     EXPECT_EQ(1u, contactBoxCapsule);
     EXPECT_EQ(1u, contactBoxEllipsoid);
+    EXPECT_EQ(1u, contactBoxCone);
 
     // move cylinder away
     cylinderFreeGroup->SetWorldPose(math::eigen3::convert(
@@ -744,6 +770,10 @@ TEST_P(SimulationFeatures_TEST, RetrieveContacts)
     ellipsoidFreeGroup->SetWorldPose(math::eigen3::convert(
         math::Pose3d(0, -100, -100, 0, 0, 0)));
 
+    // move cone away
+    coneFreeGroup->SetWorldPose(math::eigen3::convert(
+        math::Pose3d(0, -100, -200, 0, 0, 0)));
+
     // step and get contacts
     checkedOutput = StepWorld(world, false);
     EXPECT_FALSE(checkedOutput);