Add ellipsoid shape to sdf (#434)

* Add ellipsoid shape to sdf
* Set geometry enum to 8
* Linter cleanup
* Update ign-math dependency to 6.8
* Add ubuntu-prerelease for apt
* PR Fixup

Signed-off-by: Stephen Brawner <[email protected]>

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

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
 

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

include/sdf/CMakeLists.txt

@@ -13,6 +13,7 @@ set (headers
   Console.hh
   Cylinder.hh
   Element.hh
+  Ellipsoid.hh
   Error.hh
   Exception.hh
   Filesystem.hh

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 <ignition/math/Ellipsoid.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 {
+  //
+
+  // 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 radii 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

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 = 8,
   };
 
   /// \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 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

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

sdf/1.8/ellipsoid_shape.sdf

@@ -0,0 +1,6 @@
+<element name="ellipsoid" required="0">
+  <description>Ellipsoid shape</description>
+  <element name="radii" type="vector3" default="1 1 1" required="1">
+    <description>The three radii of the ellipsoid. The origin of the ellipsoid is in its geometric center (inside the center of the ellipsoid).</description>
+  </element>
+</element>

sdf/1.8/geometry.sdf

@@ -9,6 +9,7 @@
   <include filename="box_shape.sdf" required="0"/>
   <include filename="capsule_shape.sdf" required="0"/>
   <include filename="cylinder_shape.sdf" required="0"/>
+  <include filename="ellipsoid_shape.sdf" required="0"/>
   <include filename="heightmap_shape.sdf" required="0"/>
   <include filename="image_shape.sdf" required="0"/>
   <include filename="mesh_shape.sdf" required="0"/>

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

src/Ellipsoid.cc

@@ -0,0 +1,148 @@
+/*
+ * 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 <sstream>
+#include "sdf/Ellipsoid.hh"
+
+using namespace sdf;
+
+// Private data class
+class sdf::EllipsoidPrivate
+{
+  /// \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.
+  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 <ellipsoid>."});
+    return errors;
+  }
+
+  if (_sdf->HasElement("radii"))
+  {
+    std::pair<ignition::math::Vector3d, bool> pair =
+      _sdf->Get<ignition::math::Vector3d>(
+        "radii", this->dataPtr->ellipsoid.Radii());
+
+    if (!pair.second)
+    {
+      errors.push_back({ErrorCode::ELEMENT_INVALID,
+          "Invalid <radii> data for a <ellipsoid> 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 <radii> 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;
+}