Adds python interface to Filter, MovingWindowFilter, RotationSpline. (#…

…230)

* Adds python interface to Filter, MovingWindowFilter, RotationSpline.

Signed-off-by: LolaSegura <lsegura@ekumenlabs.com>

Co-authored-by: Steve Peters <scpeters@openrobotics.org>
Co-authored-by: Franco Cipollone <franco.c@ekumenlabs.com>

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

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 <ignition/math/config.hh>
+#include <ignition/math/Filter.hh>
+#include <ignition/math/Helpers.hh>
+#include <ignition/math/Quaternion.hh>
+#include <ignition/math/Vector3.hh>
+%}
+
+%import Quaternion.i
+
+namespace ignition
+{
+namespace math
+{
+    template <class T>
+    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<double>;
+    %template(Filterq) Filter<Quaternion<double>>;
+
+    template <class T>
+    class OnePole : public Filter<T>
+    {
+        %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<double>;
+
+    class OnePoleQuaternion
+    {
+        %rename("%(undercase)s", %$isfunction, %$ismember, %$not %$isconstructor) "";
+        public: virtual const Quaternion<double> &Value() const;
+        public: OnePoleQuaternion();
+        public: OnePoleQuaternion(double _fc, double _fs)
+            : OnePole<Quaternion<double>>(_fc, _fs);
+        public: const Quaternion<double>& Process(
+            const Quaternion<double> &_x);
+    };
+
+
+    class OnePoleVector3
+    {
+        %rename("%(undercase)s", %$isfunction, %$ismember, %$not %$isconstructor) "";
+        public: virtual const Vector3<double> &Value() const;
+        public: const Vector3<double>& Process(const Vector3<double> &_x);
+        public: OnePoleVector3();
+        public: OnePoleVector3(double _fc, double _fs)
+            : OnePole<Vector3<double>>(_fc, _fs);
+    };
+
+    template <class T>
+    class BiQuad : public Filter<T>
+    {
+        %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<double>;
+
+    class BiQuadVector3
+    {
+        %rename("%(undercase)s", %$isfunction, %$ismember, %$not %$isconstructor) "";
+        public: virtual const Vector3<double>& Process(const Vector3<double> &_x);
+        public: virtual const Vector3<double> &Value() const;
+        public: BiQuadVector3();
+        public: BiQuadVector3(double _fc, double _fs)
+            : BiQuad<Vector3<double>>(_fc, _fs);
+    };
+}
+}

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()

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<int>;
+    %template(MovingWindowFilterd) MovingWindowFilter<double>;
+    %template(MovingWindowFilterv3) MovingWindowFilter<Vector3<double>>;
+}
+}

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()

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 <ignition/math/RotationSpline.hh>
+#include <ignition/math/Quaternion.hh>
+#include <ignition/math/config.hh>
+%}
+
+namespace ignition
+{
+namespace math
+{
+    class RotationSpline
+    {
+        %rename("%(undercase)s", %$isfunction, %$ismember, %$not %$isconstructor) "";
+        public: RotationSpline();
+        public: ~RotationSpline();
+        public: void AddPoint(const Quaternion<double> &_p);
+        public: const Quaternion<double> &Point(const unsigned int _index) const;
+        public: unsigned int PointCount() const;
+        public: void Clear();
+        public: bool UpdatePoint(const unsigned int _index,
+                                 const Quaternion<double> &_value);
+        public: Quaternion<double> Interpolate(double _t,
+                                        const bool _useShortestPath = true);
+        public: Quaternion<double> Interpolate(const unsigned int _fromIndex,
+                    const double _t, const bool _useShortestPath = true);
+        public: void AutoCalculate(bool _autoCalc);
+        public: void RecalcTangents();
+    };
+}
+}