Merge pull request #113 from Ipuch/another_fext_xp

[RTR] Preparing the work for refactoring external forces

.github/workflows/run_tests.yml

@@ -72,7 +72,7 @@ jobs:
           xvfb-run --server-args="-screen 0 1024x768x24" python -c "import bionc"
         if: matrix.label == 'linux-64'
 
-      - name: Test installed version of bioviz on WINDOWS and MAC
+      - name: Test installed version of bionc on WINDOWS and MAC
         run: | 
           python setup.py install
           cd

bionc/__init__.py

@@ -18,7 +18,7 @@
     InertiaParameters,
     BiomechanicalModel,
     JointType,
-    ExternalForceList,
+    ExternalForceSet,
     ExternalForce,
     NaturalInertialParameters,
     compute_transformation_matrix,

bionc/bionc_casadi/__init__.py

@@ -15,5 +15,5 @@
 from .joints import Joint
 from .joints_with_ground import GroundJoint
 from .natural_vector import NaturalVector
-from .external_force import ExternalForceList, ExternalForce
+from .external_force import ExternalForceSet, ExternalForce
 from .cartesian_vector import vector_projection_in_non_orthogonal_basis

bionc/bionc_casadi/biomechanical_model.py

@@ -6,12 +6,29 @@
 from .natural_velocities import NaturalVelocities
 from .natural_accelerations import NaturalAccelerations
 from ..protocols.biomechanical_model import GenericBiomechanicalModel
-from .external_force import ExternalForceList, ExternalForce
+from .external_force import ExternalForceSet, 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):
+    """
+
+    Attributes
+    ----------
+    _numpy_model : NumpyBiomechanicalModel
+        The numpy model from which the casadi model is built
+
+    Methods
+    -------
+    set_numpy_model(numpy_model: BiomechanicalModel)
+        Set the numpy model from which the casadi model is built
+    numpy_model
+        Return the numpy model from which the casadi model is built
+    express_joint_torques_in_euler_basis
+        This function returns the joint torques expressed in the euler basis
+    """
+
     def __init__(self):
         super().__init__()
         self._numpy_model = None
@@ -612,7 +629,7 @@ def forward_dynamics(
         self,
         Q: NaturalCoordinates,
         Qdot: NaturalCoordinates,
-        external_forces: ExternalForceList = None,
+        external_forces: ExternalForceSet = None,
         # external_forces: ExternalForces
     ):
         """
@@ -624,7 +641,7 @@ def forward_dynamics(
             The natural coordinates of the segment [12 * nb_segments, 1]
         Qdot : NaturalCoordinates
             The natural coordinates time derivative of the segment [12 * nb_segments, 1]
-        external_forces : ExternalForceList
+        external_forces : ExternalForceSet
             The list of external forces applied on the system
 
         Returns
@@ -638,9 +655,7 @@ def forward_dynamics(
         K = self.holonomic_constraints_jacobian(Q)
         Kdot = self.holonomic_constraints_jacobian_derivative(Qdot)
 
-        external_forces = (
-            ExternalForceList.empty_from_nb_segment(self.nb_segments) if external_forces is None else external_forces
-        )
+        external_forces = self.external_force_set() if external_forces is None else external_forces
         fext = external_forces.to_natural_external_forces(Q)
         # if stabilization is not None:
         #     biais -= stabilization["alpha"] * self.rigid_body_constraint(Qi) + stabilization[
@@ -664,14 +679,17 @@ def forward_dynamics(
         lagrange_multipliers = x[self.nb_Qddot :]
         return NaturalAccelerations(Qddot), lagrange_multipliers
 
+    def external_force_set(self) -> ExternalForceSet:
+        return ExternalForceSet.empty_from_nb_segment(self.nb_segments)
+
     def inverse_dynamics(
         self,
         Q: NaturalCoordinates,
         Qddot: NaturalAccelerations,
-        external_forces: ExternalForceList = None,
+        external_forces: ExternalForceSet = None,
     ) -> tuple[MX, MX, MX]:
         if external_forces is None:
-            external_forces = ExternalForceList.empty_from_nb_segment(self.nb_segments)
+            external_forces = self.external_force_set()
         else:
             if external_forces.nb_segments != self.nb_segments:
                 raise ValueError(
@@ -728,7 +746,7 @@ def _inverse_dynamics_recursive_step(
         self,
         Q: NaturalCoordinates,
         Qddot: NaturalAccelerations,
-        external_forces: ExternalForceList,
+        external_forces: ExternalForceSet,
         segment_index: int = 0,
         visited_segments: list[bool, ...] = None,
         torques: MX = None,
@@ -744,7 +762,7 @@ def _inverse_dynamics_recursive_step(
             The generalized coordinates of the model
         Qddot: NaturalAccelerations
             The generalized accelerations of the model
-        external_forces: ExternalForceList
+        external_forces: ExternalForceSet
             The external forces applied to the model
         segment_index: int
             The index of the segment to start the search from

bionc/bionc_casadi/external_force.py

@@ -137,7 +137,7 @@ def transport_to(
         return fext
 
 
-class ExternalForceList:
+class ExternalForceSet:
     """
     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:
