forked from gazebosim/sdformat
-
Notifications
You must be signed in to change notification settings - Fork 1
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
Signed-off-by: Benjamin Perseghetti <[email protected]>
- Loading branch information
1 parent
6f1c365
commit 17924a3
Showing
10 changed files
with
354 additions
and
2 deletions.
There are no files selected for viewing
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -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 <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(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<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 |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -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> |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -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 <sstream> | ||
#include <optional> | ||
|
||
#include <gz/math/Inertial.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(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<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()); | ||
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; | ||
} |
Oops, something went wrong.