From f6c26bfc32b730a4a18b735ba492eca53188b186 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Alejandro=20Hern=C3=A1ndez=20Cordero?= Date: Mon, 27 Dec 2021 22:30:31 +0100 Subject: [PATCH] Added Pose3 pybind11 interface (#334) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Alejandro Hernández --- src/python/CMakeLists.txt | 1 - src/python_pybind11/CMakeLists.txt | 1 + src/python_pybind11/src/Pose3.hh | 150 ++++++++++++++++++ .../src/_ignition_math_pybind11.cc | 5 + .../test}/Pose3_TEST.py | 1 - 5 files changed, 156 insertions(+), 2 deletions(-) create mode 100644 src/python_pybind11/src/Pose3.hh rename src/{python => python_pybind11/test}/Pose3_TEST.py (99%) diff --git a/src/python/CMakeLists.txt b/src/python/CMakeLists.txt index 39a5915a3..cc85cf23d 100644 --- a/src/python/CMakeLists.txt +++ b/src/python/CMakeLists.txt @@ -101,7 +101,6 @@ if (PYTHONLIBS_FOUND) OrientedBox_TEST PID_TEST Plane_TEST - Pose3_TEST python_TEST RotationSpline_TEST SignalStats_TEST diff --git a/src/python_pybind11/CMakeLists.txt b/src/python_pybind11/CMakeLists.txt index a9bfb2e31..ffaa46299 100644 --- a/src/python_pybind11/CMakeLists.txt +++ b/src/python_pybind11/CMakeLists.txt @@ -82,6 +82,7 @@ if (${pybind11_FOUND}) Line3_TEST Matrix3_TEST MovingWindowFilter_TEST + Pose3_TEST Quaternion_TEST Rand_TEST RollingMean_TEST diff --git a/src/python_pybind11/src/Pose3.hh b/src/python_pybind11/src/Pose3.hh new file mode 100644 index 000000000..8d5d9b4ee --- /dev/null +++ b/src/python_pybind11/src/Pose3.hh @@ -0,0 +1,150 @@ +/* + * 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__POSE3_HH_ +#define IGNITION_MATH_PYTHON__POSE3_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::Pose3 +/** + * \param[in] module a pybind11 module to add the definition to + * \param[in] typestr name of the type used by Python + */ +template +void defineMathPose3(py::module &m, const std::string &typestr) +{ + using Class = ignition::math::Pose3; + 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&, + const ignition::math::Quaternion&>()) + .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 != py::self) + .def(py::self * py::self) + .def(py::self *= py::self) + .def("set", + py::overload_cast&, + const ignition::math::Quaternion&>(&Class::Set), + "Set the pose from a Vector3 and a Quaternion") + .def("set", + py::overload_cast&, + const ignition::math::Vector3&>(&Class::Set), + "Set the pose from pos and rpy vectors") + .def("set", + py::overload_cast(&Class::Set), + "Set the pose from a six tuple.") + .def("is_finite", + &Class::IsFinite, + "See if a pose is finite (e.g., not nan)") + .def("correct", + &Class::Correct, + "Fix any nan values") + .def("inverse", + &Class::Inverse, + "Get the inverse of this pose") + .def("coord_position_add", + py::overload_cast&>( + &Class::CoordPositionAdd, py::const_), + "Add one point to a vector: result = this + pos") + .def("coord_position_add", + py::overload_cast( + &Class::CoordPositionAdd, py::const_), + "Add one point to another: result = this + pose") + .def("coord_position_sub", + &Class::CoordPositionSub, + "Subtract one position from another: result = this - pose") + .def("coord_rotation_add", + &Class::CoordRotationAdd, + "Add one rotation to another: result = this->q + rot") + .def("coord_rotation_sub", + &Class::CoordRotationSub, + "Subtract one rotation from another: result = this->q - rot") + .def("coord_pose_solve", + &Class::CoordPoseSolve, + "Find the inverse of a pose; i.e., if b = this + a, given b and " + "this, find a") + .def("reset", &Class::Reset, "Reset the pose") + .def("rotate_position_about_origin", + &Class::RotatePositionAboutOrigin, + "Rotate vector part of a pose about the origin") + .def("round", + &Class::Round, + "Round all values to _precision decimal places") + .def("pos", + py::overload_cast<>(&Class::Pos, py::const_), + py::return_value_policy::reference, + "Get the position.") + .def("x", &Class::X, "Get the X value of the position") + .def("y", &Class::Y, "Get the Y value of the position") + .def("z", &Class::Z, "Get the Z value of the position") + .def("set_x", &Class::SetX, "Set the X value of the position") + .def("set_y", &Class::SetY, "Set the Y value of the position") + .def("set_z", &Class::SetZ, "Set the Z value of the position") + .def("rot", + py::overload_cast<>(&Class::Rot, py::const_), + py::return_value_policy::reference, + "Get the rotation.") + .def("roll", &Class::Roll, "Get the Roll value of the position") + .def("pitch", &Class::Pitch, "Get the Pitch value of the position") + .def("yaw", &Class::Yaw, "Get the Yaw value of the position") + .def("__copy__", [](const Class &self) { + return Class(self); + }) + .def("__deepcopy__", [](const Class &self, py::dict) { + return Class(self); + }, "memo"_a) + .def_readonly_static("ZERO", &Class::Zero, "Zero matrix") + .def("__str__", toString) + .def("__repr__", toString); +} +} // namespace python +} // namespace gazebo +} // namespace ignition + +#endif // IGNITION_MATH_PYTHON__POSE3_HH_ diff --git a/src/python_pybind11/src/_ignition_math_pybind11.cc b/src/python_pybind11/src/_ignition_math_pybind11.cc index c579d1a98..b7169b1ad 100644 --- a/src/python_pybind11/src/_ignition_math_pybind11.cc +++ b/src/python_pybind11/src/_ignition_math_pybind11.cc @@ -22,6 +22,7 @@ #include "Line3.hh" #include "Matrix3.hh" #include "MovingWindowFilter.hh" +#include "Pose3.hh" #include "Quaternion.hh" #include "Rand.hh" #include "RollingMean.hh" @@ -94,6 +95,10 @@ PYBIND11_MODULE(math, m) ignition::math::python::defineMathQuaternion(m, "Quaterniond"); ignition::math::python::defineMathQuaternion(m, "Quaternionf"); + ignition::math::python::defineMathPose3(m, "Pose3i"); + ignition::math::python::defineMathPose3(m, "Pose3d"); + ignition::math::python::defineMathPose3(m, "Pose3f"); + ignition::math::python::defineMathFilter(m, "Filteri"); ignition::math::python::defineMathFilter(m, "Filterf"); ignition::math::python::defineMathFilter(m, "Filterd"); diff --git a/src/python/Pose3_TEST.py b/src/python_pybind11/test/Pose3_TEST.py similarity index 99% rename from src/python/Pose3_TEST.py rename to src/python_pybind11/test/Pose3_TEST.py index 7b1c8b508..413796230 100644 --- a/src/python/Pose3_TEST.py +++ b/src/python_pybind11/test/Pose3_TEST.py @@ -114,7 +114,6 @@ def test_pose(self): pose.pos().set(5, 6, 7) pose.rot().euler(Vector3d(.4, .6, 0)) - self.assertTrue(pose.coord_position_add(Vector3d(1, 2, 3)) == Vector3d(7.82531, 6.67387, 9.35871))