Added Ellipsoid Python interface

src/python_pybind11/CMakeLists.txt

@@ -14,6 +14,7 @@ pybind11_add_module(math SHARED
   src/Capsule.cc
   src/Color.cc
   src/DiffDriveOdometry.cc
+  src/Ellipsoid.cc
   src/Filter.cc
   src/Frustum.cc
   src/GaussMarkovProcess.cc
@@ -103,6 +104,7 @@ if (BUILD_TESTING)
     Color_TEST
     Cylinder_TEST
     DiffDriveOdometry_TEST
+    Ellipsoid_TEST
     Filter_TEST
     Frustum_TEST
     GaussMarkovProcess_TEST

src/python_pybind11/src/Ellipsoid.cc

@@ -0,0 +1,33 @@
+/*
+ * Copyright (C) 2022 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 "Ellipsoid.hh"
+
+namespace ignition
+{
+namespace math
+{
+namespace python
+{
+void defineMathEllipsoid(py::module &m, const std::string &typestr)
+{
+  helpDefineMathEllipsoid<float>(m, typestr + "f");
+  helpDefineMathEllipsoid<double>(m, typestr + "d");
+}
+}  // namespace python
+}  // namespace math
+}  // namespace ignition

src/python_pybind11/src/Ellipsoid.hh

@@ -0,0 +1,103 @@
+/*
+ * Copyright (C) 2022 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 IGNITION_MATH_PYTHON__Ellipsoid_HH_
+#define IGNITION_MATH_PYTHON__Ellipsoid_HH_
+
+#include <string>
+
+#include <pybind11/pybind11.h>
+#include <pybind11/operators.h>
+#include <pybind11/stl.h>
+
+#include <ignition/math/Ellipsoid.hh>
+#include <ignition/math/Vector3.hh>
+
+namespace py = pybind11;
+using namespace pybind11::literals;
+
+namespace ignition
+{
+namespace math
+{
+namespace python
+{
+/// Define a pybind11 wrapper for an ignition::math::Ellipsoid
+/**
+ * \param[in] module a pybind11 module to add the definition to
+ * \param[in] typestr name of the type used by Python
+ */
+template<typename T>
+void helpDefineMathEllipsoid(py::module &m, const std::string &typestr)
+{
+  using Class = ignition::math::Ellipsoid<T>;
+  std::string pyclass_name = typestr;
+  py::class_<Class>(m,
+                    pyclass_name.c_str(),
+                    py::buffer_protocol(),
+                    py::dynamic_attr())
+    .def(py::init<>())
+    .def(py::init<ignition::math::Vector3<T>>())
+    .def(py::init<ignition::math::Vector3<T>,
+                  const ignition::math::Material&>())
+    .def(py::self == py::self)
+    .def("radii",
+         &Class::Radii,
+         "Get the radius in meters.")
+    .def("set_radii",
+         &Class::SetRadii,
+         "Set the radius in meters.")
+    .def("material",
+         &Class::Mat,
+         "Get the material associated with this Ellipsoid.")
+    .def("set_material",
+         &Class::SetMat,
+         "Set the material associated with this Ellipsoid.")
+    .def("mass_matrix",
+         &Class::MassMatrix,
+         "Get the mass matrix for this box. This function "
+         "is only meaningful if the box's size and material "
+         "have been set.")
+    .def("volume",
+         &Class::Volume,
+         "Get the volume of the Ellipsoid in m^3.")
+    .def("density_from_mass",
+         &Class::DensityFromMass,
+         "Compute the box's density given a mass value.")
+    .def("set_density_from_mass",
+         &Class::SetDensityFromMass,
+         "Set the density of this box based on a mass value.")
+    .def("__copy__", [](const Class &self) {
+      return Class(self);
+    })
+    .def("__deepcopy__", [](const Class &self, py::dict) {
+      return Class(self);
+    }, "memo"_a);
+}
+
+/// Define a pybind11 wrapper for an ignition::math::Ellipsoid
+/**
+ * \param[in] module a pybind11 module to add the definition to
+ * \param[in] typestr name of the type used by Python
+ */
+void defineMathEllipsoid(py::module &m, const std::string &typestr);
+
+}  // namespace python
+}  // namespace math
+}  // namespace ignition
+
+#endif  // IGNITION_MATH_PYTHON__BOX_HH_

src/python_pybind11/src/_ignition_math_pybind11.cc

@@ -21,6 +21,7 @@
 #include "Color.hh"
 #include "Cylinder.hh"
 #include "DiffDriveOdometry.hh"
+#include "Ellipsoid.hh"
 #include "Filter.hh"
 #include "Frustum.hh"
 #include "GaussMarkovProcess.hh"
@@ -73,6 +74,9 @@ PYBIND11_MODULE(math, m)
   ignition::math::python::defineMathDiffDriveOdometry(
     m, "DiffDriveOdometry");
 
+  ignition::math::python::defineMathEllipsoid(
+    m, "Ellipsoid");
+
   ignition::math::python::defineMathGaussMarkovProcess(
     m, "GaussMarkovProcess");
 

src/python_pybind11/test/Ellipsoid_TEST.py

