diff --git a/src/python/CMakeLists.txt b/src/python/CMakeLists.txt index 10102a3f9..5510400a6 100644 --- a/src/python/CMakeLists.txt +++ b/src/python/CMakeLists.txt @@ -90,7 +90,6 @@ if (PYTHONLIBS_FOUND) Box_TEST Cylinder_TEST Frustum_TEST - OrientedBox_TEST Plane_TEST python_TEST SignalStats_TEST diff --git a/src/python_pybind11/CMakeLists.txt b/src/python_pybind11/CMakeLists.txt index 027934333..5f088ab82 100644 --- a/src/python_pybind11/CMakeLists.txt +++ b/src/python_pybind11/CMakeLists.txt @@ -98,6 +98,7 @@ if (${pybind11_FOUND}) Matrix4_TEST MassMatrix3_TEST MovingWindowFilter_TEST + OrientedBox_TEST PID_TEST Pose3_TEST Quaternion_TEST diff --git a/src/python_pybind11/src/OrientedBox.hh b/src/python_pybind11/src/OrientedBox.hh new file mode 100644 index 000000000..efb73e72d --- /dev/null +++ b/src/python_pybind11/src/OrientedBox.hh @@ -0,0 +1,129 @@ +/* + * 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__ORIENTEDBOX_HH_ +#define IGNITION_MATH_PYTHON__ORIENTEDBOX_HH_ + +#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::OrientedBox +/** + * \param[in] module a pybind11 module to add the definition to + * \param[in] typestr name of the type used by Python + */ +template +void defineMathOrientedBox(py::module &m, const std::string &typestr) +{ + + using Class = ignition::math::OrientedBox; + std::string pyclass_name = typestr; + auto toString = [](const Class &si) { + std::stringstream stream; + stream << si; + return stream.str(); + }; + py::class_(m, + pyclass_name.c_str(), + py::buffer_protocol(), + py::dynamic_attr()) + .def(py::init<>()) + .def(py::init&, + const ignition::math::Pose3&>()) + .def(py::init&, + const ignition::math::Pose3&, + const ignition::math::Material&>()) + .def(py::init&>()) + .def(py::init()) + .def(py::init&, + const ignition::math::Material&>()) + .def(py::self != py::self) + .def(py::self == py::self) + .def("pose", + py::overload_cast&>(&Class::Pose), + "Set the box pose, which is the pose of its center.") + .def("pose", + py::overload_cast<>(&Class::Pose, py::const_), + py::return_value_policy::reference, + "Get the box pose, which is the pose of its center.") + .def("size", + py::overload_cast<>(&Class::Size, py::const_), + "Get the size of the OrientedBox.") + .def("size", + py::overload_cast&> + (&Class::Size), + "Set the size of the OrientedBox.") + .def("x_length", + &Class::XLength, + "Get the length along the x dimension") + .def("y_length", + &Class::YLength, + "Get the length along the y dimension") + .def("z_length", + &Class::ZLength, + "Get the length along the z dimension") + .def("material", + &Class::Material, + "Get the material associated with this OrientedBox.") + .def("set_material", + &Class::SetMaterial, + "Set the material associated with this OrientedBox.") + .def("volume", + &Class::Volume, + "Get the volume of the OrientedBox in m^3.") + .def("contains", + &Class::Contains, + "Check if a point lies inside the box.") + .def("density_from_mass", + &Class::DensityFromMass, + "Compute the OrientedBox's density given a mass value.") + .def("set_density_from_mass", + &Class::SetDensityFromMass, + "Set the density of this OrientedBox based on a mass value.") + .def("mass_matrix", + &Class::MassMatrix, + "Get the mass matrix for this OrientedBox. This function " + "is only meaningful if the OrientedBox'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) + .def("__str__", toString) + .def("__repr__", toString); +} + +} // namespace python +} // namespace math +} // namespace ignition + +#endif // IGNITION_MATH_PYTHON__ORIENTEDBOX_HH_ diff --git a/src/python_pybind11/src/_ignition_math_pybind11.cc b/src/python_pybind11/src/_ignition_math_pybind11.cc index 4c0496813..2766afc2f 100644 --- a/src/python_pybind11/src/_ignition_math_pybind11.cc +++ b/src/python_pybind11/src/_ignition_math_pybind11.cc @@ -30,6 +30,7 @@ #include "Matrix3.hh" #include "Matrix4.hh" #include "MovingWindowFilter.hh" +#include "OrientedBox.hh" #include "PID.hh" #include "Pose3.hh" #include "Quaternion.hh" @@ -133,6 +134,8 @@ PYBIND11_MODULE(math, m) ignition::math::python::defineMathQuaternion(m, "Quaterniond"); ignition::math::python::defineMathQuaternion(m, "Quaternionf"); + ignition::math::python::defineMathOrientedBox(m, "OrientedBoxd"); + ignition::math::python::defineMathPose3(m, "Pose3i"); ignition::math::python::defineMathPose3(m, "Pose3d"); ignition::math::python::defineMathPose3(m, "Pose3f"); diff --git a/src/python/OrientedBox_TEST.py b/src/python_pybind11/test/OrientedBox_TEST.py similarity index 98% rename from src/python/OrientedBox_TEST.py rename to src/python_pybind11/test/OrientedBox_TEST.py index 5eb671383..d750bd4ed 100644 --- a/src/python/OrientedBox_TEST.py +++ b/src/python_pybind11/test/OrientedBox_TEST.py @@ -12,9 +12,10 @@ # See the License for the specific language governing permissions and # limitations under the License. +import math import unittest -from ignition.math import IGN_PI, OrientedBoxd, MassMatrix3d, Material, Pose3d, Vector3d +from ignition.math import OrientedBoxd, MassMatrix3d, Material, Pose3d, Vector3d g_tolerance = 1e-6 @@ -205,7 +206,7 @@ def test_contains_oriented_position(self): def test_contains_oriented_rotation(self): # Rotate PI/2 about +x: swap Z and Y - box = OrientedBoxd(Vector3d(1, 2, 3), Pose3d(0, 0, 0, IGN_PI*0.5, 0, 0)) + box = OrientedBoxd(Vector3d(1, 2, 3), Pose3d(0, 0, 0, math.pi*0.5, 0, 0)) # Doesn't contain non-rotated vertices self.assertFalse(box.contains(Vector3d(-0.5, -1.0, -1.5)))