diff --git a/navipy/maths/constants.py b/navipy/maths/constants.py
index 282096c56a4f767a3311c98c27e58ae4fb05f19e..62599860921ed8e5918f9ece80d451e1ffd8f287 100644
--- a/navipy/maths/constants.py
+++ b/navipy/maths/constants.py
@@ -13,8 +13,4 @@ _AXES2TUPLE = {
'sxyz': (0, 0, 1, 2), 'sxyx': (0, 0, 1, 0), 'sxzy': (0, 0, 2, 1),
'sxzx': (0, 0, 2, 0), 'syzx': (0, 1, 2, 0), 'syzy': (0, 1, 2, 1),
'syxz': (0, 1, 0, 2), 'syxy': (0, 1, 0, 1), 'szxy': (0, 2, 0, 1),
- 'szxz': (0, 2, 0, 2), 'szyx': (0, 2, 1, 0), 'szyz': (0, 2, 1, 2),
- 'rzyx': (1, 2, 1, 0), 'rxyx': (1, 0, 1, 0), 'ryzx': (1, 1, 2, 0),
- 'rxzx': (1, 0, 2, 0), 'rxzy': (1, 0, 2, 1), 'ryzy': (1, 1, 2, 1),
- 'rzxy': (1, 2, 0, 1), 'ryxy': (1, 1, 2, 1), 'ryxz': (1, 1, 0, 2),
- 'rzxz': (1, 2, 0, 2), 'rxyz': (1, 0, 1, 2), 'rzyz': (1, 2, 1, 2)}
+ 'szxz': (0, 2, 0, 2), 'szyx': (0, 2, 1, 0), 'szyz': (0, 2, 1, 2)}
diff --git a/navipy/maths/euler.py b/navipy/maths/euler.py
index bad8f03f360259c70d4bebb26535e9f6e71fd78f..01e5a225c2dcc09ad6695369ef4b788e8f1c1eea 100644
--- a/navipy/maths/euler.py
+++ b/navipy/maths/euler.py
@@ -89,10 +89,7 @@ def matrix(ai, aj, ak, axes='sxyz'):
ID = np.identity(4)
ID[:3, :3] = Rijk
Rijk = ID
- if not r:
- return Rijk
- else:
- return np.transpose(Rijk)
+ return Rijk
def from_matrix(matrix, axes='sxyz'):
@@ -118,11 +115,12 @@ def from_matrix(matrix, axes='sxyz'):
matrix = matrix[:3, :3]
u = None
rot, i, j, k = _AXES2TUPLE[axes]
- if rot:
- matrix = np.transpose(matrix)
- new = list(axes)
- new[0] = 's'
- axes = ''.join(new)
+ #matrix = np.transpose(matrix)
+ # if rot:
+ # matrix = np.transpose(matrix)
+ # new = list(axes)
+ # new[0] = 's'
+ # axes = ''.join(new)
if axes == 'sxyx':
u = [np.arctan2(matrix[1, 0], matrix[2, 0]),
np.arccos(matrix[0, 0]),
@@ -132,17 +130,17 @@ def from_matrix(matrix, axes='sxyz'):
-np.arcsin(matrix[0, 2]),
np.arctan2(matrix[0, 1], matrix[0, 0])]
elif axes == 'sxzx':
- u = [np.arctan2(matrix[2, 1], -matrix[1, 0]),
+ u = [np.arctan2(matrix[2, 0], -matrix[1, 0]),
np.arccos(matrix[0, 0]),
np.arctan2(matrix[0, 2], matrix[0, 1])]
elif axes == 'sxzy':
u = [np.arctan2(-matrix[2, 1], matrix[1, 1]),
- np.arcsin(matrix[0, 0]),
+ np.arcsin(matrix[0, 1]),
np.arctan2(-matrix[0, 2], matrix[0, 0])]
elif axes == 'syxy':
u = [np.arctan2(matrix[0, 1], -matrix[2, 1]),
np.arccos(matrix[1, 1]),
- np.arctan2(-matrix[1, 0], matrix[1, 2])]
+ np.arctan2(matrix[1, 0], matrix[1, 2])]
elif axes == 'syxz':
u = [np.arctan2(-matrix[0, 2], matrix[2, 2]),
np.arcsin(matrix[1, 2]),
diff --git a/navipy/maths/test_euler.py b/navipy/maths/test_euler.py
index 19857ec20ffcb851461c5704d9c568374f007cc1..14b892ee21e95eb4bdc95b403f541ef6f8a3f879 100644
--- a/navipy/maths/test_euler.py
+++ b/navipy/maths/test_euler.py
@@ -13,6 +13,15 @@ s = np.sin
class TestEuler(unittest.TestCase):
+ # def test_identity(self):
+ # for key in list(_AXES2TUPLE.keys()):
+ # angles = np.zeros(3)
+ # rotation_0 = euler.matrix(angles[0],
+ # angles[1],
+ # angles[2], key)[:3, :3]
+ # [ai, aj, ak] = euler.from_matrix(rotation_0, key)
+ # np.testing.assert_allclose(angles, np.array([ai, aj, ak]))
+ # NOT WORKING DUE TO NUMERICAL APPROX...
def test_forwardreverse(self):
"""
tests if the matrix and from_matrix (decompose) function
@@ -23,35 +32,31 @@ class TestEuler(unittest.TestCase):
- use obtained angles to build euler matrix (euler.matrix)
- compare old and new euler matrix
"""
- condition = dict()
for key in list(_AXES2TUPLE.keys()):
- # print("key", key)
- rotation_0 = euler.matrix(1, -1, 3, key)[:3, :3]
- # print("roation_0", rotation_0)
+ angles = np.pi * (np.random.rand(3) - 0.5)
+ angles[0] *= 2
+ angles[2] *= 2
+ if np.unique(list(key)).shape[0] < len(list(key)):
+ # Repeting axis
+ angles[1] += np.pi / 2
+ # angles *= 0
+ rotation_0 = euler.matrix(angles[0],
+ angles[1],
+ angles[2], key)[:3, :3]
[ai, aj, ak] = euler.from_matrix(rotation_0, key)
- # print("res", ai, aj, ak)
- rotation_1 = euler.matrix(ai, aj, ak, key)[:3, :3]
- condition[key] = np.allclose(rotation_0,
- rotation_1)
- if condition[key] is False:
- print('axes', key, 'failed from matrix')
- self.assertTrue(np.all(list(condition.values())))
+ np.testing.assert_allclose(angles, np.array([ai, aj, ak]))
def test_betweenconvention_new(self):
"""
Test orientation from one convention to another
"""
- condition = dict()
- refkey = 'rxyz'
+ refkey = 'sxyz'
for key in list(_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:
- print('axes', key, 'failed between')
- self.assertTrue(np.all(np.array(condition.values())))
+ np.testing.assert_allclose(rotation_0,
+ rotation_1)
def test_from_quaternion(self):
angles = euler.from_quaternion([0.99810947, 0.06146124, 0, 0])
@@ -107,54 +112,18 @@ class TestEuler(unittest.TestCase):
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'),
+ for a, b, c, d, e, f, g in [(None, 2, 6, 8, 7, 8, 'sxyz'),
(9.0, 'er', 2, 3, 3, 5, 'sxyz'),
- (5.0, 4.0, None, 8.0, 8.0, 4.0, 'rxyz'),
- (np.nan, 8.0, 7.0, '0', 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, None, 8.0, 8.0, 4.0, 'sxyz'),
+ (np.nan, 8.0, 7.0, '0', 6.0, 9.0, 'sxyz'),
+ (4.0, 2.0, 1.0, 3.0, 'w', 2.0, 'sxyz'),
+ (1.0, 2.0, 3.0, 4.0, None, 4, 'sxyz')]:
with self.assertRaises(TypeError):
euler.angular_velocity(a, b, c, d, e, f, g)
with self.assertRaises(ValueError):
euler.angular_velocity(5.0, 4.0, 3.0, 8.0, 8.0, 4.0, 'l')
- def test_rotMatrix(self):
- """the stationary rotation matrix should be
- the transpose of the rotational
- with the same axis order"""
- ai = 0.10
- aj = 0.20
- ak = 0.70
- 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)))
-
- def test_convertE(self):
- """
- the rotation matrix can be obtained by Rijk=E'ijk*(Eijk)^(-1)
- where E is the angular rate matrix for stationary frames, and
- E' is the angular rate matrix for rotational frames
- and Rijk^T = Eijk*(E'ijk)^(-1)
- """
- ai = 0.10
- aj = 0.20
- ak = 0.70
- # dai = 12
- # daj = 0
- # dak = 0
- 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)
- newR = np.dot(E_p, E_inv)
- self.assertTrue(np.allclose(Rijk, newR))
- E_p_inv = np.linalg.inv(E_p)
- newR = np.dot(E, E_p_inv)
- self.assertTrue(np.allclose(Rijk_r, newR))
-
def test_E313(self):
"""
example from the attitude paper
@@ -165,15 +134,10 @@ class TestEuler(unittest.TestCase):
# dai = 0
# daj = 0
# dak = 1
- E_p = angle_rate_matrix(ai, aj, ak, 'rzxz')
E = angle_rate_matrix(ai, aj, ak, 'szxz')
testE = [[np.sin(aj) * np.sin(ak), np.cos(ak), 0],
[-np.sin(aj) * np.cos(ak), np.sin(ak), 0],
[np.cos(aj), 0, 1]]
- testE_p = [[0, np.cos(ai), np.sin(aj) * np.sin(ai)],
- [0, -np.sin(ai), np.sin(aj) * np.cos(ai)],
- [1, 0, np.cos(aj)]]
- self.assertTrue(np.allclose(E_p, testE_p))
self.assertTrue(np.allclose(E, testE))
def test_velocity_paper_example(self):
@@ -192,9 +156,7 @@ class TestEuler(unittest.TestCase):
[0, -np.sin(ai), np.sin(aj) * np.cos(ai)],
[1, 0, np.cos(aj)]]
E_fkt = angle_rate_matrix(ai, aj, ak, 'szxz')
- E_p_fkt = angle_rate_matrix(ai, aj, ak, 'rzxz')
self.assertTrue(np.allclose(E, E_fkt))
- self.assertTrue(np.allclose(E_p, E_p_fkt))
w = np.dot(E, [dai, daj, dak])
w_p = np.dot(E_p, [dai, daj, dak])
Rijk = [[c(ai) * c(ak) - s(ai) * c(aj) * s(ak),
@@ -206,43 +168,13 @@ class TestEuler(unittest.TestCase):
[s(aj) * s(ak),
-s(aj) * c(ak),
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')[: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))
new_w_p = np.dot(Rijk, w)
- new_w = np.dot(Rijk_p, w_p)
- self.assertTrue(np.allclose(w, new_w))
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')[: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_angle_rate(self):
ea = 0.1
eb = 0.15
diff --git a/navipy/trajectories/test_markers.py b/navipy/trajectories/test_markers.py
index abe4e1bef9e2fd928635ddc702b1cacdff852c0e..d33807ce000411d362f9dfabf695a30a4c3ed4d3 100644
--- a/navipy/trajectories/test_markers.py
+++ b/navipy/trajectories/test_markers.py
@@ -88,7 +88,7 @@ class TestMarkersTransform(unittest.TestCase):
apex0 = self.random_apex()
apex1 = self.random_apex()
apex2 = self.random_apex()
- euler_axes = 'rxyz'
+ euler_axes = 'sxyz'
for triangle_mode in mtf._modes:
scale, shear, angles, translate, perspective =\
mtf.markers2decompose(