@@ -154,7 +154,7 @@ class ExternalForceList:
     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.
+        This function creates an empty ExternalForceSet 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)
@@ -164,9 +164,9 @@ class ExternalForceList:
 
     Examples
     --------
-    >>> from bionc import ExternalForceList, ExternalForce
+    >>> from bionc import ExternalForceSet, ExternalForce
     >>> import numpy as np
-    >>> f_ext = ExternalForceList.empty_from_nb_segment(2)
+    >>> f_ext = ExternalForceSet.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)
     """
@@ -175,7 +175,7 @@ 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)"
+                "NaturalExternalForceSet.empty_from_nb_segment(nb_segment)"
             )
         self.external_forces = external_forces
 
@@ -187,7 +187,7 @@ def nb_segments(self) -> int:
     @classmethod
     def empty_from_nb_segment(cls, nb_segment: int):
         """
-        Create an empty NaturalExternalForceList from the model size
+        Create an empty NaturalExternalForceSet from the model size
         """
         return cls(external_forces=[[] for _ in range(nb_segment)])
 

bionc/bionc_casadi/generalized_force.py

@@ -0,0 +1,38 @@
+from casadi import MX
+
+from .external_force import ExternalForce
+from .natural_coordinates import SegmentNaturalCoordinates, NaturalCoordinates
+from ..utils.enums import CartesianAxis, EulerSequence
+from .rotations import euler_axes_from_rotation_matrices
+from ..protocols.joint import JointBase as Joint
+
+
+class JointGeneralizedForces(ExternalForce):
+    """
+    Made to handle joint generalized forces, it inherits from ExternalForce
+
+    Attributes
+    ----------
+    external_forces : np.ndarray
+        The external forces
+    application_point_in_local : np.ndarray
+        The application point in local coordinates
+
+    Methods
+    -------
+    from_joint_generalized_forces(forces, torques, translation_dof, rotation_dof, joint, Q_parent, Q_child)
+        This function creates a JointGeneralizedForces from the forces and torques
+
+    Notes
+    -----
+    The application point of torques is set to the proximal point of the child.
+    """
+
+    def __init__(
+        self,
+        external_forces: MX,
+        application_point_in_local: MX,
+    ):
+        super().__init__(external_forces=external_forces, application_point_in_local=application_point_in_local)
+
+        # TODO !

bionc/bionc_numpy/__init__.py

@@ -17,7 +17,7 @@
 from .joints_with_ground import GroundJoint
 from .natural_vector import NaturalVector
 from .inverse_kinematics import InverseKinematics
-from .external_force import ExternalForceList, ExternalForce
+from .external_force import ExternalForceSet, ExternalForce
 from .cartesian_vector import vector_projection_in_non_orthogonal_basis
 from .natural_inertial_parameters import NaturalInertialParameters
 from .transformation_matrix import compute_transformation_matrix

bionc/bionc_numpy/biomechanical_model.py

@@ -6,12 +6,23 @@
 from .natural_accelerations import NaturalAccelerations
 from ..protocols.biomechanical_model import GenericBiomechanicalModel
 from .inverse_kinematics import InverseKinematics
-from .external_force import ExternalForceList, ExternalForce, JointGeneralizedForces, JointGeneralizedForcesList
+from .external_force import ExternalForceSet, ExternalForce
+from .generalized_force import JointGeneralizedForces, JointGeneralizedForcesList
 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):
+    """
+
+    Methods
+    -------
+    to_mx()
+        This function returns the equivalent of the current Casadi BiomechanicalModel compatible with CasADi
+    express_joint_torques_in_euler_basis
+        This function returns the joint torques expressed in the euler basis
+    """
+
     def __init__(self):
         super().__init__()
 
@@ -659,7 +670,7 @@ def forward_dynamics(
         Q: NaturalCoordinates,
         Qdot: NaturalCoordinates,
         joint_generalized_forces: np.ndarray = None,
-        external_forces: ExternalForceList = None,
+        external_forces: ExternalForceSet = None,
         stabilization: dict = None,
     ) -> np.ndarray:
         """
