quaterniontest.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 
00003 
00004 """
00005 Unit tests for the quaternion library
00006 """
00007 
00008 from __future__ import absolute_import, division, print_function
00009 import unittest
00010 import numpy as np
00011 from pymavlink.quaternion import QuaternionBase, Quaternion
00012 from pymavlink.rotmat import Vector3, Matrix3
00013 
00014 __author__ = "Thomas Gubler"
00015 __copyright__ = "Copyright (C) 2014 Thomas Gubler"
00016 __license__ = "GNU Lesser General Public License v3"
00017 __email__ = "thomasgubler@gmail.com"
00018 
00019 
00020 class QuaternionBaseTest(unittest.TestCase):
00021 
00022     """
00023     Class to test QuaternionBase
00024     """
00025 
00026     def __init__(self, *args, **kwargs):
00027         """Constructor, set up some data that is reused in many tests"""
00028         super(QuaternionBaseTest, self).__init__(*args, **kwargs)
00029         self.quaternions = self._all_quaternions()
00030 
00031     def test_constructor(self):
00032         """Test the constructor functionality"""
00033         # Test the identity case
00034         q = [1, 0, 0, 0]
00035         euler = [0, 0, 0]
00036         dcm = np.eye(3)
00037         self._helper_test_constructor(q, euler, dcm)
00038 
00039         # test a case with rotations around all euler angles
00040         q = [0.707106781186547, 0, 0.707106781186547, 0]
00041         euler = [np.radians(90), np.radians(90), np.radians(90)]
00042         dcm = [[0, 0, 1],
00043                [0, 1, 0],
00044                [-1, 0, 0]]
00045         # test a case with rotations around all angles (values from matlab)
00046         q = [0.774519052838329, 0.158493649053890, 0.591506350946110,
00047              0.158493649053890]
00048         euler = [np.radians(60), np.radians(60), np.radians(60)]
00049         dcm = [[0.25, -0.058012701892219, 0.966506350946110],
00050                [0.433012701892219, 0.899519052838329, -0.058012701892219],
00051                [-0.866025403784439, 0.433012701892219, 0.25]]
00052         self._helper_test_constructor(q, euler, dcm)
00053 
00054         # test another case (values from matlab)
00055         q = [0.754971823897152, 0.102564313848771, -0.324261369073765,
00056              -0.560671625082406]
00057         euler = [np.radians(34), np.radians(-22), np.radians(-80)]
00058         dcm = [[0.161003786707723, 0.780067269138261, -0.604626195500121],
00059                [-0.913097848445116, 0.350255780704370, 0.208741963313735],
00060                [0.374606593415912, 0.518474631686401, 0.768670252102276]]
00061         self._helper_test_constructor(q, euler, dcm)
00062 
00063     def _helper_test_constructor(self, q, euler, dcm):
00064         """
00065         Helper function for constructor test
00066 
00067         Calls constructor for the quaternion from q euler and dcm and checks
00068         if the resulting converions are equivalent to the arguments.
00069         The test for the euler angles is weak as the solution is not unique
00070 
00071         :param q: quaternion 4x1, [w, x, y, z]
00072         :param euler: [roll, pitch, yaw], needs to be equivalent to q
00073         :param q: dcm 3x3, needs to be equivalent to q
00074         """
00075         # construct q from a QuaternionBase
00076         quaternion_instance = QuaternionBase(q)
00077         q_test = QuaternionBase(quaternion_instance)
00078         np.testing.assert_almost_equal(q_test.q, q)
00079         q_test = QuaternionBase(quaternion_instance)
00080         np.testing.assert_almost_equal(q_test.dcm, dcm)
00081         q_test = QuaternionBase(quaternion_instance)
00082         q_euler = QuaternionBase(q_test.euler)
00083         assert(np.allclose(q_test.euler, euler) or
00084                np.allclose(q_test.q, q_euler.q))
00085 
00086         # construct q from a quaternion
00087         q_test = QuaternionBase(q)
00088         np.testing.assert_almost_equal(q_test.q, q)
00089         q_test = QuaternionBase(q)
00090         np.testing.assert_almost_equal(q_test.dcm, dcm)
00091         q_test = QuaternionBase(q)
00092         q_euler = QuaternionBase(q_test.euler)
00093         assert(np.allclose(q_test.euler, euler) or
00094                np.allclose(q_test.q, q_euler.q))
00095 
00096         # construct q from a euler angles
00097         q_test = QuaternionBase(euler)
00098         np.testing.assert_almost_equal(q_test.q, q)
00099         q_test = QuaternionBase(euler)
00100         np.testing.assert_almost_equal(q_test.dcm, dcm)
00101         q_test = QuaternionBase(euler)
00102         q_euler = QuaternionBase(q_test.euler)
00103         assert(np.allclose(q_test.euler, euler) or
00104                np.allclose(q_test.q, q_euler.q))
00105 
00106         # construct q from dcm
00107         q_test = QuaternionBase(dcm)
00108         np.testing.assert_almost_equal(q_test.q, q)
00109         q_test = QuaternionBase(dcm)
00110         np.testing.assert_almost_equal(q_test.dcm, dcm)
00111         q_test = QuaternionBase(dcm)
00112         q_euler = QuaternionBase(q_test.euler)
00113         assert(np.allclose(q_test.euler, euler) or
00114                np.allclose(q_test.q, q_euler.q))
00115 
00116     def test_norm(self):
00117         # """Tests the norm functions"""
00118         qa = [1, 2, 3, 4]
00119         q = QuaternionBase(qa)
00120         n = np.sqrt(np.dot(qa, qa))
00121         qan = qa / n
00122 
00123         self.assertAlmostEqual(n, QuaternionBase.norm_array(qa))
00124         np.testing.assert_almost_equal(qan, QuaternionBase.normalize_array(qa))
00125         np.testing.assert_almost_equal(n, q.norm)
00126         q.normalize()
00127         np.testing.assert_almost_equal(qan, q.q)
00128         self.assertAlmostEqual(1, q.norm)
00129 
00130     def _all_angles(self, step=np.radians(45)):
00131         """
00132         Creates a list of all euler angles
00133 
00134         :param step: stepsixe in radians
00135         :returns: euler angles [[phi, thea, psi], [phi, theta, psi], ...]
00136         """
00137         e = 0.5
00138         r_phi = np.arange(-np.pi + e, np.pi - e, step)
00139         r_theta = np.arange(-np.pi/2 + e, np.pi/2 - e, step)
00140         r_psi = np.arange(-np.pi + e, np.pi - e, step)
00141         return [[phi, theta, psi] for phi in r_phi for theta in r_theta
00142                 for psi in r_psi]
00143 
00144     def _all_quaternions(self):
00145         """Generate quaternions from all euler angles"""
00146         return [QuaternionBase(e) for e in self._all_angles()]
00147 
00148     def test_conversion(self):
00149         """
00150         Tests forward and backward conversions
00151         """
00152         for q in self.quaternions:
00153             # quaternion -> euler -> quaternion
00154             q0 = q
00155             e = QuaternionBase(q.q).euler
00156             q1 = QuaternionBase(e)
00157             self.assertTrue(q0.close(q1))
00158 
00159             # quaternion -> dcm -> quaternion
00160             q0 = q
00161             dcm = QuaternionBase(q.q).dcm
00162             q1 = QuaternionBase(dcm)
00163             self.assertTrue(q0.close(q1))
00164 
00165     def test_inversed(self):
00166         """Test inverse"""
00167         for q in self.quaternions:
00168             q_inv = q.inversed
00169             q_inv_inv = q_inv.inversed
00170             self.assertTrue(q.close(q_inv_inv))
00171 
00172     def test_mul(self):
00173         """Test multiplication"""
00174         for q in self.quaternions:
00175             for p in self.quaternions:
00176                 self.assertTrue(q.close(p * p.inversed * q))
00177                 r = p * q
00178                 r_dcm = np.dot(p.dcm, q.dcm)
00179                 np.testing.assert_almost_equal(r_dcm, r.dcm)
00180 
00181     def test_div(self):
00182         """Test division"""
00183         for q in self.quaternions:
00184             for p in self.quaternions:
00185                 mul = q * p.inversed
00186                 div = q / p
00187                 self.assertTrue(mul.close(div))
00188 
00189     def test_transform(self):
00190         """Test transform"""
00191         for q in self.quaternions:
00192             q_inv = q.inversed
00193             v = np.array([1, 2, 3])
00194             v1 = q.transform(v)
00195             v1_dcm = np.dot(q.dcm, v)
00196             np.testing.assert_almost_equal(v1, v1_dcm)
00197             # test versus slower solution using multiplication
00198             v1_mul = q * QuaternionBase(np.hstack([0, v])) * q.inversed
00199             np.testing.assert_almost_equal(v1, v1_mul[1:4])
00200             v2 = q_inv.transform(v1)
00201             np.testing.assert_almost_equal(v, v2)
00202 
00203 
00204 class QuaternionTest(QuaternionBaseTest):
00205     """
00206     Class to test Quaternion
00207     """
00208 
00209     def __init__(self, *args, **kwargs):
00210         """Constructor, set up some data that is reused in many tests"""
00211         super(QuaternionTest, self).__init__(*args, **kwargs)
00212         self.quaternions = self._all_quaternions()
00213 
00214     def _all_quaternions(self):
00215         """Generate quaternions from all euler angles"""
00216         return [Quaternion(e) for e in self._all_angles()]
00217 
00218     def test_constructor(self):
00219         """Test the constructor functionality"""
00220         # Test the identity case
00221         q = [1, 0, 0, 0]
00222         euler = [0, 0, 0]
00223         dcm = Matrix3()
00224         self._helper_test_constructor(q, euler, dcm)
00225 
00226         # test a case with rotations around all angles (values from matlab)
00227         q = [0.774519052838329, 0.158493649053890, 0.591506350946110,
00228              0.158493649053890]
00229         euler = [np.radians(60), np.radians(60), np.radians(60)]
00230         dcm = Matrix3(Vector3(0.25, -0.058012701892219, 0.966506350946110),
00231                       Vector3(0.433012701892219, 0.899519052838329,
00232                               -0.058012701892219),
00233                       Vector3(-0.866025403784439, 0.433012701892219, 0.25))
00234 
00235         self._helper_test_constructor(q, euler, dcm)
00236 
00237     def _helper_test_constructor(self, q, euler, dcm):
00238         """
00239         Helper function for constructor test
00240 
00241         Calls constructor for the quaternion from q euler and dcm and checks
00242         if the resulting converions are equivalent to the arguments.
00243         The test for the euler angles is weak as the solution is not unique
00244 
00245         :param q: quaternion 4x1, [w, x, y, z]
00246         :param euler: Vector3(roll, pitch, yaw), needs to be equivalent to q
00247         :param q: Matrix3, needs to be equivalent to q
00248         """
00249         # construct q from a Quaternion
00250         quaternion_instance = Quaternion(q)
00251         q_test = Quaternion(quaternion_instance)
00252         np.testing.assert_almost_equal(q_test.q, q)
00253         q_test = Quaternion(quaternion_instance)
00254         self.assertTrue(q_test.dcm.close(dcm))
00255         q_test = Quaternion(quaternion_instance)
00256         q_euler = Quaternion(q_test.euler)
00257         assert(np.allclose(q_test.euler, euler) or
00258                np.allclose(q_test.q, q_euler.q))
00259 
00260         # construct q from a QuaternionBase
00261         quaternion_instance = QuaternionBase(q)
00262         q_test = Quaternion(quaternion_instance)
00263         np.testing.assert_almost_equal(q_test.q, q)
00264         q_test = Quaternion(quaternion_instance)
00265         self.assertTrue(q_test.dcm.close(dcm))
00266         q_test = Quaternion(quaternion_instance)
00267         q_euler = Quaternion(q_test.euler)
00268         assert(np.allclose(q_test.euler, euler) or
00269                np.allclose(q_test.q, q_euler.q))
00270 
00271         # construct q from a quaternion
00272         q_test = Quaternion(q)
00273         np.testing.assert_almost_equal(q_test.q, q)
00274         q_test = Quaternion(q)
00275         self.assertTrue(q_test.dcm.close(dcm))
00276         q_test = Quaternion(q)
00277         q_euler = Quaternion(q_test.euler)
00278         assert(np.allclose(q_test.euler, euler) or
00279                np.allclose(q_test.q, q_euler.q))
00280 
00281         # # construct q from a euler angles
00282         q_test = Quaternion(euler)
00283         np.testing.assert_almost_equal(q_test.q, q)
00284         q_test = Quaternion(euler)
00285         self.assertTrue(q_test.dcm.close(dcm))
00286         q_test = Quaternion(euler)
00287         q_euler = Quaternion(q_test.euler)
00288         assert(np.allclose(q_test.euler, euler) or
00289                np.allclose(q_test.q, q_euler.q))
00290 
00291         # # construct q from dcm (Matrix3 instance)
00292         q_test = Quaternion(dcm)
00293         np.testing.assert_almost_equal(q_test.q, q)
00294         q_test = Quaternion(dcm)
00295         self.assertTrue(q_test.dcm.close(dcm))
00296         q_test = Quaternion(dcm)
00297         q_euler = Quaternion(q_test.euler)
00298         assert(np.allclose(q_test.euler, euler) or
00299                np.allclose(q_test.q, q_euler.q))
00300 
00301     def test_conversion(self):
00302         """
00303         Tests forward and backward conversions
00304         """
00305         for q in self.quaternions:
00306             # quaternion -> euler -> quaternion
00307             q0 = q
00308             e = Quaternion(q.q).euler
00309             q1 = Quaternion(e)
00310             self.assertTrue(q0.close(q1))
00311 
00312             # quaternion -> dcm (Matrix3) -> quaternion
00313             q0 = q
00314             dcm = Quaternion(q.q).dcm
00315             q1 = Quaternion(dcm)
00316             self.assertTrue(q0.close(q1))
00317 
00318     def test_transform(self):
00319         """Test transform"""
00320         for q in self.quaternions:
00321             q_inv = q.inversed
00322             v = Vector3(1, 2, 3)
00323             v1 = q.transform(v)
00324             v1_dcm = q.dcm * v
00325             self.assertTrue(v1.close(v1_dcm))
00326             v2 = q_inv.transform(v1)
00327             self.assertTrue(v.close(v2))
00328 
00329     def test_mul(self):
00330         """Test multiplication"""
00331         for q in self.quaternions:
00332             for p in self.quaternions:
00333                 self.assertTrue(q.close(p * p.inversed * q))
00334                 r = p * q
00335                 r_dcm = p.dcm * q.dcm
00336                 self.assertTrue(r_dcm.close(r.dcm))
00337 
00338 
00339 if __name__ == '__main__':
00340     unittest.main()


mavlink
Author(s): Lorenz Meier
autogenerated on Thu Jun 6 2019 19:01:57