diff --git a/navipy/maths/test_euler.py b/navipy/maths/test_euler.py
index 60f66b503f711a37b35fe3b4a7f456a4ac920643..30bf334abb27a05ab9e57c1ad719773a2898ea64 100644
--- a/navipy/maths/test_euler.py
+++ b/navipy/maths/test_euler.py
@@ -1,6 +1,6 @@
import numpy as np
# import navipy.maths.euler as euler
-import navipy.maths.euler as new_euler
+import navipy.maths.euler as euler
# import navipy.maths.constants as constants
import unittest
# from navipy.maths.euler import matrix
@@ -141,14 +141,23 @@ class TestEuler(unittest.TestCase):
"""
def test_from_matrix_new(self):
+ """
+ tests if the matrix and from_matrix (decompose) function
+ from the euler angles works correctly.
+ - take some angles
+ - build matrix from angles (euler.matrix)
+ - decompose obtained matrix to angles (euler.from_matrix)
+ - use obtained angles to build euler matrix (euler.matrix)
+ - compare old and new euler matrix
+ """
condition = dict()
- for key in list(new_euler._AXES2TUPLE.keys()):
+ for key in list(euler._AXES2TUPLE.keys()):
# print("key", key)
- rotation_0 = new_euler.matrix(1, 2, 3, key)[1:4,1:4]
+ rotation_0 = euler.matrix(1, 2, 3, key)[:3, :3]
# print("roation_0", rotation_0)
- [ai, aj, ak] = new_euler.from_matrix(rotation_0, key)
+ [ai, aj, ak] = euler.from_matrix(rotation_0, key)
# print("res", ai, aj, ak)
- rotation_1 = new_euler.matrix(ai, aj, ak, key)[1:4,1:4]
+ rotation_1 = euler.matrix(ai, aj, ak, key)[:3, :3]
condition[key] = np.allclose(rotation_0,
rotation_1)
if condition[key] is False:
@@ -257,13 +266,10 @@ class TestEuler(unittest.TestCase):
"""
condition = dict()
refkey = 'rxyz'
- for key in list(new_euler._AXES2TUPLE.keys()):
- print("between key", key)
- rotation_0 = new_euler.matrix(1, 2, 3, refkey)[1:4,1:4]
- print("betewwen matrix", rotation_0)
- [ai, aj, ak] = new_euler.from_matrix(rotation_0, key)
- print("between", ai, aj, ak)
- rotation_1 = new_euler.matrix(ai, aj, ak, key)[1:4,1:4]
+ for key in list(euler._AXES2TUPLE.keys()):
+ rotation_0 = euler.matrix(1, 2, 3, refkey)[:3, :3]
+ [ai, aj, ak] = euler.from_matrix(rotation_0, key)
+ rotation_1 = euler.matrix(ai, aj, ak, key)[:3, :3]
condition[key] = np.allclose(rotation_0,
rotation_1)
if condition[key] is False:
@@ -271,19 +277,22 @@ class TestEuler(unittest.TestCase):
self.assertTrue(np.all(np.array(condition.values())))
def test_from_quaternion(self):
- angles = new_euler.from_quaternion([0.99810947, 0.06146124, 0, 0])
- print("angles",angles)
- self.assertTrue(np.allclose(angles, [-0.123, 0, 0])) # 0.123
+ angles = euler.from_quaternion([0.99810947, 0.06146124, 0, 0])
+ self.assertTrue(np.allclose(angles, [0.123, 0, 0])) # 0.123
def test_from_quaternion_params(self):
+ """
+ test if errors are raised correctly when parameters of wrong
+ type etc are passed to the euler.from_quaternion function
+ """
for a, b, c, d in [(None, 2, 6, 'rxyz'), (9.0, 'w', 2, 'rxyz'),
(5.0, 4.0, None, 'rxyz'),
(1.0, 2.0, 3.0, 'w')]:
with self.assertRaises(ValueError):
- new_euler.from_quaternion([a, b, c, d])
+ euler.from_quaternion([a, b, c, d])
with self.assertRaises(Exception):
- new_euler.from_quaternion([9.0, 8.0, 7.0, 0.3], 'w')
+ euler.from_quaternion([9.0, 8.0, 7.0, 0.3], 'w')
"""
def test_matrix_params(self):
@@ -300,46 +309,50 @@ class TestEuler(unittest.TestCase):
"""
def test_from_matrix_params(self):
+ """
+ test if errors are raised correctly if parameters
+ of wrong type, value are passed to the euler.from_matrix
+ function
+ """
for a, b, c, d in [(None, 2, 6, 8), (9.0, np.nan, 2, 3),
(5.0, 4.0, None, 8.0)]:
with self.assertRaises(ValueError):
- new_euler.from_matrix([[a], b, c, d])
+ euler.from_matrix([[a], b, c, d])
with self.assertRaises(Exception):
- new_euler.from_matrix(9.0, 8.0, 7.0, 'w')
+ euler.from_matrix(9.0, 8.0, 7.0, 'w')
def test_angle_rate_matrix_params(self):
+ """
+ test if errors are raised correctly if parameters
+ of wrong type, value are passed to the
+ euler.angle_rate_matrix function
+ """
for a, b, c, d in [(None, 2, 6, 'rxyz'), (5.0, 4.0, None, 'sxyx')]:
with self.assertRaises(TypeError):
- new_euler.angle_rate_matrix(a, b, c, d)
+ euler.angle_rate_matrix(a, b, c, d)
# for a, b, c, d in [(9.0, np.nan, 2, 'rxyz')]:
# with self.assertRaises(ValueError):
- # new_euler.angle_rate_matrix(a, b, c, d)
+ # euler.angle_rate_matrix(a, b, c, d)
with self.assertRaises(Exception):
- new_euler.angle_rate_matrix(9.0, 8.0, 7.0, 'w')
+ euler.angle_rate_matrix(9.0, 8.0, 7.0, 'w')
- def test_angular_velocity_params(self):
- for a, b, c, d, e, f, g in [(None, 2, 6, 8, 7, 8, 'b'),
- (9.0, 8, 2, 3, 3, 5, 7),
- (5.0, 4.0, None, 8.0, 8.0, 4.0, 2.0),
- (9.0, 8.0, 7.0, np.nan, 6.0, 9.0, 'w'),
- (4.0, 2.0, 1.0, 3.0, 'w', 2.0, 9),
- (1.0, 2.0, 3.0, 4.0, None, 4, 'w'),
- (5.0, 4.0, 3.0, 8.0, 8.0, 4.0, 'l')]:
- with self.assertRaises(TypeError):
- new_euler.angle_rate_matrix(a, b, c, d, e, f, g)
-
- def test_angular_velocity_params_new(self):
- for a, b, c, d, e, f, g in [(None, 2, 6, 8, 7, 8, 'b'),
+ def test_angular_velocity(self):
+ """
+ test if errors are raised correctly if parameters
+ of wrong type, value are passed to the
+ euler.angle_rate_matrix function
+ """
+ for a, b, c, d, e, f, g in [(None, 2, 6, 8, 7, 8, 'rxyz'),
(9.0, 8, 2, 3, 3, 5, 7),
- (5.0, 4.0, None, 8.0, 8.0, 4.0, 2.0),
- (9.0, 8.0, 7.0, np.nan, 6.0, 9.0, 'w'),
- (4.0, 2.0, 1.0, 3.0, 'w', 2.0, 9),
- (1.0, 2.0, 3.0, 4.0, None, 4, 'w'),
+ (5.0, 4.0, None, 8.0, 8.0, 4.0, 'rxyz'),
+ (9.0, 8.0, 7.0, np.nan, 6.0, 9.0, 'rxyz'),
+ (4.0, 2.0, 1.0, 3.0, 'w', 2.0, 'rxyz'),
+ (1.0, 2.0, 3.0, 4.0, None, 4, 'rxyz'),
(5.0, 4.0, 3.0, 8.0, 8.0, 4.0, 'l')]:
with self.assertRaises(TypeError):
- new_euler.angle_rate_matrix(a, b, c, d, e, f, g)
+ euler.angular_velocity(a, b, c, d, e, f, g)
# def test_old_vs_newXZX(self):
# """ example from wikipedia
@@ -401,8 +414,8 @@ class TestEuler(unittest.TestCase):
ai = 0.10
aj = 0.20
ak = 0.70
- R1 = matrix(ai, aj, ak, 'rxyz')[1:4,1:4]
- R2 = matrix(ai, aj, ak, 'sxyz')[1:4,1:4]
+ R1 = matrix(ai, aj, ak, 'rxyz')[:3, :3]
+ R2 = matrix(ai, aj, ak, 'sxyz')[:3, :3]
# print("test trans",R1, R2)
self.assertTrue(np.allclose(R1, np.transpose(R2)))
@@ -436,8 +449,8 @@ class TestEuler(unittest.TestCase):
# dai = 12
# daj = 0
# dak = 0
- Rijk_r = matrix(ai, aj, ak, 'rxyz')[1:4,1:4]
- Rijk = matrix(ai, aj, ak, 'sxyz')[1:4,1:4]
+ Rijk_r = matrix(ai, aj, ak, 'rxyz')[:3, :3]
+ Rijk = matrix(ai, aj, ak, 'sxyz')[:3, :3]
E_p = angle_rate_matrix(ai, aj, ak, 'rxyz')
E = angle_rate_matrix(ai, aj, ak, 'sxyz')
E_inv = np.linalg.inv(E)
@@ -500,8 +513,8 @@ class TestEuler(unittest.TestCase):
c(aj)]]
Rijk_p = np.transpose(Rijk)
Rijk_test = np.dot(R3(ai), np.dot(R1(aj), R3(ak)))
- Rijk_fkt = matrix(ai, aj, ak, 'szxz')[1:4,1:4]
- Rijk_p_fkt = matrix(ai, aj, ak, 'rzxz')[1:4,1:4]
+ Rijk_fkt = matrix(ai, aj, ak, 'szxz')[:3, :3]
+ Rijk_p_fkt = matrix(ai, aj, ak, 'rzxz')[:3, :3]
self.assertTrue(np.allclose(Rijk, Rijk_fkt))
self.assertTrue(np.allclose(Rijk, Rijk_test))
self.assertTrue(np.allclose(Rijk_p, Rijk_p_fkt))
@@ -511,22 +524,28 @@ class TestEuler(unittest.TestCase):
self.assertTrue(np.allclose(w_p, new_w_p))
def test_velocity(self):
+ """ the angular velocity in a stationary axis system
+ should be the same as the angular velocity in a rotational
+ axis sytem when multiplied by the rotation matrix with
+ with the same angles and coordinate system
+ w = angular_vel(a,b,c)
+ Rijk = rot_matrix(a,b,c)
+ vel' = Rijk * w
ai = 0.10
aj = 0.20
ak = 0
dai = 0
daj = 0
dak = 0.01
- Rijk = matrix(ai, aj, ak, 'szxz')[1:4,1:4]
- Rijk_r = matrix(ai, aj, ak, 'rzxz')[1:4,1:4]
+ Rijk = matrix(ai, aj, ak, 'szxz')[:3, :3]
+ Rijk_r = matrix(ai, aj, ak, 'rzxz')[:3, :3]
vel = angular_velocity(ai, aj, ak, dai, daj, dak, 'szxz')
vel_r = angular_velocity(ai, aj, ak, dai, daj, dak, 'rzxz')
vel_p = np.dot(Rijk, vel)
vel_new = np.dot(Rijk_r, vel_p)
self.assertTrue(np.allclose(vel_p, vel_r))
self.assertTrue(np.allclose(vel_new, vel))
-
- """
+
def test_large_rot(self):
ai = 0
aj = 0.0002
@@ -540,7 +559,6 @@ class TestEuler(unittest.TestCase):
print("vels", vel, vel_p)
self.assertTrue(np.allclose([100, 0, 0], vel, 0.1))
self.assertTrue(np.allclose([100, 0, 0], vel_p, 0.1))
- """
def test_angle_rate(self):
ea = 0.1
@@ -691,7 +709,7 @@ class TestEuler(unittest.TestCase):
rotM = np.column_stack([p1, p2, e3])
M = angle_rate_matrix(ea, eb, ec, 'szyz')
self.assertTrue(np.allclose(rotM, M))
-
+"""
if __name__ == '__main__':
unittest.main()
diff --git a/navipy/maths/test_homogeneous_transformations.py b/navipy/maths/test_homogeneous_transformations.py
index d4672798f70491331ea99475d88c0763be0413ac..b6dd662b683c056d40ae3cf8e469c7f2ce5c4ec8 100644
--- a/navipy/maths/test_homogeneous_transformations.py
+++ b/navipy/maths/test_homogeneous_transformations.py
@@ -3,7 +3,7 @@ import numpy as np
import navipy.maths.homogeneous_transformations as ht
import navipy.maths.euler as euler
import navipy.maths.random as random
-import navipy.maths.euler_old as euler_old
+import navipy.maths.euler as euler
class TestHT(unittest.TestCase):
@@ -222,13 +222,8 @@ class TestHT(unittest.TestCase):
def test_decompose_matrix_rotation(self):
rotation_0 = euler.matrix(1, 2, 3, 'sxyz')
- rotation_0 = ht.new2old(rotation_0)
- print("matrix 0", rotation_0)
_, _, angles, _, _ = ht.decompose_matrix(rotation_0)
- print("angles", angles)
rotation_1 = euler.matrix(*angles, 'sxyz')
- rotation_1 = ht.new2old(rotation_1)
- print("matrix 1", rotation_1)
self.assertTrue(np.allclose(rotation_0, rotation_1))
def test_compose_matrix(self):
@@ -238,10 +233,8 @@ class TestHT(unittest.TestCase):
trans = np.random.random(3) - 0.5
persp = np.random.random(4) - 0.5
matrix_0 = ht.compose_matrix(scale, shear, angles, trans, persp)
- #print("matrix 1", matrix_0)
result = ht.decompose_matrix(matrix_0)
matrix_1 = ht.compose_matrix(*result)
- #print("matrix 0", matrix_1)
self.assertTrue(ht.is_same_transform(matrix_0, matrix_1))
def test_is_same_transform(self):
diff --git a/navipy/maths/test_quaternion.py b/navipy/maths/test_quaternion.py
index 917f303bb7f51b2f4d66c444ee070e31b0261d55..6554425288afcf3345479bf647ede9b4b40875fe 100644
--- a/navipy/maths/test_quaternion.py
+++ b/navipy/maths/test_quaternion.py
@@ -8,26 +8,20 @@ import navipy.maths.random as random
class TestQuaternions(unittest.TestCase):
def test_quaternion_from_euler(self):
quaternion = quat.from_euler(1, 2, 3, 'syxz')
- self.assertTrue(np.allclose(quaternion,[[0.03354074],
- [0],
- [0.01832339],
- [0]]))
-
-
- # [0.435953, 0.310622,
- # -0.718287, 0.444435]))
+ self.assertTrue(np.allclose(quaternion,
+ [0.435953, 0.310622,
+ -0.718287, 0.444435]))
def test_about_axis(self):
quaternion = quat.about_axis(0.123, [1, 0, 0])
self.assertTrue(np.allclose(quaternion,
[0.99810947, 0.06146124, 0, 0]))
- # [0.99810947, 0.06146124, 0, 0]))
def test_matrix(self):
- matrix = quat.matrix([0.99810947, 0.06146124, 0, 0])
- matrix2 = ht.rotation_matrix(0.123, [1, 0, 0])
+ matrix = quat.matrix([0.99810947, 0.06146124, 0, 0], 'rxyz')
self.assertTrue(np.allclose(matrix,
- ht.old2new(matrix2)))
+ ht.rotation_matrix(0.123,
+ [1, 0, 0])))
def test_matrix_identity(self):
matrix = quat.matrix([1, 0, 0, 0])
@@ -35,24 +29,22 @@ class TestQuaternions(unittest.TestCase):
def test_matrix_diagonal(self):
matrix = quat.matrix([0, 1, 0, 0])
- print("last", matrix)
- self.assertTrue(np.allclose(matrix, np.diag([1, 1, -1, -1])))
+ self.assertTrue(np.allclose(matrix, np.diag([1, -1, -1, 1])))
def test_from_matrix_identity(self):
quaternion = quat.from_matrix(np.identity(4), True)
self.assertTrue(np.allclose(quaternion, [1, 0, 0, 0]))
def test_from_matrix_diagonal(self):
- mat = np.diag([1, -1, -1, 1])
- quaternion = quat.from_matrix(mat)
- self.assertTrue(np.allclose(quaternion, [0, 0, 0, 1])
- or np.allclose(quaternion, [0, 0, 0, -1]))
+ quaternion = quat.from_matrix(np.diag([1, -1, -1, 1]))
+ self.assertTrue(np.allclose(quaternion, [0, 1, 0, 0])
+ or np.allclose(quaternion, [0, -1, 0, 0]))
def test_from_matrix_rotation(self):
rotation = random.rotation_matrix()
quaternion = quat.from_matrix(rotation)
self.assertTrue(ht.is_same_transform(
- rotation, quat.matrix(quaternion, 'sxyz')))
+ rotation, quat.matrix(quaternion)))
# Seem to fail from time to time....
# def test_quaternion_multiply(self):
@@ -70,8 +62,6 @@ class TestQuaternions(unittest.TestCase):
def test_inverse(self):
quaternion_0 = random.quaternion()
quaternion_1 = quat.inverse(quaternion_0)
- print("quaternion0", quaternion_0)
- print("quaternion1", quaternion_1)
self.assertTrue(np.allclose(
quat.multiply(quaternion_0, quaternion_1),
[1, 0, 0, 0]))
@@ -79,13 +69,11 @@ class TestQuaternions(unittest.TestCase):
def test_quaternion(self):
alpha, beta, gamma = 0.123, -1.234, 2.345
xaxis, yaxis, zaxis = [1, 0, 0], [0, 1, 0], [0, 0, 1]
- rotx = ht.rotation_matrix(alpha, xaxis)
- roty = ht.rotation_matrix(beta, yaxis)
- rotz = ht.rotation_matrix(gamma, zaxis)
- rotx = ht.old2new(rotx)
- roty = ht.old2new(roty)
- rotz = ht.old2new(rotz)
+ rotx = ht.rotation_matrix(alpha, xaxis,)
+ roty = ht.rotation_matrix(beta, yaxis,)
+ rotz = ht.rotation_matrix(gamma, zaxis,)
rotation = rotx.dot(roty.dot(rotz))
+ rotation = np.transpose(rotation)
quatx = quat.about_axis(alpha, xaxis)
quaty = quat.about_axis(beta, yaxis)
@@ -98,3 +86,4 @@ class TestQuaternions(unittest.TestCase):
if __name__ == '__main__':
unittest.main()
+