Skip to content

Commit c3e5690

Browse files
bperseghettiscpeters
authored andcommitted
Add Cone as a primitive parametric shape. (gazebosim#1415)
* Backport: Add cone shape to SDFormat spec (gazebosim#1418) Signed-off-by: Benjamin Perseghetti <bperseghetti@rudislabs.com> Co-authored-by: Steve Peters <computersthatmove@gmail.com>
1 parent f0dfc5f commit c3e5690

25 files changed

+1109
-5
lines changed

include/sdf/Cone.hh

+108
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,108 @@
1+
/*
2+
* Copyright 2024 CogniPilot Foundation
3+
* Copyright 2024 Open Source Robotics Foundation
4+
* Copyright 2024 Rudis Laboratories
5+
*
6+
* Licensed under the Apache License, Version 2.0 (the "License");
7+
* you may not use this file except in compliance with the License.
8+
* You may obtain a copy of the License at
9+
*
10+
* http://www.apache.org/licenses/LICENSE-2.0
11+
*
12+
* Unless required by applicable law or agreed to in writing, software
13+
* distributed under the License is distributed on an "AS IS" BASIS,
14+
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
15+
* See the License for the specific language governing permissions and
16+
* limitations under the License.
17+
*
18+
*/
19+
#ifndef SDF_CONE_HH_
20+
#define SDF_CONE_HH_
21+
22+
#include <optional>
23+
24+
#include <gz/math/Cone.hh>
25+
#include <gz/math/Inertial.hh>
26+
#include <gz/utils/ImplPtr.hh>
27+
#include <sdf/Error.hh>
28+
#include <sdf/Element.hh>
29+
#include <sdf/sdf_config.h>
30+
31+
namespace sdf
32+
{
33+
// Inline bracket to help doxygen filtering.
34+
inline namespace SDF_VERSION_NAMESPACE {
35+
/// \brief Cone represents a cone shape, and is usually accessed
36+
/// through a Geometry.
37+
class SDFORMAT_VISIBLE Cone
38+
{
39+
/// \brief Constructor
40+
public: Cone();
41+
42+
/// \brief Load the cone geometry based on a element pointer.
43+
/// This is *not* the usual entry point. Typical usage of the SDF DOM is
44+
/// through the Root object.
45+
/// \param[in] _sdf The SDF Element pointer
46+
/// \return Errors, which is a vector of Error objects. Each Error includes
47+
/// an error code and message. An empty vector indicates no error.
48+
public: Errors Load(ElementPtr _sdf);
49+
50+
/// \brief Get the cone's radius in meters.
51+
/// \return The radius of the cone in meters.
52+
public: double Radius() const;
53+
54+
/// \brief Set the cone's radius in meters.
55+
/// \param[in] _radius The radius of the cone in meters.
56+
public: void SetRadius(double _radius);
57+
58+
/// \brief Get the cone's length in meters.
59+
/// \return The length of the cone in meters.
60+
public: double Length() const;
61+
62+
/// \brief Set the cone's length in meters.
63+
/// \param[in] _length The length of the cone in meters.
64+
public: void SetLength(double _length);
65+
66+
/// \brief Get a pointer to the SDF element that was used during
67+
/// load.
68+
/// \return SDF element pointer. The value will be nullptr if Load has
69+
/// not been called.
70+
public: sdf::ElementPtr Element() const;
71+
72+
/// \brief Get the Gazebo Math representation of this cone.
73+
/// \return A const reference to a gz::math::Sphered object.
74+
public: const gz::math::Coned &Shape() const;
75+
76+
/// \brief Get a mutable Gazebo Math representation of this cone.
77+
/// \return A reference to a gz::math::Coned object.
78+
public: gz::math::Coned &Shape();
79+
80+
/// \brief Calculate and return the Inertial values for the cone. In
81+
/// order to calculate the inertial properties, the function mutates the
82+
/// object by updating its material properties.
83+
/// \param[in] _density Density of the cone in kg/m^3
84+
/// \return A std::optional with gz::math::Inertiald object or std::nullopt
85+
public: std::optional<gz::math::Inertiald>
86+
CalculateInertial(double _density);
87+
88+
/// \brief Create and return an SDF element filled with data from this
89+
/// cone.
90+
/// Note that parameter passing functionality is not captured with this
91+
/// function.
92+
/// \return SDF element pointer with updated cone values.
93+
public: sdf::ElementPtr ToElement() const;
94+
95+
/// \brief Create and return an SDF element filled with data from this
96+
/// cone.
97+
/// Note that parameter passing functionality is not captured with this
98+
/// function.
99+
/// \param[out] _errors Vector of errors.
100+
/// \return SDF element pointer with updated cone values.
101+
public: sdf::ElementPtr ToElement(sdf::Errors &_errors) const;
102+
103+
/// \brief Private data pointer.
104+
GZ_UTILS_IMPL_PTR(dataPtr)
105+
};
106+
}
107+
}
108+
#endif

include/sdf/Geometry.hh

+15
Original file line numberDiff line numberDiff line change
@@ -37,6 +37,7 @@ namespace sdf
3737
// Forward declare private data class.
3838
class Box;
3939
class Capsule;
40+
class Cone;
4041
class Cylinder;
4142
class Ellipsoid;
4243
class Heightmap;
@@ -79,6 +80,9 @@ namespace sdf
7980

8081
/// \brief A polyline geometry.
8182
POLYLINE = 9,
83+
84+
/// \brief A polyline geometry.
85+
CONE = 10,
8286
};
8387

8488
/// \brief Geometry provides access to a shape, such as a Box. Use the
@@ -140,6 +144,17 @@ namespace sdf
140144
/// \param[in] _capsule The capsule shape.
141145
public: void SetCapsuleShape(const Capsule &_capsule);
142146

147+
/// \brief Get the cone geometry, or nullptr if the contained
148+
/// geometry is not a cone.
149+
/// \return Pointer to the visual's cone geometry, or nullptr if the
150+
/// geometry is not a cone.
151+
/// \sa GeometryType Type() const
152+
public: const Cone *ConeShape() const;
153+
154+
/// \brief Set the cone shape.
155+
/// \param[in] _cone The cone shape.
156+
public: void SetConeShape(const Cone &_cone);
157+
143158
/// \brief Get the cylinder geometry, or nullptr if the contained
144159
/// geometry is not a cylinder.
145160
/// \return Pointer to the visual's cylinder geometry, or nullptr if the

include/sdf/Visual.hh

+1
Original file line numberDiff line numberDiff line change
@@ -22,6 +22,7 @@
2222
#include <gz/math/Pose3.hh>
2323
#include <gz/utils/ImplPtr.hh>
2424
#include "sdf/Box.hh"
25+
#include "sdf/Cone.hh"
2526
#include "sdf/Cylinder.hh"
2627
#include "sdf/Element.hh"
2728
#include "sdf/Material.hh"

python/CMakeLists.txt

+2
Original file line numberDiff line numberDiff line change
@@ -52,6 +52,7 @@ pybind11_add_module(${BINDINGS_MODULE_NAME} MODULE
5252
src/sdf/pyCamera.cc
5353
src/sdf/pyCapsule.cc
5454
src/sdf/pyCollision.cc
55+
src/sdf/pyCone.cc
5556
src/sdf/pyConvexDecomposition.cc
5657
src/sdf/pyCylinder.cc
5758
src/sdf/pyElement.cc
@@ -135,6 +136,7 @@ if (BUILD_TESTING AND NOT WIN32)
135136
pyCamera_TEST
136137
pyCapsule_TEST
137138
pyCollision_TEST
139+
pyCone_TEST
138140
pyCylinder_TEST
139141
pyElement_TEST
140142
pyEllipsoid_TEST

python/src/sdf/_gz_sdformat_pybind11.cc

+2
Original file line numberDiff line numberDiff line change
@@ -26,6 +26,7 @@
2626
#include "pyCamera.hh"
2727
#include "pyCapsule.hh"
2828
#include "pyCollision.hh"
29+
#include "pyCone.hh"
2930
#include "pyConvexDecomposition.hh"
3031
#include "pyCylinder.hh"
3132
#include "pyElement.hh"
@@ -87,6 +88,7 @@ PYBIND11_MODULE(BINDINGS_MODULE_NAME, m) {
8788
sdf::python::defineCamera(m);
8889
sdf::python::defineCapsule(m);
8990
sdf::python::defineCollision(m);
91+
sdf::python::defineCone(m);
9092
sdf::python::defineConvexDecomposition(m);
9193
sdf::python::defineContact(m);
9294
sdf::python::defineCylinder(m);

python/src/sdf/pyCone.cc

+60
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,60 @@
1+
/*
2+
* Copyright 2024 CogniPilot Foundation
3+
* Copyright 2024 Open Source Robotics Foundation
4+
* Copyright 2024 Rudis Laboratories
5+
*
6+
* Licensed under the Apache License, Version 2.0 (the "License");
7+
* you may not use this file except in compliance with the License.
8+
* You may obtain a copy of the License at
9+
*
10+
* http://www.apache.org/licenses/LICENSE-2.0
11+
*
12+
* Unless required by applicable law or agreed to in writing, software
13+
* distributed under the License is distributed on an "AS IS" BASIS,
14+
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
15+
* See the License for the specific language governing permissions and
16+
* limitations under the License.
17+
*/
18+
#include "pyCone.hh"
19+
20+
#include <pybind11/pybind11.h>
21+
22+
#include "sdf/Cone.hh"
23+
24+
using namespace pybind11::literals;
25+
26+
namespace sdf
27+
{
28+
// Inline bracket to help doxygen filtering.
29+
inline namespace SDF_VERSION_NAMESPACE {
30+
namespace python
31+
{
32+
/////////////////////////////////////////////////
33+
void defineCone(pybind11::object module)
34+
{
35+
pybind11::class_<sdf::Cone>(module, "Cone")
36+
.def(pybind11::init<>())
37+
.def(pybind11::init<sdf::Cone>())
38+
.def("radius", &sdf::Cone::Radius,
39+
"Get the cone's radius in meters.")
40+
.def("set_radius", &sdf::Cone::SetRadius,
41+
"Set the cone's radius in meters.")
42+
.def("length", &sdf::Cone::Length,
43+
"Get the cone's length in meters.")
44+
.def("set_length", &sdf::Cone::SetLength,
45+
"Set the cone's length in meters.")
46+
.def(
47+
"shape",
48+
pybind11::overload_cast<>(&sdf::Cone::Shape, pybind11::const_),
49+
pybind11::return_value_policy::reference,
50+
"Get a mutable Gazebo Math representation of this Cone.")
51+
.def("__copy__", [](const sdf::Cone &self) {
52+
return sdf::Cone(self);
53+
})
54+
.def("__deepcopy__", [](const sdf::Cone &self, pybind11::dict) {
55+
return sdf::Cone(self);
56+
}, "memo"_a);
57+
}
58+
} // namespace python
59+
} // namespace SDF_VERSION_NAMESPACE
60+
} // namespace sdf

python/src/sdf/pyCone.hh

+43
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,43 @@
1+
/*
2+
* Copyright 2024 CogniPilot Foundation
3+
* Copyright 2024 Open Source Robotics Foundation
4+
* Copyright 2024 Rudis Laboratories
5+
*
6+
* Licensed under the Apache License, Version 2.0 (the "License");
7+
* you may not use this file except in compliance with the License.
8+
* You may obtain a copy of the License at
9+
*
10+
* http://www.apache.org/licenses/LICENSE-2.0
11+
*
12+
* Unless required by applicable law or agreed to in writing, software
13+
* distributed under the License is distributed on an "AS IS" BASIS,
14+
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
15+
* See the License for the specific language governing permissions and
16+
* limitations under the License.
17+
*/
18+
19+
#ifndef SDFORMAT_PYTHON_CONE_HH_
20+
#define SDFORMAT_PYTHON_CONE_HH_
21+
22+
#include <pybind11/pybind11.h>
23+
24+
#include "sdf/Cone.hh"
25+
26+
#include "sdf/config.hh"
27+
28+
namespace sdf
29+
{
30+
// Inline bracket to help doxygen filtering.
31+
inline namespace SDF_VERSION_NAMESPACE {
32+
namespace python
33+
{
34+
/// Define a pybind11 wrapper for an sdf::Cone
35+
/**
36+
* \param[in] module a pybind11 module to add the definition to
37+
*/
38+
void defineCone(pybind11::object module);
39+
} // namespace python
40+
} // namespace SDF_VERSION_NAMESPACE
41+
} // namespace sdf
42+
43+
#endif // SDFORMAT_PYTHON_CONE_HH_

python/src/sdf/pyGeometry.cc

+8
Original file line numberDiff line numberDiff line change
@@ -23,6 +23,7 @@
2323

2424
#include "sdf/Box.hh"
2525
#include "sdf/Capsule.hh"
26+
#include "sdf/Cone.hh"
2627
#include "sdf/Cylinder.hh"
2728
#include "sdf/Ellipsoid.hh"
2829
#include "sdf/Geometry.hh"
@@ -64,6 +65,12 @@ void defineGeometry(pybind11::object module)
6465
"geometry is not a capsule.")
6566
.def("set_capsule_shape", &sdf::Geometry::SetCapsuleShape,
6667
"Set the capsule shape.")
68+
.def("cone_shape", &sdf::Geometry::ConeShape,
69+
pybind11::return_value_policy::reference,
70+
"Get the cone geometry, or None if the contained "
71+
"geometry is not a cone.")
72+
.def("set_cone_shape", &sdf::Geometry::SetConeShape,
73+
"Set the cone shape.")
6774
.def("cylinder_shape", &sdf::Geometry::CylinderShape,
6875
pybind11::return_value_policy::reference,
6976
"Get the cylinder geometry, or None if the contained "
@@ -104,6 +111,7 @@ void defineGeometry(pybind11::object module)
104111
pybind11::enum_<sdf::GeometryType>(module, "GeometryType")
105112
.value("EMPTY", sdf::GeometryType::EMPTY)
106113
.value("BOX", sdf::GeometryType::BOX)
114+
.value("CONE", sdf::GeometryType::CONE)
107115
.value("CYLINDER", sdf::GeometryType::CYLINDER)
108116
.value("PLANE", sdf::GeometryType::PLANE)
109117
.value("SPHERE", sdf::GeometryType::SPHERE)

python/test/pyCollision_TEST.py

+2-1
Original file line numberDiff line numberDiff line change
@@ -14,7 +14,7 @@
1414

1515
import copy
1616
from gz_test_deps.math import Inertiald, MassMatrix3d, Pose3d, Vector3d
17-
from gz_test_deps.sdformat import (Box, Collision, Contact, Cylinder, Error,
17+
from gz_test_deps.sdformat import (Box, Collision, Cone, Contact, Cylinder, Error,
1818
Geometry, ParserConfig, Plane, Root, Surface, Sphere,
1919
SDFErrorsException)
2020
import gz_test_deps.sdformat as sdf
@@ -61,6 +61,7 @@ def test_default_construction(self):
6161
self.assertNotEqual(None, collision.geometry())
6262
self.assertEqual(sdf.GeometryType.EMPTY, collision.geometry().type())
6363
self.assertEqual(None, collision.geometry().box_shape())
64+
self.assertEqual(None, collision.geometry().cone_shape())
6465
self.assertEqual(None, collision.geometry().cylinder_shape())
6566
self.assertEqual(None, collision.geometry().plane_shape())
6667
self.assertEqual(None, collision.geometry().sphere_shape())

0 commit comments

Comments
 (0)