From 17924a32a789c0585eb661d28e7e50d0a12d56ce Mon Sep 17 00:00:00 2001 From: Benjamin Perseghetti Date: Sun, 12 May 2024 04:00:36 -0400 Subject: [PATCH] Cone. Signed-off-by: Benjamin Perseghetti --- include/sdf/Cone.hh | 106 +++++++++++++++++++ include/sdf/Geometry.hh | 15 +++ include/sdf/ParticleEmitter.hh | 3 + sdf/1.11/CMakeLists.txt | 1 + sdf/1.11/cone_shape.sdf | 9 ++ sdf/1.11/geometry.sdf | 1 + sdf/1.11/particle_emitter.sdf | 4 +- src/Cone.cc | 184 +++++++++++++++++++++++++++++++++ src/Geometry.cc | 30 ++++++ src/ParticleEmitter.cc | 3 +- 10 files changed, 354 insertions(+), 2 deletions(-) create mode 100644 include/sdf/Cone.hh create mode 100644 sdf/1.11/cone_shape.sdf create mode 100644 src/Cone.cc diff --git a/include/sdf/Cone.hh b/include/sdf/Cone.hh new file mode 100644 index 000000000..91c3d2d33 --- /dev/null +++ b/include/sdf/Cone.hh @@ -0,0 +1,106 @@ +/* + * Copyright 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. + * + */ +#ifndef SDF_CONE_HH_ +#define SDF_CONE_HH_ + +#include + +#include +#include +#include +#include +#include +#include + +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(const 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(const 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 + 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 35fd1e1cd..5d75b860d 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 @@ -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 diff --git a/include/sdf/ParticleEmitter.hh b/include/sdf/ParticleEmitter.hh index bf592de24..c2e09dec5 100644 --- a/include/sdf/ParticleEmitter.hh +++ b/include/sdf/ParticleEmitter.hh @@ -52,6 +52,9 @@ namespace sdf /// \brief An ellipsoid emitter. ELLIPSOID = 3, + + /// \brief An cone emitter. + CONE = 4, }; /// \brief A description of a particle emitter, which can be attached 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 @@ + + Cone shape + + Radius of the cone + + + Length of the cone along the z axis + + 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 @@ + diff --git a/sdf/1.11/particle_emitter.sdf b/sdf/1.11/particle_emitter.sdf index d6a2fdf7e..1be49ccc8 100755 --- a/sdf/1.11/particle_emitter.sdf +++ b/sdf/1.11/particle_emitter.sdf @@ -7,7 +7,7 @@ - The type of a particle emitter. One of "box", "cylinder", "ellipsoid", or "point". + The type of a particle emitter. One of "box", "cylinder", "cone", "ellipsoid", or "point". @@ -26,6 +26,8 @@ depending on the emmiter type: - point: The area is ignored. - box: The area is interpreted as width X height X depth. + - cone: The area is interpreted as the bounding box of the + cone. The cone is oriented along the Z-axis. - cylinder: The area is interpreted as the bounding box of the cylinder. The cylinder is oriented along the Z-axis. - ellipsoid: The area is interpreted as the bounding box of an diff --git a/src/Cone.cc b/src/Cone.cc new file mode 100644 index 000000000..632ac46a6 --- /dev/null +++ b/src/Cone.cc @@ -0,0 +1,184 @@ +/* + * Copyright 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 +#include + +#include +#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()) +{ +} + +///////////////////////////////////////////////// +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 ."}); + return errors; + } + + { + std::pair pair = _sdf->Get(errors, "radius", + this->dataPtr->cone.Radius()); + + if (!pair.second) + { + std::stringstream ss; + ss << "Invalid data for a 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 pair = _sdf->Get(errors, "length", + this->dataPtr->cone.Length()); + + if (!pair.second) + { + std::stringstream ss; + ss << "Invalid data for a 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(const double _radius) +{ + this->dataPtr->cone.SetRadius(_radius); +} + +////////////////////////////////////////////////// +double Cone::Length() const +{ + return this->dataPtr->cone.Length(); +} + +////////////////////////////////////////////////// +void Cone::SetLength(const 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 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()); + 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(_errors, this->Radius()); + + sdf::ElementPtr lengthElem = elem->GetElement("length", _errors); + lengthElem->Set(_errors, this->Length()); + + return elem; +} diff --git a/src/Geometry.cc b/src/Geometry.cc index 9afb20212..89dd0784d 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; + /// \brief Optional cone. + public: std::optional cone; + /// \brief Optional cylinder. public: std::optional 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 Geometry::CalculateInertial( case GeometryType::CAPSULE: geomInertial = this->dataPtr->capsule->CalculateInertial(_density); break; + case GeometryType::CONE: + geomInertial = this->dataPtr->capsule->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/ParticleEmitter.cc b/src/ParticleEmitter.cc index 605b58812..dad6ff3e8 100644 --- a/src/ParticleEmitter.cc +++ b/src/ParticleEmitter.cc @@ -34,10 +34,11 @@ using namespace sdf; /// Particle emitter type strings. These should match the data in /// `enum class ParticleEmitterType` located in ParticleEmitter.hh, and the size /// template parameter should match the number of elements as well. -constexpr std::array kEmitterTypeStrs = +constexpr std::array kEmitterTypeStrs = { "point", "box", + "cone", "cylinder", "ellipsoid", };