diff --git a/src/python_pybind11/CMakeLists.txt b/src/python_pybind11/CMakeLists.txt index 59faa9a60..22adef2d0 100644 --- a/src/python_pybind11/CMakeLists.txt +++ b/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 diff --git a/src/python_pybind11/src/Ellipsoid.cc b/src/python_pybind11/src/Ellipsoid.cc new file mode 100644 index 000000000..ce654c43e --- /dev/null +++ b/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(m, typestr + "f"); + helpDefineMathEllipsoid(m, typestr + "d"); +} +} // namespace python +} // namespace math +} // namespace ignition diff --git a/src/python_pybind11/src/Ellipsoid.hh b/src/python_pybind11/src/Ellipsoid.hh new file mode 100644 index 000000000..418cd17e0 --- /dev/null +++ b/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 + +#include +#include +#include + +#include +#include + +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 +void helpDefineMathEllipsoid(py::module &m, const std::string &typestr) +{ + using Class = ignition::math::Ellipsoid; + std::string pyclass_name = typestr; + py::class_(m, + pyclass_name.c_str(), + py::buffer_protocol(), + py::dynamic_attr()) + .def(py::init<>()) + .def(py::init>()) + .def(py::init, + 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_ diff --git a/src/python_pybind11/src/_ignition_math_pybind11.cc b/src/python_pybind11/src/_ignition_math_pybind11.cc index b269a667e..00497c61c 100644 --- a/src/python_pybind11/src/_ignition_math_pybind11.cc +++ b/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"); diff --git a/src/python_pybind11/test/Ellipsoid_TEST.py b/src/python_pybind11/test/Ellipsoid_TEST.py new file mode 100644 index 000000000..bdfd086e1 --- /dev/null +++ b/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()