Add Cone as a primitive parametric shape. (#1415)

* Backport:  Add cone shape to SDFormat spec (#1418)

Signed-off-by: Benjamin Perseghetti <[email protected]>
Co-authored-by: Steve Peters <[email protected]>

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

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
@@ -137,6 +141,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

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"

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

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);

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

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_

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)

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())