Merge pull request #651 from EveCharbie/corrections_track_vectors

Correction to TRACK_VECTOR_ORIENTAION_FROM_MARKERS

Author: pariterre

bioptim/limits/penalty.py

@@ -3,7 +3,7 @@
 import inspect
 
 import biorbd_casadi as biorbd
-from casadi import horzcat, vertcat, SX, Function, acos, norm_2, dot
+from casadi import horzcat, vertcat, SX, Function, atan2, dot, cross, sqrt
 
 from .penalty_option import PenaltyOption
 from .penalty_node import PenaltyNodeList
@@ -799,6 +799,7 @@ def track_vector_orientations_from_markers(
             The second vector is defined by vector_1_marker_1 - vector_1_marker_0.
             Note that is minimizes the angle between the two vectors, thus it is not possible ti specify an axis.
             By default, this function is quadratic, meaning that it minimizes the angle between the two vectors.
+            WARNING: please be careful as there is a discontinuity when the two vectors are orthogonal.
 
             Parameters
             ----------
@@ -839,10 +840,11 @@ def track_vector_orientations_from_markers(
 
             vector_0 = vector_0_marker_1_position - vector_0_marker_0_position
             vector_1 = vector_1_marker_1_position - vector_1_marker_0_position
+            cross_prod = cross(vector_0, vector_1)
+            cross_prod_norm = sqrt(cross_prod[0] ** 2 + cross_prod[1] ** 2 + cross_prod[2] ** 2)
+            out = atan2(cross_prod_norm, dot(vector_0, vector_1))
 
-            angle = acos(dot(vector_0, vector_1) ** 2 / (norm_2(vector_0) * norm_2(vector_1)))
-
-            angle_objective = nlp.mx_to_cx("vector_orientations_difference", angle, nlp.states["q"])
+            angle_objective = nlp.mx_to_cx("vector_orientations_difference", out, nlp.states["q"])
 
             return angle_objective
 

tests/test_global_align.py

@@ -118,7 +118,7 @@ def test_track_vector_orientation():
     # Check objective function value
     f = np.array(sol.cost)
     np.testing.assert_equal(f.shape, (1, 1))
-    np.testing.assert_almost_equal(f[0, 0], 1206.2849823554816)
+    np.testing.assert_almost_equal(f[0, 0], 2.614556858357712e-08)
 
     # Check constraints
     g = np.array(sol.constraints)
@@ -129,17 +129,17 @@ def test_track_vector_orientation():
     q, qdot, tau = sol.states["q"], sol.states["qdot"], sol.controls["tau"]
 
     # initial and final position
-    np.testing.assert_almost_equal(q[:, 0], np.array([0.80000001, -0.68299837, -1.57, -1.56999931]))
-    np.testing.assert_almost_equal(q[:, -1], np.array([0.80000001, -0.68299837, 1.57, 1.56999966]))
+    np.testing.assert_almost_equal(q[:, 0], np.array([0.80000001, -0.68299837, -1.57, -1.56999089]))
+    np.testing.assert_almost_equal(q[:, -1], np.array([0.80000001, -0.68299837, 1.57, 1.56999138]))
     # initial and final velocities
-    np.testing.assert_almost_equal(qdot[:, 0], np.array((-5.62939992e-16, 4.90499998e00, 3.14000113e00, 3.13999784e00)))
+    np.testing.assert_almost_equal(qdot[:, 0], np.array((3.37753150e-16, 4.90499995e00, 3.14001172e00, 3.13997056e00)))
     np.testing.assert_almost_equal(
-        qdot[:, -1], np.array((-5.62940009e-16, -4.90499998e00, 3.14000029e00, 3.13999868e00))
+        qdot[:, -1], np.array((3.37753131e-16, -4.90499995e00, 3.14001059e00, 3.13997170e00))
     )
     # initial and final controls
     np.testing.assert_almost_equal(
-        tau[:, 0], np.array([1.05886391e-25, 6.28215525e-09, -2.53398134e-06, 2.52582173e-06])
+        tau[:, 0], np.array([-1.27269674e-24, 1.97261036e-08, -3.01420965e-05, 3.01164220e-05])
     )
     np.testing.assert_almost_equal(
-        tau[:, -2], np.array([-6.38733211e-24, 6.28215525e-09, 1.31751960e-06, -1.30936553e-06])
+        tau[:, -2], np.array([-2.10041085e-23, 1.97261036e-08, 2.86303889e-05, -2.86047182e-05])
     )

tests/test_penalty.py

@@ -694,9 +694,9 @@ def test_penalty_minimize_vector_orientation(penalty_origin, value):
     res = get_penalty_value(ocp, penalty, t, x, u, [])
 
     if value == 0.1:
-        np.testing.assert_almost_equal(float(res), 1.0529423391195598)
+        np.testing.assert_almost_equal(float(res), 0.09999999999999999)
     else:
-        np.testing.assert_almost_equal(float(res), 1.2110674089002156)
+        np.testing.assert_almost_equal(float(res), 2.566370614359173)
 
 
 @pytest.mark.parametrize("penalty_origin", [ConstraintFcn])