Merge pull request #67 from Ipuch/another_fext_xp

adding external forces

.github/workflows/codecov.yml

@@ -0,0 +1,26 @@
+# Check if the tests covers all the code
+name: Codecov(erage).
+
+on: [pull_request]
+
+jobs:
+  build:
+    runs-on: ubuntu-latest
+    name: Test python API
+    steps:
+    - uses: actions/checkout@master
+    - name: Install requirements
+      run: pip install -r requirements.txt
+    - name: Run tests and collect coverage
+      run: pytest --cov .
+    - name: Upload coverage reports to Codecov
+#      run: |
+#        # Replace `linux` below with the appropriate OS
+#        # Options are `alpine`, `linux`, `macos`, `windows`
+#        curl -Os https://uploader.codecov.io/latest/linux/codecov
+#        chmod +x codecov
+#        ./codecov -t ${CODECOV_TOKEN}
+    - uses: codecov/codecov-action@v3
+      name: Upload coverage to Codecov
+      with:
+        token: ${{ secrets.CODECOV_TOKEN }}

.github/workflows/run_tests.yml

@@ -21,7 +21,7 @@ jobs:
       run:
         shell: bash -l {0}
     steps:
-      - uses: actions/checkout@v2
+      - uses: actions/checkout@master
 
       - name: Setup environment
         uses: conda-incubator/setup-miniconda@v2
@@ -39,7 +39,7 @@ jobs:
           mamba list
           
       - name: Install extra dependencies
-        run: mamba install pytest-cov pytest pytest-cov codecov -cconda-forge
+        run: mamba install pytest-cov pytest codecov -cconda-forge
 
       - name: Run the actual tests on LINUX
         run: |
@@ -48,13 +48,23 @@ jobs:
         if: matrix.label == 'linux-64'
 
       - name: Run the actual tests
-        run: pytest -v --color=yes --cov-report term-missing --cov=bionc tests
+        run: pytest -v --color=yes --cov-report=xml --cov=bionc tests
         if: matrix.label == 'osx-64'
 
       - name: Upload coverage to Codecov
-        run: codecov
+        run: codecov --token ${{ secrets.CODECOV_TOKEN }}
         if: matrix.label == 'linux-64'
 
+#      - name: Upload coverage to Codecov
+#        uses: codecov/codecov-action@v3
+#        with:
+#          token: ${{ secrets.CODECOV_TOKEN }}
+#          fail_ci_if_error: true
+#          directory: /home/runner/work/bioNC
+#          files: ./coverage.xml
+#        if: matrix.label == 'linux-64'
+
+
       - name: Test installed version of bioviz on LINUX
         run: |
           python setup.py install

bionc/__init__.py

@@ -18,6 +18,8 @@
     InertiaParameters,
     BiomechanicalModel,
     JointType,
+    ExternalForceList,
+    ExternalForce,
 )
 
 from .protocols import natural_coordinates

bionc/bionc_casadi/__init__.py

@@ -14,3 +14,4 @@
 from .inertia_parameters import InertiaParameters
 from .joint import Joint, GroundJoint
 from .natural_vector import NaturalVector
+from .external_force import ExternalForceList, ExternalForce

bionc/bionc_casadi/external_force.py

