From 961cd0dfecd9ca9669f7885c969b2f1942d082ce Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Alejandro=20Herna=CC=81ndez?= Date: Mon, 27 Dec 2021 20:11:00 +0100 Subject: [PATCH 1/8] Added SignalStats pybind11 interface 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 | 2 + src/python_pybind11/src/SignalStats.cc | 548 ++++++++++++++++++ src/python_pybind11/src/SignalStats.hh | 91 +++ .../src/_ignition_math_pybind11.cc | 12 + .../test}/SignalStats_TEST.py | 0 6 files changed, 653 insertions(+), 1 deletion(-) create mode 100644 src/python_pybind11/src/SignalStats.cc create mode 100644 src/python_pybind11/src/SignalStats.hh rename src/{python => python_pybind11/test}/SignalStats_TEST.py (100%) diff --git a/src/python/CMakeLists.txt b/src/python/CMakeLists.txt index 8ea584796..548bbe728 100644 --- a/src/python/CMakeLists.txt +++ b/src/python/CMakeLists.txt @@ -107,7 +107,6 @@ if (PYTHONLIBS_FOUND) python_TEST RotationSpline_TEST SemanticVersion_TEST - SignalStats_TEST Sphere_TEST SphericalCoordinates_TEST Spline_TEST diff --git a/src/python_pybind11/CMakeLists.txt b/src/python_pybind11/CMakeLists.txt index 9f5600705..399b9d8a0 100644 --- a/src/python_pybind11/CMakeLists.txt +++ b/src/python_pybind11/CMakeLists.txt @@ -19,6 +19,7 @@ if (${pybind11_FOUND}) src/Helpers.cc src/Rand.cc src/RollingMean.cc + src/SignalStats.cc src/StopWatch.cc ) @@ -81,6 +82,7 @@ if (${pybind11_FOUND}) Quaternion_TEST Rand_TEST RollingMean_TEST + SignalStats_TEST StopWatch_TEST Vector2_TEST Vector3_TEST diff --git a/src/python_pybind11/src/SignalStats.cc b/src/python_pybind11/src/SignalStats.cc new file mode 100644 index 000000000..61ed54613 --- /dev/null +++ b/src/python_pybind11/src/SignalStats.cc @@ -0,0 +1,548 @@ +/* + * 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 + +#include "SignalStats.hh" +#include + +namespace ignition +{ +namespace math +{ +namespace python +{ +////////////////////////////////////////////////// +void defineMathSignalStats(py::module &m, const std::string &typestr) +{ + using Class = ignition::math::SignalStats; + 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("count", &Class::Count, "Get number of data points in first statistic.") + .def("reset", &Class::Reset, "Forget all previous data.") + .def("map", &Class::Map, + "Get the current values of each statistical measure, " + "stored in a map using the short name as the key.") + .def("insert_data", + &Class::InsertData, + "Add a new sample to the statistical measures.") + .def("insert_statistic", + &Class::InsertStatistic, + "Add a new type of statistic.") + .def("insert_statistics", + &Class::InsertStatistics, + "Add multiple statistics."); + py::bind_map>(m, "SignalMap"); +} + +////////////////////////////////////////////////// +class SignalStatisticTrampoline : public ignition::math::SignalStatistic +{ +public: + SignalStatisticTrampoline() : SignalStatistic() {}; + SignalStatisticTrampoline(const SignalStatistic &_ss) : SignalStatistic(_ss) {}; + + // Trampoline (need one for each virtual function) + double Value() const override + { + PYBIND11_OVERLOAD_PURE( + double, // Return type (ret_type) + ignition::math::SignalStatistic, // Parent class (cname) + // Name of function in C++ (must match Python name) (fn) + Value, + ); + } + // Trampoline (need one for each virtual function) + std::string ShortName() const override + { + PYBIND11_OVERLOAD_PURE( + std::string, // Return type (ret_type) + ignition::math::SignalStatistic, // Parent class (cname) + // Name of function in C++ (must match Python name) (fn) + ShortName, + ); + } + // Trampoline (need one for each virtual function) + size_t Count() const override + { + PYBIND11_OVERLOAD_PURE( + size_t, // Return type (ret_type) + ignition::math::SignalStatistic, // Parent class (cname) + // Name of function in C++ (must match Python name) (fn) + Count, + ); + } + // Trampoline (need one for each virtual function) + void InsertData(const double _data) override + { + PYBIND11_OVERLOAD_PURE( + void, // Return type (ret_type) + ignition::math::SignalStatistic, // Parent class (cname) + // Name of function in C++ (must match Python name) (fn) + InsertData, + _data // Argument(s) (...) + ); + } + // Trampoline (need one for each virtual function) + void Reset() override + { + PYBIND11_OVERLOAD_PURE( + void, // Return type (ret_type) + ignition::math::SignalStatistic, // Parent class (cname) + // Name of function in C++ (must match Python name) (fn) + Reset, + ); + } +}; + +////////////////////////////////////////////////// +void defineMathSignalStatistic(py::module &m, const std::string &typestr) +{ + using Class = ignition::math::SignalStatistic; + 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("value", + &Class::Value, + "Get a short version of the name of this statistical measure.") + .def("short_name", + &Class::ShortName, + "Get a short version of the name of this statistical measure.") + .def("count", + &Class::Count, + "Get number of data points in measurement.") + .def("insert_data", + &Class::InsertData, + "Add a new sample to the statistical measure.") + .def("reset", + &Class::Reset, + "Forget all previous data."); +} + +////////////////////////////////////////////////// +class SignalVarianceTrampoline : public ignition::math::SignalVariance { +public: + // Inherit the constructors + SignalVarianceTrampoline(){} + + // Trampoline (need one for each virtual function) + double Value() const override + { + PYBIND11_OVERLOAD_PURE( + double, // Return type (ret_type) + ignition::math::SignalStatistic, // Parent class (cname) + // Name of function in C++ (must match Python name) (fn) + Value, + ); + } + // Trampoline (need one for each virtual function) + std::string ShortName() const override + { + PYBIND11_OVERLOAD_PURE( + std::string, // Return type (ret_type) + ignition::math::SignalStatistic, // Parent class (cname) + // Name of function in C++ (must match Python name) (fn) + ShortName, + ); + } + // Trampoline (need one for each virtual function) + void InsertData(const double _data) override + { + PYBIND11_OVERLOAD_PURE( + void, // Return type (ret_type) + ignition::math::SignalStatistic, // Parent class (cname) + // Name of function in C++ (must match Python name) (fn) + InsertData, + _data // Argument(s) (...) + ); + } +}; + +////////////////////////////////////////////////// +void defineMathSignalVariance(py::module &m, const std::string &typestr) +{ + using Class = ignition::math::SignalVariance; + std::string pyclass_name = typestr; + py::class_(m, + pyclass_name.c_str(), + py::buffer_protocol(), + py::dynamic_attr()) + .def(py::init<>()) + .def("reset", + &Class::Reset, + "Forget all previous data.") + .def("count", + &Class::Count, + "Get number of data points in measurement.") + .def("value", + &Class::Value, + "Get a short version of the name of this statistical measure.") + .def("short_name", + &Class::ShortName, + "Get a short version of the name of this statistical measure.") + .def("insert_data", + &Class::InsertData, + "Add a new sample to the statistical measure."); +} + +////////////////////////////////////////////////// +class SignalMaximumTrampoline : public ignition::math::SignalMaximum +{ +public: + // Inherit the constructors + SignalMaximumTrampoline(){} + + // Trampoline (need one for each virtual function) + double Value() const override + { + PYBIND11_OVERLOAD_PURE( + double, // Return type (ret_type) + ignition::math::SignalStatistic, // Parent class (cname) + // Name of function in C++ (must match Python name) (fn) + Value, + ); + } + // Trampoline (need one for each virtual function) + std::string ShortName() const override + { + PYBIND11_OVERLOAD_PURE( + std::string, // Return type (ret_type) + ignition::math::SignalStatistic, // Parent class (cname) + // Name of function in C++ (must match Python name) (fn) + ShortName, + ); + } + // Trampoline (need one for each virtual function) + void InsertData(const double _data) override + { + PYBIND11_OVERLOAD_PURE( + void, // Return type (ret_type) + ignition::math::SignalStatistic, // Parent class (cname) + // Name of function in C++ (must match Python name) (fn) + InsertData, + _data // Argument(s) (...) + ); + } +}; + +////////////////////////////////////////////////// +void defineMathSignalMaximum(py::module &m, const std::string &typestr) +{ + using Class = ignition::math::SignalMaximum; + std::string pyclass_name = typestr; + py::class_(m, + pyclass_name.c_str(), + py::buffer_protocol(), + py::dynamic_attr()) + .def(py::init<>()) + .def("reset", + &Class::Reset, + "Forget all previous data.") + .def("count", + &Class::Count, + "Get number of data points in measurement.") + .def("value", + &Class::Value, + "Get a short version of the name of this statistical measure.") + .def("short_name", + &Class::ShortName, + "Get a short version of the name of this statistical measure.") + .def("insert_data", + &Class::InsertData, + "Add a new sample to the statistical measure."); +} + +////////////////////////////////////////////////// +class SignalMinimumTrampoline : public ignition::math::SignalMinimum +{ +public: + // Inherit the constructors + SignalMinimumTrampoline(){} + + // Trampoline (need one for each virtual function) + double Value() const override + { + PYBIND11_OVERLOAD_PURE( + double, // Return type (ret_type) + ignition::math::SignalStatistic, // Parent class (cname) + // Name of function in C++ (must match Python name) (fn) + Value, + ); + } + // Trampoline (need one for each virtual function) + std::string ShortName() const override + { + PYBIND11_OVERLOAD_PURE( + std::string, // Return type (ret_type) + ignition::math::SignalStatistic, // Parent class (cname) + // Name of function in C++ (must match Python name) (fn) + ShortName, + ); + } + // Trampoline (need one for each virtual function) + void InsertData(const double _data) override + { + PYBIND11_OVERLOAD_PURE( + void, // Return type (ret_type) + ignition::math::SignalStatistic, // Parent class (cname) + // Name of function in C++ (must match Python name) (fn) + InsertData, + _data // Argument(s) (...) + ); + } +}; + +////////////////////////////////////////////////// +void defineMathSignalMinimum(py::module &m, const std::string &typestr) +{ + using Class = ignition::math::SignalMinimum; + std::string pyclass_name = typestr; + py::class_(m, + pyclass_name.c_str(), + py::buffer_protocol(), + py::dynamic_attr()) + .def(py::init<>()) + .def("reset", + &Class::Reset, + "Forget all previous data.") + .def("count", + &Class::Count, + "Get number of data points in measurement.") + .def("value", + &Class::Value, + "Get a short version of the name of this statistical measure.") + .def("short_name", + &Class::ShortName, + "Get a short version of the name of this statistical measure.") + .def("insert_data", + &Class::InsertData, + "Add a new sample to the statistical measure."); +} + +////////////////////////////////////////////////// +class SignalMeanTrampoline : public ignition::math::SignalMean +{ +public: + // Inherit the constructors + SignalMeanTrampoline(){} + + // Trampoline (need one for each virtual function) + double Value() const override + { + PYBIND11_OVERLOAD_PURE( + double, // Return type (ret_type) + ignition::math::SignalStatistic, // Parent class (cname) + // Name of function in C++ (must match Python name) (fn) + Value, + ); + } + // Trampoline (need one for each virtual function) + std::string ShortName() const override + { + PYBIND11_OVERLOAD_PURE( + std::string, // Return type (ret_type) + ignition::math::SignalStatistic, // Parent class (cname) + // Name of function in C++ (must match Python name) (fn) + ShortName, + ); + } + // Trampoline (need one for each virtual function) + void InsertData(const double _data) override + { + PYBIND11_OVERLOAD_PURE( + void, // Return type (ret_type) + ignition::math::SignalStatistic, // Parent class (cname) + // Name of function in C++ (must match Python name) (fn) + InsertData, + _data // Argument(s) (...) + ); + } +}; + +////////////////////////////////////////////////// +void defineMathSignalMean(py::module &m, const std::string &typestr) +{ + using Class = ignition::math::SignalMean; + std::string pyclass_name = typestr; + py::class_(m, + pyclass_name.c_str(), + py::buffer_protocol(), + py::dynamic_attr()) + .def(py::init<>()) + .def("reset", + &Class::Reset, + "Forget all previous data.") + .def("count", + &Class::Count, + "Get number of data points in measurement.") + .def("value", + &Class::Value, + "Get a short version of the name of this statistical measure.") + .def("short_name", + &Class::ShortName, + "Get a short version of the name of this statistical measure.") + .def("insert_data", + &Class::InsertData, + "Add a new sample to the statistical measure."); +} + +////////////////////////////////////////////////// +class SignalRootMeanSquareTrampoline : public ignition::math::SignalRootMeanSquare +{ +public: + // Inherit the constructors + SignalRootMeanSquareTrampoline(){} + + // Trampoline (need one for each virtual function) + double Value() const override + { + PYBIND11_OVERLOAD_PURE( + double, // Return type (ret_type) + ignition::math::SignalStatistic, // Parent class (cname) + // Name of function in C++ (must match Python name) (fn) + Value, + ); + } + // Trampoline (need one for each virtual function) + std::string ShortName() const override + { + PYBIND11_OVERLOAD_PURE( + std::string, // Return type (ret_type) + ignition::math::SignalStatistic, // Parent class (cname) + // Name of function in C++ (must match Python name) (fn) + ShortName + ); + } + // Trampoline (need one for each virtual function) + void InsertData(const double _data) override + { + PYBIND11_OVERLOAD_PURE( + void, // Return type (ret_type) + ignition::math::SignalStatistic, // Parent class (cname) + // Name of function in C++ (must match Python name) (fn) + InsertData, + _data // Argument(s) (...) + ); + } +}; + +////////////////////////////////////////////////// +void defineMathSignalRootMeanSquare(py::module &m, const std::string &typestr) +{ + using Class = ignition::math::SignalRootMeanSquare; + std::string pyclass_name = typestr; + py::class_(m, + pyclass_name.c_str(), + py::buffer_protocol(), + py::dynamic_attr()) + .def(py::init<>()) + .def("reset", + &Class::Reset, + "Forget all previous data.") + .def("count", + &Class::Count, + "Get number of data points in measurement.") + .def("value", + &Class::Value, + "Get a short version of the name of this statistical measure.") + .def("short_name", + &Class::ShortName, + "Get a short version of the name of this statistical measure.") + .def("insert_data", + &Class::InsertData, + "Add a new sample to the statistical measure."); +} + +////////////////////////////////////////////////// +class SignalMaxAbsoluteValueTrampoline : public ignition::math::SignalMaxAbsoluteValue +{ +public: + // Inherit the constructors + SignalMaxAbsoluteValueTrampoline(){} + + // Trampoline (need one for each virtual function) + double Value() const override + { + PYBIND11_OVERLOAD_PURE( + double, // Return type (ret_type) + ignition::math::SignalStatistic, // Parent class (cname) + // Name of function in C++ (must match Python name) (fn) + Value, + ); + } + // Trampoline (need one for each virtual function) + std::string ShortName() const override + { + PYBIND11_OVERLOAD_PURE( + std::string, // Return type (ret_type) + ignition::math::SignalStatistic, // Parent class (cname) + // Name of function in C++ (must match Python name) (fn) + ShortName, + ); + } + // Trampoline (need one for each virtual function) + void InsertData(const double _data) override + { + PYBIND11_OVERLOAD_PURE( + void, // Return type (ret_type) + ignition::math::SignalStatistic, // Parent class (cname) + // Name of function in C++ (must match Python name) (fn) + InsertData, + _data // Argument(s) (...) + ); + } +}; + +////////////////////////////////////////////////// +void defineMathSignalMaxAbsoluteValue(py::module &m, const std::string &typestr) +{ + using Class = ignition::math::SignalMaxAbsoluteValue; + std::string pyclass_name = typestr; + py::class_(m, + pyclass_name.c_str(), + py::buffer_protocol(), + py::dynamic_attr()) + .def(py::init<>()) + .def("reset", + &Class::Reset, + "Forget all previous data.") + .def("count", + &Class::Count, + "Get number of data points in measurement.") + .def("value", + &Class::Value, + "Get a short version of the name of this statistical measure.") + .def("short_name", + &Class::ShortName, + "Get a short version of the name of this statistical measure.") + .def("insert_data", + &Class::InsertData, + "Add a new sample to the statistical measure."); +} +} // namespace python +} // namespace math +} // namespace ignition diff --git a/src/python_pybind11/src/SignalStats.hh b/src/python_pybind11/src/SignalStats.hh new file mode 100644 index 000000000..a5a5eae45 --- /dev/null +++ b/src/python_pybind11/src/SignalStats.hh @@ -0,0 +1,91 @@ +/* + * 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__SIGNALSTATS_HH_ +#define IGNITION_MATH_PYTHON__SIGNALSTATS_HH_ + +#include +#include + +namespace py = pybind11; + +namespace ignition +{ +namespace math +{ +namespace python +{ +/// Define a pybind11 wrapper for an ignition::math::SignalStats +/** + * \param[in] module a pybind11 module to add the definition to + * \param[in] typestr name of the type used by Python + */ +void defineMathSignalStats(py::module &m, const std::string &typestr); + +/// Define a pybind11 wrapper for an ignition::math::SignalStatistic +/** + * \param[in] module a pybind11 module to add the definition to + * \param[in] typestr name of the type used by Python + */ +void defineMathSignalStatistic(py::module &m, const std::string &typestr); + +/// Define a pybind11 wrapper for an ignition::math::SignalMaximum +/** + * \param[in] module a pybind11 module to add the definition to + * \param[in] typestr name of the type used by Python + */ +void defineMathSignalMaximum(py::module &m, const std::string &typestr); + +/// Define a pybind11 wrapper for an ignition::math::SignalMinimum +/** + * \param[in] module a pybind11 module to add the definition to + * \param[in] typestr name of the type used by Python + */ +void defineMathSignalMinimum(py::module &m, const std::string &typestr); + +/// Define a pybind11 wrapper for an ignition::math::SignalVariance +/** + * \param[in] module a pybind11 module to add the definition to + * \param[in] typestr name of the type used by Python + */ +void defineMathSignalVariance(py::module &m, const std::string &typestr); + +/// Define a pybind11 wrapper for an ignition::math::SignalMaxAbsoluteValue +/** + * \param[in] module a pybind11 module to add the definition to + * \param[in] typestr name of the type used by Python + */ +void defineMathSignalMaxAbsoluteValue(py::module &m, const std::string &typestr); + +/// Define a pybind11 wrapper for an ignition::math::SignalRootMeanSquare +/** + * \param[in] module a pybind11 module to add the definition to + * \param[in] typestr name of the type used by Python + */ +void defineMathSignalRootMeanSquare(py::module &m, const std::string &typestr); + +/// Define a pybind11 wrapper for an ignition::math::SignalMean +/** + * \param[in] module a pybind11 module to add the definition to + * \param[in] typestr name of the type used by Python + */ +void defineMathSignalMean(py::module &m, const std::string &typestr); +} // namespace python +} // namespace math +} // namespace ignition + +#endif // IGNITION_MATH_PYTHON__SIGNALSTATS_HH_ diff --git a/src/python_pybind11/src/_ignition_math_pybind11.cc b/src/python_pybind11/src/_ignition_math_pybind11.cc index cca30a6fe..f8b043c49 100644 --- a/src/python_pybind11/src/_ignition_math_pybind11.cc +++ b/src/python_pybind11/src/_ignition_math_pybind11.cc @@ -23,6 +23,7 @@ #include "Quaternion.hh" #include "Rand.hh" #include "RollingMean.hh" +#include "SignalStats.hh" #include "StopWatch.hh" #include "Vector2.hh" #include "Vector3.hh" @@ -51,6 +52,17 @@ PYBIND11_MODULE(math, m) ignition::math::python::defineMathRollingMean(m, "RollingMean"); + ignition::math::python::defineMathSignalStats(m, "SignalStats"); + ignition::math::python::defineMathSignalStatistic(m, "SignalStatistic"); + ignition::math::python::defineMathSignalVariance(m, "SignalVariance"); + ignition::math::python::defineMathSignalMaximum(m, "SignalMaximum"); + ignition::math::python::defineMathSignalMinimum(m, "SignalMinimum"); + ignition::math::python::defineMathSignalMaxAbsoluteValue( + m, "SignalMaxAbsoluteValue"); + ignition::math::python::defineMathSignalRootMeanSquare( + m, "SignalRootMeanSquare"); + ignition::math::python::defineMathSignalMean(m, "SignalMean"); + ignition::math::python::defineMathStopwatch(m, "Stopwatch"); ignition::math::python::defineMathVector2(m, "Vector2d"); diff --git a/src/python/SignalStats_TEST.py b/src/python_pybind11/test/SignalStats_TEST.py similarity index 100% rename from src/python/SignalStats_TEST.py rename to src/python_pybind11/test/SignalStats_TEST.py From 9c9996b625748f7414505c063bbbf18f194d9433 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Alejandro=20Herna=CC=81ndez?= Date: Mon, 27 Dec 2021 20:13:18 +0100 Subject: [PATCH 2/8] Fixed tests MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Alejandro Hernández --- src/python_pybind11/test/SignalStats_TEST.py | 97 ++++++++++---------- 1 file changed, 49 insertions(+), 48 deletions(-) diff --git a/src/python_pybind11/test/SignalStats_TEST.py b/src/python_pybind11/test/SignalStats_TEST.py index e8497e6d2..8d3c89221 100644 --- a/src/python_pybind11/test/SignalStats_TEST.py +++ b/src/python_pybind11/test/SignalStats_TEST.py @@ -372,7 +372,7 @@ def test_signal_variance_random_values(self): def test_signal_stats_constructor(self): # Constructor stats = SignalStats() - self.assertTrue(stats.map().empty()) + self.assertTrue(len(stats.map()) == 0) self.assertEqual(stats.count(), 0) stats2 = SignalStats(stats) @@ -380,110 +380,111 @@ def test_signal_stats_constructor(self): # reset stats.reset() - self.assertTrue(stats.map().empty()) + self.assertTrue(len(stats.map()) == 0) self.assertEqual(stats.count(), 0) def test_01_signal_stats_intern_statistic(self): # insert static stats = SignalStats() - self.assertTrue(stats.map().empty()) + self.assertTrue(len(stats.map()) == 0) self.assertTrue(stats.insert_statistic("max")) self.assertFalse(stats.insert_statistic("max")) - self.assertFalse(stats.map().empty()) + self.assertFalse(len(stats.map()) == 0) self.assertTrue(stats.insert_statistic("maxAbs")) self.assertFalse(stats.insert_statistic("maxAbs")) - self.assertFalse(stats.map().empty()) + self.assertFalse(len(stats.map()) == 0) self.assertTrue(stats.insert_statistic("mean")) self.assertFalse(stats.insert_statistic("mean")) - self.assertFalse(stats.map().empty()) + self.assertFalse(len(stats.map()) == 0) self.assertTrue(stats.insert_statistic("min")) self.assertFalse(stats.insert_statistic("min")) - self.assertFalse(stats.map().empty()) + self.assertFalse(len(stats.map()) == 0) self.assertTrue(stats.insert_statistic("rms")) self.assertFalse(stats.insert_statistic("rms")) - self.assertFalse(stats.map().empty()) + self.assertFalse(len(stats.map()) == 0) self.assertTrue(stats.insert_statistic("var")) self.assertFalse(stats.insert_statistic("var")) - self.assertFalse(stats.map().empty()) + self.assertFalse(len(stats.map()) == 0) self.assertFalse(stats.insert_statistic("FakeStatistic")) # map with no data map = stats.map() - self.assertFalse(map.empty()) - self.assertEqual(map.size(), 6) - self.assertEqual(map.count("max"), 1) - self.assertEqual(map.count("maxAbs"), 1) - self.assertEqual(map.count("mean"), 1) - self.assertEqual(map.count("min"), 1) - self.assertEqual(map.count("rms"), 1) - self.assertEqual(map.count("var"), 1) - self.assertEqual(map.count("FakeStatistic"), 0) + self.assertFalse(len(map) == 0) + self.assertEqual(len(map), 6) + print(map) + self.assertEqual("max" in map.keys(), 1) + self.assertEqual("maxAbs" in map.keys(), 1) + self.assertEqual("mean" in map.keys(), 1) + self.assertEqual("min" in map.keys(), 1) + self.assertEqual("rms" in map.keys(), 1) + self.assertEqual("var" in map.keys(), 1) + self.assertEqual("FakeStatistic" in map.keys(), 0) stats2 = SignalStats(stats) map2 = stats2.map() - self.assertFalse(map2.empty()) - self.assertEqual(map.size(), map2.size()) - self.assertEqual(map.count("max"), map2.count("max")) - self.assertEqual(map.count("maxAbs"), map2.count("maxAbs")) - self.assertEqual(map.count("mean"), map2.count("mean")) - self.assertEqual(map.count("min"), map2.count("min")) - self.assertEqual(map.count("rms"), map2.count("rms")) - self.assertEqual(map.count("var"), map2.count("var")) - self.assertEqual(map.count("FakeStatistic"), - map2.count("FakeStatistic")) + self.assertFalse(len(map2) == 0) + self.assertEqual(len(map), len(map2)) + self.assertEqual("max" in map.keys(), "max" in map2.keys()) + self.assertEqual("maxAbs" in map.keys(), "maxAbs" in map2.keys()) + self.assertEqual("mean" in map.keys(), "mean" in map2.keys()) + self.assertEqual("min" in map.keys(), "min" in map2.keys()) + self.assertEqual("rms" in map.keys(), "rms" in map2.keys()) + self.assertEqual("var" in map.keys(), "var" in map2.keys()) + self.assertEqual("FakeStatistic" in map.keys(), + "FakeStatistic" in map2.keys()) def test_02_signal_stats_intern_statistic(self): # insert statics stats = SignalStats() self.assertFalse(stats.insert_statistics("")) - self.assertTrue(stats.map().empty()) + self.assertTrue(len(stats.map()) == 0) self.assertTrue(stats.insert_statistics("maxAbs,rms")) - self.assertEqual(stats.map().size(), 2) + self.assertEqual(len(stats.map()), 2) self.assertFalse(stats.insert_statistics("maxAbs,rms")) self.assertFalse(stats.insert_statistics("maxAbs")) self.assertFalse(stats.insert_statistics("rms")) - self.assertEqual(stats.map().size(), 2) + self.assertEqual(len(stats.map()), 2) self.assertFalse(stats.insert_statistics("mean,FakeStatistic")) - self.assertEqual(stats.map().size(), 3) + self.assertEqual(len(stats.map()), 3) self.assertFalse(stats.insert_statistics("var,FakeStatistic")) - self.assertEqual(stats.map().size(), 4) + self.assertEqual(len(stats.map()), 4) self.assertFalse(stats.insert_statistics("max,FakeStatistic")) - self.assertEqual(stats.map().size(), 5) + self.assertEqual(len(stats.map()), 5) self.assertFalse(stats.insert_statistics("min,FakeStatistic")) - self.assertEqual(stats.map().size(), 6) + self.assertEqual(len(stats.map()), 6) self.assertFalse(stats.insert_statistics("FakeStatistic")) - self.assertEqual(stats.map().size(), 6) + self.assertEqual(len(stats.map()), 6) # map with no data map = stats.map() - self.assertFalse(map.empty()) - self.assertEqual(map.size(), 6) - self.assertEqual(map.count("max"), 1) - self.assertEqual(map.count("maxAbs"), 1) - self.assertEqual(map.count("mean"), 1) - self.assertEqual(map.count("min"), 1) - self.assertEqual(map.count("rms"), 1) - self.assertEqual(map.count("var"), 1) - self.assertEqual(map.count("FakeStatistic"), 0) + self.assertFalse(len(map) == 0) + self.assertEqual(len(map), 6) + self.assertEqual("max" in map.keys(), 1) + self.assertEqual("maxAbs" in map.keys(), 1) + self.assertEqual("mean" in map.keys(), 1) + self.assertEqual("min" in map.keys(), 1) + self.assertEqual("rms" in map.keys(), 1) + self.assertEqual("var" in map.keys(), 1) + self.assertEqual("FakeStatistic" in map.keys(), 0) def test_signal_stats_alternating_values(self): # Add some statistics stats = SignalStats() self.assertTrue(stats.insert_statistics("max,maxAbs,mean,min,rms")) - self.assertEqual(stats.map().size(), 5) + self.assertEqual(len(stats.map()), 5) # No data yet self.assertEqual(stats.count(), 0) @@ -505,7 +506,7 @@ def test_signal_stats_alternating_values(self): copy = SignalStats(stats) self.assertEqual(copy.count(), 2) map = stats.map() - self.assertEqual(map.size(), 5) + self.assertEqual(len(map), 5) self.assertAlmostEqual(map["max"], value) self.assertAlmostEqual(map["maxAbs"], value) self.assertAlmostEqual(map["min"], -value) @@ -513,7 +514,7 @@ def test_signal_stats_alternating_values(self): self.assertAlmostEqual(map["mean"], 0.0) stats.reset() - self.assertEqual(stats.map().size(), 5) + self.assertEqual(len(stats.map()), 5) self.assertEqual(stats.count(), 0) map = stats.map() self.assertAlmostEqual(map["max"], 0.0) From eb0b9fcb555cc176ce56ccbc8e5b9d54a3a82c0e Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Alejandro=20Herna=CC=81ndez?= Date: Mon, 27 Dec 2021 20:28:39 +0100 Subject: [PATCH 3/8] make linters happy MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Alejandro Hernández --- src/python_pybind11/src/SignalStats.cc | 12 +++++++----- src/python_pybind11/src/SignalStats.hh | 3 ++- 2 files changed, 9 insertions(+), 6 deletions(-) diff --git a/src/python_pybind11/src/SignalStats.cc b/src/python_pybind11/src/SignalStats.cc index 61ed54613..2688ff7e0 100644 --- a/src/python_pybind11/src/SignalStats.cc +++ b/src/python_pybind11/src/SignalStats.cc @@ -16,6 +16,7 @@ */ #include +#include #include #include "SignalStats.hh" @@ -59,15 +60,15 @@ void defineMathSignalStats(py::module &m, const std::string &typestr) class SignalStatisticTrampoline : public ignition::math::SignalStatistic { public: - SignalStatisticTrampoline() : SignalStatistic() {}; - SignalStatisticTrampoline(const SignalStatistic &_ss) : SignalStatistic(_ss) {}; + SignalStatisticTrampoline() : SignalStatistic() {} + SignalStatisticTrampoline(const SignalStatistic &_ss) : SignalStatistic(_ss) {} // Trampoline (need one for each virtual function) double Value() const override { PYBIND11_OVERLOAD_PURE( - double, // Return type (ret_type) - ignition::math::SignalStatistic, // Parent class (cname) + double, // Return type (ret_type) + ignition::math::SignalStatistic, // Parent class (cname) // Name of function in C++ (must match Python name) (fn) Value, ); @@ -411,7 +412,8 @@ void defineMathSignalMean(py::module &m, const std::string &typestr) } ////////////////////////////////////////////////// -class SignalRootMeanSquareTrampoline : public ignition::math::SignalRootMeanSquare +class SignalRootMeanSquareTrampoline : + public ignition::math::SignalRootMeanSquare { public: // Inherit the constructors diff --git a/src/python_pybind11/src/SignalStats.hh b/src/python_pybind11/src/SignalStats.hh index a5a5eae45..7e92e1940 100644 --- a/src/python_pybind11/src/SignalStats.hh +++ b/src/python_pybind11/src/SignalStats.hh @@ -69,7 +69,8 @@ void defineMathSignalVariance(py::module &m, const std::string &typestr); * \param[in] module a pybind11 module to add the definition to * \param[in] typestr name of the type used by Python */ -void defineMathSignalMaxAbsoluteValue(py::module &m, const std::string &typestr); +void defineMathSignalMaxAbsoluteValue( + py::module &m, const std::string &typestr); /// Define a pybind11 wrapper for an ignition::math::SignalRootMeanSquare /** From 8070b054f757f9bc31cdcee00a13c0006b20e87d Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Alejandro=20Herna=CC=81ndez?= Date: Tue, 28 Dec 2021 00:18:35 +0100 Subject: [PATCH 4/8] make linters happy MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Alejandro Hernández --- src/python_pybind11/src/SignalStats.cc | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/src/python_pybind11/src/SignalStats.cc b/src/python_pybind11/src/SignalStats.cc index 2688ff7e0..f2fa1fc6d 100644 --- a/src/python_pybind11/src/SignalStats.cc +++ b/src/python_pybind11/src/SignalStats.cc @@ -61,7 +61,8 @@ class SignalStatisticTrampoline : public ignition::math::SignalStatistic { public: SignalStatisticTrampoline() : SignalStatistic() {} - SignalStatisticTrampoline(const SignalStatistic &_ss) : SignalStatistic(_ss) {} + explicit SignalStatisticTrampoline(const SignalStatistic &_ss) : + SignalStatistic(_ss) {} // Trampoline (need one for each virtual function) double Value() const override @@ -480,7 +481,8 @@ void defineMathSignalRootMeanSquare(py::module &m, const std::string &typestr) } ////////////////////////////////////////////////// -class SignalMaxAbsoluteValueTrampoline : public ignition::math::SignalMaxAbsoluteValue +class SignalMaxAbsoluteValueTrampoline : + public ignition::math::SignalMaxAbsoluteValue { public: // Inherit the constructors From a33967fd16004801eb024e098d63ea786d32ac5a Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Alejandro=20Herna=CC=81ndez?= Date: Wed, 29 Dec 2021 16:27:13 +0100 Subject: [PATCH 5/8] Added Vector3Stats pybind11 interface MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Alejandro Hernández --- src/python_pybind11/CMakeLists.txt | 2 + src/python_pybind11/src/Vector3Stats.cc | 78 +++++++++++++++++++ src/python_pybind11/src/Vector3Stats.hh | 43 ++++++++++ .../src/_ignition_math_pybind11.cc | 3 + .../test}/Vector3Stats_TEST.py | 57 +++++++------- 5 files changed, 155 insertions(+), 28 deletions(-) create mode 100644 src/python_pybind11/src/Vector3Stats.cc create mode 100644 src/python_pybind11/src/Vector3Stats.hh rename src/{python => python_pybind11/test}/Vector3Stats_TEST.py (73%) diff --git a/src/python_pybind11/CMakeLists.txt b/src/python_pybind11/CMakeLists.txt index d5629e76e..243fc93f0 100644 --- a/src/python_pybind11/CMakeLists.txt +++ b/src/python_pybind11/CMakeLists.txt @@ -31,6 +31,7 @@ if (${pybind11_FOUND}) src/Spline.cc src/StopWatch.cc src/Temperature.cc + src/Vector3Stats.cc ) target_link_libraries(math PRIVATE @@ -113,6 +114,7 @@ if (${pybind11_FOUND}) Triangle3_TEST Vector2_TEST Vector3_TEST + Vector3Stats_TEST Vector4_TEST ) diff --git a/src/python_pybind11/src/Vector3Stats.cc b/src/python_pybind11/src/Vector3Stats.cc new file mode 100644 index 000000000..047f70508 --- /dev/null +++ b/src/python_pybind11/src/Vector3Stats.cc @@ -0,0 +1,78 @@ +/* + * 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 + +#include "Vector3Stats.hh" + +using namespace pybind11::literals; + +namespace ignition +{ +namespace math +{ +namespace python +{ +void defineMathVector3Stats(py::module &m, const std::string &typestr) +{ + using Class = ignition::math::Vector3Stats; + std::string pyclass_name = typestr; + py::class_(m, + pyclass_name.c_str(), + py::buffer_protocol(), + py::dynamic_attr()) + .def(py::init<>()) + .def("insert_data", + &Class::InsertData, + "Add a new sample to the statistical measures") + .def("insert_statistic", + &Class::InsertStatistic, + "Add a new type of statistic.") + .def("insert_statistics", + &Class::InsertStatistics, + "Set the size of the box.") + .def("reset", + &Class::Reset, + "Forget all previous data.") + .def("x", + py::overload_cast<>(&Class::X), + py::return_value_policy::reference, + "Get statistics for x component of signal.") + .def("y", + py::overload_cast<>(&Class::Y), + py::return_value_policy::reference, + "Get statistics for y component of signal.") + .def("z", + py::overload_cast<>(&Class::Z), + py::return_value_policy::reference, + "Get statistics for z component of signal.") + .def("mag", + py::overload_cast<>(&Class::Mag), + py::return_value_policy::reference, + "Get statistics for mag component of signal.") + .def("__copy__", [](const Class &self) { + return Class(self); + }) + .def("__deepcopy__", [](const Class &self, py::dict) { + return Class(self); + }, "memo"_a); +} +} // namespace python +} // namespace math +} // namespace ignition diff --git a/src/python_pybind11/src/Vector3Stats.hh b/src/python_pybind11/src/Vector3Stats.hh new file mode 100644 index 000000000..bd1848ac3 --- /dev/null +++ b/src/python_pybind11/src/Vector3Stats.hh @@ -0,0 +1,43 @@ +/* + * 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__VECTOR3STATS_HH_ +#define IGNITION_MATH_PYTHON__VECTOR3STATS_HH_ + +#include + +#include + +namespace py = pybind11; + +namespace ignition +{ +namespace math +{ +namespace python +{ +/// Define a pybind11 wrapper for an ignition::math::Vector3Stats +/** + * \param[in] module a pybind11 module to add the definition to + * \param[in] typestr name of the type used by Python + */ +void defineMathVector3Stats(py::module &m, const std::string &typestr); +} // namespace python +} // namespace math +} // namespace ignition + +#endif // IGNITION_MATH_PYTHON__VECTOR3STATS_HH_ diff --git a/src/python_pybind11/src/_ignition_math_pybind11.cc b/src/python_pybind11/src/_ignition_math_pybind11.cc index b7ba194c0..e058b8455 100644 --- a/src/python_pybind11/src/_ignition_math_pybind11.cc +++ b/src/python_pybind11/src/_ignition_math_pybind11.cc @@ -44,6 +44,7 @@ #include "Triangle3.hh" #include "Vector2.hh" #include "Vector3.hh" +#include "Vector3Stats.hh" #include "Vector4.hh" namespace py = pybind11; @@ -96,6 +97,8 @@ PYBIND11_MODULE(math, m) ignition::math::python::defineMathRotationSpline(m, "RotationSpline"); + ignition::math::python::defineMathVector3Stats(m, "Vector3Stats"); + ignition::math::python::defineMathSemanticVersion(m, "SemanticVersion"); ignition::math::python::defineMathSpline(m, "Spline"); diff --git a/src/python/Vector3Stats_TEST.py b/src/python_pybind11/test/Vector3Stats_TEST.py similarity index 73% rename from src/python/Vector3Stats_TEST.py rename to src/python_pybind11/test/Vector3Stats_TEST.py index 293c8bda6..d9e04f9d8 100644 --- a/src/python/Vector3Stats_TEST.py +++ b/src/python_pybind11/test/Vector3Stats_TEST.py @@ -36,10 +36,10 @@ def mag(self, _name): def test_vector3stats_constructor(self): # Constructor v3stats = Vector3Stats() - self.assertTrue(v3stats.x().map().empty()) - self.assertTrue(v3stats.y().map().empty()) - self.assertTrue(v3stats.z().map().empty()) - self.assertTrue(v3stats.mag().map().empty()) + self.assertTrue(len(v3stats.x().map()) == 0) + self.assertTrue(len(v3stats.y().map()) == 0) + self.assertTrue(len(v3stats.z().map()) == 0) + self.assertTrue(len(v3stats.mag().map()) == 0) self.assertAlmostEqual(v3stats.x().count(), 0) self.assertAlmostEqual(v3stats.y().count(), 0) self.assertAlmostEqual(v3stats.z().count(), 0) @@ -47,10 +47,10 @@ def test_vector3stats_constructor(self): # Reset v3stats.reset() - self.assertTrue(v3stats.x().map().empty()) - self.assertTrue(v3stats.y().map().empty()) - self.assertTrue(v3stats.z().map().empty()) - self.assertTrue(v3stats.mag().map().empty()) + self.assertTrue(len(v3stats.x().map()) == 0) + self.assertTrue(len(v3stats.y().map()) == 0) + self.assertTrue(len(v3stats.z().map()) == 0) + self.assertTrue(len(v3stats.mag().map()) == 0) self.assertAlmostEqual(v3stats.x().count(), 0) self.assertAlmostEqual(v3stats.y().count(), 0) self.assertAlmostEqual(v3stats.z().count(), 0) @@ -58,35 +58,36 @@ def test_vector3stats_constructor(self): # InsertStatistics v3stats = Vector3Stats() - self.assertTrue(v3stats.x().map().empty()) - self.assertTrue(v3stats.y().map().empty()) - self.assertTrue(v3stats.z().map().empty()) - self.assertTrue(v3stats.mag().map().empty()) + self.assertTrue(len(v3stats.x().map()) == 0) + self.assertTrue(len(v3stats.y().map()) == 0) + self.assertTrue(len(v3stats.z().map()) == 0) + self.assertTrue(len(v3stats.mag().map()) == 0) self.assertTrue(v3stats.insert_statistics("maxAbs")) self.assertFalse(v3stats.insert_statistics("maxAbs")) self.assertFalse(v3stats.insert_statistic("maxAbs")) - self.assertFalse(v3stats.x().map().empty()) - self.assertFalse(v3stats.y().map().empty()) - self.assertFalse(v3stats.z().map().empty()) - self.assertFalse(v3stats.mag().map().empty()) + self.assertFalse(len(v3stats.x().map()) == 0) + self.assertFalse(len(v3stats.y().map()) == 0) + self.assertFalse(len(v3stats.z().map()) == 0) + self.assertFalse(len(v3stats.mag().map()) == 0) # Map with no data map = v3stats.x().map() - self.assertAlmostEqual(map.size(), 1) - self.assertAlmostEqual(map.count("maxAbs"), 1) + + self.assertAlmostEqual(len(map), 1) + self.assertTrue("maxAbs" in map, 1) map = v3stats.y().map() - self.assertAlmostEqual(map.size(), 1) - self.assertAlmostEqual(map.count("maxAbs"), 1) + self.assertAlmostEqual(len(map), 1) + self.assertTrue("maxAbs" in map, 1) map = v3stats.z().map() - self.assertAlmostEqual(map.size(), 1) - self.assertAlmostEqual(map.count("maxAbs"), 1) + self.assertAlmostEqual(len(map), 1) + self.assertTrue("maxAbs" in map, 1) map = v3stats.mag().map() - self.assertAlmostEqual(map.size(), 1) - self.assertAlmostEqual(map.count("maxAbs"), 1) + self.assertAlmostEqual(len(map), 1) + self.assertTrue("maxAbs" in map, 1) # Insert some data self.assertAlmostEqual(v3stats.x().count(), 0) @@ -110,10 +111,10 @@ def test_vector3stats_constructor(self): def test_vector3stats_const_accessor(self): # Const accessors - self.assertTrue(self.stats.x().map().empty()) - self.assertTrue(self.stats.y().map().empty()) - self.assertTrue(self.stats.z().map().empty()) - self.assertTrue(self.stats.mag().map().empty()) + self.assertTrue(len(self.stats.x().map()) == 0) + self.assertTrue(len(self.stats.y().map()) == 0) + self.assertTrue(len(self.stats.z().map()) == 0) + self.assertTrue(len(self.stats.mag().map()) == 0) name = "maxAbs" self.assertTrue(self.stats.insert_statistics(name)) From b8e8b45361d7f990b67948c59462e43b906f3abe Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Alejandro=20Herna=CC=81ndez?= Date: Wed, 29 Dec 2021 17:11:57 +0100 Subject: [PATCH 6/8] Remove ununsed tests 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 - 1 file changed, 1 deletion(-) diff --git a/src/python/CMakeLists.txt b/src/python/CMakeLists.txt index ed44210f9..2de6e7930 100644 --- a/src/python/CMakeLists.txt +++ b/src/python/CMakeLists.txt @@ -96,7 +96,6 @@ if (PYTHONLIBS_FOUND) python_TEST Sphere_TEST SphericalCoordinates_TEST - Vector3Stats_TEST ) foreach (test ${python_tests}) From c94b63a0a3346e228bd7520f49b1dff850c9bd50 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Alejandro=20Herna=CC=81ndez?= Date: Thu, 30 Dec 2021 14:01:58 +0100 Subject: [PATCH 7/8] Fixed test MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Alejandro Hernández --- src/python_pybind11/src/SignalStats.cc | 3 +-- src/python_pybind11/test/SignalStats_TEST.py | 2 +- 2 files changed, 2 insertions(+), 3 deletions(-) diff --git a/src/python_pybind11/src/SignalStats.cc b/src/python_pybind11/src/SignalStats.cc index f2fa1fc6d..69b31e661 100644 --- a/src/python_pybind11/src/SignalStats.cc +++ b/src/python_pybind11/src/SignalStats.cc @@ -14,7 +14,7 @@ * limitations under the License. * */ -#include +#include #include #include @@ -53,7 +53,6 @@ void defineMathSignalStats(py::module &m, const std::string &typestr) .def("insert_statistics", &Class::InsertStatistics, "Add multiple statistics."); - py::bind_map>(m, "SignalMap"); } ////////////////////////////////////////////////// diff --git a/src/python_pybind11/test/SignalStats_TEST.py b/src/python_pybind11/test/SignalStats_TEST.py index 8d3c89221..26e4e4974 100644 --- a/src/python_pybind11/test/SignalStats_TEST.py +++ b/src/python_pybind11/test/SignalStats_TEST.py @@ -418,7 +418,7 @@ def test_01_signal_stats_intern_statistic(self): map = stats.map() self.assertFalse(len(map) == 0) self.assertEqual(len(map), 6) - print(map) + self.assertEqual("max" in map.keys(), 1) self.assertEqual("maxAbs" in map.keys(), 1) self.assertEqual("mean" in map.keys(), 1) From 2c6abfe8800b682199c2b1a1bfcad57a0a8f5037 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Alejandro=20Herna=CC=81ndez?= Date: Thu, 30 Dec 2021 14:07:39 +0100 Subject: [PATCH 8/8] Removed SWIG python interfaces MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Alejandro Hernández --- src/CMakeLists.txt | 1 - src/python/Angle.i | 59 -- src/python/Box_TEST.py | 405 ------------- src/python/CMakeLists.txt | 114 ---- src/python/Cylinder_TEST.py | 119 ---- src/python/Frustum_TEST.py | 604 -------------------- src/python/GaussMarkovProcess.i | 44 -- src/python/Inertial_TEST.py | 495 ---------------- src/python/OrientedBox_TEST.py | 310 ---------- src/python/Plane_TEST.py | 165 ------ src/python/Rand.i | 38 -- src/python/Sphere_TEST.py | 202 ------- src/python/SphericalCoordinates_TEST.py | 562 ------------------ src/python/Vector2.i | 81 --- src/python/Vector3.i | 101 ---- src/python/Vector4.i | 77 --- src/python/python.i | 42 -- src/python/python_TEST.py | 32 -- src/{python => ruby}/AxisAlignedBox.i | 0 src/{python => ruby}/Box.i | 0 src/{python => ruby}/Color.i | 0 src/{python => ruby}/Cylinder.i | 0 src/{python => ruby}/DiffDriveOdometry.i | 0 src/{python => ruby}/Filter.i | 0 src/{python => ruby}/Frustum.i | 0 src/{python => ruby}/Helpers.i | 0 src/{python => ruby}/Inertial.i | 0 src/{python => ruby}/Kmeans.i | 0 src/{python => ruby}/Line2.i | 0 src/{python => ruby}/Line3.i | 0 src/{python => ruby}/MassMatrix3.i | 0 src/{python => ruby}/Material.i | 0 src/{python => ruby}/MaterialType.i | 0 src/{python => ruby}/Matrix3.i | 0 src/{python => ruby}/Matrix4.i | 0 src/{python => ruby}/MovingWindowFilter.i | 0 src/{python => ruby}/OrientedBox.i | 0 src/{python => ruby}/PID.i | 0 src/{python => ruby}/Plane.i | 0 src/{python => ruby}/Pose3.i | 0 src/{python => ruby}/Quaternion.i | 0 src/{python => ruby}/RollingMean.i | 0 src/{python => ruby}/RotationSpline.i | 0 src/{python => ruby}/SemanticVersion.i | 0 src/{python => ruby}/SignalStats.i | 0 src/{python => ruby}/Sphere.i | 0 src/{python => ruby}/SphericalCoordinates.i | 0 src/{python => ruby}/Spline.i | 0 src/{python => ruby}/StopWatch.i | 0 src/{python => ruby}/Temperature.i | 0 src/{python => ruby}/Triangle.i | 0 src/{python => ruby}/Triangle3.i | 0 src/{python => ruby}/Vector3Stats.i | 0 53 files changed, 3451 deletions(-) delete mode 100644 src/python/Angle.i delete mode 100644 src/python/Box_TEST.py delete mode 100644 src/python/CMakeLists.txt delete mode 100644 src/python/Cylinder_TEST.py delete mode 100644 src/python/Frustum_TEST.py delete mode 100644 src/python/GaussMarkovProcess.i delete mode 100644 src/python/Inertial_TEST.py delete mode 100644 src/python/OrientedBox_TEST.py delete mode 100644 src/python/Plane_TEST.py delete mode 100644 src/python/Rand.i delete mode 100644 src/python/Sphere_TEST.py delete mode 100644 src/python/SphericalCoordinates_TEST.py delete mode 100644 src/python/Vector2.i delete mode 100644 src/python/Vector3.i delete mode 100644 src/python/Vector4.i delete mode 100644 src/python/python.i delete mode 100644 src/python/python_TEST.py rename src/{python => ruby}/AxisAlignedBox.i (100%) rename src/{python => ruby}/Box.i (100%) rename src/{python => ruby}/Color.i (100%) rename src/{python => ruby}/Cylinder.i (100%) rename src/{python => ruby}/DiffDriveOdometry.i (100%) rename src/{python => ruby}/Filter.i (100%) rename src/{python => ruby}/Frustum.i (100%) rename src/{python => ruby}/Helpers.i (100%) rename src/{python => ruby}/Inertial.i (100%) rename src/{python => ruby}/Kmeans.i (100%) rename src/{python => ruby}/Line2.i (100%) rename src/{python => ruby}/Line3.i (100%) rename src/{python => ruby}/MassMatrix3.i (100%) rename src/{python => ruby}/Material.i (100%) rename src/{python => ruby}/MaterialType.i (100%) rename src/{python => ruby}/Matrix3.i (100%) rename src/{python => ruby}/Matrix4.i (100%) rename src/{python => ruby}/MovingWindowFilter.i (100%) rename src/{python => ruby}/OrientedBox.i (100%) rename src/{python => ruby}/PID.i (100%) rename src/{python => ruby}/Plane.i (100%) rename src/{python => ruby}/Pose3.i (100%) rename src/{python => ruby}/Quaternion.i (100%) rename src/{python => ruby}/RollingMean.i (100%) rename src/{python => ruby}/RotationSpline.i (100%) rename src/{python => ruby}/SemanticVersion.i (100%) rename src/{python => ruby}/SignalStats.i (100%) rename src/{python => ruby}/Sphere.i (100%) rename src/{python => ruby}/SphericalCoordinates.i (100%) rename src/{python => ruby}/Spline.i (100%) rename src/{python => ruby}/StopWatch.i (100%) rename src/{python => ruby}/Temperature.i (100%) rename src/{python => ruby}/Triangle.i (100%) rename src/{python => ruby}/Triangle3.i (100%) rename src/{python => ruby}/Vector3Stats.i (100%) diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index 6c3a024b6..b8f2a625e 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -13,6 +13,5 @@ ign_build_tests(TYPE UNIT SOURCES ${gtest_sources}) add_subdirectory(graph) # Bindings subdirectories -add_subdirectory(python) add_subdirectory(python_pybind11) add_subdirectory(ruby) diff --git a/src/python/Angle.i b/src/python/Angle.i deleted file mode 100644 index 77bd73341..000000000 --- a/src/python/Angle.i +++ /dev/null @@ -1,59 +0,0 @@ -/* - * 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. - * -*/ - -%module angle -%{ -#include -%} - -namespace ignition -{ - namespace math - { - class Angle - { - %rename("%(undercase)s", %$isfunction, %$ismember, %$not %$isconstructor) ""; - %rename("%(uppercase)s", %$isstatic, %$isvariable) ""; - public: static const Angle Zero; - public: static const Angle Pi; - %rename(HALF_PI) HalfPi; - public: static const Angle HalfPi; - %rename(TWO_PI) TwoPi; - public: static const Angle TwoPi; - public: Angle(); - public: Angle(double _radian); - public: Angle(const Angle &_angle); - public: virtual ~Angle(); - public: void SetRadian(double _radian); - public: void SetDegree(double _degree); - public: double Radian() const; - public: double Degree() const; - public: void Normalize(); - public: Angle Normalized() const; - public: inline double operator*() const; - public: Angle operator-(const Angle &_angle) const; - public: Angle operator+(const Angle &_angle) const; - public: Angle operator*(const Angle &_angle) const; - public: Angle operator/(const Angle &_angle) const; - public: bool operator==(const Angle &_angle) const; - public: bool operator<(const Angle &_angle) const; - public: bool operator<=(const Angle &_angle) const; - public: bool operator>(const Angle &_angle) const; - public: bool operator>=(const Angle &_angle) const; - }; - } -} diff --git a/src/python/Box_TEST.py b/src/python/Box_TEST.py deleted file mode 100644 index 905716243..000000000 --- a/src/python/Box_TEST.py +++ /dev/null @@ -1,405 +0,0 @@ -# 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. - -import unittest - -import ignition -from ignition.math import Boxd, MassMatrix3d, Material, Planed, Vector3d - - -class TestBox(unittest.TestCase): - - def test_constructor(self): - # Default constructor - box = Boxd() - self.assertEqual(Vector3d.ZERO, box.size()) - self.assertEqual(Material(), box.material()) - - box2 = Boxd() - self.assertEqual(box, box2) - - # Individual dimension constructor - box = Boxd(1.0, 2.0, 3.0) - self.assertEqual(Vector3d(1.0, 2.0, 3.0), box.size()) - self.assertEqual(Material(), box.material()) - - box2 = Boxd(1.0, 2.0, 3.0) - self.assertEqual(box, box2) - - # Vector dimension constructor - box = Boxd(Vector3d(1.3, 2.5, 4.6)) - self.assertEqual(Vector3d(1.3, 2.5, 4.6), box.size()) - self.assertEqual(Material(), box.material()) - - box2 = Boxd(Vector3d(1.3, 2.5, 4.6)) - self.assertEqual(box, box2) - - # Dimension and mat constructor - box = Boxd(1.0, 2.0, 5.0, Material(ignition.math.MaterialType_WOOD)) - self.assertEqual(Vector3d(1.0, 2.0, 5.0), box.size()) - self.assertEqual(Material(ignition.math.MaterialType_WOOD), box.material()) - - box2 = Boxd(1.0, 2.0, 5.0, Material(ignition.math.MaterialType_WOOD)) - self.assertEqual(box, box2) - - # Vector Dimension and mat constructor - box = Boxd(Vector3d(2.2, 2.0, 10.0), Material(ignition.math.MaterialType_WOOD)) - self.assertEqual(Vector3d(2.2, 2.0, 10.0), box.size()) - self.assertEqual(Material(ignition.math.MaterialType_WOOD), box.material()) - - box2 = Boxd(Vector3d(2.2, 2.0, 10.0), Material(ignition.math.MaterialType_WOOD)) - self.assertEqual(box, box2) - - def test_mutators(self): - box = Boxd() - box.set_size(100.1, 2.3, 5.6) - box.set_material(Material(ignition.math.MaterialType_PINE)) - - self.assertEqual(100.1, box.size().x()) - self.assertEqual(2.3, box.size().y()) - self.assertEqual(5.6, box.size().z()) - self.assertEqual(Material(ignition.math.MaterialType_PINE), box.material()) - - box.set_size(Vector3d(3.4, 1.2, 0.5)) - self.assertEqual(3.4, box.size().x()) - self.assertEqual(1.2, box.size().y()) - self.assertEqual(0.5, box.size().z()) - - def test_volume_and_density(self): - mass = 1.0 - box = Boxd(1.0, 0.1, 10.4) - expectedVolume = 1.0 * 0.1 * 10.4 - self.assertEqual(expectedVolume, box.volume()) - - expectedDensity = mass / expectedVolume - self.assertEqual(expectedDensity, box.density_from_mass(mass)) - - # Bad density - box2 = Boxd() - self.assertGreater(0.0, box2.density_from_mass(mass)) - - def test_intersections(self): - # No intersections - box = Boxd(2.0, 2.0, 2.0) - plane = Planed(Vector3d(0.0, 0.0, 1.0), -5.0) - self.assertEqual(0, box.intersections(plane).size()) - - # Plane crosses 4 edges - box = Boxd(2.0, 2.0, 2.0) - plane = Planed(Vector3d(0.0, 0.0, 1.0), 0) - - intersections = box.intersections(plane) - self.assertEqual(4, intersections.size()) - self.assertEqual(intersections.count(Vector3d(-1.0, -1.0, 0.0)), 1) - self.assertEqual(intersections.count(Vector3d(-1.0, 1.0, 0.0)), 1) - self.assertEqual(intersections.count(Vector3d(1.0, -1.0, 0.0)), 1) - self.assertEqual(intersections.count(Vector3d(1.0, 1.0, 0.0)), 1) - - # Plane coincides with box's face - box = Boxd(2.0, 2.0, 2.0) - plane = Planed(Vector3d(0.0, 0.0, 1.0), 1.0) - - intersections = box.intersections(plane) - self.assertEqual(4, intersections.size()) - self.assertEqual(intersections.count(Vector3d(-1.0, -1.0, 1.0)), 1) - self.assertEqual(intersections.count(Vector3d(-1.0, 1.0, 1.0)), 1) - self.assertEqual(intersections.count(Vector3d(1.0, -1.0, 1.0)), 1) - self.assertEqual(intersections.count(Vector3d(1.0, 1.0, 1.0)), 1) - - # 3 intersections - box = Boxd(2.0, 2.0, 2.0) - plane = Planed(Vector3d(1.0, 1.0, 1.0), 1.0) - - intersections = box.intersections(plane) - self.assertEqual(3, intersections.size()) - self.assertEqual(intersections.count(Vector3d(1.0, -1.0, 1.0)), 1) - self.assertEqual(intersections.count(Vector3d(-1.0, 1.0, 1.0)), 1) - self.assertEqual(intersections.count(Vector3d(1.0, 1.0, -1.0)), 1) - - # 6 intersections - box = Boxd(2.0, 2.0, 2.0) - plane = Planed(Vector3d(1.0, 1.0, 1.0), 0.5) - - intersections = box.intersections(plane) - self.assertEqual(6, intersections.size()) - self.assertEqual(intersections.count(Vector3d(-1.0, 1.0, 0.5)), 1) - self.assertEqual(intersections.count(Vector3d(-1.0, 0.5, 1.0)), 1) - self.assertEqual(intersections.count(Vector3d(1.0, -1.0, 0.5)), 1) - self.assertEqual(intersections.count(Vector3d(0.5, -1.0, 1.0)), 1) - self.assertEqual(intersections.count(Vector3d(1.0, 0.5, -1.0)), 1) - self.assertEqual(intersections.count(Vector3d(0.5, 1.0, -1.0)), 1) - - # 5 intersections - # This is the plane above tilted further up - box = Boxd(2.0, 2.0, 2.0) - plane = Planed(Vector3d(1.0, 1.0, 2.0), 0.5) - - intersections = box.intersections(plane) - self.assertEqual(5, intersections.size()) - self.assertEqual(intersections.count(Vector3d(-1.0, 1.0, 0.25)), 1) - self.assertEqual(intersections.count(Vector3d(-1.0, -0.5, 1.0)), 1) - self.assertEqual(intersections.count(Vector3d(1.0, -1.0, 0.25)), 1) - self.assertEqual(intersections.count(Vector3d(-0.5, -1.0, 1.0)), 1) - self.assertEqual(intersections.count(Vector3d(1.0, 1.0, -0.75)), 1) - - def test_volume_below(self): - # Fully above - box = Boxd(2.0, 2.0, 2.0) - plane = Planed(Vector3d(0.0, 0.0, 1.0), -5.0) - self.assertEqual(0.0, box.volume_below(plane)) - - # Fully below - box = Boxd(2.0, 2.0, 2.0) - plane = Planed(Vector3d(0.0, 0.0, 1.0), 20.0) - self.assertEqual(box.volume(), box.volume_below(plane)) - - # Fully below (because plane is rotated down) - box = Boxd(2.0, 2.0, 2.0) - plane = Planed(Vector3d(0.0, 0.0, -1.0), 20.0) - self.assertEqual(box.volume(), box.volume_below(plane)) - - # Cut in half - box = Boxd(2.0, 2.0, 2.0) - plane = Planed(Vector3d(0, 0, 1.0), 0) - - self.assertEqual(box.volume()/2, box.volume_below(plane)) - - box = Boxd(2.0, 2.0, 2.0) - plane = Planed(Vector3d(0, 1, 0), 0) - - self.assertEqual(box.volume()/2, box.volume_below(plane)) - - box = Boxd(2.0, 2.0, 2.0) - plane = Planed(Vector3d(-1, 0, 0), 0) - - self.assertEqual(box.volume()/2, box.volume_below(plane)) - - box = Boxd(2.0, 2.0, 2.0) - plane = Planed(Vector3d(-1, -1, 0), 0) - - self.assertEqual(box.volume()/2, box.volume_below(plane)) - - box = Boxd(2.0, 2.0, 2.0) - plane = Planed(Vector3d(0, 1, 1), 0) - - self.assertEqual(box.volume()/2, box.volume_below(plane)) - - box = Boxd(2.0, 2.0, 2.0) - plane = Planed(Vector3d(1, 1, 1), 0) - - self.assertAlmostEqual(box.volume()/2, box.volume_below(plane), delta=1e-15) - - # Cut in 3/4 - box = Boxd(2.0, 2.0, 2.0) - plane = Planed(Vector3d(0, 0, 1.0), 0.5) - - self.assertEqual(3*box.volume()/4, box.volume_below(plane)) - - # Opposites add to the total volume - box = Boxd(2.0, 2.0, 2.0) - planeA = Planed(Vector3d(0, 0, 1.0), 0.5) - planeB = Planed(Vector3d(0, 0, 1.0), -0.5) - - self.assertEqual(box.volume(), - box.volume_below(planeA) + box.volume_below(planeB)) - - box = Boxd(2.0, 2.0, 2.0) - planeA = Planed(Vector3d(0, 1.0, 1.0), 0.5) - planeB = Planed(Vector3d(0, 1.0, 1.0), -0.5) - - self.assertEqual(box.volume(), - box.volume_below(planeA) + box.volume_below(planeB)) - - box = Boxd(2.0, 2.0, 2.0) - planeA = Planed(Vector3d(-1, 1.0, 1.0), 0.5) - planeB = Planed(Vector3d(-1, 1.0, 1.0), -0.5) - - self.assertEqual(box.volume(), - box.volume_below(planeA) + box.volume_below(planeB)) - - def test_center_of_volume_below(self): - # Fully above - box = Boxd(2.0, 2.0, 2.0) - plane = Planed(Vector3d(0.0, 0.0, 1.0), -5.0) - self.assertFalse(box.center_of_volume_below(plane) is not None) - - # Fully below - box = Boxd(2.0, 2.0, 2.0) - plane = Planed(Vector3d(0.0, 0.0, 1.0), 5.0) - self.assertTrue(box.center_of_volume_below(plane) is not None) - self.assertEqual(box.center_of_volume_below(plane), Vector3d(0, 0, 0)) - - # Cut in half - box = Boxd(2.0, 2.0, 2.0) - plane = Planed(Vector3d(0.0, 0.0, 1.0), 0) - self.assertTrue(box.center_of_volume_below(plane) is not None) - self.assertEqual(box.center_of_volume_below(plane), - Vector3d(0, 0, -0.5)) - - box = Boxd(2.0, 2.0, 2.0) - plane = Planed(Vector3d(0.0, 0.0, -1.0), 0) - self.assertTrue(box.center_of_volume_below(plane) is not None) - self.assertEqual(box.center_of_volume_below(plane), - Vector3d(0, 0, 0.5)) - - def test_vertices_below(self): - - box = Boxd(2.0, 2.0, 2.0) - size = box.size() - - pXpYpZ = Vector3d(size.x()/2, size.y()/2, size.z()/2) - nXpYpZ = Vector3d(-size.x()/2, size.y()/2, size.z()/2) - pXnYpZ = Vector3d(size.x()/2, -size.y()/2, size.z()/2) - nXnYpZ = Vector3d(-size.x()/2, -size.y()/2, size.z()/2) - pXpYnZ = Vector3d(size.x()/2, size.y()/2, -size.z()/2) - nXpYnZ = Vector3d(-size.x()/2, size.y()/2, -size.z()/2) - pXnYnZ = Vector3d(size.x()/2, -size.y()/2, -size.z()/2) - nXnYnZ = Vector3d(-size.x()/2, -size.y()/2, -size.z()/2) - - # Fully above - plane = Planed(Vector3d(0.0, 0.0, 1.0), -5.0) - self.assertTrue(box.vertices_below(plane).empty()) - - # Fully below - - plane = Planed(Vector3d(0.0, 0.0, 1.0), 20.0) - self.assertEqual(8, box.vertices_below(plane).size()) - - # Fully below (because plane is rotated down) - - plane = Planed(Vector3d(0.0, 0.0, -1.0), 20.0) - self.assertEqual(8, box.vertices_below(plane).size()) - - # 4 vertices - - plane = Planed(Vector3d(0, 0, 1.0), 0) - - vertices = box.vertices_below(plane) - self.assertEqual(4, vertices.size()) - - self.assertEqual(vertices.count(nXnYnZ), 1) - self.assertEqual(vertices.count(nXpYnZ), 1) - self.assertEqual(vertices.count(pXnYnZ), 1) - self.assertEqual(vertices.count(pXpYnZ), 1) - - plane = Planed(Vector3d(0, 1, 0), 0.5) - - vertices = box.vertices_below(plane) - self.assertEqual(4, vertices.size()) - - self.assertEqual(vertices.count(nXnYnZ), 1) - self.assertEqual(vertices.count(nXnYpZ), 1) - self.assertEqual(vertices.count(pXnYnZ), 1) - self.assertEqual(vertices.count(pXnYpZ), 1) - - plane = Planed(Vector3d(-1, 0, 0), -0.5) - - vertices = box.vertices_below(plane) - self.assertEqual(4, vertices.size()) - - self.assertEqual(vertices.count(pXnYnZ), 1) - self.assertEqual(vertices.count(pXnYpZ), 1) - self.assertEqual(vertices.count(pXpYnZ), 1) - self.assertEqual(vertices.count(pXpYpZ), 1) - - plane = Planed(Vector3d(1, 1, 1), 0.0) - - vertices = box.vertices_below(plane) - self.assertEqual(4, vertices.size()) - - self.assertEqual(vertices.count(nXnYnZ), 1) - self.assertEqual(vertices.count(nXnYpZ), 1) - self.assertEqual(vertices.count(nXpYnZ), 1) - self.assertEqual(vertices.count(pXnYnZ), 1) - - # 6 vertices - plane = Planed(Vector3d(-1, -1, 0), 0.3) - - vertices = box.vertices_below(plane) - self.assertEqual(6, vertices.size()) - - self.assertEqual(vertices.count(nXpYnZ), 1) - self.assertEqual(vertices.count(nXpYpZ), 1) - self.assertEqual(vertices.count(pXnYnZ), 1) - self.assertEqual(vertices.count(pXnYpZ), 1) - self.assertEqual(vertices.count(pXpYnZ), 1) - self.assertEqual(vertices.count(pXpYpZ), 1) - - plane = Planed(Vector3d(0, 1, 1), 0.9) - - vertices = box.vertices_below(plane) - self.assertEqual(6, vertices.size()) - - self.assertEqual(vertices.count(nXnYnZ), 1) - self.assertEqual(vertices.count(nXnYpZ), 1) - self.assertEqual(vertices.count(pXnYpZ), 1) - self.assertEqual(vertices.count(nXpYnZ), 1) - self.assertEqual(vertices.count(pXnYnZ), 1) - self.assertEqual(vertices.count(pXpYnZ), 1) - - # 2 vertices - plane = Planed(Vector3d(-1, -1, 0), -0.5) - - vertices = box.vertices_below(plane) - self.assertEqual(2, vertices.size()) - - self.assertEqual(vertices.count(pXpYnZ), 1) - self.assertEqual(vertices.count(pXpYpZ), 1) - - # 7 vertices - plane = Planed(Vector3d(1, 1, 1), 1.0) - - vertices = box.vertices_below(plane) - self.assertEqual(7, vertices.size()) - - self.assertEqual(vertices.count(nXnYnZ), 1) - self.assertEqual(vertices.count(nXnYpZ), 1) - self.assertEqual(vertices.count(pXnYpZ), 1) - self.assertEqual(vertices.count(nXpYnZ), 1) - self.assertEqual(vertices.count(nXpYpZ), 1) - self.assertEqual(vertices.count(pXnYnZ), 1) - self.assertEqual(vertices.count(pXpYnZ), 1) - - # 1 vertex - plane = Planed(Vector3d(1, 1, 1), -1.2) - - vertices = box.vertices_below(plane) - self.assertEqual(1, vertices.size()) - - self.assertEqual(vertices.count(nXnYnZ), 1) - - def test_mass(self): - mass = 2.0 - length = 2.0 - w = 0.1 - h = 34.12 - box = Boxd(length, w, h) - box.set_density_from_mass(mass) - - massMat = MassMatrix3d() - ixx = (1.0/12.0) * mass * (w*w + h*h) - iyy = (1.0/12.0) * mass * (length * length + h*h) - izz = (1.0/12.0) * mass * (length * length + w*w) - - expectedMassMat = MassMatrix3d() - expectedMassMat.set_inertia_matrix(ixx, iyy, izz, 0.0, 0.0, 0.0) - expectedMassMat.set_mass(mass) - - box.mass_matrix(massMat) - self.assertEqual(expectedMassMat, massMat) - self.assertEqual(expectedMassMat.mass(), massMat.mass()) - - -if __name__ == '__main__': - unittest.main() diff --git a/src/python/CMakeLists.txt b/src/python/CMakeLists.txt deleted file mode 100644 index ed44210f9..000000000 --- a/src/python/CMakeLists.txt +++ /dev/null @@ -1,114 +0,0 @@ -################################################# -# Setup swig -if (SWIG_FOUND) - if (POLICY CMP0078) - cmake_policy(SET CMP0078 NEW) - endif() - if (POLICY CMP0086) - cmake_policy(SET CMP0086 NEW) - endif() - - include(${SWIG_USE_FILE}) - set(CMAKE_SWIG_FLAGS "") - - include_directories(${PROJECT_SOURCE_DIR}/include) - include_directories(${PYTHON_INCLUDE_PATH}) -endif() - -################################# -# Create and install Python interfaces -# Example usage -# $ export PYTHONPATH=/ws/install/lib/python/:$PYTHONPATH -if (PYTHONLIBS_FOUND) - set_source_files_properties(python.i PROPERTIES CPLUSPLUS ON) - set_source_files_properties(python.i PROPERTIES SWIG_FLAGS "-includeall") - set_source_files_properties(python.i PROPERTIES SWIG_MODULE_NAME "math") - set(SWIG_PY_LIB pymath) - set(SWIG_PY_LIB_OUTPUT math) - - set(CMAKE_SWIG_OUTDIR "${CMAKE_BINARY_DIR}/lib/python/swig") - if(CMAKE_VERSION VERSION_GREATER 3.8.0) - SWIG_ADD_LIBRARY(${SWIG_PY_LIB} LANGUAGE python SOURCES python.i) - else() - SWIG_ADD_MODULE(${SWIG_PY_LIB} python python.i) - endif() - - SWIG_LINK_LIBRARIES(${SWIG_PY_LIB} - ${PYTHON_LIBRARIES} - ignition-math${PROJECT_VERSION_MAJOR} - ) - - if(NOT CMAKE_VERSION VERSION_GREATER_EQUAL 3.13.0) - set(SWIG_PY_LIB "_${SWIG_PY_LIB}") - set(SWIG_PY_LIB_OUTPUT "_${SWIG_PY_LIB_OUTPUT}") - endif() - - set_target_properties(${SWIG_PY_LIB} - PROPERTIES - OUTPUT_NAME ${SWIG_PY_LIB_OUTPUT} - ) - - # Suppress warnings on SWIG-generated files - target_compile_options(${SWIG_PY_LIB} PRIVATE - $<$:-Wno-pedantic -Wno-shadow -Wno-maybe-uninitialized -Wno-unused-parameter -Wno-cast-function-type -Wno-missing-field-initializers -Wno-class-memaccess> - $<$:-Wno-shadow -Wno-maybe-uninitialized -Wno-unused-parameter -Wno-cast-function-type -Wno-missing-field-initializers -Wno-class-memaccess> - $<$:-Wno-shadow -Wno-maybe-uninitialized -Wno-unused-parameter -Wno-cast-function-type -Wno-missing-field-initializers -Wno-class-memaccess> - ) - - if(USE_SYSTEM_PATHS_FOR_PYTHON_INSTALLATION) - if(${CMAKE_VERSION} VERSION_LESS "3.12.0") - execute_process( - COMMAND "${PYTHON_EXECUTABLE}" -c "if True: - from distutils import sysconfig as sc - print(sc.get_python_lib(plat_specific=True))" - OUTPUT_VARIABLE Python3_SITEARCH - OUTPUT_STRIP_TRAILING_WHITESPACE) - else() - # Get install variable from Python3 module - # Python3_SITEARCH is available from 3.12 on, workaround if needed: - find_package(Python3 COMPONENTS Interpreter) - endif() - - if(USE_DIST_PACKAGES_FOR_PYTHON) - string(REPLACE "site-packages" "dist-packages" IGN_PYTHON_INSTALL_PATH ${Python3_SITEARCH}) - else() - # custom cmake command is returning dist-packages - string(REPLACE "dist-packages" "site-packages" IGN_PYTHON_INSTALL_PATH ${Python3_SITEARCH}) - endif() - else() - # If not a system installation, respect local paths - set(IGN_PYTHON_INSTALL_PATH ${IGN_LIB_INSTALL_DIR}/python/swig) - endif() - - set(IGN_PYTHON_INSTALL_PATH "${IGN_PYTHON_INSTALL_PATH}/ignition") - install(TARGETS ${SWIG_PY_LIB} DESTINATION ${IGN_PYTHON_INSTALL_PATH}) - install(FILES ${CMAKE_BINARY_DIR}/lib/python/swig/math.py DESTINATION ${IGN_PYTHON_INSTALL_PATH}) - - if (BUILD_TESTING) - # Add the Python tests - set(python_tests - Box_TEST - Cylinder_TEST - Frustum_TEST - Inertial_TEST - OrientedBox_TEST - Plane_TEST - python_TEST - Sphere_TEST - SphericalCoordinates_TEST - Vector3Stats_TEST - ) - - foreach (test ${python_tests}) - add_test(NAME ${test}.py COMMAND - "${PYTHON_EXECUTABLE}" "${CMAKE_SOURCE_DIR}/src/python/${test}.py") - - set(_env_vars) - list(APPEND _env_vars "PYTHONPATH=${FAKE_INSTALL_PREFIX}/lib/python/swig") - list(APPEND _env_vars "LD_LIBRARY_PATH=${FAKE_INSTALL_PREFIX}/lib:$ENV{LD_LIBRARY_PATH}") - set_tests_properties(${test}.py PROPERTIES - ENVIRONMENT "${_env_vars}") - endforeach() - endif() - -endif() diff --git a/src/python/Cylinder_TEST.py b/src/python/Cylinder_TEST.py deleted file mode 100644 index d5c866342..000000000 --- a/src/python/Cylinder_TEST.py +++ /dev/null @@ -1,119 +0,0 @@ -# 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. - -import math -import unittest - -import ignition -from ignition.math import Cylinderd, IGN_PI, MassMatrix3d, Material, Quaterniond - - -class TestCylinder(unittest.TestCase): - - def test_constructor(self): - # Default constructor - cylinder = Cylinderd() - self.assertEqual(0.0, cylinder.length()) - self.assertEqual(0.0, cylinder.radius()) - self.assertEqual(Quaterniond.IDENTITY, cylinder.rotational_offset()) - self.assertEqual(Material(), cylinder.mat()) - - cylinder2 = Cylinderd() - self.assertEqual(cylinder, cylinder2) - - # Length and radius constructor - cylinder = Cylinderd(1.0, 2.0) - self.assertEqual(1.0, cylinder.length()) - self.assertEqual(2.0, cylinder.radius()) - self.assertEqual(Quaterniond.IDENTITY, cylinder.rotational_offset()) - self.assertEqual(Material(), cylinder.mat()) - - cylinder2 = Cylinderd(1.0, 2.0) - self.assertEqual(cylinder, cylinder2) - - # Length, radius, and rot constructor - cylinder = Cylinderd(1.0, 2.0, Quaterniond(0.1, 0.2, 0.3)) - self.assertEqual(1.0, cylinder.length()) - self.assertEqual(2.0, cylinder.radius()) - self.assertEqual(Quaterniond(0.1, 0.2, 0.3), - cylinder.rotational_offset()) - self.assertEqual(Material(), cylinder.mat()) - - cylinder2 = Cylinderd(1.0, 2.0, Quaterniond(0.1, 0.2, 0.3)) - self.assertEqual(cylinder, cylinder2) - - # Length, radius, mat and rot constructor - cylinder = Cylinderd(1.0, 2.0, Material(ignition.math.MaterialType_WOOD), - Quaterniond(0.1, 0.2, 0.3)) - self.assertEqual(1.0, cylinder.length()) - self.assertEqual(2.0, cylinder.radius()) - self.assertEqual(Quaterniond(0.1, 0.2, 0.3), cylinder.rotational_offset()) - self.assertEqual(Material(ignition.math.MaterialType_WOOD), cylinder.mat()) - - cylinder2 = Cylinderd(1.0, 2.0, Material(ignition.math.MaterialType_WOOD), - Quaterniond(0.1, 0.2, 0.3)) - self.assertEqual(cylinder, cylinder2) - - def test_mutators(self): - cylinder = Cylinderd() - self.assertEqual(0.0, cylinder.length()) - self.assertEqual(0.0, cylinder.radius()) - self.assertEqual(Quaterniond.IDENTITY, cylinder.rotational_offset()) - self.assertEqual(Material(), cylinder.mat()) - - cylinder.set_length(100.1) - cylinder.set_radius(.123) - cylinder.set_rotational_offset(Quaterniond(1.2, 2.3, 3.4)) - cylinder.set_mat(Material(ignition.math.MaterialType_PINE)) - - self.assertEqual(100.1, cylinder.length()) - self.assertEqual(.123, cylinder.radius()) - self.assertEqual(Quaterniond(1.2, 2.3, 3.4), cylinder.rotational_offset()) - self.assertEqual(Material(ignition.math.MaterialType_PINE), cylinder.mat()) - - def test_volume_and_density(self): - mass = 1.0 - cylinder = Cylinderd(1.0, 0.001) - expectedVolume = (IGN_PI * math.pow(0.001, 2) * 1.0) - self.assertEqual(expectedVolume, cylinder.volume()) - - expectedDensity = mass / expectedVolume - self.assertEqual(expectedDensity, cylinder.density_from_mass(mass)) - - # Bad density - cylinder2 = Cylinderd() - self.assertGreater(0.0, cylinder2.density_from_mass(mass)) - - def test_mass(self): - mass = 2.0 - length = 2.0 - r = 0.1 - cylinder = Cylinderd(length, r) - cylinder.set_density_from_mass(mass) - - massMat = MassMatrix3d() - ixxIyy = (1/12.0) * mass * (3*r*r + length*length) - izz = 0.5 * mass * r * r - - expectedMassMat = MassMatrix3d() - expectedMassMat.set_inertia_matrix(ixxIyy, ixxIyy, izz, 0.0, 0.0, 0.0) - expectedMassMat.set_mass(mass) - - cylinder.mass_matrix(massMat) - self.assertEqual(expectedMassMat, massMat) - self.assertEqual(expectedMassMat.mass(), massMat.mass()) - - -if __name__ == '__main__': - unittest.main() diff --git a/src/python/Frustum_TEST.py b/src/python/Frustum_TEST.py deleted file mode 100644 index f54f4070c..000000000 --- a/src/python/Frustum_TEST.py +++ /dev/null @@ -1,604 +0,0 @@ -# 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. - -import math -import unittest - -from ignition.math import Angle, AxisAlignedBox, Frustum, IGN_PI, Planed, Pose3d, Vector3d - - -class TestFrustrum(unittest.TestCase): - - def test_constructor(self): - frustum = Frustum() - - self.assertEqual(frustum.near(), 0.0) - self.assertEqual(frustum.far(), 1.0) - self.assertEqual(frustum.fov().radian(), 45 * IGN_PI / 180.0) - self.assertEqual(frustum.aspect_ratio(), 1.0) - self.assertEqual(frustum.pose(), Pose3d.ZERO) - - def test_copy_constructor(self): - - # Frustum pointing down the +x axis - frustum = Frustum( - # Near distance - 1, - # Far distance - 10, - # Field of view - Angle(45 * IGN_PI / 180.0), - # Aspect ratio - 320.0 / 240.0, - # Pose - Pose3d(0, 0, 0, 0, 0, 0)) - - frustum2 = Frustum(frustum) - - self.assertEqual(frustum.fov(), frustum2.fov()) - self.assertEqual(frustum.near(), frustum2.near()) - self.assertEqual(frustum.far(), frustum2.far()) - self.assertEqual(frustum.aspect_ratio(), frustum2.aspect_ratio()) - self.assertEqual(frustum.aspect_ratio(), frustum2.aspect_ratio()) - - self.assertEqual(frustum.plane(Frustum.FRUSTUM_PLANE_NEAR).normal(), - frustum2.plane(Frustum.FRUSTUM_PLANE_NEAR).normal()) - - self.assertEqual(frustum.plane(Frustum.FRUSTUM_PLANE_FAR).normal(), - frustum2.plane(Frustum.FRUSTUM_PLANE_FAR).normal()) - - self.assertEqual(frustum.plane(Frustum.FRUSTUM_PLANE_LEFT).normal(), - frustum2.plane(Frustum.FRUSTUM_PLANE_LEFT).normal()) - - self.assertEqual(frustum.plane(Frustum.FRUSTUM_PLANE_RIGHT).normal(), - frustum2.plane(Frustum.FRUSTUM_PLANE_RIGHT).normal()) - - self.assertEqual(frustum.plane(Frustum.FRUSTUM_PLANE_TOP).normal(), - frustum2.plane(Frustum.FRUSTUM_PLANE_TOP).normal()) - - self.assertEqual(frustum.plane(Frustum.FRUSTUM_PLANE_BOTTOM).normal(), - frustum2.plane(Frustum.FRUSTUM_PLANE_BOTTOM).normal()) - - def test_assignment_operator(self): - # Frustum pointing to the +X+Y diagonal - frustum = Frustum( - # Near distance - 1, - # Far distance - 10, - # Field of view - Angle(45 * IGN_PI / 180.0), - # Aspect ratio - 320.0/240.0, - # Pose - Pose3d(0, 0, 0, 0, 0, 45 * IGN_PI / 180.0)) - - frustum2 = Frustum() - frustum2 = frustum - - self.assertEqual(frustum.fov(), frustum2.fov()) - self.assertEqual(frustum.near(), frustum2.near()) - self.assertEqual(frustum.far(), frustum2.far()) - self.assertEqual(frustum.aspect_ratio(), frustum2.aspect_ratio()) - self.assertEqual(frustum.aspect_ratio(), frustum2.aspect_ratio()) - - self.assertEqual(frustum.plane(Frustum.FRUSTUM_PLANE_NEAR).normal(), - frustum2.plane(Frustum.FRUSTUM_PLANE_NEAR).normal()) - - self.assertEqual(frustum.plane(Frustum.FRUSTUM_PLANE_FAR).normal(), - frustum2.plane(Frustum.FRUSTUM_PLANE_FAR).normal()) - - self.assertEqual(frustum.plane(Frustum.FRUSTUM_PLANE_LEFT).normal(), - frustum2.plane(Frustum.FRUSTUM_PLANE_LEFT).normal()) - - self.assertEqual(frustum.plane(Frustum.FRUSTUM_PLANE_RIGHT).normal(), - frustum2.plane(Frustum.FRUSTUM_PLANE_RIGHT).normal()) - - self.assertEqual(frustum.plane(Frustum.FRUSTUM_PLANE_TOP).normal(), - frustum2.plane(Frustum.FRUSTUM_PLANE_TOP).normal()) - - self.assertEqual(frustum.plane(Frustum.FRUSTUM_PLANE_BOTTOM).normal(), - frustum2.plane(Frustum.FRUSTUM_PLANE_BOTTOM).normal()) - - def test_pyramid_x_axis_pos(self): - # Frustum pointing down the +x axis - frustum = Frustum( - # Near distance - 1, - # Far distance - 10, - # Field of view - Angle(45 * IGN_PI / 180.0), - # Aspect ratio - 320.0/240.0, - # Pose - Pose3d(0, 0, 0, 0, 0, 0)) - - self.assertFalse(frustum.contains(Vector3d(0, 0, 0))) - self.assertTrue(frustum.contains(Vector3d(1, 0, 0))) - - self.assertTrue(frustum.contains(Vector3d(2, 0, 0))) - self.assertTrue(frustum.contains(Vector3d(10, 0, 0))) - self.assertFalse(frustum.contains(Vector3d(10.1, 0, 0))) - - self.assertTrue(frustum.contains( - AxisAlignedBox(Vector3d(1, 0, 0), Vector3d(5, 5, 5)))) - self.assertFalse(frustum.contains( - AxisAlignedBox(Vector3d(-1, 0, 0), Vector3d(.1, .2, .3)))) - - def test_pyramid_x_axis_neg(self): - # Frustum pointing down the -x axis - frustum = Frustum( - # Near distance - 1, - # Far distance - 10, - # Field of view - Angle(45 * IGN_PI / 180.0), - # Aspect ratio - 320.0/240.0, - # Pose - Pose3d(0, 0, 0, 0, 0, IGN_PI)) - - self.assertFalse(frustum.contains(Vector3d(0, 0, 0))) - self.assertFalse(frustum.contains(Vector3d(-0.5, 0, 0))) - self.assertFalse(frustum.contains(Vector3d(-10.1, 0, 0))) - - self.assertTrue(frustum.contains(Vector3d(-1, 0, 0))) - self.assertTrue(frustum.contains(Vector3d(-2, 0, 0))) - self.assertTrue(frustum.contains(Vector3d(-10, 0, 0))) - - self.assertFalse(frustum.contains( - AxisAlignedBox(Vector3d(1, 0, 0), Vector3d(5, 5, 5)))) - self.assertTrue(frustum.contains( - AxisAlignedBox(Vector3d(-1, 0, 0), Vector3d(.1, .2, .3)))) - - def test_pyramid_y_axis(self): - # Frustum pointing down the +y axis - frustum = Frustum( - # Near distance - .1, - # Far distance - 5, - # Field of view - Angle(45 * IGN_PI / 180.0), - # Aspect ratio - 1.0, - # Pose - Pose3d(0, 0, 0, 0, 0, IGN_PI*0.5)) - - self.assertFalse(frustum.contains(Vector3d(0, 0, 0))) - self.assertFalse(frustum.contains(Vector3d(1, 0, 0))) - self.assertFalse(frustum.contains(Vector3d(.05, 0, 0))) - - self.assertTrue(frustum.contains(Vector3d(0, .1, 0))) - self.assertTrue(frustum.contains(Vector3d(0, 1, 0))) - self.assertTrue(frustum.contains(Vector3d(0, 5, 0))) - - self.assertTrue(frustum.contains( - AxisAlignedBox(Vector3d(0, 1, 0), Vector3d(5, 5, 5)))) - self.assertFalse(frustum.contains( - AxisAlignedBox(Vector3d(0, -1, 0), Vector3d(.1, 0, .3)))) - - def test_pyramid_z_axis(self): - # Frustum pointing down the -z axis - frustum = Frustum( - # Near distance - 1, - # Far distance - 10, - # Field of view - Angle(45 * IGN_PI / 180.0), - # Aspect ratio - 1.0, - # Pose - Pose3d(0, 0, 0, 0, IGN_PI*0.5, 0)) - - self.assertFalse(frustum.contains(Vector3d(0, 0, 0))) - self.assertFalse(frustum.contains(Vector3d(0, 0, -0.9))) - self.assertFalse(frustum.contains(Vector3d(0, 0, -10.5))) - self.assertFalse(frustum.contains(Vector3d(0, 0, 0.9))) - self.assertFalse(frustum.contains(Vector3d(0, 0, 10.5))) - - self.assertTrue(frustum.contains(Vector3d(0, 0, -1.1))) - self.assertTrue(frustum.contains(Vector3d(0.5, 0.5, -5.5))) - self.assertTrue(frustum.contains(Vector3d(0, 0, -10))) - - self.assertFalse(frustum.contains( - AxisAlignedBox(Vector3d(0, 0, 0), Vector3d(5, 5, 5)))) - self.assertTrue(frustum.contains( - AxisAlignedBox(Vector3d(0, 0, -1), Vector3d(.1, 0, .3)))) - - def test_near_far(self): - frustum = Frustum( - # Near distance - 1, - # Far distance - 10, - # Field of view - Angle(45 * IGN_PI / 180.0), - # Aspect ratio - 1.0, - # Pose - Pose3d(0, 0, 0, 0, IGN_PI*0.5, 0)) - - self.assertEqual(frustum.near(), 1.0) - self.assertEqual(frustum.far(), 10.0) - - frustum.set_near(-1.0) - frustum.set_far(-10.0) - - self.assertEqual(frustum.near(), -1.0) - self.assertEqual(frustum.far(), -10.0) - - def test_fov(self): - frustum = Frustum( - # Near distance - 1, - # Far distance - 10, - # Field of view - Angle(45 * IGN_PI / 180.0), - # Aspect ratio - 1.0, - # Pose - Pose3d(0, 0, 0, 0, IGN_PI*0.5, 0)) - - self.assertEqual(frustum.fov(), Angle(45 * IGN_PI / 180.0)) - - frustum.set_fov(Angle(1.5707)) - - self.assertEqual(frustum.fov(), Angle(1.5707)) - - def test_aspect_ratio(self): - frustum = Frustum( - # Near distance - 1, - # Far distance - 10, - # Field of view - Angle(45 * IGN_PI / 180.0), - # Aspect ratio - 1.0, - # Pose - Pose3d(0, 0, 0, 0, IGN_PI*0.5, 0)) - - self.assertEqual(frustum.aspect_ratio(), 1) - - frustum.set_aspect_ratio(1.3434) - - self.assertEqual(frustum.aspect_ratio(), 1.3434) - - def test_pose(self): - - frustum = Frustum( - # Near distance - 1, - # Far distance - 10, - # Field of view - Angle(45 * IGN_PI / 180.0), - # Aspect ratio - 1.0, - # Pose - Pose3d(0, 0, 0, 0, IGN_PI*0.5, 0)) - - self.assertEqual(frustum.pose(), Pose3d(0, 0, 0, 0, IGN_PI*0.5, 0)) - - frustum.set_pose(Pose3d(1, 2, 3, IGN_PI, 0, 0)) - - self.assertEqual(frustum.pose(), Pose3d(1, 2, 3, IGN_PI, 0, 0)) - - def test_pose_contains(self): - frustum = Frustum( - # Near distance - 1, - # Far distance - 10, - # Field of view - Angle(60 * IGN_PI / 180.0), - # Aspect ratio - 1920.0/1080.0, - # Pose - Pose3d(0, -5, 0, 0, 0, IGN_PI*0.5)) - - # Test the near clip boundary - self.assertFalse(frustum.contains(Vector3d(0, -4.01, 0))) - self.assertTrue(frustum.contains(Vector3d(0, -4.0, 0))) - - # Test a point between the near and far clip planes - self.assertTrue(frustum.contains(Vector3d(0, 1, 0))) - - # Test the far clip boundary - self.assertTrue(frustum.contains(Vector3d(0, 5, 0))) - self.assertFalse(frustum.contains(Vector3d(0, 5.001, 0))) - - # Use an offset for the test points. This makes the test more stable, and - # is also used to generate point outside the frustum. - offset = 0.00001 - - # Compute near clip points - nearTopLeft = Vector3d( - -math.tan(30 * IGN_PI / 180.0) + offset, - frustum.pose().pos().y() + frustum.near() + offset, - math.tan(30 * IGN_PI / 180.0) / frustum.aspect_ratio() - offset) - - nearTopLeftBad = Vector3d( - -math.tan(30 * IGN_PI / 180.0) - offset, - frustum.pose().pos().y() + frustum.near() - offset, - math.tan(30 * IGN_PI / 180.0) / frustum.aspect_ratio() + offset) - - nearTopRight = Vector3d( - math.tan(30 * IGN_PI / 180.0) - offset, - frustum.pose().pos().y() + frustum.near() + offset, - math.tan(30 * IGN_PI / 180.0) / frustum.aspect_ratio() - offset) - - nearTopRightBad = Vector3d( - math.tan(30 * IGN_PI / 180.0) + offset, - frustum.pose().pos().y() + frustum.near() - offset, - math.tan(30 * IGN_PI / 180.0) / frustum.aspect_ratio() + offset) - - nearBottomLeft = Vector3d( - -math.tan(30 * IGN_PI / 180.0) + offset, - frustum.pose().pos().y() + frustum.near() + offset, - -math.tan(30 * IGN_PI / 180.0) / frustum.aspect_ratio() + offset) - - nearBottomLeftBad = Vector3d( - -math.tan(30 * IGN_PI / 180.0) - offset, - frustum.pose().pos().y() + frustum.near() - offset, - -math.tan(30 * IGN_PI / 180.0) / frustum.aspect_ratio() - offset) - - nearBottomRight = Vector3d( - math.tan(30 * IGN_PI / 180.0) - offset, - frustum.pose().pos().y() + frustum.near() + offset, - -math.tan(30 * IGN_PI / 180.0) / frustum.aspect_ratio() + offset) - - nearBottomRightBad = Vector3d( - math.tan(30 * IGN_PI / 180.0) + offset, - frustum.pose().pos().y() + frustum.near() - offset, - -math.tan(30 * IGN_PI / 180.0) / frustum.aspect_ratio() - offset) - - # Test near clip corners - self.assertTrue(frustum.contains(nearTopLeft)) - self.assertFalse(frustum.contains(nearTopLeftBad)) - - self.assertTrue(frustum.contains(nearTopRight)) - self.assertFalse(frustum.contains(nearTopRightBad)) - - self.assertTrue(frustum.contains(nearBottomLeft)) - self.assertFalse(frustum.contains(nearBottomLeftBad)) - - self.assertTrue(frustum.contains(nearBottomRight)) - self.assertFalse(frustum.contains(nearBottomRightBad)) - - # Compute far clip points - farTopLeft = Vector3d( - -math.tan(30 * IGN_PI / 180.0) * frustum.far() + offset, - frustum.pose().pos().y() + frustum.far() - offset, - (math.tan(30 * IGN_PI / 180.0) * frustum.far()) / frustum.aspect_ratio() - offset) - - farTopLeftBad = Vector3d( - -math.tan(30 * IGN_PI / 180.0)*frustum.far() - offset, - frustum.pose().pos().y() + frustum.far() + offset, - (math.tan(30 * IGN_PI / 180.0 * frustum.far())) / frustum.aspect_ratio() + offset) - - farTopRight = Vector3d( - math.tan(30 * IGN_PI / 180.0)*frustum.far() - offset, - frustum.pose().pos().y() + frustum.far() - offset, - (math.tan(30 * IGN_PI / 180.0) * frustum.far()) / frustum.aspect_ratio() - offset) - - farTopRightBad = Vector3d( - math.tan(30 * IGN_PI / 180.0)*frustum.far() + offset, - frustum.pose().pos().y() + frustum.far() + offset, - (math.tan(30 * IGN_PI / 180.0) * frustum.far()) / frustum.aspect_ratio() + offset) - - farBottomLeft = Vector3d( - -math.tan(30 * IGN_PI / 180.0)*frustum.far() + offset, - frustum.pose().pos().y() + frustum.far() - offset, - (-math.tan(30 * IGN_PI / 180.0) * frustum.far()) / frustum.aspect_ratio() + offset) - - farBottomLeftBad = Vector3d( - -math.tan(30 * IGN_PI / 180.0)*frustum.far() - offset, - frustum.pose().pos().y() + frustum.far() + offset, - (-math.tan(30 * IGN_PI / 180.0) * frustum.far()) / frustum.aspect_ratio() - offset) - - farBottomRight = Vector3d( - math.tan(30 * IGN_PI / 180.0)*frustum.far() - offset, - frustum.pose().pos().y() + frustum.far() - offset, - (-math.tan(30 * IGN_PI / 180.0) * frustum.far()) / frustum.aspect_ratio() + offset) - - farBottomRightBad = Vector3d( - math.tan(30 * IGN_PI / 180.0)*frustum.far() + offset, - frustum.pose().pos().y() + frustum.far() + offset, - (-math.tan(30 * IGN_PI / 180.0) * frustum.far()) / frustum.aspect_ratio() - offset) - - # Test far clip corners - self.assertTrue(frustum.contains(farTopLeft)) - self.assertFalse(frustum.contains(farTopLeftBad)) - - self.assertTrue(frustum.contains(farTopRight)) - self.assertFalse(frustum.contains(farTopRightBad)) - - self.assertTrue(frustum.contains(farBottomLeft)) - self.assertFalse(frustum.contains(farBottomLeftBad)) - - self.assertTrue(frustum.contains(farBottomRight)) - self.assertFalse(frustum.contains(farBottomRightBad)) - - # Adjust to 45 degrees rotation - frustum.set_pose(Pose3d(1, 1, 0, 0, 0, -IGN_PI*0.25)) - self.assertTrue(frustum.contains(Vector3d(2, -1, 0))) - self.assertFalse(frustum.contains(Vector3d(0, 0, 0))) - self.assertFalse(frustum.contains(Vector3d(1, 1, 0))) - - def test_contains_aabb_no_overlap(self): - frustum = Frustum() - frustum.set_near(0.55) - frustum.set_far(2.5) - frustum.set_fov(Angle(1.05)) - frustum.set_aspect_ratio(1.8) - frustum.set_pose(Pose3d(0, 0, 2, 0, 0, 0)) - - # AxisAlignedBoxes that don't overlapp any planes - self.assertTrue(frustum.contains( - AxisAlignedBox(Vector3d(1.45, -0.05, 1.95), Vector3d(1.55, 0.05, 2.05)))) - self.assertFalse(frustum.contains( - AxisAlignedBox(Vector3d(2.55, -0.05, 1.95), Vector3d(2.65, 0.05, 2.05)))) - self.assertFalse(frustum.contains( - AxisAlignedBox(Vector3d(0.35, -0.05, 1.95), Vector3d(0.45, 0.05, 2.05)))) - self.assertFalse(frustum.contains( - AxisAlignedBox(Vector3d(1.45, -0.05, 2.55), Vector3d(1.55, 0.05, 2.65)))) - self.assertFalse(frustum.contains( - AxisAlignedBox(Vector3d(1.45, -0.05, 1.35), Vector3d(1.55, 0.05, 1.45)))) - self.assertFalse(frustum.contains( - AxisAlignedBox(Vector3d(1.45, -1.05, 1.95), Vector3d(1.55, -0.95, 2.05)))) - self.assertFalse(frustum.contains( - AxisAlignedBox(Vector3d(1.45, 0.95, 1.95), Vector3d(1.55, 1.05, 2.05)))) - - def test_contains_aabb_one_plane(self): - frustum = Frustum() - frustum.set_near(0.55) - frustum.set_far(2.5) - frustum.set_fov(Angle(1.05)) - frustum.set_aspect_ratio(1.8) - frustum.set_pose(Pose3d(0, 0, 2, 0, 0, 0)) - - # AxisAlignedBoxes overlapping one plane - self.assertTrue(frustum.contains( - AxisAlignedBox(Vector3d(2.43, -0.05, 1.95), Vector3d(2.53, 0.05, 2.05)))) - self.assertTrue(frustum.contains( - AxisAlignedBox(Vector3d(0.495, -0.05, 1.95), Vector3d(0.595, 0.05, 2.05)))) - self.assertTrue(frustum.contains( - AxisAlignedBox(Vector3d(1.45, -0.05, 2.42), Vector3d(1.55, 0.05, 2.52)))) - self.assertTrue(frustum.contains( - AxisAlignedBox(Vector3d(1.45, -0.05, 1.48), Vector3d(1.55, 0.05, 1.58)))) - self.assertTrue(frustum.contains( - AxisAlignedBox(Vector3d(1.45, -0.9, 1.95), Vector3d(1.55, -0.8, 2.05)))) - self.assertTrue(frustum.contains( - AxisAlignedBox(Vector3d(1.45, 0.8, 1.95), Vector3d(1.55, 0.9, 2.05)))) - - def test_contains_aabb_two_planes(self): - frustum = Frustum() - frustum.set_near(0.55) - frustum.set_far(2.5) - frustum.set_fov(Angle(1.05)) - frustum.set_aspect_ratio(1.8) - frustum.set_pose(Pose3d(0, 0, 2, 0, 0, 0)) - - # AxisAlignedBoxes overlapping two planes - self.assertTrue(frustum.contains( - AxisAlignedBox(Vector3d(2.42, -0.05, 2.7), Vector3d(2.52, 0.05, 2.8)))) - self.assertTrue(frustum.contains( - AxisAlignedBox(Vector3d(2.42, -0.05, 1.2), Vector3d(2.52, 0.05, 1.3)))) - self.assertTrue(frustum.contains( - AxisAlignedBox(Vector3d(2.42, -1.44, 1.95), Vector3d(2.52, -1.34, 2.05)))) - self.assertTrue(frustum.contains( - AxisAlignedBox(Vector3d(2.42, 1.34, 1.95), Vector3d(2.52, 1.44, 2.05)))) - self.assertTrue(frustum.contains( - AxisAlignedBox(Vector3d(0.495, -0.05, 2.1), Vector3d(0.595, 0.05, 2.2)))) - self.assertTrue(frustum.contains( - AxisAlignedBox(Vector3d(0.495, -0.05, 1.8), Vector3d(0.595, 0.05, 1.9)))) - self.assertTrue(frustum.contains( - AxisAlignedBox(Vector3d(0.495, 0.25, 1.95), Vector3d(0.595, 0.35, 2.05)))) - self.assertTrue(frustum.contains( - AxisAlignedBox(Vector3d(0.495, -0.35, 1.95), Vector3d(0.595, -0.25, 2.05)))) - self.assertFalse(frustum.contains( - AxisAlignedBox(Vector3d(2.48, -0.05, 2.81), Vector3d(2.58, 0.05, 2.91)))) - self.assertFalse(frustum.contains( - AxisAlignedBox(Vector3d(2.48, -0.05, 1.09), Vector3d(2.58, 0.05, 1.19)))) - self.assertFalse(frustum.contains( - AxisAlignedBox(Vector3d(2.48, -1.55, 1.95), Vector3d(2.58, -1.45, 2.05)))) - self.assertFalse(frustum.contains( - AxisAlignedBox(Vector3d(2.48, 1.45, 1.95), Vector3d(2.58, 1.55, 2.05)))) - - def test_contains_aabb_three_planes(self): - frustum = Frustum() - frustum.set_near(0.55) - frustum.set_far(2.5) - frustum.set_fov(Angle(1.05)) - frustum.set_aspect_ratio(1.8) - frustum.set_pose(Pose3d(0, 0, 2, 0, 0, 0)) - - # AxisAlignedBoxes overlapping three planes - self.assertTrue(frustum.contains( - AxisAlignedBox(Vector3d(0.495, 0.25, 2.1), Vector3d(0.595, 0.35, 2.2)))) - self.assertTrue(frustum.contains( - AxisAlignedBox(Vector3d(0.495, 0.25, 1.8), Vector3d(0.595, 0.35, 1.9)))) - self.assertTrue(frustum.contains( - AxisAlignedBox(Vector3d(0.495, -0.35, 2.1), Vector3d(0.595, -0.25, 2.2)))) - self.assertTrue(frustum.contains( - AxisAlignedBox(Vector3d(0.495, -0.35, 1.8), Vector3d(0.595, -0.25, 1.9)))) - self.assertTrue(frustum.contains( - AxisAlignedBox(Vector3d(2.42, 1.34, 2.7), Vector3d(2.52, 1.44, 2.8)))) - self.assertTrue(frustum.contains( - AxisAlignedBox(Vector3d(2.42, 1.34, 1.2), Vector3d(2.52, 1.44, 1.3)))) - self.assertTrue(frustum.contains( - AxisAlignedBox(Vector3d(2.42, -1.44, 2.7), Vector3d(2.52, -1.34, 2.8)))) - self.assertTrue(frustum.contains( - AxisAlignedBox(Vector3d(2.42, -1.44, 1.2), Vector3d(2.52, -1.34, 1.3)))) - self.assertFalse(frustum.contains( - AxisAlignedBox(Vector3d(2.48, 1.45, 2.81), Vector3d(2.58, 1.55, 2.91)))) - self.assertFalse(frustum.contains( - AxisAlignedBox(Vector3d(2.48, -1.55, 2.81), Vector3d(2.58, -1.45, 2.91)))) - self.assertFalse(frustum.contains( - AxisAlignedBox(Vector3d(2.48, 1.45, 1.09), Vector3d(2.58, 1.55, 1.19)))) - self.assertFalse(frustum.contains( - AxisAlignedBox(Vector3d(2.48, -1.55, 1.09), Vector3d(2.58, -1.45, 1.19)))) - - def test_contains_frustum(self): - frustum = Frustum() - frustum.set_near(0.55) - frustum.set_far(2.5) - frustum.set_fov(Angle(1.05)) - frustum.set_aspect_ratio(1.8) - frustum.set_pose(Pose3d(0, 0, 2, 0, 0, 0)) - - self.assertTrue(frustum.contains( - AxisAlignedBox(Vector3d(-100, -100, -100), Vector3d(100, 100, 100)))) - - def test_AABB_frustum_edge_overlap(self): - # This test case has the top of an AABB overlap a frustum, but all the - # corners of AABB fall outside the frustum. - - ybounds = 10 - - frustum = Frustum() - frustum.set_near(0.55) - frustum.set_far(2.5) - frustum.set_fov(Angle(1.05)) - frustum.set_aspect_ratio(1.8) - frustum.set_pose(Pose3d(0, 0, 2, 0, 0, 0)) - - self.assertTrue(frustum.contains( - AxisAlignedBox(Vector3d(1, -ybounds, 0), Vector3d(2, ybounds, 2)))) - - def test_AABBB_f_wall(self): - # Frustum contains at a large but thin wall - frustum = Frustum() - frustum.set_near(0.55) - frustum.set_far(2.5) - frustum.set_fov(Angle(1.05)) - frustum.set_aspect_ratio(1.8) - frustum.set_pose(Pose3d(0, 0, 2, 0, 0, 0)) - - self.assertTrue(frustum.contains( - AxisAlignedBox(Vector3d(1, -10, -10), Vector3d(2, 10, 10)))) - self.assertTrue(frustum.contains( - AxisAlignedBox(Vector3d(-10, 1, -10), Vector3d(10, 1.1, 10)))) - self.assertTrue(frustum.contains( - AxisAlignedBox(Vector3d(-10, -10, 1.95), Vector3d(10, 10, 2.05)))) - - -if __name__ == '__main__': - unittest.main() diff --git a/src/python/GaussMarkovProcess.i b/src/python/GaussMarkovProcess.i deleted file mode 100644 index f0914e17c..000000000 --- a/src/python/GaussMarkovProcess.i +++ /dev/null @@ -1,44 +0,0 @@ -/* - * Copyright (C) 2020 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. - * -*/ - -%module gaussMarkovProcess -%{ -#include -%} - -namespace ignition -{ - namespace math - { - class GaussMarkovProcess - { - %rename("%(undercase)s", %$isfunction, %$ismember, %$not %$isconstructor) ""; - public: GaussMarkovProcess(); - public: GaussMarkovProcess(double _start, double _theta, double _mu, - double _sigma); - public: ~GaussMarkovProcess(); - public: void Set(double _start, double _theta, double _mu, double _sigma); - public: double Start() const; - public: double Value() const; - public: double Theta() const; - public: double Mu() const; - public: double Sigma() const; - public: void Reset(); - public: double Update(double _dt); - }; - } -} diff --git a/src/python/Inertial_TEST.py b/src/python/Inertial_TEST.py deleted file mode 100644 index 289a556d8..000000000 --- a/src/python/Inertial_TEST.py +++ /dev/null @@ -1,495 +0,0 @@ -# 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. - -import copy -import math -import unittest - -from ignition.math import IGN_PI, IGN_PI_2, IGN_PI_4, Inertiald, Quaterniond, Pose3d, Matrix3d, MassMatrix3d, Vector3d - - -class TestInertial(unittest.TestCase): - - def CompareModuloPi(self, _q1, _q2, _tol=1e-6): - rotErrorEuler = (_q1.inverse() * _q2).euler() - self.assertAlmostEqual(math.sin(rotErrorEuler.x()), 0.0, delta=_tol) - self.assertAlmostEqual(math.sin(rotErrorEuler.y()), 0.0, delta=_tol) - self.assertAlmostEqual(math.sin(rotErrorEuler.z()), 0.0, delta=_tol) - - def test_constructor(self): - inertial = Inertiald() - self.assertEqual(inertial.pose(), Pose3d.ZERO) - self.assertEqual(inertial.mass_matrix(), MassMatrix3d()) - self.assertEqual(inertial.moi(), Matrix3d.ZERO) - - def test_constructor_default_values(self): - inertial = Inertiald(MassMatrix3d(), Pose3d.ZERO) - self.assertEqual(inertial, Inertiald()) - self.assertEqual(inertial, Inertiald(inertial)) - - def test_constructor_non_default_values(self): - mass = 5.0 - Ixxyyzz = Vector3d(2.0, 3.0, 4.0) - Ixyxzyz = Vector3d(0.2, 0.3, 0.4) - m = MassMatrix3d(mass, Ixxyyzz, Ixyxzyz) - self.assertTrue(m.is_positive()) - self.assertTrue(m.is_valid()) - pose = Pose3d(1, 2, 3, IGN_PI/6, 0, 0) - inertial = Inertiald(m, pose) - - # Should not match simple constructor - self.assertNotEqual(inertial, Inertiald()) - - # Should match with copy constructor - self.assertEqual(inertial, Inertiald(inertial)) - - # Test accessors - self.assertEqual(inertial.mass_matrix(), m) - self.assertEqual(inertial.pose(), pose) - self.assertTrue(inertial.mass_matrix().is_positive()) - self.assertTrue(inertial.mass_matrix().is_valid()) - - # Test assignment operator - inertial2 = Inertiald() - self.assertNotEqual(inertial, inertial2) - inertial2 = inertial - self.assertEqual(inertial, inertial2) - - def test_set_mass_matrix(self): - inertial = Inertiald() - m = MassMatrix3d() - - # This will be true because the default mass of zero is considered valid - self.assertTrue(inertial.set_mass_matrix(m, 0)) - # Set the mass to a negative value, and set_mass_matrix should complain. - m.set_mass(-1) - self.assertFalse(inertial.set_mass_matrix(m, 0)) - - def test_setters(self): - mass = 5.0 - Ixxyyzz = Vector3d(2.0, 3.0, 4.0) - Ixyxzyz = Vector3d(0.2, 0.3, 0.4) - m = MassMatrix3d(mass, Ixxyyzz, Ixyxzyz) - self.assertTrue(m.is_positive()) - self.assertTrue(m.is_valid()) - pose = Pose3d(1, 2, 3, IGN_PI/6, 0, 0) - inertial = Inertiald() - - # Initially valid - self.assertTrue(inertial.set_pose(pose)) - - # Valid once valid mass matrix is set - self.assertTrue(inertial.set_mass_matrix(m)) - - # Verify values - self.assertEqual(inertial.mass_matrix(), m) - self.assertEqual(inertial.pose(), pose) - - # Invalid again if an invalid inertia is set - mInvalid = MassMatrix3d(-1, Ixxyyzz, Ixyxzyz) - self.assertFalse(inertial.set_mass_matrix(mInvalid)) - - def test_moi_diagonal(self): - mass = 12.0 - Ixxyyzz = Vector3d(2.0, 3.0, 4.0) - Ixyxzyz = Vector3d(0, 0, 0) - m = MassMatrix3d(mass, Ixxyyzz, Ixyxzyz) - self.assertTrue(m.is_positive()) - self.assertTrue(m.is_valid()) - - # no rotation, expect MOI's to match - pose = Pose3d(0, 0, 0, 0, 0, 0) - inertial = Inertiald(m, pose) - self.assertEqual(inertial.moi(), m.moi()) - - # 90 deg rotation about X axis, expect different MOI - pose = Pose3d(0, 0, 0, IGN_PI_2, 0, 0) - expectedMOI = Matrix3d(2, 0, 0, 0, 4, 0, 0, 0, 3) - inertial = Inertiald(m, pose) - self.assertNotEqual(inertial.moi(), m.moi()) - self.assertEqual(inertial.moi(), expectedMOI) - - # 90 deg rotation about Y axis, expect different MOI - pose = Pose3d(0, 0, 0, 0, IGN_PI_2, 0) - expectedMOI = Matrix3d(4, 0, 0, 0, 3, 0, 0, 0, 2) - inertial = Inertiald(m, pose) - self.assertNotEqual(inertial.moi(), m.moi()) - self.assertEqual(inertial.moi(), expectedMOI) - - # 90 deg rotation about Z axis, expect different MOI - pose = Pose3d(0, 0, 0, 0, 0, IGN_PI_2) - expectedMOI = Matrix3d(3, 0, 0, 0, 2, 0, 0, 0, 4) - inertial = Inertiald(m, pose) - self.assertNotEqual(inertial.moi(), m.moi()) - self.assertEqual(inertial.moi(), expectedMOI) - - # 45 deg rotation about Z axis, expect different MOI - pose = Pose3d(0, 0, 0, 0, 0, IGN_PI_4) - expectedMOI = Matrix3d(2.5, -0.5, 0, -0.5, 2.5, 0, 0, 0, 4) - inertial = Inertiald(m, pose) - self.assertNotEqual(inertial.moi(), m.moi()) - self.assertEqual(inertial.moi(), expectedMOI) - - # check with a second MassMatrix3 instance - # that has the same base frame MOI but no pose rotation - m2 = MassMatrix3d() - self.assertTrue(m2.set_mass(mass)) - self.assertTrue(m2.set_moi(expectedMOI)) - self.assertEqual(inertial.moi(), m2.moi()) - # There are multiple correct rotations due to symmetry - self.CompareModuloPi(m2.principal_axes_offset(), pose.rot()) - - def SetRotation(self, _mass, _ixxyyzz, _ixyxzyz, _unique=True): - m = MassMatrix3d(_mass, _ixxyyzz, _ixyxzyz) - self.assertTrue(m.is_positive()) - self.assertTrue(m.is_valid()) - - pose = Pose3d(Vector3d.ZERO, Quaterniond.IDENTITY) - inertialRef = Inertiald(m, pose) - moi = inertialRef.moi() - - rotations = [ - Quaterniond.IDENTITY, - Quaterniond(IGN_PI, 0, 0), - Quaterniond(0, IGN_PI, 0), - Quaterniond(0, 0, IGN_PI), - Quaterniond(IGN_PI_2, 0, 0), - Quaterniond(0, IGN_PI_2, 0), - Quaterniond(0, 0, IGN_PI_2), - Quaterniond(IGN_PI_4, 0, 0), - Quaterniond(0, IGN_PI_4, 0), - Quaterniond(0, 0, IGN_PI_4), - Quaterniond(IGN_PI/6, 0, 0), - Quaterniond(0, IGN_PI/6, 0), - Quaterniond(0, 0, IGN_PI/6), - Quaterniond(0.1, 0.2, 0.3), - Quaterniond(-0.1, 0.2, -0.3), - Quaterniond(0.4, 0.2, 0.5), - Quaterniond(-0.1, 0.7, -0.7)] - for rot in rotations: - inertial = Inertiald(m, pose) - tol = -1e-6 - self.assertTrue(inertial.set_mass_matrix_rotation(rot, tol)) - self.assertEqual(moi, inertial.moi()) - if (_unique): - self.CompareModuloPi(rot, inertial.mass_matrix().principal_axes_offset(tol)) - - self.assertTrue(inertial.set_inertial_rotation(rot)) - self.assertEqual(rot, inertial.pose().rot()) - self.assertEqual(moi, inertial.moi()) - - inertial = Inertiald(m, pose) - - self.assertTrue(inertial.set_inertial_rotation(rot)) - self.assertEqual(rot, inertial.pose().rot()) - self.assertEqual(moi, inertial.moi()) - - tol = -1e-6 - self.assertTrue(inertial.set_mass_matrix_rotation(rot, tol)) - self.assertEqual(moi, inertial.moi()) - if (_unique): - self.CompareModuloPi(rot, inertial.mass_matrix().principal_axes_offset(tol)) - - def test_set_rotation_unique_diagonal(self): - self.SetRotation(12, Vector3d(2, 3, 4), Vector3d.ZERO) - self.SetRotation(12, Vector3d(3, 2, 4), Vector3d.ZERO) - self.SetRotation(12, Vector3d(2, 4, 3), Vector3d.ZERO) - self.SetRotation(12, Vector3d(3, 4, 2), Vector3d.ZERO) - self.SetRotation(12, Vector3d(4, 2, 3), Vector3d.ZERO) - self.SetRotation(12, Vector3d(4, 3, 2), Vector3d.ZERO) - - def test_set_rotation_unique_non_diagonal(self): - self.SetRotation(12, Vector3d(2, 3, 4), Vector3d(0.3, 0.2, 0.1)) - - def test_set_rotation_non_unique_diagonal(self): - self.SetRotation(12, Vector3d(2, 2, 2), Vector3d.ZERO, False) - self.SetRotation(12, Vector3d(2, 2, 3), Vector3d.ZERO, False) - self.SetRotation(12, Vector3d(2, 3, 2), Vector3d.ZERO, False) - self.SetRotation(12, Vector3d(3, 2, 2), Vector3d.ZERO, False) - self.SetRotation(12, Vector3d(2, 3, 3), Vector3d.ZERO, False) - self.SetRotation(12, Vector3d(3, 2, 3), Vector3d.ZERO, False) - self.SetRotation(12, Vector3d(3, 3, 2), Vector3d.ZERO, False) - - def test_set_rotation_non_unique_non_diagonal(self): - self.SetRotation(12, Vector3d(4, 4, 3), Vector3d(-1, 0, 0), False) - self.SetRotation(12, Vector3d(4, 3, 4), Vector3d(0, -1, 0), False) - self.SetRotation(12, Vector3d(3, 4, 4), Vector3d(0, 0, -1), False) - self.SetRotation(12, Vector3d(4, 4, 5), Vector3d(-1, 0, 0), False) - self.SetRotation(12, Vector3d(5, 4, 4), Vector3d(0, 0, -1), False) - self.SetRotation(12, Vector3d(5.5, 4.125, 4.375), - Vector3d(-math.sqrt(3), 3.0, -math.sqrt(3)/2)*0.25, False) - self.SetRotation(12, Vector3d(4.125, 5.5, 4.375), - Vector3d(-math.sqrt(3), -math.sqrt(3)/2, 3.0)*0.25, False) - - # test for diagonalizing MassMatrix - # verify MOI is conserved - # and that off-diagonal terms are zero - def Diagonalize(self, _mass, _ixxyyzz, _ixyxzyz): - m = MassMatrix3d(_mass, _ixxyyzz, _ixyxzyz) - self.assertTrue(m.is_positive()) - self.assertTrue(m.is_valid()) - - pose = Pose3d(Vector3d.ZERO, Quaterniond.IDENTITY) - inertial = Inertiald(m, pose) - moi = inertial.moi() - - self.assertTrue(inertial.set_mass_matrix_rotation(Quaterniond.IDENTITY)) - self.assertEqual(moi, inertial.moi()) - self.assertEqual(inertial.mass_matrix().off_diagonal_moments(), Vector3d.ZERO) - - # try again with negative tolerance - self.assertTrue(inertial.set_mass_matrix_rotation(Quaterniond.IDENTITY, -1e-6)) - self.assertEqual(moi, inertial.moi()) - self.assertEqual(inertial.mass_matrix().off_diagonal_moments(), Vector3d.ZERO) - - def test_diagonalize(self): - self.Diagonalize(12, Vector3d(2, 3, 4), Vector3d.ZERO) - self.Diagonalize(12, Vector3d(3, 2, 4), Vector3d.ZERO) - self.Diagonalize(12, Vector3d(2, 4, 3), Vector3d.ZERO) - self.Diagonalize(12, Vector3d(3, 4, 2), Vector3d.ZERO) - self.Diagonalize(12, Vector3d(4, 2, 3), Vector3d.ZERO) - self.Diagonalize(12, Vector3d(4, 3, 2), Vector3d.ZERO) - self.Diagonalize(12, Vector3d(2, 3, 4), Vector3d(0.3, 0.2, 0.1)) - self.Diagonalize(12, Vector3d(2, 2, 2), Vector3d.ZERO) - self.Diagonalize(12, Vector3d(2, 2, 3), Vector3d.ZERO) - self.Diagonalize(12, Vector3d(2, 3, 2), Vector3d.ZERO) - self.Diagonalize(12, Vector3d(3, 2, 2), Vector3d.ZERO) - self.Diagonalize(12, Vector3d(2, 3, 3), Vector3d.ZERO) - self.Diagonalize(12, Vector3d(3, 2, 3), Vector3d.ZERO) - self.Diagonalize(12, Vector3d(3, 3, 2), Vector3d.ZERO) - self.Diagonalize(12, Vector3d(4, 4, 3), Vector3d(-1, 0, 0)) - self.Diagonalize(12, Vector3d(4, 3, 4), Vector3d(0, -1, 0)) - self.Diagonalize(12, Vector3d(3, 4, 4), Vector3d(0, 0, -1)) - self.Diagonalize(12, Vector3d(4, 4, 5), Vector3d(-1, 0, 0)) - self.Diagonalize(12, Vector3d(5, 4, 4), Vector3d(0, 0, -1)) - self.Diagonalize(12, Vector3d(5.5, 4.125, 4.375), - Vector3d(-math.sqrt(3), 3.0, -math.sqrt(3)/2)*0.25) - self.Diagonalize(12, Vector3d(4.125, 5.5, 4.375), - Vector3d(-math.sqrt(3), -math.sqrt(3)/2, 3.0)*0.25) - - def test_addition(self): - # Add two half-cubes together - mass = 12.0 - size = Vector3d(1, 1, 1) - cubeMM3 = MassMatrix3d() - self.assertTrue(cubeMM3.set_from_box(mass, size)) - cube = Inertiald(cubeMM3, Pose3d.ZERO) - half = MassMatrix3d() - self.assertTrue(half.set_from_box(0.5*mass, Vector3d(0.5, 1, 1))) - left = Inertiald(half, Pose3d(-0.25, 0, 0, 0, 0, 0)) - right = Inertiald(half, Pose3d(0.25, 0, 0, 0, 0, 0)) - self.assertEqual(cube, left + right) - self.assertEqual(cube, right + left) - - # test += operator - tmp = left - tmp += right - self.assertTrue(cube == tmp) - - left = Inertiald(half, Pose3d(-0.25, 0, 0, 0, 0, 0)) - right = Inertiald(half, Pose3d(0.25, 0, 0, 0, 0, 0)) - tmp = copy.copy(right) - tmp += left - self.assertTrue(cube == tmp) - - # Test equivalent_box - left = Inertiald(half, Pose3d(-0.25, 0, 0, 0, 0, 0)) - right = Inertiald(half, Pose3d(0.25, 0, 0, 0, 0, 0)) - size2 = Vector3d() - rot2 = Quaterniond() - self.assertTrue((left + right).mass_matrix().equivalent_box(size2, rot2)) - self.assertEqual(size, size2) - self.assertEqual(rot2, Quaterniond.IDENTITY) - - size2 = Vector3d() - rot2 = Quaterniond() - self.assertTrue((right + left).mass_matrix().equivalent_box(size2, rot2)) - self.assertEqual(size, size2) - self.assertEqual(rot2, Quaterniond.IDENTITY) - - # Add two rotated half-cubes together - mass = 12.0 - size = Vector3d(1, 1, 1) - cubeMM3 = MassMatrix3d() - self.assertTrue(cubeMM3.set_from_box(mass, size)) - cube = Inertiald(cubeMM3, Pose3d(0, 0, 0, IGN_PI_4, 0, 0)) - - half = MassMatrix3d() - self.assertTrue(half.set_from_box(0.5*mass, Vector3d(0.5, 1, 1))) - left = Inertiald(half, Pose3d(-0.25, 0, 0, IGN_PI_4, 0, 0)) - right = Inertiald(half, Pose3d(0.25, 0, 0, IGN_PI_4, 0, 0)) - - # objects won't match exactly - # since inertia matrices will all be in base frame - # but mass, center of mass, and base-frame MOI should match - self.assertNotEqual(cube, left + right) - self.assertNotEqual(cube, right + left) - self.assertEqual(cubeMM3.mass(), (left + right).mass_matrix().mass()) - self.assertEqual(cubeMM3.mass(), (right + left).mass_matrix().mass()) - self.assertEqual(cube.pose().pos(), (left + right).pose().pos()) - self.assertEqual(cube.pose().pos(), (right + left).pose().pos()) - self.assertEqual(cube.moi(), (left + right).moi()) - self.assertEqual(cube.moi(), (right + left).moi()) - - mass = 12.0 - size = Vector3d(1, 1, 1) - cubeMM3 = MassMatrix3d() - self.assertTrue(cubeMM3.set_from_box(mass, size)) - addedCube = Inertiald( - Inertiald(cubeMM3, Pose3d(-0.5, -0.5, -0.5, 0, 0, 0)) + - Inertiald(cubeMM3, Pose3d(-0.5, 0.5, -0.5, 0, 0, 0)) + - Inertiald(cubeMM3, Pose3d(0.5, -0.5, -0.5, 0, 0, 0)) + - Inertiald(cubeMM3, Pose3d(0.5, 0.5, -0.5, 0, 0, 0)) + - Inertiald(cubeMM3, Pose3d(-0.5, -0.5, 0.5, 0, 0, 0)) + - Inertiald(cubeMM3, Pose3d(-0.5, 0.5, 0.5, 0, 0, 0)) + - Inertiald(cubeMM3, Pose3d(0.5, -0.5, 0.5, 0, 0, 0)) + - Inertiald(cubeMM3, Pose3d(0.5, 0.5, 0.5, 0, 0, 0))) - - trueCubeMM3 = MassMatrix3d() - self.assertTrue(trueCubeMM3.set_from_box(mass * 8, size * 2)) - self.assertEqual(addedCube, Inertiald(trueCubeMM3, Pose3d.ZERO)) - - # Add eight rotated cubes together into larger cube - mass = 12.0 - size = Vector3d(1, 1, 1) - cubeMM3 = MassMatrix3d() - self.assertTrue(cubeMM3.set_from_box(mass, size)) - addedCube = Inertiald( - Inertiald(cubeMM3, Pose3d(-0.5, -0.5, -0.5, 0, 0, 0)) + - Inertiald(cubeMM3, Pose3d(-0.5, 0.5, -0.5, IGN_PI_2, 0, 0)) + - Inertiald(cubeMM3, Pose3d(0.5, -0.5, -0.5, 0, IGN_PI_2, 0)) + - Inertiald(cubeMM3, Pose3d(0.5, 0.5, -0.5, 0, 0, IGN_PI_2)) + - Inertiald(cubeMM3, Pose3d(-0.5, -0.5, 0.5, IGN_PI, 0, 0)) + - Inertiald(cubeMM3, Pose3d(-0.5, 0.5, 0.5, 0, IGN_PI, 0)) + - Inertiald(cubeMM3, Pose3d(0.5, -0.5, 0.5, 0, 0, IGN_PI)) + - Inertiald(cubeMM3, Pose3d(0.5, 0.5, 0.5, 0, 0, 0))) - - trueCubeMM3 = MassMatrix3d() - self.assertTrue(trueCubeMM3.set_from_box(mass * 8, size * 2)) - self.assertEqual(addedCube, Inertiald(trueCubeMM3, Pose3d.ZERO)) - - # Add two cubes with diagonal corners touching at one point - # ┌---------┐ - # | | - # | | - # | | - # | | - # ┌---------+---------┘ - # | | - # | | - # | | - # | | - # └---------┘ - - # properties of each cube to be added - # side length: 1 - # mass: 6 - # diagonal moment of inertia values: 1 - # off-diagonal moment of inertia values: 0 - mass = 6.0 - size = Vector3d(1, 1, 1) - cubeMM3 = MassMatrix3d() - self.assertTrue(cubeMM3.set_from_box(mass, size)) - self.assertEqual( - Vector3d.ONE, - cubeMM3.diagonal_moments()) - self.assertEqual( - Vector3d.ZERO, - cubeMM3.off_diagonal_moments()) - - diagonalCubes = Inertiald( - Inertiald(cubeMM3, Pose3d(-0.5, -0.5, -0.5, 0, 0, 0)) + - Inertiald(cubeMM3, Pose3d(0.5, 0.5, 0.5, 0, 0, 0))) - - # lumped mass = 6 + 6 = 12 - # lumped center of mass at (0, 0, 0) - # lumped Moment of inertia: - # for each cube - # [ 1 0 0 ] [ 0.5^2 + 0.5^2 -0.5*0.5 -0.5*0.5 ] - # [ 0 1 0 ] + 6 * [ -0.5*0.5 0.5^2 + 0.5^2 -0.5*0.5 ] - # [ 0 0 1 ] [ -0.5*0.5 -0.5*0.5 0.5^2 + 0.5^2 ] - # - # [ 1 0 0 ] [ 0.5 -0.25 -0.25 ] - # [ 0 1 0 ] + 6 * [ -0.25 0.5 -0.25 ] - # [ 0 0 1 ] [ -0.25 -0.25 0.5 ] - # - # [ 1 0 0 ] [ 3.0 -1.5 -1.5 ] - # [ 0 1 0 ] + [ -1.5 3.0 -1.5 ] - # [ 0 0 1 ] [ -1.5 -1.5 3.0 ] - # - # [ 4.0 -1.5 -1.5 ] - # [ -1.5 4.0 -1.5 ] - # [ -1.5 -1.5 4.0 ] - # - # then it to account for both cubes - self.assertEqual(Pose3d.ZERO, diagonalCubes.pose()) - self.assertEqual(mass * 2.0, diagonalCubes.mass_matrix().mass()) - self.assertEqual( - Vector3d(8, 8, 8), - diagonalCubes.mass_matrix().diagonal_moments()) - self.assertEqual( - Vector3d(-3, -3, -3), - diagonalCubes.mass_matrix().off_diagonal_moments()) - - def test_addition_invalid(self): - # inertias all zero - m0 = MassMatrix3d(0.0, Vector3d.ZERO, Vector3d.ZERO) - self.assertFalse(m0.is_positive()) - self.assertTrue(m0.is_near_positive()) - self.assertTrue(m0.is_valid()) - - # both inertials with zero mass - left = Inertiald(m0, Pose3d(-1, 0, 0, 0, 0, 0)) - right = Inertiald(m0, Pose3d(1, 0, 0, 0, 0, 0)) - - # expect sum to equal left argument - self.assertEqual(left, left + right) - self.assertEqual(right, right + left) - - tmp = left - tmp += right - self.assertEqual(tmp, left) - - tmp = right - tmp += left - self.assertEqual(tmp, right) - - # one inertial with zero inertias should not affect the sum - m = MassMatrix3d(12.0, Vector3d(2, 3, 4), Vector3d(0.1, 0.2, 0.3)) - self.assertTrue(m.is_positive()) - self.assertTrue(m.is_valid()) - - i = Inertiald(m, Pose3d(-1, 0, 0, 0, 0, 0)) - i0 = Inertiald(m0, Pose3d(1, 0, 0, 0, 0, 0)) - - # expect i0 to not affect the sum - self.assertEqual(i, i + i0) - self.assertEqual(i, i0 + i) - - tmp = i - tmp += i0 - self.assertEqual(tmp, i) - - tmp = i0 - tmp += i - self.assertEqual(tmp, i) - - self.assertTrue((i + i0).mass_matrix().is_positive()) - self.assertTrue((i0 + i).mass_matrix().is_positive()) - self.assertTrue((i + i0).mass_matrix().is_valid()) - self.assertTrue((i0 + i).mass_matrix().is_valid()) - - -if __name__ == '__main__': - unittest.main() diff --git a/src/python/OrientedBox_TEST.py b/src/python/OrientedBox_TEST.py deleted file mode 100644 index 5eb671383..000000000 --- a/src/python/OrientedBox_TEST.py +++ /dev/null @@ -1,310 +0,0 @@ -# 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. - -import unittest - -from ignition.math import IGN_PI, OrientedBoxd, MassMatrix3d, Material, Pose3d, Vector3d - -g_tolerance = 1e-6 - - -class TestOrientedBox(unittest.TestCase): - - def test_empty_constructor_new(self): - box = OrientedBoxd() - - self.assertEqual(box.size(), Vector3d.ZERO) - self.assertEqual(box.pose(), Pose3d.ZERO) - # self.assertEqual(Material(), box.material()) - - def test_size_only_constructor(self): - box = OrientedBoxd(Vector3d(1, 2, 3)) - self.assertEqual(box.size(), Vector3d(1, 2, 3)) - self.assertEqual(box.pose(), Pose3d.ZERO) - - def test_negative_size_constructor(self): - box = OrientedBoxd(Vector3d(-1, 0, -3)) - self.assertEqual(box.size(), Vector3d(1, 0, 3)) - - def test_size_pose_constructor(self): - box = OrientedBoxd(Vector3d(1, 2, 3), Pose3d(-1, -2, -3, 0, 1, 2)) - self.assertEqual(box.size(), Vector3d(1, 2, 3)) - self.assertEqual(box.pose(), Pose3d(-1, -2, -3, 0, 1, 2)) - - def test_copy_constructor(self): - - box1 = OrientedBoxd(Vector3d(0.1, 0.2, 0.3), - Pose3d(-0.1, -0.2, 0.0, 1.1, 1.2, 1.3)) - box2 = OrientedBoxd(box1) - - self.assertEqual(box2.size(), Vector3d(0.1, 0.2, 0.3)) - self.assertEqual(box2.pose(), Pose3d(-0.1, -0.2, 0.0, 1.1, 1.2, 1.3)) - - def test_length(self): - box = OrientedBoxd(Vector3d(0.1, -2.1, 0.0)) - self.assertEqual(box.x_length(), 0.1) - self.assertEqual(box.y_length(), 2.1) - self.assertEqual(box.z_length(), 0.0) - - def test_operator_equal(self): - box = OrientedBoxd(Vector3d(1, 1, 1)) - box2 = OrientedBoxd(Vector3d(1, 1, 1), - Pose3d(1, 2, 3, 4, 5, 6)) - box3 = OrientedBoxd(Vector3d(0, 0, 0), - Pose3d(1, 2, 3, 4, 5, 6)) - self.assertEqual(box, OrientedBoxd(Vector3d(1, 1, 1))) - self.assertNotEqual(box, box2) - self.assertNotEqual(box3, box) - - def test_constains_zero_box(self): - box = OrientedBoxd() - - self.assertTrue(box.contains(Vector3d(0, 0, 0))) - self.assertFalse(box.contains(Vector3d(0, 0, 0.0001))) - - def test_constains_zero_pose(self): - box = OrientedBoxd(Vector3d(1, 2, 3)) - - # Vertices - self.assertTrue(box.contains(Vector3d(-0.5, -1.0, -1.5))) - self.assertTrue(box.contains(Vector3d(-0.5, -1.0, +1.5))) - self.assertTrue(box.contains(Vector3d(-0.5, +1.0, -1.5))) - self.assertTrue(box.contains(Vector3d(-0.5, +1.0, +1.5))) - - self.assertTrue(box.contains(Vector3d(+0.5, -1.0, -1.5))) - self.assertTrue(box.contains(Vector3d(+0.5, -1.0, +1.5))) - self.assertTrue(box.contains(Vector3d(+0.5, +1.0, -1.5))) - self.assertTrue(box.contains(Vector3d(+0.5, +1.0, +1.5))) - - # Edges - self.assertTrue(box.contains(Vector3d(0.0, -1.0, -1.5))) - self.assertTrue(box.contains(Vector3d(0.0, -1.0, +1.5))) - self.assertTrue(box.contains(Vector3d(0.0, +1.0, -1.5))) - self.assertTrue(box.contains(Vector3d(0.0, +1.0, +1.5))) - - self.assertTrue(box.contains(Vector3d(-0.5, -1.0, 0.0))) - self.assertTrue(box.contains(Vector3d(-0.5, +1.0, 0.0))) - self.assertTrue(box.contains(Vector3d(+0.5, -1.0, 0.0))) - self.assertTrue(box.contains(Vector3d(+0.5, +1.0, 0.0))) - - self.assertTrue(box.contains(Vector3d(-0.5, 0.0, -1.5))) - self.assertTrue(box.contains(Vector3d(-0.5, 0.0, +1.5))) - self.assertTrue(box.contains(Vector3d(+0.5, 0.0, -1.5))) - self.assertTrue(box.contains(Vector3d(+0.5, 0.0, +1.5))) - - # Inside - self.assertTrue(box.contains(Vector3d(-0.5+g_tolerance, - -1.0+g_tolerance, - -1.5+g_tolerance))) - self.assertTrue(box.contains(Vector3d(-0.5+g_tolerance, - -1.0+g_tolerance, - +1.5-g_tolerance))) - self.assertTrue(box.contains(Vector3d(-0.5+g_tolerance, - +1.0-g_tolerance, - -1.5+g_tolerance))) - self.assertTrue(box.contains(Vector3d(-0.5+g_tolerance, - +1.0-g_tolerance, - +1.5-g_tolerance))) - - self.assertTrue(box.contains(Vector3d(+0.5-g_tolerance, - -1.0+g_tolerance, - -1.5+g_tolerance))) - self.assertTrue(box.contains(Vector3d(+0.5-g_tolerance, - -1.0+g_tolerance, - +1.5-g_tolerance))) - self.assertTrue(box.contains(Vector3d(+0.5-g_tolerance, - +1.0-g_tolerance, - -1.5+g_tolerance))) - self.assertTrue(box.contains(Vector3d(+0.5-g_tolerance, - +1.0-g_tolerance, - +1.5-g_tolerance))) - - # Outside - self.assertFalse(box.contains(Vector3d(-0.5-g_tolerance, - -1.0-g_tolerance, - -1.5-g_tolerance))) - self.assertFalse(box.contains(Vector3d(-0.5-g_tolerance, - -1.0-g_tolerance, - +1.5+g_tolerance))) - self.assertFalse(box.contains(Vector3d(-0.5-g_tolerance, - +1.0+g_tolerance, - -1.5-g_tolerance))) - self.assertFalse(box.contains(Vector3d(-0.5-g_tolerance, - +1.0+g_tolerance, - +1.5+g_tolerance))) - - self.assertFalse(box.contains(Vector3d(+0.5-g_tolerance, - -1.0-g_tolerance, - -1.5-g_tolerance))) - self.assertFalse(box.contains(Vector3d(+0.5-g_tolerance, - -1.0-g_tolerance, - +1.5+g_tolerance))) - self.assertFalse(box.contains(Vector3d(+0.5-g_tolerance, - +1.0+g_tolerance, - -1.5-g_tolerance))) - self.assertFalse(box.contains(Vector3d(+0.5-g_tolerance, - +1.0+g_tolerance, - +1.5+g_tolerance))) - - def test_contains_oriented_position(self): - - box = OrientedBoxd(Vector3d(1, 2, 3), - Pose3d(10, 20, 30, 0, 0, 0)) - - # Vertices - self.assertTrue(box.contains(Vector3d(10-0.5, 20-1.0, 30-1.5))) - self.assertTrue(box.contains(Vector3d(10-0.5, 20-1.0, 30+1.5))) - self.assertTrue(box.contains(Vector3d(10-0.5, 20+1.0, 30-1.5))) - self.assertTrue(box.contains(Vector3d(10-0.5, 20+1.0, 30+1.5))) - - self.assertTrue(box.contains(Vector3d(10+0.5, 20-1.0, 30-1.5))) - self.assertTrue(box.contains(Vector3d(10+0.5, 20-1.0, 30+1.5))) - self.assertTrue(box.contains(Vector3d(10+0.5, 20+1.0, 30-1.5))) - self.assertTrue(box.contains(Vector3d(10+0.5, 20+1.0, 30+1.5))) - - # Edges - self.assertTrue(box.contains(Vector3d(10.0, 20-1.0, 30-1.5))) - self.assertTrue(box.contains(Vector3d(10.0, 20-1.0, 30+1.5))) - self.assertTrue(box.contains(Vector3d(10.0, 20+1.0, 30-1.5))) - self.assertTrue(box.contains(Vector3d(10.0, 20+1.0, 30+1.5))) - - self.assertTrue(box.contains(Vector3d(10-0.5, 20-1.0, 30.0))) - self.assertTrue(box.contains(Vector3d(10-0.5, 20+1.0, 30.0))) - self.assertTrue(box.contains(Vector3d(10+0.5, 20-1.0, 30.0))) - self.assertTrue(box.contains(Vector3d(10+0.5, 20+1.0, 30.0))) - - self.assertTrue(box.contains(Vector3d(10-0.5, 20.0, 30-1.5))) - self.assertTrue(box.contains(Vector3d(10-0.5, 20.0, 30+1.5))) - self.assertTrue(box.contains(Vector3d(10+0.5, 20.0, 30-1.5))) - self.assertTrue(box.contains(Vector3d(10+0.5, 20.0, 30+1.5))) - - # Inside - self.assertTrue(box.contains(Vector3d(10, 20, 30))) - self.assertTrue(box.contains(Vector3d(10-0.25, 20-0.5, 30-0.75))) - self.assertTrue(box.contains(Vector3d(10+0.25, 20+0.5, 30+0.75))) - - # Outside - self.assertFalse(box.contains(Vector3d(10-1.0, 20-1.0, 30-1.5))) - self.assertFalse(box.contains(Vector3d(10-0.5, 20-2.0, 30-1.5))) - self.assertFalse(box.contains(Vector3d(10-0.5, 20-1.0, 30-2.0))) - - self.assertFalse(box.contains(Vector3d(10+1.0, 20+1.0, 30+1.5))) - self.assertFalse(box.contains(Vector3d(10+0.5, 20+2.0, 30+1.5))) - self.assertFalse(box.contains(Vector3d(10+0.5, 20+1.0, 30+2.0))) - - def test_contains_oriented_rotation(self): - # Rotate PI/2 about +x: swap Z and Y - box = OrientedBoxd(Vector3d(1, 2, 3), Pose3d(0, 0, 0, IGN_PI*0.5, 0, 0)) - - # Doesn't contain non-rotated vertices - self.assertFalse(box.contains(Vector3d(-0.5, -1.0, -1.5))) - self.assertFalse(box.contains(Vector3d(-0.5, -1.0, +1.5))) - self.assertFalse(box.contains(Vector3d(-0.5, +1.0, -1.5))) - self.assertFalse(box.contains(Vector3d(-0.5, +1.0, +1.5))) - - self.assertFalse(box.contains(Vector3d(+0.5, -1.0, -1.5))) - self.assertFalse(box.contains(Vector3d(+0.5, -1.0, +1.5))) - self.assertFalse(box.contains(Vector3d(+0.5, +1.0, -1.5))) - self.assertFalse(box.contains(Vector3d(+0.5, +1.0, +1.5))) - - # Inside - self.assertTrue(box.contains(Vector3d(-0.5+g_tolerance, - -1.5+g_tolerance, - -1.0+g_tolerance))) - self.assertTrue(box.contains(Vector3d(-0.5+g_tolerance, - -1.5+g_tolerance, - +1.0-g_tolerance))) - self.assertTrue(box.contains(Vector3d(-0.5+g_tolerance, - +1.5-g_tolerance, - -1.0+g_tolerance))) - self.assertTrue(box.contains(Vector3d(-0.5+g_tolerance, - +1.5-g_tolerance, - +1.0-g_tolerance))) - - self.assertTrue(box.contains(Vector3d(+0.5-g_tolerance, - -1.5+g_tolerance, - -1.0+g_tolerance))) - self.assertTrue(box.contains(Vector3d(+0.5-g_tolerance, - -1.5+g_tolerance, - +1.0-g_tolerance))) - self.assertTrue(box.contains(Vector3d(+0.5-g_tolerance, - +1.5-g_tolerance, -1.0+g_tolerance))) - self.assertTrue(box.contains(Vector3d(+0.5-g_tolerance, - +1.5-g_tolerance, - +1.0-g_tolerance))) - - # Outside - self.assertFalse(box.contains(Vector3d(-0.5-g_tolerance, - -1.5-g_tolerance, - -1.0-g_tolerance))) - self.assertFalse(box.contains(Vector3d(-0.5-g_tolerance, - -1.5-g_tolerance, - +1.0+g_tolerance))) - self.assertFalse(box.contains(Vector3d(-0.5-g_tolerance, - +1.5+g_tolerance, - -1.0-g_tolerance))) - self.assertFalse(box.contains(Vector3d(-0.5-g_tolerance, - +1.5+g_tolerance, - +1.0+g_tolerance))) - - self.assertFalse(box.contains(Vector3d(+0.5-g_tolerance, - -1.5-g_tolerance, - -1.0-g_tolerance))) - self.assertFalse(box.contains(Vector3d(+0.5-g_tolerance, - -1.5-g_tolerance, - +1.0+g_tolerance))) - self.assertFalse(box.contains(Vector3d(+0.5-g_tolerance, - +1.5+g_tolerance, - -1.0-g_tolerance))) - self.assertFalse(box.contains(Vector3d(+0.5-g_tolerance, - +1.5+g_tolerance, - +1.0+g_tolerance))) - - def test_volume_and_density(self): - mass = 1.0 - box = OrientedBoxd(Vector3d(1.0, 0.1, 10.4)) - expectedVolume = 1.0 * 0.1 * 10.4 - self.assertEqual(expectedVolume, box.volume()) - - expectedDensity = mass / expectedVolume - self.assertEqual(expectedDensity, box.density_from_mass(mass)) - - # Bad density - box2 = OrientedBoxd() - self.assertGreater(0.0, box2.density_from_mass(mass)) - - def test_mass(self): - mass = 2.0 - length = 2.0 - w = 0.1 - h = 34.12 - box = OrientedBoxd(Vector3d(length, w, h)) - box.set_density_from_mass(mass) - - massMat = MassMatrix3d() - ixx = (1.0/12.0) * mass * (w*w + h*h) - iyy = (1.0/12.0) * mass * (length * length + h*h) - izz = (1.0/12.0) * mass * (length * length + w*w) - - expectedMassMat = MassMatrix3d() - expectedMassMat.set_inertia_matrix(ixx, iyy, izz, 0.0, 0.0, 0.0) - expectedMassMat.set_mass(mass) - - box.mass_matrix(massMat) - self.assertEqual(expectedMassMat, massMat) - self.assertEqual(expectedMassMat.mass(), massMat.mass()) - - -if __name__ == '__main__': - unittest.main() diff --git a/src/python/Plane_TEST.py b/src/python/Plane_TEST.py deleted file mode 100644 index c1e1a6d25..000000000 --- a/src/python/Plane_TEST.py +++ /dev/null @@ -1,165 +0,0 @@ -# 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. - -import unittest - -from ignition.math import AxisAlignedBox, Planed, Vector2d, Vector3d - - -class TestPlane(unittest.TestCase): - - def test_plane_constructor(self): - plane = Planed(Vector3d(1, 0, 0), 0.1) - self.assertEqual(plane.normal(), Vector3d(1, 0, 0)) - self.assertAlmostEqual(plane.offset(), 0.1, 1e-6) - - planeCopy = Planed(plane) - self.assertEqual(plane.normal(), planeCopy.normal()) - self.assertEqual(plane.offset(), planeCopy.offset()) - self.assertEqual(plane.size(), planeCopy.size()) - - def test_plane_distance(self): - plane = Planed(Vector3d(0, 0, 1), 0.1) - self.assertAlmostEqual(plane.distance( - Vector3d(0, 0, 0), - Vector3d(0, 0, 1)), 0.1, delta=1e-6) - - self.assertAlmostEqual(plane.distance( - Vector3d(0, 0, 0.1), - Vector3d(0, 0, 1)), 0, delta=1e-6) - - self.assertAlmostEqual(plane.distance( - Vector3d(0, 0, 0.2), - Vector3d(0, 0, 1)), -0.1, delta=1e-6) - self.assertAlmostEqual(plane.distance( - Vector3d(0, 0, 0.1), - Vector3d(1, 0, 0)), 0, delta=1e-6) - - def test_plane(self): - plane = Planed() - self.assertEqual(plane.offset(), 0.0) - self.assertEqual(plane.normal(), Vector3d()) - self.assertEqual(plane.size(), Vector2d(0, 0)) - - plane = Planed(Vector3d(0, 0, 1), Vector2d(2, 3), 2.0) - self.assertEqual(plane.offset(), 2.0) - self.assertEqual(plane.normal(), Vector3d(0, 0, 1)) - self.assertEqual(plane.size(), Vector2d(2, 3)) - - self.assertEqual(-1, plane.distance( - Vector3d(0, 0, 1), - Vector3d(0, 0, -1))) - - plane.set(Vector3d(1, 0, 0), Vector2d(1, 1), 1.0) - self.assertEqual(plane.offset(), 1.0) - self.assertEqual(plane.normal(), Vector3d(1, 0, 0)) - self.assertEqual(plane.size(), Vector2d(1, 1)) - - plane = Planed(Vector3d(0, 1, 0), Vector2d(4, 4), 5.0) - self.assertEqual(plane.offset(), 5.0) - self.assertEqual(plane.normal(), Vector3d(0, 1, 0)) - self.assertEqual(plane.size(), Vector2d(4, 4)) - - def test_side_point(self): - plane = Planed(Vector3d(0, 0, 1), 1) - - # On the negative side of the plane (below the plane) - point = Vector3d(0, 0, 0) - self.assertEqual(plane.side(point), Planed.NEGATIVE_SIDE) - - # Still on the negative side of the plane (below the plane) - point.set(1, 1, 0) - self.assertEqual(plane.side(point), Planed.NEGATIVE_SIDE) - - # Above the plane (positive side) - point.set(1, 1, 2) - self.assertEqual(plane.side(point), Planed.POSITIVE_SIDE) - - # On the plane - point.set(0, 0, 1) - self.assertEqual(plane.side(point), Planed.NO_SIDE) - - # Change the plane, but the point is still on the negative side - plane.set(Vector3d(1, 0, 0), 4) - self.assertEqual(plane.side(point), Planed.NEGATIVE_SIDE) - - # Point is now on the positive side - point.set(4.1, 0, 1) - self.assertEqual(plane.side(point), Planed.POSITIVE_SIDE) - - def test_side__axis_aligned_box(self): - plane = Planed(Vector3d(0, 0, 1), 1) - - # On the negative side of the plane (below the plane) - box = AxisAlignedBox(Vector3d(-.5, -.5, -.5), Vector3d(.5, .5, .5)) - self.assertEqual(plane.side(box), Planed.NEGATIVE_SIDE) - - # Still on the negative side of the plane (below the plane) - box = AxisAlignedBox(Vector3d(-10, -10, -10), Vector3d(.9, .9, .9)) - self.assertEqual(plane.side(box), Planed.NEGATIVE_SIDE) - - # Above the plane (positive side) - box = AxisAlignedBox(Vector3d(2, 2, 2), Vector3d(3, 3, 3)) - self.assertEqual(plane.side(box), Planed.POSITIVE_SIDE) - - # On both sides the plane - box = AxisAlignedBox(Vector3d(0, 0, 0), Vector3d(3, 3, 3)) - self.assertEqual(plane.side(box), Planed.BOTH_SIDE) - - def test_intersection(self): - plane = Planed(Vector3d(0.5, 0, 1), 1) - intersect = plane.intersection(Vector3d(0, 0, 0), Vector3d(1, 0, 1)) - self.assertTrue(intersect is not None) - self.assertAlmostEqual(intersect.dot(plane.normal()), plane.offset(), 1e-6) - - plane.set(Vector3d(1, 0, 0), 2) - intersect = plane.intersection(Vector3d(0, 0, 0), Vector3d(1, 0, 0)) - self.assertTrue(intersect is not None) - self.assertEqual(intersect, Vector3d(2, 0, 0)) - - intersect = plane.intersection(Vector3d(1, 1, 0), Vector3d(-1, -1, 0)) - self.assertTrue(intersect is not None) - self.assertEqual(intersect, Vector3d(2, 2, 0)) - - # Lines on plane - intersect = plane.intersection(Vector3d(2, 0, 0), Vector3d(0, 1, 0)) - self.assertTrue(intersect is None) - - intersect = plane.intersection(Vector3d(2, 0, 0), Vector3d(0, 0, 1)) - self.assertTrue(intersect is None) - - intersect = plane.intersection(Vector3d(2, 0, 0), Vector3d(0, 1, 1)) - self.assertTrue(intersect is None) - - # Lines parallel to plane - intersect = plane.intersection(Vector3d(0, 0, 0), Vector3d(0, 1, 0)) - self.assertTrue(intersect is None) - - intersect = plane.intersection(Vector3d(0, 0, 0), Vector3d(0, 0, 1)) - self.assertTrue(intersect is None) - - intersect = plane.intersection(Vector3d(0, 0, 0), Vector3d(0, 1, 1)) - self.assertTrue(intersect is None) - - # Bounded plane - planeBounded = Planed(Vector3d(0, 0, 1), Vector2d(0.5, 0.5), 0) - intersect1 = planeBounded.intersection(Vector3d(0, 0, 0), Vector3d(0, 0, 1)) - self.assertTrue(intersect1 is not None) - self.assertEqual(intersect1, Vector3d(0, 0, 0)) - intersect2 = planeBounded.intersection(Vector3d(20, 20, 0), Vector3d(0, 0, 1)) - self.assertFalse(intersect2 is not None) - - -if __name__ == '__main__': - unittest.main() diff --git a/src/python/Rand.i b/src/python/Rand.i deleted file mode 100644 index 238d65fc2..000000000 --- a/src/python/Rand.i +++ /dev/null @@ -1,38 +0,0 @@ -/* - * 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. - * -*/ - -%module rand -%{ -#include -%} - -namespace ignition -{ - namespace math - { - class Rand - { - %rename("%(undercase)s", %$isfunction, %$ismember, %$not %$isconstructor) ""; - public: static void Seed(unsigned int _seed); - public: static unsigned int Seed(); - public: static double DblUniform(double _min, double _max); - public: static double DblNormal(double _mean, double _sigma); - public: static int IntUniform(int _min, int _max); - public: static int IntNormal(int _mean, int _sigma); - }; - } -} diff --git a/src/python/Sphere_TEST.py b/src/python/Sphere_TEST.py deleted file mode 100644 index 7c44d5de2..000000000 --- a/src/python/Sphere_TEST.py +++ /dev/null @@ -1,202 +0,0 @@ -# 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. - -import math -import unittest - -import ignition -from ignition.math import IGN_PI, MassMatrix3d, Material, Planed, Sphered, Vector2d, Vector3d - - -class TestSphere(unittest.TestCase): - - def test_constructor(self): - # Default constructor - sphere = Sphered() - self.assertEqual(0.0, sphere.radius()) - self.assertEqual(Material(), sphere.material()) - - sphere2 = Sphered() - self.assertEqual(sphere, sphere2) - - # Radius constructor - sphere = Sphered(1.0) - self.assertEqual(1.0, sphere.radius()) - self.assertEqual(Material(), sphere.material()) - - sphere2 = Sphered(1.0) - self.assertEqual(sphere, sphere2) - - # Radius and mat - sphere = Sphered(1.0, Material(ignition.math.MaterialType_WOOD)) - self.assertEqual(1.0, sphere.radius()) - self.assertEqual(Material(ignition.math.MaterialType_WOOD), sphere.material()) - - sphere2 = Sphered(1.0, Material(ignition.math.MaterialType_WOOD)) - self.assertEqual(sphere, sphere2) - - def test_comparison(self): - wood = Sphered(0.1, Material(ignition.math.MaterialType_WOOD)) - - modified = wood - self.assertEqual(wood, modified) - - modified.set_radius(1.0) - wood = Sphered(0.1, Material(ignition.math.MaterialType_WOOD)) - self.assertNotEqual(wood, modified) - - modified = wood - wood = Sphered(0.1, Material(ignition.math.MaterialType_WOOD)) - self.assertEqual(wood, modified) - - modified.set_material(Material(ignition.math.MaterialType_PINE)) - self.assertNotEqual(wood, modified) - - def test_mutators(self): - sphere = Sphered() - self.assertEqual(0.0, sphere.radius()) - self.assertEqual(Material(), sphere.material()) - - sphere.set_radius(.123) - sphere.set_material(Material(ignition.math.MaterialType_PINE)) - - self.assertEqual(.123, sphere.radius()) - self.assertEqual(Material(ignition.math.MaterialType_PINE), sphere.material()) - - def test_volume_and_density(self): - mass = 1.0 - sphere = Sphered(0.001) - expectedVolume = (4.0/3.0) * IGN_PI * math.pow(0.001, 3) - self.assertEqual(expectedVolume, sphere.volume()) - - expectedDensity = mass / expectedVolume - self.assertEqual(expectedDensity, sphere.density_from_mass(mass)) - - # Bad density - sphere2 = Sphered() - self.assertGreater(0.0, sphere2.density_from_mass(mass)) - sphere2.set_radius(1.0) - self.assertGreater(0.0, sphere2.density_from_mass(0.0)) - self.assertFalse(sphere.set_density_from_mass(0.0)) - - def test_mass(self): - mass = 2.0 - r = 0.1 - sphere = Sphered(r) - self.assertTrue(sphere.set_density_from_mass(mass)) - - massMat = MassMatrix3d() - ixxIyyIzz = 0.4 * mass * r * r - - expectedMassMat = MassMatrix3d() - expectedMassMat.set_inertia_matrix(ixxIyyIzz, ixxIyyIzz, ixxIyyIzz, - 0.0, 0.0, 0.0) - expectedMassMat.set_mass(mass) - - sphere.mass_matrix(massMat) - self.assertEqual(expectedMassMat, massMat) - self.assertEqual(expectedMassMat.mass(), massMat.mass()) - - def test_volume_below(self): - - r = 2 - sphere = Sphered(r) - - # Fully below - plane = Planed(Vector3d(0, 0, 1), Vector2d(4, 4), 2*r) - self.assertAlmostEqual(sphere.volume(), sphere.volume_below(plane), delta=1e-3) - - # Fully below (because plane is rotated down) - plane = Planed(Vector3d(0, 0, -1), Vector2d(4, 4), 2*r) - self.assertAlmostEqual(sphere.volume(), sphere.volume_below(plane), delta=1e-3) - - # Fully above - plane = Planed(Vector3d(0, 0, 1), Vector2d(4, 4), -2*r) - self.assertAlmostEqual(sphere.volume_below(plane), 0, 1e-3) - - # Hemisphere - plane = Planed(Vector3d(0, 0, 1), 0) - self.assertAlmostEqual(sphere.volume() / 2, sphere.volume_below(plane), delta=1e-3) - - # Vertical plane - plane = Planed(Vector3d(1, 0, 0), 0) - self.assertAlmostEqual(sphere.volume() / 2, sphere.volume_below(plane), delta=1e-3) - - # Expectations from https:#planetcalc.com/283/ - plane = Planed(Vector3d(0, 0, 1), 0.5) - self.assertAlmostEqual(22.90745, sphere.volume_below(plane), delta=1e-3) - - plane = Planed(Vector3d(0, 0, 1), -0.5) - self.assertAlmostEqual(10.60288, sphere.volume_below(plane), delta=1e-3) - - def test_center_of_volume_below(self): - r = 2 - sphere = Sphered(r) - - # Entire sphere below plane - plane = Planed(Vector3d(0, 0, 1), Vector2d(0, 0), 2 * r) - self.assertEqual(Vector3d(0, 0, 0), sphere.center_of_volume_below(plane)) - - # Entire sphere above plane - plane = Planed(Vector3d(0, 0, 1), Vector2d(0, 0), -2 * r) - self.assertFalse(sphere.center_of_volume_below(plane) is not None) - - # Halfway point is a good spot to test. Center of Volume for a hemisphere - # is 3/8 its radius. In this case the point should fall below the y-plane - plane = Planed(Vector3d(0, 1, 0), Vector2d(0, 0), 0) - self.assertEqual(Vector3d(0, -0.75, 0), sphere.center_of_volume_below(plane)) - - # Halfway point is a good spot to test. Center of Volume for a hemisphere - # is 3/8 its radius. In this case the point should fall above the y-plane - # thanks to flipped normal - plane = Planed(Vector3d(0, -1, 0), Vector2d(0, 0), 0) - self.assertEqual(Vector3d(0, 0.75, 0), sphere.center_of_volume_below(plane)) - - # Handcalculated value. - # Plane at y = 0.8 pointing upwards - # Cap height is 2.8 - # Centroid should be at 0.3375. However, keep in mind this assumes an - # inverted cap. - # Center of volume below should be at -0.3375 - plane = Planed(Vector3d(0, 1, 0), Vector2d(0, 0), 0.4 * r) - self.assertEqual(Vector3d(0, -0.3375, 0), sphere.center_of_volume_below(plane)) - - # Handcalculated value. - plane = Planed(Vector3d(0, 1, 0), Vector2d(0, 0), -0.4 * r) - - self.assertEqual(Vector3d(0, -1.225, 0), sphere.center_of_volume_below(plane)) - - # Handcalculated value. - plane = Planed(Vector3d(0, -1, 0), Vector2d(0, 0), -0.4 * r) - - self.assertEqual(Vector3d(0, 1.225, 0), sphere.center_of_volume_below(plane)) - - # Handcalculated value. - plane = Planed(Vector3d(0, -1, 0), Vector2d(0, 0), 0.4 * r) - - self.assertEqual(Vector3d(0, 0.3375, 0), sphere.center_of_volume_below(plane)) - - # Weighted sums of the center of volume results in (0,0,0). - plane1 = Planed(Vector3d(0, 0, 1), -0.5) - # Flip plane1 axis - plane2 = Planed(Vector3d(0, 0, -1), -0.5) - self.assertEqual( - sphere.center_of_volume_below(plane1) * sphere.volume_below(plane1) + - sphere.center_of_volume_below(plane2) * - sphere.volume_below(plane2), - Vector3d(0, 0, 0)) - - -if __name__ == '__main__': - unittest.main() diff --git a/src/python/SphericalCoordinates_TEST.py b/src/python/SphericalCoordinates_TEST.py deleted file mode 100644 index 415faec01..000000000 --- a/src/python/SphericalCoordinates_TEST.py +++ /dev/null @@ -1,562 +0,0 @@ -# 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. - -import unittest - -import ignition -from ignition.math import Angle, IGN_PI, SphericalCoordinates, Vector3d - - -class TestSphericalCoordinates(unittest.TestCase): - - def test_constructor(self): - # Default surface type - st = SphericalCoordinates.EARTH_WGS84 - - # No arguments, default parameters - sc = SphericalCoordinates() - self.assertEqual(sc.surface(), st) - self.assertEqual(sc.latitude_reference(), Angle()) - self.assertEqual(sc.longitude_reference(), Angle()) - self.assertEqual(sc.heading_offset(), Angle()) - self.assertAlmostEqual(sc.elevation_reference(), 0.0, delta=1e-6) - - # SurfaceType argument, default parameters - sc = SphericalCoordinates(st) - self.assertEqual(sc.surface(), st) - self.assertEqual(sc.latitude_reference(), Angle()) - self.assertEqual(sc.longitude_reference(), Angle()) - self.assertEqual(sc.heading_offset(), Angle()) - self.assertAlmostEqual(sc.elevation_reference(), 0.0, delta=1e-6) - - # All arguments - lat = Angle(0.3) - lon = Angle(-1.2) - heading = Angle(0.5) - elev = 354.1 - sc = SphericalCoordinates(st, lat, lon, elev, heading) - self.assertEqual(sc.surface(), st) - self.assertEqual(sc.latitude_reference(), lat) - self.assertEqual(sc.longitude_reference(), lon) - self.assertEqual(sc.heading_offset(), heading) - self.assertAlmostEqual(sc.elevation_reference(), elev, delta=1e-6) - - # Copy constructor - sc2 = SphericalCoordinates(sc) - self.assertEqual(sc, sc2) - - def test_convert(self): - # Default surface type - st = SphericalCoordinates.EARTH_WGS84 - - self.assertEqual(SphericalCoordinates.convert("EARTH_WGS84"), st) - - self.assertEqual(SphericalCoordinates.EARTH_WGS84, - SphericalCoordinates.convert("OTHER-COORD")) - - self.assertEqual("EARTH_WGS84", SphericalCoordinates.convert(st)) - - def test_set_functions(self): - # Default surface type - st = SphericalCoordinates.EARTH_WGS84 - - # Default parameters - sc = SphericalCoordinates() - self.assertEqual(sc.surface(), st) - self.assertEqual(sc.latitude_reference(), Angle()) - self.assertEqual(sc.longitude_reference(), Angle()) - self.assertEqual(sc.heading_offset(), Angle()) - self.assertAlmostEqual(sc.elevation_reference(), 0.0, delta=1e-6) - - lat = Angle(0.3) - lon = Angle(-1.2) - heading = Angle(0.5) - elev = 354.1 - sc.set_surface(st) - sc.set_latitude_reference(lat) - sc.set_longitude_reference(lon) - sc.set_heading_offset(heading) - sc.set_elevation_reference(elev) - - self.assertEqual(sc.surface(), st) - self.assertEqual(sc.latitude_reference(), lat) - self.assertEqual(sc.longitude_reference(), lon) - self.assertEqual(sc.heading_offset(), heading) - self.assertAlmostEqual(sc.elevation_reference(), elev, delta=1e-6) - - def test_coordinate_transforms(self): - # Default surface type - st = SphericalCoordinates.EARTH_WGS84 - - # Parameters - lat = Angle(0.3) - lon = Angle(-1.2) - heading = Angle(Angle.HALF_PI) - elev = 354.1 - sc = SphericalCoordinates(st, lat, lon, elev, heading) - - # Check GlobalFromLocal with heading offset of 90 degrees - # Heading 0: X == East, Y == North, Z == Up - # Heading 90: X == North, Y == West , Z == Up - # local frame - xyz = Vector3d() - # east, north, up - enu = Vector3d() - - xyz.set(1, 0, 0) - enu = sc.global_from_local_velocity(xyz) - self.assertAlmostEqual(enu.y(), xyz.x(), delta=1e-6) - self.assertAlmostEqual(enu.x(), -xyz.y(), delta=1e-6) - self.assertEqual(xyz, sc.local_from_global_velocity(enu)) - - xyz.set(0, 1, 0) - enu = sc.global_from_local_velocity(xyz) - self.assertAlmostEqual(enu.y(), xyz.x(), delta=1e-6) - self.assertAlmostEqual(enu.x(), -xyz.y(), delta=1e-6) - self.assertEqual(xyz, sc.local_from_global_velocity(enu)) - - xyz.set(1, -1, 0) - enu = sc.global_from_local_velocity(xyz) - self.assertAlmostEqual(enu.y(), xyz.x(), delta=1e-6) - self.assertAlmostEqual(enu.x(), -xyz.y(), delta=1e-6) - self.assertEqual(xyz, sc.local_from_global_velocity(enu)) - - xyz.set(2243.52334, 556.35, 435.6553) - enu = sc.global_from_local_velocity(xyz) - self.assertAlmostEqual(enu.y(), xyz.x(), delta=1e-6) - self.assertAlmostEqual(enu.x(), -xyz.y(), delta=1e-6) - self.assertEqual(xyz, sc.local_from_global_velocity(enu)) - - # Check SphericalFromLocal - # local frame - xyz = Vector3d() - # spherical coordinates - sph = Vector3d() - - # No offset - xyz.set(0, 0, 0) - sph = sc.spherical_from_local_position(xyz) - # latitude - self.assertAlmostEqual(sph.x(), lat.degree(), delta=1e-6) - # longitude - self.assertAlmostEqual(sph.y(), lon.degree(), delta=1e-6) - # elevation - self.assertAlmostEqual(sph.z(), elev, delta=1e-6) - - # 200 km offset in x (pi/2 heading offset means North). We use - # SphericalFromLocal, which means that xyz is a linear movement on - # a plane (not along the curvature of Earth). This will result in - # a large height offset. - xyz.set(2e5, 0, 0) - sph = sc.spherical_from_local_position(xyz) - # increase in latitude about 1.8 degrees - self.assertAlmostEqual(sph.x(), lat.degree() + 1.8, delta=0.008) - # no change in longitude - self.assertAlmostEqual(sph.z(), 3507.024791, delta=1e-6) - - xyz2 = sc.local_from_spherical_position(sph) - self.assertEqual(xyz, xyz2) - - # Check position projection - # WGS84 coordinate obtained from online mapping software - # > gdaltransform -s_srs WGS84 -t_srs EPSG:4978 - # > latitude longitude altitude - # > X Y Z - tmp = Vector3d() - osrf_s = Vector3d(37.3877349, -122.0651166, 32.0) - osrf_e = Vector3d(-2693701.91434394, -4299942.14687992, 3851691.0393571) - goog_s = Vector3d(37.4216719, -122.0821853, 30.0) - - # Local tangent plane coordinates (ENU = GLOBAL) coordinates of - # Google when OSRF is taken as the origin: - # > proj +ellps=WGS84 +proj=tmerc - # +lat_0=37.3877349 +lon_0=-122.0651166 +k=1 +x_0=0 +y_0=0 - # > -122.0821853 37.4216719 (LON,LAT) - # > -1510.88 3766.64 (EAST,NORTH) - vec = Vector3d(-1510.88, 3766.64, -3.29) - - # Convert degrees to radians - osrf_s.x(osrf_s.x() * 0.0174532925) - osrf_s.y(osrf_s.y() * 0.0174532925) - - # Set the ORIGIN to be the Open Source Robotics Foundation - sc2 = SphericalCoordinates(st, Angle(osrf_s.x()), - Angle(osrf_s.y()), osrf_s.z(), Angle.ZERO) - - # Check that SPHERICAL -> ECEF works - tmp = sc2.position_transform(osrf_s, SphericalCoordinates.SPHERICAL, - SphericalCoordinates.ECEF) - - self.assertAlmostEqual(tmp.x(), osrf_e.x(), delta=8e-2) - self.assertAlmostEqual(tmp.y(), osrf_e.y(), delta=8e-2) - self.assertAlmostEqual(tmp.z(), osrf_e.z(), delta=1e-2) - - # Check that ECEF -> SPHERICAL works - tmp = sc2.position_transform(tmp, SphericalCoordinates.ECEF, SphericalCoordinates.SPHERICAL) - - self.assertAlmostEqual(tmp.x(), osrf_s.x(), delta=1e-2) - self.assertAlmostEqual(tmp.y(), osrf_s.y(), delta=1e-2) - self.assertAlmostEqual(tmp.z(), osrf_s.z(), delta=1e-2) - - # Check that SPHERICAL -> LOCAL works - tmp = sc2.local_from_spherical_position(goog_s) - self.assertAlmostEqual(tmp.x(), vec.x(), delta=8e-2) - self.assertAlmostEqual(tmp.y(), vec.y(), delta=8e-2) - self.assertAlmostEqual(tmp.z(), vec.z(), delta=1e-2) - - # Check that SPHERICAL -> LOCAL -> SPHERICAL works - tmp = sc2.spherical_from_local_position(tmp) - self.assertAlmostEqual(tmp.x(), goog_s.x(), delta=8e-2) - self.assertAlmostEqual(tmp.y(), goog_s.y(), delta=8e-2) - self.assertAlmostEqual(tmp.z(), goog_s.z(), delta=1e-2) - - # Give no heading offset to confirm ENU frame - lat = Angle(0.3) - lon = Angle(-1.2) - heading = Angle(0.0) - elev = 354.1 - sc = SphericalCoordinates(st, lat, lon, elev, heading) - - # Check GlobalFromLocal with no heading offset - # local frame - xyz = Vector3d() - # east, north, up - enu = Vector3d() - - xyz.set(1, 0, 0) - enu = sc.velocity_transform(xyz, SphericalCoordinates.LOCAL2, SphericalCoordinates.GLOBAL) - self.assertEqual(xyz, enu) - self.assertEqual(xyz, sc.local_from_global_velocity(enu)) - - xyz.set(0, 1, 0) - enu = sc.velocity_transform(xyz, SphericalCoordinates.LOCAL2, SphericalCoordinates.GLOBAL) - self.assertEqual(xyz, enu) - self.assertEqual(xyz, sc.local_from_global_velocity(enu)) - - xyz.set(1, -1, 0) - enu = sc.velocity_transform(xyz, SphericalCoordinates.LOCAL2, SphericalCoordinates.GLOBAL) - self.assertEqual(xyz, enu) - self.assertEqual(xyz, sc.local_from_global_velocity(enu)) - - xyz.set(2243.52334, 556.35, 435.6553) - enu = sc.velocity_transform(xyz, SphericalCoordinates.LOCAL2, SphericalCoordinates.GLOBAL) - self.assertEqual(xyz, enu) - self.assertEqual(xyz, sc.local_from_global_velocity(enu)) - - def test_distance(self): - latA = Angle() - longA = Angle() - latB = Angle() - longB = Angle() - latA.set_degree(46.250944) - longA.set_degree(-122.249972) - latB.set_degree(46.124953) - longB.set_degree(-122.251683) - d = SphericalCoordinates.distance(latA, longA, latB, longB) - - self.assertAlmostEqual(14002, d, delta=20) - - def test_bad_set_surface(self): - sc = SphericalCoordinates() - sc.set_surface(2) - self.assertEqual(sc.surface(), 2) - - def test_transform(self): - sc = SphericalCoordinates() - vel = Vector3d(1, 2, -4) - result = sc.velocity_transform( - vel, - SphericalCoordinates.ECEF, - SphericalCoordinates.ECEF) - - self.assertEqual(result, vel) - - pos = Vector3d(-1510.88, 2, -4) - result = sc.position_transform( - pos, - SphericalCoordinates.ECEF, - SphericalCoordinates.GLOBAL) - - self.assertAlmostEqual(result.x(), 2, delta=1e-6) - self.assertAlmostEqual(result.y(), -4, delta=1e-6) - self.assertAlmostEqual(result.z(), -6379647.8799999999, delta=1e-6) - - print('NEW POS[', result.x(), ' ', result.y(), ' ', result.z(), ']\n') - - def test_bad_coordinate_type(self): - sc = SphericalCoordinates() - pos = Vector3d(1, 2, -4) - result = sc.position_transform(pos, 7, 6) - - self.assertEqual(result, pos) - - result = sc.position_transform(pos, 4, 6) - - self.assertEqual(result, pos) - - result = sc.velocity_transform( - pos, - SphericalCoordinates.SPHERICAL, - SphericalCoordinates.ECEF) - self.assertEqual(result, pos) - - result = sc.velocity_transform( - pos, - SphericalCoordinates.ECEF, - SphericalCoordinates.SPHERICAL) - self.assertEqual(result, pos) - - result = sc.velocity_transform(pos, 7, SphericalCoordinates.ECEF) - self.assertEqual(result, pos) - - result = sc.velocity_transform(pos, SphericalCoordinates.ECEF, 7) - self.assertEqual(result, pos) - - def test_equality_ops(self): - # Default surface type - st = SphericalCoordinates.EARTH_WGS84 - lat = Angle(0.3) - lon = Angle(-1.2) - heading = Angle(0.5) - elev = 354.1 - sc1 = SphericalCoordinates(st, lat, lon, elev, heading) - - sc2 = SphericalCoordinates(st, lat, lon, elev, heading) - self.assertTrue(sc1 == sc2) - self.assertFalse(sc1 != sc2) - sc3 = SphericalCoordinates(st, Angle.ZERO, lon, elev, heading) - self.assertFalse(sc1 == sc3) - self.assertTrue(sc1 != sc3) - sc4 = SphericalCoordinates(st, lat, Angle.ZERO, elev, heading) - self.assertFalse(sc1 == sc4) - self.assertTrue(sc1 != sc4) - sc5 = SphericalCoordinates(st, lat, lon, elev + 1, heading) - self.assertFalse(sc1 == sc5) - self.assertTrue(sc1 != sc5) - sc6 = SphericalCoordinates(st, lat, lon, elev, Angle.ZERO) - self.assertFalse(sc1 == sc6) - self.assertTrue(sc1 != sc6) - - def test_assigment_op(self): - # Default surface type - st = SphericalCoordinates.EARTH_WGS84 - lat = Angle(0.3) - lon = Angle(-1.2) - heading = Angle(0.5) - elev = 354.1 - sc1 = SphericalCoordinates(st, lat, lon, elev, heading) - - sc2 = sc1 - self.assertEqual(sc1, sc2) - - def test_no_heading(self): - # Default heading - st = SphericalCoordinates.EARTH_WGS84 - lat = Angle(-22.9 * IGN_PI / 180.0) - lon = Angle(-43.2 * IGN_PI / 180.0) - heading = Angle(0.0) - elev = 0 - sc = SphericalCoordinates(st, lat, lon, elev, heading) - - # Origin matches input - latLonAlt = sc.spherical_from_local_position(Vector3d(0, 0, 0)) - self.assertEqual(lat.degree(), latLonAlt.x()) - self.assertEqual(lon.degree(), latLonAlt.y()) - self.assertEqual(elev, latLonAlt.z()) - - xyzOrigin = sc.local_from_spherical_position(latLonAlt) - self.assertEqual(Vector3d.ZERO, xyzOrigin) - - # Check how different lat/lon affect the local position - - # Increase latitude == go North == go +Y - xyz = sc.local_from_spherical_position( - Vector3d(lat.degree() + 1.0, lon.degree(), elev)) - self.assertAlmostEqual(xyzOrigin.x(), xyz.x(), delta=1e-6) - self.assertLess(xyzOrigin.y(), xyz.y()) - - # Decrease latitude == go South == go -Y - xyz = sc.local_from_spherical_position( - Vector3d(lat.degree() - 1.0, lon.degree(), elev)) - self.assertAlmostEqual(xyzOrigin.x(), xyz.x(), delta=1e-6) - self.assertGreater(xyzOrigin.y(), xyz.y()) - - # Increase longitude == go East == go +X - # Also move a bit -Y because this is the Southern Hemisphere - xyz = sc.local_from_spherical_position( - Vector3d(lat.degree(), lon.degree() + 1.0, elev)) - self.assertLess(xyzOrigin.x(), xyz.x()) - self.assertGreater(xyzOrigin.y(), xyz.y()) - - # Decrease longitude == go West == go -X - # Also move a bit -Y because this is the Southern Hemisphere - xyz = sc.local_from_spherical_position( - Vector3d(lat.degree(), lon.degree() - 1.0, elev)) - self.assertGreater(xyzOrigin.x(), xyz.x()) - self.assertGreater(xyzOrigin.y(), xyz.y()) - - # Increase altitude - xyz = sc.local_from_spherical_position( - Vector3d(lat.degree(), lon.degree(), elev + 10.0)) - self.assertAlmostEqual(xyzOrigin.x(), xyz.x(), delta=1e-6) - self.assertAlmostEqual(xyzOrigin.y(), xyz.y(), delta=1e-6) - self.assertAlmostEqual(xyzOrigin.z() + 10.0, xyz.z(), delta=1e-6) - - # Decrease altitude - xyz = sc.local_from_spherical_position( - Vector3d(lat.degree(), lon.degree(), elev - 10.0)) - self.assertAlmostEqual(xyzOrigin.x(), xyz.x(), delta=1e-6) - self.assertAlmostEqual(xyzOrigin.y(), xyz.y(), delta=1e-6) - self.assertAlmostEqual(xyzOrigin.z() - 10.0, xyz.z(), delta=1e-6) - - # Check how global and local velocities are connected - - # Velocity in - # +X (East), +Y (North), -X (West), -Y (South), +Z (up), -Z (down) - for global_var in [Vector3d.UNIT_X, Vector3d.UNIT_Y, Vector3d.UNIT_Z, - -Vector3d.UNIT_X, -Vector3d.UNIT_Y, -Vector3d.UNIT_Z]: - local = sc.local_from_global_velocity(global_var) - self.assertEqual(global_var, local) - - # This function is broken for horizontal velocities - global_var = sc.global_from_local_velocity(local) - if abs(global_var.z()) < 0.1: - self.assertNotEqual(global_var, local) - else: - self.assertEqual(global_var, local) - - # Directly call fixed version - global_var = sc.velocity_transform( - local, - SphericalCoordinates.LOCAL2, - SphericalCoordinates.GLOBAL) - self.assertEqual(global_var, local) - - def test_with_heading(self): - # Heading 90 deg: X == North, Y == West , Z == Up - st = SphericalCoordinates.EARTH_WGS84 - lat = Angle(-22.9 * IGN_PI / 180.0) - lon = Angle(-43.2 * IGN_PI / 180.0) - heading = Angle(90.0 * IGN_PI / 180.0) - elev = 0 - sc = SphericalCoordinates(st, lat, lon, elev, heading) - - # Origin matches input - latLonAlt = sc.spherical_from_local_position(Vector3d(0, 0, 0)) - self.assertEqual(lat.degree(), latLonAlt.x()) - self.assertEqual(lon.degree(), latLonAlt.y()) - self.assertEqual(elev, latLonAlt.z()) - - xyzOrigin = sc.local_from_spherical_position(latLonAlt) - self.assertEqual(Vector3d.ZERO, xyzOrigin) - - # Check how different lat/lon affect the local position - - # Increase latitude == go North == go +X - xyz = sc.local_from_spherical_position( - Vector3d(lat.degree() + 1.0, lon.degree(), elev)) - self.assertAlmostEqual(xyzOrigin.y(), xyz.y(), delta=1e-6) - self.assertLess(xyzOrigin.x(), xyz.x()) - - # Decrease latitude == go South == go -X - xyz = sc.local_from_spherical_position( - Vector3d(lat.degree() - 1.0, lon.degree(), elev)) - self.assertAlmostEqual(xyzOrigin.y(), xyz.y(), delta=1e-6) - self.assertGreater(xyzOrigin.x(), xyz.x()) - - # Increase longitude == go East == go -Y (and a bit -X) - xyz = sc.local_from_spherical_position( - Vector3d(lat.degree(), lon.degree() + 1.0, elev)) - self.assertGreater(xyzOrigin.y(), xyz.y()) - self.assertGreater(xyzOrigin.x(), xyz.x()) - - # Decrease longitude == go West == go +Y (and a bit -X) - xyz = sc.local_from_spherical_position( - Vector3d(lat.degree(), lon.degree() - 1.0, elev)) - self.assertLess(xyzOrigin.y(), xyz.y()) - self.assertGreater(xyzOrigin.x(), xyz.x()) - - # Check how global and local velocities are connected - - # Global | Local - # ---------- | ------ - # +X (East) | -Y - # -X (West) | +Y - # +Y (North) | +X - # -Y (South) | -X - globalLocal = [ - [Vector3d.UNIT_X, -Vector3d.UNIT_Y], - [-Vector3d.UNIT_X, Vector3d.UNIT_Y], - [Vector3d.UNIT_Y, Vector3d.UNIT_X], - [-Vector3d.UNIT_Y, -Vector3d.UNIT_X]] - for [global_var, local] in globalLocal: - localRes = sc.local_from_global_velocity(global_var) - self.assertEqual(local, localRes) - - # Directly call fixed version - globalRes = sc.velocity_transform( - local, - SphericalCoordinates.LOCAL2, - SphericalCoordinates.GLOBAL) - self.assertEqual(global_var, globalRes) - - def test_inverse(self): - st = SphericalCoordinates.EARTH_WGS84 - lat = Angle(0.3) - lon = Angle(-1.2) - heading = Angle(0.5) - elev = 354.1 - sc = SphericalCoordinates(st, lat, lon, elev, heading) - - # GLOBAL <-> LOCAL2 - in_vector = Vector3d(1, 2, -4) - out = sc.velocity_transform( - in_vector, - SphericalCoordinates.LOCAL2, - SphericalCoordinates.GLOBAL) - self.assertNotEqual(in_vector, out) - reverse = sc.velocity_transform( - out, - SphericalCoordinates.GLOBAL, - SphericalCoordinates.LOCAL2) - self.assertEqual(in_vector, reverse) - - in_vector = Vector3d(1, 2, -4) - out = sc.position_transform( - in_vector, - SphericalCoordinates.LOCAL2, - SphericalCoordinates.GLOBAL) - self.assertNotEqual(in_vector, out) - reverse = sc.position_transform( - out, - SphericalCoordinates.GLOBAL, - SphericalCoordinates.LOCAL2) - self.assertEqual(in_vector, reverse) - - # SPHERICAL <-> LOCAL2 - in_vector = Vector3d(1, 2, -4) - out = sc.position_transform( - in_vector, - SphericalCoordinates.LOCAL2, - SphericalCoordinates.SPHERICAL) - self.assertNotEqual(in_vector, out) - reverse = sc.position_transform( - out, - SphericalCoordinates.SPHERICAL, - SphericalCoordinates.LOCAL2) - self.assertEqual(in_vector, reverse) - - -if __name__ == '__main__': - unittest.main() diff --git a/src/python/Vector2.i b/src/python/Vector2.i deleted file mode 100644 index 6ff486814..000000000 --- a/src/python/Vector2.i +++ /dev/null @@ -1,81 +0,0 @@ -/* - * 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. - * -*/ - -#ifdef SWIGRUBY -%begin %{ -#define HAVE_ISFINITE 1 -%} -#endif - -%module vector2 -%{ -#include -%} - -namespace ignition -{ - namespace math - { - template - class Vector2 - { - %rename("%(undercase)s", %$isfunction, %$ismember, %$not %$isconstructor) ""; - %rename("%(uppercase)s", %$isstatic, %$isvariable) ""; - public: static const Vector2 Zero; - public: static const Vector2 One; - public: static const Vector2 NaN; - - public: Vector2(); - public: Vector2(const T &_x, const T &_y); - public: Vector2(const Vector2 &_v); - public: virtual ~Vector2(); - public: double Distance(const Vector2 &_pt) const; - public: T Length() const; - public: T SquaredLength() const; - public: void Normalize(); - public: void Set(T _x, T _y); - public: T Dot(const Vector2 &_v) const; - public: Vector2 Abs() const; - public: T AbsDot(const Vector2 &_v) const; - public: inline void Correct(); - public: void Max(const Vector2 &_v); - public: void Min(const Vector2 &_v); - public: T Max() const; - public: T Min() const; - public: Vector2 operator+(const Vector2 &_v) const; - public: inline Vector2 operator+(const T _s) const; - public: inline Vector2 operator-() const; - public: Vector2 operator-(const Vector2 &_v) const; - public: inline Vector2 operator-(const T _s) const; - public: const Vector2 operator/(const Vector2 &_v) const; - public: const Vector2 operator/(T _v) const; - public: const Vector2 operator*(const Vector2 &_v) const; - public: const Vector2 operator*(T _v) const; - public: bool operator==(const Vector2 &_v) const; - public: bool IsFinite() const; - public: inline T X() const; - public: inline T Y() const; - public: inline void X(const T &_v); - public: inline void Y(const T &_v); - public: bool Equal(const Vector2 &_v, const T &_tol) const; - }; - - %template(Vector2i) Vector2; - %template(Vector2d) Vector2; - %template(Vector2f) Vector2; - } -} diff --git a/src/python/Vector3.i b/src/python/Vector3.i deleted file mode 100644 index 751350fb7..000000000 --- a/src/python/Vector3.i +++ /dev/null @@ -1,101 +0,0 @@ -/* - * 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. - * -*/ - -#ifdef SWIGRUBY -%begin %{ -#define HAVE_ISFINITE 1 -%} -#endif - -%module vector3 -%{ -#include -%} - -namespace ignition -{ - namespace math - { - template - class Vector3 - { - %rename("%(undercase)s", %$isfunction, %$ismember, %$not %$isconstructor) ""; - %rename("%(uppercase)s", %$isstatic, %$isvariable) ""; - public: static const Vector3 Zero; - public: static const Vector3 One; - %rename(UNIT_X) UnitX; - public: static const Vector3 UnitX; - %rename(UNIT_Y) UnitY; - public: static const Vector3 UnitY; - %rename(UNIT_Z) UnitZ; - public: static const Vector3 UnitZ; - public: static const Vector3 NaN; - public: Vector3(); - public: Vector3(const T &_x, const T &_y, const T &_z); - public: Vector3(const Vector3 &_v); - public: virtual ~Vector3(); - public: T Sum() const; - public: T Distance(const Vector3 &_pt) const; - public: T Distance(T _x, T _y, T _z) const; - public: T Length() const; - public: T SquaredLength() const; - public: Vector3 Normalize(); - public: Vector3 Normalized() const; - public: Vector3 Round(); - public: Vector3 Rounded() const; - public: inline void Set(T _x = 0, T _y = 0, T _z = 0); - public: Vector3 Cross(const Vector3 &_v) const; - public: T Dot(const Vector3 &_v) const; - public: T AbsDot(const Vector3 &_v) const; - public: Vector3 Abs() const; - public: Vector3 Perpendicular() const; - public: static Vector3 Normal(const Vector3 &_v1, - const Vector3 &_v2, const Vector3 &_v3); - public: T DistToLine(const Vector3 &_pt1, const Vector3 &_pt2); - public: void Max(const Vector3 &_v); - public: void Min(const Vector3 &_v); - public: T Max() const; - public: T Min() const; - public: Vector3 operator+(const Vector3 &_v) const; - public: inline Vector3 operator+(const T _s) const; - public: inline Vector3 operator-() const; - public: inline Vector3 operator-(const Vector3 &_pt) const; - public: inline Vector3 operator-(const T _s) const; - public: const Vector3 operator/(const Vector3 &_pt) const; - public: const Vector3 operator/(T _v) const; - public: Vector3 operator*(const Vector3 &_p) const; - public: inline Vector3 operator*(T _s) const; - public: bool Equal(const Vector3 &_v, const T &_tol) const; - public: bool operator==(const Vector3 &_v) const; - public: bool IsFinite() const; - public: inline void Correct(); - public: void Round(int _precision); - public: bool Equal(const Vector3 &_v) const; - public: inline T X() const; - public: inline T Y() const; - public: inline T Z() const; - public: inline void X(const T &_v); - public: inline void Y(const T &_v); - public: inline void Z(const T &_v); - public: bool operator<(const Vector3 &_pt) const; - }; - - %template(Vector3i) Vector3; - %template(Vector3d) Vector3; - %template(Vector3f) Vector3; - } -} diff --git a/src/python/Vector4.i b/src/python/Vector4.i deleted file mode 100644 index aaea4dbc6..000000000 --- a/src/python/Vector4.i +++ /dev/null @@ -1,77 +0,0 @@ -/* - * 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. - * -*/ - -#ifdef SWIGRUBY -%begin %{ -#define HAVE_ISFINITE 1 -%} -#endif - -%module vector4 -%{ -#include -%} - -namespace ignition -{ - namespace math - { - template - class Vector4 - { - %rename("%(undercase)s", %$isfunction, %$ismember, %$not %$isconstructor) ""; - %rename("%(uppercase)s", %$isstatic, %$isvariable) ""; - public: static const Vector4 Zero; - public: static const Vector4 One; - public: static const Vector4 NaN; - public: Vector4(); - public: Vector4(const T &_x, const T &_y, const T &_z, const T &_w); - public: Vector4(const Vector4 &_v); - public: virtual ~Vector4(); - public: T Distance(const Vector4 &_pt) const; - public: T Length() const; - public: T SquaredLength() const; - public: void Normalize(); - public: inline void Set(T _x = 0, T _y = 0, T _z = 0, T _w = 0); - public: Vector4 operator+(const Vector4 &_v) const; - public: inline Vector4 operator+(const T _s) const; - public: inline Vector4 operator-() const; - public: inline Vector4 operator-(const Vector4 &_pt) const; - public: inline Vector4 operator-(const T _s) const; - public: const Vector4 operator/(const Vector4 &_pt) const; - public: const Vector4 operator/(T _v) const; - public: Vector4 operator*(const Vector4 &_p) const; - public: inline Vector4 operator*(T _s) const; - public: bool operator==(const Vector4 &_v) const; - public: bool Equal(const Vector4 &_v, const T &_tol) const; - public: bool IsFinite() const; - public: inline void Correct(); - public: inline T X() const; - public: inline T Y() const; - public: inline T Z() const; - public: inline T W() const; - public: inline void X(const T &_v); - public: inline void Y(const T &_v); - public: inline void Z(const T &_v); - public: inline void W(const T &_v); - }; - - %template(Vector4i) Vector4; - %template(Vector4d) Vector4; - %template(Vector4f) Vector4; - } -} diff --git a/src/python/python.i b/src/python/python.i deleted file mode 100644 index 44e9808e7..000000000 --- a/src/python/python.i +++ /dev/null @@ -1,42 +0,0 @@ -%module "math" -%include Angle.i -%include DiffDriveOdometry.i -%include GaussMarkovProcess.i -%include Helpers.i -%include Rand.i -%include StopWatch.i -%include Vector2.i -%include Vector3.i -%include SphericalCoordinates.i -%include Vector4.i -%include Color.i -%include Pose3.i -%include Quaternion.i -%include Line2.i -%include Line3.i -%include Matrix3.i -%include Matrix4.i -%include Filter.i -%include MovingWindowFilter.i -%include PID.i -%include RollingMean.i -%include RotationSpline.i -%include SemanticVersion.i -%include SignalStats.i -%include Spline.i -%include Temperature.i -%include MaterialType.i -%include Material.i -%include Triangle.i -%include Triangle3.i -%include Kmeans.i -%include Vector3Stats.i -%include AxisAlignedBox.i -%include Plane.i -%include Frustum.i -%include MassMatrix3.i -%include OrientedBox.i -%include Box.i -%include Cylinder.i -%include Sphere.i -%include Inertial.i diff --git a/src/python/python_TEST.py b/src/python/python_TEST.py deleted file mode 100644 index ed393904b..000000000 --- a/src/python/python_TEST.py +++ /dev/null @@ -1,32 +0,0 @@ -# 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. - -import unittest -import ignition.math - - -class TestPythonInterface(unittest.TestCase): - - def test_construction(self): - angle1 = ignition.math.Angle() - self.assertEqual(angle1.radian(), 0.0) - v1 = ignition.math.Vector3d(0, 0, 0) - self.assertEqual(v1, ignition.math.Vector3d.ZERO) - v2 = ignition.math.Vector2d(1, 2) - self.assertEqual(v2.x(), 1) - self.assertEqual(v2.y(), 2) - - -if __name__ == '__main__': - unittest.main() diff --git a/src/python/AxisAlignedBox.i b/src/ruby/AxisAlignedBox.i similarity index 100% rename from src/python/AxisAlignedBox.i rename to src/ruby/AxisAlignedBox.i diff --git a/src/python/Box.i b/src/ruby/Box.i similarity index 100% rename from src/python/Box.i rename to src/ruby/Box.i diff --git a/src/python/Color.i b/src/ruby/Color.i similarity index 100% rename from src/python/Color.i rename to src/ruby/Color.i diff --git a/src/python/Cylinder.i b/src/ruby/Cylinder.i similarity index 100% rename from src/python/Cylinder.i rename to src/ruby/Cylinder.i diff --git a/src/python/DiffDriveOdometry.i b/src/ruby/DiffDriveOdometry.i similarity index 100% rename from src/python/DiffDriveOdometry.i rename to src/ruby/DiffDriveOdometry.i diff --git a/src/python/Filter.i b/src/ruby/Filter.i similarity index 100% rename from src/python/Filter.i rename to src/ruby/Filter.i diff --git a/src/python/Frustum.i b/src/ruby/Frustum.i similarity index 100% rename from src/python/Frustum.i rename to src/ruby/Frustum.i diff --git a/src/python/Helpers.i b/src/ruby/Helpers.i similarity index 100% rename from src/python/Helpers.i rename to src/ruby/Helpers.i diff --git a/src/python/Inertial.i b/src/ruby/Inertial.i similarity index 100% rename from src/python/Inertial.i rename to src/ruby/Inertial.i diff --git a/src/python/Kmeans.i b/src/ruby/Kmeans.i similarity index 100% rename from src/python/Kmeans.i rename to src/ruby/Kmeans.i diff --git a/src/python/Line2.i b/src/ruby/Line2.i similarity index 100% rename from src/python/Line2.i rename to src/ruby/Line2.i diff --git a/src/python/Line3.i b/src/ruby/Line3.i similarity index 100% rename from src/python/Line3.i rename to src/ruby/Line3.i diff --git a/src/python/MassMatrix3.i b/src/ruby/MassMatrix3.i similarity index 100% rename from src/python/MassMatrix3.i rename to src/ruby/MassMatrix3.i diff --git a/src/python/Material.i b/src/ruby/Material.i similarity index 100% rename from src/python/Material.i rename to src/ruby/Material.i diff --git a/src/python/MaterialType.i b/src/ruby/MaterialType.i similarity index 100% rename from src/python/MaterialType.i rename to src/ruby/MaterialType.i diff --git a/src/python/Matrix3.i b/src/ruby/Matrix3.i similarity index 100% rename from src/python/Matrix3.i rename to src/ruby/Matrix3.i diff --git a/src/python/Matrix4.i b/src/ruby/Matrix4.i similarity index 100% rename from src/python/Matrix4.i rename to src/ruby/Matrix4.i diff --git a/src/python/MovingWindowFilter.i b/src/ruby/MovingWindowFilter.i similarity index 100% rename from src/python/MovingWindowFilter.i rename to src/ruby/MovingWindowFilter.i diff --git a/src/python/OrientedBox.i b/src/ruby/OrientedBox.i similarity index 100% rename from src/python/OrientedBox.i rename to src/ruby/OrientedBox.i diff --git a/src/python/PID.i b/src/ruby/PID.i similarity index 100% rename from src/python/PID.i rename to src/ruby/PID.i diff --git a/src/python/Plane.i b/src/ruby/Plane.i similarity index 100% rename from src/python/Plane.i rename to src/ruby/Plane.i diff --git a/src/python/Pose3.i b/src/ruby/Pose3.i similarity index 100% rename from src/python/Pose3.i rename to src/ruby/Pose3.i diff --git a/src/python/Quaternion.i b/src/ruby/Quaternion.i similarity index 100% rename from src/python/Quaternion.i rename to src/ruby/Quaternion.i diff --git a/src/python/RollingMean.i b/src/ruby/RollingMean.i similarity index 100% rename from src/python/RollingMean.i rename to src/ruby/RollingMean.i diff --git a/src/python/RotationSpline.i b/src/ruby/RotationSpline.i similarity index 100% rename from src/python/RotationSpline.i rename to src/ruby/RotationSpline.i diff --git a/src/python/SemanticVersion.i b/src/ruby/SemanticVersion.i similarity index 100% rename from src/python/SemanticVersion.i rename to src/ruby/SemanticVersion.i diff --git a/src/python/SignalStats.i b/src/ruby/SignalStats.i similarity index 100% rename from src/python/SignalStats.i rename to src/ruby/SignalStats.i diff --git a/src/python/Sphere.i b/src/ruby/Sphere.i similarity index 100% rename from src/python/Sphere.i rename to src/ruby/Sphere.i diff --git a/src/python/SphericalCoordinates.i b/src/ruby/SphericalCoordinates.i similarity index 100% rename from src/python/SphericalCoordinates.i rename to src/ruby/SphericalCoordinates.i diff --git a/src/python/Spline.i b/src/ruby/Spline.i similarity index 100% rename from src/python/Spline.i rename to src/ruby/Spline.i diff --git a/src/python/StopWatch.i b/src/ruby/StopWatch.i similarity index 100% rename from src/python/StopWatch.i rename to src/ruby/StopWatch.i diff --git a/src/python/Temperature.i b/src/ruby/Temperature.i similarity index 100% rename from src/python/Temperature.i rename to src/ruby/Temperature.i diff --git a/src/python/Triangle.i b/src/ruby/Triangle.i similarity index 100% rename from src/python/Triangle.i rename to src/ruby/Triangle.i diff --git a/src/python/Triangle3.i b/src/ruby/Triangle3.i similarity index 100% rename from src/python/Triangle3.i rename to src/ruby/Triangle3.i diff --git a/src/python/Vector3Stats.i b/src/ruby/Vector3Stats.i similarity index 100% rename from src/python/Vector3Stats.i rename to src/ruby/Vector3Stats.i