Merge pull request #74 from Ipuch/another_fext_xp

[huge PR] Projecting joint torques into euler basis, transformation matrices, projection of natural coordinates into minimal coordinates, euler joint torque on going, custom ik function, more examples, more tests.

.gitignore

@@ -128,3 +128,9 @@ dmypy.json
 *.c3d
 
 sandbox/
+
+.vscode/
+
+tests/pendulum.nmod
+
+examples/forward_dynamics/pendulum_3d.nmod

README.md

@@ -33,6 +33,9 @@ The easiest way to learn bionc is to dive into it.
 So let's build our first model.
 Please note that this tutorial is designed to recreate example which builds a lower limb model (Pelvis, Thigh, Shank, Foot). You can have look to [https://github.com/Ipuch/bioNC/blob/main/examples/model_creation/main.py](https://github.com/Ipuch/bioNC/blob/main/examples/model_creation/main.py)
 
+<img src="./docs/inverse_kinematics_viz.png" alt="Inverse kinematics" width="200"/>
+
+
 # Mathematical backends
 This toolbox support two mathematical backends: `numpy` and `casadi`.
 

bionc/__init__.py

@@ -31,14 +31,9 @@
 from .bionc_numpy.natural_accelerations import SegmentNaturalAccelerations, NaturalAccelerations
 from .bionc_numpy.homogenous_transform import HomogeneousTransform
 
-from .utils.enums import NaturalAxis, CartesianAxis
-
-from casadi.casadi import MX as MX_type
-from numpy import ndarray
-
-# global variable to store the type of the math interface
-casadi_type = MX_type
-numpy_type = ndarray
+from .utils.enums import NaturalAxis, CartesianAxis, EulerSequence, TransformationMatrixType
+from .utils.transformation_matrix import TransformationMatrixUtil
+from .utils.ode_solver import RK4, forward_integration
 
 from .vizualization import Viz
 from .bionc_numpy import InverseKinematics

bionc/bionc_casadi/__init__.py

@@ -15,3 +15,4 @@
 from .joint import Joint, GroundJoint
 from .natural_vector import NaturalVector
 from .external_force import ExternalForceList, ExternalForce
+from .cartesian_vector import vector_projection_in_non_orthogonal_basis

bionc/bionc_casadi/biomechanical_model.py

@@ -7,11 +7,21 @@
 from .natural_accelerations import NaturalAccelerations
 from ..protocols.biomechanical_model import GenericBiomechanicalModel
 from .external_force import ExternalForceList, ExternalForce
+from .rotations import euler_axes_from_rotation_matrices, euler_angles_from_rotation_matrix
+from .cartesian_vector import vector_projection_in_non_orthogonal_basis
 
 
 class BiomechanicalModel(GenericBiomechanicalModel):
     def __init__(self):
         super().__init__()
+        self._numpy_model = None
+
+    def set_numpy_model(self, numpy_model: GenericBiomechanicalModel):
+        self._numpy_model = numpy_model
+
+    @property
+    def numpy_model(self):
+        return self._numpy_model
 
     def save(self, filename: str):
         raise NotImplementedError("Saving a biomechanical model is not implemented yet with casadi models.")
@@ -121,7 +131,7 @@ def joint_constraints(self, Q: NaturalCoordinates) -> MX:
 
         Phi_k = MX.zeros(self.nb_joint_constraints)
         nb_constraints = 0
-        for joint_name, joint in self.joints.items():
+        for joint_name, joint in self.joints_with_constraints.items():
             idx = slice(nb_constraints, nb_constraints + joint.nb_constraints)
 
             Q_parent = (
@@ -147,7 +157,7 @@ def joint_constraints_jacobian(self, Q: NaturalCoordinates) -> np.ndarray:
 
         K_k = MX.zeros((self.nb_joint_constraints, Q.shape[0]))
         nb_constraints = 0
-        for joint_name, joint in self.joints.items():
+        for joint_name, joint in self.joints_with_constraints.items():
             idx_row = slice(nb_constraints, nb_constraints + joint.nb_constraints)
 
             idx_col_child = slice(
@@ -188,7 +198,7 @@ def joint_constraints_jacobian_derivative(self, Qdot: NaturalVelocities) -> MX:
 
         K_k_dot = MX.zeros((self.nb_joint_constraints, Qdot.shape[0]))
         nb_constraints = 0
-        for joint_name, joint in self.joints.items():
+        for joint_name, joint in self.joints_with_constraints.items():
             idx_row = slice(nb_constraints, nb_constraints + joint.nb_constraints)
 
             idx_col_parent = slice(
@@ -439,7 +449,7 @@ def holonomic_constraints(self, Q: NaturalCoordinates) -> MX:
         # it follows the order of the segments
         for i, segment in enumerate(self.segments_no_ground.values()):
             # add the joint constraints first
-            joints = self.joints_from_child_index(i)
+            joints = self.joints_from_child_index(i, remove_free_joints=True)
             if len(joints) != 0:
                 for j in joints:
                     idx = slice(nb_constraints, nb_constraints + j.nb_constraints)
@@ -485,7 +495,7 @@ def holonomic_constraints_jacobian(self, Q: NaturalCoordinates) -> MX:
         K = MX.zeros((self.nb_holonomic_constraints, 12 * self.nb_segments))
         for i, segment in enumerate(self.segments_no_ground.values()):
             # add the joint constraints first
-            joints = self.joints_from_child_index(i)
+            joints = self.joints_from_child_index(i, remove_free_joints=True)
             if len(joints) != 0:
                 for j in joints:
                     idx_row = slice(nb_constraints, nb_constraints + j.nb_constraints)
@@ -546,7 +556,7 @@ def holonomic_constraints_jacobian_derivative(self, Qdot: NaturalVelocities) ->
         Kdot = MX.zeros((self.nb_holonomic_constraints, 12 * self.nb_segments))
         for i in range(self.nb_segments):
             # add the joint constraints first
-            joints = self.joints_from_child_index(i)
+            joints = self.joints_from_child_index(i, remove_free_joints=True)
             if len(joints) != 0:
                 for j in joints:
                     idx_row = slice(nb_constraints, nb_constraints + j.nb_constraints)
@@ -802,3 +812,108 @@ def _inverse_dynamics_recursive_step(
         lambdas[:, segment_index] = lambda_i
 
         return visited_segments, torques, forces, lambdas
+
+    def express_joint_torques_in_euler_basis(self, Q: NaturalCoordinates, torques: MX) -> MX:
+        """
+        This function expresses the joint torques in the euler basis.
+
+        Parameters
+        ----------
+        Q: NaturalCoordinates
+            The generalized coordinates of the model
+        torques: np.ndarray
+            The joint torques in global coordinates system
+
+        Returns
+        -------
+        np.ndarray
+            The joint torques expressed in the euler basis
+        """
+        if torques.shape != (3, self.nb_segments):
+            raise ValueError(f"The shape of the joint torques must be (3, {self.nb_segments}) but is {torques.shape}")
+
+        euler_torques = MX.zeros((3, self.nb_segments))
+        for i, (joint_name, joint) in enumerate(self.joints.items()):
+            if joint.projection_basis is None:
+                raise RuntimeError(
+                    "The projection basis of the joint must be defined to express the torques in an Euler basis."
+                    f"Joint {joint_name} has no projection basis defined."
+                    f"Please define a projection basis for this joint, "
+                    f"using argument `projection_basis` of the joint constructor"
+                    f" and enum `EulerSequence` for the type of entry."
+                )
+
+            parent_segment = joint.parent
+            child_segment = joint.child
+
+            Q_parent = (
+                None if joint.parent is None else Q.vector(self.segments[joint.parent.name].index)
+            )  # if the joint is a joint with the ground, the parent is None
+            Q_child = Q.vector(child_segment.index)
+
+            # compute rotation matrix from Qi
+            R_parent = (
+                np.eye(3)
+                if joint.parent is None
+                else parent_segment.segment_coordinates_system(Q_parent, joint.parent_basis).rot
+            )
+            R_child = child_segment.segment_coordinates_system(Q_child, joint.child_basis).rot
+
+            e1, e2, e3 = euler_axes_from_rotation_matrices(
+                R_parent, R_child, sequence=joint.projection_basis, axes_source_frame="mixed"
+            )
+
+            # compute the euler torques
+            euler_torques[:, i] = vector_projection_in_non_orthogonal_basis(torques[:, i], e1, e2, e3)
+
+        return euler_torques
+
+    def natural_coordinates_to_joint_angles(self, Q: NaturalCoordinates) -> np.ndarray:
+        """
+        This function converts the natural coordinates to joint angles with Euler Sequences defined for each joint
+
+        Parameters
+        ----------
+        Q: NaturalCoordinates
+            The natural coordinates of the model
+
+        Returns
+        -------
+        np.ndarray
+            The joint angles [3 x nb_joints]
+        """
+        euler_angles = MX.zeros((3, self.nb_joints))
+
+        for i, (joint_name, joint) in enumerate(self.joints.items()):
+            if joint.projection_basis is None:
+                raise RuntimeError(
+                    "The projection basis of the joint must be defined to express the torques in an Euler basis."
+                    f"Joint {joint_name} has no projection basis defined."
+                    f"Please define a projection basis for this joint, "
+                    f"using argument `projection_basis` of the joint constructor"
+                    f" and enum `EulerSequence` for the type of entry."
+                )
+
+            parent_segment = joint.parent
+            child_segment = joint.child
+
+            Q_parent = (
+                None if joint.parent is None else Q.vector(self.segments[joint.parent.name].index)
+            )  # if the joint is a joint with the ground, the parent is None
+            Q_child = Q.vector(child_segment.index)
+
+            # compute rotation matrix from Qi
+            R_parent = (
+                np.eye(3)
+                if joint.parent is None
+                else parent_segment.segment_coordinates_system(Q_parent, joint.parent_basis).rot
+            )
+            R_child = child_segment.segment_coordinates_system(Q_child, joint.child_basis).rot
+
+            euler_angles[:, i] = euler_angles_from_rotation_matrix(
+                R_parent,
+                R_child,
+                joint_sequence=joint.projection_basis,
+            )
+
+        return euler_angles

bionc/bionc_casadi/cartesian_vector.py

@@ -1,4 +1,4 @@
-from casadi import MX
+from casadi import MX, sum1, cross
 import numpy as np
 from ..utils.enums import CartesianAxis
 
@@ -41,3 +41,56 @@ def axis(cls, axis: CartesianAxis):
             return cls(np.array([0, 1, 0]))
         elif axis == CartesianAxis.Z:
             return cls(np.array([0, 0, 1]))
+
+
+def vector_projection_in_non_orthogonal_basis(vector: np.ndarray | MX, e1: MX, e2: MX, e3: MX) -> MX:
+    """
+    This function converts a vector expressed in the global coordinate system
+    to a vector expressed in a non-orthogonal coordinate system.
+
+    Parameters
+    ----------
+    vector: np.ndarray | MX
+        The vector expressed in the global coordinate system
+    e1: MX
+        The first vector of the non-orthogonal coordinate system, usually the u-axis
+    e2: MX
+        The second vector of the non-orthogonal coordinate system, usually the v-axis
+    e3: MX
+        The third vector of the non-orthogonal coordinate system, usually the w-axis
+
+    Returns
+    -------
+    vnop: MX
+        The vector expressed in the non-orthogonal coordinate system
+
+    Source
+    ------
+    Desroches, G., Chèze, L., & Dumas, R. (2010).
+    Expression of joint moment in the joint coordinate system. Journal of biomechanical engineering, 132(11).
+    https://doi.org/10.1115/1.4002537
+
+    2.1 Expression of a 3D Vector in a Nonorthogonal Coordinate Base.
+    """
+
+    if vector.shape[0] != 3:
+        raise ValueError("The vector must be expressed in 3D.")
+    if isinstance(vector, np.ndarray):
+        vector = MX(vector)
+
+    if e1.shape[0] != 3:
+        raise ValueError("The first vector of the non-orthogonal coordinate system must be expressed in 3D.")
+
+    if e2.shape[0] != 3:
+        raise ValueError("The second vector of the non-orthogonal coordinate system must be expressed in 3D.")
+
+    if e3.shape[0] != 3:
+        raise ValueError("The third vector of the non-orthogonal coordinate system must be expressed in 3D.")
+
+    vnop = MX.zeros(vector.shape)
+
+    vnop[0, 0] = sum1(cross(e2, e3) * vector) / sum1(cross(e1, e2) * e3)
+    vnop[1, 0] = sum1(cross(e3, e1) * vector) / sum1(cross(e1, e2) * e3)
+    vnop[2, 0] = sum1(cross(e1, e2) * vector) / sum1(cross(e1, e2) * e3)
+
+    return vnop

bionc/bionc_casadi/enums.py

@@ -0,0 +1,23 @@
+from enum import Enum
+from .joint import Joint, GroundJoint
+
+
+class JointType(Enum):
+    """
+    This class represents the different types of joints
+    """
+
+    # WELD = "not implemented yet"
+    GROUND_FREE = GroundJoint.Free
+    GROUND_WELD = GroundJoint.Weld
+    GROUND_REVOLUTE = GroundJoint.Hinge
+    GROUND_UNIVERSAL = GroundJoint.Universal
+    GROUND_SPHERICAL = GroundJoint.Spherical
+    CONSTANT_LENGTH = Joint.ConstantLength
+    REVOLUTE = Joint.Hinge
+    # PRISMATIC = "not implemented yet"
+    UNIVERSAL = Joint.Universal
+    SPHERICAL = Joint.Spherical
+    SPHERE_ON_PLANE = Joint.SphereOnPlane
+
+    # PLANAR = "planar"

bionc/bionc_casadi/interface_biorbd.py

@@ -0,0 +1,54 @@
+from biorbd_casadi import Rotation
+from casadi import MX
+import numpy as np
+
+
+def rotation_matrix_from_mx_to_biorbd(R: np.ndarray | MX) -> Rotation:
+    """
+    This function returns the rotation matrix in biorbd formalism
+
+    Parameters
+    ---------
+    R : np.ndarray
+        Rotation matrix (3x3)
+
+    Returns
+    ---------
+    biorbd.Rotation
+        The rotation matrix object
+    """
+
+    return Rotation(
+        R[0, 0],
+        R[0, 1],
+        R[0, 2],
+        R[1, 0],
+        R[1, 1],
+        R[1, 2],
+        R[2, 0],
+        R[2, 1],
+        R[2, 2],
+    )
+
+
+def rotation_matrix_to_euler_angles(rotation_matrix: MX, seq: str = "xyz") -> MX:
+    """
+    This function returns the rotation matrix in euler angles vector
+
+    Parameters
+    ---------
+    rotation_matrix : np.ndarray
+        Rotation matrix (3x3)
+    seq: str = "xyz"
+        order of the coordinates in the returned vector
+    Returns
+    ---------
+    MX
+        The Euler vector in radian as an MX
+    """
+
+    rotation_matrix_biorbd = rotation_matrix_from_mx_to_biorbd(rotation_matrix)
+    return Rotation.toEulerAngles(rotation_matrix_biorbd, seq).to_mx()
+
+
+# not possible to use scipy with casadi ...