Update two marker transform to restrict misusage.

Not all convention can be used with this transform

navipy/trajectories/test_markers.py

@@ -4,6 +4,7 @@ import pandas as pd
 from navipy.maths.homogeneous_transformations import compose_matrix
 import navipy.trajectories.transformations as mtf
 from navipy.trajectories.triangle import Triangle
+from navipy.maths.constants import _AXES2TUPLE
 
 
 class TestMarkersTransform(unittest.TestCase):
@@ -99,17 +100,15 @@ class TestMarkersTransform(unittest.TestCase):
 
     def test_twomarker2euler(self):
         mark0 = pd.Series(data=0, index=['x', 'y', 'z'])
-        for axis_alignement, euler_axes in zip(
-                ['x-axis', 'z-axis', 'y-axis'],
-                ['zyx', 'yxz', 'zxy']):
+        for axis_alignement, euler_axes in zip(['x-axis', 'z-axis', 'y-axis'],
+                                               ['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]
+            index = euler_axes.find(axis_alignement[0])
+            if index < 0:
+                continue
+            known_angle = angles[index].copy()
 
             triangle_mode = 'x-axis=median-from-0'
             transform = compose_matrix(angles=angles,
@@ -128,7 +127,8 @@ 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_allclose(angles, angles_estimate, rtol=1e-03)
+            np.testing.assert_allclose(
+                angles, angles_estimate, rtol=1e-02, atol=1e-02)
 
 
 if __name__ == '__main__':

navipy/trajectories/transformations.py

@@ -3,6 +3,8 @@
 import numpy as np
 from navipy.trajectories.triangle import Triangle
 import navipy.maths.homogeneous_transformations as ht
+import navipy.maths.euler as euler
+import navipy.maths.quaternion as qt
 from scipy.optimize import minimize
 
 _modes = []
@@ -282,13 +284,12 @@ def twomarkers2euler_score(x, mark0, mark1,
     matrix = ht.compose_matrix(
         angles=angles, axes=axes_convention)
     _, _, angles, _, _ = ht.decompose_matrix(matrix, axes=euler_axes)
-
     return np.nanmean((angles - known_angles)**2)
 
 
 def twomarkers2euler(mark0, mark1, axis_alignement,
                      known_angle, euler_axes,
-                     method='Nelder-Mead', tol=1e-8):
+                     method='SLSQP', 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.
@@ -302,11 +303,24 @@ def twomarkers2euler(mark0, mark1, axis_alignement,
     known_angles = np.nan * np.zeros(3)
     # Find the first rotation axis
     index = euler_axes.find(axis_alignement[0])
-    if index < 1:
+    if index < 0:
         raise KeyError(
             'Axis aligment {} can not work with euler_axes {}'.format(
                 axis_alignement, euler_axes))
-    known_angles[index - 1] = known_angle
+    if (axis_alignement == 'x-axis') and (euler_axes != 'zyx'):
+        msg = 'When x-axis is known, euler_axis zyx should be used\n'
+        msg += 'Other convention have not been implemented, sorry'
+        raise NameError(msg)
+    elif (axis_alignement == 'z-axis') and (euler_axes != 'yxz'):
+        msg = 'When z-axis is known, euler_axis yxz should be used\n'
+        msg += 'Other convention have not been implemented, sorry'
+        raise NameError(msg)
+    elif (axis_alignement == 'y-axis') and (euler_axes != 'zxy'):
+        msg = 'When y-axis is known, euler_axis zxy should be used\n'
+        msg += 'Other convention have not been implemented, sorry'
+        raise NameError(msg)
+
+    known_angles[index] = known_angle
     args = (mark0, mark1, axis_alignement, known_angles, euler_axes)
     res = minimize(twomarkers2euler_score, x0=known_angle, args=args,
                    method=method, tol=tol)