diff --git a/navipy/maths/homogeneous_transformations.py b/navipy/maths/homogeneous_transformations.py index e815c535ee071aa71358430346ae0abaafb1e6da..d0db3a15c187457cc6747d0655e5500fc16213e0 100644 --- a/navipy/maths/homogeneous_transformations.py +++ b/navipy/maths/homogeneous_transformations.py @@ -70,61 +70,6 @@ def reflection_from_matrix(matrix): return point, normal -def rotation_matrix(angle, direction, point=None): - """Return matrix to rotate about axis defined by point and direction. - """ - sina = np.sin(angle) - cosa = np.cos(angle) - direction = unit_vector(direction[:3]) - # rotation matrix around unit vector - R = np.diag([cosa, cosa, cosa]) - R += np.outer(direction, direction) * (1.0 - cosa) - direction *= sina - R += np.array([[0.0, -direction[2], direction[1]], - [direction[2], 0.0, -direction[0]], - [-direction[1], direction[0], 0.0]]) - M = np.identity(4) - M[:3, :3] = R - if point is not None: - # rotation not around origin - point = np.array(point[:3], dtype=np.float64, copy=False) - M[:3, 3] = point - np.dot(R, point) - return M - - -def rotation_from_matrix(matrix): - """Return rotation angle and axis from rotation matrix. - """ - R = np.array(matrix, dtype=np.float64, copy=False) - R33 = R[:3, :3] - # direction: unit eigenvector of R33 corresponding to eigenvalue of 1 - w, W = np.linalg.eig(R33.T) - i = np.where(abs(np.real(w) - 1.0) < 1e-8)[0] - if not len(i): - raise ValueError("no unit eigenvector corresponding to eigenvalue 1") - direction = np.real(W[:, i[-1]]).squeeze() - # point: unit eigenvector of R33 corresponding to eigenvalue of 1 - w, Q = np.linalg.eig(R) - i = np.where(abs(np.real(w) - 1.0) < 1e-8)[0] - if not len(i): - raise ValueError("no unit eigenvector corresponding to eigenvalue 1") - point = np.real(Q[:, i[-1]]).squeeze() - point /= point[3] - # rotation angle depending on direction - cosa = (np.trace(R33) - 1.0) / 2.0 - if abs(direction[2]) > 1e-8: - sina = (R[1, 0] + (cosa - 1.0) * direction[0] - * direction[1]) / direction[2] - elif abs(direction[1]) > 1e-8: - sina = (R[0, 2] + (cosa - 1.0) * direction[0] - * direction[2]) / direction[1] - else: - sina = (R[2, 1] + (cosa - 1.0) * direction[1] - * direction[2]) / direction[0] - angle = np.arctan2(sina, cosa) - return angle, direction, point - - def scale_matrix(factor, origin=None, direction=None): """Return matrix to scale by factor around origin in direction. Use factor -1 for point symmetry. @@ -427,3 +372,13 @@ def is_same_transform(matrix0, matrix1): matrix1 = np.array(matrix1, dtype=np.float64, copy=True) matrix1 /= matrix1[3, 3] return np.allclose(matrix0, matrix1) + + +def testing_is_same_transform(matrix0, matrix1): + """Return True if two matrices perform same transformation. + """ + matrix0 = np.array(matrix0, dtype=np.float64, copy=True) + matrix0 /= matrix0[3, 3] + matrix1 = np.array(matrix1, dtype=np.float64, copy=True) + matrix1 /= matrix1[3, 3] + np.testing.assert_allclose(matrix0, matrix1) diff --git a/navipy/maths/quaternion.py b/navipy/maths/quaternion.py index ef0d018b41743a53eec239917bb1024dd6b2dba4..51ef70fef44ce5c42c6f24bd30cf46224c6b0f39 100644 --- a/navipy/maths/quaternion.py +++ b/navipy/maths/quaternion.py @@ -5,7 +5,6 @@ import numpy as np import navipy.maths.constants as constants from navipy.maths.tools import vector_norm -# import navipy.maths.new_euler as new_euler def qat(a, n): @@ -43,8 +42,6 @@ def from_euler(ai, aj, ak, axes='xyz'): Vectors." (2006): p. 18 (6.14) """ - # shouldnt there be a difference between rotating - # and stationary frame? vects = [[1, 0, 0], [0, 1, 0], [0, 0, 1]] r, i, j, k = constants._AXES2TUPLE[axes] q1 = qat(ai, vects[i]) @@ -52,12 +49,16 @@ def from_euler(ai, aj, ak, axes='xyz'): q3 = qat(ak, vects[k]) quaternion = multiply(np.array(q1), np.array(q2)) qijk = multiply(quaternion, np.array(q3)) - # qijk = np.dot(np.dot(np.transpose(q1), q2), np.transpose(q3)) return qijk def about_axis(angle, axis): """Return quaternion for rotation about axis. + + ..ref: James Diebel + "Representing Attitude: Euler Angles, Unit Quaternions, and Rotation + Vectors." + (2006): p. 17 (6.4), equation 175 """ q = np.array([0.0, axis[0], axis[1], axis[2]]) qlen = vector_norm(q) @@ -67,11 +68,9 @@ def about_axis(angle, axis): return q -def matrix(quaternion, axes='xyz'): +def matrix(quaternion): """Return homogeneous rotation matrix from quaternion. :param quaternion : vector with at least 3 entrences (unit quaternion) - :param axes: string that encodes the order of the axes and - whether rotational or stationary axes should be used :returns: a matrix :rtype: (np.ndarray) ..ref: James Diebel @@ -80,48 +79,27 @@ def matrix(quaternion, axes='xyz'): (2006): p. 15 (6.4) """ q = np.array(quaternion, dtype=np.float64, copy=True) - # n = np.dot(q, q) - # if n < constants._EPS: - # return np.identity(4) - # q *= np.sqrt(2.0 / n) - # print("within q2", q) - # q = np.outer(q, q) - # print("within q3", q) - # return np.array([ - # [1.0 - q[2, 2] - q[3, 3], q[1, 2] - q[3, 0], - # q[1, 3] + q[2, 0], 0.0], - # [ q[1, 2] + q[3, 0], 1.0 - q[1, 1] - q[3, 3], - # q[2, 3] - q[1, 0], 0.0], - # [ q[1, 3] - q[2, 0], q[2, 3] + q[1, 0], - # 1.0 - q[1, 1] - q[2, 2], 0.0], - # [0.0, 0.0, 0.0, 1.0]]) - r, _, _, _ = constants._AXES2TUPLE[axes] - # q=q[0] - if np.sqrt(q[0]*q[0]+q[1]*q[1]+q[2]*q[2]+q[3]*q[3]) != 1: - q = q/np.sqrt(q[0]*q[0]+q[1]*q[1]+q[2]*q[2]+q[3]*q[3]) - mat = np.array([[q[0]*q[0] + q[1]*q[1] - q[2]*q[2] - q[3]*q[3], - 2*q[1] * q[2] + 2*q[0]*q[3], - 2*q[1]*q[3] - 2*q[0] * q[2], 0], - [2 * q[1] * q[2] - 2 * q[0] * q[3], - q[0]*q[0] - q[1]*q[1] + q[2]*q[2] - q[3]*q[3], - 2 * q[2] * q[3] + 2 * q[0] * q[1], 0], - [2 * q[1] * q[3] + 2 * q[0] * q[2], - 2 * q[2] * q[3] - 2 * q[0] * q[1], - q[0] * q[0] - q[1] * q[1] - q[2] * q[2] + q[3]*q[3], 0], - [0, 0, 0, 1]]) - if not r: - return mat - else: - return np.transpose(mat) - - -def from_matrix(matrix, isprecise=False): + qnorm = np.sqrt(q[0]**2+q[1]**2+q[2]**2+q[3]**2) + if qnorm != 1: + q = q/qnorm + q0 = q[0] + q1 = q[1] + q2 = q[2] + q3 = q[3] + + mat = np.identity(4) + # According to equation 125 + mat[:3, :3] = np.array([ + [q0**2 + q1**2 - q2**2 - q3**2, 2*q1*q2 + 2*q0*q3, 2*q1*q3 - 2*q0*q2], + [2*q1*q2 - 2*q0*q3, q0**2 - q1**2 + q2**2 - q3**2, 2*q2*q3 + 2*q0*q1], + [2*q1*q3 + 2*q0*q2, 2*q2*q3 - 2*q0*q1, q0**2 - q1**2 - q2**2 + q3**2]]) + return mat + + +def from_matrix(matrix): """Return quaternion from rotation matrix. - If isprecise is True, the input matrix is assumed to be a precise rotation - matrix and a faster algorithm is used. + :param matrix: a rotation matrix - :param axes: string that encodes the order of the axes and - whether rotational or stationary axes should be used :returns: a vector :rtype: (np.ndarray) ..ref: James Diebel @@ -130,8 +108,9 @@ def from_matrix(matrix, isprecise=False): (2006): p. 15 (6.5) """ r = matrix - + # Split cases according to equation 145 if r[1, 1] > -r[2, 2] or r[0, 0] > -r[1, 1] or r[0, 0] > -r[2, 2]: + # equation 141 return (1/2)*np.array([np.sqrt(1 + r[0, 0] + r[1, 1] + r[2, 2]), (r[1, 2] - r[2, 1]) / np.sqrt(1 + r[0, 0] + r[1, 1] + r[2, 2]), @@ -141,6 +120,7 @@ def from_matrix(matrix, isprecise=False): np.sqrt(1 + r[0, 0] + r[1, 1] + r[2, 2])]) if r[1, 1] < -r[2, 2] or r[0, 0] > r[1, 1] or r[0, 0] > r[2, 2]: + # equation 142 return (1/2)*np.array([(r[1, 2] - r[2, 1]) / np.sqrt(1 + r[0, 0] - r[1, 1] - r[2, 2]), np.sqrt(1 + r[0, 0] - r[1, 1] - r[2, 2]), @@ -149,6 +129,7 @@ def from_matrix(matrix, isprecise=False): (r[2, 0] + r[0, 2]) / np.sqrt(1 + r[0, 0] - r[1, 1] - r[2, 2])]) if r[1, 1] > r[2, 2] or r[0, 0] < r[1, 1] or r[0, 0] < -r[2, 2]: + # equation 143 return (1/2)*np.array([(r[2, 0] - r[0, 2]) / np.sqrt(1 - r[0, 0] + r[1, 1] - r[2, 2]), (r[0, 1] + r[1, 0]) / @@ -157,6 +138,7 @@ def from_matrix(matrix, isprecise=False): (r[1, 2] + r[2, 1]) / np.sqrt(1 - r[0, 0] + r[1, 1] - r[2, 2])]) if r[1, 1] < r[2, 2] or r[0, 0] < -r[1, 1] or r[0, 0] < r[2, 2]: + # equation 144 return (1/2)*np.array([(r[0, 1] - r[1, 0]) / np.sqrt(1 - r[0, 0] - r[1, 1] + r[2, 2]), (r[2, 0] + r[0, 2]) / @@ -165,52 +147,6 @@ def from_matrix(matrix, isprecise=False): np.sqrt(1 - r[0, 0] - r[1, 1] + r[2, 2]), np.sqrt(1 - r[0, 0] - r[1, 1] + r[2, 2])]) - """ - M = np.array(matrix, dtype=np.float64, copy=False)[:4, :4] - if isprecise: - q = np.empty((4, )) - t = np.trace(M) - if t > M[3, 3]: - q[0] = t - q[3] = M[1, 0] - M[0, 1] - q[2] = M[0, 2] - M[2, 0] - q[1] = M[2, 1] - M[1, 2] - else: - i, j, k = 1, 2, 3 - if M[1, 1] > M[0, 0]: - i, j, k = 2, 3, 1 - if M[2, 2] > M[i, i]: - i, j, k = 3, 1, 2 - t = M[i, i] - (M[j, j] + M[k, k]) + M[3, 3] - q[i] = t - q[j] = M[i, j] + M[j, i] - q[k] = M[k, i] + M[i, k] - q[3] = M[k, j] - M[j, k] - q *= 0.5 / np.sqrt(t * M[3, 3]) - else: - m00 = M[0, 0] - m01 = M[0, 1] - m02 = M[0, 2] - m10 = M[1, 0] - m11 = M[1, 1] - m12 = M[1, 2] - m20 = M[2, 0] - m21 = M[2, 1] - m22 = M[2, 2] - # symmetric matrix K - K = np.array([[m00 - m11 - m22, 0.0, 0.0, 0.0], - [m01 + m10, m11 - m00 - m22, 0.0, 0.0], - [m02 + m20, m12 + m21, m22 - m00 - m11, 0.0], - [m21 - m12, m02 - m20, m10 - m01, m00 + m11 + m22]]) - K /= 3.0 - # quaternion is eigenvector of K that corresponds to largest eigenvalue - w, V = np.linalg.eigh(K) - q = V[[3, 0, 1, 2], np.argmax(w)] - if q[0] < 0.0: - np.negative(q, q) - return q - """ - def multiply(quaternion1, quaternion0): """Return multiplication of two quaternions. diff --git a/navipy/maths/test_euler.py b/navipy/maths/test_euler.py index 26f2506502aa68b5b46f4ddc412d678d1775283a..caa453583cdc872ce2c114afb48f22d16e5968e7 100644 --- a/navipy/maths/test_euler.py +++ b/navipy/maths/test_euler.py @@ -60,7 +60,7 @@ class TestEuler(unittest.TestCase): def test_from_quaternion(self): angles = euler.from_quaternion([0.99810947, 0.06146124, 0, 0]) - self.assertTrue(np.allclose(angles, [0.123, 0, 0])) # 0.123 + np.testing.assert_allclose(angles, [0.123, 0, 0]) # 0.123 def test_from_quaternion_params(self): """ diff --git a/navipy/maths/test_homogeneous_transformations.py b/navipy/maths/test_homogeneous_transformations.py index 805ca8aba2460539f5790f11b3c139a15429950b..babb4280aec4489c37d528a42ec0c56ee9ac2839 100644 --- a/navipy/maths/test_homogeneous_transformations.py +++ b/navipy/maths/test_homogeneous_transformations.py @@ -50,59 +50,6 @@ class TestHT(unittest.TestCase): condition = ht.is_same_transform(matrix_0, matrix_1) self.assertTrue(condition) - def test_rotation_matrix_example(self): - rotation = ht.rotation_matrix(np.pi / 2, - [0, 0, 1], - [1, 0, 0]) - condition = np.allclose(np.dot(rotation, - [0, 0, 0, 1]), - [1, -1, 0, 1]) - self.assertTrue(condition) - - def test_rotation_matrix_periodicity(self): - angle = (np.random.random() - 0.5) * (2 * np.pi) - direc = np.random.random(3) - 0.5 - point = np.random.random(3) - 0.5 - rotation_0 = ht.rotation_matrix(angle, direc, point) - rotation_1 = ht.rotation_matrix(angle - 2 * np.pi, direc, point) - condition = ht.is_same_transform(rotation_0, rotation_1) - self.assertTrue(condition) - - def test_rotation_matrix_opposite(self): - angle = (np.random.random() - 0.5) * (2 * np.pi) - direc = np.random.random(3) - 0.5 - point = np.random.random(3) - 0.5 - rotation_0 = ht.rotation_matrix(angle, direc, point) - rotation_1 = ht.rotation_matrix(-angle, -direc, point) - condition = ht.is_same_transform(rotation_0, rotation_1) - self.assertTrue(condition) - - def test_rotation_matrix_identity(self): - direc = np.random.random(3) - 0.5 - identity = np.identity(4) - condition = np.allclose(identity, - ht.rotation_matrix(np.pi * 2, direc)) - self.assertTrue(condition) - - def test_rotation_matrix_trace(self): - direc = np.random.random(3) - 0.5 - point = np.random.random(3) - 0.5 - condition = np.allclose(2, - np.trace(ht.rotation_matrix(np.pi / 2, - direc, - point))) - self.assertTrue(condition) - - def test_rotation_from_matrix(self): - angle = (np.random.random() - 0.5) * (2 * np.pi) - direc = np.random.random(3) - 0.5 - point = np.random.random(3) - 0.5 - rotation_0 = ht.rotation_matrix(angle, direc, point) - angle, direc, point = ht.rotation_from_matrix(rotation_0) - rotation_1 = ht.rotation_matrix(angle, direc, point) - self.assertTrue(ht.is_same_transform(rotation_0, - rotation_1)) - def test_scale(self): vector = (np.random.rand(4, 5) - 0.5) * 20 vector[3] = 1 diff --git a/navipy/maths/test_quaternion.py b/navipy/maths/test_quaternion.py index 19a51d479c2dc34697202eaf7de7a555fa7984f2..93f868e4f85985e8894c31bcd3576cfb189c1d21 100644 --- a/navipy/maths/test_quaternion.py +++ b/navipy/maths/test_quaternion.py @@ -18,10 +18,11 @@ class TestQuaternions(unittest.TestCase): [0.99810947, 0.06146124, 0, 0])) def test_matrix(self): - matrix = quat.matrix([0.99810947, 0.06146124, 0, 0], 'xyz') - self.assertTrue(np.allclose(matrix, - ht.rotation_matrix(0.123, - [1, 0, 0]))) + quaternion = np.random.rand(4) + quaternion /= np.sqrt(np.sum(quaternion**2)) + matrix = quat.matrix(quaternion) + quaternion_fm = quat.from_matrix(matrix) + np.testing.assert_allclose(quaternion_fm, quaternion) def test_matrix_identity(self): matrix = quat.matrix([1, 0, 0, 0]) @@ -32,7 +33,7 @@ class TestQuaternions(unittest.TestCase): 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) + quaternion = quat.from_matrix(np.identity(4)) self.assertTrue(np.allclose(quaternion, [1, 0, 0, 0])) def test_from_matrix_diagonal(self): @@ -43,21 +44,8 @@ class TestQuaternions(unittest.TestCase): 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))) - - # Seem to fail from time to time.... - # def test_quaternion_multiply(self): - # rotation_0 = random.rotation_matrix() - # quaternion_0 = quat.from_matrix(rotation_0) - # rotation_1 = random.rotation_matrix() - # quaternion_1 = quat.from_matrix(rotation_1) - # quaternion_01 = quat.multiply(quaternion_0, - # quaternion_1) - # rotation_01 = rotation_0.dot(rotation_1) - # quaternion_01_fromrot = quat.from_matrix(rotation_01) - # self.assertTrue(np.allclose(quaternion_01, - # quaternion_01_fromrot)) + ht.testing_is_same_transform( + rotation, quat.matrix(quaternion)) def test_inverse(self): quaternion_0 = random.quaternion() @@ -66,23 +54,6 @@ class TestQuaternions(unittest.TestCase): quat.multiply(quaternion_0, quaternion_1), [1, 0, 0, 0])) - 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,) - rotation = rotx.dot(roty.dot(rotz)) - rotation = np.transpose(rotation) - - quatx = quat.about_axis(alpha, xaxis) - quaty = quat.about_axis(beta, yaxis) - quatz = quat.about_axis(gamma, zaxis) - quaternion = quat.multiply(quatx, quaty) - quaternion = quat.multiply(quaternion, quatz) - rotq = quat.matrix(quaternion) - self.assertTrue(ht.is_same_transform(rotation, rotq)) - if __name__ == '__main__': unittest.main() diff --git a/navipy/trajectories/test_markers.py b/navipy/trajectories/test_markers.py index 1b2275831e138fa72321ffd767809adadf01ff33..4027e9206dc2b0bed63f65d33b2345b6571d443c 100644 --- a/navipy/trajectories/test_markers.py +++ b/navipy/trajectories/test_markers.py @@ -98,15 +98,19 @@ class TestMarkersTransform(unittest.TestCase): np.testing.assert_almost_equal(perspective, [0, 0, 0, 1]) def test_twomarker2euler(self): - angles = np.pi * (np.random.rand(3) - 0.5) - angles[0] *= 2 - # angle[1] is not multipliy because in range [-pi/2,pi/2] - angles[2] *= 2 mark0 = pd.Series(data=0, index=['x', 'y', 'z']) - for axis_alignement, euler_axes, known_angle in zip( + for axis_alignement, euler_axes in zip( ['x-axis', 'z-axis', 'y-axis'], - ['zyx', 'yxz', 'zxy'], - [angles[2], angles[2], angles[2]]): + ['zyx', 'yxz', 'zxy']): + angles = np.pi * (np.random.rand(3) - 0.5) + angles[0] *= 2 + angles[2] *= 2 + if np.unique(list(euler_axes)).shape[0] < len(list(euler_axes)): + # Repeting axis + angles[1] += np.pi / 2 + # no repeting between [-pi/2,pi/2] + known_angle = angles[2] + triangle_mode = 'x-axis=median-from-0' transform = compose_matrix(angles=angles, axes=euler_axes) @@ -124,7 +128,7 @@ class TestMarkersTransform(unittest.TestCase): mark1 = pd.Series(z_axis, index=['x', 'y', 'z']) angles_estimate = mtf.twomarkers2euler( mark0, mark1, axis_alignement, known_angle, euler_axes) - np.testing.assert_almost_equal(angles, angles_estimate) + np.testing.assert_allclose(angles, angles_estimate, rtol=1e-03) if __name__ == '__main__': diff --git a/navipy/trajectories/test_triangle.py b/navipy/trajectories/test_triangle.py index b76cc9241a3d30752b9274589e4f5456e0320669..fee57f400d4648b96b9e0aa2d44c8b3ed13d7d1e 100644 --- a/navipy/trajectories/test_triangle.py +++ b/navipy/trajectories/test_triangle.py @@ -76,7 +76,7 @@ class TestTriangle(unittest.TestCase): triangle = Triangle(self.apex0, self.apex1, self.apex2) cm = triangle.center_of_mass() cm_test = (self.apex0 + self.apex1 + self.apex2) / 3 - self.assertTrue(np.allclose(cm, cm_test)) + np.testing.assert_allclose(cm, cm_test) def test_apexes2vectors(self): """The vectors are from one apex to the other in a directed \ @@ -85,7 +85,7 @@ should be zero """ triangle = Triangle(self.apex0, self.apex1, self.apex2) vec = triangle.apexes2vectors() - self.assertTrue(np.allclose(vec.sum(axis=1).data, [0, 0, 0])) + np.testing.assert_allclose(vec.sum(axis=1).values, [0, 0, 0]) def test_apexes2edges_norm(self): triangle = Triangle(self.apex0, self.apex1, self.apex2) diff --git a/navipy/trajectories/transformations.py b/navipy/trajectories/transformations.py index e3d0631d16b2ca3e8fcf4aea546fb19a78c03823..2bcf95ccb73570b2382ec725f0ed2c62815871b9 100644 --- a/navipy/trajectories/transformations.py +++ b/navipy/trajectories/transformations.py @@ -288,7 +288,7 @@ def twomarkers2euler_score(x, mark0, mark1, def twomarkers2euler(mark0, mark1, axis_alignement, known_angle, euler_axes, - method='Nelder-Mead', tol=1e-6): + method='Nelder-Mead', tol=1e-8): """ Determine euler angles from two markers and a known angle. The known angle is assumed to be the one around the axis aligment.