Merge pull request #150 from ANaaim/add_full_kin_calculation

Author: Ipuch

bionc/bionc_numpy/inverse_kinematics.py

@@ -547,6 +547,8 @@ def sol(self):
             for ind_max in ind_max_joint_constraint_error
         ]
 
+        total_euler_angles = TimeSeriesUtils.total_euler_angles(self.model, self.Qopt)
+
         self.output = dict(
             objective_function=self.objective_function,
             success=self.success_optim,
@@ -558,6 +560,7 @@ def sol(self):
             segment_rigidity_residual_norm=segment_rigidity_residual_norm,
             total_rigidity_residuals=total_rigidity_residuals,
             max_rigidbody_violation=max_rigidbody_violation,
+            total_euler_angles=total_euler_angles,
         )
 
         if self.experimental_markers is not None:

bionc/bionc_numpy/time_series_utils.py

@@ -61,6 +61,19 @@ def marker_constraints_xyz(model, Q: np.ndarray, markers: np.ndarray):
     return marker_residuals_xyz
 
 
+def total_euler_angles(model, Q: np.ndarray):
+    """Compute Euler angle for all joint"""
+    Qi_temp = NaturalCoordinates(Q[:, 0])
+    euler_temp = model.natural_coordinates_to_joint_angles(Qi_temp)
+    total_euler_angles = np.zeros((euler_temp.shape[0], euler_temp.shape[1], Q.shape[1]))
+
+    for ind_frame in range(Q.shape[1]):
+        Qi = NaturalCoordinates(Q[:, ind_frame])
+        total_euler_angles[:, :, ind_frame] = model.natural_coordinates_to_joint_angles(Qi)
+
+    return total_euler_angles
+
+
 class TimeSeriesUtils:
     """
     This class contains utility functions to compute the constraints of a biomechanical model for
@@ -75,3 +88,4 @@ class TimeSeriesUtils:
     joint_constraints = joint_constraints
     total_marker_constraints = total_marker_constraints
     marker_constraints_xyz = marker_constraints_xyz
+    total_euler_angles = total_euler_angles

tests/test_ultimate_inverse_kinematics_heatmap.py

@@ -240,6 +240,7 @@ def test_global_heatmap_ik():
         "segment_rigidity_residual_norm",
         "total_rigidity_residuals",
         "max_rigidbody_violation",
+        "total_euler_angles",
         "total_heatmap_confidence",
         "heatmap_confidence_3d",
         "heatmap_confidence_2d",
@@ -268,6 +269,13 @@ def test_global_heatmap_ik():
     )
     np.testing.assert_almost_equal(solutions["heatmap_confidence_2d"], expected_heatmap_confidence_2d, decimal=1e-6)
 
+    expected_total_euler_angles = [
+        [[1.25975366e00, 1.24523863e00], [1.49707656e00, 1.47663685e00]],
+        [[-1.71958799e-01, -1.48109177e-01], [2.25847119e-13, 2.99721359e-12]],
+        [[-6.95863809e-01, -7.44889927e-01], [-1.42358347e-12, 8.85611862e-11]],
+    ]
+    np.testing.assert_almost_equal(solutions["total_euler_angles"], expected_total_euler_angles, decimal=1e-6)
+
 
 def test_error_Qinit_is_none():
     bionc = TestUtils.bionc_folder()