From 908540189628f9c563dfee888a73863f47b57a2d Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Alejandro=20Hern=C3=A1ndez=20Cordero?= Date: Tue, 28 Dec 2021 02:33:55 +0100 Subject: [PATCH] Added RotationSpline pybind11 interface (#339) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Alejandro Hernández Signed-off-by: Louise Poubel Co-authored-by: Louise Poubel --- src/python/CMakeLists.txt | 1 - src/python_pybind11/CMakeLists.txt | 2 + src/python_pybind11/src/RotationSpline.cc | 72 +++++++++++++++++++ src/python_pybind11/src/RotationSpline.hh | 42 +++++++++++ .../src/_ignition_math_pybind11.cc | 3 + .../test}/RotationSpline_TEST.py | 0 6 files changed, 119 insertions(+), 1 deletion(-) create mode 100644 src/python_pybind11/src/RotationSpline.cc create mode 100644 src/python_pybind11/src/RotationSpline.hh rename src/{python => python_pybind11/test}/RotationSpline_TEST.py (100%) diff --git a/src/python/CMakeLists.txt b/src/python/CMakeLists.txt index f6a4d9d7c..85f9a3a19 100644 --- a/src/python/CMakeLists.txt +++ b/src/python/CMakeLists.txt @@ -100,7 +100,6 @@ if (PYTHONLIBS_FOUND) PID_TEST Plane_TEST python_TEST - RotationSpline_TEST SignalStats_TEST Sphere_TEST SphericalCoordinates_TEST diff --git a/src/python_pybind11/CMakeLists.txt b/src/python_pybind11/CMakeLists.txt index c94a8952a..e0c42e8b9 100644 --- a/src/python_pybind11/CMakeLists.txt +++ b/src/python_pybind11/CMakeLists.txt @@ -21,6 +21,7 @@ if (${pybind11_FOUND}) src/Material.cc src/Rand.cc src/RollingMean.cc + src/RotationSpline.cc src/SemanticVersion.cc src/Spline.cc src/StopWatch.cc @@ -90,6 +91,7 @@ if (${pybind11_FOUND}) Quaternion_TEST Rand_TEST RollingMean_TEST + RotationSpline_TEST SemanticVersion_TEST Spline_TEST StopWatch_TEST diff --git a/src/python_pybind11/src/RotationSpline.cc b/src/python_pybind11/src/RotationSpline.cc new file mode 100644 index 000000000..9c1b91df5 --- /dev/null +++ b/src/python_pybind11/src/RotationSpline.cc @@ -0,0 +1,72 @@ +/* + * 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. + * +*/ +#include +#include "RotationSpline.hh" +#include + +namespace ignition +{ +namespace math +{ +namespace python +{ +void defineMathRotationSpline(py::module &m, const std::string &typestr) +{ + using Class = ignition::math::RotationSpline; + std::string pyclass_name = typestr; + py::class_(m, + pyclass_name.c_str(), + py::buffer_protocol(), + py::dynamic_attr()) + .def(py::init<>()) + .def("add_point", + &Class::AddPoint, + "Adds a control point to the end of the spline.") + .def("point", + &Class::Point, + py::return_value_policy::reference, + "Gets the detail of one of the control points of the spline.") + .def("point_count", + &Class::PointCount, + "Gets the number of control points in the spline.") + .def("clear", + &Class::Clear, + "Clears all the points in the spline.") + .def("update_point", + &Class::UpdatePoint, + "Updates a single point in the spline.") + .def("interpolate", + py::overload_cast(&Class::Interpolate), + py::arg("_t") = 0, py::arg("_useShortestPath") = true, + "Returns an interpolated point based on a parametric " + "value over the whole series.") + .def("interpolate", + py::overload_cast + (&Class::Interpolate), + py::arg("_fromIndex") = 0, + py::arg("_t") = 0, py::arg("_useShortestPath") = true, + "Interpolates a single segment of the spline " + "given a parametric value.") + .def("auto_calculate", &Class::AutoCalculate, + "Tells the spline whether it should automatically calculate " + "tangents on demand as points are added.") + .def("recalc_tangents", &Class::RecalcTangents, + "Recalculates the tangents associated with this spline."); +} +} // namespace python +} // namespace math +} // namespace ignition diff --git a/src/python_pybind11/src/RotationSpline.hh b/src/python_pybind11/src/RotationSpline.hh new file mode 100644 index 000000000..670b2ad97 --- /dev/null +++ b/src/python_pybind11/src/RotationSpline.hh @@ -0,0 +1,42 @@ +/* + * 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__ROTATIONSPLINE_HH_ +#define IGNITION_MATH_PYTHON__ROTATIONSPLINE_HH_ + +#include +#include + +namespace py = pybind11; + +namespace ignition +{ +namespace math +{ +namespace python +{ +/// Define a pybind11 wrapper for an ignition::math::RotationSpline +/** + * \param[in] module a pybind11 module to add the definition to + * \param[in] typestr name of the type used by Python + */ +void defineMathRotationSpline(py::module &m, const std::string &typestr); +} // namespace python +} // namespace math +} // namespace ignition + +#endif // IGNITION_MATH_PYTHON__ROTATIONSPLINE_HH_ diff --git a/src/python_pybind11/src/_ignition_math_pybind11.cc b/src/python_pybind11/src/_ignition_math_pybind11.cc index 07b280036..6288d9c5a 100644 --- a/src/python_pybind11/src/_ignition_math_pybind11.cc +++ b/src/python_pybind11/src/_ignition_math_pybind11.cc @@ -28,6 +28,7 @@ #include "Quaternion.hh" #include "Rand.hh" #include "RollingMean.hh" +#include "RotationSpline.hh" #include "SemanticVersion.hh" #include "Spline.hh" #include "StopWatch.hh" @@ -64,6 +65,8 @@ PYBIND11_MODULE(math, m) ignition::math::python::defineMathRollingMean(m, "RollingMean"); + ignition::math::python::defineMathRotationSpline(m, "RotationSpline"); + ignition::math::python::defineMathSemanticVersion(m, "SemanticVersion"); ignition::math::python::defineMathSpline(m, "Spline"); diff --git a/src/python/RotationSpline_TEST.py b/src/python_pybind11/test/RotationSpline_TEST.py similarity index 100% rename from src/python/RotationSpline_TEST.py rename to src/python_pybind11/test/RotationSpline_TEST.py