From b1abc7909c0548666e7ef2acdc7fbb78d84ce070 Mon Sep 17 00:00:00 2001 From: Stephen Brawner Date: Thu, 10 Dec 2020 17:56:18 -0800 Subject: [PATCH 1/6] Add ellipsoid shape to sdf Signed-off-by: Stephen Brawner --- Migration.md | 7 +- include/sdf/CMakeLists.txt | 1 + include/sdf/Ellipsoid.hh | 97 ++++++++++++++++++++ include/sdf/Geometry.hh | 15 ++++ sdf/1.8/CMakeLists.txt | 1 + sdf/1.8/ellipsoid_shape.sdf | 6 ++ sdf/1.8/geometry.sdf | 1 + src/CMakeLists.txt | 2 + src/Ellipsoid.cc | 147 +++++++++++++++++++++++++++++++ src/Ellipsoid_TEST.cc | 141 +++++++++++++++++++++++++++++ src/Geometry.cc | 29 ++++++ src/Geometry_TEST.cc | 20 +++++ test/integration/geometry_dom.cc | 21 +++++ test/sdf/shapes.sdf | 16 ++++ 14 files changed, 502 insertions(+), 2 deletions(-) create mode 100644 include/sdf/Ellipsoid.hh create mode 100644 sdf/1.8/ellipsoid_shape.sdf create mode 100644 src/Ellipsoid.cc create mode 100644 src/Ellipsoid_TEST.cc diff --git a/Migration.md b/Migration.md index e043bcd38..bffce89e8 100644 --- a/Migration.md +++ b/Migration.md @@ -269,10 +269,13 @@ but with improved human-readability.. ### Additions -1. **capsule.sdf** new shape type included in `//geometry` - + description: A shape consisting of a cylinder capped by hemispheres +1. **capsule.sdf and ellipsoid.sdf** new shape types included in `//geometry` + + `capsule.sdf`: A shape consisting of a cylinder capped by hemispheres with parameters for the `radius` and `length` of cylindrical section. + + `ellipsoid.sdf`: A convex shape with up to three radii defining its + shape in of the form (x^2/a^2 + y^2/b^2 + z^2/c^2 = 1). * [Pull request 389](https://github.com/osrf/sdformat/pull/389) + * [Pull request 434](https://github.com/osrf/sdformat/pull/434) ### Modifications diff --git a/include/sdf/CMakeLists.txt b/include/sdf/CMakeLists.txt index d94f9226f..bfa65f3cf 100644 --- a/include/sdf/CMakeLists.txt +++ b/include/sdf/CMakeLists.txt @@ -13,6 +13,7 @@ set (headers Console.hh Cylinder.hh Element.hh + Ellipsoid.hh Error.hh Exception.hh Filesystem.hh diff --git a/include/sdf/Ellipsoid.hh b/include/sdf/Ellipsoid.hh new file mode 100644 index 000000000..ef50d464d --- /dev/null +++ b/include/sdf/Ellipsoid.hh @@ -0,0 +1,97 @@ +/* + * Copyright 2020 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_ELLIPSOID_HH_ +#define SDF_ELLIPSOID_HH_ + +#include +#include +#include +#include + +namespace sdf +{ + // Inline bracket to help doxygen filtering. + inline namespace SDF_VERSION_NAMESPACE { + // + + // Forward declare private data class. + class EllipsoidPrivate; + + /// \brief Ellipsoid represents a ellipsoid shape, and is usually accessed + /// through a Geometry. + class SDFORMAT_VISIBLE Ellipsoid + { + /// \brief Constructor + public: Ellipsoid(); + + /// \brief Copy constructor + /// \param[in] _ellipsoid Ellipsoid to copy. + public: Ellipsoid(const Ellipsoid &_ellipsoid); + + /// \brief Move constructor + /// \param[in] _ellipsoid Ellipsoid to move. + public: Ellipsoid(Ellipsoid &&_ellipsoid) noexcept; + + /// \brief Destructor + public: virtual ~Ellipsoid(); + + /// \brief Move assignment operator. + /// \param[in] _ellipsoid Ellipsoid to move. + /// \return Reference to this. + public: Ellipsoid &operator=(Ellipsoid &&_ellipsoid); + + /// \brief Assignment operator. + /// \param[in] _ellipsoid The ellipsoid to set values from. + /// \return *this + public: Ellipsoid &operator=(const Ellipsoid &_ellipsoid); + + /// \brief Load the ellipsoid 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 ellipsoid's radius in meters. + /// \return The radius of the ellipsoid in meters. + public: ignition::math::Vector3d Radii() const; + + /// \brief Set the ellipsoid's x, y, and z radii in meters. + /// \param[in] _radius Vector of radii (x, y, z) of the ellipsoid in meters. + public: void SetRadii(const ignition::math::Vector3d &_radii); + + /// \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 Ignition Math representation of this Ellipsoid. + /// \return A const reference to an ignition::math::Ellipsoidd object. + public: const ignition::math::Ellipsoidd &Shape() const; + + /// \brief Get a mutable Ignition Math representation of this Ellipsoid. + /// \return A reference to an ignition::math::Ellipsoidd object. + public: ignition::math::Ellipsoidd &Shape(); + + /// \brief Private data pointer. + private: EllipsoidPrivate *dataPtr; + }; + } +} +#endif diff --git a/include/sdf/Geometry.hh b/include/sdf/Geometry.hh index 348d38d98..0049adfc4 100644 --- a/include/sdf/Geometry.hh +++ b/include/sdf/Geometry.hh @@ -32,6 +32,7 @@ namespace sdf class Box; class Capsule; class Cylinder; + class Ellipsoid; class Mesh; class Plane; class Sphere; @@ -60,6 +61,9 @@ namespace sdf /// \brief A capsule geometry. CAPSULE = 7, + + /// \brief An ellipsoid geometry + ELLIPSOID = 6, }; /// \brief Geometry provides access to a shape, such as a Box. Use the @@ -141,6 +145,17 @@ namespace sdf /// \param[in] _cylinder The cylinder shape. public: void SetCylinderShape(const Cylinder &_cylinder); + /// \brief Get the ellipsoid geometry, or nullptr if the contained + /// geometry is not an ellipsoid. + /// \return Pointer to the visual's ellipsoid geometry, or nullptr if the + /// geometry is not an ellipsoid. + /// \sa GeometryType Type() const + public: const Ellipsoid *EllipsoidShape() const; + + /// \brief Set the ellipsoid shape. + /// \param[in] _ellipsoid The ellipsoid shape. + public: void SetEllipsoidShape(const Ellipsoid &_ellipsoid); + /// \brief Get the sphere geometry, or nullptr if the contained geometry is /// not a sphere. /// \return Pointer to the visual's sphere geometry, or nullptr if the diff --git a/sdf/1.8/CMakeLists.txt b/sdf/1.8/CMakeLists.txt index e0b3a612c..a1a70717b 100644 --- a/sdf/1.8/CMakeLists.txt +++ b/sdf/1.8/CMakeLists.txt @@ -11,6 +11,7 @@ set (sdfs collision.sdf contact.sdf cylinder_shape.sdf + ellipsoid_shape.sdf frame.sdf forcetorque.sdf geometry.sdf diff --git a/sdf/1.8/ellipsoid_shape.sdf b/sdf/1.8/ellipsoid_shape.sdf new file mode 100644 index 000000000..821aadf12 --- /dev/null +++ b/sdf/1.8/ellipsoid_shape.sdf @@ -0,0 +1,6 @@ + + Ellipsoid shape + + The three radii of the ellipsoid. The origin of the ellipsoid is in its geometric center (inside the center of the ellipsoid). + + diff --git a/sdf/1.8/geometry.sdf b/sdf/1.8/geometry.sdf index 9317ec9a6..884902afb 100644 --- a/sdf/1.8/geometry.sdf +++ b/sdf/1.8/geometry.sdf @@ -9,6 +9,7 @@ + diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index 0016b166c..c9769c651 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -21,6 +21,7 @@ set (sources Converter.cc Cylinder.cc Element.cc + Ellipsoid.cc EmbeddedSdf.cc Error.cc Exception.cc @@ -92,6 +93,7 @@ if (BUILD_SDF_TEST) Console_TEST.cc Cylinder_TEST.cc Element_TEST.cc + Ellipsoid_TEST.cc Error_TEST.cc Exception_TEST.cc Frame_TEST.cc diff --git a/src/Ellipsoid.cc b/src/Ellipsoid.cc new file mode 100644 index 000000000..59e46b437 --- /dev/null +++ b/src/Ellipsoid.cc @@ -0,0 +1,147 @@ +/* + * Copyright 2020 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 "sdf/Ellipsoid.hh" + +using namespace sdf; + +// Private data class +class sdf::EllipsoidPrivate +{ + // An ellipsoid with a three radiuses of 1 meter + public: ignition::math::Ellipsoidd ellipsoid{ignition::math::Vector3d::One}; + + /// \brief The SDF element pointer used during load. + public: sdf::ElementPtr sdf; +}; + +///////////////////////////////////////////////// +Ellipsoid::Ellipsoid() + : dataPtr(new EllipsoidPrivate) +{ +} + +///////////////////////////////////////////////// +Ellipsoid::~Ellipsoid() +{ + delete this->dataPtr; + this->dataPtr = nullptr; +} + +////////////////////////////////////////////////// +Ellipsoid::Ellipsoid(const Ellipsoid &_ellipsoid) + : dataPtr(new EllipsoidPrivate) +{ + *this->dataPtr = *_ellipsoid.dataPtr; +} + +////////////////////////////////////////////////// +Ellipsoid::Ellipsoid(Ellipsoid &&_ellipsoid) noexcept + : dataPtr(std::exchange(_ellipsoid.dataPtr, nullptr)) +{ +} + +///////////////////////////////////////////////// +Ellipsoid &Ellipsoid::operator=(const Ellipsoid &_ellipsoid) +{ + return *this = Ellipsoid(_ellipsoid); +} + +///////////////////////////////////////////////// +Ellipsoid &Ellipsoid::operator=(Ellipsoid &&_ellipsoid) +{ + std::swap(this->dataPtr, _ellipsoid.dataPtr); + return *this; +} + +///////////////////////////////////////////////// +Errors Ellipsoid::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 ellipsoid, but the provided SDF " + "element is null."}); + return errors; + } + + // We need a ellipsoid child element + if (_sdf->GetName() != "ellipsoid") + { + errors.push_back({ErrorCode::ELEMENT_INCORRECT_TYPE, + "Attempting to load a ellipsoid geometry, but the provided SDF " + "element is not a ."}); + return errors; + } + + if (_sdf->HasElement("radii")) + { + std::pair pair = + _sdf->Get("radii", this->dataPtr->ellipsoid.Radii()); + + if (!pair.second) + { + errors.push_back({ErrorCode::ELEMENT_INVALID, + "Invalid data for a geometry. " + "Using a radii of 1, 1, 1 "}); + } + this->dataPtr->ellipsoid.SetRadii(pair.first); + } + else + { + errors.push_back({ErrorCode::ELEMENT_MISSING, + "Ellipsoid geometry is missing a child element. " + "Using a radii of 1, 1, 1."}); + } + + return errors; +} + +////////////////////////////////////////////////// +ignition::math::Vector3d Ellipsoid::Radii() const +{ + return this->dataPtr->ellipsoid.Radii(); +} + +////////////////////////////////////////////////// +void Ellipsoid::SetRadii(const ignition::math::Vector3d &_radii) +{ + this->dataPtr->ellipsoid.SetRadii(_radii); +} + +///////////////////////////////////////////////// +sdf::ElementPtr Ellipsoid::Element() const +{ + return this->dataPtr->sdf; +} + +///////////////////////////////////////////////// +const ignition::math::Ellipsoidd &Ellipsoid::Shape() const +{ + return this->dataPtr->ellipsoid; +} + +///////////////////////////////////////////////// +ignition::math::Ellipsoidd &Ellipsoid::Shape() +{ + return this->dataPtr->ellipsoid; +} diff --git a/src/Ellipsoid_TEST.cc b/src/Ellipsoid_TEST.cc new file mode 100644 index 000000000..8a4bf4138 --- /dev/null +++ b/src/Ellipsoid_TEST.cc @@ -0,0 +1,141 @@ +/* + * Copyright (C) 2020 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 "sdf/Ellipsoid.hh" + +///////////////////////////////////////////////// +TEST(DOMEllipsoid, Construction) +{ + sdf::Ellipsoid ellipsoid; + EXPECT_EQ(nullptr, ellipsoid.Element()); + // A default ellipsoid has all three radii set to 1 + EXPECT_DOUBLE_EQ(IGN_PI * 4. / 3., ellipsoid.Shape().Volume()); + EXPECT_EQ(ignition::math::Vector3d::One, ellipsoid.Shape().Radii()); + + const ignition::math::Vector3d expectedRadii(1.0, 2.0, 3.0); + ellipsoid.SetRadii(expectedRadii); + EXPECT_EQ(expectedRadii, ellipsoid.Shape().Radii()); +} + +///////////////////////////////////////////////// +TEST(DOMEllipsoid, MoveConstructor) +{ + sdf::Ellipsoid ellipsoid; + const ignition::math::Vector3d expectedRadii(1.0, 2.0, 3.0); + ellipsoid.SetRadii(expectedRadii); + + sdf::Ellipsoid ellipsoid2(std::move(ellipsoid)); + EXPECT_EQ(expectedRadii, ellipsoid2.Shape().Radii()); +} + +///////////////////////////////////////////////// +TEST(DOMEllipsoid, CopyConstructor) +{ + sdf::Ellipsoid ellipsoid; + const ignition::math::Vector3d expectedRadii(1.0, 2.0, 3.0); + ellipsoid.SetRadii(expectedRadii); + + sdf::Ellipsoid ellipsoid2(ellipsoid); + EXPECT_EQ(expectedRadii, ellipsoid2.Shape().Radii()); +} + +///////////////////////////////////////////////// +TEST(DOMEllipsoid, CopyAssignmentOperator) +{ + sdf::Ellipsoid ellipsoid; + const ignition::math::Vector3d expectedRadii(1.0, 2.0, 3.0); + ellipsoid.SetRadii(expectedRadii); + + sdf::Ellipsoid ellipsoid2; + ellipsoid2 = ellipsoid; + EXPECT_EQ(expectedRadii, ellipsoid2.Shape().Radii()); +} + +///////////////////////////////////////////////// +TEST(DOMEllipsoid, MoveAssignmentConstructor) +{ + sdf::Ellipsoid ellipsoid; + const ignition::math::Vector3d expectedRadii(1.0, 2.0, 3.0); + ellipsoid.SetRadii(expectedRadii); + + sdf::Ellipsoid ellipsoid2; + ellipsoid2 = std::move(ellipsoid); + EXPECT_EQ(expectedRadii, ellipsoid2.Shape().Radii()); +} + +///////////////////////////////////////////////// +TEST(DOMEllipsoid, CopyAssignmentAfterMove) +{ + sdf::Ellipsoid ellipsoid1; + const ignition::math::Vector3d expectedRadii1(1.0, 2.0, 3.0); + ellipsoid1.SetRadii(expectedRadii1); + + sdf::Ellipsoid ellipsoid2; + const ignition::math::Vector3d expectedRadii2(10.0, 20.0, 30.0); + ellipsoid2.SetRadii(expectedRadii2); + + // This is similar to what std::swap does except it uses std::move for each + // assignment + sdf::Ellipsoid tmp = std::move(ellipsoid1); + ellipsoid1 = ellipsoid2; + ellipsoid2 = tmp; + + EXPECT_EQ(expectedRadii1, ellipsoid2.Shape().Radii()); + EXPECT_EQ(expectedRadii2, ellipsoid1.Shape().Radii()); +} + +///////////////////////////////////////////////// +TEST(DOMEllipsoid, Load) +{ + sdf::Ellipsoid ellipsoid; + sdf::Errors errors; + + // Null element name + errors = ellipsoid.Load(nullptr); + ASSERT_EQ(1u, errors.size()); + EXPECT_EQ(sdf::ErrorCode::ELEMENT_MISSING, errors[0].Code()); + EXPECT_EQ(nullptr, ellipsoid.Element()); + + // Bad element name + sdf::ElementPtr sdf(new sdf::Element()); + sdf->SetName("bad"); + errors = ellipsoid.Load(sdf); + ASSERT_EQ(1u, errors.size()); + EXPECT_EQ(sdf::ErrorCode::ELEMENT_INCORRECT_TYPE, errors[0].Code()); + EXPECT_NE(nullptr, ellipsoid.Element()); + + // Missing element + sdf->SetName("ellipsoid"); + errors = ellipsoid.Load(sdf); + ASSERT_EQ(1u, errors.size()); + EXPECT_EQ(sdf::ErrorCode::ELEMENT_MISSING, errors[0].Code()); + EXPECT_NE(std::string::npos, errors[0].Message().find("missing a ")) + << errors[0].Message(); + EXPECT_NE(nullptr, ellipsoid.Element()); +} + +///////////////////////////////////////////////// +TEST(DOMEllipsoid, Shape) +{ + sdf::Ellipsoid ellipsoid; + EXPECT_EQ(ignition::math::Vector3d::One, ellipsoid.Radii()); + + const ignition::math::Vector3d expectedRadii(1.0, 2.0, 3.0); + ellipsoid.Shape().SetRadii(expectedRadii); + EXPECT_EQ(expectedRadii, ellipsoid.Radii()); +} diff --git a/src/Geometry.cc b/src/Geometry.cc index a91596af6..624f93920 100644 --- a/src/Geometry.cc +++ b/src/Geometry.cc @@ -18,6 +18,7 @@ #include "sdf/Box.hh" #include "sdf/Capsule.hh" #include "sdf/Cylinder.hh" +#include "sdf/Ellipsoid.hh" #include "sdf/Mesh.hh" #include "sdf/Plane.hh" #include "sdf/Sphere.hh" @@ -39,6 +40,9 @@ class sdf::GeometryPrivate /// \brief Pointer to a cylinder. public: std::unique_ptr cylinder; + /// \brief Pointer to an ellipsoid + public: std::unique_ptr ellipsoid; + /// \brief Pointer to a plane. public: std::unique_ptr plane; @@ -88,6 +92,12 @@ Geometry::Geometry(const Geometry &_geometry) *_geometry.dataPtr->cylinder); } + if (_geometry.dataPtr->ellipsoid) + { + this->dataPtr->ellipsoid = std::make_unique( + *_geometry.dataPtr->ellipsoid); + } + if (_geometry.dataPtr->plane) { this->dataPtr->plane = std::make_unique( @@ -174,6 +184,13 @@ Errors Geometry::Load(ElementPtr _sdf) Errors err = this->dataPtr->cylinder->Load(_sdf->GetElement("cylinder")); errors.insert(errors.end(), err.begin(), err.end()); } + else if (_sdf->HasElement("ellipsoid")) + { + this->dataPtr->type = GeometryType::ELLIPSOID; + this->dataPtr->ellipsoid.reset(new Ellipsoid()); + Errors err = this->dataPtr->ellipsoid->Load(_sdf->GetElement("ellipsoid")); + errors.insert(errors.end(), err.begin(), err.end()); + } else if (_sdf->HasElement("plane")) { this->dataPtr->type = GeometryType::PLANE; @@ -259,6 +276,18 @@ void Geometry::SetCylinderShape(const Cylinder &_cylinder) this->dataPtr->cylinder = std::make_unique(_cylinder); } +///////////////////////////////////////////////// +const Ellipsoid *Geometry::EllipsoidShape() const +{ + return this->dataPtr->ellipsoid.get(); +} + +///////////////////////////////////////////////// +void Geometry::SetEllipsoidShape(const Ellipsoid &_ellipsoid) +{ + this->dataPtr->ellipsoid = std::make_unique(_ellipsoid); +} + ///////////////////////////////////////////////// const Plane *Geometry::PlaneShape() const { diff --git a/src/Geometry_TEST.cc b/src/Geometry_TEST.cc index 2150f68ac..6aaab1a2a 100644 --- a/src/Geometry_TEST.cc +++ b/src/Geometry_TEST.cc @@ -19,6 +19,7 @@ #include "sdf/Box.hh" #include "sdf/Capsule.hh" #include "sdf/Cylinder.hh" +#include "sdf/Ellipsoid.hh" #include "sdf/Geometry.hh" #include "sdf/Mesh.hh" #include "sdf/Plane.hh" @@ -40,6 +41,9 @@ TEST(DOMGeometry, Construction) geom.SetType(sdf::GeometryType::CYLINDER); EXPECT_EQ(sdf::GeometryType::CYLINDER, geom.Type()); + geom.SetType(sdf::GeometryType::ELLIPSOID); + EXPECT_EQ(sdf::GeometryType::ELLIPSOID, geom.Type()); + geom.SetType(sdf::GeometryType::PLANE); EXPECT_EQ(sdf::GeometryType::PLANE, geom.Type()); @@ -202,6 +206,22 @@ TEST(DOMGeometry, Cylinder) EXPECT_DOUBLE_EQ(4.56, geom.CylinderShape()->Length()); } +///////////////////////////////////////////////// +TEST(DOMGeometry, Ellipsoid) +{ + sdf::Geometry geom; + geom.SetType(sdf::GeometryType::ELLIPSOID); + + sdf::Ellipsoid ellipsoidShape; + const ignition::math::Vector3d expectedRadii(1, 2, 3); + ellipsoidShape.SetRadii(expectedRadii); + geom.SetEllipsoidShape(ellipsoidShape); + + EXPECT_EQ(sdf::GeometryType::ELLIPSOID, geom.Type()); + EXPECT_NE(nullptr, geom.EllipsoidShape()); + EXPECT_EQ(expectedRadii, geom.EllipsoidShape()->Radii()); +} + ///////////////////////////////////////////////// TEST(DOMGeometry, Mesh) { diff --git a/test/integration/geometry_dom.cc b/test/integration/geometry_dom.cc index 3d9c4637e..a19cfa822 100644 --- a/test/integration/geometry_dom.cc +++ b/test/integration/geometry_dom.cc @@ -23,6 +23,7 @@ #include "sdf/Cylinder.hh" #include "sdf/Collision.hh" #include "sdf/Element.hh" +#include "sdf/Ellipsoid.hh" #include "sdf/Filesystem.hh" #include "sdf/Geometry.hh" #include "sdf/Link.hh" @@ -112,6 +113,26 @@ TEST(DOMGeometry, Shapes) EXPECT_DOUBLE_EQ(2.1, cylinderVisGeom->Radius()); EXPECT_DOUBLE_EQ(10.2, cylinderVisGeom->Length()); + // Test ellipsoid collision + const sdf::Collision *ellipsoidCol = link->CollisionByName("ellipsoid_col"); + ASSERT_NE(nullptr, ellipsoidCol); + ASSERT_NE(nullptr, ellipsoidCol->Geom()); + EXPECT_EQ(sdf::GeometryType::ELLIPSOID, ellipsoidCol->Geom()->Type()); + const sdf::Ellipsoid *ellipsoidColGeom = + ellipsoidCol->Geom()->EllipsoidShape(); + ASSERT_NE(nullptr, ellipsoidColGeom); + EXPECT_EQ(ignition::math::Vector3d(1.0, 2.0, 3.0), ellipsoidColGeom->Radii()); + + // Test ellipsoid visual + const sdf::Visual *ellipsoidVis = link->VisualByName("ellipsoid_vis"); + ASSERT_NE(nullptr, ellipsoidVis); + ASSERT_NE(nullptr, ellipsoidVis->Geom()); + EXPECT_EQ(sdf::GeometryType::ELLIPSOID, ellipsoidVis->Geom()->Type()); + const sdf::Ellipsoid *ellipsoidVisGeom = + ellipsoidVis->Geom()->EllipsoidShape(); + ASSERT_NE(nullptr, ellipsoidVisGeom); + EXPECT_EQ(ignition::math::Vector3d(0.1, 0.2, 0.3), ellipsoidVisGeom->Radii()); + // Test plane collision const sdf::Collision *planeCol = link->CollisionByName("plane_col"); ASSERT_NE(nullptr, planeCol); diff --git a/test/sdf/shapes.sdf b/test/sdf/shapes.sdf index 05a2be133..76b15f4cd 100644 --- a/test/sdf/shapes.sdf +++ b/test/sdf/shapes.sdf @@ -59,6 +59,22 @@ + + + + 1.0 2.0 3.0 + + + + + + + + 0.1 0.2 0.3 + + + + From a3ced206492d1b4442c49f64d0525d6d9ea3f38a Mon Sep 17 00:00:00 2001 From: Stephen Brawner Date: Mon, 14 Dec 2020 13:15:59 -0800 Subject: [PATCH 2/6] Set geometry enum to 8 Signed-off-by: Stephen Brawner --- include/sdf/Geometry.hh | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/include/sdf/Geometry.hh b/include/sdf/Geometry.hh index 0049adfc4..da430dc66 100644 --- a/include/sdf/Geometry.hh +++ b/include/sdf/Geometry.hh @@ -63,7 +63,7 @@ namespace sdf CAPSULE = 7, /// \brief An ellipsoid geometry - ELLIPSOID = 6, + ELLIPSOID = 8, }; /// \brief Geometry provides access to a shape, such as a Box. Use the From fa181c5bb93fb25883f002a90af537d64dec5b0a Mon Sep 17 00:00:00 2001 From: Stephen Brawner Date: Tue, 15 Dec 2020 14:16:15 -0800 Subject: [PATCH 3/6] Linter cleanup Signed-off-by: Stephen Brawner --- src/Ellipsoid.cc | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/Ellipsoid.cc b/src/Ellipsoid.cc index 59e46b437..671a13148 100644 --- a/src/Ellipsoid.cc +++ b/src/Ellipsoid.cc @@ -96,7 +96,8 @@ Errors Ellipsoid::Load(ElementPtr _sdf) if (_sdf->HasElement("radii")) { std::pair pair = - _sdf->Get("radii", this->dataPtr->ellipsoid.Radii()); + _sdf->Get( + "radii", this->dataPtr->ellipsoid.Radii()); if (!pair.second) { From 8939e3b5540b9a2191fbc6779c96a87b9d42aba3 Mon Sep 17 00:00:00 2001 From: Stephen Brawner Date: Tue, 22 Dec 2020 15:00:16 -0800 Subject: [PATCH 4/6] Update ign-math dependency to 6.8 Signed-off-by: Stephen Brawner --- cmake/SearchForStuff.cmake | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/cmake/SearchForStuff.cmake b/cmake/SearchForStuff.cmake index c35cf587a..b25c54600 100644 --- a/cmake/SearchForStuff.cmake +++ b/cmake/SearchForStuff.cmake @@ -97,7 +97,7 @@ endmacro() ######################################## # Find ignition math # Set a variable for generating ProjectConfig.cmake -find_package(ignition-math6 6.7 QUIET) +find_package(ignition-math6 6.8 QUIET) if (NOT ignition-math6_FOUND) message(STATUS "Looking for ignition-math6-config.cmake - not found") BUILD_ERROR ("Missing: Ignition math (libignition-math6-dev)") From 5eb55d5db4bb5b766e061472df81dde8a319038f Mon Sep 17 00:00:00 2001 From: Stephen Brawner Date: Tue, 29 Dec 2020 12:08:44 -0800 Subject: [PATCH 5/6] Add ubuntu-prerelease for apt Signed-off-by: Stephen Brawner --- .github/workflows/linux-ubuntu-bionic.yml | 1 + 1 file changed, 1 insertion(+) diff --git a/.github/workflows/linux-ubuntu-bionic.yml b/.github/workflows/linux-ubuntu-bionic.yml index de34e7ad5..0c9e64ca8 100644 --- a/.github/workflows/linux-ubuntu-bionic.yml +++ b/.github/workflows/linux-ubuntu-bionic.yml @@ -14,6 +14,7 @@ jobs: sudo apt update; sudo apt -y install wget lsb-release gnupg; sudo sh -c 'echo "deb http://packages.osrfoundation.org/gazebo/ubuntu-stable $(lsb_release -cs) main" > /etc/apt/sources.list.d/gazebo-stable.list'; + sudo sh -c 'echo "deb http://packages.osrfoundation.org/gazebo/ubuntu-prerelease $(lsb_release -cs) main" > /etc/apt/sources.list.d/gazebo-prerelease.list'; sudo apt-key adv --keyserver keyserver.ubuntu.com --recv-keys D2486D2DD83DB69272AFE98867170598AF249743; sudo apt-get update; sudo apt -y install cmake build-essential curl g++-8 git cppcheck; From 7e958739ae39bd1218dfdca875766fd7353506a0 Mon Sep 17 00:00:00 2001 From: Stephen Brawner Date: Tue, 29 Dec 2020 12:31:13 -0800 Subject: [PATCH 6/6] PR Fixup Signed-off-by: Stephen Brawner --- include/sdf/Ellipsoid.hh | 2 +- include/sdf/Geometry.hh | 4 ++-- src/Ellipsoid.cc | 2 +- 3 files changed, 4 insertions(+), 4 deletions(-) diff --git a/include/sdf/Ellipsoid.hh b/include/sdf/Ellipsoid.hh index ef50d464d..e888591c5 100644 --- a/include/sdf/Ellipsoid.hh +++ b/include/sdf/Ellipsoid.hh @@ -67,7 +67,7 @@ namespace sdf /// an error code and message. An empty vector indicates no error. public: Errors Load(ElementPtr _sdf); - /// \brief Get the ellipsoid's radius in meters. + /// \brief Get the ellipsoid's radii in meters. /// \return The radius of the ellipsoid in meters. public: ignition::math::Vector3d Radii() const; diff --git a/include/sdf/Geometry.hh b/include/sdf/Geometry.hh index da430dc66..1ce3c730e 100644 --- a/include/sdf/Geometry.hh +++ b/include/sdf/Geometry.hh @@ -147,8 +147,8 @@ namespace sdf /// \brief Get the ellipsoid geometry, or nullptr if the contained /// geometry is not an ellipsoid. - /// \return Pointer to the visual's ellipsoid geometry, or nullptr if the - /// geometry is not an ellipsoid. + /// \return Pointer to the ellipsoid geometry, or nullptr if the geometry is + /// not an ellipsoid. /// \sa GeometryType Type() const public: const Ellipsoid *EllipsoidShape() const; diff --git a/src/Ellipsoid.cc b/src/Ellipsoid.cc index 671a13148..5c67eccea 100644 --- a/src/Ellipsoid.cc +++ b/src/Ellipsoid.cc @@ -22,7 +22,7 @@ using namespace sdf; // Private data class class sdf::EllipsoidPrivate { - // An ellipsoid with a three radiuses of 1 meter + /// \brief An ellipsoid with all three radii of 1 meter public: ignition::math::Ellipsoidd ellipsoid{ignition::math::Vector3d::One}; /// \brief The SDF element pointer used during load.