From abd66cf0d6280bfc31cc6822032bbba23b83f0d6 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Alejandro=20Hern=C3=A1ndez=20Cordero?= Date: Mon, 27 Dec 2021 17:59:05 +0100 Subject: [PATCH] Added Matrix3 pybind11 interface (#325) Signed-off-by: ahcorde Signed-off-by: Louise Poubel Co-authored-by: Louise Poubel --- src/python/CMakeLists.txt | 1 - src/python_pybind11/CMakeLists.txt | 1 + src/python_pybind11/src/Matrix3.hh | 121 ++++++++++++++++++ .../src/_ignition_math_pybind11.cc | 5 + .../test}/Matrix3_TEST.py | 8 +- src/python_pybind11/test/Quaternion_TEST.py | 24 ++-- 6 files changed, 143 insertions(+), 17 deletions(-) create mode 100644 src/python_pybind11/src/Matrix3.hh rename src/{python => python_pybind11/test}/Matrix3_TEST.py (98%) diff --git a/src/python/CMakeLists.txt b/src/python/CMakeLists.txt index 8ea584796..81c082969 100644 --- a/src/python/CMakeLists.txt +++ b/src/python/CMakeLists.txt @@ -98,7 +98,6 @@ if (PYTHONLIBS_FOUND) Kmeans_TEST MassMatrix3_TEST Material_TEST - Matrix3_TEST Matrix4_TEST OrientedBox_TEST PID_TEST diff --git a/src/python_pybind11/CMakeLists.txt b/src/python_pybind11/CMakeLists.txt index 9f5600705..a4dd7af6c 100644 --- a/src/python_pybind11/CMakeLists.txt +++ b/src/python_pybind11/CMakeLists.txt @@ -77,6 +77,7 @@ if (${pybind11_FOUND}) Helpers_TEST Line2_TEST Line3_TEST + Matrix3_TEST MovingWindowFilter_TEST Quaternion_TEST Rand_TEST diff --git a/src/python_pybind11/src/Matrix3.hh b/src/python_pybind11/src/Matrix3.hh new file mode 100644 index 000000000..4f0322d07 --- /dev/null +++ b/src/python_pybind11/src/Matrix3.hh @@ -0,0 +1,121 @@ +/* + * 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__MATRIX3_HH_ +#define IGNITION_MATH_PYTHON__MATRIX3_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::Matrix3 +/** + * \param[in] module a pybind11 module to add the definition to + * \param[in] typestr name of the type used by Python + */ +template +void defineMathMatrix3(py::module &m, const std::string &typestr) +{ + using Class = ignition::math::Matrix3; + auto toString = [](const Class &si) { + std::stringstream stream; + stream << si; + return stream.str(); + }; + 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()) + .def(py::init&>()) + .def(py::self - py::self) + .def(py::self + py::self) + .def(py::self * py::self) + .def(py::self * float()) + .def(py::self * ignition::math::Vector3()) + // .def(py::self * py::self) + // .def(py::self += py::self) + // .def(-py::self) + // .def(py::self -= py::self) + .def(py::self == py::self) + .def(py::self != py::self) + .def("__call__", + py::overload_cast(&Class::operator()), + py::return_value_policy::reference) + // .def(py::self *= py::self) + .def("set", + &Class::Set, + "Set values") + .def("axes", + &Class::Axes, + "Set the matrix from three axis (1 per column)") + .def("axis", + &Class::Axis, + "Set the matrix from an axis and angle") + .def("from_2_axes", + &Class::From2Axes, + "Set the matrix to represent rotation from " + "vector _v1 to vector _v2, so that") + .def("col", + &Class::Col, + "Set a column.") + .def("equal", + &Class::Equal, + "Equality test operator") + .def("determinant", + &Class::Determinant, + "Return the determinant of the matrix") + .def("inverse", + &Class::Inverse, + "Return the inverse matrix") + .def("transpose", + &Class::Transpose, + "Transpose this matrix.") + .def("transposed", + &Class::Transposed, + "Return the transpose of this matrix") + .def("__copy__", [](const Class &self) { + return Class(self); + }) + .def("__deepcopy__", [](const Class &self, py::dict) { + return Class(self); + }, "memo"_a) + .def_readonly_static("IDENTITY", &Class::Identity, "Identity matrix") + .def_readonly_static("ZERO", &Class::Zero, "Zero matrix") + .def("__str__", toString) + .def("__repr__", toString); +} +} // namespace python +} // namespace math +} // namespace ignition + +#endif // IGNITION_MATH_PYTHON__MATRIX3_HH_ diff --git a/src/python_pybind11/src/_ignition_math_pybind11.cc b/src/python_pybind11/src/_ignition_math_pybind11.cc index cca30a6fe..cec86c1a8 100644 --- a/src/python_pybind11/src/_ignition_math_pybind11.cc +++ b/src/python_pybind11/src/_ignition_math_pybind11.cc @@ -19,6 +19,7 @@ #include "Helpers.hh" #include "Line2.hh" #include "Line3.hh" +#include "Matrix3.hh" #include "MovingWindowFilter.hh" #include "Quaternion.hh" #include "Rand.hh" @@ -73,6 +74,10 @@ PYBIND11_MODULE(math, m) ignition::math::python::defineMathLine3(m, "Line3d"); ignition::math::python::defineMathLine3(m, "Line3f"); + ignition::math::python::defineMathMatrix3(m, "Matrix3i"); + ignition::math::python::defineMathMatrix3(m, "Matrix3d"); + ignition::math::python::defineMathMatrix3(m, "Matrix3f"); + ignition::math::python::defineMathQuaternion(m, "Quaternioni"); ignition::math::python::defineMathQuaternion(m, "Quaterniond"); ignition::math::python::defineMathQuaternion(m, "Quaternionf"); diff --git a/src/python/Matrix3_TEST.py b/src/python_pybind11/test/Matrix3_TEST.py similarity index 98% rename from src/python/Matrix3_TEST.py rename to src/python_pybind11/test/Matrix3_TEST.py index 22941264f..e259366cb 100644 --- a/src/python/Matrix3_TEST.py +++ b/src/python_pybind11/test/Matrix3_TEST.py @@ -306,7 +306,7 @@ def test_from_2axes(self): def test_to_quaternion(self): q = Quaterniond(math.pi/2.0, math.pi/2.0, 0) matFromQuat = Matrix3d(q) - quatFromMat = matFromQuat.to_quaternion() + quatFromMat = Quaterniond(matFromQuat) self.assertTrue(q == quatFromMat) # test the cases where matrix trace is negative @@ -315,21 +315,21 @@ def test_to_quaternion(self): mat = Matrix3d(-1, 0, 0, 0, -1, 0, 0, 0, 1) - q2 = mat.to_quaternion() + q2 = Quaterniond(mat) self.assertTrue(q == q2) q = Quaterniond(0, 0, 1, 0) mat = Matrix3d(-1, 0, 0, 0, 1, 0, 0, 0, -1) - q2 = mat.to_quaternion() + q2 = Quaterniond(mat) self.assertTrue(q == q2) q = Quaterniond(0, 1, 0, 0) mat = Matrix3d(1, 0, 0, 0, -1, 0, 0, 0, -1) - q2 = mat.to_quaternion() + q2 = Quaterniond(mat) self.assertTrue(q == q2) diff --git a/src/python_pybind11/test/Quaternion_TEST.py b/src/python_pybind11/test/Quaternion_TEST.py index 215159f19..7d5d0e164 100644 --- a/src/python_pybind11/test/Quaternion_TEST.py +++ b/src/python_pybind11/test/Quaternion_TEST.py @@ -14,9 +14,9 @@ import math import unittest +from ignition.math import Matrix3d # TODO(ahcorde): Enable the corresponding tests when these classes # are ported -# from ignition.math import Matrix3d # from ignition.math import Matrix4d from ignition.math import Quaterniond from ignition.math import Quaternionf @@ -344,19 +344,19 @@ def test_math(self): self.assertTrue(-q == Quaterniond(-0.891007, -0.121334, -0.242668, -0.364002)) + self.assertTrue(Matrix3d(q) == Matrix3d( + 0.617229, -0.589769, 0.52077, + 0.707544, 0.705561, -0.0395554, + -0.344106, 0.392882, 0.85278)) + + matFromQ = Matrix3d(q) + self.assertTrue(matFromQ == Matrix3d( + 0.617229, -0.589769, 0.52077, + 0.707544, 0.705561, -0.0395554, + -0.344106, 0.392882, 0.85278)) + # TODO(ahcorde): Enable the corresponding tests when these classes # are ported - # self.assertTrue(Matrix3d(q) == Matrix3d( - # 0.617229, -0.589769, 0.52077, - # 0.707544, 0.705561, -0.0395554, - # -0.344106, 0.392882, 0.85278)) - # - # matFromQ = Matrix3d(q) - # self.assertTrue(matFromQ == Matrix3d( - # 0.617229, -0.589769, 0.52077, - # 0.707544, 0.705561, -0.0395554, - # -0.344106, 0.392882, 0.85278)) - # # self.assertTrue(Matrix4d(q) == Matrix4d( # 0.617229, -0.589769, 0.52077, 0, # 0.707544, 0.705561, -0.0395554, 0,