diff --git a/include/sdf/Cone.hh b/include/sdf/Cone.hh
new file mode 100644
index 000000000..641c5a304
--- /dev/null
+++ b/include/sdf/Cone.hh
@@ -0,0 +1,108 @@
+/*
+ * 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 SDF_CONE_HH_
+#define SDF_CONE_HH_
+
+#include <optional>
+
+#include <gz/math/Cone.hh>
+#include <gz/math/Inertial.hh>
+#include <gz/utils/ImplPtr.hh>
+#include <sdf/Error.hh>
+#include <sdf/Element.hh>
+#include <sdf/sdf_config.h>
+
+namespace sdf
+{
+  // Inline bracket to help doxygen filtering.
+  inline namespace SDF_VERSION_NAMESPACE {
+  /// \brief Cone represents a cone shape, and is usually accessed
+  /// through a Geometry.
+  class SDFORMAT_VISIBLE Cone
+  {
+    /// \brief Constructor
+    public: Cone();
+
+    /// \brief Load the cone geometry based on a element pointer.
+    /// This is *not* the usual entry point. Typical usage of the SDF DOM is
+    /// through the Root object.
+    /// \param[in] _sdf The SDF Element pointer
+    /// \return Errors, which is a vector of Error objects. Each Error includes
+    /// an error code and message. An empty vector indicates no error.
+    public: Errors Load(ElementPtr _sdf);
+
+    /// \brief Get the cone's radius in meters.
+    /// \return The radius of the cone in meters.
+    public: double Radius() const;
+
+    /// \brief Set the cone's radius in meters.
+    /// \param[in] _radius The radius of the cone in meters.
+    public: void SetRadius(double _radius);
+
+    /// \brief Get the cone's length in meters.
+    /// \return The length of the cone in meters.
+    public: double Length() const;
+
+    /// \brief Set the cone's length in meters.
+    /// \param[in] _length The length of the cone in meters.
+    public: void SetLength(double _length);
+
+    /// \brief Get a pointer to the SDF element that was used during
+    /// load.
+    /// \return SDF element pointer. The value will be nullptr if Load has
+    /// not been called.
+    public: sdf::ElementPtr Element() const;
+
+    /// \brief Get the Gazebo Math representation of this cone.
+    /// \return A const reference to a gz::math::Sphered object.
+    public: const gz::math::Coned &Shape() const;
+
+    /// \brief Get a mutable Gazebo Math representation of this cone.
+    /// \return A reference to a gz::math::Coned object.
+    public: gz::math::Coned &Shape();
+
+    /// \brief Calculate and return the Inertial values for the cone. In
+    /// order to calculate the inertial properties, the function mutates the
+    /// object by updating its material properties.
+    /// \param[in] _density Density of the cone in kg/m^3
+    /// \return A std::optional with gz::math::Inertiald object or std::nullopt
+    public: std::optional<gz::math::Inertiald>
+            CalculateInertial(double _density);
+
+    /// \brief Create and return an SDF element filled with data from this
+    /// cone.
+    /// Note that parameter passing functionality is not captured with this
+    /// function.
+    /// \return SDF element pointer with updated cone values.
+    public: sdf::ElementPtr ToElement() const;
+
+    /// \brief Create and return an SDF element filled with data from this
+    /// cone.
+    /// Note that parameter passing functionality is not captured with this
+    /// function.
+    /// \param[out] _errors Vector of errors.
+    /// \return SDF element pointer with updated cone values.
+    public: sdf::ElementPtr ToElement(sdf::Errors &_errors) const;
+
+    /// \brief Private data pointer.
+    GZ_UTILS_IMPL_PTR(dataPtr)
+  };
+  }
+}
+#endif
diff --git a/include/sdf/Geometry.hh b/include/sdf/Geometry.hh
index 800dfb9a6..5f5443ec0 100644
--- a/include/sdf/Geometry.hh
+++ b/include/sdf/Geometry.hh
@@ -37,6 +37,7 @@ namespace sdf
   // Forward declare private data class.
   class Box;
   class Capsule;
+  class Cone;
   class Cylinder;
   class Ellipsoid;
   class Heightmap;
@@ -79,6 +80,9 @@ namespace sdf
 
     /// \brief A polyline geometry.
     POLYLINE = 9,
+
+    /// \brief A polyline geometry.
+    CONE = 10,
   };
 
   /// \brief Geometry provides access to a shape, such as a Box. Use the
@@ -140,6 +144,17 @@ namespace sdf
     /// \param[in] _capsule The capsule shape.
     public: void SetCapsuleShape(const Capsule &_capsule);
 
+    /// \brief Get the cone geometry, or nullptr if the contained
+    /// geometry is not a cone.
+    /// \return Pointer to the visual's cone geometry, or nullptr if the
+    /// geometry is not a cone.
+    /// \sa GeometryType Type() const
+    public: const Cone *ConeShape() const;
+
+    /// \brief Set the cone shape.
+    /// \param[in] _cone The cone shape.
+    public: void SetConeShape(const Cone &_cone);
+
     /// \brief Get the cylinder geometry, or nullptr if the contained
     /// geometry is not a cylinder.
     /// \return Pointer to the visual's cylinder geometry, or nullptr if the
diff --git a/include/sdf/Visual.hh b/include/sdf/Visual.hh
index 571cfcdd9..0f5808851 100644
--- a/include/sdf/Visual.hh
+++ b/include/sdf/Visual.hh
@@ -22,6 +22,7 @@
 #include <gz/math/Pose3.hh>
 #include <gz/utils/ImplPtr.hh>
 #include "sdf/Box.hh"
+#include "sdf/Cone.hh"
 #include "sdf/Cylinder.hh"
 #include "sdf/Element.hh"
 #include "sdf/Material.hh"
diff --git a/python/CMakeLists.txt b/python/CMakeLists.txt
index f9cc0917d..864d38c18 100644
--- a/python/CMakeLists.txt
+++ b/python/CMakeLists.txt
@@ -52,6 +52,7 @@ pybind11_add_module(${BINDINGS_MODULE_NAME} MODULE
   src/sdf/pyCamera.cc
   src/sdf/pyCapsule.cc
   src/sdf/pyCollision.cc
+  src/sdf/pyCone.cc
   src/sdf/pyConvexDecomposition.cc
   src/sdf/pyCylinder.cc
   src/sdf/pyElement.cc
@@ -135,6 +136,7 @@ if (BUILD_TESTING AND NOT WIN32)
     pyCamera_TEST
     pyCapsule_TEST
     pyCollision_TEST
+    pyCone_TEST
     pyCylinder_TEST
     pyElement_TEST
     pyEllipsoid_TEST
diff --git a/python/src/sdf/_gz_sdformat_pybind11.cc b/python/src/sdf/_gz_sdformat_pybind11.cc
index 0a2c9fff4..80892100e 100644
--- a/python/src/sdf/_gz_sdformat_pybind11.cc
+++ b/python/src/sdf/_gz_sdformat_pybind11.cc
@@ -26,6 +26,7 @@
 #include "pyCamera.hh"
 #include "pyCapsule.hh"
 #include "pyCollision.hh"
+#include "pyCone.hh"
 #include "pyConvexDecomposition.hh"
 #include "pyCylinder.hh"
 #include "pyElement.hh"
@@ -87,6 +88,7 @@ PYBIND11_MODULE(BINDINGS_MODULE_NAME, m) {
   sdf::python::defineCamera(m);
   sdf::python::defineCapsule(m);
   sdf::python::defineCollision(m);
+  sdf::python::defineCone(m);
   sdf::python::defineConvexDecomposition(m);
   sdf::python::defineContact(m);
   sdf::python::defineCylinder(m);
diff --git a/python/src/sdf/pyCone.cc b/python/src/sdf/pyCone.cc
new file mode 100644
index 000000000..05cb9cb15
--- /dev/null
+++ b/python/src/sdf/pyCone.cc
@@ -0,0 +1,60 @@
+/*
+ * 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.
+ */
+#include "pyCone.hh"
+
+#include <pybind11/pybind11.h>
+
+#include "sdf/Cone.hh"
+
+using namespace pybind11::literals;
+
+namespace sdf
+{
+// Inline bracket to help doxygen filtering.
+inline namespace SDF_VERSION_NAMESPACE {
+namespace python
+{
+/////////////////////////////////////////////////
+void defineCone(pybind11::object module)
+{
+  pybind11::class_<sdf::Cone>(module, "Cone")
+    .def(pybind11::init<>())
+    .def(pybind11::init<sdf::Cone>())
+    .def("radius", &sdf::Cone::Radius,
+         "Get the cone's radius in meters.")
+    .def("set_radius", &sdf::Cone::SetRadius,
+         "Set the cone's radius in meters.")
+    .def("length", &sdf::Cone::Length,
+         "Get the cone's length in meters.")
+    .def("set_length", &sdf::Cone::SetLength,
+         "Set the cone's length in meters.")
+    .def(
+        "shape",
+        pybind11::overload_cast<>(&sdf::Cone::Shape, pybind11::const_),
+        pybind11::return_value_policy::reference,
+        "Get a mutable Gazebo Math representation of this Cone.")
+    .def("__copy__", [](const sdf::Cone &self) {
+      return sdf::Cone(self);
+    })
+    .def("__deepcopy__", [](const sdf::Cone &self, pybind11::dict) {
+      return sdf::Cone(self);
+    }, "memo"_a);
+}
+}  // namespace python
+}  // namespace SDF_VERSION_NAMESPACE
+}  // namespace sdf
diff --git a/python/src/sdf/pyCone.hh b/python/src/sdf/pyCone.hh
new file mode 100644
index 000000000..1f7f3dc85
--- /dev/null
+++ b/python/src/sdf/pyCone.hh
@@ -0,0 +1,43 @@
+/*
+ * 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 SDFORMAT_PYTHON_CONE_HH_
+#define SDFORMAT_PYTHON_CONE_HH_
+
+#include <pybind11/pybind11.h>
+
+#include "sdf/Cone.hh"
+
+#include "sdf/config.hh"
+
+namespace sdf
+{
+// Inline bracket to help doxygen filtering.
+inline namespace SDF_VERSION_NAMESPACE {
+namespace python
+{
+/// Define a pybind11 wrapper for an sdf::Cone
+/**
+ * \param[in] module a pybind11 module to add the definition to
+ */
+void defineCone(pybind11::object module);
+}  // namespace python
+}  // namespace SDF_VERSION_NAMESPACE
+}  // namespace sdf
+
+#endif  // SDFORMAT_PYTHON_CONE_HH_
diff --git a/python/src/sdf/pyGeometry.cc b/python/src/sdf/pyGeometry.cc
index ffcbabbe5..a30d3106f 100644
--- a/python/src/sdf/pyGeometry.cc
+++ b/python/src/sdf/pyGeometry.cc
@@ -23,6 +23,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"
@@ -64,6 +65,12 @@ void defineGeometry(pybind11::object module)
          "geometry is not a capsule.")
     .def("set_capsule_shape", &sdf::Geometry::SetCapsuleShape,
          "Set the capsule shape.")
