diff --git a/src/python/CMakeLists.txt b/src/python/CMakeLists.txt index 390ed1e84..4081d9b77 100644 --- a/src/python/CMakeLists.txt +++ b/src/python/CMakeLists.txt @@ -89,18 +89,21 @@ if (PYTHONLIBS_FOUND) set(python_tests Angle_TEST Color_TEST + Filter_TEST GaussMarkovProcess_TEST Kmeans_TEST Line2_TEST Line3_TEST Matrix3_TEST Matrix4_TEST + MovingWindowFilter_TEST PID_TEST Pose3_TEST python_TEST Quaternion_TEST Rand_TEST RollingMean_TEST + RotationSpline_TEST SemanticVersion_TEST SignalStats_TEST Spline_TEST diff --git a/src/python/Filter.i b/src/python/Filter.i new file mode 100644 index 000000000..be9e70562 --- /dev/null +++ b/src/python/Filter.i @@ -0,0 +1,104 @@ +/* + * 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 filter +%{ +#include +#include +#include +#include +#include +%} + +%import Quaternion.i + +namespace ignition +{ +namespace math +{ + template + class Filter + { + public: virtual ~Filter(); + public: virtual void Set(const T &_val); + public: virtual void Fc(double _fc, double _fs) = 0; + public: virtual const T &Value() const; + }; + + %template(Filterd) Filter; + %template(Filterq) Filter>; + + template + class OnePole : public Filter + { + %rename("%(undercase)s", %$isfunction, %$ismember, %$not %$isconstructor) ""; + public: OnePole() = default; + public: OnePole(double _fc, double _fs); + public: virtual void Fc(double _fc, double _fs) override; + public: const T& Process(const T &_x); + }; + + + %template(OnePoled) OnePole; + + class OnePoleQuaternion + { + %rename("%(undercase)s", %$isfunction, %$ismember, %$not %$isconstructor) ""; + public: virtual const Quaternion &Value() const; + public: OnePoleQuaternion(); + public: OnePoleQuaternion(double _fc, double _fs) + : OnePole>(_fc, _fs); + public: const Quaternion& Process( + const Quaternion &_x); + }; + + + class OnePoleVector3 + { + %rename("%(undercase)s", %$isfunction, %$ismember, %$not %$isconstructor) ""; + public: virtual const Vector3 &Value() const; + public: const Vector3& Process(const Vector3 &_x); + public: OnePoleVector3(); + public: OnePoleVector3(double _fc, double _fs) + : OnePole>(_fc, _fs); + }; + + template + class BiQuad : public Filter + { + %rename("%(undercase)s", %$isfunction, %$ismember, %$not %$isconstructor) ""; + public: BiQuad() = default; + public: BiQuad(double _fc, double _fs); + public: void Fc(double _fc, double _fs) override; + public: void Fc(double _fc, double _fs, double _q); + public: virtual void Set(const T &_val) override; + public: virtual const T& Process(const T &_x); + }; + + %template(BiQuadd) BiQuad; + + class BiQuadVector3 + { + %rename("%(undercase)s", %$isfunction, %$ismember, %$not %$isconstructor) ""; + public: virtual const Vector3& Process(const Vector3 &_x); + public: virtual const Vector3 &Value() const; + public: BiQuadVector3(); + public: BiQuadVector3(double _fc, double _fs) + : BiQuad>(_fc, _fs); + }; +} +} diff --git a/src/python/Filter_TEST.py b/src/python/Filter_TEST.py new file mode 100644 index 000000000..def7d9aa4 --- /dev/null +++ b/src/python/Filter_TEST.py @@ -0,0 +1,98 @@ +# 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 BiQuadd +from ignition.math import BiQuadVector3 +from ignition.math import OnePoled +from ignition.math import OnePoleQuaternion +from ignition.math import OnePoleVector3 +from ignition.math import Quaterniond +from ignition.math import Vector3d + + +class TestFilter(unittest.TestCase): + + def test_one_pole(self): + filter_a = OnePoled() + self.assertAlmostEqual(filter_a.process(0.2), 0.0) + + filter_a.fc(0.6, 1.4) + self.assertAlmostEqual(filter_a.process(2.5), 2.3307710879153634) + + filter_b = OnePoled(0.1, 0.2) + self.assertAlmostEqual(filter_b.process(0.5), 0.47839304086811385) + + filter_b.set(5.4) + self.assertAlmostEqual(filter_b.value(), 5.4) + + def test_one_pole_quaternion(self): + filter_a = OnePoleQuaternion() + self.assertAlmostEqual(filter_a.value(), Quaterniond(1, 0, 0, 0)) + + filter_b = OnePoleQuaternion(0.4, 1.4) + self.assertAlmostEqual(filter_b.value(), Quaterniond(1, 0, 0, 0)) + + self.assertAlmostEqual(filter_a.process(Quaterniond(0.1, 0.2, 0.3)), + Quaterniond(1, 0, 0, 0)) + + self.assertAlmostEqual(filter_b.process(Quaterniond(0.1, 0.2, 0.3)), + Quaterniond(0.98841, 0.0286272, + 0.0885614, 0.119929)) + + def test_one_pole_vector3(self): + filter_a = OnePoleVector3() + self.assertAlmostEqual(filter_a.value(), Vector3d(0, 0, 0)) + + filter_b = OnePoleVector3(1.2, 3.4) + self.assertAlmostEqual(filter_b.value(), Vector3d(0, 0, 0)) + + self.assertAlmostEqual(filter_a.process(Vector3d(0.1, 0.2, 0.3)), + Vector3d(0, 0, 0)) + + self.assertAlmostEqual(filter_b.process(Vector3d(0.1, 0.2, 0.3)), + Vector3d(0.089113, 0.178226, 0.267339)) + + def test_biquad(self): + filter_a = BiQuadd() + self.assertAlmostEqual(filter_a.value(), 0.0, delta=1e-10) + self.assertAlmostEqual(filter_a.process(1.1), 0.0, delta=1e-10) + + filter_a.fc(0.3, 1.4) + self.assertAlmostEqual(filter_a.process(1.2), 0.66924691484768517) + + filter_a.fc(0.3, 1.4, 0.1) + self.assertAlmostEqual(filter_a.process(10.25), 0.96057152402651302) + + filter_b = BiQuadd(4.3, 10.6) + self.assertAlmostEqual(filter_b.value(), 0.0, delta=1e-10) + self.assertAlmostEqual(filter_b.process(0.1234), 0.072418159950486546) + + filter_b.set(4.5) + self.assertAlmostEqual(filter_b.value(), 4.5) + + def test_biquad_vector3(self): + filter_a = BiQuadVector3() + self.assertEqual(filter_a.value(), Vector3d(0, 0, 0)) + self.assertEqual(filter_a.process(Vector3d(1.1, 2.3, 3.4)), + Vector3d(0, 0, 0)) + + filter_b = BiQuadVector3(6.5, 22.4) + self.assertEqual(filter_b.value(), Vector3d(0, 0, 0)) + self.assertEqual(filter_b.process(Vector3d(0.1, 20.3, 33.45)), + Vector3d(0.031748, 6.44475, 10.6196)) + + +if __name__ == '__main__': + unittest.main() diff --git a/src/python/MovingWindowFilter.i b/src/python/MovingWindowFilter.i new file mode 100644 index 000000000..ea6fc003d --- /dev/null +++ b/src/python/MovingWindowFilter.i @@ -0,0 +1,44 @@ +/* + * 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 movingwindowfilter +%{ +#include "ignition/math/MovingWindowFilter.hh" +#include "ignition/math/Vector3.hh" +%} + +namespace ignition +{ +namespace math +{ + template< typename T> + class MovingWindowFilter + { + %rename("%(undercase)s", %$isfunction, %$ismember, %$not %$isconstructor) ""; + public: MovingWindowFilter(); + public: virtual ~MovingWindowFilter(); + public: void Update(const T _val); + public: void SetWindowSize(const unsigned int _n); + public: unsigned int WindowSize() const; + public: bool WindowFilled() const; + public: T Value() const; + }; + + %template(MovingWindowFilteri) MovingWindowFilter; + %template(MovingWindowFilterd) MovingWindowFilter; + %template(MovingWindowFilterv3) MovingWindowFilter>; +} +} diff --git a/src/python/MovingWindowFilter_TEST.py b/src/python/MovingWindowFilter_TEST.py new file mode 100644 index 000000000..bf8bf691c --- /dev/null +++ b/src/python/MovingWindowFilter_TEST.py @@ -0,0 +1,66 @@ +# 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 MovingWindowFilterd +from ignition.math import MovingWindowFilteri +from ignition.math import MovingWindowFilterv3 +from ignition.math import Vector3d + + +class TestFilter(unittest.TestCase): + + def test_set_window_size(self): + filter_int = MovingWindowFilteri() + + self.assertEqual(filter_int.window_size(), 4) + self.assertFalse(filter_int.window_filled()) + + filter_int.set_window_size(10) + self.assertEqual(filter_int.window_size(), 10) + self.assertFalse(filter_int.window_filled()) + + def test_filter_something(self): + double_mwf = MovingWindowFilterd() + double_mwf_2 = MovingWindowFilterd() + vector_mwf = MovingWindowFilterv3() + + double_mwf.set_window_size(10) + double_mwf_2.set_window_size(2) + vector_mwf.set_window_size(40) + + for i in range(20): + double_mwf.update(i) + double_mwf_2.update(i) + v = Vector3d(1.0*i, + 2.0*i, + 3.0*i) + vector_mwf.update(v) + + sum = 0 + for i in range(20-10, 20, 1): + sum += i + self.assertAlmostEqual(double_mwf.value(), sum/10.0) + self.assertAlmostEqual(double_mwf_2.value(), (18.0+19.0)/2.0) + + vsum = Vector3d() + for i in range(20): + vsum += Vector3d(1.0*i, + 2.0*i, + 3.0*i) + self.assertAlmostEqual(vector_mwf.value(), vsum / 20.0) + + +if __name__ == '__main__': + unittest.main() diff --git a/src/python/RotationSpline.i b/src/python/RotationSpline.i new file mode 100644 index 000000000..7f62e815d --- /dev/null +++ b/src/python/RotationSpline.i @@ -0,0 +1,48 @@ +/* + * 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 rotationspline +%{ +#include +#include +#include +%} + +namespace ignition +{ +namespace math +{ + class RotationSpline + { + %rename("%(undercase)s", %$isfunction, %$ismember, %$not %$isconstructor) ""; + public: RotationSpline(); + public: ~RotationSpline(); + public: void AddPoint(const Quaternion &_p); + public: const Quaternion &Point(const unsigned int _index) const; + public: unsigned int PointCount() const; + public: void Clear(); + public: bool UpdatePoint(const unsigned int _index, + const Quaternion &_value); + public: Quaternion Interpolate(double _t, + const bool _useShortestPath = true); + public: Quaternion Interpolate(const unsigned int _fromIndex, + const double _t, const bool _useShortestPath = true); + public: void AutoCalculate(bool _autoCalc); + public: void RecalcTangents(); + }; +} +} diff --git a/src/python/RotationSpline_TEST.py b/src/python/RotationSpline_TEST.py new file mode 100644 index 000000000..598226157 --- /dev/null +++ b/src/python/RotationSpline_TEST.py @@ -0,0 +1,83 @@ +# 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 Quaterniond +from ignition.math import RotationSpline +from ignition.math import Vector3d + + +class TestRotationSpline(unittest.TestCase): + def test_rotation_spline(self): + s = RotationSpline() + + s.add_point(Quaterniond(0, 0, 0)) + self.assertEqual(1, s.point_count()) + + s.clear() + self.assertEqual(0, s.point_count()) + + s.add_point(Quaterniond(0, 0, 0)) + self.assertTrue(s.point(0) == Quaterniond(0, 0, 0)) + s.add_point(Quaterniond(.1, .1, .1)) + self.assertTrue(s.point(1) == Quaterniond(.1, .1, .1)) + + # update_point + self.assertFalse(s.update_point(2, Quaterniond(.2, .2, .2))) + + self.assertTrue(s.update_point(1, Quaterniond(.2, .2, .2))) + s.auto_calculate(False) + self.assertTrue(s.update_point(0, Quaterniond( + Vector3d(-.1, -.1, -.1)))) + s.auto_calculate(True) + + # interpolate + self.assertTrue(s.interpolate(0.5) == + Quaterniond(0.998089, 0.0315333, 0.0427683, 0.0315333)) + + # interpolate + s.add_point(Quaterniond(.4, .4, .4)) + self.assertFalse(s.interpolate(4, 0.2).is_finite()) + + self.assertEqual(s.interpolate(s.point_count()-1, 0.2), + s.point(s.point_count()-1)) + self.assertTrue(s.interpolate(1, 0.2) == + Quaterniond(0.978787, 0.107618, 0.137159, 0.107618)) + self.assertEqual(s.interpolate(1, 0.0), s.point(1)) + self.assertEqual(s.interpolate(1, 1.0), s.point(2)) + + def test_get_point(self): + s = RotationSpline() + self.assertFalse(s.point(0).is_finite()) + self.assertFalse(s.point(1).is_finite()) + + s.add_point(Quaterniond(0, 0, 0)) + self.assertTrue(s.point(1).is_finite()) + + def test_recalc_tangents(self): + s = RotationSpline() + s.add_point(Quaterniond(0, 0, 0)) + s.add_point(Quaterniond(.4, .4, .4)) + s.add_point(Quaterniond(0, 0, 0)) + + s.recalc_tangents() + self.assertEqual(s.interpolate(0, 0.5), + Quaterniond(0.987225, 0.077057, 0.11624, 0.077057)) + + self.assertEqual(s.interpolate(1, 0.5), + Quaterniond(0.987225, 0.077057, 0.11624, 0.077057)) + + +if __name__ == '__main__': + unittest.main() diff --git a/src/python/python.i b/src/python/python.i index 18a73da43..ff0046db6 100644 --- a/src/python/python.i +++ b/src/python/python.i @@ -12,8 +12,11 @@ %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