From 58a6d2ad7deefddba912f42107c8f098e68d6f1e Mon Sep 17 00:00:00 2001 From: ahcorde Date: Tue, 21 Dec 2021 13:45:05 +0100 Subject: [PATCH] Added Quaternion pybind11 interface Signed-off-by: ahcorde --- src/python/CMakeLists.txt | 1 - src/python_pybind11/CMakeLists.txt | 1 + src/python_pybind11/src/Quaternion.hh | 210 ++++++++++++++++++ .../src/_ignition_math_pybind11.cc | 8 +- .../test}/Quaternion_TEST.py | 40 ++-- 5 files changed, 238 insertions(+), 22 deletions(-) create mode 100644 src/python_pybind11/src/Quaternion.hh rename src/{python => python_pybind11/test}/Quaternion_TEST.py (95%) diff --git a/src/python/CMakeLists.txt b/src/python/CMakeLists.txt index 01284951e..d6951dc4d 100644 --- a/src/python/CMakeLists.txt +++ b/src/python/CMakeLists.txt @@ -111,7 +111,6 @@ if (PYTHONLIBS_FOUND) Plane_TEST Pose3_TEST python_TEST - Quaternion_TEST Rand_TEST RollingMean_TEST RotationSpline_TEST diff --git a/src/python_pybind11/CMakeLists.txt b/src/python_pybind11/CMakeLists.txt index 368492352..6355588cc 100644 --- a/src/python_pybind11/CMakeLists.txt +++ b/src/python_pybind11/CMakeLists.txt @@ -62,6 +62,7 @@ if (${pybind11_FOUND}) if (BUILD_TESTING) # Add the Python tests set(python_tests + Quaternion_TEST Vector2_TEST Vector3_TEST Vector4_TEST diff --git a/src/python_pybind11/src/Quaternion.hh b/src/python_pybind11/src/Quaternion.hh new file mode 100644 index 000000000..8b0555595 --- /dev/null +++ b/src/python_pybind11/src/Quaternion.hh @@ -0,0 +1,210 @@ +/* + * 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__QUATERNION_HH_ +#define IGNITION_MATH_PYTHON__QUATERNION_HH_ + +#include + +#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::Quaternion +/** + * \param[in] module a pybind11 module to add the definition to + * \param[in] typestr name of the type used by Python + */ +template +void defineMathQuaternion(py::module &m, const std::string &typestr) +{ + using Class = ignition::math::Quaternion; + 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&, T>()) + .def(py::init&>()) + .def(py::init&>()) + .def(py::init()) + .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(py::self * float()) + .def(py::self * ignition::math::Vector3()) + .def(py::self *= py::self) + .def(py::self == py::self) + .def(py::self != py::self) + // .def(py::self * py::self) + // .def(py::self *= py::self) + .def("invert", + &Class::Invert, + "Invert the quaternion") + .def("inverse", + &Class::Inverse, + "Get the inverse of this quaternion") + .def("log", + &Class::Log, + "Return the logarithm") + .def("exp", + &Class::Exp, + "Return the exponent") + .def("normalize", + &Class::Normalize, + "Normalize the quaternion") + .def("normalized", + &Class::Normalized, + "Gets a normalized version of this quaternion") + .def("axis", + py::overload_cast(&Class::Axis), + "Set the quaternion from an axis and angle") + .def("axis", + py::overload_cast&, T>(&Class::Axis), + "Set the quaternion from an axis and angle") + .def("set", + &Class::Set, + "Set this quaternion from 4 floating numbers") + .def("euler", + py::overload_cast&>(&Class::Euler), + "Set the quaternion from Euler angles. The order of operations " + "is roll, pitch, yaw around a fixed body frame axis " + "(the original frame of the object before rotation is applied).") + .def("euler", + py::overload_cast(&Class::Euler), + "Set the quaternion from Euler angles.") + .def("euler", + py::overload_cast<>(&Class::Euler, py::const_), + "Return the rotation in Euler angles") + .def("euler_to_quaternion", + py::overload_cast&>( + &Class::EulerToQuaternion), + "Convert euler angles to quatern.") + .def("euler_to_quaternion", + py::overload_cast(&Class::EulerToQuaternion), + "Convert euler angles to quatern.") + .def("roll", &Class::Roll, "Get the Euler roll angle in radians") + .def("pitch", &Class::Pitch, "Get the Euler pitch angle in radians") + .def("yaw", &Class::Yaw, "Get the Euler yaw angle in radians") + .def("to_axis", + [](const Class &self) { + ignition::math::Vector3 _axis; + T _angle; + self.ToAxis(_axis, _angle); + return std::make_tuple(_axis, _angle); + }, + "Return rotation as axis and angle") + .def("matrix", + &Class::Matrix, + "Set from a rotation matrix.") + .def("from_2_axes", + &Class::From2Axes, + "Set this quaternion to represent rotation from " + "vector _v1 to vector _v2, so that") + .def("scale", + &Class::Scale, + "Scale a Quaternion") + .def("equal", + &Class::Equal, + "Equality test with tolerance.") + .def("rotate_vector", + &Class::RotateVector, + "Rotate a vector using the quaternion") + .def("rotate_vector_reverse", + &Class::RotateVectorReverse, + "Do the reverse rotation of a vector by this quaternion") + .def("is_finite", + &Class::IsFinite, + "See if a quaternion is finite (e.g., not nan)") + .def("correct", + &Class::Correct, + "Correct any nan values in this quaternion") + .def("x_axis", + &Class::XAxis, + "Return the X axis") + .def("y_axis", + &Class::YAxis, + "Return the Y axis") + .def("z_axis", + &Class::ZAxis, + "Return the Z axis") + .def("round", + &Class::Round, + "Round all values to _precision decimal places") + .def("dot", + &Class::Dot, + "Dot product") + .def("squad", + &Class::Squad, + "Spherical quadratic interpolation " + "given the ends and an interpolation parameter between 0 and 1") + .def("slerp", + &Class::Slerp, + "Spherical linear interpolation between 2 quaternions, " + " given the ends and an interpolation parameter between 0 and 1") + .def("integrate", + &Class::Integrate, + "Integrate quaternion for constant angular velocity vector " + "along specified interval `_deltaT`.") + .def("x", py::overload_cast<>(&Class::X), "Get the x value.") + .def("y", py::overload_cast<>(&Class::Y), "Get the y value.") + .def("z", py::overload_cast<>(&Class::Z), "Get the z value.") + .def("w", py::overload_cast<>(&Class::W), "Get the w value.") + .def("x", py::overload_cast(&Class::X), "Set the x value.") + .def("y", py::overload_cast(&Class::Y), "Set the y value.") + .def("z", py::overload_cast(&Class::Z), "Set the z value.") + .def("w", py::overload_cast(&Class::W), "Set the w value.") + .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__QUATERNION_HH_ diff --git a/src/python_pybind11/src/_ignition_math_pybind11.cc b/src/python_pybind11/src/_ignition_math_pybind11.cc index da0029102..2b1f0b042 100644 --- a/src/python_pybind11/src/_ignition_math_pybind11.cc +++ b/src/python_pybind11/src/_ignition_math_pybind11.cc @@ -14,6 +14,7 @@ #include +#include "Quaternion.hh" #include "Vector2.hh" #include "Vector3.hh" #include "Vector4.hh" @@ -24,9 +25,6 @@ PYBIND11_MODULE(math, m) { m.doc() = "Ignition Math Python Library."; - - - ignition::math::python::defineMathVector2(m, "Vector2d"); ignition::math::python::defineMathVector2(m, "Vector2i"); ignition::math::python::defineMathVector2(m, "Vector2f"); @@ -38,4 +36,8 @@ PYBIND11_MODULE(math, m) ignition::math::python::defineMathVector4(m, "Vector4d"); ignition::math::python::defineMathVector4(m, "Vector4i"); ignition::math::python::defineMathVector4(m, "Vector4f"); + + ignition::math::python::defineMathQuaternion(m, "Quaternioni"); + ignition::math::python::defineMathQuaternion(m, "Quaterniond"); + ignition::math::python::defineMathQuaternion(m, "Quaternionf"); } diff --git a/src/python/Quaternion_TEST.py b/src/python_pybind11/test/Quaternion_TEST.py similarity index 95% rename from src/python/Quaternion_TEST.py rename to src/python_pybind11/test/Quaternion_TEST.py index c99ffc07f..215159f19 100644 --- a/src/python/Quaternion_TEST.py +++ b/src/python_pybind11/test/Quaternion_TEST.py @@ -14,8 +14,10 @@ import math import unittest -from ignition.math import Matrix3d -from ignition.math import Matrix4d +# 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 from ignition.math import Quaternioni @@ -342,22 +344,24 @@ 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)) - - self.assertTrue(Matrix4d(q) == Matrix4d( - 0.617229, -0.589769, 0.52077, 0, - 0.707544, 0.705561, -0.0395554, 0, - -0.344106, 0.392882, 0.85278, 0, - 0, 0, 0, 1)) + # 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, + # -0.344106, 0.392882, 0.85278, 0, + # 0, 0, 0, 1)) def test_stream_out(self): q = Quaterniond(0.1, 1.2, 2.3)