@@ -0,0 +1,130 @@
+# Copyright (C) 2022 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.
+
+import unittest
+
+import ignition
+from ignition.math import Ellipsoidd, Material, MassMatrix3d, Vector3d
+
+import math
+
+class TestEllipsoid(unittest.TestCase):
+
+    def test_constructor(self):
+        ellipsoid = Ellipsoidd()
+        self.assertEqual(Vector3d.ZERO, ellipsoid.radii())
+        self.assertEqual(Material(), ellipsoid.material())
+
+        ellipsoid2 = Ellipsoidd()
+        self.assertEqual(ellipsoid, ellipsoid2)
+
+        # Vector3 of radii constructor
+        expectedRadii = Vector3d(1.0, 2.0, 3.0)
+        ellipsoid = Ellipsoidd(expectedRadii)
+        self.assertEqual(expectedRadii, ellipsoid.radii())
+        self.assertEqual(Material(), ellipsoid.material())
+
+        ellipsoid2 = Ellipsoidd(expectedRadii)
+        self.assertEqual(ellipsoid, ellipsoid2)
+
+        # Vector3 of radii and material
+        expectedRadii = Vector3d(1.0, 2.0, 3.0)
+        expectedMaterial = Material(ignition.math.MaterialType.WOOD)
+        ellipsoid = Ellipsoidd(expectedRadii, expectedMaterial)
+        self.assertEqual(expectedRadii, ellipsoid.radii())
+        self.assertEqual(expectedMaterial, ellipsoid.material())
+
+        ellipsoid2 = Ellipsoidd(expectedRadii, expectedMaterial)
+        self.assertEqual(ellipsoid, ellipsoid2)
+
+
+    def test_mutators(self):
+        ellipsoid = Ellipsoidd()
+        self.assertEqual(Vector3d.ZERO, ellipsoid.radii())
+        self.assertEqual(Material(), ellipsoid.material())
+
+        expectedRadii = Vector3d(1.0, 2.0, 3.0)
+        ellipsoid.set_radii(expectedRadii)
+
+        expectedMaterial = Material(ignition.math.MaterialType.PINE)
+        ellipsoid.set_material(expectedMaterial)
+
+        self.assertEqual(expectedRadii, ellipsoid.radii())
+        self.assertEqual(expectedMaterial, ellipsoid.material())
+
+
+    def test_volume_an_density(self):
+        mass = 1.0
+        # Basic sphere
+        ellipsoid = Ellipsoidd(Vector3d.ONE * 2.)
+
+        expectedVolume = (4. / 3.) * math.pi * math.pow(2.0, 3)
+        self.assertEqual(expectedVolume, ellipsoid.volume())
+
+        expectedDensity = mass / expectedVolume
+        self.assertEqual(expectedDensity, ellipsoid.density_from_mass(mass))
+
+        ellipsoid2 = Ellipsoidd(Vector3d(1, 10, 100))
+        expectedVolume = (4. / 3.) * math.pi * 1. * 10. * 100.
+        self.assertEqual(expectedVolume, ellipsoid2.volume())
+
+        expectedDensity = mass / expectedVolume
+        self.assertEqual(expectedDensity, ellipsoid2.density_from_mass(mass))
+
+        # Check bad cases
+        ellipsoid3 = Ellipsoidd(Vector3d.ZERO)
+        self.assertFalse(ellipsoid3.set_density_from_mass(mass))
+
+        ellipsoid4 = Ellipsoidd(-Vector3d.ONE)
+        self.assertFalse(ellipsoid4.set_density_from_mass(mass))
+
+        ellipsoid5 = Ellipsoidd(Vector3d(-1, 1, 1))
+        self.assertFalse(ellipsoid5.set_density_from_mass(mass))
+
+        ellipsoid6 = Ellipsoidd(Vector3d(-1, -1, 1))
+        self.assertFalse(ellipsoid6.set_density_from_mass(mass))
+
+
+    def test_mass(self):
+        mass = 2.0
+        ellipsoid = Ellipsoidd(Vector3d(1, 10, 100))
+        ellipsoid.set_density_from_mass(mass)
+
+        ixx = (mass / 5.0) * (10. * 10. + 100. * 100.)
+        iyy = (mass / 5.0) * (1. * 1. + 100. * 100.)
+        izz = (mass / 5.0) * (1. * 1. + 10. * 10.)
+        expectedMassMat = MassMatrix3d(
+            mass, Vector3d(ixx, iyy, izz), Vector3d.ZERO)
+
+        massMat = ellipsoid.mass_matrix()
+        self.assertEqual(expectedMassMat, massMat)
+        self.assertEqual(expectedMassMat.diagonal_moments(), massMat.diagonal_moments())
+        self.assertEqual(expectedMassMat.mass(), massMat.mass())
+
+        # Zero case
+        ellipsoid2 = Ellipsoidd()
+        self.assertEqual(None, ellipsoid2.mass_matrix())
+
+        # Check bad cases
+        ellipsoid3 = Ellipsoidd(-Vector3d.ONE)
+        self.assertEqual(None, ellipsoid3.mass_matrix())
+
+        ellipsoid4 = Ellipsoidd(Vector3d(-1, 1, 1))
+        self.assertEqual(None, ellipsoid4.mass_matrix())
+
+        ellipsoid5 = Ellipsoidd(Vector3d(-1, -1, 1))
+        self.assertEqual(None, ellipsoid5.mass_matrix())
+
+if __name__ == '__main__':
+    unittest.main()