Merge pull request #30 from Ipuch/casadi_take_2

Handling casadi

bionc/__init__.py

@@ -1,9 +1,5 @@
-from .utils.natural_coordinates import SegmentNaturalCoordinates, NaturalCoordinates
-from .utils.natural_velocities import SegmentNaturalVelocities, NaturalVelocities
-from .utils.natural_accelerations import SegmentNaturalAccelerations, NaturalAccelerations
-from .utils.homogenous_transform import HomogeneousTransform
-from .utils.interpolation_matrix import interpolate_natural_vector, to_natural_vector
-from .utils.natural_coordinates import vnop_array
+# from bionc.bionc_numpy.interpolation_matrix import interpolate_natural_vector, to_natural_vector
+from .utils.vnop_array import vnop_array
 
 from .model_creation import (
     AxisTemplate,
@@ -16,12 +12,27 @@
     Data,
     GenericDynamicModel,
 )
-from .model_computations import (
+from .bionc_numpy import (
     Axis,
     SegmentMarker,
     Marker,
-    Segment,
     NaturalSegment,
     InertiaParameters,
     BiomechanicalModel,
 )
+
+from .protocols import natural_coordinates
+from bionc import bionc_casadi
+from bionc import bionc_numpy
+
+from .protocols.natural_coordinates import SegmentNaturalCoordinates, NaturalCoordinates
+from .protocols.natural_velocities import SegmentNaturalVelocities, NaturalVelocities
+from .protocols.natural_accelerations import SegmentNaturalAccelerations, NaturalAccelerations
+from .protocols.homogenous_transform import HomogeneousTransform
+
+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

bionc/bionc_casadi/__init__.py

@@ -0,0 +1,15 @@
+from .natural_coordinates import SegmentNaturalCoordinates, NaturalCoordinates
+from .natural_velocities import SegmentNaturalVelocities, NaturalVelocities
+from .natural_accelerations import SegmentNaturalAccelerations, NaturalAccelerations
+from .homogenous_transform import HomogeneousTransform
+from .natural_segment import NaturalSegment
+
+# The actual model to inherit from
+from .biomechanical_model import BiomechanicalModel
+
+# Some classes to define the BiomechanicalModel
+from .natural_axis import Axis
+from .natural_marker import SegmentMarker, Marker
+from .natural_segment import NaturalSegment
+from .inertia_parameters import InertiaParameters
+from .interpolation_matrix import interpolate_natural_vector, to_natural_vector

bionc/bionc_casadi/biomechanical_model.py

@@ -0,0 +1,162 @@
+import numpy as np
+from casadi import MX
+
+from .natural_coordinates import SegmentNaturalCoordinates, NaturalCoordinates
+from .natural_velocities import SegmentNaturalVelocities, NaturalVelocities
+from ..protocols.biomechanical_model import AbstractBiomechanicalModel
+
+
+class BiomechanicalModel(AbstractBiomechanicalModel):
+    def __init__(self):
+        from .natural_segment import NaturalSegment  # Imported here to prevent from circular imports
+        from .joint import Joint  # Imported here to prevent from circular imports
+
+        self.segments: dict[str:NaturalSegment, ...] = {}
+        self.joints: dict[str:Joint, ...] = {}
+        # From Pythom 3.7 the insertion order in a dict is preserved. This is important because when writing a new
+        # the order of the segment matters
+        self._mass_matrix = self._update_mass_matrix()
+
+    def __getitem__(self, name: str):
+        return self.segments[name]
+
+    def __setitem__(self, name: str, segment: "NaturalSegment"):
+        if segment.name == name:  # Make sure the name of the segment fits the internal one
+            self.segments[name] = segment
+            self._update_mass_matrix()  # Update the generalized mass matrix
+        else:
+            raise ValueError("The name of the segment does not match the name of the segment")
+
+    def __str__(self):
+        out_string = "version 4\n\n"
+        for name in self.segments:
+            out_string += str(self.segments[name])
+            out_string += "\n\n\n"  # Give some space between segments
+        return out_string
+
+    def nb_segments(self):
+        return len(self.segments)
+
+    def nb_markers(self):
+        nb_markers = 0
+        for key in self.segments:
+            nb_markers += self.segments[key].nb_markers()
+        return nb_markers
+
+    def nb_joints(self):
+        return len(self.joints)
+
+    def nb_Q(self):
+        return 12 * self.nb_segments()
+
+    def nb_Qdot(self):
+        return 12 * self.nb_segments()
+
+    def nb_Qddot(self):
+        return 12 * self.nb_segments()
+
+    def rigid_body_constraints(self, Q: NaturalCoordinates) -> MX:
+        """
+        This function returns the rigid body constraints of all segments, denoted Phi_r
+        as a function of the natural coordinates Q.
+
+        Returns
+        -------
+        MX
+            Rigid body constraints of the segment [6 * nb_segments, 1]
+        """
+
+        Phi_r = MX.zeros(6 * self.nb_segments())
+        for i, segment_name in enumerate(self.segments):
+            idx = slice(6 * i, 6 * (i + 1))
+            Phi_r[idx] = self.segments[segment_name].rigid_body_constraint(Q.vector(i))
+
+        return Phi_r
+
+    def rigid_body_constraints_jacobian(self, Q: NaturalCoordinates) -> MX:
+        """
+        This function returns the rigid body constraints of all segments, denoted K_r
+        as a function of the natural coordinates Q.
+
+        Returns
+        -------
+        MX
+            Rigid body constraints of the segment [6 * nb_segments, nbQ]
+        """
+
+        K_r = MX.zeros((6 * self.nb_segments(), Q.shape[0]))
+        for i, segment_name in enumerate(self.segments):
+            idx_row = slice(6 * i, 6 * (i + 1))
+            idx_col = slice(12 * i, 12 * (i + 1))
+            K_r[idx_row, idx_col] = self.segments[segment_name].rigid_body_constraint_jacobian(Q.vector(i))
+
+        return K_r
+
+    def rigid_body_constraint_jacobian_derivative(self, Qdot: NaturalVelocities) -> MX:
+        """
+        This function returns the derivative of the Jacobian matrix of the rigid body constraints denoted Kr_dot
+
+        Parameters
+        ----------
+        Qdot : NaturalVelocities
+            The natural velocities of the segment [12, 1]
+
+        Returns
+        -------
+        MX
+            The derivative of the Jacobian matrix of the rigid body constraints [6, 12]
+        """
+
+        Kr_dot = MX.zeros((6 * self.nb_segments(), Qdot.shape[0]))
+        for i, segment_name in enumerate(self.segments):
+            idx_row = slice(6 * i, 6 * (i + 1))
+            idx_col = slice(12 * i, 12 * (i + 1))
+            Kr_dot[idx_row, idx_col] = self.segments[segment_name].rigid_body_constraint_jacobian_derivative(
+                Qdot.vector(i)
+            )
+
+        return Kr_dot
+
+    def _update_mass_matrix(self):
+        """
+        This function computes the generalized mass matrix of the system, denoted G
+
+        Returns
+        -------
+        MX
+            generalized mass matrix of the segment [12 * nbSegment x 12 * * nbSegment]
+        """
+        G = MX.zeros((12 * self.nb_segments(), 12 * self.nb_segments()))
+        for i, segment_name in enumerate(self.segments):
+            Gi = self.segments[segment_name].mass_matrix
+            if Gi is None:
+                # mass matrix is None if one the segment doesn't have any inertial properties
+                self._mass_matrix = None
+                return
+            idx = slice(12 * i, 12 * (i + 1))
+            G[idx, idx] = self.segments[segment_name].mass_matrix
+
+        self._mass_matrix = G
+
+    @property
+    def mass_matrix(self):
+        """
+        This function returns the generalized mass matrix of the system, denoted G
+
+        Returns
+        -------
+        MX
+            generalized mass matrix of the segment [12 * nbSegment x 12 * * nbSegment]
+
+        """
+        return self._mass_matrix
+
+
+# def kinematicConstraints(self, Q):
+#     # Method to calculate the kinematic constraints
+
+# def forwardDynamics(self, Q, Qdot):
+#
+#     return Qddot, lambdas
+
+# def inverseDynamics(self):

bionc/bionc_casadi/homogenous_transform.py

@@ -0,0 +1,130 @@
+from typing import Union
+from casadi import MX, vertcat, horzcat, transpose
+import numpy as np
+
+
+class HomogeneousTransform(MX):
+    """
+    Homogenous transform class
+    """
+
+    def __new__(cls, input_array: MX):
+        """
+        Create a new instance of the class.
+        """
+
+        if input_array.shape != (4, 4):
+            raise ValueError("input_array must be a 4x4 array")
+
+        obj = MX.__new__(cls)
+
+        return obj
+
+    @classmethod
+    def from_components(cls, x: MX, y: MX, z: MX, t: MX):
+        """
+        Constructor of the class from the components of the homogenous transform
+
+        Parameters
+        ----------
+        x: MX
+            The x axis of the homogenous transform, a 3x1 array
+        y: MX
+            The y axis of the homogenous transform, a 3x1 array
+        z: MX
+            The z axis of the homogenous transform, a 3x1 array
+        t: MX
+            translation vector, a 3x1 array
+        """
+
+        if x is None:
+            raise ValueError("u must be a numpy array (3x1) or a list of 3 elements")
+        if y is None:
+            raise ValueError("rp must be a numpy array (3x1) or a list of 3 elements")
+        if z is None:
+            raise ValueError("rd must be a numpy array (3x1) or a list of 3 elements")
+        if t is None:
+            raise ValueError("w must be a numpy array (3x1) or a list of 3 elements")
+
+        if not isinstance(x, MX):
+            x = MX(x)
+        if not isinstance(y, MX):
+            y = MX(y)
+        if not isinstance(z, MX):
+            z = MX(z)
+        if not isinstance(t, MX):
+            t = MX(t)
+
+        if x.shape != (3, 1):
+            raise ValueError("x must be a 3x1 array")
+        if y.shape != (3, 1):
+            raise ValueError("y must be a 3x1 array")
+        if z.shape != (3, 1):
+            raise ValueError("z must be a 3x1 array")
+        if t.shape != (3, 1):
+            raise ValueError("t must be a 3x1 array")
+
+        input_array = horzcat(*(x, y, z, t))
+        input_array = vertcat(*(input_array, np.array([[0, 0, 0, 1]])))
+        return cls(input_array)
+
+    @classmethod
+    def from_rt(cls, rotation: MX, translation: MX):
+        """
+        Constructor of the class from a rotation matrix and a translation vector
+
+        Parameters
+        ----------
+        rotation: MX
+            A 3x3 rotation matrix
+        translation: MX
+            A 3x1 translation vector
+        """
+        if rotation is None:
+            raise ValueError("rotation must be a 3x3 array")
+        if translation is None:
+            raise ValueError("translation must be a 3x1 array")
+
+        if not isinstance(rotation, MX):
+            MX(rotation)
+        if not isinstance(translation, MX):
+            MX(translation)
+
+        if rotation.shape != (3, 3):
+            raise ValueError("r must be a 3x3 array")
+        if translation.shape != (3, 1):
+            raise ValueError("t must be a 3x1 array")
+
+        input_array = horzcat(*(rotation, translation))
+        input_array = vertcat(*(input_array, np.array([[0, 0, 0, 1]])))
+
+        return cls(input_array)
+
+    @classmethod
+    def eye(cls):
+        """
+        Returns the identity homogenous transform
+        """
+        return cls(np.eye(4))
+
+    @property
+    def rot(self):
+        return self[:3, :3]
+
+    @property
+    def translation(self):
+        return self[3, 0:3]
+
+    def inv(self):
+        """
+        Returns the inverse of the homogenous transform
+        """
+        inv_mat = MX.zeros((4, 4))
+        inv_mat[:3, :3] = transpose(self[:3, :3])
+        inv_mat[:3, 3] = -inv_mat[:3, :3] @ self[:3, 3]
+        inv_mat[3, :] = MX([0, 0, 0, 1])
+
+        return HomogeneousTransform(inv_mat)
+
+    def to_array(self):
+        return self