Merge pull request #72 from Ipuch/another_fext_xp

Inverse dynamics #70 first milestone

bionc/bionc_casadi/biomechanical_model.py

@@ -6,7 +6,7 @@
 from .natural_velocities import NaturalVelocities
 from .natural_accelerations import NaturalAccelerations
 from ..protocols.biomechanical_model import GenericBiomechanicalModel
-from .external_force import ExternalForceList
+from .external_force import ExternalForceList, ExternalForce
 
 
 class BiomechanicalModel(GenericBiomechanicalModel):
@@ -583,18 +583,18 @@ def holonomic_constraints_jacobian_derivative(self, Qdot: NaturalVelocities) ->
 
         return Kdot
 
-    def weight(self) -> MX:
+    def gravity_forces(self) -> MX:
         """
         This function returns the weights caused by the gravity forces on each segment
 
         Returns
         -------
-            The weight of each segment [12 * nb_segments, 1]
+            The gravity_force of each segment [12 * nb_segments, 1]
         """
         weight_vector = MX.zeros((self.nb_segments * 12, 1))
         for i, segment in enumerate(self.segments_no_ground.values()):
             idx = slice(12 * i, 12 * (i + 1))
-            weight_vector[idx] = segment.weight()
+            weight_vector[idx] = segment.gravity_force()
 
         return weight_vector
 
