Merge pull request #68 from Ipuch/another_fext_xp

doctstrings and external forces
Ipuch · Apr 20, 2023 · a5c98af · a5c98af
2 parents 6b67181 + 88ca152
commit a5c98af
Show file tree

Hide file tree

Showing 5 changed files with 23 additions and 11 deletions.
diff --git a/.github/workflows/run_tests.yml b/.github/workflows/run_tests.yml
@@ -44,7 +44,7 @@ jobs:
       - name: Run the actual tests on LINUX
         run: |
           sudo apt-get install xvfb
-          xvfb-run --server-args="-screen 0 1024x768x24" pytest -v --color=yes --cov-report term-missing --cov=bioviz tests
+          xvfb-run --server-args="-screen 0 1024x768x24" pytest -v --color=yes --cov-report term-missing --cov=bionc tests
         if: matrix.label == 'linux-64'
 
       - name: Run the actual tests

diff --git a/bionc/bionc_casadi/biomechanical_model.py b/bionc/bionc_casadi/biomechanical_model.py
@@ -6,6 +6,7 @@
 from .natural_velocities import NaturalVelocities
 from .natural_accelerations import NaturalAccelerations
 from ..protocols.biomechanical_model import GenericBiomechanicalModel
+from .external_force import ExternalForceList
 
 
 class BiomechanicalModel(GenericBiomechanicalModel):
@@ -601,6 +602,7 @@ def forward_dynamics(
         self,
         Q: NaturalCoordinates,
         Qdot: NaturalCoordinates,
+        external_forces: ExternalForceList = None,
         # external_forces: ExternalForces
     ):
         """
@@ -612,6 +614,8 @@ 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
+            The list of external forces applied on the system
 
         Returns
         -------
@@ -624,6 +628,10 @@ 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
+        )
+        fext = external_forces.to_natural_external_forces(Q)
         # if stabilization is not None:
         #     biais -= stabilization["alpha"] * self.rigid_body_constraint(Qi) + stabilization[
         #         "beta"
@@ -636,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()
+        forces = self.weight() + fext
         biais = -Kdot @ Qdot
         B = vertcat(forces, biais)
 

diff --git a/bionc/bionc_casadi/external_force.py b/bionc/bionc_casadi/external_force.py
@@ -35,7 +35,9 @@ def __init__(self, application_point_in_local: np.ndarray | MX, external_forces:
         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):
+    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.
 

diff --git a/bionc/bionc_numpy/biomechanical_model.py b/bionc/bionc_numpy/biomechanical_model.py
@@ -658,6 +658,8 @@ def forward_dynamics(
         -------
             Qddot : NaturalAccelerations
                 The natural accelerations [12 * nb_segments, 1]
+            lagrange_multipliers : np.ndarray
+                The lagrange multipliers [nb_holonomic_constraints, 1]
         """
         G = self.mass_matrix
         K = self.holonomic_constraints_jacobian(Q)

diff --git a/examples/play_with_joints/two_constant_length.py b/examples/play_with_joints/two_constant_length.py
@@ -39,7 +39,7 @@ def build_two_link_segment():
         NaturalMarker(
             name="point_A",
             parent_name="segment_0",
-            position=np.array([0, -1, 0]),
+            position=np.array([0, -1, 0.05]),
             is_technical=True,
             is_anatomical=False,
         )
@@ -49,7 +49,7 @@ def build_two_link_segment():
         NaturalMarker(
             name="point_AA",
             parent_name="segment_0",
-            position=np.array([0, -1, 0.05]),
+            position=np.array([0, -1, -0.05]),
             is_technical=True,
             is_anatomical=False,
         )
@@ -59,7 +59,7 @@ def build_two_link_segment():
         NaturalMarker(
             name="point_B",
             parent_name="segment_1",
-            position=np.array([0, 0, 0]),
+            position=np.array([0, 0, 0.05]),
             is_technical=True,
             is_anatomical=False,
         )
@@ -69,7 +69,7 @@ def build_two_link_segment():
         NaturalMarker(
             name="point_BB",
             parent_name="segment_1",
-            position=np.array([0, 0, 0.05]),
+            position=np.array([0, 0, -0.05]),
             is_technical=True,
             is_anatomical=False,
         )
@@ -129,11 +129,11 @@ def main(initial_pose: str = "hanged"):
                 1,
                 0,
                 0,
-                0.2 * np.cos(np.pi / 4),
                 0,
-                -(0.8 + 0.2 * np.sin(np.pi / 4)),
                 0.2 * np.cos(np.pi / 4),
+                -(0.8 + 0.2 * np.sin(np.pi / 4)),
                 0,
+                0.2 * np.cos(np.pi / 4),
                 -(0.8 + 0.2 * np.sin(np.pi / 4)) - 0.8,
                 0,
                 -1,
@@ -203,5 +203,5 @@ def main(initial_pose: str = "hanged"):
 
 
 if __name__ == "__main__":
-    main("hanged")
-    # main("ready_to_swing")
+    # main("hanged")
+    main("ready_to_swing")