@@ -0,0 +1,229 @@
+from casadi import MX, inv, cross
+import numpy as np
+
+from .natural_vector import NaturalVector
+from .natural_coordinates import SegmentNaturalCoordinates, NaturalCoordinates
+
+
+class ExternalForce:
+    """
+    This class represents an external force applied to a segment.
+
+    Attributes
+    ----------
+    application_point_in_local : np.ndarray
+        The application point of the force in the natural coordinate system of the segment
+    external_forces : np.ndarray
+        The external force vector in the global coordinate system (torque, force)
+
+    Methods
+    -------
+    from_components(application_point_in_local, force, torque)
+        This function creates an external force from its components.
+    force
+        This function returns the force vector of the external force.
+    torque
+        This function returns the torque vector of the external force.
+    compute_pseudo_interpolation_matrix()
+        This function computes the pseudo interpolation matrix of the external force.
+    to_natural_force
+        This function returns the external force in the natural coordinate format.
+    """
+
+    def __init__(self, application_point_in_local: np.ndarray | MX, external_forces: np.ndarray | MX):
+        self.application_point_in_local = MX(application_point_in_local)
+        self.external_forces = MX(external_forces)
+
+    @classmethod
+    def from_components(cls, application_point_in_local: np.ndarray | MX, force: np.ndarray | MX, torque: np.ndarray | MX):
+        """
+        This function creates an external force from its components.
+
+        Parameters
+        ----------
+        application_point_in_local : np.ndarray
+            The application point of the force in the natural coordinate system of the segment
+        force
+            The force vector in the global coordinate system
+        torque
+            The torque vector in the global coordinate system
+
+        Returns
+        -------
+        ExternalForce
+        """
+
+        return cls(application_point_in_local, np.concatenate((torque, force)))
+
+    @property
+    def force(self) -> MX:
+        """The force vector in the global coordinate system"""
+        return self.external_forces[3:6]
+
+    @property
+    def torque(self) -> MX:
+        """The torque vector in the global coordinate system"""
+        return self.external_forces[0:3]
+
+    @staticmethod
+    def compute_pseudo_interpolation_matrix(Qi: SegmentNaturalCoordinates) -> MX:
+        """
+        Return the force moment transformation matrix
+
+        Parameters
+        ----------
+        Qi : SegmentNaturalCoordinates
+            The natural coordinates of the segment
+
+        Returns
+        -------
+        np.ndarray
+            The force moment transformation matrix
+        """
+        # default we apply force at the proximal point
+
+        left_interpolation_matrix = MX.zeros((12, 3))
+
+        left_interpolation_matrix[9:12, 0] = Qi.u
+        left_interpolation_matrix[0:3, 1] = Qi.v
+        left_interpolation_matrix[3:6, 2] = -Qi.w
+        left_interpolation_matrix[6:9, 2] = Qi.w
+
+        # Matrix of lever arms for forces equivalent to moment at proximal endpoint, denoted Bstar
+        lever_arm_force_matrix = MX.zeros((3, 3))
+
+        lever_arm_force_matrix[:, 0] = cross(Qi.w, Qi.u)
+        lever_arm_force_matrix[:, 1] = cross(Qi.u, Qi.v)
+        lever_arm_force_matrix[:, 2] = cross(-Qi.v, Qi.w)
+
+        return (left_interpolation_matrix @ inv(lever_arm_force_matrix)).T  # NOTE: inv may induce symbolic error.
+
+    def to_natural_force(self, Qi: SegmentNaturalCoordinates) -> MX:
+        """
+        Apply external forces to the segment
+
+        Parameters
+        ----------
+        Qi: SegmentNaturalCoordinates
+            Segment natural coordinates
+
+        Returns
+        -------
+        np.ndarray
+            The external forces adequately transformed for the equation of motion in natural coordinates
+        """
+
+        pseudo_interpolation_matrix = self.compute_pseudo_interpolation_matrix(Qi)
+        point_interpolation_matrix = NaturalVector(self.application_point_in_local).interpolate()
+        application_point_in_global = point_interpolation_matrix @ Qi
+
+        fext = point_interpolation_matrix.T @ self.force
+        fext += pseudo_interpolation_matrix.T @ self.torque
+
+        # Bour's formula to transport the moment from the application point to the proximal point
+        # fext += pseudo_interpolation_matrix.T @ np.cross(application_point_in_global - Qi.rp, self.force)
+
+        return fext
+
+
+class ExternalForceList:
+    """
+    This class is made to handle all the external forces of each segment, if none are provided, it will be an empty list.
+    All segment forces are expressed in natural coordinates to be added to the equation of motion as:
+
+    Q @ Qddot + K^T @ lambda = Weight + f_ext
+
+    Attributes
+    ----------
+    external_forces : list
+        List of ExternalForces for each segment
+
+    Methods
+    -------
+    add_external_force(segment_index, external_force)
+        This function adds an external force to the list of external forces.
+    empty_from_nb_segment(nb_segment)
+        This function creates an empty ExternalForceList from the number of segments.
+    to_natural_external_forces(Q)
+        This function returns the external forces in the natural coordinate format.
+    segment_external_forces(segment_index)
+        This function returns the external forces of a segment.
+    nb_segments
+        This function returns the number of segments.
+
+    Examples
+    --------
+    >>> from bionc import ExternalForceList, ExternalForce
+    >>> import numpy as np
+    >>> f_ext = ExternalForceList.empty_from_nb_segment(2)
+    >>> segment_force = ExternalForce(force=np.array([0,1,1.1]), torque=np.zeros(3), application_point_in_local=np.array([0,0.5,0]))
+    >>> f_ext.add_external_force(segment_index=0, external_force=segment_force)
+    """
+
+    def __init__(self, external_forces: list[list[ExternalForce, ...]] = None):
+        if external_forces is None:
+            raise ValueError(
+                "f_ext must be a list of ExternalForces, or use the classmethod"
+                "NaturalExternalForceList.empty_from_nb_segment(nb_segment)"
+            )
+        self.external_forces = external_forces
+
+    @property
+    def nb_segments(self) -> int:
+        """Returns the number of segments"""
+        return len(self.external_forces)
+
+    @classmethod
+    def empty_from_nb_segment(cls, nb_segment: int):
+        """
+        Create an empty NaturalExternalForceList from the model size
+        """
+        return cls(external_forces=[[] for _ in range(nb_segment)])
+
+    def segment_external_forces(self, segment_index: int) -> list[ExternalForce]:
+        """Returns the external forces of the segment"""
+        return self.external_forces[segment_index]
+
+    def add_external_force(self, segment_index: int, external_force: ExternalForce):
+        """
+        Add an external force to the segment
+
+        Parameters
+        ----------
+        segment_index: int
+            The index of the segment
+        external_force:
+            The external force to add
+        """
+        self.external_forces[segment_index].append(external_force)
+
+    def to_natural_external_forces(self, Q: NaturalCoordinates) -> MX:
+        """
+        Converts and sums all the segment natural external forces to the full vector of natural external forces
+
+        Parameters
+        ----------
+        Q : NaturalCoordinates
+            The natural coordinates of the model
+        """
+
+        if len(self.external_forces) != Q.nb_qi():
+            raise ValueError(
+                "The number of segment in the model and the number of segment in the external forces must be the same"
+            )
+
+        natural_external_forces = MX.zeros((12 * Q.nb_qi(), 1))
+        for segment_index, segment_external_forces in enumerate(self.external_forces):
+            segment_natural_external_forces = MX.zeros((12, 1))
+            slice_index = slice(segment_index * 12, (segment_index + 1) * 12)
+            for external_force in segment_external_forces:
+                segment_natural_external_forces += external_force.to_natural_force(Q.vector(segment_index))
+            natural_external_forces[slice_index, 0:1] = segment_natural_external_forces
+
+        return natural_external_forces
+
+    def __iter__(self):
+        return iter(self.external_forces)
+
+    def __len__(self):
+        return len(self.external_forces)

