test_quaternion.py
Go to the documentation of this file.
1 #!/usr/bin/env python
2 
3 
4 """
5 Unit tests for the quaternion library
6 """
7 
8 from __future__ import absolute_import, division, print_function
9 import unittest
10 import numpy as np
11 from pymavlink.quaternion import QuaternionBase, Quaternion
12 from pymavlink.rotmat import Vector3, Matrix3
13 
14 __author__ = "Thomas Gubler"
15 __copyright__ = "Copyright (C) 2014 Thomas Gubler"
16 __license__ = "GNU Lesser General Public License v3"
17 __email__ = "thomasgubler@gmail.com"
18 
19 
20 class QuaternionBaseTest(unittest.TestCase):
21 
22  """
23  Class to test QuaternionBase
24  """
25 
26  def __init__(self, *args, **kwargs):
27  """Constructor, set up some data that is reused in many tests"""
28  super(QuaternionBaseTest, self).__init__(*args, **kwargs)
30 
31  def test_constructor(self):
32  """Test the constructor functionality"""
33  # Test the identity case
34  q = [1, 0, 0, 0]
35  euler = [0, 0, 0]
36  dcm = np.eye(3)
37  self._helper_test_constructor(q, euler, dcm)
38 
39  # test a case with rotations around all euler angles
40  q = [0.707106781186547, 0, 0.707106781186547, 0]
41  euler = [np.radians(90), np.radians(90), np.radians(90)]
42  dcm = [[0, 0, 1],
43  [0, 1, 0],
44  [-1, 0, 0]]
45  # test a case with rotations around all angles (values from matlab)
46  q = [0.774519052838329, 0.158493649053890, 0.591506350946110,
47  0.158493649053890]
48  euler = [np.radians(60), np.radians(60), np.radians(60)]
49  dcm = [[0.25, -0.058012701892219, 0.966506350946110],
50  [0.433012701892219, 0.899519052838329, -0.058012701892219],
51  [-0.866025403784439, 0.433012701892219, 0.25]]
52  self._helper_test_constructor(q, euler, dcm)
53 
54  # test another case (values from matlab)
55  q = [0.754971823897152, 0.102564313848771, -0.324261369073765,
56  -0.560671625082406]
57  euler = [np.radians(34), np.radians(-22), np.radians(-80)]
58  dcm = [[0.161003786707723, 0.780067269138261, -0.604626195500121],
59  [-0.913097848445116, 0.350255780704370, 0.208741963313735],
60  [0.374606593415912, 0.518474631686401, 0.768670252102276]]
61  self._helper_test_constructor(q, euler, dcm)
62 
63  def _helper_test_constructor(self, q, euler, dcm):
64  """
65  Helper function for constructor test
66 
67  Calls constructor for the quaternion from q euler and dcm and checks
68  if the resulting converions are equivalent to the arguments.
69  The test for the euler angles is weak as the solution is not unique
70 
71  :param q: quaternion 4x1, [w, x, y, z]
72  :param euler: [roll, pitch, yaw], needs to be equivalent to q
73  :param q: dcm 3x3, needs to be equivalent to q
74  """
75  # construct q from a QuaternionBase
76  quaternion_instance = QuaternionBase(q)
77  q_test = QuaternionBase(quaternion_instance)
78  np.testing.assert_almost_equal(q_test.q, q)
79  q_test = QuaternionBase(quaternion_instance)
80  np.testing.assert_almost_equal(q_test.dcm, dcm)
81  q_test = QuaternionBase(quaternion_instance)
82  q_euler = QuaternionBase(q_test.euler)
83  assert(np.allclose(q_test.euler, euler) or
84  np.allclose(q_test.q, q_euler.q))
85 
86  # construct q from a quaternion
87  q_test = QuaternionBase(q)
88  np.testing.assert_almost_equal(q_test.q, q)
89  q_test = QuaternionBase(q)
90  np.testing.assert_almost_equal(q_test.dcm, dcm)
91  q_test = QuaternionBase(q)
92  q_euler = QuaternionBase(q_test.euler)
93  assert(np.allclose(q_test.euler, euler) or
94  np.allclose(q_test.q, q_euler.q))
95 
96  # construct q from a euler angles
97  q_test = QuaternionBase(euler)
98  np.testing.assert_almost_equal(q_test.q, q)
99  q_test = QuaternionBase(euler)
100  np.testing.assert_almost_equal(q_test.dcm, dcm)
101  q_test = QuaternionBase(euler)
102  q_euler = QuaternionBase(q_test.euler)
103  assert(np.allclose(q_test.euler, euler) or
104  np.allclose(q_test.q, q_euler.q))
105 
106  # construct q from dcm
107  q_test = QuaternionBase(dcm)
108  np.testing.assert_almost_equal(q_test.q, q)
109  q_test = QuaternionBase(dcm)
110  np.testing.assert_almost_equal(q_test.dcm, dcm)
111  q_test = QuaternionBase(dcm)
112  q_euler = QuaternionBase(q_test.euler)
113  assert(np.allclose(q_test.euler, euler) or
114  np.allclose(q_test.q, q_euler.q))
115 
116  def test_norm(self):
117  # """Tests the norm functions"""
118  qa = [1, 2, 3, 4]
119  q = QuaternionBase(qa)
120  n = np.sqrt(np.dot(qa, qa))
121  qan = qa / n
122 
123  self.assertAlmostEqual(n, QuaternionBase.norm_array(qa))
124  np.testing.assert_almost_equal(qan, QuaternionBase.normalize_array(qa))
125  np.testing.assert_almost_equal(n, q.norm)
126  q.normalize()
127  np.testing.assert_almost_equal(qan, q.q)
128  self.assertAlmostEqual(1, q.norm)
129 
130  def _all_angles(self, step=np.radians(45)):
131  """
132  Creates a list of all euler angles
133 
134  :param step: stepsixe in radians
135  :returns: euler angles [[phi, thea, psi], [phi, theta, psi], ...]
136  """
137  e = 0.5
138  r_phi = np.arange(-np.pi + e, np.pi - e, step)
139  r_theta = np.arange(-np.pi/2 + e, np.pi/2 - e, step)
140  r_psi = np.arange(-np.pi + e, np.pi - e, step)
141  return [[phi, theta, psi] for phi in r_phi for theta in r_theta
142  for psi in r_psi]
143 
144  def _all_quaternions(self):
145  """Generate quaternions from all euler angles"""
146  return [QuaternionBase(e) for e in self._all_angles()]
147 
148  def test_conversion(self):
149  """
150  Tests forward and backward conversions
151  """
152  for q in self.quaternions:
153  # quaternion -> euler -> quaternion
154  q0 = q
155  e = QuaternionBase(q.q).euler
156  q1 = QuaternionBase(e)
157  assert q0.close(q1)
158 
159  # quaternion -> dcm -> quaternion
160  q0 = q
161  dcm = QuaternionBase(q.q).dcm
162  q1 = QuaternionBase(dcm)
163  assert q0.close(q1)
164 
165  def test_inversed(self):
166  """Test inverse"""
167  for q in self.quaternions:
168  q_inv = q.inversed
169  q_inv_inv = q_inv.inversed
170  assert q.close(q_inv_inv)
171 
172  def test_mul(self):
173  """Test multiplication"""
174  for q in self.quaternions:
175  for p in self.quaternions:
176  assert q.close(p * p.inversed * q)
177  r = p * q
178  r_dcm = np.dot(p.dcm, q.dcm)
179  np.testing.assert_almost_equal(r_dcm, r.dcm)
180 
181  def test_div(self):
182  """Test division"""
183  for q in self.quaternions:
184  for p in self.quaternions:
185  mul = q * p.inversed
186  div = q / p
187  assert mul.close(div)
188 
189  def test_transform(self):
190  """Test transform"""
191  for q in self.quaternions:
192  q_inv = q.inversed
193  v = np.array([1, 2, 3])
194  v1 = q.transform(v)
195  v1_dcm = np.dot(q.dcm, v)
196  np.testing.assert_almost_equal(v1, v1_dcm)
197  # test versus slower solution using multiplication
198  v1_mul = q * QuaternionBase(np.hstack([0, v])) * q.inversed
199  np.testing.assert_almost_equal(v1, v1_mul[1:4])
200  v2 = q_inv.transform(v1)
201  np.testing.assert_almost_equal(v, v2)
202 
203 
205  """
206  Class to test Quaternion
207  """
208 
209  def __init__(self, *args, **kwargs):
210  """Constructor, set up some data that is reused in many tests"""
211  super(QuaternionTest, self).__init__(*args, **kwargs)
213 
214  def _all_quaternions(self):
215  """Generate quaternions from all euler angles"""
216  return [Quaternion(e) for e in self._all_angles()]
217 
218  def test_constructor(self):
219  """Test the constructor functionality"""
220  # Test the identity case
221  q = [1, 0, 0, 0]
222  euler = [0, 0, 0]
223  dcm = Matrix3()
224  self._helper_test_constructor(q, euler, dcm)
225 
226  # test a case with rotations around all angles (values from matlab)
227  q = [0.774519052838329, 0.158493649053890, 0.591506350946110,
228  0.158493649053890]
229  euler = [np.radians(60), np.radians(60), np.radians(60)]
230  dcm = Matrix3(Vector3(0.25, -0.058012701892219, 0.966506350946110),
231  Vector3(0.433012701892219, 0.899519052838329,
232  -0.058012701892219),
233  Vector3(-0.866025403784439, 0.433012701892219, 0.25))
234 
235  self._helper_test_constructor(q, euler, dcm)
236 
237  def _helper_test_constructor(self, q, euler, dcm):
238  """
239  Helper function for constructor test
240 
241  Calls constructor for the quaternion from q euler and dcm and checks
242  if the resulting converions are equivalent to the arguments.
243  The test for the euler angles is weak as the solution is not unique
244 
245  :param q: quaternion 4x1, [w, x, y, z]
246  :param euler: Vector3(roll, pitch, yaw), needs to be equivalent to q
247  :param q: Matrix3, needs to be equivalent to q
248  """
249  # construct q from a Quaternion
250  quaternion_instance = Quaternion(q)
251  q_test = Quaternion(quaternion_instance)
252  np.testing.assert_almost_equal(q_test.q, q)
253  q_test = Quaternion(quaternion_instance)
254  assert q_test.dcm.close(dcm)
255  q_test = Quaternion(quaternion_instance)
256  q_euler = Quaternion(q_test.euler)
257  assert(np.allclose(q_test.euler, euler) or
258  np.allclose(q_test.q, q_euler.q))
259 
260  # construct q from a QuaternionBase
261  quaternion_instance = QuaternionBase(q)
262  q_test = Quaternion(quaternion_instance)
263  np.testing.assert_almost_equal(q_test.q, q)
264  q_test = Quaternion(quaternion_instance)
265  assert q_test.dcm.close(dcm)
266  q_test = Quaternion(quaternion_instance)
267  q_euler = Quaternion(q_test.euler)
268  assert(np.allclose(q_test.euler, euler) or
269  np.allclose(q_test.q, q_euler.q))
270 
271  # construct q from a quaternion
272  q_test = Quaternion(q)
273  np.testing.assert_almost_equal(q_test.q, q)
274  q_test = Quaternion(q)
275  assert q_test.dcm.close(dcm)
276  q_test = Quaternion(q)
277  q_euler = Quaternion(q_test.euler)
278  assert(np.allclose(q_test.euler, euler) or
279  np.allclose(q_test.q, q_euler.q))
280 
281  # # construct q from a euler angles
282  q_test = Quaternion(euler)
283  np.testing.assert_almost_equal(q_test.q, q)
284  q_test = Quaternion(euler)
285  assert q_test.dcm.close(dcm)
286  q_test = Quaternion(euler)
287  q_euler = Quaternion(q_test.euler)
288  assert(np.allclose(q_test.euler, euler) or
289  np.allclose(q_test.q, q_euler.q))
290 
291  # # construct q from dcm (Matrix3 instance)
292  q_test = Quaternion(dcm)
293  np.testing.assert_almost_equal(q_test.q, q)
294  q_test = Quaternion(dcm)
295  assert q_test.dcm.close(dcm)
296  q_test = Quaternion(dcm)
297  q_euler = Quaternion(q_test.euler)
298  assert(np.allclose(q_test.euler, euler) or
299  np.allclose(q_test.q, q_euler.q))
300 
301  def test_conversion(self):
302  """
303  Tests forward and backward conversions
304  """
305  for q in self.quaternions:
306  # quaternion -> euler -> quaternion
307  q0 = q
308  e = Quaternion(q.q).euler
309  q1 = Quaternion(e)
310  assert q0.close(q1)
311 
312  # quaternion -> dcm (Matrix3) -> quaternion
313  q0 = q
314  dcm = Quaternion(q.q).dcm
315  q1 = Quaternion(dcm)
316  assert q0.close(q1)
317 
318  def test_transform(self):
319  """Test transform"""
320  for q in self.quaternions:
321  q_inv = q.inversed
322  v = Vector3(1, 2, 3)
323  v1 = q.transform(v)
324  v1_dcm = q.dcm * v
325  assert v1.close(v1_dcm)
326  v2 = q_inv.transform(v1)
327  assert v.close(v2)
328 
329  def test_mul(self):
330  """Test multiplication"""
331  for q in self.quaternions:
332  for p in self.quaternions:
333  assert q.close(p * p.inversed * q)
334  r = p * q
335  r_dcm = p.dcm * q.dcm
336  assert r_dcm.close(r.dcm)
337 
338 
339 if __name__ == '__main__':
340  unittest.main()
def _all_angles(self, step=np.radians(45))
def _helper_test_constructor(self, q, euler, dcm)
def __init__(self, args, kwargs)
def _helper_test_constructor(self, q, euler, dcm)
def __init__(self, args, kwargs)


mavlink
Author(s): Lorenz Meier
autogenerated on Sun Jul 7 2019 03:22:07