diff --git a/src/python/CMakeLists.txt b/src/python/CMakeLists.txt index 5510400a6..70c086ba6 100644 --- a/src/python/CMakeLists.txt +++ b/src/python/CMakeLists.txt @@ -88,7 +88,6 @@ if (PYTHONLIBS_FOUND) # Add the Python tests set(python_tests Box_TEST - Cylinder_TEST Frustum_TEST Plane_TEST python_TEST diff --git a/src/python_pybind11/CMakeLists.txt b/src/python_pybind11/CMakeLists.txt index 5f088ab82..7c85b8bbd 100644 --- a/src/python_pybind11/CMakeLists.txt +++ b/src/python_pybind11/CMakeLists.txt @@ -85,6 +85,7 @@ if (${pybind11_FOUND}) Angle_TEST AxisAlignedBox_TEST Color_TEST + Cylinder_TEST DiffDriveOdometry_TEST Filter_TEST GaussMarkovProcess_TEST diff --git a/src/python_pybind11/src/Cylinder.hh b/src/python_pybind11/src/Cylinder.hh new file mode 100644 index 000000000..5c2f32ae6 --- /dev/null +++ b/src/python_pybind11/src/Cylinder.hh @@ -0,0 +1,118 @@ +/* + * Copyright (C) 2021 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__CYLINDER_HH_ +#define IGNITION_MATH_PYTHON__CYLINDER_HH_ + +#include <string> + +#include <pybind11/pybind11.h> +#include <pybind11/operators.h> +#include <pybind11/stl.h> + +#include <ignition/math/Cylinder.hh> + +namespace py = pybind11; +using namespace pybind11::literals; + +namespace ignition +{ +namespace math +{ +namespace python +{ +/// Define a pybind11 wrapper for an ignition::math::Cylinder +/** + * \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 defineMathCylinder(py::module &m, const std::string &typestr) +{ + + using Class = ignition::math::Cylinder<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<const T, const T, + const ignition::math::Quaternion<T>&>(), + py::arg("_length") = 0, + py::arg("_radius") = 0, + py::arg("_rotOffset") = ignition::math::Quaternion<T>::Identity) + .def(py::init<const T, const T, + const ignition::math::Material&, + const ignition::math::Quaternion<T>&>(), + py::arg("_length") = 0, + py::arg("_radius") = 0, + py::arg("_material") = ignition::math::Material(), + py::arg("_rotOffset") = ignition::math::Quaternion<T>::Identity) + .def(py::self == py::self) + .def("radius", + &Class::Radius, + "Get the radius in meters.") + .def("set_radius", + &Class::SetRadius, + "Set the radius in meters.") + .def("length", + &Class::Length, + "Get the length in meters.") + .def("set_length", + &Class::SetLength, + "Set the length in meters.") + .def("rotational_offset", + &Class::RotationalOffset, + "Get the rotation offset.") + .def("set_rotational_offset", + &Class::SetRotationalOffset, + "Set the rotation offset.") + .def("mat", + &Class::Mat, + py::return_value_policy::reference, + "Get the material associated with this box.") + .def("set_mat", + &Class::SetMat, + "Set the material associated with this box.") + .def("volume", + &Class::Volume, + "Get the volume of the box 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("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("__copy__", [](const Class &self) { + return Class(self); + }) + .def("__deepcopy__", [](const Class &self, py::dict) { + return Class(self); + }, "memo"_a); +} + +} // namespace python +} // namespace math +} // namespace ignition + +#endif // IGNITION_MATH_PYTHON__CYLINDER_HH_ diff --git a/src/python_pybind11/src/_ignition_math_pybind11.cc b/src/python_pybind11/src/_ignition_math_pybind11.cc index 2766afc2f..b0f8e9414 100644 --- a/src/python_pybind11/src/_ignition_math_pybind11.cc +++ b/src/python_pybind11/src/_ignition_math_pybind11.cc @@ -16,6 +16,7 @@ #include "Angle.hh" #include "AxisAlignedBox.hh" +#include "Cylinder.hh" #include "Color.hh" #include "DiffDriveOdometry.hh" #include "Filter.hh" @@ -143,6 +144,8 @@ PYBIND11_MODULE(math, m) ignition::math::python::defineMathMassMatrix3<double>(m, "MassMatrix3d"); ignition::math::python::defineMathMassMatrix3<float>(m, "MassMatrix3f"); + ignition::math::python::defineMathCylinder<double>(m, "Cylinderd"); + ignition::math::python::defineMathInertial<double>(m, "Inertiald"); ignition::math::python::defineMathFilter<int>(m, "Filteri"); diff --git a/src/python/Cylinder_TEST.py b/src/python_pybind11/test/Cylinder_TEST.py similarity index 91% rename from src/python/Cylinder_TEST.py rename to src/python_pybind11/test/Cylinder_TEST.py index d5c866342..dc6da79ed 100644 --- a/src/python/Cylinder_TEST.py +++ b/src/python_pybind11/test/Cylinder_TEST.py @@ -16,7 +16,7 @@ import unittest import ignition -from ignition.math import Cylinderd, IGN_PI, MassMatrix3d, Material, Quaterniond +from ignition.math import Cylinderd, MassMatrix3d, Material, Quaterniond class TestCylinder(unittest.TestCase): @@ -54,14 +54,14 @@ def test_constructor(self): self.assertEqual(cylinder, cylinder2) # Length, radius, mat and rot constructor - cylinder = Cylinderd(1.0, 2.0, Material(ignition.math.MaterialType_WOOD), + cylinder = Cylinderd(1.0, 2.0, Material(ignition.math.MaterialType.WOOD), Quaterniond(0.1, 0.2, 0.3)) self.assertEqual(1.0, cylinder.length()) self.assertEqual(2.0, cylinder.radius()) self.assertEqual(Quaterniond(0.1, 0.2, 0.3), cylinder.rotational_offset()) - self.assertEqual(Material(ignition.math.MaterialType_WOOD), cylinder.mat()) + self.assertEqual(Material(ignition.math.MaterialType.WOOD), cylinder.mat()) - cylinder2 = Cylinderd(1.0, 2.0, Material(ignition.math.MaterialType_WOOD), + cylinder2 = Cylinderd(1.0, 2.0, Material(ignition.math.MaterialType.WOOD), Quaterniond(0.1, 0.2, 0.3)) self.assertEqual(cylinder, cylinder2) @@ -75,17 +75,17 @@ def test_mutators(self): cylinder.set_length(100.1) cylinder.set_radius(.123) cylinder.set_rotational_offset(Quaterniond(1.2, 2.3, 3.4)) - cylinder.set_mat(Material(ignition.math.MaterialType_PINE)) + cylinder.set_mat(Material(ignition.math.MaterialType.PINE)) self.assertEqual(100.1, cylinder.length()) self.assertEqual(.123, cylinder.radius()) self.assertEqual(Quaterniond(1.2, 2.3, 3.4), cylinder.rotational_offset()) - self.assertEqual(Material(ignition.math.MaterialType_PINE), cylinder.mat()) + self.assertEqual(Material(ignition.math.MaterialType.PINE), cylinder.mat()) def test_volume_and_density(self): mass = 1.0 cylinder = Cylinderd(1.0, 0.001) - expectedVolume = (IGN_PI * math.pow(0.001, 2) * 1.0) + expectedVolume = (math.pi * math.pow(0.001, 2) * 1.0) self.assertEqual(expectedVolume, cylinder.volume()) expectedDensity = mass / expectedVolume