From a22cf39ef19d28e9db3d73cf8ec6e90156836b76 Mon Sep 17 00:00:00 2001 From: LolaSegura Date: Fri, 13 Aug 2021 16:51:02 -0300 Subject: [PATCH 01/15] Adds scripting interface to Quaternion and a python test Signed-off-by: LolaSegura --- src/Quaternion.i | 142 ++++++++++++++ src/Quaternion_TEST.py | 424 +++++++++++++++++++++++++++++++++++++++++ 2 files changed, 566 insertions(+) create mode 100644 src/Quaternion.i create mode 100644 src/Quaternion_TEST.py diff --git a/src/Quaternion.i b/src/Quaternion.i new file mode 100644 index 000000000..31897a8d3 --- /dev/null +++ b/src/Quaternion.i @@ -0,0 +1,142 @@ +/* + * 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 quaternion +%{ +#include +#include +#include +#include +%} + +%include "std_string.i" + +namespace ignition +{ + namespace math + { + template + class Quaternion + { + public: static const Quaternion Identity; + public: static const Quaternion Zero; + + public: Quaternion(); + public: Quaternion(const T &_w, const T &_x, const T &_y, const T &_z); + public: Quaternion(const T &_roll, const T &_pitch, const T &_yaw); + public: Quaternion(const Vector3 &_axis, const T &_angle); + public: explicit Quaternion(const Vector3 &_rpy); + public: Quaternion(const Quaternion &_qt); + public: ~Quaternion(); + public: void Invert(); + public: inline Quaternion Inverse() const; + public: Quaternion Log() const; + public: Quaternion Exp() const; + public: void Normalize(); + public: Quaternion Normalized() const; + public: void Axis(T _ax, T _ay, T _az, T _aa); + public: void Axis(const Vector3 &_axis, T _a); + public: void Set(T _w, T _x, T _y, T _z); + public: void Euler(const Vector3 &_vec); + public: void Euler(T _roll, T _pitch, T _yaw); + public: Vector3 Euler() const; + public: static Quaternion EulerToQuaternion(const Vector3 &_vec); + public: static Quaternion EulerToQuaternion(T _x, T _y, T _z); + public: T Roll() const; + public: T Pitch() const; + public: T Yaw() const; + public: void ToAxis(Vector3 &_axis, T &_angle) const; + public: void From2Axes(const Vector3 &_v1, const Vector3 &_v2); + public: void Scale(T _scale); + public: Quaternion operator+(const Quaternion &_qt) const; + public: Quaternion operator+=(const Quaternion &_qt); + public: Quaternion operator-(const Quaternion &_qt) const; + public: Quaternion operator-=(const Quaternion &_qt); + public: inline Quaternion operator*(const Quaternion &_q) const; + public: Quaternion operator*(const T &_f) const; + public: Quaternion operator*=(const Quaternion &_qt); + public: Vector3 operator*(const Vector3 &_v) const; + public: bool Equal(const Quaternion &_qt, const T &_tol) const; + public: bool operator==(const Quaternion &_qt) const; + public: bool operator!=(const Quaternion &_qt) const; + public: Quaternion operator-() const; + public: inline Vector3 RotateVector(const Vector3 &_vec) const; + public: Vector3 RotateVectorReverse(const Vector3 &_vec) const; + public: bool IsFinite() const; + public: inline void Correct(); + public: Vector3 XAxis() const; + public: Vector3 YAxis() const; + public: Vector3 ZAxis() const; + public: void Round(int _precision); + public: T Dot(const Quaternion &_q) const; + public: static Quaternion Squad(T _fT, + const Quaternion &_rkP, const Quaternion &_rkA, + const Quaternion &_rkB, const Quaternion &_rkQ, + bool _shortestPath = false); + public: static Quaternion Slerp(T _fT, + const Quaternion &_rkP, const Quaternion &_rkQ, + bool _shortestPath = false); + public: Quaternion Integrate(const Vector3 &_angularVelocity, + const T _deltaT) const; + + public: inline void X(T _v); + public: inline void Y(T _v); + public: inline void Z(T _v); + public: inline void W(T _v); + + private: T qw; + private: T qx; + private: T qy; + private: T qz; + }; + + %extend Quaternion{ + inline T W() const + { + return (*$self).W(); + } + + inline T X() const + { + return (*$self).X(); + } + + inline T Y() const + { + return (*$self).Y(); + } + + inline T Z() const + { + return (*$self).Z(); + } + } + + %extend Quaternion { + std::string __str__() const { + std::ostringstream out; + out << *$self; + return out.str(); + } + } + + %template(Quaternioni) Quaternion; + %template(Quaterniond) Quaternion; + %template(Quaternionf) Quaternion; + } +} + diff --git a/src/Quaternion_TEST.py b/src/Quaternion_TEST.py new file mode 100644 index 000000000..52f631555 --- /dev/null +++ b/src/Quaternion_TEST.py @@ -0,0 +1,424 @@ +# 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 math +from ignition.math import Quaterniond +from ignition.math import Quaternionf +from ignition.math import Quaternioni +from ignition.math import Vector3d + + +class TestQuaternion(unittest.TestCase): + def test_construction(self): + q = Quaterniond(0, 0, 0, 1) + + q2 = Quaterniond(q) + self.assertEqual(q2, q) + + q3 = q + self.assertEqual(q3, q) + + q4 = Quaterniond(q) + self.assertEqual(q4, q2) + q = q4 + self.assertEqual(q, q2) + + q5 = q2 + self.assertEqual(q5, q3) + q2 = q5 + self.assertEqual(q2, q3) + + q6 = Quaterniond() + self.assertNotEqual(q6, q3) + + def test_unit(self): + q = Quaterniond() + self.assertAlmostEqual(q.W(), 1.0) + self.assertAlmostEqual(q.X(), 0.0) + self.assertAlmostEqual(q.Y(), 0.0) + self.assertAlmostEqual(q.Z(), 0.0) + + def test_construct_values(self): + q = Quaterniond(1.0, 2.0, 3.0, 4.0) + self.assertAlmostEqual(q.W(), 1.0) + self.assertAlmostEqual(q.X(), 2.0) + self.assertAlmostEqual(q.Y(), 3.0) + self.assertAlmostEqual(q.Z(), 4.0) + + def test_construct_zero(self): + q = Quaterniond(0.0, 0.0, 0.0, 0.0) + self.assertAlmostEqual(q.W(), 0.0) + self.assertAlmostEqual(q.X(), 0.0) + self.assertAlmostEqual(q.Y(), 0.0) + self.assertAlmostEqual(q.Z(), 0.0) + + qI = q.Inverse() + self.assertAlmostEqual(qI.W(), 1.0) + self.assertAlmostEqual(qI.X(), 0.0) + self.assertAlmostEqual(qI.Y(), 0.0) + self.assertAlmostEqual(qI.Z(), 0.0) + + def test_construct_euler(self): + q = Quaterniond(0, 1, 2) + self.assertAlmostEqual(q, Quaterniond(Vector3d(0, 1, 2))) + + def test_construct_axis_angle(self): + q1 = Quaterniond(Vector3d(0, 0, 1), math.pi) + + self.assertAlmostEqual(q1.X(), 0.0) + self.assertAlmostEqual(q1.Y(), 0.0) + self.assertAlmostEqual(q1.Z(), 1.0) + self.assertAlmostEqual(q1.W(), 0.0) + + q = Quaterniond(q1) + self.assertTrue(q == q1) + + def test_equal(self): + # double + q = Quaterniond(1, 2, 3, 4) + q2 = Quaterniond(1.01, 2.015, 3.002, 4.007) + self.assertTrue(q.Equal(q2, 0.02)) + self.assertFalse(q.Equal(q2, 0.01)) + + # floats + q3 = Quaternionf(1, 2, 3, 4) + q4 = Quaternionf(1.05, 2.1, 3.03, 4.04) + self.assertTrue(q3.Equal(q4, 0.2)) + self.assertFalse(q3.Equal(q4, 0.04)) + + # ints + q5 = Quaternioni(3, 5, -1, 9) + q6 = Quaternioni(3, 6, 1, 12) + self.assertTrue(q5.Equal(q6, 3)) + self.assertFalse(q5.Equal(q6, 2)) + + def test_identity(self): + q = Quaterniond.Identity + self.assertAlmostEqual(q.W(), 1.0) + self.assertAlmostEqual(q.X(), 0.0) + self.assertAlmostEqual(q.Y(), 0.0) + self.assertAlmostEqual(q.Z(), 0.0) + + def test_mathlog(self): + q = Quaterniond(math.pi*0.1, math.pi*0.5, math.pi) + self.assertAlmostEqual(q.Log(), + Quaterniond(0, -1.02593, 0.162491, 1.02593)) + + q1 = Quaterniond(q) + q1.W(2.0) + self.assertAlmostEqual(q1.Log(), + Quaterniond(0, -0.698401, 0.110616, 0.698401)) + + def test_math_exp(self): + q = Quaterniond(math.pi*0.1, math.pi*0.5, math.pi) + self.assertAlmostEqual(q.Exp(), Quaterniond(0.545456, -0.588972, + 0.093284, 0.588972)) + + q1 = Quaterniond(q) + q1.X(0.000000001) + q1.Y(0.0) + q1.Z(0.0) + q1.W(0.0) + self.assertAlmostEqual(q1.Exp(), Quaterniond(1, 0, 0, 0)) + + def test_math_invert(self): + q = Quaterniond(math.pi*0.1, math.pi*0.5, math.pi) + + q.Invert() + self.assertAlmostEqual(q, Quaterniond(0.110616, 0.698401, + -0.110616, -0.698401)) + + def test_math_axis(self): + q = Quaterniond(math.pi*0.1, math.pi*0.5, math.pi) + + q.Axis(0, 1, 0, math.pi) + self.assertAlmostEqual(q, Quaterniond(6.12303e-17, 0, 1, 0)) + + q.Axis(Vector3d(1, 0, 0), math.pi) + self.assertAlmostEqual(q, Quaterniond(0, 1, 0, 0)) + + def test_math_set(self): + q = Quaterniond(math.pi*0.1, math.pi*0.5, math.pi) + + q.Set(1, 2, 3, 4) + self.assertAlmostEqual(q.W(), 1.0) + self.assertAlmostEqual(q.X(), 2.0) + self.assertAlmostEqual(q.Y(), 3.0) + self.assertAlmostEqual(q.Z(), 4.0) + + def test_math_normalized(self): + q = Quaterniond(1, 2, 3, 4) + + q2 = q.Normalized() + self.assertAlmostEqual(q2, Quaterniond(0.182574, 0.365148, + 0.547723, 0.730297)) + + def test_normalize(self): + q = Quaterniond(1, 2, 3, 4) + + q.Normalize() + self.assertAlmostEqual(q, Quaterniond(0.182574, 0.365148, + 0.547723, 0.730297)) + + def test_slerp(self): + q1 = Quaterniond(0.1, 1.2, 2.3) + q2 = Quaterniond(1.2, 2.3, -3.4) + + q3 = Quaterniond.Slerp(1.0, q1, q2, True) + self.assertAlmostEqual(q3, Quaterniond(0.554528, -0.717339, + 0.32579, 0.267925)) + + def test_from2axes(self): + v1 = Vector3d(1.0, 0.0, 0.0) + v2 = Vector3d(0.0, 1.0, 0.0) + + q1 = Quaterniond() + q1.From2Axes(v1, v2) + + q2 = Quaterniond() + q2.From2Axes(v2, v1) + + q2Correct = Quaterniond(math.sqrt(2)/2, 0, 0, -math.sqrt(2)/2) + q1Correct = Quaterniond(math.sqrt(2)/2, 0, 0, math.sqrt(2)/2) + + self.assertNotEqual(q1, q2) + self.assertAlmostEqual(q1Correct, q1) + self.assertAlmostEqual(q2Correct, q2) + self.assertAlmostEqual(Quaterniond.Identity, q1 * q2) + self.assertAlmostEqual(v2, q1 * v1) + self.assertAlmostEqual(v1, q2 * v2) + + # still the same rotation, but with non-unit vectors + v1.Set(0.5, 0.5, 0) + v2.Set(-0.5, 0.5, 0) + + q1.From2Axes(v1, v2) + q2.From2Axes(v2, v1) + + self.assertNotEqual(q1, q2) + self.assertAlmostEqual(q1Correct, q1) + self.assertAlmostEqual(q2Correct, q2) + self.assertAlmostEqual(Quaterniond.Identity, q1 * q2) + self.assertAlmostEqual(v2, q1 * v1) + self.assertAlmostEqual(v1, q2 * v2) + + # Test various settings of opposite vectors (which need special care) + tolerance = 1e-4 + + v1.Set(1, 0, 0) + v2.Set(-1, 0, 0) + q1.From2Axes(v1, v2) + q2 = q1 * q1 + self.assertTrue(abs(q2.W()-1.0) <= tolerance or + abs(q2.W()-(-1.0)) <= tolerance) + self.assertAlmostEqual(q2.X(), 0.0) + self.assertAlmostEqual(q2.Y(), 0.0) + self.assertAlmostEqual(q2.Z(), 0.0) + + v1.Set(0, 1, 0) + v2.Set(0, -1, 0) + q1.From2Axes(v1, v2) + q2 = q1 * q1 + self.assertTrue(abs(q2.W()-1.0) <= tolerance or + abs(q2.W()-(-1.0)) <= tolerance) + self.assertAlmostEqual(q2.X(), 0.0) + self.assertAlmostEqual(q2.Y(), 0.0) + self.assertAlmostEqual(q2.Z(), 0.0) + + v1.Set(0, 0, 1) + v2.Set(0, 0, -1) + q1.From2Axes(v1, v2) + q2 = q1 * q1 + self.assertTrue(abs(q2.W()-1.0) <= tolerance or + abs(q2.W()-(-1.0)) <= tolerance) + self.assertAlmostEqual(q2.X(), 0.0) + self.assertAlmostEqual(q2.Y(), 0.0) + self.assertAlmostEqual(q2.Z(), 0.0) + + v1.Set(0, 1, 1) + v2.Set(0, -1, -1) + q1.From2Axes(v1, v2) + q2 = q1 * q1 + self.assertTrue(abs(q2.W()-1.0) <= tolerance or + abs(q2.W()-(-1.0)) <= tolerance) + self.assertAlmostEqual(q2.X(), 0.0) + self.assertAlmostEqual(q2.Y(), 0.0) + self.assertAlmostEqual(q2.Z(), 0.0) + + def test_math(self): + q = Quaterniond(math.pi*0.1, math.pi*0.5, math.pi) + self.assertTrue(q == Quaterniond(0.110616, -0.698401, + 0.110616, 0.698401)) + + q.Set(1, 2, 3, 4) + + q.Normalize() + + self.assertAlmostEqual(q.Roll(), 1.4289, delta=1e-3) + self.assertAlmostEqual(q.Pitch(), -0.339837, delta=1e-3) + self.assertAlmostEqual(q.Yaw(), 2.35619, delta=1e-3) + + q.Scale(0.1) + self.assertTrue(q == Quaterniond(0.990394, 0.051354, + 0.0770309, 0.102708)) + + q = q + Quaterniond(0, 1, 2) + self.assertTrue(q == Quaterniond(1.46455, -0.352069, + 0.336066, 0.841168)) + + q += q + self.assertTrue(q == Quaterniond(2.92911, -0.704137, + 0.672131, 1.68234)) + + q -= Quaterniond(.4, .2, .1) + self.assertTrue(q == Quaterniond(1.95416, -0.896677, 0.56453, 1.65341)) + + q = q - Quaterniond(0, 1, 2) + self.assertTrue(q == Quaterniond(1.48, -0.493254, + 0.305496, 0.914947)) + + q *= Quaterniond(.4, .1, .01) + self.assertTrue(q == Quaterniond(1.53584, -0.236801, + 0.551841, 0.802979)) + + q = q * 5.0 + self.assertTrue(q == Quaterniond(7.67918, -1.184, 2.7592, 4.0149)) + + self.assertTrue(q.RotateVectorReverse(Vector3d(1, 2, 3)) == + Vector3d(-0.104115, 0.4975, 3.70697)) + + self.assertAlmostEqual(q.Dot(Quaterniond(.4, .2, .1)), 7.67183, + delta=1e-3) + + self.assertTrue(Quaterniond.Squad(1.1, Quaterniond(.1, 0, .2), + Quaterniond(0, .3, .4), Quaterniond(.5, .2, 1), + Quaterniond(0, 0, 2), True) == + Quaterniond(0.346807, -0.0511734, + -0.0494723, 0.935232)) + + self.assertTrue(Quaterniond.EulerToQuaternion( + Vector3d(.1, .2, .3)) == + Quaterniond(0.983347, 0.0342708, + 0.106021, 0.143572)) + + q.Round(2) + self.assertAlmostEqual(-1.18, q.X()) + self.assertAlmostEqual(2.76, q.Y()) + self.assertAlmostEqual(4.01, q.Z()) + self.assertAlmostEqual(7.68, q.W()) + + q.X(0.0) + q.Y(0.0) + q.Z(0.0) + q.W(0.0) + q.Normalize() + self.assertTrue(q == Quaterniond()) + + q.Axis(0, 0, 0, 0) + self.assertTrue(q == Quaterniond()) + + self.assertTrue(Quaterniond.EulerToQuaternion(0.1, 0.2, 0.3) == + Quaterniond(0.983347, 0.0342708, 0.106021, 0.143572)) + + def test_stream_out(self): + q = Quaterniond(0.1, 1.2, 2.3) + self.assertEqual(str(q), "0.1 1.2 2.3") + + def test_integrate(self): + # Integrate by zero, expect no change + q = Quaterniond(0.5, 0.5, 0.5, 0.5) + self.assertAlmostEqual(q, q.Integrate(Vector3d.Zero, 1.0)) + self.assertAlmostEqual(q, q.Integrate(Vector3d.UnitX, 0.0)) + self.assertAlmostEqual(q, q.Integrate(Vector3d.UnitY, 0.0)) + self.assertAlmostEqual(q, q.Integrate(Vector3d.UnitZ, 0.0)) + + # Integrate along single axes, + # expect linear change in roll, pitch, yaw + q = Quaterniond(1, 0, 0, 0) + qRoll = q.Integrate(Vector3d.UnitX, 1.0) + qPitch = q.Integrate(Vector3d.UnitY, 1.0) + qYaw = q.Integrate(Vector3d.UnitZ, 1.0) + self.assertAlmostEqual(qRoll.Euler(), Vector3d.UnitX) + self.assertAlmostEqual(qPitch.Euler(), Vector3d.UnitY) + self.assertAlmostEqual(qYaw.Euler(), Vector3d.UnitZ) + + # Integrate sequentially along single axes in order XYZ, + # expect rotations to match Euler Angles + q = Quaterniond(1, 0, 0, 0) + angle = 0.5 + qX = q.Integrate(Vector3d.UnitX, angle) + qXY = qX.Integrate(Vector3d.UnitY, angle) + self.assertAlmostEqual(qXY.Euler(), Vector3d(1, 1, 0)*angle) + + q = Quaterniond(1, 0, 0, 0) + angle = 0.5 + qX = q.Integrate(Vector3d.UnitX, angle) + qXZ = qX.Integrate(Vector3d.UnitZ, angle) + self.assertAlmostEqual(qXZ.Euler(), Vector3d(1, 0, 1)*angle) + + q = Quaterniond(1, 0, 0, 0) + angle = 0.5 + qY = q.Integrate(Vector3d.UnitY, angle) + qYZ = qY.Integrate(Vector3d.UnitZ, angle) + self.assertAlmostEqual(qYZ.Euler(), Vector3d(0, 1, 1)*angle) + + q = Quaterniond(1, 0, 0, 0) + angle = 0.5 + qX = q.Integrate(Vector3d.UnitX, angle) + qXY = qX.Integrate(Vector3d.UnitY, angle) + qXYZ = qXY.Integrate(Vector3d.UnitZ, angle) + self.assertAlmostEqual(qXYZ.Euler(), Vector3d.One*angle) + + # Integrate sequentially along single axes in order ZYX, + # expect rotations to not match Euler Angles + q = Quaterniond(1, 0, 0, 0) + angle = 0.5 + qZ = q.Integrate(Vector3d.UnitZ, angle) + qZY = qZ.Integrate(Vector3d.UnitY, angle) + self.assertNotEqual(qZY.Euler(), Vector3d(0, 1, 1)*angle) + + q = Quaterniond(1, 0, 0, 0) + angle = 0.5 + qZ = q.Integrate(Vector3d.UnitZ, angle) + qZX = qZ.Integrate(Vector3d.UnitX, angle) + self.assertNotEqual(qZX.Euler(), Vector3d(1, 0, 1)*angle) + + q = Quaterniond(1, 0, 0, 0) + angle = 0.5 + qZ = q.Integrate(Vector3d.UnitZ, angle) + qZY = qZ.Integrate(Vector3d.UnitY, angle) + qZYX = qZY.Integrate(Vector3d.UnitX, angle) + self.assertNotEqual(qZYX.Euler(), Vector3d(1, 1, 1)*angle) + + q = Quaterniond(1, 0, 0, 0) + angle = 0.5 + qY = q.Integrate(Vector3d.UnitY, angle) + qYX = qY.Integrate(Vector3d.UnitX, angle) + self.assertNotEqual(qYX.Euler(), Vector3d(1, 1, 0)*angle) + + # Integrate a full rotation about different axes, + # expect no change. + q = Quaterniond(0.5, 0.5, 0.5, 0.5) + fourPi = 4 * math.pi + qX = q.Integrate(Vector3d.UnitX, fourPi) + qY = q.Integrate(Vector3d.UnitY, fourPi) + qZ = q.Integrate(Vector3d.UnitZ, fourPi) + self.assertAlmostEqual(q, qX) + self.assertAlmostEqual(q, qY) + self.assertAlmostEqual(q, qZ) + + +if __name__ == '__main__': + unittest.main() From aec4a6fdc1011ff112a6e6b0ff7e666cdda2b555 Mon Sep 17 00:00:00 2001 From: LolaSegura Date: Fri, 13 Aug 2021 16:52:43 -0300 Subject: [PATCH 02/15] Adds scripting interface to Matrix3 and a python test Signed-off-by: LolaSegura --- src/Matrix3.i | 87 +++++++++++++ src/Matrix3_TEST.py | 299 ++++++++++++++++++++++++++++++++++++++++++++ 2 files changed, 386 insertions(+) create mode 100644 src/Matrix3.i create mode 100644 src/Matrix3_TEST.py diff --git a/src/Matrix3.i b/src/Matrix3.i new file mode 100644 index 000000000..1b8ef81a7 --- /dev/null +++ b/src/Matrix3.i @@ -0,0 +1,87 @@ +/* + * 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 matrix3 +%{ +#include +#include +#include +#include +#include +%} + +%include "std_string.i" + +namespace ignition +{ + namespace math + { + template class Quaternion; + template + class Matrix3 + { + public: static const Matrix3 Identity; + public: static const Matrix3 Zero; + public: Matrix3(); + public: Matrix3(const Matrix3 &_m); + public: Matrix3(T _v00, T _v01, T _v02, + T _v10, T _v11, T _v12, + T _v20, T _v21, T _v22); + public: explicit Matrix3(const Quaternion &_q); + public: virtual ~Matrix3() {} + public: void Set(T _v00, T _v01, T _v02, + T _v10, T _v11, T _v12, + T _v20, T _v21, T _v22); + public: void Axes(const Vector3 &_xAxis, + const Vector3 &_yAxis, + const Vector3 &_zAxis); + public: void Axis(const Vector3 &_axis, T _angle); + public: void From2Axes(const Vector3 &_v1, const Vector3 &_v2); + public: void Col(unsigned int _c, const Vector3 &_v); + public: Matrix3 operator-(const Matrix3 &_m) const; + public: Matrix3 operator+(const Matrix3 &_m) const; + public: Matrix3 operator*(const T &_s) const; + public: Matrix3 operator*(const Matrix3 &_m) const; + public: Vector3 operator*(const Vector3 &_vec) const; + public: bool Equal(const Matrix3 &_m, const T &_tol) const; + public: bool operator==(const Matrix3 &_m) const; + public: bool operator!=(const Matrix3 &_m) const;; + public: T Determinant() const; + public: Matrix3 Inverse() const; + public: void Transpose(); + public: Matrix3 Transposed() const; + }; + + %extend Matrix3{ + T __call__(size_t row, size_t col) const { + return (*$self)(row, col); + } + } + + %extend Matrix3 { + std::string __str__() const { + std::ostringstream out; + out << *$self; + return out.str(); + } + } + + %template(Matrix3i) Matrix3; + %template(Matrix3d) Matrix3; + %template(Matrix3f) Matrix3; + } +} \ No newline at end of file diff --git a/src/Matrix3_TEST.py b/src/Matrix3_TEST.py new file mode 100644 index 000000000..a7bf268ea --- /dev/null +++ b/src/Matrix3_TEST.py @@ -0,0 +1,299 @@ +# 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 math +from ignition.math import Matrix3d +from ignition.math import Vector3d + + +class TestMatrix3(unittest.TestCase): + + def test_matrix3d(self): + matrix = Matrix3d() + self.assertAlmostEqual(matrix, Matrix3d(0, 0, 0, 0, 0, 0, 0, 0, 0)) + + matrix = Matrix3d(1, 2, 3, 4, 5, 6, 7, 8, 9) + self.assertAlmostEqual(matrix, Matrix3d(1, 2, 3, 4, 5, 6, 7, 8, 9)) + + matrix1 = Matrix3d(matrix) + self.assertAlmostEqual(matrix1, Matrix3d(1, 2, 3, 4, 5, 6, 7, 8, 9)) + + matrix = Matrix3d() + matrix.Axes(Vector3d(1, 1, 1), Vector3d(2, 2, 2), + Vector3d(3, 3, 3)) + self.assertAlmostEqual(matrix, Matrix3d(1, 2, 3, 1, 2, 3, 1, 2, 3)) + + matrix.Axis(Vector3d(1, 1, 1), math.pi) + self.assertAlmostEqual(matrix, Matrix3d(1, 2, 2, 2, 1, 2, 2, 2, 1)) + + matrix.Col(0, Vector3d(3, 4, 5)) + self.assertAlmostEqual(matrix, Matrix3d(3, 2, 2, 4, 1, 2, 5, 2, 1)) + + matrix.Col(3, Vector3d(1, 1, 1)) + self.assertAlmostEqual(matrix, Matrix3d(3, 2, 1, 4, 1, 1, 5, 2, 1)) + + def test_sub(self): + matZero = Matrix3d.Zero + matIdent = Matrix3d.Identity + + mat = matIdent - matZero + self.assertAlmostEqual(mat, matIdent) + + matA = Matrix3d(1, 2, 3, + 4, 5, 6, + 7, 8, 9) + matB = Matrix3d(10, 20, 30, + 40, 50, 60, + 70, 80, 90) + + mat = matB - matA + self.assertAlmostEqual(mat, Matrix3d(9, 18, 27, + 36, 45, 54, + 63, 72, 81)) + + def test_add(self): + + matZero = Matrix3d.Zero + matIdent = Matrix3d.Identity + + mat = matIdent + matZero + self.assertAlmostEqual(mat, matIdent) + + matA = Matrix3d(1, 2, 3, + 4, 5, 6, + 7, 8, 9) + matB = Matrix3d(10, 20, 30, + 40, 50, 60, + 70, 80, 90) + + mat = matB + matA + self.assertAlmostEqual(mat, Matrix3d(11, 22, 33, + 44, 55, 66, + 77, 88, 99)) + + def test_mul(self): + matZero = Matrix3d.Zero + matIdent = Matrix3d.Identity + + mat = matIdent * matZero + self.assertAlmostEqual(mat, matZero) + + matA = Matrix3d(1, 2, 3, + 4, 5, 6, + 7, 8, 9) + matB = Matrix3d(10, 20, 30, + 40, 50, 60, + 70, 80, 90) + + mat = matB * matA + self.assertAlmostEqual(mat, Matrix3d(300, 360, 420, + 660, 810, 960, + 1020, 1260, 1500)) + + mat = matB * matA + self.assertAlmostEqual(mat, Matrix3d(300, 360, 420, + 660, 810, 960, + 1020, 1260, 1500)) + + mat = mat * 2.0 + self.assertAlmostEqual(mat, Matrix3d(600, 720, 840, + 1320, 1620, 1920, + 2040, 2520, 3000)) + + def test_vector3_mul(self): + matrix = Matrix3d(1, 2, 3, 4, 5, 6, 7, 8, 9) + + # Scalar + self.assertAlmostEqual(Matrix3d.Zero, matrix * 0) + + # Vector3.Zero + self.assertAlmostEqual(Vector3d.Zero, matrix * Vector3d.Zero) + + # Matrix3.Zero + self.assertAlmostEqual(Matrix3d.Zero, matrix * Matrix3d.Zero) + self.assertAlmostEqual(Matrix3d.Zero, Matrix3d.Zero * matrix) + + matrix = Matrix3d(1, 2, 3, 4, 5, 6, 7, 8, 9) + + # scalar 1.0 + self.assertAlmostEqual(matrix, matrix * 1.0) + + # Vector3.Unit + # right multiply + self.assertAlmostEqual(Vector3d(matrix(0, 0), matrix(1, 0), + matrix(2, 0)), matrix * Vector3d.UnitX) + self.assertAlmostEqual(Vector3d(matrix(0, 1), matrix(1, 1), + matrix(2, 1)), matrix * Vector3d.UnitY) + self.assertAlmostEqual(Vector3d(matrix(0, 2), matrix(1, 2), + matrix(2, 2)), matrix * Vector3d.UnitZ) + + # Matrix3.Identity + self.assertAlmostEqual(matrix, matrix * Matrix3d.Identity) + self.assertAlmostEqual(matrix, Matrix3d.Identity * matrix) + + # Multiply arbitrary matrix by itself + matrix = Matrix3d(1, 2, 3, 4, 5, 6, 7, 8, 9) + matrix2 = Matrix3d(30, 36, 42, + 66, 81, 96, + 102, 126, 150) + + self.assertAlmostEqual(matrix * matrix, matrix2) + + def test_not_equal(self): + matrix1 = Matrix3d() + matrix2 = Matrix3d() + + self.assertTrue(matrix1 == matrix2) + self.assertFalse(matrix1 != matrix2) + + matrix1 = Matrix3d(1, 2, 3, 4, 5, 6, 7, 8, 9) + matrix2 = Matrix3d(matrix1) + + self.assertFalse(matrix1 != matrix1) + + matrix2 = Matrix3d(1.00001, 2, 3, 4, 5, 6, 7, 8, 9) + self.assertTrue(matrix1 != matrix2) + + matrix2 = Matrix3d(1.000001, 2, 3, 4, 5, 6, 7, 8, 9) + self.assertFalse(matrix1 != matrix2) + + def test_equal_tolerance(self): + self.assertFalse(Matrix3d.Zero.Equal(Matrix3d.Identity, 1e-6)) + self.assertFalse(Matrix3d.Zero.Equal(Matrix3d.Identity, 1e-3)) + self.assertFalse(Matrix3d.Zero.Equal(Matrix3d.Identity, 1e-1)) + self.assertTrue(Matrix3d.Zero.Equal(Matrix3d.Identity, 1)) + self.assertTrue(Matrix3d.Zero.Equal(Matrix3d.Identity, 1.1)) + + def test_inverse(self): + self.assertAlmostEqual(Matrix3d.Identity, Matrix3d.Identity.Inverse()) + + # Matrix multiplied by its inverse results in the identity matrix + matrix1 = Matrix3d(-2, 4, 0, 0.1, 9, 55, -7, 1, 26) + matrix2 = matrix1.Inverse() + self.assertAlmostEqual(matrix1 * matrix2, Matrix3d.Identity) + self.assertAlmostEqual(matrix2 * matrix1, Matrix3d.Identity) + + # Inverse of inverse results in the same matrix + self.assertAlmostEqual((matrix1.Inverse()).Inverse(), matrix1) + + # Invert multiplication by scalar + scalar = 2.5 + self.assertAlmostEqual((matrix1 * scalar).Inverse(), + matrix1.Inverse() * (1.0/scalar)) + + def test_determinant(self): + # |Zero matrix| = 0.0 + self.assertAlmostEqual(0.0, Matrix3d.Zero.Determinant()) + + # |Identity matrix| = 1.0 + self.assertAlmostEqual(1.0, Matrix3d.Identity.Determinant()) + + # Determinant of arbitrary matrix + m = Matrix3d(-2, 4, 0, 0.1, 9, 55, -7, 1, 26) + self.assertAlmostEqual(-1908.4, m.Determinant()) + + def test_transpose(self): + # Transpose of zero matrix is itself + self.assertAlmostEqual(Matrix3d.Zero, Matrix3d.Zero.Transposed()) + + # Transpose of identity matrix is itself + self.assertAlmostEqual(Matrix3d.Identity, + Matrix3d.Identity.Transposed()) + + # Matrix and expected transpose + m = Matrix3d(-2, 4, 0, + 0.1, 9, 55, + -7, 1, 26) + mT = Matrix3d(-2, 0.1, -7, + 4, 9, 1, + 0, 55, 26) + self.assertNotEqual(m, mT) + self.assertAlmostEqual(m.Transposed(), mT) + self.assertAlmostEqual(m.Determinant(), m.Transposed().Determinant()) + + mT.Transpose() + self.assertAlmostEqual(m, mT) + + def test_from2axes(self): + v1 = Vector3d(1.0, 0.0, 0.0) + v2 = Vector3d(0.0, 1.0, 0.0) + + m1 = Matrix3d() + m1.From2Axes(v1, v2) + + m2 = Matrix3d() + m2.From2Axes(v2, v1) + + m1Correct = Matrix3d(0, -1, 0, + 1, 0, 0, + 0, 0, 1) + m2Correct = Matrix3d(m1Correct) + m2Correct.Transpose() + + self.assertNotEqual(m1, m2) + self.assertAlmostEqual(m1Correct, m1) + self.assertAlmostEqual(m2Correct, m2) + self.assertAlmostEqual(Matrix3d.Identity, m1 * m2) + self.assertAlmostEqual(v2, m1 * v1) + self.assertAlmostEqual(v1, m2 * v2) + + # rotation about 45 degrees + v1.Set(1.0, 0.0, 0.0) + v2.Set(1.0, 1.0, 0.0) + m2.From2Axes(v1, v2) + # m1 is 90 degrees rotation + self.assertAlmostEqual(m1, m2*m2) + + # with non-unit vectors + v1.Set(0.5, 0.5, 0) + v2.Set(-0.5, 0.5, 0) + + m1.From2Axes(v1, v2) + m2.From2Axes(v2, v1) + + self.assertNotEqual(m1, m2) + self.assertAlmostEqual(m1Correct, m1) + self.assertAlmostEqual(m2Correct, m2) + self.assertAlmostEqual(Matrix3d.Identity, m1 * m2) + self.assertAlmostEqual(v2, m1 * v1) + self.assertAlmostEqual(v1, m2 * v2) + + # For zero-length vectors, a unit matrix is returned + v1.Set(0, 0, 0) + v2.Set(-0.5, 0.5, 0) + m1.From2Axes(v1, v2) + self.assertAlmostEqual(Matrix3d.Identity, m1) + + # For zero-length vectors, a unit matrix is returned + v1.Set(-0.5, 0.5, 0) + v2.Set(0, 0, 0) + m1.From2Axes(v1, v2) + self.assertAlmostEqual(Matrix3d.Identity, m1) + + # Parallel vectors + v1.Set(1, 0, 0) + v2.Set(2, 0, 0) + m1.From2Axes(v1, v2) + self.assertAlmostEqual(Matrix3d.Identity, m1) + + # Opposite vectors + v1.Set(1, 0, 0) + v2.Set(-2, 0, 0) + m1.From2Axes(v1, v2) + self.assertAlmostEqual(Matrix3d.Zero - Matrix3d.Identity, m1) + + +if __name__ == '__main__': + unittest.main() From 44228a14606bd64dd6e2e568c78376373018b405 Mon Sep 17 00:00:00 2001 From: LolaSegura Date: Fri, 13 Aug 2021 16:53:27 -0300 Subject: [PATCH 03/15] Adds scripting interface to Pose3 and a python test Signed-off-by: LolaSegura --- src/Pose3.i | 110 ++++++++++++++++++++++++++++ src/Pose3_TEST.py | 183 ++++++++++++++++++++++++++++++++++++++++++++++ 2 files changed, 293 insertions(+) create mode 100644 src/Pose3.i create mode 100644 src/Pose3_TEST.py diff --git a/src/Pose3.i b/src/Pose3.i new file mode 100644 index 000000000..a23e0d1ca --- /dev/null +++ b/src/Pose3.i @@ -0,0 +1,110 @@ +/* + * 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 quaternion +%{ + #include + #include + #include + #include +%} + +%include "std_string.i" +%include "Quaternion.i" + + +namespace ignition +{ + namespace math + { + template + class Pose3 + { + public: static const Pose3 Zero; + + public: Pose3() : p(0, 0, 0), q(1, 0, 0, 0); + public: Pose3(const Vector3 &_pos, const Quaternion &_rot) + : p(_pos), q(_rot); + public: Pose3(T _x, T _y, T _z, T _roll, T _pitch, T _yaw) + : p(_x, _y, _z), q(_roll, _pitch, _yaw); + public: Pose3(T _x, T _y, T _z, T _qw, T _qx, T _qy, T _qz) + : p(_x, _y, _z), q(_qw, _qx, _qy, _qz); + public: Pose3(const Pose3 &_pose) + : p(_pose.p), q(_pose.q); + public: virtual ~Pose3(); + public: void Set(const Vector3 &_pos, const Quaternion &_rot); + public: void Set(const Vector3 &_pos, const Vector3 &_rpy); + public: void Set(T _x, T _y, T _z, T _roll, T _pitch, T _yaw); + public: bool IsFinite() const; + public: inline void Correct(); + public: Pose3 Inverse() const; + public: Pose3 operator+(const Pose3 &_pose) const; + public: const Pose3 &operator+=(const Pose3 &_pose); + public: inline Pose3 operator-() const; + public: inline Pose3 operator-(const Pose3 &_pose) const; + public: const Pose3 &operator-=(const Pose3 &_pose); + public: bool operator==(const Pose3 &_pose) const; + public: bool operator!=(const Pose3 &_pose) const; + public: Pose3 operator*(const Pose3 &_pose) const; + public: const Pose3 &operator*=(const Pose3 &_pose); + public: Vector3 CoordPositionAdd(const Vector3 &_pos) const; + public: Vector3 CoordPositionAdd(const Pose3 &_pose) const; + public: inline Vector3 CoordPositionSub(const Pose3 &_pose) const; + public: Quaternion CoordRotationAdd(const Quaternion &_rot) const; + public: inline Quaternion CoordRotationSub( + const Quaternion &_rot) const; + public: Pose3 CoordPoseSolve(const Pose3 &_b) const; + public: Pose3 RotatePositionAboutOrigin(const Quaternion &_q) const; + public: void Round(int _precision); + public: inline const Vector3 &Pos() const; + public: inline Vector3 &Pos(); + public: inline const T X() const; + public: inline void SetX(T x); + public: inline const T Y() const; + public: inline void SetY(T y); + public: inline const T Z() const; + public: inline void SetZ(T z); + public: inline const Quaternion &Rot() const; + public: inline Quaternion &Rot(); + public: inline const T Roll() const; + public: inline const T Pitch() const; + public: inline const T Yaw() const; + + private: Vector3 p; + private: Quaternion q; + }; + + %extend Pose3 { + void Reset() { + ignition::math::Vector3 vect; + (*$self).Set(vect, ignition::math::Quaternion::Identity); + } + } + + %extend Pose3 { + std::string __str__() const { + std::ostringstream out; + out << *$self; + return out.str(); + } + } + + %template(Pose3i) Pose3; + %template(Pose3d) Pose3; + %template(Pose3f) Pose3; + } +} \ No newline at end of file diff --git a/src/Pose3_TEST.py b/src/Pose3_TEST.py new file mode 100644 index 000000000..cd7a5749c --- /dev/null +++ b/src/Pose3_TEST.py @@ -0,0 +1,183 @@ +# 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 math +from ignition.math import Pose3d +from ignition.math import Quaterniond +from ignition.math import Vector3d + + +class TestPose3(unittest.TestCase): + def test_construction(self): + pose = Pose3d(1, 0, 0, 0, 0, 0) + + # Copy + pose2 = Pose3d(pose) + self.assertAlmostEqual(pose2, pose) + + # Inequality + pose3 = Pose3d() + self.assertNotEqual(pose3, pose) + + def test_pose(self): + A = Pose3d(Vector3d(1, 0, 0), + Quaterniond(0, 0, math.pi/4.0)) + B = Pose3d(Vector3d(1, 0, 0), + Quaterniond(0, 0, math.pi/2.0)) + + # test hypothesis that if + # A is the transform from O to P specified in frame O + # B is the transform from P to Q specified in frame P + # then, B + A is the transform from O to Q specified in frame O + self.assertAlmostEqual((B + A).Pos().X(), 1.0 + 1.0/math.sqrt(2)) + self.assertAlmostEqual((B + A).Pos().Y(), 1.0/math.sqrt(2)) + self.assertAlmostEqual((B + A).Pos().Z(), 0.0) + self.assertAlmostEqual((B + A).Rot().Euler().X(), 0.0) + self.assertAlmostEqual((B + A).Rot().Euler().Y(), 0.0) + self.assertAlmostEqual((B + A).Rot().Euler().Z(), 3.0*math.pi/4.0) + + # If: + # A is the transform from O to P in frame O + # B is the transform from O to Q in frame O + # then -A is transform from P to O specified in frame P + self.assertAlmostEqual((Pose3d() - A).Pos().X(), -1.0/math.sqrt(2)) + self.assertAlmostEqual((Pose3d() - A).Pos().Y(), 1.0/math.sqrt(2)) + self.assertAlmostEqual((Pose3d() - A).Pos().Z(), 0.0) + self.assertAlmostEqual((Pose3d() - A).Rot().Euler().X(), 0.0) + self.assertAlmostEqual((Pose3d() - A).Rot().Euler().Y(), 0.0) + self.assertAlmostEqual((Pose3d() - A).Rot().Euler().Z(), -math.pi/4) + + # test negation operator + self.assertAlmostEqual((-A).Pos().X(), -1.0/math.sqrt(2)) + self.assertAlmostEqual((-A).Pos().Y(), 1.0/math.sqrt(2)) + self.assertAlmostEqual((-A).Pos().Z(), 0.0) + self.assertAlmostEqual((-A).Rot().Euler().X(), 0.0) + self.assertAlmostEqual((-A).Rot().Euler().Y(), 0.0) + self.assertAlmostEqual((-A).Rot().Euler().Z(), -math.pi/4.0) + + # If: + # A is the transform from O to P in frame O + # B is the transform from O to Q in frame O + # B - A is the transform from P to Q in frame P + B = Pose3d(Vector3d(1, 1, 0), + Quaterniond(0, 0, math.pi/2.0)) + self.assertAlmostEqual((B - A).Pos().X(), 1.0/math.sqrt(2)) + self.assertAlmostEqual((B - A).Pos().Y(), 1.0/math.sqrt(2)) + self.assertAlmostEqual((B - A).Pos().Z(), 0.0) + self.assertAlmostEqual((B - A).Rot().Euler().X(), 0.0) + self.assertAlmostEqual((B - A).Rot().Euler().Y(), 0.0) + self.assertAlmostEqual((B - A).Rot().Euler().Z(), math.pi/4.0) + + pose = Pose3d() + self.assertTrue(pose.Pos() == Vector3d(0, 0, 0)) + self.assertTrue(pose.Rot() == Quaterniond(0, 0, 0)) + + pose = Pose3d(Vector3d(1, 2, 3), Quaterniond(.1, .2, .3)) + self.assertTrue(pose.Pos() == Vector3d(1, 2, 3)) + self.assertTrue(pose.Rot() == Quaterniond(.1, .2, .3)) + + pose1 = Pose3d(pose) + self.assertTrue(pose1 == pose) + + pose.Set(Vector3d(2, 3, 4), Quaterniond(.3, .4, .5)) + self.assertTrue(pose.Pos() == Vector3d(2, 3, 4)) + self.assertTrue(pose.Rot() == Quaterniond(.3, .4, .5)) + self.assertTrue(pose.IsFinite()) + + pose1 = pose.Inverse() + self.assertTrue(pose1.Pos() == Vector3d(-1.38368, -3.05541, -4.21306)) + self.assertTrue(pose1.Rot() == Quaterniond(0.946281, -0.0933066, + -0.226566, -0.210984)) + + pose = Pose3d(1, 2, 3, .1, .2, .3) + Pose3d(4, 5, 6, .4, .5, .6) + self.assertTrue(pose == Pose3d(5.74534, 7.01053, 8.62899, + 0.675732, 0.535753, 1.01174)) + + pose += pose + self.assertTrue(pose == Pose3d(11.314, 16.0487, 15.2559, + 1.49463, 0.184295, 2.13932)) + + pose -= Pose3d(pose) + self.assertTrue(pose == Pose3d(0, 0, 0, 0, 0, 0)) + + pose.Pos().Set(5, 6, 7) + pose.Rot().Euler(Vector3d(.4, .6, 0)) + + self.assertTrue(pose.CoordPositionAdd(Vector3d(1, 2, 3)) == + Vector3d(7.82531, 6.67387, 9.35871)) + + self.assertTrue(pose.CoordPositionAdd(pose1) == + Vector3d(2.58141, 2.4262, 3.8013)) + self.assertTrue(pose.CoordRotationAdd(Quaterniond(0.1, 0, 0.2)) == + Quaterniond(0.520975, 0.596586, 0.268194)) + self.assertTrue(pose.CoordPoseSolve(pose1) == + Pose3d(-0.130957, -11.552, -10.2329, + -0.462955, -1.15624, -0.00158047)) + + self.assertTrue(pose.RotatePositionAboutOrigin( + Quaterniond(0.1, 0, 0.2)) == + Pose3d(6.09235, 5.56147, 6.47714, 0.4, 0.6, 0)) + + pose.Reset() + self.assertTrue(pose.Pos() == Vector3d(0, 0, 0)) + self.assertTrue(pose.Rot() == Quaterniond(0, 0, 0)) + + def test_pose_atributes(self): + pose = Pose3d(0, 1, 2, 1, 0, 0) + + self.assertTrue(pose.Pos() == Vector3d(0, 1, 2)) + self.assertTrue(pose.Rot() == Quaterniond(1, 0, 0)) + + def test_stream_out(self): + p = Pose3d(0.1, 1.2, 2.3, 0.0, 0.1, 1.0) + self.assertAlmostEqual(str(p), "0.1 1.2 2.3 0 0.1 1") + + def test_mutable_pose(self): + pose = Pose3d(0, 1, 2, 0, 0, 0) + + self.assertTrue(pose.Pos() == Vector3d(0, 1, 2)) + self.assertTrue(pose.Rot() == Quaterniond(0, 0, 0)) + + pose = Pose3d(Vector3d(10, 20, 30), Quaterniond(1, 2, 1)) + + self.assertTrue(pose.Pos() == Vector3d(10, 20, 30)) + self.assertTrue(pose.Rot() == Quaterniond(1, 2, 1)) + + def test_pose_elements(self): + pose = Pose3d(0, 1, 2, 1, 1, 2) + self.assertAlmostEqual(pose.X(), 0) + self.assertAlmostEqual(pose.Y(), 1) + self.assertAlmostEqual(pose.Z(), 2) + self.assertAlmostEqual(pose.Roll(), 1) + self.assertAlmostEqual(pose.Pitch(), 1) + self.assertAlmostEqual(pose.Yaw(), 2) + + def test_set_pose_element(self): + pose = Pose3d(1, 2, 3, 1.57, 1, 2) + self.assertAlmostEqual(pose.X(), 1) + self.assertAlmostEqual(pose.Y(), 2) + self.assertAlmostEqual(pose.Z(), 3) + + pose.SetX(10) + pose.SetY(12) + pose.SetZ(13) + + self.assertAlmostEqual(pose.X(), 10) + self.assertAlmostEqual(pose.Y(), 12) + self.assertAlmostEqual(pose.Z(), 13) + + +if __name__ == '__main__': + unittest.main() \ No newline at end of file From b8aca5848888e3fd451d521fa470ec160947779b Mon Sep 17 00:00:00 2001 From: LolaSegura Date: Fri, 13 Aug 2021 16:55:50 -0300 Subject: [PATCH 04/15] Solves bugg in the Reset() method inside Pose3 Signed-off-by: LolaSegura --- include/ignition/math/Pose3.hh | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/include/ignition/math/Pose3.hh b/include/ignition/math/Pose3.hh index e6e75f33a..b971cee8b 100644 --- a/include/ignition/math/Pose3.hh +++ b/include/ignition/math/Pose3.hh @@ -327,7 +327,7 @@ namespace ignition { // set the position to zero this->p.Set(); - this->q = Quaterniond::Identity; + this->q = Quaternion::Identity; } /// \brief Rotate vector part of a pose about the origin From 6da328211fbf50c1e2361511b01461a651dfff4f Mon Sep 17 00:00:00 2001 From: LolaSegura Date: Fri, 13 Aug 2021 16:56:37 -0300 Subject: [PATCH 05/15] Adds scripting interface to Matrix4 and a python test Signed-off-by: LolaSegura --- src/Matrix4.i | 101 +++++++++ src/Matrix4_TEST.py | 520 ++++++++++++++++++++++++++++++++++++++++++++ 2 files changed, 621 insertions(+) create mode 100644 src/Matrix4.i create mode 100644 src/Matrix4_TEST.py diff --git a/src/Matrix4.i b/src/Matrix4.i new file mode 100644 index 000000000..b3e145ed9 --- /dev/null +++ b/src/Matrix4.i @@ -0,0 +1,101 @@ +/* + * 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 matrix3 +%{ +#include +#include +#include +#include +#include +#include +%} + +%include "std_string.i" + +namespace ignition +{ + namespace math + { + template + class Matrix4 + { + public: static const Matrix4 Identity; + public: static const Matrix4 Zero; + + public: Matrix4(); + public: Matrix4(const Matrix4 &_m); + public: Matrix4(T _v00, T _v01, T _v02, T _v03, + T _v10, T _v11, T _v12, T _v13, + T _v20, T _v21, T _v22, T _v23, + T _v30, T _v31, T _v32, T _v33); + public: explicit Matrix4(const Quaternion &_q); + public: explicit Matrix4(const Pose3 &_pose) : Matrix4(_pose.Rot()); + public: virtual ~Matrix4() {}; + public: void Set( + T _v00, T _v01, T _v02, T _v03, + T _v10, T _v11, T _v12, T _v13, + T _v20, T _v21, T _v22, T _v23, + T _v30, T _v31, T _v32, T _v33); + public: void Axis(const Vector3 &_axis, T _angle); + public: void SetTranslation(const Vector3 &_t); + public: void SetTranslation(T _x, T _y, T _z); + public: Vector3 Translation() const; + public: Vector3 Scale() const; + public: Quaternion Rotation() const; + public: Vector3 EulerRotation(bool _firstSolution) const; + public: Pose3 Pose() const; + public: void Scale(const Vector3 &_s); + public: void Scale(T _x, T _y, T _z); + public: bool IsAffine() const; + public: bool TransformAffine(const Vector3 &_v, + Vector3 &_result) const; + public: T Determinant() const; + public: Matrix4 Inverse() const; + public: void Transpose(); + public: Matrix4 Transposed() const; + public: Matrix4 operator*=(const Matrix4 &_m2); + public: Matrix4 operator*(const Matrix4 &_m2) const; + public: Vector3 operator*(const Vector3 &_vec) const; + public: bool Equal(const Matrix4 &_m, const T &_tol) const; + public: bool operator==(const Matrix4 &_m) const; + public: bool operator!=(const Matrix4 &_m) const; + public: static Matrix4 LookAt(const Vector3 &_eye, + const Vector3 &_target, const Vector3 &_up = Vector3::UnitZ); + + private: T data[4][4]; + }; + + %extend Matrix4{ + T __call__(size_t row, size_t col) const { + return (*$self)(row, col); + } + } + + %extend Matrix4 { + std::string __str__() const { + std::ostringstream out; + out << *$self; + return out.str(); + } + } + + %template(Matrix4i) Matrix4; + %template(Matrix4d) Matrix4; + %template(Matrix4f) Matrix4; + } +} diff --git a/src/Matrix4_TEST.py b/src/Matrix4_TEST.py new file mode 100644 index 000000000..f4c6a3354 --- /dev/null +++ b/src/Matrix4_TEST.py @@ -0,0 +1,520 @@ +# 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 math +from ignition.math import Matrix4d +from ignition.math import Pose3d +from ignition.math import Quaterniond +from ignition.math import Vector3d + + +class TestMatrix4(unittest.TestCase): + def test_construct(self): + mat = Matrix4d() + + for i in range(5): + for j in range(5): + self.assertAlmostEqual(mat(i, j), 0.0) + + mat2 = Matrix4d(mat) + for i in range(5): + for j in range(5): + self.assertAlmostEqual(mat2(i, j), 0.0) + + self.assertTrue(mat2 == mat) + + mat3 = Matrix4d(0.0, 1.0, 2.0, 3.0, + 4.0, 5.0, 6.0, 7.0, + 8.0, 9.0, 10.0, 11.0, + 12.0, 13.0, 14.0, 15.0) + + mat4 = Matrix4d(mat3) + self.assertAlmostEqual(mat4, mat3) + + self.assertAlmostEqual(mat3(0, 0), 0.0) + self.assertAlmostEqual(mat3(0, 1), 1.0) + self.assertAlmostEqual(mat3(0, 2), 2.0) + self.assertAlmostEqual(mat3(0, 3), 3.0) + self.assertAlmostEqual(mat3(1, 0), 4.0) + self.assertAlmostEqual(mat3(1, 1), 5.0) + self.assertAlmostEqual(mat3(1, 2), 6.0) + self.assertAlmostEqual(mat3(1, 3), 7.0) + self.assertAlmostEqual(mat3(2, 0), 8.0) + self.assertAlmostEqual(mat3(2, 1), 9.0) + self.assertAlmostEqual(mat3(2, 2), 10.0) + self.assertAlmostEqual(mat3(2, 3), 11.0) + self.assertAlmostEqual(mat3(3, 0), 12.0) + self.assertAlmostEqual(mat3(3, 1), 13.0) + self.assertAlmostEqual(mat3(3, 2), 14.0) + self.assertAlmostEqual(mat3(3, 3), 15.0) + + def test_construct_from_pose3(self): + trans = Vector3d(1, 2, 3) + qt = Quaterniond(0.1, 0.2, 0.3) + pose = Pose3d(trans, qt) + mat = Matrix4d(pose) + + self.assertAlmostEqual(pose, mat.Pose()) + self.assertAlmostEqual(trans, mat.Translation()) + self.assertAlmostEqual(qt, mat.Rotation()) + self.assertAlmostEqual(pose.Inverse(), mat.Inverse().Pose()) + # ensure inverses multiply to identity + self.assertAlmostEqual(mat.Inverse() * mat, Matrix4d.Identity) + self.assertAlmostEqual(mat * mat.Inverse(), Matrix4d.Identity) + self.assertAlmostEqual(pose.Inverse() * pose, Pose3d.Zero) + self.assertAlmostEqual(pose * pose.Inverse(), Pose3d.Zero) + + # repeat test with *= + m = Matrix4d(Matrix4d.Identity) + m *= mat + self.assertAlmostEqual(m, mat) + m *= mat.Inverse() + self.assertAlmostEqual(m, Matrix4d.Identity) + + p = Pose3d() + p *= pose + self.assertAlmostEqual(p, pose) + p *= pose.Inverse() + self.assertAlmostEqual(p, Pose3d.Zero) + + # Zero values + trans = Vector3d(0, 0, 0) + qt = Quaterniond(0, 0, 0) + pose = Pose3d(trans, qt) + mat = Matrix4d(pose) + + self.assertAlmostEqual(pose, mat.Pose()) + self.assertAlmostEqual(trans, mat.Translation()) + self.assertAlmostEqual(qt, mat.Rotation()) + self.assertAlmostEqual(pose.Inverse(), mat.Inverse().Pose()) + + # Rotate pitch by pi/2 so yaw coincides with roll causing a gimbal lock + trans = Vector3d(3, 2, 1) + qt = Quaterniond(0, math.pi/2, 0) + pose = Pose3d(trans, qt) + mat = Matrix4d(pose) + + self.assertAlmostEqual(pose, mat.Pose()) + self.assertAlmostEqual(trans, mat.Translation()) + self.assertAlmostEqual(qt, mat.Rotation()) + self.assertAlmostEqual(pose.Inverse(), mat.Inverse().Pose()) + + # setup a ZXZ rotation to ensure non-commutative rotations + pose1 = Pose3d(1, -2, 3, 0, 0, math.pi/4) + pose2 = Pose3d(0, 1, -1, -math.pi/4, 0, 0) + pose3 = Pose3d(-1, 0, 0, 0, 0, -math.pi/4) + + m1 = Matrix4d(pose1) + m2 = Matrix4d(pose2) + m3 = Matrix4d(pose3) + + # ensure rotations are not commutative + self.assertNotEqual(m1 * m2 * m3, m3 * m2 * m1) + + # ensure pose multiplication order matches matrix order + self.assertAlmostEqual(m1 * m2 * m3, Matrix4d(pose1 * pose2 * pose3)) + self.assertAlmostEqual(m3 * m2 * m1, Matrix4d(pose3 * pose2 * pose1)) + + # repeat test with *= + m = Matrix4d(Matrix4d.Identity) + p = Pose3d() + + m *= m1 + p *= pose1 + self.assertAlmostEqual(m, m1) + self.assertAlmostEqual(p, pose1) + self.assertAlmostEqual(m, Matrix4d(p)) + + m *= m2 + p *= pose2 + self.assertAlmostEqual(m, m1 * m2) + self.assertAlmostEqual(p, pose1 * pose2) + self.assertAlmostEqual(m, Matrix4d(p)) + + m *= m3 + p *= pose3 + self.assertAlmostEqual(m, m1 * m2 * m3) + self.assertAlmostEqual(p, pose1 * pose2 * pose3) + self.assertAlmostEqual(m, Matrix4d(p)) + + def test_scale(self): + mat = Matrix4d() + mat2 = Matrix4d() + + mat.Scale(Vector3d(1, 2, 3)) + mat2.Scale(1, 2, 3) + + self.assertAlmostEqual(mat, mat2) + + self.assertAlmostEqual(mat(0, 0), 1.0) + self.assertAlmostEqual(mat(1, 1), 2.0) + self.assertAlmostEqual(mat(2, 2), 3.0) + self.assertAlmostEqual(mat(3, 3), 1.0) + + self.assertAlmostEqual(mat2(0, 0), 1.0) + self.assertAlmostEqual(mat2(1, 1), 2.0) + self.assertAlmostEqual(mat2(2, 2), 3.0) + self.assertAlmostEqual(mat2(3, 3), 1.0) + + self.assertAlmostEqual(mat.Scale(), mat2.Scale()) + self.assertAlmostEqual(mat.Scale(), Vector3d(1, 2, 3)) + + for i in range(0, 4): + for j in range(0, 4): + if i != j: + self.assertAlmostEqual(mat(i, j), 0.0) + self.assertAlmostEqual(mat2(i, j), 0.0) + elif i == 3 and j == 3: + self.assertAlmostEqual(mat(i, j), 1.0) + self.assertAlmostEqual(mat2(i, j), 1.0) + + def test_multiply_vect(self): + mat = Matrix4d() + vec = Vector3d(-1.2, 2.3, 10.5) + + self.assertAlmostEqual(mat * vec, Vector3d(0.0, 0.0, 0.0)) + + mat = Matrix4d(Matrix4d.Identity) + self.assertAlmostEqual(mat * vec, vec) + + def test_multiply_mat(self): + mat = Matrix4d(0, -1, -2, -3, + 1, 0, -1, -2, + 2, 1, 0, -1, + 3, 2, 1, 0) + mat1 = Matrix4d(0, 1, 2, 3, + 1, 2, 3, 4, + 2, 3, 4, 5, + 3, 4, 5, 6) + + mat3 = Matrix4d(-14, -20, -26, -32, + -8, -10, -12, -14, + -2, 0, 2, 4, + 4, 10, 16, 22) + + mat2 = mat * mat1 + + self.assertAlmostEqual(mat2, mat3) + + mat4 = mat + mat4 *= mat1 + self.assertAlmostEqual(mat2, mat4) + + def test_inverse(self): + mat = Matrix4d(2, 3, 1, 5, + 1, 0, 3, 1, + 0, 2, -3, 2, + 0, 2, 3, 1) + + mat1 = mat.Inverse() + self.assertAlmostEqual(mat1, Matrix4d(18, -35, -28, 1, + 9, -18, -14, 1, + -2, 4, 3, 0, + -12, 24, 19, -1)) + + def test_get_pose3(self): + mat = Matrix4d(2, 3, 1, 5, + 1, 0, 3, 1, + 0, 2, -3, 2, + 0, 2, 3, 1) + pose = mat.Pose() + + self.assertAlmostEqual(pose, Pose3d(5, 1, 2, + -0.204124, 1.22474, 0.816497, 0.204124)) + + def test_translation(self): + mat = Matrix4d() + mat2 = Matrix4d() + + mat.SetTranslation(Vector3d(1, 2, 3)) + mat2.SetTranslation(1, 2, 3) + + self.assertEqual(mat, mat2) + + self.assertAlmostEqual(mat(0, 3), 1.0) + self.assertAlmostEqual(mat(1, 3), 2.0) + self.assertAlmostEqual(mat(2, 3), 3.0) + + self.assertAlmostEqual(mat2(0, 3), 1.0) + self.assertAlmostEqual(mat2(1, 3), 2.0) + self.assertAlmostEqual(mat2(2, 3), 3.0) + + self.assertEqual(mat.Translation(), mat2.Translation()) + self.assertEqual(mat.Translation(), Vector3d(1, 2, 3)) + + for i in range(0, 4): + for j in range(0, 2): + self.assertAlmostEqual(mat(i, j), 0.0) + self.assertAlmostEqual(mat2(i, j), 0.0) + + self.assertAlmostEqual(mat(3, 3), 0.0) + self.assertAlmostEqual(mat2(3, 3), 0.0) + + def test_rotation_diag_zero(self): + mat = Matrix4d(0, 0.2, 0.3, 0.4, + 0.5, 0, 0.7, 0.8, + 0.9, 1.0, 0, 1.2, + 1.3, 1.4, 1.5, 1.0) + + quat = mat.Rotation() + self.assertAlmostEqual(quat.X(), 0.5, delta=1e-6) + self.assertAlmostEqual(quat.Y(), 0.35, delta=1e-6) + self.assertAlmostEqual(quat.Z(), 0.6, delta=1e-6) + self.assertAlmostEqual(quat.W(), 0.15, delta=1e-6) + + euler = mat.EulerRotation(True) + self.assertAlmostEqual(euler, Vector3d(1.5708, -1.11977, 1.5708)) + + euler = mat.EulerRotation(False) + self.assertAlmostEqual(euler, Vector3d(-1.5708, 4.26136, -1.5708)) + + def test_rotation_diag_less_zero(self): + mat = Matrix4d(-0.1, 0.2, 0.3, 0.4, + 0.5, 0, 0.7, 0.8, + 0.9, 1.0, 0, 1.2, + 1.3, 1.4, 1.5, 1.0) + + quat = mat.Rotation() + self.assertAlmostEqual(quat.X(), 0.333712, delta=1e-6) + self.assertAlmostEqual(quat.Y(), 0.524404, delta=1e-6) + self.assertAlmostEqual(quat.Z(), 0.810443, delta=1e-6) + self.assertAlmostEqual(quat.W(), -0.286039, delta=1e-6) + + euler = mat.EulerRotation(True) + self.assertAlmostEqual(euler, Vector3d(1.5708, -1.11977, 1.76819)) + + euler = mat.EulerRotation(False) + self.assertAlmostEqual(euler, Vector3d(-1.5708, 4.26136, -1.3734)) + + mat = Matrix4d(-0.1, 0.2, 0.3, 0.4, + 0.5, -0.2, 0.7, 0.8, + 0.9, 1.0, 0.0, 1.2, + 1.3, 1.4, 1.5, 1.0) + + quat = mat.Rotation() + self.assertAlmostEqual(quat.X(), 0.526235, delta=1e-6) + self.assertAlmostEqual(quat.Y(), 0.745499, delta=1e-6) + self.assertAlmostEqual(quat.Z(), 0.570088, delta=1e-6) + self.assertAlmostEqual(quat.W(), 0.131559, delta=1e-6) + + euler = mat.EulerRotation(True) + self.assertAlmostEqual(euler, Vector3d(1.5708, -1.11977, 1.76819)) + + euler = mat.EulerRotation(False) + self.assertAlmostEqual(euler, Vector3d(-1.5708, 4.26136, -1.3734)) + + def test_rotation(self): + mat = Matrix4d(0.1, 0.2, 0.3, 0.4, + 0.5, 0.6, 0.7, 0.8, + 0.9, 1.0, 1.1, 1.2, + 1.3, 1.4, 1.5, 1.6) + + quat = mat.Rotation() + self.assertAlmostEqual(quat.X(), 0.0896421, delta=1e-6) + self.assertAlmostEqual(quat.Y(), -0.179284, delta=1e-6) + self.assertAlmostEqual(quat.Z(), 0.0896421, delta=1e-6) + self.assertAlmostEqual(quat.W(), 0.83666, delta=1e-6) + + euler = mat.EulerRotation(True) + self.assertAlmostEqual(euler, Vector3d(0.737815, -1.11977, 1.3734)) + + euler = mat.EulerRotation(False) + self.assertAlmostEqual(euler, Vector3d(-2.40378, 4.26136, -1.76819)) + + def test_euler_rotation2(self): + mat = Matrix4d(0.1, 0.2, 0.3, 0.4, + 0.5, 0.6, 0.7, 0.8, + 1.9, 1.2, 1.1, 1.2, + 1.3, 1.4, 1.5, 1.6) + + euler = mat.EulerRotation(True) + self.assertAlmostEqual(euler, Vector3d(-2.55359, -1.5708, 0)) + + euler = mat.EulerRotation(False) + self.assertAlmostEqual(euler, Vector3d(-2.55359, -1.5708, 0)) + + mat = Matrix4d(0.1, 0.2, 0.3, 0.4, + 0.5, 0.6, 0.7, 0.8, + -1.2, 1.2, 1.1, 1.2, + 1.3, 1.4, 1.5, 1.6) + + euler = mat.EulerRotation(True) + self.assertAlmostEqual(euler, Vector3d(0.588003, 1.5708, 0)) + + euler = mat.EulerRotation(False) + self.assertAlmostEqual(euler, Vector3d(0.588003, 1.5708, 0)) + + def test_affine_transform(self): + mat = Matrix4d(Matrix4d.Zero) + vec = Vector3d(1, 2, 3) + + v = Vector3d() + + self.assertFalse(mat.TransformAffine(vec, v)) + + mat = Matrix4d(Matrix4d.Identity) + self.assertTrue(mat.TransformAffine(vec, v)) + + def test_stream_out(self): + matA = Matrix4d(1, 2, 3, 4, + 5, 6, 7, 8, + 9, 10, 11, 12, + 13, 14, 15, 16) + + self.assertEqual(str(matA), "1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16") + + def test_not_equal(self): + matrix1 = Matrix4d() + matrix2 = Matrix4d() + self.assertTrue(matrix1 == matrix2) + self.assertFalse(matrix1 != matrix2) + + matrix1 = Matrix4d(1, 2, 3, 4, + 5, 6, 7, 8, + 9, 10, 11, 12, + 13, 14, 15, 16) + matrix2 = Matrix4d(matrix1) + + self.assertFalse(matrix1 != matrix2) + + matrix2 = Matrix4d(1.00001, 2, 3, 4, + 5, 6, 7, 8, + 9, 10, 11, 12, + 13, 14, 15, 16) + self.assertTrue(matrix1 != matrix2) + + matrix2 = Matrix4d(1.0000001, 2, 3, 4, + 5, 6, 7, 8, + 9, 10, 11, 12, + 13, 14, 15, 16) + self.assertFalse(matrix1 != matrix2) + + def test_equal_tolerance(self): + self.assertFalse(Matrix4d.Zero.Equal(Matrix4d.Identity, 1e-6)) + self.assertFalse(Matrix4d.Zero.Equal(Matrix4d.Identity, 1e-3)) + self.assertFalse(Matrix4d.Zero.Equal(Matrix4d.Identity, 1e-1)) + self.assertTrue(Matrix4d.Zero.Equal(Matrix4d.Identity, 1)) + self.assertTrue(Matrix4d.Zero.Equal(Matrix4d.Identity, 1.1)) + + def test_determinant(self): + # |Zero matrix| = 0.0 + self.assertAlmostEqual(0.0, Matrix4d.Zero.Determinant()) + + # |Identity matrix| = 1.0 + self.assertAlmostEqual(1.0, Matrix4d.Identity.Determinant()) + + # Determinant of arbitrary matrix + m = Matrix4d(2, 3, 0.1, -5, + 1, 0, 3.2, 1, + 0, 2, -3, 2.1, + 0, 2, 3.2, 1) + self.assertAlmostEqual(129.82, m.Determinant()) + + def test_transpose(self): + # Transpose of zero matrix is itself + self.assertAlmostEqual(Matrix4d.Zero, Matrix4d.Zero.Transposed()) + + # Transpose of identity matrix is itself + self.assertAlmostEqual(Matrix4d.Identity, + Matrix4d.Identity.Transposed()) + + # Matrix and expected transpose + m = Matrix4d(-2, 4, 0, -3.5, + 0.1, 9, 55, 1.2, + -7, 1, 26, 11.5, + .2, 3, -5, -0.1) + mT = Matrix4d(-2, 0.1, -7, .2, + 4, 9, 1, 3, + 0, 55, 26, -5, + -3.5, 1.2, 11.5, -0.1) + self.assertNotEqual(m, mT) + self.assertEqual(m.Transposed(), mT) + self.assertAlmostEqual(m.Determinant(), m.Transposed().Determinant()) + + mT.Transpose() + self.assertEqual(m, mT) + + def test_look_at(self): + self.assertAlmostEqual(Matrix4d.LookAt(-Vector3d.UnitX, + Vector3d.Zero).Pose(), + Pose3d(-1, 0, 0, 0, 0, 0)) + + self.assertAlmostEqual(Matrix4d.LookAt(Vector3d(3, 2, 0), + Vector3d(0, 2, 0)).Pose(), + Pose3d(3, 2, 0, 0, 0, math.pi)) + + self.assertAlmostEqual(Matrix4d.LookAt(Vector3d(1, 6, 1), + Vector3d.One).Pose(), + Pose3d(1, 6, 1, 0, 0, -math.pi/2)) + + self.assertAlmostEqual(Matrix4d.LookAt(Vector3d(-1, -1, 0), + Vector3d(1, 1, 0)).Pose(), + Pose3d(-1, -1, 0, 0, 0, math.pi/4)) + + # Default up is Z + self.assertAlmostEqual(Matrix4d.LookAt(Vector3d(0.1, -5, 222), + Vector3d(999, -0.6, 0)), + Matrix4d.LookAt(Vector3d(0.1, -5, 222), + Vector3d(999, -0.6, 0), + Vector3d.UnitZ)) + + # up == zero, default up = +Z + self.assertAlmostEqual(Matrix4d.LookAt(Vector3d(1.23, 456, 0.7), + Vector3d(0, 8.9, -10), + Vector3d.Zero), + Matrix4d.LookAt(Vector3d(1.23, 456, 0.7), + Vector3d(0, 8.9, -10))) + + # up == +X, default up = +Z + self.assertAlmostEqual(Matrix4d.LookAt(Vector3d(0.25, 9, -5), + Vector3d(-6, 0, 0.4), + Vector3d.UnitX), + Matrix4d.LookAt(Vector3d(0.25, 9, -5), + Vector3d(-6, 0, 0.4))) + + # up == -X, default up = +Z + self.assertAlmostEqual(Matrix4d.LookAt(Vector3d(0, 0, 0.2), + Vector3d(-8, 0, -6), + -Vector3d.UnitX), + Matrix4d.LookAt(Vector3d(0, 0, 0.2), + Vector3d(-8, 0, -6))) + + # eye == target, default direction = +X + self.assertAlmostEqual(Matrix4d.LookAt(Vector3d.One, + Vector3d.One), + Matrix4d.LookAt(Vector3d.One, + Vector3d(1.0001, 1, 1))) + + # Not possible to keep _up on +Z + self.assertAlmostEqual(Matrix4d.LookAt(Vector3d(-1, 0, 10), + Vector3d(-1, 0, 0)), + Matrix4d.LookAt(Vector3d(-1, 0, 10), + Vector3d(-1, 0, 0), + -Vector3d.UnitX)) + + # Different ups + self.assertAlmostEqual(Matrix4d.LookAt(Vector3d.One, + Vector3d(0, 1, 1), + Vector3d.UnitY).Pose(), + Pose3d(1, 1, 1, math.pi/2, 0, math.pi)) + + self.assertAlmostEqual(Matrix4d.LookAt(Vector3d.One, + Vector3d(0, 1, 1), + Vector3d(0, 1, 1)).Pose(), + Pose3d(1, 1, 1, math.pi/4, 0, math.pi)) + + +if __name__ == '__main__': + unittest.main() From 9d8f5f9378bf176576cacb117779ad1fce2e37eb Mon Sep 17 00:00:00 2001 From: LolaSegura Date: Fri, 13 Aug 2021 16:58:48 -0300 Subject: [PATCH 06/15] Solves bugg in the Construct test for Matrix4 class Signed-off-by: LolaSegura --- src/Matrix4_TEST.cc | 78 +++++++++++++++++++++++---------------------- 1 file changed, 40 insertions(+), 38 deletions(-) diff --git a/src/Matrix4_TEST.cc b/src/Matrix4_TEST.cc index a1f7e62f9..d1459f87a 100644 --- a/src/Matrix4_TEST.cc +++ b/src/Matrix4_TEST.cc @@ -32,7 +32,7 @@ TEST(Matrix4dTest, Construct) { for (int j = 0; j < 4; ++j) { - EXPECT_DOUBLE_EQ(mat(i, i), 0.0); + EXPECT_DOUBLE_EQ(mat(i, j), 0.0); } } @@ -41,17 +41,16 @@ TEST(Matrix4dTest, Construct) { for (int j = 0; j < 4; ++j) { - EXPECT_DOUBLE_EQ(mat2(i, i), 0.0); + EXPECT_DOUBLE_EQ(mat2(i, j), 0.0); } } EXPECT_TRUE(mat2 == mat); - // Set individual values. math::Matrix4d mat3(0.0, 1.0, 2.0, 3.0, - 4.0, 5.0, 6.0, 7.0, - 8.0, 9.0, 10.0, 11.0, - 12.0, 13.0, 14.0, 15.0); + 4.0, 5.0, 6.0, 7.0, + 8.0, 9.0, 10.0, 11.0, + 12.0, 13.0, 14.0, 15.0); math::Matrix4d mat4; mat4 = mat3; @@ -126,7 +125,7 @@ TEST(Matrix4dTest, ConstructFromPose3d) // Rotate pitch by pi/2 so yaw coincides with roll causing a gimbal lock { math::Vector3d trans(3, 2, 1); - math::Quaterniond qt(0, IGN_PI/2, 0); + math::Quaterniond qt(0, IGN_PI / 2, 0); math::Pose3d pose(trans, qt); math::Matrix4d mat(pose); @@ -138,9 +137,9 @@ TEST(Matrix4dTest, ConstructFromPose3d) { // setup a ZXZ rotation to ensure non-commutative rotations - math::Pose3d pose1(1, -2, 3, 0, 0, IGN_PI/4); - math::Pose3d pose2(0, 1, -1, -IGN_PI/4, 0, 0); - math::Pose3d pose3(-1, 0, 0, 0, 0, -IGN_PI/4); + math::Pose3d pose1(1, -2, 3, 0, 0, IGN_PI / 4); + math::Pose3d pose2(0, 1, -1, -IGN_PI / 4, 0, 0); + math::Pose3d pose3(-1, 0, 0, 0, 0, -IGN_PI / 4); math::Matrix4d m1(pose1); math::Matrix4d m2(pose2); @@ -238,7 +237,7 @@ TEST(Matrix4dTest, MultiplyV) { for (int j = 0; j < 4; ++j) { - mat(i, j) = i-j; + mat(i, j) = i - j; } } @@ -254,8 +253,8 @@ TEST(Matrix4dTest, Multiply4) { for (int j = 0; j < 4; ++j) { - mat(i, j) = i-j; - mat1(j, i) = i+j; + mat(i, j) = i - j; + mat1(j, i) = i + j; } } @@ -277,28 +276,28 @@ TEST(Matrix4dTest, Multiply4) TEST(Matrix4dTest, Inverse) { math::Matrix4d mat(2, 3, 1, 5, - 1, 0, 3, 1, - 0, 2, -3, 2, - 0, 2, 3, 1); + 1, 0, 3, 1, + 0, 2, -3, 2, + 0, 2, 3, 1); math::Matrix4d mat1 = mat.Inverse(); EXPECT_EQ(mat1, math::Matrix4d(18, -35, -28, 1, - 9, -18, -14, 1, - -2, 4, 3, 0, - -12, 24, 19, -1)); + 9, -18, -14, 1, + -2, 4, 3, 0, + -12, 24, 19, -1)); } ///////////////////////////////////////////////// TEST(Matrix4dTest, GetAsPose3d) { math::Matrix4d mat(2, 3, 1, 5, - 1, 0, 3, 1, - 0, 2, -3, 2, - 0, 2, 3, 1); + 1, 0, 3, 1, + 0, 2, -3, 2, + 0, 2, 3, 1); math::Pose3d pose = mat.Pose(); EXPECT_EQ(pose, - math::Pose3d(5, 1, 2, -0.204124, 1.22474, 0.816497, 0.204124)); + math::Pose3d(5, 1, 2, -0.204124, 1.22474, 0.816497, 0.204124)); } ///////////////////////////////////////////////// @@ -408,7 +407,6 @@ TEST(Matrix4dTest, RotationDiagLessThanZero) EXPECT_EQ(euler, math::Vector3d(-1.5708, 4.26136, -1.3734)); } - { mat(0, 0) = -0.1; mat(1, 1) = -0.2; @@ -428,7 +426,6 @@ TEST(Matrix4dTest, RotationDiagLessThanZero) } } - ///////////////////////////////////////////////// TEST(Matrix4dTest, Rotation) { @@ -632,14 +629,14 @@ TEST(Matrix4dTest, Transpose) EXPECT_EQ(math::Matrix4d::Identity, math::Matrix4d::Identity.Transposed()); // Matrix and expected transpose - math::Matrix4d m(-2, 4, 0, -3.5, - 0.1, 9, 55, 1.2, + math::Matrix4d m(-2, 4, 0, -3.5, + 0.1, 9, 55, 1.2, -7, 1, 26, 11.5, .2, 3, -5, -0.1); - math::Matrix4d mT(-2, 0.1, -7, .2, - 4, 9, 1, 3, - 0, 55, 26, -5, - -3.5, 1.2, 11.5, -0.1); + math::Matrix4d mT(-2, 0.1, -7, .2, + 4, 9, 1, 3, + 0, 55, 26, -5, + -3.5, 1.2, 11.5, -0.1); EXPECT_NE(m, mT); EXPECT_EQ(m.Transposed(), mT); EXPECT_DOUBLE_EQ(m.Determinant(), m.Transposed().Determinant()); @@ -652,19 +649,23 @@ TEST(Matrix4dTest, Transpose) TEST(Matrix4dTest, LookAt) { EXPECT_EQ(math::Matrix4d::LookAt(-math::Vector3d::UnitX, - math::Vector3d::Zero).Pose(), + math::Vector3d::Zero) + .Pose(), math::Pose3d(-1, 0, 0, 0, 0, 0)); EXPECT_EQ(math::Matrix4d::LookAt(math::Vector3d(3, 2, 0), - math::Vector3d(0, 2, 0)).Pose(), + math::Vector3d(0, 2, 0)) + .Pose(), math::Pose3d(3, 2, 0, 0, 0, IGN_PI)); EXPECT_EQ(math::Matrix4d::LookAt(math::Vector3d(1, 6, 1), - math::Vector3d::One).Pose(), + math::Vector3d::One) + .Pose(), math::Pose3d(1, 6, 1, 0, 0, -IGN_PI_2)); EXPECT_EQ(math::Matrix4d::LookAt(math::Vector3d(-1, -1, 0), - math::Vector3d(1, 1, 0)).Pose(), + math::Vector3d(1, 1, 0)) + .Pose(), math::Pose3d(-1, -1, 0, 0, 0, IGN_PI_4)); // Default up is Z @@ -711,12 +712,13 @@ TEST(Matrix4dTest, LookAt) // Different ups EXPECT_EQ(math::Matrix4d::LookAt(math::Vector3d::One, math::Vector3d(0, 1, 1), - math::Vector3d::UnitY).Pose(), + math::Vector3d::UnitY) + .Pose(), math::Pose3d(1, 1, 1, IGN_PI_2, 0, IGN_PI)); EXPECT_EQ(math::Matrix4d::LookAt(math::Vector3d::One, math::Vector3d(0, 1, 1), - math::Vector3d(0, 1, 1)).Pose(), + math::Vector3d(0, 1, 1)) + .Pose(), math::Pose3d(1, 1, 1, IGN_PI_4, 0, IGN_PI)); } - From 7ac05b68d4a7287e657790903e1d850c12cb8108 Mon Sep 17 00:00:00 2001 From: LolaSegura Date: Wed, 18 Aug 2021 16:14:00 -0300 Subject: [PATCH 07/15] Adds %rename tag to interface files in order to match pep-8 naiming style. Signed-off-by: LolaSegura --- src/Matrix3.i | 3 + src/Matrix3_TEST.py | 120 ++++++++-------- src/Matrix4.i | 5 +- src/Matrix4_TEST.py | 246 ++++++++++++++++----------------- src/Pose3.i | 14 +- src/Pose3_TEST.py | 132 +++++++++--------- src/Quaternion.i | 8 +- src/Quaternion_TEST.py | 304 ++++++++++++++++++++--------------------- 8 files changed, 414 insertions(+), 418 deletions(-) diff --git a/src/Matrix3.i b/src/Matrix3.i index 1b8ef81a7..6b9ebbdb3 100644 --- a/src/Matrix3.i +++ b/src/Matrix3.i @@ -34,6 +34,9 @@ namespace ignition template class Matrix3 { + %rename("%(undercase)s", %$isfunction, %$ismember, %$not %$isconstructor) ""; + %rename("%(uppercase)s", %$isstatic, %$isvariable) ""; + public: static const Matrix3 Identity; public: static const Matrix3 Zero; public: Matrix3(); diff --git a/src/Matrix3_TEST.py b/src/Matrix3_TEST.py index a7bf268ea..abf325ed1 100644 --- a/src/Matrix3_TEST.py +++ b/src/Matrix3_TEST.py @@ -31,22 +31,22 @@ def test_matrix3d(self): self.assertAlmostEqual(matrix1, Matrix3d(1, 2, 3, 4, 5, 6, 7, 8, 9)) matrix = Matrix3d() - matrix.Axes(Vector3d(1, 1, 1), Vector3d(2, 2, 2), + matrix.axes(Vector3d(1, 1, 1), Vector3d(2, 2, 2), Vector3d(3, 3, 3)) self.assertAlmostEqual(matrix, Matrix3d(1, 2, 3, 1, 2, 3, 1, 2, 3)) - matrix.Axis(Vector3d(1, 1, 1), math.pi) + matrix.axis(Vector3d(1, 1, 1), math.pi) self.assertAlmostEqual(matrix, Matrix3d(1, 2, 2, 2, 1, 2, 2, 2, 1)) - matrix.Col(0, Vector3d(3, 4, 5)) + matrix.col(0, Vector3d(3, 4, 5)) self.assertAlmostEqual(matrix, Matrix3d(3, 2, 2, 4, 1, 2, 5, 2, 1)) - matrix.Col(3, Vector3d(1, 1, 1)) + matrix.col(3, Vector3d(1, 1, 1)) self.assertAlmostEqual(matrix, Matrix3d(3, 2, 1, 4, 1, 1, 5, 2, 1)) def test_sub(self): - matZero = Matrix3d.Zero - matIdent = Matrix3d.Identity + matZero = Matrix3d.ZERO + matIdent = Matrix3d.IDENTITY mat = matIdent - matZero self.assertAlmostEqual(mat, matIdent) @@ -65,8 +65,8 @@ def test_sub(self): def test_add(self): - matZero = Matrix3d.Zero - matIdent = Matrix3d.Identity + matZero = Matrix3d.ZERO + matIdent = Matrix3d.IDENTITY mat = matIdent + matZero self.assertAlmostEqual(mat, matIdent) @@ -84,8 +84,8 @@ def test_add(self): 77, 88, 99)) def test_mul(self): - matZero = Matrix3d.Zero - matIdent = Matrix3d.Identity + matZero = Matrix3d.ZERO + matIdent = Matrix3d.IDENTITY mat = matIdent * matZero self.assertAlmostEqual(mat, matZero) @@ -116,14 +116,14 @@ def test_vector3_mul(self): matrix = Matrix3d(1, 2, 3, 4, 5, 6, 7, 8, 9) # Scalar - self.assertAlmostEqual(Matrix3d.Zero, matrix * 0) + self.assertAlmostEqual(Matrix3d.ZERO, matrix * 0) - # Vector3.Zero + # Vector3.ZERO self.assertAlmostEqual(Vector3d.Zero, matrix * Vector3d.Zero) - # Matrix3.Zero - self.assertAlmostEqual(Matrix3d.Zero, matrix * Matrix3d.Zero) - self.assertAlmostEqual(Matrix3d.Zero, Matrix3d.Zero * matrix) + # Matrix3.ZERO + self.assertAlmostEqual(Matrix3d.ZERO, matrix * Matrix3d.ZERO) + self.assertAlmostEqual(Matrix3d.ZERO, Matrix3d.ZERO * matrix) matrix = Matrix3d(1, 2, 3, 4, 5, 6, 7, 8, 9) @@ -139,9 +139,9 @@ def test_vector3_mul(self): self.assertAlmostEqual(Vector3d(matrix(0, 2), matrix(1, 2), matrix(2, 2)), matrix * Vector3d.UnitZ) - # Matrix3.Identity - self.assertAlmostEqual(matrix, matrix * Matrix3d.Identity) - self.assertAlmostEqual(matrix, Matrix3d.Identity * matrix) + # Matrix3.IDENTITY + self.assertAlmostEqual(matrix, matrix * Matrix3d.IDENTITY) + self.assertAlmostEqual(matrix, Matrix3d.IDENTITY * matrix) # Multiply arbitrary matrix by itself matrix = Matrix3d(1, 2, 3, 4, 5, 6, 7, 8, 9) @@ -170,47 +170,47 @@ def test_not_equal(self): self.assertFalse(matrix1 != matrix2) def test_equal_tolerance(self): - self.assertFalse(Matrix3d.Zero.Equal(Matrix3d.Identity, 1e-6)) - self.assertFalse(Matrix3d.Zero.Equal(Matrix3d.Identity, 1e-3)) - self.assertFalse(Matrix3d.Zero.Equal(Matrix3d.Identity, 1e-1)) - self.assertTrue(Matrix3d.Zero.Equal(Matrix3d.Identity, 1)) - self.assertTrue(Matrix3d.Zero.Equal(Matrix3d.Identity, 1.1)) + self.assertFalse(Matrix3d.ZERO.equal(Matrix3d.IDENTITY, 1e-6)) + self.assertFalse(Matrix3d.ZERO.equal(Matrix3d.IDENTITY, 1e-3)) + self.assertFalse(Matrix3d.ZERO.equal(Matrix3d.IDENTITY, 1e-1)) + self.assertTrue(Matrix3d.ZERO.equal(Matrix3d.IDENTITY, 1)) + self.assertTrue(Matrix3d.ZERO.equal(Matrix3d.IDENTITY, 1.1)) def test_inverse(self): - self.assertAlmostEqual(Matrix3d.Identity, Matrix3d.Identity.Inverse()) + self.assertAlmostEqual(Matrix3d.IDENTITY, Matrix3d.IDENTITY.inverse()) # Matrix multiplied by its inverse results in the identity matrix matrix1 = Matrix3d(-2, 4, 0, 0.1, 9, 55, -7, 1, 26) - matrix2 = matrix1.Inverse() - self.assertAlmostEqual(matrix1 * matrix2, Matrix3d.Identity) - self.assertAlmostEqual(matrix2 * matrix1, Matrix3d.Identity) + matrix2 = matrix1.inverse() + self.assertAlmostEqual(matrix1 * matrix2, Matrix3d.IDENTITY) + self.assertAlmostEqual(matrix2 * matrix1, Matrix3d.IDENTITY) # Inverse of inverse results in the same matrix - self.assertAlmostEqual((matrix1.Inverse()).Inverse(), matrix1) + self.assertAlmostEqual((matrix1.inverse()).inverse(), matrix1) # Invert multiplication by scalar scalar = 2.5 - self.assertAlmostEqual((matrix1 * scalar).Inverse(), - matrix1.Inverse() * (1.0/scalar)) + self.assertAlmostEqual((matrix1 * scalar).inverse(), + matrix1.inverse() * (1.0/scalar)) def test_determinant(self): - # |Zero matrix| = 0.0 - self.assertAlmostEqual(0.0, Matrix3d.Zero.Determinant()) + # |ZERO matrix| = 0.0 + self.assertAlmostEqual(0.0, Matrix3d.ZERO.determinant()) - # |Identity matrix| = 1.0 - self.assertAlmostEqual(1.0, Matrix3d.Identity.Determinant()) + # |IDENTITY matrix| = 1.0 + self.assertAlmostEqual(1.0, Matrix3d.IDENTITY.determinant()) # Determinant of arbitrary matrix m = Matrix3d(-2, 4, 0, 0.1, 9, 55, -7, 1, 26) - self.assertAlmostEqual(-1908.4, m.Determinant()) + self.assertAlmostEqual(-1908.4, m.determinant()) def test_transpose(self): - # Transpose of zero matrix is itself - self.assertAlmostEqual(Matrix3d.Zero, Matrix3d.Zero.Transposed()) + # transpose of zero matrix is itself + self.assertAlmostEqual(Matrix3d.ZERO, Matrix3d.ZERO.transposed()) - # Transpose of identity matrix is itself - self.assertAlmostEqual(Matrix3d.Identity, - Matrix3d.Identity.Transposed()) + # transpose of identity matrix is itself + self.assertAlmostEqual(Matrix3d.IDENTITY, + Matrix3d.IDENTITY.transposed()) # Matrix and expected transpose m = Matrix3d(-2, 4, 0, @@ -220,39 +220,39 @@ def test_transpose(self): 4, 9, 1, 0, 55, 26) self.assertNotEqual(m, mT) - self.assertAlmostEqual(m.Transposed(), mT) - self.assertAlmostEqual(m.Determinant(), m.Transposed().Determinant()) + self.assertAlmostEqual(m.transposed(), mT) + self.assertAlmostEqual(m.determinant(), m.transposed().determinant()) - mT.Transpose() + mT.transpose() self.assertAlmostEqual(m, mT) - def test_from2axes(self): + def test_from_2axes(self): v1 = Vector3d(1.0, 0.0, 0.0) v2 = Vector3d(0.0, 1.0, 0.0) m1 = Matrix3d() - m1.From2Axes(v1, v2) + m1.from_2axes(v1, v2) m2 = Matrix3d() - m2.From2Axes(v2, v1) + m2.from_2axes(v2, v1) m1Correct = Matrix3d(0, -1, 0, 1, 0, 0, 0, 0, 1) m2Correct = Matrix3d(m1Correct) - m2Correct.Transpose() + m2Correct.transpose() self.assertNotEqual(m1, m2) self.assertAlmostEqual(m1Correct, m1) self.assertAlmostEqual(m2Correct, m2) - self.assertAlmostEqual(Matrix3d.Identity, m1 * m2) + self.assertAlmostEqual(Matrix3d.IDENTITY, m1 * m2) self.assertAlmostEqual(v2, m1 * v1) self.assertAlmostEqual(v1, m2 * v2) # rotation about 45 degrees v1.Set(1.0, 0.0, 0.0) v2.Set(1.0, 1.0, 0.0) - m2.From2Axes(v1, v2) + m2.from_2axes(v1, v2) # m1 is 90 degrees rotation self.assertAlmostEqual(m1, m2*m2) @@ -260,39 +260,39 @@ def test_from2axes(self): v1.Set(0.5, 0.5, 0) v2.Set(-0.5, 0.5, 0) - m1.From2Axes(v1, v2) - m2.From2Axes(v2, v1) + m1.from_2axes(v1, v2) + m2.from_2axes(v2, v1) self.assertNotEqual(m1, m2) self.assertAlmostEqual(m1Correct, m1) self.assertAlmostEqual(m2Correct, m2) - self.assertAlmostEqual(Matrix3d.Identity, m1 * m2) + self.assertAlmostEqual(Matrix3d.IDENTITY, m1 * m2) self.assertAlmostEqual(v2, m1 * v1) self.assertAlmostEqual(v1, m2 * v2) # For zero-length vectors, a unit matrix is returned v1.Set(0, 0, 0) v2.Set(-0.5, 0.5, 0) - m1.From2Axes(v1, v2) - self.assertAlmostEqual(Matrix3d.Identity, m1) + m1.from_2axes(v1, v2) + self.assertAlmostEqual(Matrix3d.IDENTITY, m1) # For zero-length vectors, a unit matrix is returned v1.Set(-0.5, 0.5, 0) v2.Set(0, 0, 0) - m1.From2Axes(v1, v2) - self.assertAlmostEqual(Matrix3d.Identity, m1) + m1.from_2axes(v1, v2) + self.assertAlmostEqual(Matrix3d.IDENTITY, m1) # Parallel vectors v1.Set(1, 0, 0) v2.Set(2, 0, 0) - m1.From2Axes(v1, v2) - self.assertAlmostEqual(Matrix3d.Identity, m1) + m1.from_2axes(v1, v2) + self.assertAlmostEqual(Matrix3d.IDENTITY, m1) # Opposite vectors v1.Set(1, 0, 0) v2.Set(-2, 0, 0) - m1.From2Axes(v1, v2) - self.assertAlmostEqual(Matrix3d.Zero - Matrix3d.Identity, m1) + m1.from_2axes(v1, v2) + self.assertAlmostEqual(Matrix3d.ZERO - Matrix3d.IDENTITY, m1) if __name__ == '__main__': diff --git a/src/Matrix4.i b/src/Matrix4.i index b3e145ed9..a910431a5 100644 --- a/src/Matrix4.i +++ b/src/Matrix4.i @@ -34,6 +34,9 @@ namespace ignition template class Matrix4 { + %rename("%(undercase)s", %$isfunction, %$ismember, %$not %$isconstructor) ""; + %rename("%(uppercase)s", %$isstatic, %$isvariable) ""; + public: static const Matrix4 Identity; public: static const Matrix4 Zero; @@ -76,8 +79,6 @@ namespace ignition public: bool operator!=(const Matrix4 &_m) const; public: static Matrix4 LookAt(const Vector3 &_eye, const Vector3 &_target, const Vector3 &_up = Vector3::UnitZ); - - private: T data[4][4]; }; %extend Matrix4{ diff --git a/src/Matrix4_TEST.py b/src/Matrix4_TEST.py index f4c6a3354..d9dfb067a 100644 --- a/src/Matrix4_TEST.py +++ b/src/Matrix4_TEST.py @@ -66,39 +66,39 @@ def test_construct_from_pose3(self): pose = Pose3d(trans, qt) mat = Matrix4d(pose) - self.assertAlmostEqual(pose, mat.Pose()) - self.assertAlmostEqual(trans, mat.Translation()) - self.assertAlmostEqual(qt, mat.Rotation()) - self.assertAlmostEqual(pose.Inverse(), mat.Inverse().Pose()) - # ensure inverses multiply to identity - self.assertAlmostEqual(mat.Inverse() * mat, Matrix4d.Identity) - self.assertAlmostEqual(mat * mat.Inverse(), Matrix4d.Identity) - self.assertAlmostEqual(pose.Inverse() * pose, Pose3d.Zero) - self.assertAlmostEqual(pose * pose.Inverse(), Pose3d.Zero) - - # repeat test with *= - m = Matrix4d(Matrix4d.Identity) + self.assertAlmostEqual(pose, mat.pose()) + self.assertAlmostEqual(trans, mat.translation()) + self.assertAlmostEqual(qt, mat.rotation()) + self.assertAlmostEqual(pose.inverse(), mat.inverse().pose()) + # Ensure inverses multiply to identity + self.assertAlmostEqual(mat.inverse() * mat, Matrix4d.IDENTITY) + self.assertAlmostEqual(mat * mat.inverse(), Matrix4d.IDENTITY) + self.assertAlmostEqual(pose.inverse() * pose, Pose3d.ZERO) + self.assertAlmostEqual(pose * pose.inverse(), Pose3d.ZERO) + + # Repeat test with *= + m = Matrix4d(Matrix4d.IDENTITY) m *= mat self.assertAlmostEqual(m, mat) - m *= mat.Inverse() - self.assertAlmostEqual(m, Matrix4d.Identity) + m *= mat.inverse() + self.assertAlmostEqual(m, Matrix4d.IDENTITY) p = Pose3d() p *= pose self.assertAlmostEqual(p, pose) - p *= pose.Inverse() - self.assertAlmostEqual(p, Pose3d.Zero) + p *= pose.inverse() + self.assertAlmostEqual(p, Pose3d.ZERO) - # Zero values + # ZERO values trans = Vector3d(0, 0, 0) qt = Quaterniond(0, 0, 0) pose = Pose3d(trans, qt) mat = Matrix4d(pose) - self.assertAlmostEqual(pose, mat.Pose()) - self.assertAlmostEqual(trans, mat.Translation()) - self.assertAlmostEqual(qt, mat.Rotation()) - self.assertAlmostEqual(pose.Inverse(), mat.Inverse().Pose()) + self.assertAlmostEqual(pose, mat.pose()) + self.assertAlmostEqual(trans, mat.translation()) + self.assertAlmostEqual(qt, mat.rotation()) + self.assertAlmostEqual(pose.inverse(), mat.inverse().pose()) # Rotate pitch by pi/2 so yaw coincides with roll causing a gimbal lock trans = Vector3d(3, 2, 1) @@ -106,12 +106,12 @@ def test_construct_from_pose3(self): pose = Pose3d(trans, qt) mat = Matrix4d(pose) - self.assertAlmostEqual(pose, mat.Pose()) - self.assertAlmostEqual(trans, mat.Translation()) - self.assertAlmostEqual(qt, mat.Rotation()) - self.assertAlmostEqual(pose.Inverse(), mat.Inverse().Pose()) + self.assertAlmostEqual(pose, mat.pose()) + self.assertAlmostEqual(trans, mat.translation()) + self.assertAlmostEqual(qt, mat.rotation()) + self.assertAlmostEqual(pose.inverse(), mat.inverse().pose()) - # setup a ZXZ rotation to ensure non-commutative rotations + # Setup a ZXZ rotation to ensure non-commutative rotations pose1 = Pose3d(1, -2, 3, 0, 0, math.pi/4) pose2 = Pose3d(0, 1, -1, -math.pi/4, 0, 0) pose3 = Pose3d(-1, 0, 0, 0, 0, -math.pi/4) @@ -120,15 +120,15 @@ def test_construct_from_pose3(self): m2 = Matrix4d(pose2) m3 = Matrix4d(pose3) - # ensure rotations are not commutative + # Ensure rotations are not commutative self.assertNotEqual(m1 * m2 * m3, m3 * m2 * m1) - # ensure pose multiplication order matches matrix order + # Ensure pose multiplication order matches matrix order self.assertAlmostEqual(m1 * m2 * m3, Matrix4d(pose1 * pose2 * pose3)) self.assertAlmostEqual(m3 * m2 * m1, Matrix4d(pose3 * pose2 * pose1)) - # repeat test with *= - m = Matrix4d(Matrix4d.Identity) + # Repeat test with *= + m = Matrix4d(Matrix4d.IDENTITY) p = Pose3d() m *= m1 @@ -153,8 +153,8 @@ def test_scale(self): mat = Matrix4d() mat2 = Matrix4d() - mat.Scale(Vector3d(1, 2, 3)) - mat2.Scale(1, 2, 3) + mat.scale(Vector3d(1, 2, 3)) + mat2.scale(1, 2, 3) self.assertAlmostEqual(mat, mat2) @@ -168,8 +168,8 @@ def test_scale(self): self.assertAlmostEqual(mat2(2, 2), 3.0) self.assertAlmostEqual(mat2(3, 3), 1.0) - self.assertAlmostEqual(mat.Scale(), mat2.Scale()) - self.assertAlmostEqual(mat.Scale(), Vector3d(1, 2, 3)) + self.assertAlmostEqual(mat.scale(), mat2.scale()) + self.assertAlmostEqual(mat.scale(), Vector3d(1, 2, 3)) for i in range(0, 4): for j in range(0, 4): @@ -186,7 +186,7 @@ def test_multiply_vect(self): self.assertAlmostEqual(mat * vec, Vector3d(0.0, 0.0, 0.0)) - mat = Matrix4d(Matrix4d.Identity) + mat = Matrix4d(Matrix4d.IDENTITY) self.assertAlmostEqual(mat * vec, vec) def test_multiply_mat(self): @@ -218,7 +218,7 @@ def test_inverse(self): 0, 2, -3, 2, 0, 2, 3, 1) - mat1 = mat.Inverse() + mat1 = mat.inverse() self.assertAlmostEqual(mat1, Matrix4d(18, -35, -28, 1, 9, -18, -14, 1, -2, 4, 3, 0, @@ -229,7 +229,7 @@ def test_get_pose3(self): 1, 0, 3, 1, 0, 2, -3, 2, 0, 2, 3, 1) - pose = mat.Pose() + pose = mat.pose() self.assertAlmostEqual(pose, Pose3d(5, 1, 2, -0.204124, 1.22474, 0.816497, 0.204124)) @@ -238,8 +238,8 @@ def test_translation(self): mat = Matrix4d() mat2 = Matrix4d() - mat.SetTranslation(Vector3d(1, 2, 3)) - mat2.SetTranslation(1, 2, 3) + mat.set_translation(Vector3d(1, 2, 3)) + mat2.set_translation(1, 2, 3) self.assertEqual(mat, mat2) @@ -251,8 +251,8 @@ def test_translation(self): self.assertAlmostEqual(mat2(1, 3), 2.0) self.assertAlmostEqual(mat2(2, 3), 3.0) - self.assertEqual(mat.Translation(), mat2.Translation()) - self.assertEqual(mat.Translation(), Vector3d(1, 2, 3)) + self.assertEqual(mat.translation(), mat2.translation()) + self.assertEqual(mat.translation(), Vector3d(1, 2, 3)) for i in range(0, 4): for j in range(0, 2): @@ -268,16 +268,16 @@ def test_rotation_diag_zero(self): 0.9, 1.0, 0, 1.2, 1.3, 1.4, 1.5, 1.0) - quat = mat.Rotation() - self.assertAlmostEqual(quat.X(), 0.5, delta=1e-6) - self.assertAlmostEqual(quat.Y(), 0.35, delta=1e-6) - self.assertAlmostEqual(quat.Z(), 0.6, delta=1e-6) - self.assertAlmostEqual(quat.W(), 0.15, delta=1e-6) + quat = mat.rotation() + self.assertAlmostEqual(quat.x(), 0.5, delta=1e-6) + self.assertAlmostEqual(quat.y(), 0.35, delta=1e-6) + self.assertAlmostEqual(quat.z(), 0.6, delta=1e-6) + self.assertAlmostEqual(quat.w(), 0.15, delta=1e-6) - euler = mat.EulerRotation(True) + euler = mat.euler_rotation(True) self.assertAlmostEqual(euler, Vector3d(1.5708, -1.11977, 1.5708)) - euler = mat.EulerRotation(False) + euler = mat.euler_rotation(False) self.assertAlmostEqual(euler, Vector3d(-1.5708, 4.26136, -1.5708)) def test_rotation_diag_less_zero(self): @@ -286,16 +286,16 @@ def test_rotation_diag_less_zero(self): 0.9, 1.0, 0, 1.2, 1.3, 1.4, 1.5, 1.0) - quat = mat.Rotation() - self.assertAlmostEqual(quat.X(), 0.333712, delta=1e-6) - self.assertAlmostEqual(quat.Y(), 0.524404, delta=1e-6) - self.assertAlmostEqual(quat.Z(), 0.810443, delta=1e-6) - self.assertAlmostEqual(quat.W(), -0.286039, delta=1e-6) + quat = mat.rotation() + self.assertAlmostEqual(quat.x(), 0.333712, delta=1e-6) + self.assertAlmostEqual(quat.y(), 0.524404, delta=1e-6) + self.assertAlmostEqual(quat.z(), 0.810443, delta=1e-6) + self.assertAlmostEqual(quat.w(), -0.286039, delta=1e-6) - euler = mat.EulerRotation(True) + euler = mat.euler_rotation(True) self.assertAlmostEqual(euler, Vector3d(1.5708, -1.11977, 1.76819)) - euler = mat.EulerRotation(False) + euler = mat.euler_rotation(False) self.assertAlmostEqual(euler, Vector3d(-1.5708, 4.26136, -1.3734)) mat = Matrix4d(-0.1, 0.2, 0.3, 0.4, @@ -303,16 +303,16 @@ def test_rotation_diag_less_zero(self): 0.9, 1.0, 0.0, 1.2, 1.3, 1.4, 1.5, 1.0) - quat = mat.Rotation() - self.assertAlmostEqual(quat.X(), 0.526235, delta=1e-6) - self.assertAlmostEqual(quat.Y(), 0.745499, delta=1e-6) - self.assertAlmostEqual(quat.Z(), 0.570088, delta=1e-6) - self.assertAlmostEqual(quat.W(), 0.131559, delta=1e-6) + quat = mat.rotation() + self.assertAlmostEqual(quat.x(), 0.526235, delta=1e-6) + self.assertAlmostEqual(quat.y(), 0.745499, delta=1e-6) + self.assertAlmostEqual(quat.z(), 0.570088, delta=1e-6) + self.assertAlmostEqual(quat.w(), 0.131559, delta=1e-6) - euler = mat.EulerRotation(True) + euler = mat.euler_rotation(True) self.assertAlmostEqual(euler, Vector3d(1.5708, -1.11977, 1.76819)) - euler = mat.EulerRotation(False) + euler = mat.euler_rotation(False) self.assertAlmostEqual(euler, Vector3d(-1.5708, 4.26136, -1.3734)) def test_rotation(self): @@ -321,16 +321,16 @@ def test_rotation(self): 0.9, 1.0, 1.1, 1.2, 1.3, 1.4, 1.5, 1.6) - quat = mat.Rotation() - self.assertAlmostEqual(quat.X(), 0.0896421, delta=1e-6) - self.assertAlmostEqual(quat.Y(), -0.179284, delta=1e-6) - self.assertAlmostEqual(quat.Z(), 0.0896421, delta=1e-6) - self.assertAlmostEqual(quat.W(), 0.83666, delta=1e-6) + quat = mat.rotation() + self.assertAlmostEqual(quat.x(), 0.0896421, delta=1e-6) + self.assertAlmostEqual(quat.y(), -0.179284, delta=1e-6) + self.assertAlmostEqual(quat.z(), 0.0896421, delta=1e-6) + self.assertAlmostEqual(quat.w(), 0.83666, delta=1e-6) - euler = mat.EulerRotation(True) + euler = mat.euler_rotation(True) self.assertAlmostEqual(euler, Vector3d(0.737815, -1.11977, 1.3734)) - euler = mat.EulerRotation(False) + euler = mat.euler_rotation(False) self.assertAlmostEqual(euler, Vector3d(-2.40378, 4.26136, -1.76819)) def test_euler_rotation2(self): @@ -339,10 +339,10 @@ def test_euler_rotation2(self): 1.9, 1.2, 1.1, 1.2, 1.3, 1.4, 1.5, 1.6) - euler = mat.EulerRotation(True) + euler = mat.euler_rotation(True) self.assertAlmostEqual(euler, Vector3d(-2.55359, -1.5708, 0)) - euler = mat.EulerRotation(False) + euler = mat.euler_rotation(False) self.assertAlmostEqual(euler, Vector3d(-2.55359, -1.5708, 0)) mat = Matrix4d(0.1, 0.2, 0.3, 0.4, @@ -350,22 +350,22 @@ def test_euler_rotation2(self): -1.2, 1.2, 1.1, 1.2, 1.3, 1.4, 1.5, 1.6) - euler = mat.EulerRotation(True) + euler = mat.euler_rotation(True) self.assertAlmostEqual(euler, Vector3d(0.588003, 1.5708, 0)) - euler = mat.EulerRotation(False) + euler = mat.euler_rotation(False) self.assertAlmostEqual(euler, Vector3d(0.588003, 1.5708, 0)) def test_affine_transform(self): - mat = Matrix4d(Matrix4d.Zero) + mat = Matrix4d(Matrix4d.ZERO) vec = Vector3d(1, 2, 3) v = Vector3d() - self.assertFalse(mat.TransformAffine(vec, v)) + self.assertFalse(mat.transform_affine(vec, v)) - mat = Matrix4d(Matrix4d.Identity) - self.assertTrue(mat.TransformAffine(vec, v)) + mat = Matrix4d(Matrix4d.IDENTITY) + self.assertTrue(mat.transform_affine(vec, v)) def test_stream_out(self): matA = Matrix4d(1, 2, 3, 4, @@ -402,33 +402,33 @@ def test_not_equal(self): self.assertFalse(matrix1 != matrix2) def test_equal_tolerance(self): - self.assertFalse(Matrix4d.Zero.Equal(Matrix4d.Identity, 1e-6)) - self.assertFalse(Matrix4d.Zero.Equal(Matrix4d.Identity, 1e-3)) - self.assertFalse(Matrix4d.Zero.Equal(Matrix4d.Identity, 1e-1)) - self.assertTrue(Matrix4d.Zero.Equal(Matrix4d.Identity, 1)) - self.assertTrue(Matrix4d.Zero.Equal(Matrix4d.Identity, 1.1)) + self.assertFalse(Matrix4d.ZERO.equal(Matrix4d.IDENTITY, 1e-6)) + self.assertFalse(Matrix4d.ZERO.equal(Matrix4d.IDENTITY, 1e-3)) + self.assertFalse(Matrix4d.ZERO.equal(Matrix4d.IDENTITY, 1e-1)) + self.assertTrue(Matrix4d.ZERO.equal(Matrix4d.IDENTITY, 1)) + self.assertTrue(Matrix4d.ZERO.equal(Matrix4d.IDENTITY, 1.1)) def test_determinant(self): - # |Zero matrix| = 0.0 - self.assertAlmostEqual(0.0, Matrix4d.Zero.Determinant()) + # |ZERO matrix| = 0.0 + self.assertAlmostEqual(0.0, Matrix4d.ZERO.determinant()) - # |Identity matrix| = 1.0 - self.assertAlmostEqual(1.0, Matrix4d.Identity.Determinant()) + # |IDENTITY matrix| = 1.0 + self.assertAlmostEqual(1.0, Matrix4d.IDENTITY.determinant()) # Determinant of arbitrary matrix m = Matrix4d(2, 3, 0.1, -5, 1, 0, 3.2, 1, 0, 2, -3, 2.1, 0, 2, 3.2, 1) - self.assertAlmostEqual(129.82, m.Determinant()) + self.assertAlmostEqual(129.82, m.determinant()) def test_transpose(self): # Transpose of zero matrix is itself - self.assertAlmostEqual(Matrix4d.Zero, Matrix4d.Zero.Transposed()) + self.assertAlmostEqual(Matrix4d.ZERO, Matrix4d.ZERO.transposed()) # Transpose of identity matrix is itself - self.assertAlmostEqual(Matrix4d.Identity, - Matrix4d.Identity.Transposed()) + self.assertAlmostEqual(Matrix4d.IDENTITY, + Matrix4d.IDENTITY.transposed()) # Matrix and expected transpose m = Matrix4d(-2, 4, 0, -3.5, @@ -440,79 +440,79 @@ def test_transpose(self): 0, 55, 26, -5, -3.5, 1.2, 11.5, -0.1) self.assertNotEqual(m, mT) - self.assertEqual(m.Transposed(), mT) - self.assertAlmostEqual(m.Determinant(), m.Transposed().Determinant()) + self.assertEqual(m.transposed(), mT) + self.assertAlmostEqual(m.determinant(), m.transposed().determinant()) - mT.Transpose() + mT.transpose() self.assertEqual(m, mT) def test_look_at(self): - self.assertAlmostEqual(Matrix4d.LookAt(-Vector3d.UnitX, - Vector3d.Zero).Pose(), + self.assertAlmostEqual(Matrix4d.look_at(-Vector3d.UnitX, + Vector3d.Zero).pose(), Pose3d(-1, 0, 0, 0, 0, 0)) - self.assertAlmostEqual(Matrix4d.LookAt(Vector3d(3, 2, 0), - Vector3d(0, 2, 0)).Pose(), + self.assertAlmostEqual(Matrix4d.look_at(Vector3d(3, 2, 0), + Vector3d(0, 2, 0)).pose(), Pose3d(3, 2, 0, 0, 0, math.pi)) - self.assertAlmostEqual(Matrix4d.LookAt(Vector3d(1, 6, 1), - Vector3d.One).Pose(), + self.assertAlmostEqual(Matrix4d.look_at(Vector3d(1, 6, 1), + Vector3d.One).pose(), Pose3d(1, 6, 1, 0, 0, -math.pi/2)) - self.assertAlmostEqual(Matrix4d.LookAt(Vector3d(-1, -1, 0), - Vector3d(1, 1, 0)).Pose(), + self.assertAlmostEqual(Matrix4d.look_at(Vector3d(-1, -1, 0), + Vector3d(1, 1, 0)).pose(), Pose3d(-1, -1, 0, 0, 0, math.pi/4)) - # Default up is Z - self.assertAlmostEqual(Matrix4d.LookAt(Vector3d(0.1, -5, 222), + # Default up is z + self.assertAlmostEqual(Matrix4d.look_at(Vector3d(0.1, -5, 222), Vector3d(999, -0.6, 0)), - Matrix4d.LookAt(Vector3d(0.1, -5, 222), + Matrix4d.look_at(Vector3d(0.1, -5, 222), Vector3d(999, -0.6, 0), Vector3d.UnitZ)) - # up == zero, default up = +Z - self.assertAlmostEqual(Matrix4d.LookAt(Vector3d(1.23, 456, 0.7), + # up == zero, default up = +z + self.assertAlmostEqual(Matrix4d.look_at(Vector3d(1.23, 456, 0.7), Vector3d(0, 8.9, -10), Vector3d.Zero), - Matrix4d.LookAt(Vector3d(1.23, 456, 0.7), + Matrix4d.look_at(Vector3d(1.23, 456, 0.7), Vector3d(0, 8.9, -10))) - # up == +X, default up = +Z - self.assertAlmostEqual(Matrix4d.LookAt(Vector3d(0.25, 9, -5), + # up == +x, default up = +z + self.assertAlmostEqual(Matrix4d.look_at(Vector3d(0.25, 9, -5), Vector3d(-6, 0, 0.4), Vector3d.UnitX), - Matrix4d.LookAt(Vector3d(0.25, 9, -5), + Matrix4d.look_at(Vector3d(0.25, 9, -5), Vector3d(-6, 0, 0.4))) - # up == -X, default up = +Z - self.assertAlmostEqual(Matrix4d.LookAt(Vector3d(0, 0, 0.2), + # up == -x, default up = +z + self.assertAlmostEqual(Matrix4d.look_at(Vector3d(0, 0, 0.2), Vector3d(-8, 0, -6), -Vector3d.UnitX), - Matrix4d.LookAt(Vector3d(0, 0, 0.2), + Matrix4d.look_at(Vector3d(0, 0, 0.2), Vector3d(-8, 0, -6))) - # eye == target, default direction = +X - self.assertAlmostEqual(Matrix4d.LookAt(Vector3d.One, + # eye == target, default direction = +x + self.assertAlmostEqual(Matrix4d.look_at(Vector3d.One, Vector3d.One), - Matrix4d.LookAt(Vector3d.One, + Matrix4d.look_at(Vector3d.One, Vector3d(1.0001, 1, 1))) - # Not possible to keep _up on +Z - self.assertAlmostEqual(Matrix4d.LookAt(Vector3d(-1, 0, 10), + # Not possible to keep _up on +z + self.assertAlmostEqual(Matrix4d.look_at(Vector3d(-1, 0, 10), Vector3d(-1, 0, 0)), - Matrix4d.LookAt(Vector3d(-1, 0, 10), + Matrix4d.look_at(Vector3d(-1, 0, 10), Vector3d(-1, 0, 0), -Vector3d.UnitX)) # Different ups - self.assertAlmostEqual(Matrix4d.LookAt(Vector3d.One, + self.assertAlmostEqual(Matrix4d.look_at(Vector3d.One, Vector3d(0, 1, 1), - Vector3d.UnitY).Pose(), + Vector3d.UnitY).pose(), Pose3d(1, 1, 1, math.pi/2, 0, math.pi)) - self.assertAlmostEqual(Matrix4d.LookAt(Vector3d.One, + self.assertAlmostEqual(Matrix4d.look_at(Vector3d.One, Vector3d(0, 1, 1), - Vector3d(0, 1, 1)).Pose(), + Vector3d(0, 1, 1)).pose(), Pose3d(1, 1, 1, math.pi/4, 0, math.pi)) diff --git a/src/Pose3.i b/src/Pose3.i index a23e0d1ca..3937fd999 100644 --- a/src/Pose3.i +++ b/src/Pose3.i @@ -34,6 +34,9 @@ namespace ignition template class Pose3 { + %rename("%(undercase)s", %$isfunction, %$ismember, %$not %$isconstructor) ""; + %rename("%(uppercase)s", %$isstatic, %$isvariable) ""; + public: static const Pose3 Zero; public: Pose3() : p(0, 0, 0), q(1, 0, 0, 0); @@ -68,6 +71,7 @@ namespace ignition public: inline Quaternion CoordRotationSub( const Quaternion &_rot) const; public: Pose3 CoordPoseSolve(const Pose3 &_b) const; + public: void Reset(); public: Pose3 RotatePositionAboutOrigin(const Quaternion &_q) const; public: void Round(int _precision); public: inline const Vector3 &Pos() const; @@ -83,18 +87,8 @@ namespace ignition public: inline const T Roll() const; public: inline const T Pitch() const; public: inline const T Yaw() const; - - private: Vector3 p; - private: Quaternion q; }; - %extend Pose3 { - void Reset() { - ignition::math::Vector3 vect; - (*$self).Set(vect, ignition::math::Quaternion::Identity); - } - } - %extend Pose3 { std::string __str__() const { std::ostringstream out; diff --git a/src/Pose3_TEST.py b/src/Pose3_TEST.py index cd7a5749c..f75a8fc70 100644 --- a/src/Pose3_TEST.py +++ b/src/Pose3_TEST.py @@ -41,31 +41,31 @@ def test_pose(self): # A is the transform from O to P specified in frame O # B is the transform from P to Q specified in frame P # then, B + A is the transform from O to Q specified in frame O - self.assertAlmostEqual((B + A).Pos().X(), 1.0 + 1.0/math.sqrt(2)) - self.assertAlmostEqual((B + A).Pos().Y(), 1.0/math.sqrt(2)) - self.assertAlmostEqual((B + A).Pos().Z(), 0.0) - self.assertAlmostEqual((B + A).Rot().Euler().X(), 0.0) - self.assertAlmostEqual((B + A).Rot().Euler().Y(), 0.0) - self.assertAlmostEqual((B + A).Rot().Euler().Z(), 3.0*math.pi/4.0) + self.assertAlmostEqual((B + A).pos().X(), 1.0 + 1.0/math.sqrt(2)) + self.assertAlmostEqual((B + A).pos().Y(), 1.0/math.sqrt(2)) + self.assertAlmostEqual((B + A).pos().Z(), 0.0) + self.assertAlmostEqual((B + A).rot().euler().X(), 0.0) + self.assertAlmostEqual((B + A).rot().euler().Y(), 0.0) + self.assertAlmostEqual((B + A).rot().euler().Z(), 3.0*math.pi/4.0) # If: # A is the transform from O to P in frame O # B is the transform from O to Q in frame O # then -A is transform from P to O specified in frame P - self.assertAlmostEqual((Pose3d() - A).Pos().X(), -1.0/math.sqrt(2)) - self.assertAlmostEqual((Pose3d() - A).Pos().Y(), 1.0/math.sqrt(2)) - self.assertAlmostEqual((Pose3d() - A).Pos().Z(), 0.0) - self.assertAlmostEqual((Pose3d() - A).Rot().Euler().X(), 0.0) - self.assertAlmostEqual((Pose3d() - A).Rot().Euler().Y(), 0.0) - self.assertAlmostEqual((Pose3d() - A).Rot().Euler().Z(), -math.pi/4) + self.assertAlmostEqual((Pose3d() - A).pos().X(), -1.0/math.sqrt(2)) + self.assertAlmostEqual((Pose3d() - A).pos().Y(), 1.0/math.sqrt(2)) + self.assertAlmostEqual((Pose3d() - A).pos().Z(), 0.0) + self.assertAlmostEqual((Pose3d() - A).rot().euler().X(), 0.0) + self.assertAlmostEqual((Pose3d() - A).rot().euler().Y(), 0.0) + self.assertAlmostEqual((Pose3d() - A).rot().euler().Z(), -math.pi/4) # test negation operator - self.assertAlmostEqual((-A).Pos().X(), -1.0/math.sqrt(2)) - self.assertAlmostEqual((-A).Pos().Y(), 1.0/math.sqrt(2)) - self.assertAlmostEqual((-A).Pos().Z(), 0.0) - self.assertAlmostEqual((-A).Rot().Euler().X(), 0.0) - self.assertAlmostEqual((-A).Rot().Euler().Y(), 0.0) - self.assertAlmostEqual((-A).Rot().Euler().Z(), -math.pi/4.0) + self.assertAlmostEqual((-A).pos().X(), -1.0/math.sqrt(2)) + self.assertAlmostEqual((-A).pos().Y(), 1.0/math.sqrt(2)) + self.assertAlmostEqual((-A).pos().Z(), 0.0) + self.assertAlmostEqual((-A).rot().euler().X(), 0.0) + self.assertAlmostEqual((-A).rot().euler().Y(), 0.0) + self.assertAlmostEqual((-A).rot().euler().Z(), -math.pi/4.0) # If: # A is the transform from O to P in frame O @@ -73,32 +73,32 @@ def test_pose(self): # B - A is the transform from P to Q in frame P B = Pose3d(Vector3d(1, 1, 0), Quaterniond(0, 0, math.pi/2.0)) - self.assertAlmostEqual((B - A).Pos().X(), 1.0/math.sqrt(2)) - self.assertAlmostEqual((B - A).Pos().Y(), 1.0/math.sqrt(2)) - self.assertAlmostEqual((B - A).Pos().Z(), 0.0) - self.assertAlmostEqual((B - A).Rot().Euler().X(), 0.0) - self.assertAlmostEqual((B - A).Rot().Euler().Y(), 0.0) - self.assertAlmostEqual((B - A).Rot().Euler().Z(), math.pi/4.0) + self.assertAlmostEqual((B - A).pos().X(), 1.0/math.sqrt(2)) + self.assertAlmostEqual((B - A).pos().Y(), 1.0/math.sqrt(2)) + self.assertAlmostEqual((B - A).pos().Z(), 0.0) + self.assertAlmostEqual((B - A).rot().euler().X(), 0.0) + self.assertAlmostEqual((B - A).rot().euler().Y(), 0.0) + self.assertAlmostEqual((B - A).rot().euler().Z(), math.pi/4.0) pose = Pose3d() - self.assertTrue(pose.Pos() == Vector3d(0, 0, 0)) - self.assertTrue(pose.Rot() == Quaterniond(0, 0, 0)) + self.assertTrue(pose.pos() == Vector3d(0, 0, 0)) + self.assertTrue(pose.rot() == Quaterniond(0, 0, 0)) pose = Pose3d(Vector3d(1, 2, 3), Quaterniond(.1, .2, .3)) - self.assertTrue(pose.Pos() == Vector3d(1, 2, 3)) - self.assertTrue(pose.Rot() == Quaterniond(.1, .2, .3)) + self.assertTrue(pose.pos() == Vector3d(1, 2, 3)) + self.assertTrue(pose.rot() == Quaterniond(.1, .2, .3)) pose1 = Pose3d(pose) self.assertTrue(pose1 == pose) - pose.Set(Vector3d(2, 3, 4), Quaterniond(.3, .4, .5)) - self.assertTrue(pose.Pos() == Vector3d(2, 3, 4)) - self.assertTrue(pose.Rot() == Quaterniond(.3, .4, .5)) - self.assertTrue(pose.IsFinite()) + pose.set(Vector3d(2, 3, 4), Quaterniond(.3, .4, .5)) + self.assertTrue(pose.pos() == Vector3d(2, 3, 4)) + self.assertTrue(pose.rot() == Quaterniond(.3, .4, .5)) + self.assertTrue(pose.is_finite()) - pose1 = pose.Inverse() - self.assertTrue(pose1.Pos() == Vector3d(-1.38368, -3.05541, -4.21306)) - self.assertTrue(pose1.Rot() == Quaterniond(0.946281, -0.0933066, + pose1 = pose.inverse() + self.assertTrue(pose1.pos() == Vector3d(-1.38368, -3.05541, -4.21306)) + self.assertTrue(pose1.rot() == Quaterniond(0.946281, -0.0933066, -0.226566, -0.210984)) pose = Pose3d(1, 2, 3, .1, .2, .3) + Pose3d(4, 5, 6, .4, .5, .6) @@ -112,33 +112,33 @@ def test_pose(self): pose -= Pose3d(pose) self.assertTrue(pose == Pose3d(0, 0, 0, 0, 0, 0)) - pose.Pos().Set(5, 6, 7) - pose.Rot().Euler(Vector3d(.4, .6, 0)) + pose.pos().Set(5, 6, 7) + pose.rot().euler(Vector3d(.4, .6, 0)) - self.assertTrue(pose.CoordPositionAdd(Vector3d(1, 2, 3)) == + self.assertTrue(pose.coord_position_add(Vector3d(1, 2, 3)) == Vector3d(7.82531, 6.67387, 9.35871)) - self.assertTrue(pose.CoordPositionAdd(pose1) == + self.assertTrue(pose.coord_position_add(pose1) == Vector3d(2.58141, 2.4262, 3.8013)) - self.assertTrue(pose.CoordRotationAdd(Quaterniond(0.1, 0, 0.2)) == + self.assertTrue(pose.coord_rotation_add(Quaterniond(0.1, 0, 0.2)) == Quaterniond(0.520975, 0.596586, 0.268194)) - self.assertTrue(pose.CoordPoseSolve(pose1) == + self.assertTrue(pose.coord_pose_solve(pose1) == Pose3d(-0.130957, -11.552, -10.2329, -0.462955, -1.15624, -0.00158047)) - self.assertTrue(pose.RotatePositionAboutOrigin( + self.assertTrue(pose.rotate_position_about_origin( Quaterniond(0.1, 0, 0.2)) == Pose3d(6.09235, 5.56147, 6.47714, 0.4, 0.6, 0)) - pose.Reset() - self.assertTrue(pose.Pos() == Vector3d(0, 0, 0)) - self.assertTrue(pose.Rot() == Quaterniond(0, 0, 0)) + pose.reset() + self.assertTrue(pose.pos() == Vector3d(0, 0, 0)) + self.assertTrue(pose.rot() == Quaterniond(0, 0, 0)) def test_pose_atributes(self): pose = Pose3d(0, 1, 2, 1, 0, 0) - self.assertTrue(pose.Pos() == Vector3d(0, 1, 2)) - self.assertTrue(pose.Rot() == Quaterniond(1, 0, 0)) + self.assertTrue(pose.pos() == Vector3d(0, 1, 2)) + self.assertTrue(pose.rot() == Quaterniond(1, 0, 0)) def test_stream_out(self): p = Pose3d(0.1, 1.2, 2.3, 0.0, 0.1, 1.0) @@ -147,36 +147,36 @@ def test_stream_out(self): def test_mutable_pose(self): pose = Pose3d(0, 1, 2, 0, 0, 0) - self.assertTrue(pose.Pos() == Vector3d(0, 1, 2)) - self.assertTrue(pose.Rot() == Quaterniond(0, 0, 0)) + self.assertTrue(pose.pos() == Vector3d(0, 1, 2)) + self.assertTrue(pose.rot() == Quaterniond(0, 0, 0)) pose = Pose3d(Vector3d(10, 20, 30), Quaterniond(1, 2, 1)) - self.assertTrue(pose.Pos() == Vector3d(10, 20, 30)) - self.assertTrue(pose.Rot() == Quaterniond(1, 2, 1)) + self.assertTrue(pose.pos() == Vector3d(10, 20, 30)) + self.assertTrue(pose.rot() == Quaterniond(1, 2, 1)) def test_pose_elements(self): pose = Pose3d(0, 1, 2, 1, 1, 2) - self.assertAlmostEqual(pose.X(), 0) - self.assertAlmostEqual(pose.Y(), 1) - self.assertAlmostEqual(pose.Z(), 2) - self.assertAlmostEqual(pose.Roll(), 1) - self.assertAlmostEqual(pose.Pitch(), 1) - self.assertAlmostEqual(pose.Yaw(), 2) + self.assertAlmostEqual(pose.x(), 0) + self.assertAlmostEqual(pose.y(), 1) + self.assertAlmostEqual(pose.z(), 2) + self.assertAlmostEqual(pose.roll(), 1) + self.assertAlmostEqual(pose.pitch(), 1) + self.assertAlmostEqual(pose.yaw(), 2) def test_set_pose_element(self): pose = Pose3d(1, 2, 3, 1.57, 1, 2) - self.assertAlmostEqual(pose.X(), 1) - self.assertAlmostEqual(pose.Y(), 2) - self.assertAlmostEqual(pose.Z(), 3) + self.assertAlmostEqual(pose.x(), 1) + self.assertAlmostEqual(pose.y(), 2) + self.assertAlmostEqual(pose.z(), 3) - pose.SetX(10) - pose.SetY(12) - pose.SetZ(13) + pose.set_x(10) + pose.set_y(12) + pose.set_z(13) - self.assertAlmostEqual(pose.X(), 10) - self.assertAlmostEqual(pose.Y(), 12) - self.assertAlmostEqual(pose.Z(), 13) + self.assertAlmostEqual(pose.x(), 10) + self.assertAlmostEqual(pose.y(), 12) + self.assertAlmostEqual(pose.z(), 13) if __name__ == '__main__': diff --git a/src/Quaternion.i b/src/Quaternion.i index 31897a8d3..ae56c5dd1 100644 --- a/src/Quaternion.i +++ b/src/Quaternion.i @@ -32,6 +32,9 @@ namespace ignition template class Quaternion { + %rename("%(undercase)s", %$isfunction, %$ismember, %$not %$isconstructor) ""; + %rename("%(uppercase)s", %$isstatic, %$isvariable) ""; + public: static const Quaternion Identity; public: static const Quaternion Zero; @@ -97,11 +100,6 @@ namespace ignition public: inline void Y(T _v); public: inline void Z(T _v); public: inline void W(T _v); - - private: T qw; - private: T qx; - private: T qy; - private: T qz; }; %extend Quaternion{ diff --git a/src/Quaternion_TEST.py b/src/Quaternion_TEST.py index 52f631555..e8331005a 100644 --- a/src/Quaternion_TEST.py +++ b/src/Quaternion_TEST.py @@ -45,30 +45,30 @@ def test_construction(self): def test_unit(self): q = Quaterniond() - self.assertAlmostEqual(q.W(), 1.0) - self.assertAlmostEqual(q.X(), 0.0) - self.assertAlmostEqual(q.Y(), 0.0) - self.assertAlmostEqual(q.Z(), 0.0) + self.assertAlmostEqual(q.w(), 1.0) + self.assertAlmostEqual(q.x(), 0.0) + self.assertAlmostEqual(q.y(), 0.0) + self.assertAlmostEqual(q.z(), 0.0) def test_construct_values(self): q = Quaterniond(1.0, 2.0, 3.0, 4.0) - self.assertAlmostEqual(q.W(), 1.0) - self.assertAlmostEqual(q.X(), 2.0) - self.assertAlmostEqual(q.Y(), 3.0) - self.assertAlmostEqual(q.Z(), 4.0) + self.assertAlmostEqual(q.w(), 1.0) + self.assertAlmostEqual(q.x(), 2.0) + self.assertAlmostEqual(q.y(), 3.0) + self.assertAlmostEqual(q.z(), 4.0) def test_construct_zero(self): q = Quaterniond(0.0, 0.0, 0.0, 0.0) - self.assertAlmostEqual(q.W(), 0.0) - self.assertAlmostEqual(q.X(), 0.0) - self.assertAlmostEqual(q.Y(), 0.0) - self.assertAlmostEqual(q.Z(), 0.0) + self.assertAlmostEqual(q.w(), 0.0) + self.assertAlmostEqual(q.x(), 0.0) + self.assertAlmostEqual(q.y(), 0.0) + self.assertAlmostEqual(q.z(), 0.0) - qI = q.Inverse() - self.assertAlmostEqual(qI.W(), 1.0) - self.assertAlmostEqual(qI.X(), 0.0) - self.assertAlmostEqual(qI.Y(), 0.0) - self.assertAlmostEqual(qI.Z(), 0.0) + qI = q.inverse() + self.assertAlmostEqual(qI.w(), 1.0) + self.assertAlmostEqual(qI.x(), 0.0) + self.assertAlmostEqual(qI.y(), 0.0) + self.assertAlmostEqual(qI.z(), 0.0) def test_construct_euler(self): q = Quaterniond(0, 1, 2) @@ -77,10 +77,10 @@ def test_construct_euler(self): def test_construct_axis_angle(self): q1 = Quaterniond(Vector3d(0, 0, 1), math.pi) - self.assertAlmostEqual(q1.X(), 0.0) - self.assertAlmostEqual(q1.Y(), 0.0) - self.assertAlmostEqual(q1.Z(), 1.0) - self.assertAlmostEqual(q1.W(), 0.0) + self.assertAlmostEqual(q1.x(), 0.0) + self.assertAlmostEqual(q1.y(), 0.0) + self.assertAlmostEqual(q1.z(), 1.0) + self.assertAlmostEqual(q1.w(), 0.0) q = Quaterniond(q1) self.assertTrue(q == q1) @@ -89,86 +89,86 @@ def test_equal(self): # double q = Quaterniond(1, 2, 3, 4) q2 = Quaterniond(1.01, 2.015, 3.002, 4.007) - self.assertTrue(q.Equal(q2, 0.02)) - self.assertFalse(q.Equal(q2, 0.01)) + self.assertTrue(q.equal(q2, 0.02)) + self.assertFalse(q.equal(q2, 0.01)) # floats q3 = Quaternionf(1, 2, 3, 4) q4 = Quaternionf(1.05, 2.1, 3.03, 4.04) - self.assertTrue(q3.Equal(q4, 0.2)) - self.assertFalse(q3.Equal(q4, 0.04)) + self.assertTrue(q3.equal(q4, 0.2)) + self.assertFalse(q3.equal(q4, 0.04)) # ints q5 = Quaternioni(3, 5, -1, 9) q6 = Quaternioni(3, 6, 1, 12) - self.assertTrue(q5.Equal(q6, 3)) - self.assertFalse(q5.Equal(q6, 2)) + self.assertTrue(q5.equal(q6, 3)) + self.assertFalse(q5.equal(q6, 2)) def test_identity(self): - q = Quaterniond.Identity - self.assertAlmostEqual(q.W(), 1.0) - self.assertAlmostEqual(q.X(), 0.0) - self.assertAlmostEqual(q.Y(), 0.0) - self.assertAlmostEqual(q.Z(), 0.0) + q = Quaterniond.IDENTITY + self.assertAlmostEqual(q.w(), 1.0) + self.assertAlmostEqual(q.x(), 0.0) + self.assertAlmostEqual(q.y(), 0.0) + self.assertAlmostEqual(q.z(), 0.0) def test_mathlog(self): q = Quaterniond(math.pi*0.1, math.pi*0.5, math.pi) - self.assertAlmostEqual(q.Log(), + self.assertAlmostEqual(q.log(), Quaterniond(0, -1.02593, 0.162491, 1.02593)) q1 = Quaterniond(q) - q1.W(2.0) - self.assertAlmostEqual(q1.Log(), + q1.w(2.0) + self.assertAlmostEqual(q1.log(), Quaterniond(0, -0.698401, 0.110616, 0.698401)) def test_math_exp(self): q = Quaterniond(math.pi*0.1, math.pi*0.5, math.pi) - self.assertAlmostEqual(q.Exp(), Quaterniond(0.545456, -0.588972, + self.assertAlmostEqual(q.exp(), Quaterniond(0.545456, -0.588972, 0.093284, 0.588972)) q1 = Quaterniond(q) - q1.X(0.000000001) - q1.Y(0.0) - q1.Z(0.0) - q1.W(0.0) - self.assertAlmostEqual(q1.Exp(), Quaterniond(1, 0, 0, 0)) + q1.x(0.000000001) + q1.y(0.0) + q1.z(0.0) + q1.w(0.0) + self.assertAlmostEqual(q1.exp(), Quaterniond(1, 0, 0, 0)) def test_math_invert(self): q = Quaterniond(math.pi*0.1, math.pi*0.5, math.pi) - q.Invert() + q.invert() self.assertAlmostEqual(q, Quaterniond(0.110616, 0.698401, -0.110616, -0.698401)) def test_math_axis(self): q = Quaterniond(math.pi*0.1, math.pi*0.5, math.pi) - q.Axis(0, 1, 0, math.pi) + q.axis(0, 1, 0, math.pi) self.assertAlmostEqual(q, Quaterniond(6.12303e-17, 0, 1, 0)) - q.Axis(Vector3d(1, 0, 0), math.pi) + q.axis(Vector3d(1, 0, 0), math.pi) self.assertAlmostEqual(q, Quaterniond(0, 1, 0, 0)) def test_math_set(self): q = Quaterniond(math.pi*0.1, math.pi*0.5, math.pi) - q.Set(1, 2, 3, 4) - self.assertAlmostEqual(q.W(), 1.0) - self.assertAlmostEqual(q.X(), 2.0) - self.assertAlmostEqual(q.Y(), 3.0) - self.assertAlmostEqual(q.Z(), 4.0) + q.set(1, 2, 3, 4) + self.assertAlmostEqual(q.w(), 1.0) + self.assertAlmostEqual(q.x(), 2.0) + self.assertAlmostEqual(q.y(), 3.0) + self.assertAlmostEqual(q.z(), 4.0) def test_math_normalized(self): q = Quaterniond(1, 2, 3, 4) - q2 = q.Normalized() + q2 = q.normalized() self.assertAlmostEqual(q2, Quaterniond(0.182574, 0.365148, 0.547723, 0.730297)) def test_normalize(self): q = Quaterniond(1, 2, 3, 4) - q.Normalize() + q.normalize() self.assertAlmostEqual(q, Quaterniond(0.182574, 0.365148, 0.547723, 0.730297)) @@ -176,7 +176,7 @@ def test_slerp(self): q1 = Quaterniond(0.1, 1.2, 2.3) q2 = Quaterniond(1.2, 2.3, -3.4) - q3 = Quaterniond.Slerp(1.0, q1, q2, True) + q3 = Quaterniond.slerp(1.0, q1, q2, True) self.assertAlmostEqual(q3, Quaterniond(0.554528, -0.717339, 0.32579, 0.267925)) @@ -185,10 +185,10 @@ def test_from2axes(self): v2 = Vector3d(0.0, 1.0, 0.0) q1 = Quaterniond() - q1.From2Axes(v1, v2) + q1.from_2axes(v1, v2) q2 = Quaterniond() - q2.From2Axes(v2, v1) + q2.from_2axes(v2, v1) q2Correct = Quaterniond(math.sqrt(2)/2, 0, 0, -math.sqrt(2)/2) q1Correct = Quaterniond(math.sqrt(2)/2, 0, 0, math.sqrt(2)/2) @@ -196,7 +196,7 @@ def test_from2axes(self): self.assertNotEqual(q1, q2) self.assertAlmostEqual(q1Correct, q1) self.assertAlmostEqual(q2Correct, q2) - self.assertAlmostEqual(Quaterniond.Identity, q1 * q2) + self.assertAlmostEqual(Quaterniond.IDENTITY, q1 * q2) self.assertAlmostEqual(v2, q1 * v1) self.assertAlmostEqual(v1, q2 * v2) @@ -204,13 +204,13 @@ def test_from2axes(self): v1.Set(0.5, 0.5, 0) v2.Set(-0.5, 0.5, 0) - q1.From2Axes(v1, v2) - q2.From2Axes(v2, v1) + q1.from_2axes(v1, v2) + q2.from_2axes(v2, v1) self.assertNotEqual(q1, q2) self.assertAlmostEqual(q1Correct, q1) self.assertAlmostEqual(q2Correct, q2) - self.assertAlmostEqual(Quaterniond.Identity, q1 * q2) + self.assertAlmostEqual(Quaterniond.IDENTITY, q1 * q2) self.assertAlmostEqual(v2, q1 * v1) self.assertAlmostEqual(v1, q2 * v2) @@ -219,58 +219,58 @@ def test_from2axes(self): v1.Set(1, 0, 0) v2.Set(-1, 0, 0) - q1.From2Axes(v1, v2) + q1.from_2axes(v1, v2) q2 = q1 * q1 - self.assertTrue(abs(q2.W()-1.0) <= tolerance or - abs(q2.W()-(-1.0)) <= tolerance) - self.assertAlmostEqual(q2.X(), 0.0) - self.assertAlmostEqual(q2.Y(), 0.0) - self.assertAlmostEqual(q2.Z(), 0.0) + self.assertTrue(abs(q2.w()-1.0) <= tolerance or + abs(q2.w()-(-1.0)) <= tolerance) + self.assertAlmostEqual(q2.x(), 0.0) + self.assertAlmostEqual(q2.y(), 0.0) + self.assertAlmostEqual(q2.z(), 0.0) v1.Set(0, 1, 0) v2.Set(0, -1, 0) - q1.From2Axes(v1, v2) + q1.from_2axes(v1, v2) q2 = q1 * q1 - self.assertTrue(abs(q2.W()-1.0) <= tolerance or - abs(q2.W()-(-1.0)) <= tolerance) - self.assertAlmostEqual(q2.X(), 0.0) - self.assertAlmostEqual(q2.Y(), 0.0) - self.assertAlmostEqual(q2.Z(), 0.0) + self.assertTrue(abs(q2.w()-1.0) <= tolerance or + abs(q2.w()-(-1.0)) <= tolerance) + self.assertAlmostEqual(q2.x(), 0.0) + self.assertAlmostEqual(q2.y(), 0.0) + self.assertAlmostEqual(q2.z(), 0.0) v1.Set(0, 0, 1) v2.Set(0, 0, -1) - q1.From2Axes(v1, v2) + q1.from_2axes(v1, v2) q2 = q1 * q1 - self.assertTrue(abs(q2.W()-1.0) <= tolerance or - abs(q2.W()-(-1.0)) <= tolerance) - self.assertAlmostEqual(q2.X(), 0.0) - self.assertAlmostEqual(q2.Y(), 0.0) - self.assertAlmostEqual(q2.Z(), 0.0) + self.assertTrue(abs(q2.w()-1.0) <= tolerance or + abs(q2.w()-(-1.0)) <= tolerance) + self.assertAlmostEqual(q2.x(), 0.0) + self.assertAlmostEqual(q2.y(), 0.0) + self.assertAlmostEqual(q2.z(), 0.0) v1.Set(0, 1, 1) v2.Set(0, -1, -1) - q1.From2Axes(v1, v2) + q1.from_2axes(v1, v2) q2 = q1 * q1 - self.assertTrue(abs(q2.W()-1.0) <= tolerance or - abs(q2.W()-(-1.0)) <= tolerance) - self.assertAlmostEqual(q2.X(), 0.0) - self.assertAlmostEqual(q2.Y(), 0.0) - self.assertAlmostEqual(q2.Z(), 0.0) + self.assertTrue(abs(q2.w()-1.0) <= tolerance or + abs(q2.w()-(-1.0)) <= tolerance) + self.assertAlmostEqual(q2.x(), 0.0) + self.assertAlmostEqual(q2.y(), 0.0) + self.assertAlmostEqual(q2.z(), 0.0) def test_math(self): q = Quaterniond(math.pi*0.1, math.pi*0.5, math.pi) self.assertTrue(q == Quaterniond(0.110616, -0.698401, 0.110616, 0.698401)) - q.Set(1, 2, 3, 4) + q.set(1, 2, 3, 4) - q.Normalize() + q.normalize() - self.assertAlmostEqual(q.Roll(), 1.4289, delta=1e-3) - self.assertAlmostEqual(q.Pitch(), -0.339837, delta=1e-3) - self.assertAlmostEqual(q.Yaw(), 2.35619, delta=1e-3) + self.assertAlmostEqual(q.roll(), 1.4289, delta=1e-3) + self.assertAlmostEqual(q.pitch(), -0.339837, delta=1e-3) + self.assertAlmostEqual(q.yaw(), 2.35619, delta=1e-3) - q.Scale(0.1) + q.scale(0.1) self.assertTrue(q == Quaterniond(0.990394, 0.051354, 0.0770309, 0.102708)) @@ -296,40 +296,40 @@ def test_math(self): q = q * 5.0 self.assertTrue(q == Quaterniond(7.67918, -1.184, 2.7592, 4.0149)) - self.assertTrue(q.RotateVectorReverse(Vector3d(1, 2, 3)) == + self.assertTrue(q.rotate_vector_reverse(Vector3d(1, 2, 3)) == Vector3d(-0.104115, 0.4975, 3.70697)) - self.assertAlmostEqual(q.Dot(Quaterniond(.4, .2, .1)), 7.67183, + self.assertAlmostEqual(q.dot(Quaterniond(.4, .2, .1)), 7.67183, delta=1e-3) - self.assertTrue(Quaterniond.Squad(1.1, Quaterniond(.1, 0, .2), + self.assertTrue(Quaterniond.squad(1.1, Quaterniond(.1, 0, .2), Quaterniond(0, .3, .4), Quaterniond(.5, .2, 1), Quaterniond(0, 0, 2), True) == Quaterniond(0.346807, -0.0511734, -0.0494723, 0.935232)) - self.assertTrue(Quaterniond.EulerToQuaternion( + self.assertTrue(Quaterniond.euler_to_quaternion( Vector3d(.1, .2, .3)) == Quaterniond(0.983347, 0.0342708, 0.106021, 0.143572)) - q.Round(2) - self.assertAlmostEqual(-1.18, q.X()) - self.assertAlmostEqual(2.76, q.Y()) - self.assertAlmostEqual(4.01, q.Z()) - self.assertAlmostEqual(7.68, q.W()) - - q.X(0.0) - q.Y(0.0) - q.Z(0.0) - q.W(0.0) - q.Normalize() + q.round(2) + self.assertAlmostEqual(-1.18, q.x()) + self.assertAlmostEqual(2.76, q.y()) + self.assertAlmostEqual(4.01, q.z()) + self.assertAlmostEqual(7.68, q.w()) + + q.x(0.0) + q.y(0.0) + q.z(0.0) + q.w(0.0) + q.normalize() self.assertTrue(q == Quaterniond()) - q.Axis(0, 0, 0, 0) + q.axis(0, 0, 0, 0) self.assertTrue(q == Quaterniond()) - self.assertTrue(Quaterniond.EulerToQuaternion(0.1, 0.2, 0.3) == + self.assertTrue(Quaterniond.euler_to_quaternion(0.1, 0.2, 0.3) == Quaterniond(0.983347, 0.0342708, 0.106021, 0.143572)) def test_stream_out(self): @@ -337,84 +337,84 @@ def test_stream_out(self): self.assertEqual(str(q), "0.1 1.2 2.3") def test_integrate(self): - # Integrate by zero, expect no change + # integrate by zero, expect no change q = Quaterniond(0.5, 0.5, 0.5, 0.5) - self.assertAlmostEqual(q, q.Integrate(Vector3d.Zero, 1.0)) - self.assertAlmostEqual(q, q.Integrate(Vector3d.UnitX, 0.0)) - self.assertAlmostEqual(q, q.Integrate(Vector3d.UnitY, 0.0)) - self.assertAlmostEqual(q, q.Integrate(Vector3d.UnitZ, 0.0)) + self.assertAlmostEqual(q, q.integrate(Vector3d.Zero, 1.0)) + self.assertAlmostEqual(q, q.integrate(Vector3d.UnitX, 0.0)) + self.assertAlmostEqual(q, q.integrate(Vector3d.UnitY, 0.0)) + self.assertAlmostEqual(q, q.integrate(Vector3d.UnitZ, 0.0)) - # Integrate along single axes, + # integrate along single axes, # expect linear change in roll, pitch, yaw q = Quaterniond(1, 0, 0, 0) - qRoll = q.Integrate(Vector3d.UnitX, 1.0) - qPitch = q.Integrate(Vector3d.UnitY, 1.0) - qYaw = q.Integrate(Vector3d.UnitZ, 1.0) - self.assertAlmostEqual(qRoll.Euler(), Vector3d.UnitX) - self.assertAlmostEqual(qPitch.Euler(), Vector3d.UnitY) - self.assertAlmostEqual(qYaw.Euler(), Vector3d.UnitZ) - - # Integrate sequentially along single axes in order XYZ, - # expect rotations to match Euler Angles + qRoll = q.integrate(Vector3d.UnitX, 1.0) + qPitch = q.integrate(Vector3d.UnitY, 1.0) + qYaw = q.integrate(Vector3d.UnitZ, 1.0) + self.assertAlmostEqual(qRoll.euler(), Vector3d.UnitX) + self.assertAlmostEqual(qPitch.euler(), Vector3d.UnitY) + self.assertAlmostEqual(qYaw.euler(), Vector3d.UnitZ) + + # integrate sequentially along single axes in order XYZ, + # expect rotations to match euler Angles q = Quaterniond(1, 0, 0, 0) angle = 0.5 - qX = q.Integrate(Vector3d.UnitX, angle) - qXY = qX.Integrate(Vector3d.UnitY, angle) - self.assertAlmostEqual(qXY.Euler(), Vector3d(1, 1, 0)*angle) + qX = q.integrate(Vector3d.UnitX, angle) + qXY = qX.integrate(Vector3d.UnitY, angle) + self.assertAlmostEqual(qXY.euler(), Vector3d(1, 1, 0)*angle) q = Quaterniond(1, 0, 0, 0) angle = 0.5 - qX = q.Integrate(Vector3d.UnitX, angle) - qXZ = qX.Integrate(Vector3d.UnitZ, angle) - self.assertAlmostEqual(qXZ.Euler(), Vector3d(1, 0, 1)*angle) + qX = q.integrate(Vector3d.UnitX, angle) + qXZ = qX.integrate(Vector3d.UnitZ, angle) + self.assertAlmostEqual(qXZ.euler(), Vector3d(1, 0, 1)*angle) q = Quaterniond(1, 0, 0, 0) angle = 0.5 - qY = q.Integrate(Vector3d.UnitY, angle) - qYZ = qY.Integrate(Vector3d.UnitZ, angle) - self.assertAlmostEqual(qYZ.Euler(), Vector3d(0, 1, 1)*angle) + qY = q.integrate(Vector3d.UnitY, angle) + qYZ = qY.integrate(Vector3d.UnitZ, angle) + self.assertAlmostEqual(qYZ.euler(), Vector3d(0, 1, 1)*angle) q = Quaterniond(1, 0, 0, 0) angle = 0.5 - qX = q.Integrate(Vector3d.UnitX, angle) - qXY = qX.Integrate(Vector3d.UnitY, angle) - qXYZ = qXY.Integrate(Vector3d.UnitZ, angle) - self.assertAlmostEqual(qXYZ.Euler(), Vector3d.One*angle) + qX = q.integrate(Vector3d.UnitX, angle) + qXY = qX.integrate(Vector3d.UnitY, angle) + qXYZ = qXY.integrate(Vector3d.UnitZ, angle) + self.assertAlmostEqual(qXYZ.euler(), Vector3d.One*angle) - # Integrate sequentially along single axes in order ZYX, - # expect rotations to not match Euler Angles + # integrate sequentially along single axes in order ZYX, + # expect rotations to not match euler Angles q = Quaterniond(1, 0, 0, 0) angle = 0.5 - qZ = q.Integrate(Vector3d.UnitZ, angle) - qZY = qZ.Integrate(Vector3d.UnitY, angle) - self.assertNotEqual(qZY.Euler(), Vector3d(0, 1, 1)*angle) + qZ = q.integrate(Vector3d.UnitZ, angle) + qZY = qZ.integrate(Vector3d.UnitY, angle) + self.assertNotEqual(qZY.euler(), Vector3d(0, 1, 1)*angle) q = Quaterniond(1, 0, 0, 0) angle = 0.5 - qZ = q.Integrate(Vector3d.UnitZ, angle) - qZX = qZ.Integrate(Vector3d.UnitX, angle) - self.assertNotEqual(qZX.Euler(), Vector3d(1, 0, 1)*angle) + qZ = q.integrate(Vector3d.UnitZ, angle) + qZX = qZ.integrate(Vector3d.UnitX, angle) + self.assertNotEqual(qZX.euler(), Vector3d(1, 0, 1)*angle) q = Quaterniond(1, 0, 0, 0) angle = 0.5 - qZ = q.Integrate(Vector3d.UnitZ, angle) - qZY = qZ.Integrate(Vector3d.UnitY, angle) - qZYX = qZY.Integrate(Vector3d.UnitX, angle) - self.assertNotEqual(qZYX.Euler(), Vector3d(1, 1, 1)*angle) + qZ = q.integrate(Vector3d.UnitZ, angle) + qZY = qZ.integrate(Vector3d.UnitY, angle) + qZYX = qZY.integrate(Vector3d.UnitX, angle) + self.assertNotEqual(qZYX.euler(), Vector3d(1, 1, 1)*angle) q = Quaterniond(1, 0, 0, 0) angle = 0.5 - qY = q.Integrate(Vector3d.UnitY, angle) - qYX = qY.Integrate(Vector3d.UnitX, angle) - self.assertNotEqual(qYX.Euler(), Vector3d(1, 1, 0)*angle) + qY = q.integrate(Vector3d.UnitY, angle) + qYX = qY.integrate(Vector3d.UnitX, angle) + self.assertNotEqual(qYX.euler(), Vector3d(1, 1, 0)*angle) - # Integrate a full rotation about different axes, + # integrate a full rotation about different axes, # expect no change. q = Quaterniond(0.5, 0.5, 0.5, 0.5) fourPi = 4 * math.pi - qX = q.Integrate(Vector3d.UnitX, fourPi) - qY = q.Integrate(Vector3d.UnitY, fourPi) - qZ = q.Integrate(Vector3d.UnitZ, fourPi) + qX = q.integrate(Vector3d.UnitX, fourPi) + qY = q.integrate(Vector3d.UnitY, fourPi) + qZ = q.integrate(Vector3d.UnitZ, fourPi) self.assertAlmostEqual(q, qX) self.assertAlmostEqual(q, qY) self.assertAlmostEqual(q, qZ) From 88dc6f66fd86ca891c58380d3c96dd470a77d198 Mon Sep 17 00:00:00 2001 From: LolaSegura Date: Fri, 20 Aug 2021 16:51:42 -0300 Subject: [PATCH 08/15] Updates the test to follow the naiming style and moves the .i and python test to the python folder. Signed-off-by: LolaSegura --- src/python/CMakeLists.txt | 8 +++ src/{ => python}/Matrix3.i | 1 + src/{ => python}/Matrix3_TEST.py | 53 ++++++++------- src/{ => python}/Matrix4.i | 0 src/{ => python}/Matrix4_TEST.py | 28 ++++---- src/{ => python}/Pose3.i | 0 src/{ => python}/Pose3_TEST.py | 50 +++++++------- src/{ => python}/Quaternion.i | 4 ++ src/{ => python}/Quaternion_TEST.py | 100 ++++++++++++++-------------- src/python/python.i | 7 ++ 10 files changed, 137 insertions(+), 114 deletions(-) rename src/{ => python}/Matrix3.i (98%) rename src/{ => python}/Matrix3_TEST.py (90%) rename src/{ => python}/Matrix4.i (100%) rename src/{ => python}/Matrix4_TEST.py (95%) rename src/{ => python}/Pose3.i (100%) rename src/{ => python}/Pose3_TEST.py (79%) rename src/{ => python}/Quaternion.i (97%) rename src/{ => python}/Quaternion_TEST.py (84%) diff --git a/src/python/CMakeLists.txt b/src/python/CMakeLists.txt index 2d502ff5f..b078e8961 100644 --- a/src/python/CMakeLists.txt +++ b/src/python/CMakeLists.txt @@ -17,6 +17,10 @@ if (SWIG_FOUND) set(swig_files Angle GaussMarkovProcess + Matrix3 + Matrix4 + Pose3 + Quaternion Rand Vector2 Vector3 @@ -70,7 +74,11 @@ if (PYTHONLIBS_FOUND) set(python_tests Angle_TEST GaussMarkovProcess_TEST + Matrix3_TEST + Matrix4_TEST + Pose3_TEST python_TEST + Quaternion_TEST Rand_TEST Vector2_TEST Vector3_TEST diff --git a/src/Matrix3.i b/src/python/Matrix3.i similarity index 98% rename from src/Matrix3.i rename to src/python/Matrix3.i index 6b9ebbdb3..6131f2a3c 100644 --- a/src/Matrix3.i +++ b/src/python/Matrix3.i @@ -53,6 +53,7 @@ namespace ignition const Vector3 &_yAxis, const Vector3 &_zAxis); public: void Axis(const Vector3 &_axis, T _angle); + %rename(from_2_axes) From2Axes; public: void From2Axes(const Vector3 &_v1, const Vector3 &_v2); public: void Col(unsigned int _c, const Vector3 &_v); public: Matrix3 operator-(const Matrix3 &_m) const; diff --git a/src/Matrix3_TEST.py b/src/python/Matrix3_TEST.py similarity index 90% rename from src/Matrix3_TEST.py rename to src/python/Matrix3_TEST.py index abf325ed1..fca6f8c7b 100644 --- a/src/Matrix3_TEST.py +++ b/src/python/Matrix3_TEST.py @@ -119,7 +119,7 @@ def test_vector3_mul(self): self.assertAlmostEqual(Matrix3d.ZERO, matrix * 0) # Vector3.ZERO - self.assertAlmostEqual(Vector3d.Zero, matrix * Vector3d.Zero) + self.assertAlmostEqual(Vector3d.ZERO, matrix * Vector3d.ZERO) # Matrix3.ZERO self.assertAlmostEqual(Matrix3d.ZERO, matrix * Matrix3d.ZERO) @@ -133,11 +133,14 @@ def test_vector3_mul(self): # Vector3.Unit # right multiply self.assertAlmostEqual(Vector3d(matrix(0, 0), matrix(1, 0), - matrix(2, 0)), matrix * Vector3d.UnitX) + matrix(2, 0)), + matrix * Vector3d.UNIT_X) self.assertAlmostEqual(Vector3d(matrix(0, 1), matrix(1, 1), - matrix(2, 1)), matrix * Vector3d.UnitY) + matrix(2, 1)), + matrix * Vector3d.UNIT_Y) self.assertAlmostEqual(Vector3d(matrix(0, 2), matrix(1, 2), - matrix(2, 2)), matrix * Vector3d.UnitZ) + matrix(2, 2)), + matrix * Vector3d.UNIT_Z) # Matrix3.IDENTITY self.assertAlmostEqual(matrix, matrix * Matrix3d.IDENTITY) @@ -231,10 +234,10 @@ def test_from_2axes(self): v2 = Vector3d(0.0, 1.0, 0.0) m1 = Matrix3d() - m1.from_2axes(v1, v2) + m1.from_2_axes(v1, v2) m2 = Matrix3d() - m2.from_2axes(v2, v1) + m2.from_2_axes(v2, v1) m1Correct = Matrix3d(0, -1, 0, 1, 0, 0, @@ -250,18 +253,18 @@ def test_from_2axes(self): self.assertAlmostEqual(v1, m2 * v2) # rotation about 45 degrees - v1.Set(1.0, 0.0, 0.0) - v2.Set(1.0, 1.0, 0.0) - m2.from_2axes(v1, v2) + v1.set(1.0, 0.0, 0.0) + v2.set(1.0, 1.0, 0.0) + m2.from_2_axes(v1, v2) # m1 is 90 degrees rotation self.assertAlmostEqual(m1, m2*m2) # with non-unit vectors - v1.Set(0.5, 0.5, 0) - v2.Set(-0.5, 0.5, 0) + v1.set(0.5, 0.5, 0) + v2.set(-0.5, 0.5, 0) - m1.from_2axes(v1, v2) - m2.from_2axes(v2, v1) + m1.from_2_axes(v1, v2) + m2.from_2_axes(v2, v1) self.assertNotEqual(m1, m2) self.assertAlmostEqual(m1Correct, m1) @@ -271,27 +274,27 @@ def test_from_2axes(self): self.assertAlmostEqual(v1, m2 * v2) # For zero-length vectors, a unit matrix is returned - v1.Set(0, 0, 0) - v2.Set(-0.5, 0.5, 0) - m1.from_2axes(v1, v2) + v1.set(0, 0, 0) + v2.set(-0.5, 0.5, 0) + m1.from_2_axes(v1, v2) self.assertAlmostEqual(Matrix3d.IDENTITY, m1) # For zero-length vectors, a unit matrix is returned - v1.Set(-0.5, 0.5, 0) - v2.Set(0, 0, 0) - m1.from_2axes(v1, v2) + v1.set(-0.5, 0.5, 0) + v2.set(0, 0, 0) + m1.from_2_axes(v1, v2) self.assertAlmostEqual(Matrix3d.IDENTITY, m1) # Parallel vectors - v1.Set(1, 0, 0) - v2.Set(2, 0, 0) - m1.from_2axes(v1, v2) + v1.set(1, 0, 0) + v2.set(2, 0, 0) + m1.from_2_axes(v1, v2) self.assertAlmostEqual(Matrix3d.IDENTITY, m1) # Opposite vectors - v1.Set(1, 0, 0) - v2.Set(-2, 0, 0) - m1.from_2axes(v1, v2) + v1.set(1, 0, 0) + v2.set(-2, 0, 0) + m1.from_2_axes(v1, v2) self.assertAlmostEqual(Matrix3d.ZERO - Matrix3d.IDENTITY, m1) diff --git a/src/Matrix4.i b/src/python/Matrix4.i similarity index 100% rename from src/Matrix4.i rename to src/python/Matrix4.i diff --git a/src/Matrix4_TEST.py b/src/python/Matrix4_TEST.py similarity index 95% rename from src/Matrix4_TEST.py rename to src/python/Matrix4_TEST.py index d9dfb067a..2e8868838 100644 --- a/src/Matrix4_TEST.py +++ b/src/python/Matrix4_TEST.py @@ -447,8 +447,8 @@ def test_transpose(self): self.assertEqual(m, mT) def test_look_at(self): - self.assertAlmostEqual(Matrix4d.look_at(-Vector3d.UnitX, - Vector3d.Zero).pose(), + self.assertAlmostEqual(Matrix4d.look_at(-Vector3d.UNIT_X, + Vector3d.ZERO).pose(), Pose3d(-1, 0, 0, 0, 0, 0)) self.assertAlmostEqual(Matrix4d.look_at(Vector3d(3, 2, 0), @@ -456,7 +456,7 @@ def test_look_at(self): Pose3d(3, 2, 0, 0, 0, math.pi)) self.assertAlmostEqual(Matrix4d.look_at(Vector3d(1, 6, 1), - Vector3d.One).pose(), + Vector3d.ONE).pose(), Pose3d(1, 6, 1, 0, 0, -math.pi/2)) self.assertAlmostEqual(Matrix4d.look_at(Vector3d(-1, -1, 0), @@ -468,33 +468,33 @@ def test_look_at(self): Vector3d(999, -0.6, 0)), Matrix4d.look_at(Vector3d(0.1, -5, 222), Vector3d(999, -0.6, 0), - Vector3d.UnitZ)) + Vector3d.UNIT_Z)) # up == zero, default up = +z self.assertAlmostEqual(Matrix4d.look_at(Vector3d(1.23, 456, 0.7), Vector3d(0, 8.9, -10), - Vector3d.Zero), + Vector3d.ZERO), Matrix4d.look_at(Vector3d(1.23, 456, 0.7), Vector3d(0, 8.9, -10))) # up == +x, default up = +z self.assertAlmostEqual(Matrix4d.look_at(Vector3d(0.25, 9, -5), Vector3d(-6, 0, 0.4), - Vector3d.UnitX), + Vector3d.UNIT_X), Matrix4d.look_at(Vector3d(0.25, 9, -5), Vector3d(-6, 0, 0.4))) # up == -x, default up = +z self.assertAlmostEqual(Matrix4d.look_at(Vector3d(0, 0, 0.2), Vector3d(-8, 0, -6), - -Vector3d.UnitX), + -Vector3d.UNIT_X), Matrix4d.look_at(Vector3d(0, 0, 0.2), Vector3d(-8, 0, -6))) # eye == target, default direction = +x - self.assertAlmostEqual(Matrix4d.look_at(Vector3d.One, - Vector3d.One), - Matrix4d.look_at(Vector3d.One, + self.assertAlmostEqual(Matrix4d.look_at(Vector3d.ONE, + Vector3d.ONE), + Matrix4d.look_at(Vector3d.ONE, Vector3d(1.0001, 1, 1))) # Not possible to keep _up on +z @@ -502,15 +502,15 @@ def test_look_at(self): Vector3d(-1, 0, 0)), Matrix4d.look_at(Vector3d(-1, 0, 10), Vector3d(-1, 0, 0), - -Vector3d.UnitX)) + -Vector3d.UNIT_X)) # Different ups - self.assertAlmostEqual(Matrix4d.look_at(Vector3d.One, + self.assertAlmostEqual(Matrix4d.look_at(Vector3d.ONE, Vector3d(0, 1, 1), - Vector3d.UnitY).pose(), + Vector3d.UNIT_Y).pose(), Pose3d(1, 1, 1, math.pi/2, 0, math.pi)) - self.assertAlmostEqual(Matrix4d.look_at(Vector3d.One, + self.assertAlmostEqual(Matrix4d.look_at(Vector3d.ONE, Vector3d(0, 1, 1), Vector3d(0, 1, 1)).pose(), Pose3d(1, 1, 1, math.pi/4, 0, math.pi)) diff --git a/src/Pose3.i b/src/python/Pose3.i similarity index 100% rename from src/Pose3.i rename to src/python/Pose3.i diff --git a/src/Pose3_TEST.py b/src/python/Pose3_TEST.py similarity index 79% rename from src/Pose3_TEST.py rename to src/python/Pose3_TEST.py index f75a8fc70..be64adbd4 100644 --- a/src/Pose3_TEST.py +++ b/src/python/Pose3_TEST.py @@ -41,31 +41,31 @@ def test_pose(self): # A is the transform from O to P specified in frame O # B is the transform from P to Q specified in frame P # then, B + A is the transform from O to Q specified in frame O - self.assertAlmostEqual((B + A).pos().X(), 1.0 + 1.0/math.sqrt(2)) - self.assertAlmostEqual((B + A).pos().Y(), 1.0/math.sqrt(2)) - self.assertAlmostEqual((B + A).pos().Z(), 0.0) - self.assertAlmostEqual((B + A).rot().euler().X(), 0.0) - self.assertAlmostEqual((B + A).rot().euler().Y(), 0.0) - self.assertAlmostEqual((B + A).rot().euler().Z(), 3.0*math.pi/4.0) + self.assertAlmostEqual((B + A).pos().x(), 1.0 + 1.0/math.sqrt(2)) + self.assertAlmostEqual((B + A).pos().y(), 1.0/math.sqrt(2)) + self.assertAlmostEqual((B + A).pos().z(), 0.0) + self.assertAlmostEqual((B + A).rot().euler().x(), 0.0) + self.assertAlmostEqual((B + A).rot().euler().y(), 0.0) + self.assertAlmostEqual((B + A).rot().euler().z(), 3.0*math.pi/4.0) # If: # A is the transform from O to P in frame O # B is the transform from O to Q in frame O # then -A is transform from P to O specified in frame P - self.assertAlmostEqual((Pose3d() - A).pos().X(), -1.0/math.sqrt(2)) - self.assertAlmostEqual((Pose3d() - A).pos().Y(), 1.0/math.sqrt(2)) - self.assertAlmostEqual((Pose3d() - A).pos().Z(), 0.0) - self.assertAlmostEqual((Pose3d() - A).rot().euler().X(), 0.0) - self.assertAlmostEqual((Pose3d() - A).rot().euler().Y(), 0.0) - self.assertAlmostEqual((Pose3d() - A).rot().euler().Z(), -math.pi/4) + self.assertAlmostEqual((Pose3d() - A).pos().x(), -1.0/math.sqrt(2)) + self.assertAlmostEqual((Pose3d() - A).pos().y(), 1.0/math.sqrt(2)) + self.assertAlmostEqual((Pose3d() - A).pos().z(), 0.0) + self.assertAlmostEqual((Pose3d() - A).rot().euler().x(), 0.0) + self.assertAlmostEqual((Pose3d() - A).rot().euler().y(), 0.0) + self.assertAlmostEqual((Pose3d() - A).rot().euler().z(), -math.pi/4) # test negation operator - self.assertAlmostEqual((-A).pos().X(), -1.0/math.sqrt(2)) - self.assertAlmostEqual((-A).pos().Y(), 1.0/math.sqrt(2)) - self.assertAlmostEqual((-A).pos().Z(), 0.0) - self.assertAlmostEqual((-A).rot().euler().X(), 0.0) - self.assertAlmostEqual((-A).rot().euler().Y(), 0.0) - self.assertAlmostEqual((-A).rot().euler().Z(), -math.pi/4.0) + self.assertAlmostEqual((-A).pos().x(), -1.0/math.sqrt(2)) + self.assertAlmostEqual((-A).pos().y(), 1.0/math.sqrt(2)) + self.assertAlmostEqual((-A).pos().z(), 0.0) + self.assertAlmostEqual((-A).rot().euler().x(), 0.0) + self.assertAlmostEqual((-A).rot().euler().y(), 0.0) + self.assertAlmostEqual((-A).rot().euler().z(), -math.pi/4.0) # If: # A is the transform from O to P in frame O @@ -73,12 +73,12 @@ def test_pose(self): # B - A is the transform from P to Q in frame P B = Pose3d(Vector3d(1, 1, 0), Quaterniond(0, 0, math.pi/2.0)) - self.assertAlmostEqual((B - A).pos().X(), 1.0/math.sqrt(2)) - self.assertAlmostEqual((B - A).pos().Y(), 1.0/math.sqrt(2)) - self.assertAlmostEqual((B - A).pos().Z(), 0.0) - self.assertAlmostEqual((B - A).rot().euler().X(), 0.0) - self.assertAlmostEqual((B - A).rot().euler().Y(), 0.0) - self.assertAlmostEqual((B - A).rot().euler().Z(), math.pi/4.0) + self.assertAlmostEqual((B - A).pos().x(), 1.0/math.sqrt(2)) + self.assertAlmostEqual((B - A).pos().y(), 1.0/math.sqrt(2)) + self.assertAlmostEqual((B - A).pos().z(), 0.0) + self.assertAlmostEqual((B - A).rot().euler().x(), 0.0) + self.assertAlmostEqual((B - A).rot().euler().y(), 0.0) + self.assertAlmostEqual((B - A).rot().euler().z(), math.pi/4.0) pose = Pose3d() self.assertTrue(pose.pos() == Vector3d(0, 0, 0)) @@ -112,7 +112,7 @@ def test_pose(self): pose -= Pose3d(pose) self.assertTrue(pose == Pose3d(0, 0, 0, 0, 0, 0)) - pose.pos().Set(5, 6, 7) + pose.pos().set(5, 6, 7) pose.rot().euler(Vector3d(.4, .6, 0)) self.assertTrue(pose.coord_position_add(Vector3d(1, 2, 3)) == diff --git a/src/Quaternion.i b/src/python/Quaternion.i similarity index 97% rename from src/Quaternion.i rename to src/python/Quaternion.i index ae56c5dd1..b05aed41b 100644 --- a/src/Quaternion.i +++ b/src/python/Quaternion.i @@ -63,6 +63,7 @@ namespace ignition public: T Pitch() const; public: T Yaw() const; public: void ToAxis(Vector3 &_axis, T &_angle) const; + %rename(from_2_axes) From2Axes; public: void From2Axes(const Vector3 &_v1, const Vector3 &_v2); public: void Scale(T _scale); public: Quaternion operator+(const Quaternion &_qt) const; @@ -81,8 +82,11 @@ namespace ignition public: Vector3 RotateVectorReverse(const Vector3 &_vec) const; public: bool IsFinite() const; public: inline void Correct(); + %rename(x_axis) XAxis; public: Vector3 XAxis() const; + %rename(y_axis) YAxis; public: Vector3 YAxis() const; + %rename(z_axis) ZAxis; public: Vector3 ZAxis() const; public: void Round(int _precision); public: T Dot(const Quaternion &_q) const; diff --git a/src/Quaternion_TEST.py b/src/python/Quaternion_TEST.py similarity index 84% rename from src/Quaternion_TEST.py rename to src/python/Quaternion_TEST.py index e8331005a..dc09b9ea8 100644 --- a/src/Quaternion_TEST.py +++ b/src/python/Quaternion_TEST.py @@ -185,10 +185,10 @@ def test_from2axes(self): v2 = Vector3d(0.0, 1.0, 0.0) q1 = Quaterniond() - q1.from_2axes(v1, v2) + q1.from_2_axes(v1, v2) q2 = Quaterniond() - q2.from_2axes(v2, v1) + q2.from_2_axes(v2, v1) q2Correct = Quaterniond(math.sqrt(2)/2, 0, 0, -math.sqrt(2)/2) q1Correct = Quaterniond(math.sqrt(2)/2, 0, 0, math.sqrt(2)/2) @@ -201,11 +201,11 @@ def test_from2axes(self): self.assertAlmostEqual(v1, q2 * v2) # still the same rotation, but with non-unit vectors - v1.Set(0.5, 0.5, 0) - v2.Set(-0.5, 0.5, 0) + v1.set(0.5, 0.5, 0) + v2.set(-0.5, 0.5, 0) - q1.from_2axes(v1, v2) - q2.from_2axes(v2, v1) + q1.from_2_axes(v1, v2) + q2.from_2_axes(v2, v1) self.assertNotEqual(q1, q2) self.assertAlmostEqual(q1Correct, q1) @@ -217,9 +217,9 @@ def test_from2axes(self): # Test various settings of opposite vectors (which need special care) tolerance = 1e-4 - v1.Set(1, 0, 0) - v2.Set(-1, 0, 0) - q1.from_2axes(v1, v2) + v1.set(1, 0, 0) + v2.set(-1, 0, 0) + q1.from_2_axes(v1, v2) q2 = q1 * q1 self.assertTrue(abs(q2.w()-1.0) <= tolerance or abs(q2.w()-(-1.0)) <= tolerance) @@ -227,9 +227,9 @@ def test_from2axes(self): self.assertAlmostEqual(q2.y(), 0.0) self.assertAlmostEqual(q2.z(), 0.0) - v1.Set(0, 1, 0) - v2.Set(0, -1, 0) - q1.from_2axes(v1, v2) + v1.set(0, 1, 0) + v2.set(0, -1, 0) + q1.from_2_axes(v1, v2) q2 = q1 * q1 self.assertTrue(abs(q2.w()-1.0) <= tolerance or abs(q2.w()-(-1.0)) <= tolerance) @@ -237,9 +237,9 @@ def test_from2axes(self): self.assertAlmostEqual(q2.y(), 0.0) self.assertAlmostEqual(q2.z(), 0.0) - v1.Set(0, 0, 1) - v2.Set(0, 0, -1) - q1.from_2axes(v1, v2) + v1.set(0, 0, 1) + v2.set(0, 0, -1) + q1.from_2_axes(v1, v2) q2 = q1 * q1 self.assertTrue(abs(q2.w()-1.0) <= tolerance or abs(q2.w()-(-1.0)) <= tolerance) @@ -247,9 +247,9 @@ def test_from2axes(self): self.assertAlmostEqual(q2.y(), 0.0) self.assertAlmostEqual(q2.z(), 0.0) - v1.Set(0, 1, 1) - v2.Set(0, -1, -1) - q1.from_2axes(v1, v2) + v1.set(0, 1, 1) + v2.set(0, -1, -1) + q1.from_2_axes(v1, v2) q2 = q1 * q1 self.assertTrue(abs(q2.w()-1.0) <= tolerance or abs(q2.w()-(-1.0)) <= tolerance) @@ -339,82 +339,82 @@ def test_stream_out(self): def test_integrate(self): # integrate by zero, expect no change q = Quaterniond(0.5, 0.5, 0.5, 0.5) - self.assertAlmostEqual(q, q.integrate(Vector3d.Zero, 1.0)) - self.assertAlmostEqual(q, q.integrate(Vector3d.UnitX, 0.0)) - self.assertAlmostEqual(q, q.integrate(Vector3d.UnitY, 0.0)) - self.assertAlmostEqual(q, q.integrate(Vector3d.UnitZ, 0.0)) + self.assertAlmostEqual(q, q.integrate(Vector3d.ZERO, 1.0)) + self.assertAlmostEqual(q, q.integrate(Vector3d.UNIT_X, 0.0)) + self.assertAlmostEqual(q, q.integrate(Vector3d.UNIT_Y, 0.0)) + self.assertAlmostEqual(q, q.integrate(Vector3d.UNIT_Z, 0.0)) # integrate along single axes, # expect linear change in roll, pitch, yaw q = Quaterniond(1, 0, 0, 0) - qRoll = q.integrate(Vector3d.UnitX, 1.0) - qPitch = q.integrate(Vector3d.UnitY, 1.0) - qYaw = q.integrate(Vector3d.UnitZ, 1.0) - self.assertAlmostEqual(qRoll.euler(), Vector3d.UnitX) - self.assertAlmostEqual(qPitch.euler(), Vector3d.UnitY) - self.assertAlmostEqual(qYaw.euler(), Vector3d.UnitZ) + qRoll = q.integrate(Vector3d.UNIT_X, 1.0) + qPitch = q.integrate(Vector3d.UNIT_Y, 1.0) + qYaw = q.integrate(Vector3d.UNIT_Z, 1.0) + self.assertAlmostEqual(qRoll.euler(), Vector3d.UNIT_X) + self.assertAlmostEqual(qPitch.euler(), Vector3d.UNIT_Y) + self.assertAlmostEqual(qYaw.euler(), Vector3d.UNIT_Z) # integrate sequentially along single axes in order XYZ, # expect rotations to match euler Angles q = Quaterniond(1, 0, 0, 0) angle = 0.5 - qX = q.integrate(Vector3d.UnitX, angle) - qXY = qX.integrate(Vector3d.UnitY, angle) + qX = q.integrate(Vector3d.UNIT_X, angle) + qXY = qX.integrate(Vector3d.UNIT_Y, angle) self.assertAlmostEqual(qXY.euler(), Vector3d(1, 1, 0)*angle) q = Quaterniond(1, 0, 0, 0) angle = 0.5 - qX = q.integrate(Vector3d.UnitX, angle) - qXZ = qX.integrate(Vector3d.UnitZ, angle) + qX = q.integrate(Vector3d.UNIT_X, angle) + qXZ = qX.integrate(Vector3d.UNIT_Z, angle) self.assertAlmostEqual(qXZ.euler(), Vector3d(1, 0, 1)*angle) q = Quaterniond(1, 0, 0, 0) angle = 0.5 - qY = q.integrate(Vector3d.UnitY, angle) - qYZ = qY.integrate(Vector3d.UnitZ, angle) + qY = q.integrate(Vector3d.UNIT_Y, angle) + qYZ = qY.integrate(Vector3d.UNIT_Z, angle) self.assertAlmostEqual(qYZ.euler(), Vector3d(0, 1, 1)*angle) q = Quaterniond(1, 0, 0, 0) angle = 0.5 - qX = q.integrate(Vector3d.UnitX, angle) - qXY = qX.integrate(Vector3d.UnitY, angle) - qXYZ = qXY.integrate(Vector3d.UnitZ, angle) - self.assertAlmostEqual(qXYZ.euler(), Vector3d.One*angle) + qX = q.integrate(Vector3d.UNIT_X, angle) + qXY = qX.integrate(Vector3d.UNIT_Y, angle) + qXYZ = qXY.integrate(Vector3d.UNIT_Z, angle) + self.assertAlmostEqual(qXYZ.euler(), Vector3d.ONE*angle) # integrate sequentially along single axes in order ZYX, # expect rotations to not match euler Angles q = Quaterniond(1, 0, 0, 0) angle = 0.5 - qZ = q.integrate(Vector3d.UnitZ, angle) - qZY = qZ.integrate(Vector3d.UnitY, angle) + qZ = q.integrate(Vector3d.UNIT_Z, angle) + qZY = qZ.integrate(Vector3d.UNIT_Y, angle) self.assertNotEqual(qZY.euler(), Vector3d(0, 1, 1)*angle) q = Quaterniond(1, 0, 0, 0) angle = 0.5 - qZ = q.integrate(Vector3d.UnitZ, angle) - qZX = qZ.integrate(Vector3d.UnitX, angle) + qZ = q.integrate(Vector3d.UNIT_Z, angle) + qZX = qZ.integrate(Vector3d.UNIT_X, angle) self.assertNotEqual(qZX.euler(), Vector3d(1, 0, 1)*angle) q = Quaterniond(1, 0, 0, 0) angle = 0.5 - qZ = q.integrate(Vector3d.UnitZ, angle) - qZY = qZ.integrate(Vector3d.UnitY, angle) - qZYX = qZY.integrate(Vector3d.UnitX, angle) + qZ = q.integrate(Vector3d.UNIT_Z, angle) + qZY = qZ.integrate(Vector3d.UNIT_Y, angle) + qZYX = qZY.integrate(Vector3d.UNIT_X, angle) self.assertNotEqual(qZYX.euler(), Vector3d(1, 1, 1)*angle) q = Quaterniond(1, 0, 0, 0) angle = 0.5 - qY = q.integrate(Vector3d.UnitY, angle) - qYX = qY.integrate(Vector3d.UnitX, angle) + qY = q.integrate(Vector3d.UNIT_Y, angle) + qYX = qY.integrate(Vector3d.UNIT_X, angle) self.assertNotEqual(qYX.euler(), Vector3d(1, 1, 0)*angle) # integrate a full rotation about different axes, # expect no change. q = Quaterniond(0.5, 0.5, 0.5, 0.5) fourPi = 4 * math.pi - qX = q.integrate(Vector3d.UnitX, fourPi) - qY = q.integrate(Vector3d.UnitY, fourPi) - qZ = q.integrate(Vector3d.UnitZ, fourPi) + qX = q.integrate(Vector3d.UNIT_X, fourPi) + qY = q.integrate(Vector3d.UNIT_Y, fourPi) + qZ = q.integrate(Vector3d.UNIT_Z, fourPi) self.assertAlmostEqual(q, qX) self.assertAlmostEqual(q, qY) self.assertAlmostEqual(q, qZ) diff --git a/src/python/python.i b/src/python/python.i index 614c220bd..5f238dee5 100644 --- a/src/python/python.i +++ b/src/python/python.i @@ -5,3 +5,10 @@ %include Vector2.i %include Vector3.i %include Vector4.i +%include Quaternion.i +%include Pose3.i +%include Matrix3.i +%include Matrix4.i + + + From f76a40a4c5022afe66ecb5c6e315e78c181cccd8 Mon Sep 17 00:00:00 2001 From: LolaSegura Date: Mon, 23 Aug 2021 11:13:46 -0300 Subject: [PATCH 09/15] Address comments. Signed-off-by: LolaSegura --- src/python/Matrix3.i | 9 ++++----- src/python/Matrix3_TEST.py | 2 +- src/python/Matrix4.i | 8 ++++---- src/python/Matrix4_TEST.py | 2 +- src/python/Pose3.i | 6 ++---- src/python/Pose3_TEST.py | 4 ++-- src/python/Quaternion.i | 5 ++--- src/python/Quaternion_TEST.py | 2 +- src/python/python.i | 3 --- 9 files changed, 17 insertions(+), 24 deletions(-) diff --git a/src/python/Matrix3.i b/src/python/Matrix3.i index 6131f2a3c..730c7644d 100644 --- a/src/python/Matrix3.i +++ b/src/python/Matrix3.i @@ -17,11 +17,11 @@ %module matrix3 %{ -#include +#include #include -#include +#include #include -#include +#include %} %include "std_string.i" @@ -30,7 +30,6 @@ namespace ignition { namespace math { - template class Quaternion; template class Matrix3 { @@ -88,4 +87,4 @@ namespace ignition %template(Matrix3d) Matrix3; %template(Matrix3f) Matrix3; } -} \ No newline at end of file +} diff --git a/src/python/Matrix3_TEST.py b/src/python/Matrix3_TEST.py index fca6f8c7b..b92fedc54 100644 --- a/src/python/Matrix3_TEST.py +++ b/src/python/Matrix3_TEST.py @@ -12,8 +12,8 @@ # See the License for the specific language governing permissions and # limitations under the License. -import unittest import math +import unittest from ignition.math import Matrix3d from ignition.math import Vector3d diff --git a/src/python/Matrix4.i b/src/python/Matrix4.i index a910431a5..35f93b2e6 100644 --- a/src/python/Matrix4.i +++ b/src/python/Matrix4.i @@ -15,14 +15,14 @@ * */ -%module matrix3 +%module matrix4 %{ -#include +#include #include #include -#include +#include #include -#include +#include %} %include "std_string.i" diff --git a/src/python/Matrix4_TEST.py b/src/python/Matrix4_TEST.py index 2e8868838..44fe82dca 100644 --- a/src/python/Matrix4_TEST.py +++ b/src/python/Matrix4_TEST.py @@ -12,8 +12,8 @@ # See the License for the specific language governing permissions and # limitations under the License. -import unittest import math +import unittest from ignition.math import Matrix4d from ignition.math import Pose3d from ignition.math import Quaterniond diff --git a/src/python/Pose3.i b/src/python/Pose3.i index 3937fd999..5b94579ba 100644 --- a/src/python/Pose3.i +++ b/src/python/Pose3.i @@ -17,10 +17,10 @@ %module quaternion %{ + #include #include #include #include - #include %} %include "std_string.i" @@ -74,7 +74,6 @@ namespace ignition public: void Reset(); public: Pose3 RotatePositionAboutOrigin(const Quaternion &_q) const; public: void Round(int _precision); - public: inline const Vector3 &Pos() const; public: inline Vector3 &Pos(); public: inline const T X() const; public: inline void SetX(T x); @@ -82,7 +81,6 @@ namespace ignition public: inline void SetY(T y); public: inline const T Z() const; public: inline void SetZ(T z); - public: inline const Quaternion &Rot() const; public: inline Quaternion &Rot(); public: inline const T Roll() const; public: inline const T Pitch() const; @@ -101,4 +99,4 @@ namespace ignition %template(Pose3d) Pose3; %template(Pose3f) Pose3; } -} \ No newline at end of file +} diff --git a/src/python/Pose3_TEST.py b/src/python/Pose3_TEST.py index be64adbd4..7a8451b69 100644 --- a/src/python/Pose3_TEST.py +++ b/src/python/Pose3_TEST.py @@ -12,8 +12,8 @@ # See the License for the specific language governing permissions and # limitations under the License. -import unittest import math +import unittest from ignition.math import Pose3d from ignition.math import Quaterniond from ignition.math import Vector3d @@ -180,4 +180,4 @@ def test_set_pose_element(self): if __name__ == '__main__': - unittest.main() \ No newline at end of file + unittest.main() diff --git a/src/python/Quaternion.i b/src/python/Quaternion.i index b05aed41b..92285bad3 100644 --- a/src/python/Quaternion.i +++ b/src/python/Quaternion.i @@ -17,10 +17,10 @@ %module quaternion %{ -#include #include -#include #include +#include +#include %} %include "std_string.i" @@ -141,4 +141,3 @@ namespace ignition %template(Quaternionf) Quaternion; } } - diff --git a/src/python/Quaternion_TEST.py b/src/python/Quaternion_TEST.py index dc09b9ea8..5eb47ebb5 100644 --- a/src/python/Quaternion_TEST.py +++ b/src/python/Quaternion_TEST.py @@ -12,8 +12,8 @@ # See the License for the specific language governing permissions and # limitations under the License. -import unittest import math +import unittest from ignition.math import Quaterniond from ignition.math import Quaternionf from ignition.math import Quaternioni diff --git a/src/python/python.i b/src/python/python.i index 5f238dee5..a612d52e1 100644 --- a/src/python/python.i +++ b/src/python/python.i @@ -9,6 +9,3 @@ %include Pose3.i %include Matrix3.i %include Matrix4.i - - - From e46c44b6767b53d3288ab4d9d4e9f87d41580037 Mon Sep 17 00:00:00 2001 From: LolaSegura Date: Wed, 25 Aug 2021 12:23:16 -0300 Subject: [PATCH 10/15] Adds a python method to convert from a Matrix3 to a Quaternion. Signed-off-by: LolaSegura --- src/python/Matrix3.i | 11 ++ src/python/Quaternion.i | 4 - src/python/Quaternion_TEST.py | 295 ++++++++++++++++++++++++---------- 3 files changed, 223 insertions(+), 87 deletions(-) diff --git a/src/python/Matrix3.i b/src/python/Matrix3.i index 730c7644d..b17e848b9 100644 --- a/src/python/Matrix3.i +++ b/src/python/Matrix3.i @@ -25,6 +25,7 @@ %} %include "std_string.i" +%include Quaternion.i namespace ignition { @@ -86,5 +87,15 @@ namespace ignition %template(Matrix3i) Matrix3; %template(Matrix3d) Matrix3; %template(Matrix3f) Matrix3; + + %extend Quaternion { + void matrix(const Matrix3 &_mat) { + (*$self).Matrix(_mat); + } + } + + %template(Quaternioni) Quaternion; + %template(Quaterniond) Quaternion; + %template(Quaternionf) Quaternion; } } diff --git a/src/python/Quaternion.i b/src/python/Quaternion.i index 92285bad3..130932b17 100644 --- a/src/python/Quaternion.i +++ b/src/python/Quaternion.i @@ -135,9 +135,5 @@ namespace ignition return out.str(); } } - - %template(Quaternioni) Quaternion; - %template(Quaterniond) Quaternion; - %template(Quaternionf) Quaternion; } } diff --git a/src/python/Quaternion_TEST.py b/src/python/Quaternion_TEST.py index 5eb47ebb5..1add01e6c 100644 --- a/src/python/Quaternion_TEST.py +++ b/src/python/Quaternion_TEST.py @@ -1,10 +1,10 @@ # Copyright (C) 2021 Open Source Robotics Foundation # -# Licensed under the Apache License, Version 2.0 (the "License"); +# 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 +# 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, @@ -14,6 +14,8 @@ import math import unittest +from ignition.math import Matrix3d +from ignition.math import Matrix4d from ignition.math import Quaterniond from ignition.math import Quaternionf from ignition.math import Quaternioni @@ -170,7 +172,213 @@ def test_normalize(self): q.normalize() self.assertAlmostEqual(q, Quaterniond(0.182574, 0.365148, - 0.547723, 0.730297)) + 0.547723, 0.730297)) + + def test_math(self): + q = Quaterniond(math.pi*0.1, math.pi*0.5, math.pi) + self.assertTrue(q == Quaterniond(0.110616, -0.698401, + 0.110616, 0.698401)) + + q.set(1, 2, 3, 4) + + q.normalize() + + self.assertAlmostEqual(q.roll(), 1.4289, delta=1e-3) + self.assertAlmostEqual(q.pitch(), -0.339837, delta=1e-3) + self.assertAlmostEqual(q.yaw(), 2.35619, delta=1e-3) + + q.scale(0.1) + self.assertTrue(q == Quaterniond(0.990394, 0.051354, + 0.0770309, 0.102708)) + + q = q + Quaterniond(0, 1, 2) + self.assertTrue(q == Quaterniond(1.46455, -0.352069, + 0.336066, 0.841168)) + + q += q + self.assertTrue(q == Quaterniond(2.92911, -0.704137, + 0.672131, 1.68234)) + + q -= Quaterniond(.4, .2, .1) + self.assertTrue(q == Quaterniond(1.95416, -0.896677, 0.56453, 1.65341)) + + q = q - Quaterniond(0, 1, 2) + self.assertTrue(q == Quaterniond(1.48, -0.493254, + 0.305496, 0.914947)) + + q *= Quaterniond(.4, .1, .01) + self.assertTrue(q == Quaterniond(1.53584, -0.236801, + 0.551841, 0.802979)) + + q = q * 5.0 + self.assertTrue(q == Quaterniond(7.67918, -1.184, 2.7592, 4.0149)) + + self.assertTrue(q.rotate_vector_reverse(Vector3d(1, 2, 3)) == + Vector3d(-0.104115, 0.4975, 3.70697)) + + self.assertAlmostEqual(q.dot(Quaterniond(.4, .2, .1)), 7.67183, + delta=1e-3) + + self.assertTrue(Quaterniond.squad(1.1, Quaterniond(.1, 0, .2), + Quaterniond(0, .3, .4), Quaterniond(.5, .2, 1), + Quaterniond(0, 0, 2), True) == + Quaterniond(0.346807, -0.0511734, + -0.0494723, 0.935232)) + + self.assertTrue(Quaterniond.euler_to_quaternion( + Vector3d(.1, .2, .3)) == + Quaterniond(0.983347, 0.0342708, + 0.106021, 0.143572)) + + q.round(2) + self.assertAlmostEqual(-1.18, q.x()) + self.assertAlmostEqual(2.76, q.y()) + self.assertAlmostEqual(4.01, q.z()) + self.assertAlmostEqual(7.68, q.w()) + + q.x(0.0) + q.y(0.0) + q.z(0.0) + q.w(0.0) + q.normalize() + self.assertTrue(q == Quaterniond()) + + q.axis(0, 0, 0, 0) + self.assertTrue(q == Quaterniond()) + + self.assertTrue(Quaterniond.euler_to_quaternion(0.1, 0.2, 0.3) == + Quaterniond(0.983347, 0.0342708, 0.106021, 0.143572)) + + # simple 180 rotation about yaw, + # should result in x and y flipping signs + q = Quaterniond(0, 0, math.pi) + v = Vector3d(1, 2, 3) + r1 = q.rotate_vector(v) + r2 = q.rotate_vector_reverse(v) + self.assertTrue(r1 == Vector3d(-1, -2, 3)) + self.assertTrue(r2 == Vector3d(-1, -2, 3)) + + # simple 90 rotation about yaw, should map x to y, y to -x + # simple -90 rotation about yaw, should map x to -y, y to x + q = Quaterniond(0, 0, 0.5 * math.pi) + v = Vector3d(1, 2, 3) + r1 = q.rotate_vector(v) + r2 = q.rotate_vector_reverse(v) + self.assertTrue(r1 == Vector3d(-2, 1, 3)) + self.assertTrue(r2 == Vector3d(2, -1, 3)) + self.assertTrue(q.inverse().x_axis() == Vector3d(0, -1, 0)) + self.assertTrue(q.inverse().y_axis() == Vector3d(1, 0, 0)) + self.assertTrue(q.inverse().z_axis() == Vector3d(0, 0, 1)) + + # Test RPY fixed-body-frame convention: + # Rotate each unit vector in roll and pitch + q = Quaterniond(math.pi/2.0, math.pi/2.0, 0) + v1 = Vector3d(1, 0, 0) + r1 = q.rotate_vector(v1) + # 90 degrees about x does nothing, + # 90 degrees about y sends point down to -z + self.assertAlmostEqual(r1, Vector3d(0, 0, -1)) + + v2 = Vector3d(0, 1, 0) + r2 = q.rotate_vector(v2) + # 90 degrees about x sends point to +z + # 90 degrees about y sends point to +x + self.assertAlmostEqual(r2, Vector3d(1, 0, 0)) + + v3 = Vector3d(0, 0, 1) + r3 = q.rotate_vector(v3) + # 90 degrees about x sends point to -y + # 90 degrees about y does nothing + self.assertAlmostEqual(r3, Vector3d(0, -1, 0)) + + # now try a harder case (axis[1,2,3], rotation[0.3*pi]) + # verified with octave + q.axis(Vector3d(1, 2, 3), 0.3*math.pi) + self.assertTrue(q.inverse().x_axis() == + Vector3d(0.617229, -0.589769, 0.520770)) + self.assertTrue(q.inverse().y_axis() == + Vector3d(0.707544, 0.705561, -0.039555)) + self.assertTrue(q.inverse().z_axis() == + Vector3d(-0.344106, 0.392882, 0.852780)) + + # rotate about the axis of rotation should not change axis + v = Vector3d(1, 2, 3) + r1 = q.rotate_vector(v) + r2 = q.rotate_vector_reverse(v) + self.assertTrue(r1 == Vector3d(1, 2, 3)) + self.assertTrue(r2 == Vector3d(1, 2, 3)) + + # rotate unit vectors + v = Vector3d(0, 0, 1) + r1 = q.rotate_vector(v) + r2 = q.rotate_vector_reverse(v) + self.assertTrue(r1 == Vector3d(0.520770, -0.039555, 0.852780)) + self.assertTrue(r2 == Vector3d(-0.34411, 0.39288, 0.85278)) + v = Vector3d(0, 1, 0) + r1 = q.rotate_vector(v) + r2 = q.rotate_vector_reverse(v) + self.assertTrue(r1 == Vector3d(-0.58977, 0.70556, 0.39288)) + self.assertTrue(r2 == Vector3d(0.707544, 0.705561, -0.039555)) + v = Vector3d(1, 0, 0) + r1 = q.rotate_vector(v) + r2 = q.rotate_vector_reverse(v) + self.assertTrue(r1 == Vector3d(0.61723, 0.70754, -0.34411)) + self.assertTrue(r2 == Vector3d(0.61723, -0.58977, 0.52077)) + + self.assertTrue(-q == Quaterniond(-0.891007, -0.121334, + -0.242668, -0.364002)) + + self.assertTrue(Matrix3d(q) == Matrix3d( + 0.617229, -0.589769, 0.52077, + 0.707544, 0.705561, -0.0395554, + -0.344106, 0.392882, 0.85278)) + + matFromQ = Matrix3d(q) + self.assertTrue(matFromQ == Matrix3d( + 0.617229, -0.589769, 0.52077, + 0.707544, 0.705561, -0.0395554, + -0.344106, 0.392882, 0.85278)) + + self.assertTrue(Matrix4d(q) == Matrix4d( + 0.617229, -0.589769, 0.52077, 0, + 0.707544, 0.705561, -0.0395554, 0, + -0.344106, 0.392882, 0.85278, 0, + 0, 0, 0, 1)) + + matFromQuat = Matrix3d(q) + quatFromMat = Quaterniond() + quatFromMat.matrix(matFromQuat) + self.assertTrue(q == quatFromMat) + + # test the cases where matrix trace is negative + # (requires special handling) + q = Quaterniond(0, 0, 0, 1) + q2 = Quaterniond() + q2.matrix(Matrix3d( + -1, 0, 0, + 0, -1, 0, + 0, 0, 1)) + self.assertTrue(q == q2) + + q = Quaterniond(0, 0, 1, 0) + q2 = Quaterniond() + q2.matrix(Matrix3d( + -1, 0, 0, + 0, 1, 0, + 0, 0, -1)) + self.assertTrue(q == q2) + + q = Quaterniond(0, 1, 0, 0) + q2 = Quaterniond() + q2.matrix(Matrix3d( + 1, 0, 0, + 0, -1, 0, + 0, 0, -1)) + self.assertTrue(q == q2) + + def test_stream_out(self): + q = Quaterniond(0.1, 1.2, 2.3) + self.assertEqual(str(q), "0.1 1.2 2.3") def test_slerp(self): q1 = Quaterniond(0.1, 1.2, 2.3) @@ -180,7 +388,7 @@ def test_slerp(self): self.assertAlmostEqual(q3, Quaterniond(0.554528, -0.717339, 0.32579, 0.267925)) - def test_from2axes(self): + def test_from_2_axes(self): v1 = Vector3d(1.0, 0.0, 0.0) v2 = Vector3d(0.0, 1.0, 0.0) @@ -257,85 +465,6 @@ def test_from2axes(self): self.assertAlmostEqual(q2.y(), 0.0) self.assertAlmostEqual(q2.z(), 0.0) - def test_math(self): - q = Quaterniond(math.pi*0.1, math.pi*0.5, math.pi) - self.assertTrue(q == Quaterniond(0.110616, -0.698401, - 0.110616, 0.698401)) - - q.set(1, 2, 3, 4) - - q.normalize() - - self.assertAlmostEqual(q.roll(), 1.4289, delta=1e-3) - self.assertAlmostEqual(q.pitch(), -0.339837, delta=1e-3) - self.assertAlmostEqual(q.yaw(), 2.35619, delta=1e-3) - - q.scale(0.1) - self.assertTrue(q == Quaterniond(0.990394, 0.051354, - 0.0770309, 0.102708)) - - q = q + Quaterniond(0, 1, 2) - self.assertTrue(q == Quaterniond(1.46455, -0.352069, - 0.336066, 0.841168)) - - q += q - self.assertTrue(q == Quaterniond(2.92911, -0.704137, - 0.672131, 1.68234)) - - q -= Quaterniond(.4, .2, .1) - self.assertTrue(q == Quaterniond(1.95416, -0.896677, 0.56453, 1.65341)) - - q = q - Quaterniond(0, 1, 2) - self.assertTrue(q == Quaterniond(1.48, -0.493254, - 0.305496, 0.914947)) - - q *= Quaterniond(.4, .1, .01) - self.assertTrue(q == Quaterniond(1.53584, -0.236801, - 0.551841, 0.802979)) - - q = q * 5.0 - self.assertTrue(q == Quaterniond(7.67918, -1.184, 2.7592, 4.0149)) - - self.assertTrue(q.rotate_vector_reverse(Vector3d(1, 2, 3)) == - Vector3d(-0.104115, 0.4975, 3.70697)) - - self.assertAlmostEqual(q.dot(Quaterniond(.4, .2, .1)), 7.67183, - delta=1e-3) - - self.assertTrue(Quaterniond.squad(1.1, Quaterniond(.1, 0, .2), - Quaterniond(0, .3, .4), Quaterniond(.5, .2, 1), - Quaterniond(0, 0, 2), True) == - Quaterniond(0.346807, -0.0511734, - -0.0494723, 0.935232)) - - self.assertTrue(Quaterniond.euler_to_quaternion( - Vector3d(.1, .2, .3)) == - Quaterniond(0.983347, 0.0342708, - 0.106021, 0.143572)) - - q.round(2) - self.assertAlmostEqual(-1.18, q.x()) - self.assertAlmostEqual(2.76, q.y()) - self.assertAlmostEqual(4.01, q.z()) - self.assertAlmostEqual(7.68, q.w()) - - q.x(0.0) - q.y(0.0) - q.z(0.0) - q.w(0.0) - q.normalize() - self.assertTrue(q == Quaterniond()) - - q.axis(0, 0, 0, 0) - self.assertTrue(q == Quaterniond()) - - self.assertTrue(Quaterniond.euler_to_quaternion(0.1, 0.2, 0.3) == - Quaterniond(0.983347, 0.0342708, 0.106021, 0.143572)) - - def test_stream_out(self): - q = Quaterniond(0.1, 1.2, 2.3) - self.assertEqual(str(q), "0.1 1.2 2.3") - def test_integrate(self): # integrate by zero, expect no change q = Quaterniond(0.5, 0.5, 0.5, 0.5) From d885997b8bab75b8e76888294e37634f30e86d57 Mon Sep 17 00:00:00 2001 From: LolaSegura Date: Wed, 25 Aug 2021 16:19:28 -0300 Subject: [PATCH 11/15] Adds to_quaternion() method to Matrix3. Signed-off-by: LolaSegura --- src/python/Matrix3.i | 18 +++++++----------- src/python/Matrix3_TEST.py | 30 ++++++++++++++++++++++++++++++ src/python/Quaternion.i | 6 +++++- src/python/Quaternion_TEST.py | 31 ------------------------------- 4 files changed, 42 insertions(+), 43 deletions(-) diff --git a/src/python/Matrix3.i b/src/python/Matrix3.i index b17e848b9..5aa049134 100644 --- a/src/python/Matrix3.i +++ b/src/python/Matrix3.i @@ -36,7 +36,7 @@ namespace ignition { %rename("%(undercase)s", %$isfunction, %$ismember, %$not %$isconstructor) ""; %rename("%(uppercase)s", %$isstatic, %$isvariable) ""; - + public: static const Matrix3 Identity; public: static const Matrix3 Zero; public: Matrix3(); @@ -84,18 +84,14 @@ namespace ignition } } + %extend Matrix3 { + ignition::math::Quaternion to_quaternion() { + return ignition::math::Quaternion(*$self); + } + } + %template(Matrix3i) Matrix3; %template(Matrix3d) Matrix3; %template(Matrix3f) Matrix3; - - %extend Quaternion { - void matrix(const Matrix3 &_mat) { - (*$self).Matrix(_mat); - } - } - - %template(Quaternioni) Quaternion; - %template(Quaterniond) Quaternion; - %template(Quaternionf) Quaternion; } } diff --git a/src/python/Matrix3_TEST.py b/src/python/Matrix3_TEST.py index b92fedc54..7df751b03 100644 --- a/src/python/Matrix3_TEST.py +++ b/src/python/Matrix3_TEST.py @@ -15,6 +15,7 @@ import math import unittest from ignition.math import Matrix3d +from ignition.math import Quaterniond from ignition.math import Vector3d @@ -297,6 +298,35 @@ def test_from_2axes(self): m1.from_2_axes(v1, v2) self.assertAlmostEqual(Matrix3d.ZERO - Matrix3d.IDENTITY, m1) + def test_to_quaternion(self): + q = Quaterniond(math.pi/2.0, math.pi/2.0, 0) + matFromQuat = Matrix3d(q) + quatFromMat = matFromQuat.to_quaternion() + self.assertTrue(q == quatFromMat) + + # test the cases where matrix trace is negative + # (requires special handling) + q = Quaterniond(0, 0, 0, 1) + mat = Matrix3d(-1, 0, 0, + 0, -1, 0, + 0, 0, 1) + q2 = mat.to_quaternion() + self.assertTrue(q == q2) + + q = Quaterniond(0, 0, 1, 0) + mat = Matrix3d(-1, 0, 0, + 0, 1, 0, + 0, 0, -1) + q2 = mat.to_quaternion() + self.assertTrue(q == q2) + + q = Quaterniond(0, 1, 0, 0) + mat = Matrix3d(1, 0, 0, + 0, -1, 0, + 0, 0, -1) + q2 = mat.to_quaternion() + self.assertTrue(q == q2) + if __name__ == '__main__': unittest.main() diff --git a/src/python/Quaternion.i b/src/python/Quaternion.i index 130932b17..21859443c 100644 --- a/src/python/Quaternion.i +++ b/src/python/Quaternion.i @@ -99,7 +99,7 @@ namespace ignition bool _shortestPath = false); public: Quaternion Integrate(const Vector3 &_angularVelocity, const T _deltaT) const; - + public: inline void X(T _v); public: inline void Y(T _v); public: inline void Z(T _v); @@ -135,5 +135,9 @@ namespace ignition return out.str(); } } + + %template(Quaternioni) Quaternion; + %template(Quaterniond) Quaternion; + %template(Quaternionf) Quaternion; } } diff --git a/src/python/Quaternion_TEST.py b/src/python/Quaternion_TEST.py index 1add01e6c..3e9bd0aeb 100644 --- a/src/python/Quaternion_TEST.py +++ b/src/python/Quaternion_TEST.py @@ -345,37 +345,6 @@ def test_math(self): -0.344106, 0.392882, 0.85278, 0, 0, 0, 0, 1)) - matFromQuat = Matrix3d(q) - quatFromMat = Quaterniond() - quatFromMat.matrix(matFromQuat) - self.assertTrue(q == quatFromMat) - - # test the cases where matrix trace is negative - # (requires special handling) - q = Quaterniond(0, 0, 0, 1) - q2 = Quaterniond() - q2.matrix(Matrix3d( - -1, 0, 0, - 0, -1, 0, - 0, 0, 1)) - self.assertTrue(q == q2) - - q = Quaterniond(0, 0, 1, 0) - q2 = Quaterniond() - q2.matrix(Matrix3d( - -1, 0, 0, - 0, 1, 0, - 0, 0, -1)) - self.assertTrue(q == q2) - - q = Quaterniond(0, 1, 0, 0) - q2 = Quaterniond() - q2.matrix(Matrix3d( - 1, 0, 0, - 0, -1, 0, - 0, 0, -1)) - self.assertTrue(q == q2) - def test_stream_out(self): q = Quaterniond(0.1, 1.2, 2.3) self.assertEqual(str(q), "0.1 1.2 2.3") From d26e61ef75ad35eed67944a92403de9c5dd828b2 Mon Sep 17 00:00:00 2001 From: LolaSegura Date: Mon, 30 Aug 2021 10:56:50 -0300 Subject: [PATCH 12/15] Adds python interface to Filter, MovingWindowFilter, RotationSpline. Signed-off-by: LolaSegura --- src/python/CMakeLists.txt | 6 ++ src/python/Filter.i | 104 ++++++++++++++++++++++++++ src/python/Filter_TEST.py | 98 ++++++++++++++++++++++++ src/python/MovingWindowFilter.i | 50 +++++++++++++ src/python/MovingWindowFilter_TEST.py | 68 +++++++++++++++++ src/python/RotationSpline.i | 48 ++++++++++++ src/python/RotationSpline_TEST.py | 83 ++++++++++++++++++++ src/python/python.i | 4 + 8 files changed, 461 insertions(+) create mode 100644 src/python/Filter.i create mode 100644 src/python/Filter_TEST.py create mode 100644 src/python/MovingWindowFilter.i create mode 100644 src/python/MovingWindowFilter_TEST.py create mode 100644 src/python/RotationSpline.i create mode 100644 src/python/RotationSpline_TEST.py diff --git a/src/python/CMakeLists.txt b/src/python/CMakeLists.txt index b078e8961..705c6327f 100644 --- a/src/python/CMakeLists.txt +++ b/src/python/CMakeLists.txt @@ -16,12 +16,15 @@ if (SWIG_FOUND) set(swig_files Angle + Filter GaussMarkovProcess Matrix3 Matrix4 + MovingWindowFilter Pose3 Quaternion Rand + RotationSpline Vector2 Vector3 Vector4) @@ -73,13 +76,16 @@ if (PYTHONLIBS_FOUND) # Add the Python tests set(python_tests Angle_TEST + Filter_TEST GaussMarkovProcess_TEST Matrix3_TEST Matrix4_TEST + MovingWindowFilter_TEST Pose3_TEST python_TEST Quaternion_TEST Rand_TEST + RotationSpline_TEST Vector2_TEST Vector3_TEST Vector4_TEST diff --git a/src/python/Filter.i b/src/python/Filter.i new file mode 100644 index 000000000..ae1bca296 --- /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..3e4bd7421 --- /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 math +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..481f70d6d --- /dev/null +++ b/src/python/MovingWindowFilter.i @@ -0,0 +1,50 @@ +/* + * 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/Export.hh" +#include "ignition/math/MovingWindowFilter.hh" +#include "ignition/math/Vector3.hh" +%} + +%import "std_vector.i" + +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..eca4d4dfc --- /dev/null +++ b/src/python/MovingWindowFilter_TEST.py @@ -0,0 +1,68 @@ +# 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 a612d52e1..2f52c64a8 100644 --- a/src/python/python.i +++ b/src/python/python.i @@ -9,3 +9,7 @@ %include Pose3.i %include Matrix3.i %include Matrix4.i +%include Filter.i +%include RotationSpline.i +%include MovingWindowFilter.i + From fac1c9612c0971374a046c9d7a8a415ce104abf1 Mon Sep 17 00:00:00 2001 From: LolaSegura Date: Mon, 30 Aug 2021 11:27:19 -0300 Subject: [PATCH 13/15] Deletes extra blank spaces. Signed-off-by: LolaSegura --- src/python/Filter_TEST.py | 2 +- src/python/MovingWindowFilter.i | 3 --- src/python/MovingWindowFilter_TEST.py | 2 -- 3 files changed, 1 insertion(+), 6 deletions(-) diff --git a/src/python/Filter_TEST.py b/src/python/Filter_TEST.py index 3e4bd7421..def7d9aa4 100644 --- a/src/python/Filter_TEST.py +++ b/src/python/Filter_TEST.py @@ -12,7 +12,6 @@ # See the License for the specific language governing permissions and # limitations under the License. -import math import unittest from ignition.math import BiQuadd from ignition.math import BiQuadVector3 @@ -94,5 +93,6 @@ def test_biquad_vector3(self): 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 index 481f70d6d..6bd687689 100644 --- a/src/python/MovingWindowFilter.i +++ b/src/python/MovingWindowFilter.i @@ -43,8 +43,5 @@ namespace math %template(MovingWindowFilteri) MovingWindowFilter; %template(MovingWindowFilterd) MovingWindowFilter; %template(MovingWindowFilterv3) MovingWindowFilter>; - - - } } diff --git a/src/python/MovingWindowFilter_TEST.py b/src/python/MovingWindowFilter_TEST.py index eca4d4dfc..bf8bf691c 100644 --- a/src/python/MovingWindowFilter_TEST.py +++ b/src/python/MovingWindowFilter_TEST.py @@ -19,8 +19,6 @@ from ignition.math import Vector3d - - class TestFilter(unittest.TestCase): def test_set_window_size(self): From 7ebbcc73d7b8a916d5dcb0a16332b39857174caa Mon Sep 17 00:00:00 2001 From: Franco Cipollone Date: Fri, 17 Sep 2021 13:13:36 -0300 Subject: [PATCH 14/15] Removes unused includes. Signed-off-by: Franco Cipollone --- src/python/MovingWindowFilter.i | 3 --- 1 file changed, 3 deletions(-) diff --git a/src/python/MovingWindowFilter.i b/src/python/MovingWindowFilter.i index 6bd687689..ea6fc003d 100644 --- a/src/python/MovingWindowFilter.i +++ b/src/python/MovingWindowFilter.i @@ -16,13 +16,10 @@ */ %module movingwindowfilter %{ -#include "ignition/math/Export.hh" #include "ignition/math/MovingWindowFilter.hh" #include "ignition/math/Vector3.hh" %} -%import "std_vector.i" - namespace ignition { namespace math From 2eb2bab5fa9ea875aab062c406e21f99d478c04b Mon Sep 17 00:00:00 2001 From: Steve Peters Date: Fri, 17 Sep 2021 21:45:16 -0700 Subject: [PATCH 15/15] fix whitespace Signed-off-by: Steve Peters --- src/python/Filter.i | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/python/Filter.i b/src/python/Filter.i index ae1bca296..be9e70562 100644 --- a/src/python/Filter.i +++ b/src/python/Filter.i @@ -70,8 +70,8 @@ namespace math class OnePoleVector3 { %rename("%(undercase)s", %$isfunction, %$ismember, %$not %$isconstructor) ""; - public: virtual const Vector3 &Value() const; - public: const Vector3& Process(const Vector3 &_x); + public: virtual const Vector3 &Value() const; + public: const Vector3& Process(const Vector3 &_x); public: OnePoleVector3(); public: OnePoleVector3(double _fc, double _fs) : OnePole>(_fc, _fs);