@@ -674,7 +685,7 @@ def forward_dynamics(
         joint_generalized_forces : np.ndarray
             The joint generalized forces in joint euler-basis, and forces in parent basis, like in minimal coordinates,
             one per dof of the system. If None, the joint generalized forces are set to 0
-        external_forces : ExternalForceList
+        external_forces : ExternalForceSet
             The list of external forces applied on the system
         stabilization: dict
             Dictionary containing the Baumgarte's stabilization parameters:
@@ -694,9 +705,7 @@ def forward_dynamics(
         K = self.holonomic_constraints_jacobian(Q)
         Kdot = self.holonomic_constraints_jacobian_derivative(Qdot)
 
-        external_forces = (
-            ExternalForceList.empty_from_nb_segment(self.nb_segments) if external_forces is None else external_forces
-        )
+        external_forces = self.external_force_set() if external_forces is None else external_forces
         fext = external_forces.to_natural_external_forces(Q)
 
         joint_generalized_forces_object = JointGeneralizedForcesList.empty_from_nb_joint(self.nb_segments)
@@ -766,14 +775,17 @@ def inverse_kinematics(
         """
         return InverseKinematics(self, experimental_markers, Q_init, solve_frame_per_frame)
 
+    def external_force_set(self) -> ExternalForceSet:
+        return ExternalForceSet.empty_from_nb_segment(self.nb_segments)
+
     def inverse_dynamics(
         self,
         Q: NaturalCoordinates,
         Qddot: NaturalAccelerations,
-        external_forces: ExternalForceList = None,
+        external_forces: ExternalForceSet = None,
     ) -> tuple[np.ndarray, np.ndarray, np.ndarray]:
         if external_forces is None:
-            external_forces = ExternalForceList.empty_from_nb_segment(self.nb_segments)
+            external_forces = self.external_force_set()
         else:
             if external_forces.nb_segments != self.nb_segments:
                 raise ValueError(
@@ -830,7 +842,7 @@ def _inverse_dynamics_recursive_step(
         self,
         Q: NaturalCoordinates,
         Qddot: NaturalAccelerations,
-        external_forces: ExternalForceList,
+        external_forces: ExternalForceSet,
         segment_index: int = 0,
         visited_segments: list[bool, ...] = None,
         torques: np.ndarray = None,
@@ -846,7 +858,7 @@ def _inverse_dynamics_recursive_step(
             The generalized coordinates of the model
         Qddot: NaturalAccelerations
             The generalized accelerations of the model
-        external_forces: ExternalForceList
+        external_forces: ExternalForceSet
             The external forces applied to the model
         segment_index: int
             The index of the segment to start the search from