bionc/bionc_casadi/joint.py

@@ -1,4 +1,4 @@
-from casadi import MX, dot, cos, transpose, sumsqr
+from casadi import MX, dot, cos, transpose, sumsqr, vertcat
 import numpy as np
 
 from .natural_segment import NaturalSegment
@@ -568,7 +568,7 @@ def constraint(self, Q_parent: SegmentNaturalCoordinates, Q_child: SegmentNatura
             return constraint
 
         def parent_constraint_jacobian(
-            self, Q_child: SegmentNaturalCoordinates, Q_parent: SegmentNaturalCoordinates
+            self, Q_parent: SegmentNaturalCoordinates, Q_child: SegmentNaturalCoordinates
         ) -> MX:
             parent_point_location = self.parent_point.position_in_global(Q_parent)
             child_point_location = self.child_point.position_in_global(Q_child)
@@ -588,7 +588,7 @@ def child_constraint_jacobian(
             return K_k_child
 
         def parent_constraint_jacobian_derivative(
-            self, Qdot_child: SegmentNaturalVelocities, Qdot_parent: SegmentNaturalVelocities
+            self, Qdot_parent: SegmentNaturalVelocities, Qdot_child: SegmentNaturalVelocities
         ) -> MX:
             parent_point_location = self.parent_point.position_in_global(Qdot_parent)
             child_point_location = self.child_point.position_in_global(Qdot_child)
@@ -871,14 +871,16 @@ def __init__(
             self,
             name: str,
             child: NaturalSegment,
-            Q_child_ref: SegmentNaturalCoordinates,
+            rp_child_ref: SegmentNaturalCoordinates = None,
+            rd_child_ref: SegmentNaturalCoordinates = None,
             index: int = None,
         ):
             super(GroundJoint.Weld, self).__init__(name, None, child, index)
 
             # check size and type of parent axis
-            self.Q_child_ref = Q_child_ref
-            self.nb_constraints = 12
+            self.rp_child_ref = rp_child_ref
+            self.rd_child_ref = rd_child_ref
+            self.nb_constraints = 6
 
         def constraint(self, Q_parent: SegmentNaturalCoordinates, Q_child: SegmentNaturalCoordinates) -> MX:
             """
@@ -891,7 +893,7 @@ def constraint(self, Q_parent: SegmentNaturalCoordinates, Q_child: SegmentNatura
                 Kinematic constraints of the joint [12, 1]
             """
 
-            return self.Q_child_ref - Q_child
+            return vertcat(self.rp_child_ref - Q_child.rp, self.rd_child_ref - Q_child.rd)
 
         def parent_constraint_jacobian(
             self, Q_parent: SegmentNaturalCoordinates, Q_child: SegmentNaturalCoordinates
@@ -901,7 +903,7 @@ def parent_constraint_jacobian(
         def child_constraint_jacobian(
             self, Q_parent: SegmentNaturalCoordinates, Q_child: SegmentNaturalCoordinates
         ) -> MX:
-            K_k_child = -MX.eye(self.nb_constraints)
+            K_k_child = -MX.eye(12)[3:9, :]
 
             return K_k_child
 

bionc/bionc_casadi/natural_segment.py

@@ -2,7 +2,7 @@
 
 import numpy as np
 from casadi import MX
-from casadi import cos, sin, transpose, vertcat, sqrt, inv, dot, sum1, sum2
+from casadi import cos, sin, transpose, vertcat, sqrt, inv, dot, sum1, cross, norm_2
 
 from ..bionc_casadi.natural_coordinates import SegmentNaturalCoordinates
 from ..bionc_casadi.natural_velocities import SegmentNaturalVelocities

bionc/bionc_numpy/__init__.py

@@ -16,3 +16,4 @@
 from .joint import Joint, GroundJoint
 from .natural_vector import NaturalVector
 from .inverse_kinematics import InverseKinematics
+from .external_force import ExternalForceList, ExternalForce