@@ -644,7 +644,7 @@ def forward_dynamics(
         lower_KKT_matrix = horzcat(K, np.zeros((K.shape[0], K.shape[0])))
         KKT_matrix = vertcat(upper_KKT_matrix, lower_KKT_matrix)
 
-        forces = self.weight() + fext
+        forces = self.gravity_forces() + fext
         biais = -Kdot @ Qdot
         B = vertcat(forces, biais)
 
@@ -653,3 +653,152 @@ def forward_dynamics(
         Qddot = x[0 : self.nb_Qddot]
         lagrange_multipliers = x[self.nb_Qddot :]
         return NaturalAccelerations(Qddot), lagrange_multipliers
+
+    def inverse_dynamics(
+        self,
+        Q: NaturalCoordinates,
+        Qddot: NaturalAccelerations,
+        external_forces: ExternalForceList = None,
+    ) -> tuple[MX, MX, MX]:
+        if external_forces is None:
+            external_forces = ExternalForceList.empty_from_nb_segment(self.nb_segments)
+        else:
+            if external_forces.nb_segments != self.nb_segments:
+                raise ValueError(
+                    f"The number of segments in the model and the external forces must be the same:"
+                    f" segment number = {self.nb_segments}"
+                    f" external force size = {external_forces.nb_segments}"
+                )
+
+        if Q is None:
+            raise ValueError(f"The generalized coordinates must be provided")
+        if Q.nb_qi() != self.nb_segments:
+            raise ValueError(
+                f"The number of generalized coordinates in the model and the generalized coordinates must be the same:"
+                f" model number = {self.nb_segments}"
+                f" generalized coordinates size = {Q.nb_qi()}"
+            )
+        if Qddot is None:
+            raise ValueError(f"The generalized accelerations must be provided")
+        if Qddot.nb_qddoti() != self.nb_segments:
+            raise ValueError(
+                f"The number of generalized accelerations in the model and the generalized accelerations must be the same:"
+                f" model number = {self.nb_segments}"
+                f" generalized accelerations size = {Qddot.nb_qddoti()}"
+            )
+
+        # last check to verify that the model doesn't contain any closed loop
+        visited_segment = self._depth_first_search(0, visited_segments=None)
+        if not all(visited_segment):
+            raise ValueError(
+                f"The model contains free segments. The inverse dynamics can't be computed."
+                f" The free segments are: {np.where(np.logical_not(visited_segment))[0]}."
+                f" Please consider adding joints to integer them into the kinematic tree."
+            )
+
+        # NOTE: This won't work with two independent tree in the same model
+        visited_segments = [False for _ in range(self.nb_segments)]
+        torques = MX.zeros((3, self.nb_segments))
+        forces = MX.zeros((3, self.nb_segments))
+        lambdas = MX.zeros((6, self.nb_segments))
+        _, forces, torques, lambdas = self._inverse_dynamics_recursive_step(
+            Q=Q,
+            Qddot=Qddot,
+            external_forces=external_forces,
+            segment_index=0,
+            visited_segments=visited_segments,
+            torques=torques,
+            forces=forces,
+            lambdas=lambdas,
+        )
+
+        return torques, forces, lambdas
+
+    def _inverse_dynamics_recursive_step(
+        self,
+        Q: NaturalCoordinates,
+        Qddot: NaturalAccelerations,
+        external_forces: ExternalForceList,
+        segment_index: int = 0,
+        visited_segments: list[bool, ...] = None,
+        torques: MX = None,
+        forces: MX = None,
+        lambdas: MX = None,
+    ):
+        """
+        This function returns the segments in a depth first search order.
+
+        Parameters
+        ----------
+        Q: NaturalCoordinates
+            The generalized coordinates of the model
+        Qddot: NaturalAccelerations
+            The generalized accelerations of the model
+        external_forces: ExternalForceList
+            The external forces applied to the model
+        segment_index: int
+            The index of the segment to start the search from
+        visited_segments: list[bool]
+            The segments already visited
+        torques: MX
+            The intersegmental torques applied to the segments
+        forces: MX
+            The intersegmental forces applied to the segments
+        lambdas: MX
+            The lagrange multipliers applied to the segments
+
+        Returns
+        -------
+        tuple[list[bool, ...], MX, MX, MX]
+            visited_segments: list[bool]
+                The segments already visited
+            torques: MX
+                The intersegmental torques applied to the segments
+            forces: MX
+                The intersegmental forces applied to the segments
+            lambdas: MX
+                The lagrange multipliers applied to the segments
+        """
+        visited_segments[segment_index] = True
+
+        Qi = Q.vector(segment_index)
+        Qddoti = Qddot.vector(segment_index)
+        external_forces_i = external_forces.to_segment_natural_external_forces(segment_index=segment_index, Q=Q)
+
+        subtree_intersegmental_generalized_forces = MX.zeros((12, 1))
+        for child_index in self.children(segment_index):
+            if not visited_segments[child_index]:
+                visited_segments, torques, forces, lambdas = self._inverse_dynamics_recursive_step(
+                    Q,
+                    Qddot,
+                    external_forces,
+                    child_index,
+                    visited_segments=visited_segments,
+                    torques=torques,
+                    forces=forces,
+                    lambdas=lambdas,
+                )
+            # sum the generalized forces of each subsegment and transport them to the parent proximal point
+            intersegmental_generalized_forces = ExternalForce.from_components(
+                application_point_in_local=[0, 0, 0], force=forces[:, child_index], torque=torques[:, child_index]
+            )
+            subtree_intersegmental_generalized_forces += intersegmental_generalized_forces.transport_to(
+                to_segment_index=segment_index,
+                new_application_point_in_local=[0, 0, 0],  # proximal point
+                from_segment_index=child_index,
+                Q=Q,
+            )
+        segment_i = self.segment_from_index(segment_index)
+
+        force_i, torque_i, lambda_i = segment_i.inverse_dynamics(
+            Qi=Qi,
+            Qddoti=Qddoti,
+            subtree_intersegmental_generalized_forces=subtree_intersegmental_generalized_forces,
+            segment_external_forces=external_forces_i,
+        )
+        # re-assigned the computed values to the output arrays
+        torques[:, segment_index] = torque_i
+        forces[:, segment_index] = force_i
+        lambdas[:, segment_index] = lambda_i
+
+        return visited_segments, torques, forces, lambdas

bionc/bionc_casadi/external_force.py

@@ -1,4 +1,4 @@
-from casadi import MX, inv, cross
+from casadi import MX, inv, cross, vertcat
 import numpy as np
 
 from .natural_vector import NaturalVector
@@ -55,7 +55,7 @@ def from_components(
         ExternalForce
         """
 
-        return cls(application_point_in_local, np.concatenate((torque, force)))
+        return cls(application_point_in_local, vertcat(torque, force))
 
     @property
     def force(self) -> MX:
@@ -67,63 +67,72 @@ 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:
+    def to_natural_force(self, Qi: SegmentNaturalCoordinates) -> MX:
         """
-        Return the force moment transformation matrix
+        Apply external forces to the segment
 
         Parameters
         ----------
-        Qi : SegmentNaturalCoordinates
-            The natural coordinates of the segment
+        Qi: SegmentNaturalCoordinates
+            Segment natural coordinates
 
         Returns
         -------
-        np.ndarray
-            The force moment transformation matrix
+        MX
+            The external forces adequately transformed for the equation of motion in natural coordinates [12 x 1]
         """
-        # 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))
+        pseudo_interpolation_matrix = Qi.compute_pseudo_interpolation_matrix()
+        point_interpolation_matrix = NaturalVector(self.application_point_in_local).interpolate()
 
-        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)
+        fext = point_interpolation_matrix.T @ self.force
+        fext += pseudo_interpolation_matrix.T @ self.torque
 
-        return (left_interpolation_matrix @ inv(lever_arm_force_matrix)).T  # NOTE: inv may induce symbolic error.
+        return fext
 
-    def to_natural_force(self, Qi: SegmentNaturalCoordinates) -> MX:
+    def transport_to(
+        self,
+        to_segment_index: int,
+        new_application_point_in_local: np.ndarray,
+        Q: NaturalCoordinates,
+        from_segment_index: int,
+    ) -> MX:
         """
-        Apply external forces to the segment
+        Transport the external force to another segment and another application point
 
         Parameters
         ----------
-        Qi: SegmentNaturalCoordinates
-            Segment natural coordinates
+        to_segment_index: int
+            The index of the new segment
+        new_application_point_in_local: np.ndarray
+            The application point of the force in the natural coordinate system of the new segment
+        Q: NaturalCoordinates
+            The natural coordinates of the system
+        from_segment_index: int
+            The index of the current segment the force is applied on
 
         Returns
         -------
         np.ndarray
-            The external forces adequately transformed for the equation of motion in natural coordinates
+            The external forces adequately transformed for the equation of motion in natural coordinates [12 x 1]
         """
 
-        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
+        Qi_old = Q.vector(from_segment_index)
+        Qi_new = Q.vector(to_segment_index)
 
-        fext = point_interpolation_matrix.T @ self.force
-        fext += pseudo_interpolation_matrix.T @ self.torque
+        old_point_interpolation_matrix = NaturalVector(self.application_point_in_local).interpolate()
+        new_point_interpolation_matrix = NaturalVector(new_application_point_in_local).interpolate()
+
+        old_application_point_in_global = old_point_interpolation_matrix @ Qi_old
+        new_application_point_in_global = new_point_interpolation_matrix @ Qi_new
 
-        # 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)
+        new_pseudo_interpolation_matrix = Qi_new.compute_pseudo_interpolation_matrix()
+
+        # Bour's formula to transport the moment from the application point to the new application point
+        lever_arm = new_application_point_in_global - old_application_point_in_global
+        additional_torque = new_pseudo_interpolation_matrix.T @ cross(lever_arm, self.force)
+        fext = self.to_natural_force(Qi_new)
+        fext += additional_torque
 
         return fext
 
@@ -224,6 +233,37 @@ def to_natural_external_forces(self, Q: NaturalCoordinates) -> MX:
 
         return natural_external_forces
 
+    def to_segment_natural_external_forces(self, Q: NaturalCoordinates, segment_index: int) -> MX:
+        """
+        Converts and sums all the segment natural external forces to the full vector of natural external forces
+        for one segment
+
+        Parameters
+        ----------
+        Q : NaturalCoordinates
+            The natural coordinates of the model
+        segment_index: int
+            The index of the segment
+
+        Returns
+        -------
+        segment_natural_external_forces: MX
+        """
+
+        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"
+            )
+
+        if segment_index >= len(self.external_forces):
+            raise ValueError("The segment index is out of range")
+
+        segment_natural_external_forces = np.zeros((12, 1))
+        for external_force in self.external_forces[segment_index]:
+            segment_natural_external_forces += external_force.to_natural_force(Q.vector(segment_index))
+
+        return segment_natural_external_forces
+
     def __iter__(self):
         return iter(self.external_forces)
 

bionc/bionc_casadi/natural_coordinates.py

@@ -1,5 +1,5 @@
 import numpy as np
-from casadi import MX, vertcat, sum2, cross, sum1
+from casadi import MX, vertcat, inv, cross, sum1
 from typing import Union
 
 from .natural_vector import NaturalVector
@@ -203,6 +203,33 @@ def axis(self, axis: Union[str, NaturalAxis]) -> MX:
         else:
             raise ValueError("The axis must be u, v or w")
 
+    def compute_pseudo_interpolation_matrix(self) -> MX:
+        """
+        Return the force moment transformation matrix
+
+        Returns
+        -------
+        MX
+            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] = self.u
+        left_interpolation_matrix[0:3, 1] = self.v
+        left_interpolation_matrix[3:6, 2] = -self.w
+        left_interpolation_matrix[6:9, 2] = self.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(self.w, self.u)
+        lever_arm_force_matrix[:, 1] = cross(self.u, self.v)
+        lever_arm_force_matrix[:, 2] = cross(-self.v, self.w)
+
+        return (left_interpolation_matrix @ inv(lever_arm_force_matrix)).T  # NOTE: inv may induce symbolic error.
+
 
 class NaturalCoordinates(MX):
     def __new__(cls, input_array: MX):