Skip to content

Commit

Permalink
Added Matrix3 pybind11 interface (#325)
Browse files Browse the repository at this point in the history
Signed-off-by: ahcorde <[email protected]>
Signed-off-by: Louise Poubel <[email protected]>

Co-authored-by: Louise Poubel <[email protected]>
  • Loading branch information
ahcorde and chapulina authored Dec 27, 2021
1 parent 1cac654 commit abd66cf
Show file tree
Hide file tree
Showing 6 changed files with 143 additions and 17 deletions.
1 change: 0 additions & 1 deletion src/python/CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -98,7 +98,6 @@ if (PYTHONLIBS_FOUND)
Kmeans_TEST
MassMatrix3_TEST
Material_TEST
Matrix3_TEST
Matrix4_TEST
OrientedBox_TEST
PID_TEST
Expand Down
1 change: 1 addition & 0 deletions src/python_pybind11/CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -77,6 +77,7 @@ if (${pybind11_FOUND})
Helpers_TEST
Line2_TEST
Line3_TEST
Matrix3_TEST
MovingWindowFilter_TEST
Quaternion_TEST
Rand_TEST
Expand Down
121 changes: 121 additions & 0 deletions src/python_pybind11/src/Matrix3.hh
Original file line number Diff line number Diff line change
@@ -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 <string>

#include <pybind11/pybind11.h>
#include <pybind11/operators.h>

#include <ignition/math/Matrix3.hh>

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<typename T>
void defineMathMatrix3(py::module &m, const std::string &typestr)
{
using Class = ignition::math::Matrix3<T>;
auto toString = [](const Class &si) {
std::stringstream stream;
stream << si;
return stream.str();
};
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<Class>())
.def(py::init<T, T, T, T, T, T, T, T, T>())
.def(py::init<const ignition::math::Quaternion<T>&>())
.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<T>())
// .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<size_t, size_t>(&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_
5 changes: 5 additions & 0 deletions src/python_pybind11/src/_ignition_math_pybind11.cc
Original file line number Diff line number Diff line change
Expand Up @@ -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"
Expand Down Expand Up @@ -73,6 +74,10 @@ PYBIND11_MODULE(math, m)
ignition::math::python::defineMathLine3<double>(m, "Line3d");
ignition::math::python::defineMathLine3<float>(m, "Line3f");

ignition::math::python::defineMathMatrix3<int>(m, "Matrix3i");
ignition::math::python::defineMathMatrix3<double>(m, "Matrix3d");
ignition::math::python::defineMathMatrix3<float>(m, "Matrix3f");

ignition::math::python::defineMathQuaternion<int>(m, "Quaternioni");
ignition::math::python::defineMathQuaternion<double>(m, "Quaterniond");
ignition::math::python::defineMathQuaternion<float>(m, "Quaternionf");
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand All @@ -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)


Expand Down
24 changes: 12 additions & 12 deletions src/python_pybind11/test/Quaternion_TEST.py
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down Expand Up @@ -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,
Expand Down

0 comments on commit abd66cf

Please sign in to comment.