+    .def("cone_shape", &sdf::Geometry::ConeShape,
+         pybind11::return_value_policy::reference,
+         "Get the cone geometry, or None if the contained "
+         "geometry is not a cone.")
+    .def("set_cone_shape", &sdf::Geometry::SetConeShape,
+         "Set the cone shape.")
     .def("cylinder_shape", &sdf::Geometry::CylinderShape,
          pybind11::return_value_policy::reference,
          "Get the cylinder geometry, or None if the contained "
@@ -104,6 +111,7 @@ void defineGeometry(pybind11::object module)
     pybind11::enum_<sdf::GeometryType>(module, "GeometryType")
       .value("EMPTY", sdf::GeometryType::EMPTY)
       .value("BOX", sdf::GeometryType::BOX)
+      .value("CONE", sdf::GeometryType::CONE)
       .value("CYLINDER", sdf::GeometryType::CYLINDER)
       .value("PLANE", sdf::GeometryType::PLANE)
       .value("SPHERE", sdf::GeometryType::SPHERE)
diff --git a/python/test/pyCollision_TEST.py b/python/test/pyCollision_TEST.py
index 8d8d96735..7068c53d2 100644
--- a/python/test/pyCollision_TEST.py
+++ b/python/test/pyCollision_TEST.py
@@ -14,7 +14,7 @@
 
 import copy
 from gz_test_deps.math import Inertiald, MassMatrix3d, Pose3d, Vector3d
-from gz_test_deps.sdformat import (Box, Collision, Contact, Cylinder, Error,
+from gz_test_deps.sdformat import (Box, Collision, Cone, Contact, Cylinder, Error,
                                    Geometry, ParserConfig, Plane, Root, Surface, Sphere,
                                    SDFErrorsException)
 import gz_test_deps.sdformat as sdf
@@ -61,6 +61,7 @@ def test_default_construction(self):
         self.assertNotEqual(None, collision.geometry())
         self.assertEqual(sdf.GeometryType.EMPTY, collision.geometry().type())
         self.assertEqual(None, collision.geometry().box_shape())
+        self.assertEqual(None, collision.geometry().cone_shape())
         self.assertEqual(None, collision.geometry().cylinder_shape())
         self.assertEqual(None, collision.geometry().plane_shape())
         self.assertEqual(None, collision.geometry().sphere_shape())
diff --git a/python/test/pyCone_TEST.py b/python/test/pyCone_TEST.py
new file mode 100644
index 000000000..e36e65f63
--- /dev/null
+++ b/python/test/pyCone_TEST.py
@@ -0,0 +1,114 @@
+# 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.
+
+import copy
+
+import math
+
+from gz_test_deps.sdformat import Cone
+
+import unittest
+
+
+class ConeTEST(unittest.TestCase):
+
+  def test_default_construction(self):
+    cone = Cone()
+
+    self.assertEqual(math.pi * math.pow(0.5, 2) * 1.0 / 3.0,
+                     cone.shape().volume())
+
+    self.assertEqual(0.5, cone.radius())
+    self.assertEqual(1.0, cone.length())
+
+    cone.set_radius(0.5)
+    cone.set_length(2.3)
+
+    self.assertEqual(0.5, cone.radius())
+    self.assertEqual(2.3, cone.length())
+
+  def test_assignment(self):
+    cone = Cone()
+    cone.set_radius(0.2)
+    cone.set_length(3.0)
+    self.assertEqual(math.pi * math.pow(0.2, 2) * 3.0 / 3.0,
+                   cone.shape().volume())
+
+    cone2 = cone
+    self.assertEqual(0.2, cone2.radius())
+    self.assertEqual(3.0, cone2.length())
+
+    self.assertEqual(math.pi * math.pow(0.2, 2) * 3.0 / 3.0,
+                   cone2.shape().volume())
+    self.assertEqual(0.2, cone2.shape().radius())
+    self.assertEqual(3.0, cone2.shape().length())
+
+    cone.set_radius(2.0)
+    cone.set_length(0.3)
+
+    self.assertEqual(2.0, cone.radius())
+    self.assertEqual(0.3, cone.length())
+    self.assertEqual(2.0, cone2.radius())
+    self.assertEqual(0.3, cone2.length())
+
+
+  def test_copy_construction(self):
+    cone = Cone();
+    cone.set_radius(0.2)
+    cone.set_length(3.0)
+
+    cone2 = Cone(cone)
+    self.assertEqual(0.2, cone2.radius())
+    self.assertEqual(3.0, cone2.length())
+
+    cone.set_radius(2.)
+    cone.set_length(0.3)
+
+    self.assertEqual(2, cone.radius())
+    self.assertEqual(0.3, cone.length())
+    self.assertEqual(0.2, cone2.radius())
+    self.assertEqual(3.0, cone2.length())
+
+  def test_deepcopy(self):
+    cone = Cone();
+    cone.set_radius(0.2)
+    cone.set_length(3.0)
+
+    cone2 = copy.deepcopy(cone);
+    self.assertEqual(0.2, cone2.radius())
+    self.assertEqual(3.0, cone2.length())
+
+    cone.set_radius(2.)
+    cone.set_length(0.3)
+
+    self.assertEqual(2, cone.radius())
+    self.assertEqual(0.3, cone.length())
+    self.assertEqual(0.2, cone2.radius())
+    self.assertEqual(3.0, cone2.length())
+
+  def test_shape(self):
+    cone = Cone();
+    self.assertEqual(0.5, cone.radius())
+    self.assertEqual(1.0, cone.length())
+
+    cone.shape().set_radius(0.123)
+    cone.shape().set_length(0.456)
+    self.assertEqual(0.123, cone.radius())
+    self.assertEqual(0.456, cone.length())
+
+
+if __name__ == '__main__':
+    unittest.main()
diff --git a/python/test/pyGeometry_TEST.py b/python/test/pyGeometry_TEST.py
index c4cd9c511..e76506051 100644
--- a/python/test/pyGeometry_TEST.py
+++ b/python/test/pyGeometry_TEST.py
@@ -13,7 +13,7 @@
 # limitations under the License.
 
 import copy
-from gz_test_deps.sdformat import (Element, Error, Geometry, Box, Capsule, Cylinder, Ellipsoid,
+from gz_test_deps.sdformat import (Element, Error, Geometry, Box, Capsule, Cone, Cylinder, Ellipsoid,
                                    Mesh, ParserConfig, Plane, Sphere)
 from gz_test_deps.math import Inertiald, MassMatrix3d, Pose3d, Vector3d, Vector2d
 import gz_test_deps.sdformat as sdf
@@ -33,6 +33,9 @@ def test_default_construction(self):
     geom.set_type(sdf.GeometryType.CAPSULE)
     self.assertEqual(sdf.GeometryType.CAPSULE, geom.type())
 
+    geom.set_type(sdf.GeometryType.CONE)
+    self.assertEqual(sdf.GeometryType.CONE, geom.type())
+
     geom.set_type(sdf.GeometryType.CYLINDER)
     self.assertEqual(sdf.GeometryType.CYLINDER, geom.type())
 
@@ -123,6 +126,21 @@ def test_capsule(self):
     self.assertEqual(4.56, geom.capsule_shape().length())
 
 
+  def test_cone(self):
+    geom = Geometry()
+    geom.set_type(sdf.GeometryType.CONE)
+
+    coneShape = Cone()
+    coneShape.set_radius(0.123)
+    coneShape.set_length(4.56)
+    geom.set_cone_shape(coneShape)
+
+    self.assertEqual(sdf.GeometryType.CONE, geom.type())
+    self.assertNotEqual(None, geom.cone_shape())
+    self.assertEqual(0.123, geom.cone_shape().radius())
+    self.assertEqual(4.56, geom.cone_shape().length())
+
+
   def test_cylinder(self):
     geom = Geometry()
     geom.set_type(sdf.GeometryType.CYLINDER)
diff --git a/python/test/pyVisual_TEST.py b/python/test/pyVisual_TEST.py
index bedadb0eb..1e07f1412 100644
--- a/python/test/pyVisual_TEST.py
+++ b/python/test/pyVisual_TEST.py
@@ -65,6 +65,7 @@ def test_default_construction(self):
         self.assertNotEqual(None, visual.geometry())
         self.assertEqual(sdf.GeometryType.EMPTY, visual.geometry().type())
         self.assertEqual(None, visual.geometry().box_shape())
+        self.assertEqual(None, visual.geometry().cone_shape())
         self.assertEqual(None, visual.geometry().cylinder_shape())
         self.assertEqual(None, visual.geometry().plane_shape())
         self.assertEqual(None, visual.geometry().sphere_shape())
diff --git a/sdf/1.11/CMakeLists.txt b/sdf/1.11/CMakeLists.txt
index 2e56bf0a7..fb7f6a38a 100644
--- a/sdf/1.11/CMakeLists.txt
+++ b/sdf/1.11/CMakeLists.txt
@@ -11,6 +11,7 @@ set (sdfs
   camera.sdf
   capsule_shape.sdf
   collision.sdf
+  cone_shape.sdf
   contact.sdf
   cylinder_shape.sdf
   ellipsoid_shape.sdf
diff --git a/sdf/1.11/cone_shape.sdf b/sdf/1.11/cone_shape.sdf
new file mode 100644
index 000000000..5805ba6bb
--- /dev/null
+++ b/sdf/1.11/cone_shape.sdf
@@ -0,0 +1,9 @@
+<element name="cone" required="0">
+  <description>Cone shape</description>
+  <element name="radius" type="double" default="1" required="1">
+    <description>Radius of the cone</description>
+  </element>
+  <element name="length" type="double" default="1" required="1">
+    <description>Length of the cone along the z axis</description>
+  </element>
+</element>
diff --git a/sdf/1.11/geometry.sdf b/sdf/1.11/geometry.sdf
index 884902afb..447338dbf 100644
--- a/sdf/1.11/geometry.sdf
+++ b/sdf/1.11/geometry.sdf
@@ -8,6 +8,7 @@
 
   <include filename="box_shape.sdf" required="0"/>
   <include filename="capsule_shape.sdf" required="0"/>
+  <include filename="cone_shape.sdf" required="0"/>
   <include filename="cylinder_shape.sdf" required="0"/>
   <include filename="ellipsoid_shape.sdf" required="0"/>
   <include filename="heightmap_shape.sdf" required="0"/>
diff --git a/src/Collision_TEST.cc b/src/Collision_TEST.cc
index 1ab3d1b18..aaa0cd2dc 100644
--- a/src/Collision_TEST.cc
+++ b/src/Collision_TEST.cc
@@ -84,6 +84,7 @@ TEST(DOMcollision, Construction)
   ASSERT_NE(nullptr, collision.Geom());
   EXPECT_EQ(sdf::GeometryType::EMPTY, collision.Geom()->Type());
   EXPECT_EQ(nullptr, collision.Geom()->BoxShape());
+  EXPECT_EQ(nullptr, collision.Geom()->ConeShape());
   EXPECT_EQ(nullptr, collision.Geom()->CylinderShape());
   EXPECT_EQ(nullptr, collision.Geom()->PlaneShape());
   EXPECT_EQ(nullptr, collision.Geom()->SphereShape());
diff --git a/src/Cone.cc b/src/Cone.cc
new file mode 100644
index 000000000..7e094bdd0
--- /dev/null
+++ b/src/Cone.cc
@@ -0,0 +1,189 @@
+/*
+ * 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.
+ *
+*/
+#include <optional>
+#include <sstream>
+#include <utility>
+
+#include <gz/math/Inertial.hh>
+#include <gz/math/Cone.hh>
+#include "sdf/Cone.hh"
+#include "sdf/parser.hh"
+#include "Utils.hh"
+
+using namespace sdf;
+
+// Private data class
+class sdf::Cone::Implementation
+{
+  // A cone with a length of 1 meter and radius if 0.5 meters.
+  public: gz::math::Coned cone{1.0, 0.5};
+
+  /// \brief The SDF element pointer used during load.
+  public: sdf::ElementPtr sdf;
+};
+
+/////////////////////////////////////////////////
+Cone::Cone()
+  : dataPtr(gz::utils::MakeImpl<Implementation>())
+{
+}
+
+/////////////////////////////////////////////////
+Errors Cone::Load(ElementPtr _sdf)
+{
+  Errors errors;
+
+  this->dataPtr->sdf = _sdf;
+
+  // Check that sdf is a valid pointer
+  if (!_sdf)
+  {
+    errors.push_back({ErrorCode::ELEMENT_MISSING,
+        "Attempting to load a cone, but the provided SDF "
+        "element is null."});
+    return errors;
+  }
+
+  // We need a cone child element
+  if (_sdf->GetName() != "cone")
+  {
+    errors.push_back({ErrorCode::ELEMENT_INCORRECT_TYPE,
+        "Attempting to load a cone geometry, but the provided SDF "
+        "element is not a <cone>."});
+    return errors;
+  }
+
+  {
+    std::pair<double, bool> pair = _sdf->Get<double>(errors, "radius",
+        this->dataPtr->cone.Radius());
+
+    if (!pair.second)
+    {
+      std::stringstream ss;
+      ss << "Invalid <radius> data for a <cone> geometry. "
+         << "Using a radius of "
+         << this->dataPtr->cone.Radius() << ".";
+      errors.push_back({ErrorCode::ELEMENT_INVALID, ss.str()});
+    }
+    this->dataPtr->cone.SetRadius(pair.first);
+  }
+
+  {
+    std::pair<double, bool> pair = _sdf->Get<double>(errors, "length",
+        this->dataPtr->cone.Length());
+
+    if (!pair.second)
+    {
+      std::stringstream ss;
+      ss << "Invalid <length> data for a <cone> geometry. "
+         << "Using a length of "
+         << this->dataPtr->cone.Length() << ".";
+      errors.push_back({ErrorCode::ELEMENT_INVALID, ss.str()});
+    }
+    this->dataPtr->cone.SetLength(pair.first);
+  }
+
+  return errors;
+}
+
+//////////////////////////////////////////////////
+double Cone::Radius() const
+{
+  return this->dataPtr->cone.Radius();
+}
+
+//////////////////////////////////////////////////
+void Cone::SetRadius(double _radius)
+{
+  this->dataPtr->cone.SetRadius(_radius);
+}
+
+//////////////////////////////////////////////////
+double Cone::Length() const
+{
+  return this->dataPtr->cone.Length();
+}
+
+//////////////////////////////////////////////////
+void Cone::SetLength(double _length)
+{
+  this->dataPtr->cone.SetLength(_length);
+}
+
+/////////////////////////////////////////////////
+sdf::ElementPtr Cone::Element() const
+{
+  return this->dataPtr->sdf;
+}
+
+/////////////////////////////////////////////////
+const gz::math::Coned &Cone::Shape() const
+{
+  return this->dataPtr->cone;
+}
+
+/////////////////////////////////////////////////
+gz::math::Coned &Cone::Shape()
+{
+  return this->dataPtr->cone;
+}
+
+std::optional<gz::math::Inertiald> Cone::CalculateInertial(double _density)
+{
+  gz::math::Material material = gz::math::Material(_density);
+  this->dataPtr->cone.SetMat(material);
+
+  auto coneMassMatrix = this->dataPtr->cone.MassMatrix();
+
+  if (!coneMassMatrix)
+  {
+    return std::nullopt;
+  }
+  else
+  {
+    gz::math::Inertiald coneInertial;
+    coneInertial.SetMassMatrix(coneMassMatrix.value());
+    coneInertial.SetPose({0, 0, -0.25 * this->dataPtr->cone.Length(), 0, 0, 0});
+    return std::make_optional(coneInertial);
+  }
+}
+
+/////////////////////////////////////////////////
+sdf::ElementPtr Cone::ToElement() const
+{
+  sdf::Errors errors;
+  auto result = this->ToElement(errors);
+  sdf::throwOrPrintErrors(errors);
+  return result;
+}
+
+/////////////////////////////////////////////////
+sdf::ElementPtr Cone::ToElement(sdf::Errors &_errors) const
+{
+  sdf::ElementPtr elem(new sdf::Element);
+  sdf::initFile("cone_shape.sdf", elem);
+
+  sdf::ElementPtr radiusElem = elem->GetElement("radius", _errors);
+  radiusElem->Set<double>(_errors, this->Radius());
+
+  sdf::ElementPtr lengthElem = elem->GetElement("length", _errors);
+  lengthElem->Set<double>(_errors, this->Length());
+
+  return elem;
+}
diff --git a/src/Cone_TEST.cc b/src/Cone_TEST.cc
new file mode 100644
index 000000000..59c5dbf3e
--- /dev/null
+++ b/src/Cone_TEST.cc
@@ -0,0 +1,288 @@
+/*
+ * 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.
+ *
+*/
+
+#include <optional>
+#include <gtest/gtest.h>
+#include "sdf/Cone.hh"
+#include "test_utils.hh"
+#include <gz/math/Vector3.hh>
+#include <gz/math/Inertial.hh>
+#include <gz/math/Pose3.hh>
+#include <gz/math/MassMatrix3.hh>
+
+/////////////////////////////////////////////////
+TEST(DOMCone, Construction)
+{
+  sdf::Cone cone;
+  EXPECT_EQ(nullptr, cone.Element());
+  // A default cone has a length of 1 meter and radius if 0.5 meters.
+  EXPECT_DOUBLE_EQ(GZ_PI * std::pow(0.5, 2) * 1.0 / 3.0,
+                   cone.Shape().Volume());
+
+  EXPECT_DOUBLE_EQ(0.5, cone.Radius());
+  EXPECT_DOUBLE_EQ(1.0, cone.Length());
+
+  cone.SetRadius(0.5);
+  cone.SetLength(2.3);
+
+  EXPECT_DOUBLE_EQ(0.5, cone.Radius());
+  EXPECT_DOUBLE_EQ(2.3, cone.Length());
+}
+
+/////////////////////////////////////////////////
+TEST(DOMCone, MoveConstructor)
+{
+  sdf::Cone cone;
+  cone.SetRadius(0.2);
+  cone.SetLength(3.0);
+
+  sdf::Cone cone2(std::move(cone));
+  EXPECT_DOUBLE_EQ(0.2, cone2.Radius());
+  EXPECT_DOUBLE_EQ(3.0, cone2.Length());
+
+  EXPECT_DOUBLE_EQ(GZ_PI * std::pow(0.2, 2) * 3.0 / 3.0,
+                   cone2.Shape().Volume());
+  EXPECT_DOUBLE_EQ(0.2, cone2.Shape().Radius());
+  EXPECT_DOUBLE_EQ(3.0, cone2.Shape().Length());
+}
+
+/////////////////////////////////////////////////
+TEST(DOMCone, CopyConstructor)
+{
+  sdf::Cone cone;
+  cone.SetRadius(0.2);
+  cone.SetLength(3.0);
+
+  sdf::Cone cone2(cone);
+  EXPECT_DOUBLE_EQ(0.2, cone2.Radius());
+  EXPECT_DOUBLE_EQ(3.0, cone2.Length());
+}
+
+/////////////////////////////////////////////////
+TEST(DOMCone, CopyAssignmentOperator)
+{
+  sdf::Cone cone;
+  cone.SetRadius(0.2);
+  cone.SetLength(3.0);
+
+  sdf::Cone cone2;
+  cone2 = cone;
+  EXPECT_DOUBLE_EQ(0.2, cone2.Radius());
+  EXPECT_DOUBLE_EQ(3.0, cone2.Length());
+}
+
+/////////////////////////////////////////////////
+TEST(DOMCone, MoveAssignmentConstructor)
+{
+  sdf::Cone cone;
+  cone.SetRadius(0.2);
+  cone.SetLength(3.0);
+
+  sdf::Cone cone2;
+  cone2 = std::move(cone);
+  EXPECT_DOUBLE_EQ(0.2, cone2.Radius());
+  EXPECT_DOUBLE_EQ(3.0, cone2.Length());
+}
+
+/////////////////////////////////////////////////
+TEST(DOMCone, CopyAssignmentAfterMove)
+{
+  sdf::Cone cone1;
+  cone1.SetRadius(0.2);
+  cone1.SetLength(3.0);
+
+  sdf::Cone cone2;
+  cone2.SetRadius(2);
+  cone2.SetLength(30.0);
+
+  // This is similar to what std::swap does except it uses std::move for each
+  // assignment
+  sdf::Cone tmp = std::move(cone1);
+  cone1 = cone2;
+  cone2 = tmp;
+
+  EXPECT_DOUBLE_EQ(2, cone1.Radius());
+  EXPECT_DOUBLE_EQ(30, cone1.Length());
+
+  EXPECT_DOUBLE_EQ(0.2, cone2.Radius());
+  EXPECT_DOUBLE_EQ(3.0, cone2.Length());
+}
+
+/////////////////////////////////////////////////
+TEST(DOMCone, Load)
+{
+  sdf::Cone cone;
+  sdf::Errors errors;
+
+  // Null element name
+  errors = cone.Load(nullptr);
+  ASSERT_EQ(1u, errors.size());
+  EXPECT_EQ(sdf::ErrorCode::ELEMENT_MISSING, errors[0].Code());
+  EXPECT_EQ(nullptr, cone.Element());
+
+  // Bad element name
+  sdf::ElementPtr sdf(new sdf::Element());
+  sdf->SetName("bad");
+  errors = cone.Load(sdf);
+  ASSERT_EQ(1u, errors.size());
+  EXPECT_EQ(sdf::ErrorCode::ELEMENT_INCORRECT_TYPE, errors[0].Code());
+  EXPECT_NE(nullptr, cone.Element());
+
+  // Missing <radius> and <length> elements
+  sdf->SetName("cone");
+  errors = cone.Load(sdf);
+  ASSERT_EQ(2u, errors.size());
+  EXPECT_EQ(sdf::ErrorCode::ELEMENT_INVALID, errors[0].Code());
+  EXPECT_NE(std::string::npos, errors[0].Message().find("Invalid <radius>"))
+      << errors[0].Message();
+  EXPECT_EQ(sdf::ErrorCode::ELEMENT_INVALID, errors[1].Code());
+  EXPECT_NE(std::string::npos, errors[1].Message().find("Invalid <length>"))
+      << errors[1].Message();
+  EXPECT_NE(nullptr, cone.Element());
+
+  // Add a radius element
+  sdf::ElementPtr radiusDesc(new sdf::Element());
+  radiusDesc->SetName("radius");
+  radiusDesc->AddValue("double", "1.0", true, "radius");
+  sdf->AddElementDescription(radiusDesc);
+  sdf::ElementPtr radiusElem = sdf->AddElement("radius");
+  radiusElem->Set<double>(2.0);
+
+  // Missing <length> element
+  sdf->SetName("cone");
+  errors = cone.Load(sdf);
+  ASSERT_EQ(1u, errors.size());
+  EXPECT_EQ(sdf::ErrorCode::ELEMENT_INVALID, errors[0].Code());
+  EXPECT_NE(std::string::npos, errors[0].Message().find("Invalid <length>"))
+      << errors[0].Message();
+}
+
+/////////////////////////////////////////////////
+TEST(DOMCone, Shape)
+{
+  sdf::Cone cone;
+  EXPECT_DOUBLE_EQ(0.5, cone.Radius());
+  EXPECT_DOUBLE_EQ(1.0, cone.Length());
+
+  cone.Shape().SetRadius(0.123);
+  cone.Shape().SetLength(0.456);
+  EXPECT_DOUBLE_EQ(0.123, cone.Radius());
+  EXPECT_DOUBLE_EQ(0.456, cone.Length());
+}
+
+/////////////////////////////////////////////////
+TEST(DOMCone, CalculateInertial)
+{
+  sdf::Cone cone;
+
+  // density of aluminium
+  const double density = 2170;
+
+  // Invalid dimensions leading to std::nullopt return in
+  // CalculateInertial()
+  cone.SetLength(-1);
+  cone.SetRadius(0);
+  auto invalidConeInertial = cone.CalculateInertial(density);
+  ASSERT_EQ(std::nullopt, invalidConeInertial);
+
+  const double l = 2.0;
+  const double r = 0.1;
+
+  cone.SetLength(l);
+  cone.SetRadius(r);
+
+  double expectedMass = cone.Shape().Volume() * density;
+  double ixxIyy = (3 / 80.0) * expectedMass * (4 * r * r + l * l);
+  double izz = 3.0 * expectedMass * r * r / 10.0;
+
+  gz::math::MassMatrix3d expectedMassMat(
+    expectedMass,
+    gz::math::Vector3d(ixxIyy, ixxIyy, izz),
+    gz::math::Vector3d::Zero
+  );
+
+  gz::math::Inertiald expectedInertial;
+  expectedInertial.SetMassMatrix(expectedMassMat);
+  expectedInertial.SetPose({0, 0, -l / 4.0, 0, 0, 0});
+
+  auto coneInertial = cone.CalculateInertial(density);
+  EXPECT_EQ(cone.Shape().Mat().Density(), density);
+  ASSERT_NE(std::nullopt, coneInertial);
+  EXPECT_EQ(expectedInertial, *coneInertial);
+  EXPECT_EQ(expectedInertial.MassMatrix().DiagonalMoments(),
+    coneInertial->MassMatrix().DiagonalMoments());
+  EXPECT_EQ(expectedInertial.MassMatrix().Mass(),
+    coneInertial->MassMatrix().Mass());
+  EXPECT_EQ(expectedInertial.Pose(), coneInertial->Pose());
+}
+
+/////////////////////////////////////////////////
+TEST(DOMCone, ToElement)
+{
+  sdf::Cone cone;
+
+  cone.SetRadius(1.2);
+  cone.SetLength(0.5);
+
+  sdf::ElementPtr elem = cone.ToElement();
+  ASSERT_NE(nullptr, elem);
+
+  sdf::Cone cone2;
+  cone2.Load(elem);
+
+  EXPECT_DOUBLE_EQ(cone.Radius(), cone2.Radius());
+  EXPECT_DOUBLE_EQ(cone.Length(), cone2.Length());
+}
+
+/////////////////////////////////////////////////
+TEST(DOMCone, ToElementErrorOutput)
+{
+  std::stringstream buffer;
+  sdf::testing::RedirectConsoleStream redir(
+      sdf::Console::Instance()->GetMsgStream(), &buffer);
+
+  #ifdef _WIN32
+    sdf::Console::Instance()->SetQuiet(false);
+    sdf::testing::ScopeExit revertSetQuiet(
+      []
+      {
+        sdf::Console::Instance()->SetQuiet(true);
+      });
+  #endif
+
+  sdf::Cone cone;
+  sdf::Errors errors;
+
+  cone.SetRadius(1.2);
+  cone.SetLength(0.5);
+
+  sdf::ElementPtr elem = cone.ToElement(errors);
+  EXPECT_TRUE(errors.empty());
+  ASSERT_NE(nullptr, elem);
+
+  sdf::Cone cone2;
+  errors = cone2.Load(elem);
+  EXPECT_TRUE(errors.empty());
+
+  EXPECT_DOUBLE_EQ(cone.Radius(), cone2.Radius());
+  EXPECT_DOUBLE_EQ(cone.Length(), cone2.Length());
+
+  // Check nothing has been printed
+  EXPECT_TRUE(buffer.str().empty()) << buffer.str();
+}
diff --git a/src/Geometry.cc b/src/Geometry.cc
index 75bcca03f..ec87a01da 100644
--- a/src/Geometry.cc
+++ b/src/Geometry.cc
@@ -21,6 +21,7 @@
 #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/Heightmap.hh"
@@ -49,6 +50,9 @@ class sdf::Geometry::Implementation
   /// \brief Optional capsule.
   public: std::optional<Capsule> capsule;
 
+  /// \brief Optional cone.
+  public: std::optional<Cone> cone;
+
   /// \brief Optional cylinder.
   public: std::optional<Cylinder> cylinder;
 
@@ -127,6 +131,14 @@ Errors Geometry::Load(ElementPtr _sdf, const ParserConfig &_config)
         _sdf->GetElement("capsule", errors));
     errors.insert(errors.end(), err.begin(), err.end());
   }
+  else if (_sdf->HasElement("cone"))
+  {
+    this->dataPtr->type = GeometryType::CONE;
+    this->dataPtr->cone.emplace();
+    Errors err = this->dataPtr->cone->Load(
+        _sdf->GetElement("cone", errors));
+    errors.insert(errors.end(), err.begin(), err.end());
+  }
   else if (_sdf->HasElement("cylinder"))
   {
     this->dataPtr->type = GeometryType::CYLINDER;
@@ -240,6 +252,18 @@ void Geometry::SetCapsuleShape(const Capsule &_capsule)
   this->dataPtr->capsule = _capsule;
 }
 
+/////////////////////////////////////////////////
+const Cone *Geometry::ConeShape() const
+{
+  return optionalToPointer(this->dataPtr->cone);
+}
+
+/////////////////////////////////////////////////
+void Geometry::SetConeShape(const Cone &_cone)
+{
+  this->dataPtr->cone = _cone;
+}
+
 /////////////////////////////////////////////////
 const Cylinder *Geometry::CylinderShape() const
 {
@@ -327,6 +351,9 @@ std::optional<gz::math::Inertiald> Geometry::CalculateInertial(
     case GeometryType::CAPSULE:
       geomInertial = this->dataPtr->capsule->CalculateInertial(_density);
       break;
+    case GeometryType::CONE:
+      geomInertial = this->dataPtr->cone->CalculateInertial(_density);
+      break;
     case GeometryType::CYLINDER:
       geomInertial = this->dataPtr->cylinder->CalculateInertial(_density);
       break;
@@ -384,6 +411,9 @@ sdf::ElementPtr Geometry::ToElement(sdf::Errors &_errors) const
     case GeometryType::BOX:
       elem->InsertElement(this->dataPtr->box->ToElement(_errors), true);
       break;
+    case GeometryType::CONE:
+      elem->InsertElement(this->dataPtr->cone->ToElement(_errors), true);
+      break;
     case GeometryType::CYLINDER:
       elem->InsertElement(this->dataPtr->cylinder->ToElement(_errors), true);
       break;
diff --git a/src/Geometry_TEST.cc b/src/Geometry_TEST.cc
index 7b12f878a..05f4d7205 100644
--- a/src/Geometry_TEST.cc
+++ b/src/Geometry_TEST.cc
@@ -18,6 +18,7 @@
 #include <gtest/gtest.h>
 #include "sdf/Box.hh"
 #include "sdf/Capsule.hh"
+#include "sdf/Cone.hh"
 #include "sdf/Cylinder.hh"
 #include "sdf/Ellipsoid.hh"
 #include "sdf/Geometry.hh"
@@ -49,6 +50,9 @@ TEST(DOMGeometry, Construction)
   geom.SetType(sdf::GeometryType::CAPSULE);
   EXPECT_EQ(sdf::GeometryType::CAPSULE, geom.Type());
 
+  geom.SetType(sdf::GeometryType::CONE);
+  EXPECT_EQ(sdf::GeometryType::CONE, geom.Type());
+
   geom.SetType(sdf::GeometryType::CYLINDER);
   EXPECT_EQ(sdf::GeometryType::CYLINDER, geom.Type());
 
@@ -203,6 +207,23 @@ TEST(DOMGeometry, Capsule)
   EXPECT_DOUBLE_EQ(4.56, geom.CapsuleShape()->Length());
 }
 
+/////////////////////////////////////////////////
+TEST(DOMGeometry, Cone)
+{
+  sdf::Geometry geom;
+  geom.SetType(sdf::GeometryType::CONE);
+
+  sdf::Cone coneShape;
+  coneShape.SetRadius(0.123);
+  coneShape.SetLength(4.56);
+  geom.SetConeShape(coneShape);
+
+  EXPECT_EQ(sdf::GeometryType::CONE, geom.Type());
+  EXPECT_NE(nullptr, geom.ConeShape());
+  EXPECT_DOUBLE_EQ(0.123, geom.ConeShape()->Radius());
+  EXPECT_DOUBLE_EQ(4.56, geom.ConeShape()->Length());
+}
+
 /////////////////////////////////////////////////
 TEST(DOMGeometry, Cylinder)
 {
@@ -395,6 +416,37 @@ TEST(DOMGeometry, CalculateInertial)
     EXPECT_EQ(expectedInertial.Pose(), capsuleInertial->Pose());
   }
 
+  // Cone
+  {
+    sdf::Cone cone;
+    const double l = 2.0;
+    const double r = 0.1;
+
+    cone.SetLength(l);
+    cone.SetRadius(r);
+
+    expectedMass = cone.Shape().Volume() * density;
+    double ixxIyy = (3 / 80.0) * expectedMass * (4 * r * r + l * l);
+    double izz = 3.0 * expectedMass * r * r / 10.0;
+
+    expectedMassMat.SetMass(expectedMass);
+    expectedMassMat.SetDiagonalMoments(gz::math::Vector3d(ixxIyy, ixxIyy, izz));
+    expectedMassMat.SetOffDiagonalMoments(gz::math::Vector3d::Zero);
+
+    expectedInertial.SetMassMatrix(expectedMassMat);
+    expectedInertial.SetPose(gz::math::Pose3d({0, 0, -l / 4.0, 0, 0, 0}));
+
+    geom.SetType(sdf::GeometryType::CONE);
+    geom.SetConeShape(cone);
+    auto coneInertial = geom.CalculateInertial(errors,
+      sdfParserConfig, density, autoInertiaParams);
+
+    ASSERT_NE(std::nullopt, coneInertial);
+    EXPECT_EQ(expectedInertial, *coneInertial);
+    EXPECT_EQ(expectedInertial.MassMatrix(), expectedMassMat);
+    EXPECT_EQ(expectedInertial.Pose(), coneInertial->Pose());
+  }
+
   // Cylinder
   {
     sdf::Cylinder cylinder;
@@ -561,6 +613,7 @@ TEST(DOMGeometry, ToElement)
     EXPECT_EQ(geom.Type(), geom2.Type());
     EXPECT_NE(nullptr, geom2.BoxShape());
     EXPECT_EQ(nullptr, geom2.CapsuleShape());
+    EXPECT_EQ(nullptr, geom2.ConeShape());
     EXPECT_EQ(nullptr, geom2.CylinderShape());
     EXPECT_EQ(nullptr, geom2.EllipsoidShape());
     EXPECT_EQ(nullptr, geom2.SphereShape());
@@ -587,6 +640,34 @@ TEST(DOMGeometry, ToElement)
     EXPECT_EQ(geom.Type(), geom2.Type());
     EXPECT_EQ(nullptr, geom2.BoxShape());
     EXPECT_NE(nullptr, geom2.CapsuleShape());
+    EXPECT_EQ(nullptr, geom2.ConeShape());
+    EXPECT_EQ(nullptr, geom2.CylinderShape());
+    EXPECT_EQ(nullptr, geom2.EllipsoidShape());
+    EXPECT_EQ(nullptr, geom2.SphereShape());
+    EXPECT_EQ(nullptr, geom2.PlaneShape());
+    EXPECT_EQ(nullptr, geom2.MeshShape());
+    EXPECT_EQ(nullptr, geom2.HeightmapShape());
+    EXPECT_TRUE(geom2.PolylineShape().empty());
+  }
+
+  // Cone
+  {
+    sdf::Geometry geom;
+
+    geom.SetType(sdf::GeometryType::CONE);
+    sdf::Cone cone;
+    geom.SetConeShape(cone);
+
+    sdf::ElementPtr elem = geom.ToElement();
+    ASSERT_NE(nullptr, elem);
+
+    sdf::Geometry geom2;
+    geom2.Load(elem);
+
+    EXPECT_EQ(geom.Type(), geom2.Type());
+    EXPECT_EQ(nullptr, geom2.BoxShape());
+    EXPECT_EQ(nullptr, geom2.CapsuleShape());
+    EXPECT_NE(nullptr, geom2.ConeShape());
     EXPECT_EQ(nullptr, geom2.CylinderShape());
     EXPECT_EQ(nullptr, geom2.EllipsoidShape());
     EXPECT_EQ(nullptr, geom2.SphereShape());
@@ -613,6 +694,7 @@ TEST(DOMGeometry, ToElement)
     EXPECT_EQ(geom.Type(), geom2.Type());
     EXPECT_EQ(nullptr, geom2.BoxShape());
     EXPECT_EQ(nullptr, geom2.CapsuleShape());
+    EXPECT_EQ(nullptr, geom2.ConeShape());
     EXPECT_NE(nullptr, geom2.CylinderShape());
     EXPECT_EQ(nullptr, geom2.EllipsoidShape());
     EXPECT_EQ(nullptr, geom2.SphereShape());
@@ -639,6 +721,7 @@ TEST(DOMGeometry, ToElement)
     EXPECT_EQ(geom.Type(), geom2.Type());
     EXPECT_EQ(nullptr, geom2.BoxShape());
     EXPECT_EQ(nullptr, geom2.CapsuleShape());
+    EXPECT_EQ(nullptr, geom2.ConeShape());
     EXPECT_EQ(nullptr, geom2.CylinderShape());
     EXPECT_NE(nullptr, geom2.EllipsoidShape());
     EXPECT_EQ(nullptr, geom2.SphereShape());
@@ -665,6 +748,7 @@ TEST(DOMGeometry, ToElement)
     EXPECT_EQ(geom.Type(), geom2.Type());
     EXPECT_EQ(nullptr, geom2.BoxShape());
     EXPECT_EQ(nullptr, geom2.CapsuleShape());
+    EXPECT_EQ(nullptr, geom2.ConeShape());
     EXPECT_EQ(nullptr, geom2.CylinderShape());
     EXPECT_EQ(nullptr, geom2.EllipsoidShape());
     EXPECT_NE(nullptr, geom2.SphereShape());
@@ -691,6 +775,7 @@ TEST(DOMGeometry, ToElement)
     EXPECT_EQ(geom.Type(), geom2.Type());
     EXPECT_EQ(nullptr, geom2.BoxShape());
     EXPECT_EQ(nullptr, geom2.CapsuleShape());
+    EXPECT_EQ(nullptr, geom2.ConeShape());
     EXPECT_EQ(nullptr, geom2.CylinderShape());
     EXPECT_EQ(nullptr, geom2.EllipsoidShape());
     EXPECT_EQ(nullptr, geom2.SphereShape());
@@ -717,6 +802,7 @@ TEST(DOMGeometry, ToElement)
     EXPECT_EQ(geom.Type(), geom2.Type());
     EXPECT_EQ(nullptr, geom2.BoxShape());
     EXPECT_EQ(nullptr, geom2.CapsuleShape());
+    EXPECT_EQ(nullptr, geom2.ConeShape());
     EXPECT_EQ(nullptr, geom2.CylinderShape());
     EXPECT_EQ(nullptr, geom2.EllipsoidShape());
     EXPECT_EQ(nullptr, geom2.SphereShape());
@@ -743,6 +829,7 @@ TEST(DOMGeometry, ToElement)
     EXPECT_EQ(geom.Type(), geom2.Type());
     EXPECT_EQ(nullptr, geom2.BoxShape());
     EXPECT_EQ(nullptr, geom2.CapsuleShape());
+    EXPECT_EQ(nullptr, geom2.ConeShape());
     EXPECT_EQ(nullptr, geom2.CylinderShape());
     EXPECT_EQ(nullptr, geom2.EllipsoidShape());
     EXPECT_EQ(nullptr, geom2.SphereShape());
@@ -769,6 +856,7 @@ TEST(DOMGeometry, ToElement)
     EXPECT_EQ(geom.Type(), geom2.Type());
     EXPECT_EQ(nullptr, geom2.BoxShape());
     EXPECT_EQ(nullptr, geom2.CapsuleShape());
+    EXPECT_EQ(nullptr, geom2.ConeShape());
     EXPECT_EQ(nullptr, geom2.CylinderShape());
     EXPECT_EQ(nullptr, geom2.EllipsoidShape());
     EXPECT_EQ(nullptr, geom2.SphereShape());
@@ -816,6 +904,7 @@ TEST(DOMGeometry, ToElementErrorOutput)
     EXPECT_EQ(geom.Type(), geom2.Type());
     EXPECT_NE(nullptr, geom2.BoxShape());
     EXPECT_EQ(nullptr, geom2.CapsuleShape());
+    EXPECT_EQ(nullptr, geom2.ConeShape());
     EXPECT_EQ(nullptr, geom2.CylinderShape());
     EXPECT_EQ(nullptr, geom2.EllipsoidShape());
     EXPECT_EQ(nullptr, geom2.SphereShape());
@@ -844,6 +933,36 @@ TEST(DOMGeometry, ToElementErrorOutput)
     EXPECT_EQ(geom.Type(), geom2.Type());
     EXPECT_EQ(nullptr, geom2.BoxShape());
     EXPECT_NE(nullptr, geom2.CapsuleShape());
+    EXPECT_EQ(nullptr, geom2.ConeShape());
+    EXPECT_EQ(nullptr, geom2.CylinderShape());
+    EXPECT_EQ(nullptr, geom2.EllipsoidShape());
+    EXPECT_EQ(nullptr, geom2.SphereShape());
+    EXPECT_EQ(nullptr, geom2.PlaneShape());
+    EXPECT_EQ(nullptr, geom2.MeshShape());
+    EXPECT_EQ(nullptr, geom2.HeightmapShape());
+    EXPECT_TRUE(geom2.PolylineShape().empty());
+  }
+
+  // Cone
+  {
+    sdf::Geometry geom;
+
+    geom.SetType(sdf::GeometryType::CONE);
+    sdf::Cone cone;
+    geom.SetConeShape(cone);
+
+    sdf::ElementPtr elem = geom.ToElement(errors);
+    EXPECT_TRUE(errors.empty());
+    ASSERT_NE(nullptr, elem);
+
+    sdf::Geometry geom2;
+    errors = geom2.Load(elem);
+    EXPECT_TRUE(errors.empty());
+
+    EXPECT_EQ(geom.Type(), geom2.Type());
+    EXPECT_EQ(nullptr, geom2.BoxShape());
+    EXPECT_EQ(nullptr, geom2.CapsuleShape());
+    EXPECT_NE(nullptr, geom2.ConeShape());
     EXPECT_EQ(nullptr, geom2.CylinderShape());
     EXPECT_EQ(nullptr, geom2.EllipsoidShape());
     EXPECT_EQ(nullptr, geom2.SphereShape());
@@ -872,6 +991,7 @@ TEST(DOMGeometry, ToElementErrorOutput)
     EXPECT_EQ(geom.Type(), geom2.Type());
     EXPECT_EQ(nullptr, geom2.BoxShape());
     EXPECT_EQ(nullptr, geom2.CapsuleShape());
+    EXPECT_EQ(nullptr, geom2.ConeShape());
     EXPECT_NE(nullptr, geom2.CylinderShape());
     EXPECT_EQ(nullptr, geom2.EllipsoidShape());
     EXPECT_EQ(nullptr, geom2.SphereShape());
@@ -900,6 +1020,7 @@ TEST(DOMGeometry, ToElementErrorOutput)
     EXPECT_EQ(geom.Type(), geom2.Type());
     EXPECT_EQ(nullptr, geom2.BoxShape());
     EXPECT_EQ(nullptr, geom2.CapsuleShape());
+    EXPECT_EQ(nullptr, geom2.ConeShape());
     EXPECT_EQ(nullptr, geom2.CylinderShape());
     EXPECT_NE(nullptr, geom2.EllipsoidShape());
     EXPECT_EQ(nullptr, geom2.SphereShape());
@@ -928,6 +1049,7 @@ TEST(DOMGeometry, ToElementErrorOutput)
     EXPECT_EQ(geom.Type(), geom2.Type());
     EXPECT_EQ(nullptr, geom2.BoxShape());
     EXPECT_EQ(nullptr, geom2.CapsuleShape());
+    EXPECT_EQ(nullptr, geom2.ConeShape());
     EXPECT_EQ(nullptr, geom2.CylinderShape());
     EXPECT_EQ(nullptr, geom2.EllipsoidShape());
     EXPECT_NE(nullptr, geom2.SphereShape());
@@ -956,6 +1078,7 @@ TEST(DOMGeometry, ToElementErrorOutput)
     EXPECT_EQ(geom.Type(), geom2.Type());
     EXPECT_EQ(nullptr, geom2.BoxShape());
     EXPECT_EQ(nullptr, geom2.CapsuleShape());
+    EXPECT_EQ(nullptr, geom2.ConeShape());
     EXPECT_EQ(nullptr, geom2.CylinderShape());
     EXPECT_EQ(nullptr, geom2.EllipsoidShape());
     EXPECT_EQ(nullptr, geom2.SphereShape());
@@ -988,6 +1111,7 @@ TEST(DOMGeometry, ToElementErrorOutput)
     EXPECT_EQ(geom.Type(), geom2.Type());
     EXPECT_EQ(nullptr, geom2.BoxShape());
     EXPECT_EQ(nullptr, geom2.CapsuleShape());
+    EXPECT_EQ(nullptr, geom2.ConeShape());
     EXPECT_EQ(nullptr, geom2.CylinderShape());
     EXPECT_EQ(nullptr, geom2.EllipsoidShape());
     EXPECT_EQ(nullptr, geom2.SphereShape());
@@ -1020,6 +1144,7 @@ TEST(DOMGeometry, ToElementErrorOutput)
     EXPECT_EQ(geom.Type(), geom2.Type());
     EXPECT_EQ(nullptr, geom2.BoxShape());
     EXPECT_EQ(nullptr, geom2.CapsuleShape());
+    EXPECT_EQ(nullptr, geom2.ConeShape());
     EXPECT_EQ(nullptr, geom2.CylinderShape());
     EXPECT_EQ(nullptr, geom2.EllipsoidShape());
     EXPECT_EQ(nullptr, geom2.SphereShape());
@@ -1048,6 +1173,7 @@ TEST(DOMGeometry, ToElementErrorOutput)
     EXPECT_EQ(geom.Type(), geom2.Type());
     EXPECT_EQ(nullptr, geom2.BoxShape());
     EXPECT_EQ(nullptr, geom2.CapsuleShape());
+    EXPECT_EQ(nullptr, geom2.ConeShape());
     EXPECT_EQ(nullptr, geom2.CylinderShape());
     EXPECT_EQ(nullptr, geom2.EllipsoidShape());
     EXPECT_EQ(nullptr, geom2.SphereShape());
diff --git a/src/Visual_TEST.cc b/src/Visual_TEST.cc
index 935e99ed6..3e7248445 100644
--- a/src/Visual_TEST.cc
+++ b/src/Visual_TEST.cc
@@ -68,6 +68,7 @@ TEST(DOMVisual, Construction)
   ASSERT_NE(nullptr, visual.Geom());
   EXPECT_EQ(sdf::GeometryType::EMPTY, visual.Geom()->Type());
   EXPECT_EQ(nullptr, visual.Geom()->BoxShape());
+  EXPECT_EQ(nullptr, visual.Geom()->ConeShape());
   EXPECT_EQ(nullptr, visual.Geom()->CylinderShape());
   EXPECT_EQ(nullptr, visual.Geom()->PlaneShape());
   EXPECT_EQ(nullptr, visual.Geom()->SphereShape());
diff --git a/test/integration/geometry_dom.cc b/test/integration/geometry_dom.cc
index 419a04dc9..927850087 100644
--- a/test/integration/geometry_dom.cc
+++ b/test/integration/geometry_dom.cc
@@ -21,6 +21,7 @@
 #include "sdf/Box.hh"
 #include "sdf/Capsule.hh"
 #include "sdf/Collision.hh"
+#include "sdf/Cone.hh"
 #include "sdf/Cylinder.hh"
 #include "sdf/Element.hh"
 #include "sdf/Ellipsoid.hh"
@@ -94,6 +95,26 @@ TEST(DOMGeometry, Shapes)
   EXPECT_DOUBLE_EQ(2.1, capsuleVisGeom->Radius());
   EXPECT_DOUBLE_EQ(10.2, capsuleVisGeom->Length());
 
+  // Test cone collision
+  const sdf::Collision *coneCol = link->CollisionByName("cone_col");
+  ASSERT_NE(nullptr, coneCol);
+  ASSERT_NE(nullptr, coneCol->Geom());
+  EXPECT_EQ(sdf::GeometryType::CONE, coneCol->Geom()->Type());
+  const sdf::Cone *coneColGeom = coneCol->Geom()->ConeShape();
+  ASSERT_NE(nullptr, coneColGeom);
+  EXPECT_DOUBLE_EQ(0.2, coneColGeom->Radius());
+  EXPECT_DOUBLE_EQ(0.1, coneColGeom->Length());
+
+  // Test cone visual
+  const sdf::Visual *coneVis = link->VisualByName("cone_vis");
+  ASSERT_NE(nullptr, coneVis);
+  ASSERT_NE(nullptr, coneVis->Geom());
+  EXPECT_EQ(sdf::GeometryType::CONE, coneVis->Geom()->Type());
+  const sdf::Cone *coneVisGeom = coneVis->Geom()->ConeShape();
+  ASSERT_NE(nullptr, coneVisGeom);
+  EXPECT_DOUBLE_EQ(2.1, coneVisGeom->Radius());
+  EXPECT_DOUBLE_EQ(10.2, coneVisGeom->Length());
+
   // Test cylinder collision
   const sdf::Collision *cylinderCol = link->CollisionByName("cylinder_col");
   ASSERT_NE(nullptr, cylinderCol);
diff --git a/test/sdf/basic_shapes.sdf b/test/sdf/basic_shapes.sdf
index 32e0b416f..93c9fafc1 100644
--- a/test/sdf/basic_shapes.sdf
+++ b/test/sdf/basic_shapes.sdf
@@ -1,5 +1,5 @@
 <?xml version="1.0" ?>
-<sdf version="1.8">
+<sdf version="1.11">
   <world name="shapes_world">
     <model name="ground_plane">
       <static>true</static>
@@ -155,5 +155,28 @@
         </visual>
       </link>
     </model>
+
+    <model name="cone">
+      <pose relative_to="mesh">2 0 0 0 0 0</pose>
+      <link name="link">
+        <collision name="collision">
+          <geometry>
+            <cone>
+              <radius>0.2</radius>
+              <length>0.1</length>
+            </cone>
+          </geometry>
+        </collision>
+
+        <visual name="visual">
+          <geometry>
+            <cone>
+              <radius>0.2</radius>
+              <length>0.1</length>
+            </cone>
+          </geometry>
+        </visual>
+      </link>
+    </model>
   </world>
 </sdf>
diff --git a/test/sdf/shapes.sdf b/test/sdf/shapes.sdf
index 37c56277c..5a2e72d4f 100644
--- a/test/sdf/shapes.sdf
+++ b/test/sdf/shapes.sdf
@@ -1,5 +1,5 @@
 <?xml version="1.0" ?>
-<sdf version="1.6">
+<sdf version="1.11">
   <model name="shapes">
     <link name="link">
       <collision name="box_col">
@@ -65,6 +65,24 @@
         </geometry>
       </visual>
 
+      <collision name="cone_col">
+        <geometry>
+          <cone>
+            <radius>0.2</radius>
+            <length>0.1</length>
+          </cone>
+        </geometry>
+      </collision>
+
+      <visual name="cone_vis">
+        <geometry>
+          <cone>
+            <radius>2.1</radius>
+            <length>10.2</length>
+          </cone>
+        </geometry>
+      </visual>
+
       <collision name="cylinder_col">
         <geometry>
           <cylinder>
diff --git a/test/sdf/shapes_world.sdf b/test/sdf/shapes_world.sdf
index 5eb324490..3a7be8b91 100644
--- a/test/sdf/shapes_world.sdf
+++ b/test/sdf/shapes_world.sdf
@@ -1,5 +1,5 @@
 <?xml version="1.0" ?>
-<sdf version="1.8">
+<sdf version="1.11">
   <world name="shapes_world">
     <model name="ground_plane">
       <static>true</static>
@@ -147,5 +147,28 @@
         </visual>
       </link>
     </model>
+
+    <model name="cone">
+      <pose relative_to="mesh">2 0 0 0 0 0</pose>
+      <link name="link">
+        <collision name="collision">
+          <geometry>
+            <cone>
+              <radius>0.2</radius>
+              <length>0.1</length>
+            </cone>
+          </geometry>
+        </collision>
+
+        <visual name="visual">
+          <geometry>
+            <cone>
+              <radius>0.2</radius>
+              <length>0.1</length>
+            </cone>
+          </geometry>
+        </visual>
+      </link>
+    </model>
   </world>
 </sdf>